void Joint::PreStep(float inv_dt) { // Pre-compute anchors, mass matrix, and bias. Mat22 Rot1(body1->rotation); Mat22 Rot2(body2->rotation); r1 = Rot1 * localAnchor1; r2 = Rot2 * localAnchor2; // deltaV = deltaV0 + K * impulse // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] Mat22 K1; K1.col1.x = body1->invMass + body2->invMass; K1.col2.x = 0.0f; K1.col1.y = 0.0f; K1.col2.y = body1->invMass + body2->invMass; Mat22 K2; K2.col1.x = body1->invI * r1.y * r1.y; K2.col2.x = -body1->invI * r1.x * r1.y; K2.col1.y = -body1->invI * r1.x * r1.y; K2.col2.y = body1->invI * r1.x * r1.x; Mat22 K3; K3.col1.x = body2->invI * r2.y * r2.y; K3.col2.x = -body2->invI * r2.x * r2.y; K3.col1.y = -body2->invI * r2.x * r2.y; K3.col2.y = body2->invI * r2.x * r2.x; Mat22 K = K1 + K2 + K3; K.col1.x += softness; K.col2.y += softness; M = K.Invert(); Vec2 p1 = body1->position + r1; Vec2 p2 = body2->position + r2; Vec2 dp = p2 - p1; if (World::positionCorrection) { bias = -biasFactor * inv_dt * dp; } else { bias.Set(0.0f, 0.0f); } if (World::warmStarting) { // Apply accumulated impulse. body1->velocity -= body1->invMass * P; body1->angularVelocity -= body1->invI * Cross(r1, P); body2->velocity += body2->invMass * P; body2->angularVelocity += body2->invI * Cross(r2, P); } else { P.Set(0.0f, 0.0f); } }
// refine the plane given set of inlier points gtsam::Pose2 PoseEstimator::refine(const Datums& ps, const Mask& mask, boost::optional<Model> bestModel) { // filter inlier std::vector<Point2Pair> putatives; for (size_t i = 0; i < ps.size(); i++) { if (!mask[i]) continue; putatives.push_back(ps[i]); } // check inlier size if (putatives.size() < 2) throw std::runtime_error("minimal pose solver must have input inlier size >= 2"); // method of 'A Method for Registration of 3D Shapes', by Besl and McKay, 1992 // TODO: not sure this is correct in 2D // 1. find centroids of each dataset Vector2 cent1 = zero(2); Vector2 cent2 = zero(2); for (size_t i = 0; i < putatives.size(); i++) { cent1 += putatives[i].first.vector(); cent2 += putatives[i].second.vector(); } cent1 = cent1 / static_cast<double>(putatives.size()); cent2 = cent2 / static_cast<double>(putatives.size()); // 2. SVD Matrix H = zeros(2,2); for (size_t i = 0; i < putatives.size(); i++) H = H + (putatives[i].first.vector() - cent1) * (putatives[i].second.vector() - cent1).transpose(); Matrix U,V; Vector S; svd(H, U, S, V); // 3. get rotation matrix Matrix R = V * U.transpose(); if (R.determinant() < 0) { V(0,1) = -V(0,1); V(1,1) = -V(1,1); R = V * U.transpose(); } // 4. translation Vector2 t = -R * cent1 + cent2; return Pose2(Rot2(atan2(R(1,0), R(0,0))), Point2(t)); }
void Joint::Set(Body* b1, Body* b2, const Vec2& anchor) { body1 = b1; body2 = b2; Mat22 Rot1(body1->rotation); Mat22 Rot2(body2->rotation); Mat22 Rot1T = Rot1.Transpose(); Mat22 Rot2T = Rot2.Transpose(); localAnchor1 = Rot1T * (anchor - body1->position); localAnchor2 = Rot2T * (anchor - body2->position); P.Set(0.0f, 0.0f); softness = 0.0f; biasFactor = 0.2f; }