本文整理汇总了C++中eigen::Vector2f::norm方法的典型用法代码示例。如果您正苦于以下问题:C++ Vector2f::norm方法的具体用法?C++ Vector2f::norm怎么用?C++ Vector2f::norm使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::Vector2f
的用法示例。
在下文中一共展示了Vector2f::norm方法的5个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: update
void update(float t, float dt) {
Eigen::Vector2f dist = position - goal;
float dist_norm = dist.norm();
if(dist_norm < 1) {
set_random_goal();
return;
}
position += dt*dist*SPEED/dist_norm;
}
示例2: jacobian
void
Chain::solveJacobian(const float stepSize,
const int maxIterations,
const vec2& desiredPos,
std::vector<float>* iterPose)
{
vec3 G = vec3(desiredPos, 1);
vec3 endEffector; worldEndPos(endEffector);
Eigen::Vector2f error;
error(0) = G.x - endEffector.x;
error(1) = G.y - endEffector.y;
int iter = 0;
int linkCount = count();
while (iter < maxIterations && error.norm() > EPSILON) {
Eigen::Vector2f dx = error * stepSize;
// Calculate jacobian
Eigen::MatrixXf jacobian(2,linkCount);
for (int j = 0; j < linkCount; j++) {
vec3 jo; worldJointPos(j, jo);
vec3 w = endEffector - jo;
vec3 temp = cross(vec3(0,0,1.0f),vec3(w.x, w.y, 1));
jacobian(0,j) = temp.x;
jacobian(1,j) = temp.y;
}
Eigen::MatrixXf jacobianT = jacobian.transpose();
Eigen::MatrixXf invJacobian = jacobian * jacobianT;
invJacobian = jacobianT * invJacobian.inverse();
Eigen::Vector4f delta = invJacobian * dx;
for (int i = 0; i < linkCount; i++) {
Link * l = getLink(i);
if (delta(i) != delta(i)) {
// Nudge angles arbitrarily to
// break singularity for next iteration
l->angle += 1;
} else {
l->angle += delta(i);
}
if (iterPose) {
iterPose->push_back(l->angle);
}
}
// Update world frames for all joints and end effector
worldEndPos(endEffector);
error(0) = G.x - endEffector.x;
error(1) = G.y - endEffector.y;
iter++;
}
}
示例3: desiredVelocity
Eigen::Vector2f QS::Walk::evaluate(const Actor *theActor)
{
Eigen::Vector2f desiredVelocity(mySpeed_ms, 0.0);
Eigen::Vector2f velocity = theActor->getVelocity();
if (velocity.norm() >= mySpeed_ms)
{
desiredVelocity << 0.0, 0.0;
}
Eigen::Vector2f steeringForce = desiredVelocity *
theActor->getMass();
Eigen::Rotation2Df rotation(theActor->getOrientation());
steeringForce = rotation * steeringForce;
return steeringForce;
}
示例4: match
auto match(vfloat2 p, vfloat2 tr_prediction,
F A, F B, GD Ag,
const int winsize,
const float min_ev_th,
const int max_interations,
const float convergence_delta)
{
typedef typename F::value_type V;
int WS = winsize;
int ws = winsize;
int hws = ws/2;
// Gradient matrix
Eigen::Matrix2f G = Eigen::Matrix2f::Zero();
int cpt = 0;
for(int r = -hws; r <= hws; r++)
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (A.has(n.cast<int>()))
{
Eigen::Matrix2f m;
auto g = Ag.linear_interpolate(n);
float gx = g[0];
float gy = g[1];
m <<
gx * gx, gx * gy,
gx * gy, gy * gy;
G += m;
cpt++;
}
}
// Check minimum eigenvalue.
float min_ev = 99999.f;
auto ev = (G / cpt).eigenvalues();
for (int i = 0; i < ev.size(); i++)
if (fabs(ev[i].real()) < min_ev) min_ev = fabs(ev[i].real());
if (min_ev < min_ev_th)
return std::pair<vfloat2, float>(vfloat2(-1,-1), FLT_MAX);
Eigen::Matrix2f G1 = G.inverse();
// Precompute gs and as.
vfloat2 prediction_ = p + tr_prediction;
vfloat2 v = prediction_;
Eigen::Vector2f nk = Eigen::Vector2f::Ones();
char gs_buffer[WS * WS * sizeof(vfloat2)];
vfloat2* gs = (vfloat2*) gs_buffer;
// was: vfloat2 gs[WS * WS];
typedef plus_promotion<V> S;
char as_buffer[WS * WS * sizeof(S)];
S* as = (S*) as_buffer;
// was: S as[WS * WS];
{
for(int i = 0, r = -hws; r <= hws; r++)
{
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (Ag.has(n.cast<int>()))
{
gs[i] = Ag.linear_interpolate(n).template cast<float>();
as[i] = cast<S>(A.linear_interpolate(n));
}
i++;
}
}
}
auto domain = B.domain();// - border(hws + 1);
// Gradient descent
for (int k = 0; k <= max_interations && nk.norm() >= convergence_delta; k++)
{
Eigen::Vector2f bk = Eigen::Vector2f::Zero();
// Temporal difference.
int i = 0;
for(int r = -hws; r <= hws; r++)
{
for(int c = -hws; c <= hws; c++)
{
vfloat2 n = p + vfloat2(r, c);
if (Ag.has(n.cast<int>()))
{
vfloat2 n2 = v + vfloat2(r, c);
auto g = gs[i];
float dt = (cast<float>(as[i]) - cast<float>(B.linear_interpolate(n2)));
bk += Eigen::Vector2f{g[0] * dt, g[1] * dt};
}
i++;
}
}
nk = G1 * bk;
v += vfloat2{nk[0], nk[1]};
if (!domain.has(v.cast<int>()))
//.........这里部分代码省略.........
示例5: derivatives
void derivatives(const StateMatrix& states, StateMatrix& derivs,
const ControlMatrix& ctrls, const SimulationParameters& params,
const Map & map)
{
float cd_a_rho = params.linearDrag; // 0.1 coeff of drag * area * density of fluid
float k_elastic = params.elasticity; // 4000. // spring constant of ships
float rad = 1.; // leave radius 1 - we decided to change map scale instead
const Eigen::VectorXf &masses = params.shipDensities; // order of 1.0 Mass of ships
float spin_drag_ratio = params.rotationalDrag; // 1.8; // spin friction to translation friction
float eps = 1e-5; // Avoid divide by zero special cases
float mu = params.shipFriction; // 0.05; // friction coefficient between ships
float mu_wall = params.wallFriction; //0.25*?wallFriction; // 0.01; // wall friction parameter
float wall_restitution = params.wallRestitution; // 0.5
float ship_restitution = params.shipRestitution; // circa 0.5
float diameter = 2.*rad; // rad(i) + rad(j) for any i, j
float inertia_mass_ratio = 0.25;
float map_grid = rad * 2. + eps; // must be 2*radius + eps
std::unordered_map<std::pair<int, int>, std::vector<uint>,
boost::hash<std::pair<int, int>>> bins;
uint n = states.rows();
Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n, 2);
Eigen::VectorXd trq = Eigen::VectorXd::Zero(n);
// rotationalThrust Order +- 10
// linearThrust Order +100
// mapscale order 10 - thats params.pixelsize
// Accumulate forces and torques into these:
uint collide_checks = 0; // debug count...
for (uint i=0; i<n; i++) {
Eigen::Vector2f pos_i;
pos_i(0) = states(i,0);
pos_i(1) = states(i,1);
Eigen::Vector2f vel_i;
vel_i(0) = states(i,2);
vel_i(1) = states(i,3);
float theta_i = states(i,4);
float w_i = states(i,5);
// 1. Control
float thrusting = ctrls(i, 0);
float turning = ctrls(i, 1);
f(i, 0) = thrusting * params.linearThrust * cos(theta_i);
f(i, 1) = thrusting * params.linearThrust * sin(theta_i);
trq(i) = turning * params.rotationalThrust;
// 2. Drag
f(i, 0) -= cd_a_rho * vel_i(0);
f(i, 1) -= cd_a_rho * vel_i(1);
trq(i) -= spin_drag_ratio*cd_a_rho*w_i*rad*rad; // * abs(w_i)
// 3. Inter-ship collisions against ships of lower index...
// Figure out this ship's hashes: It has 4 in 2 dimensions
std::unordered_set<uint> collision_shortlist;
std::pair<int, int> my_hash;
for (int dx=-1; dx < 2; dx+=2)
for (int dy=-1; dy < 2; dy+=2)
{
float x_mod = pos_i(0) + float(dx)*rad;
float y_mod = pos_i(1) + float(dy)*rad;
my_hash = std::make_pair(int(x_mod / map_grid),
int(y_mod / map_grid));
if (bins.count(my_hash) > 0)
{
// Already exists - shortlist others and add self
std::vector<uint> current_bin = bins.find(my_hash)->second;
// -->first is the key as it returns a key/value pair
for (uint bin_idx: current_bin)
if (bin_idx != i)
collision_shortlist.insert(bin_idx);
current_bin.push_back(i);
}
else
{
// didnt exist - add self, and push into map
std::vector<uint> current_bin;
current_bin.push_back(i);
bins.insert(std::make_pair(my_hash, current_bin));
}
}
for (uint j: collision_shortlist) { // =i+1; j<n; j++) {
collide_checks ++;
// std::cout << "Checking " << i << ", " << j << "\n";
Eigen::Vector2f pos_j;
pos_j(0) = states(j,0);
pos_j(1) = states(j,1);
Eigen::Vector2f vel_j;
vel_j(0) = states(j,2);
vel_j(1) = states(j,3);
float theta_j = states(j,4);
float w_j = states(j,5);
Eigen::Vector2f dP = pos_j - pos_i;
float dist = dP.norm() + eps - diameter;
Eigen::Vector2f dPhat = dP / (dP.norm() + eps);
if (dist < 0) {
// we have a collision interaction
// A. Direct collision: apply linear spring normal force
//.........这里部分代码省略.........