public float Evaluate(int indexA, int indexB, float t) { SweepA.GetTransform(xfa, t); SweepB.GetTransform(xfb, t); switch (Type) { case Type.Points: { Rot.MulTransUnsafe(xfa.Q, Axis, axisA); Rot.MulTransUnsafe(xfb.Q, Axis.NegateLocal(), axisB); Axis.NegateLocal(); localPointA.Set(ProxyA.GetVertex(indexA)); localPointB.Set(ProxyB.GetVertex(indexB)); Transform.MulToOutUnsafe(xfa, localPointA, pointA); Transform.MulToOutUnsafe(xfb, localPointB, pointB); float separation = Vec2.Dot(pointB.SubLocal(pointA), Axis); return(separation); } case Type.FaceA: { // System.out.printf("We're faceA\n"); Rot.MulToOutUnsafe(xfa.Q, Axis, normal); Transform.MulToOutUnsafe(xfa, LocalPoint, pointA); Rot.MulTransUnsafe(xfb.Q, normal.NegateLocal(), axisB); normal.NegateLocal(); localPointB.Set(ProxyB.GetVertex(indexB)); Transform.MulToOutUnsafe(xfb, localPointB, pointB); float separation = Vec2.Dot(pointB.SubLocal(pointA), normal); return(separation); } case Type.FaceB: { // System.out.printf("We're faceB\n"); Rot.MulToOutUnsafe(xfb.Q, Axis, normal); Transform.MulToOutUnsafe(xfb, LocalPoint, pointB); Rot.MulTransUnsafe(xfa.Q, normal.NegateLocal(), axisA); normal.NegateLocal(); localPointA.Set(ProxyA.GetVertex(indexA)); Transform.MulToOutUnsafe(xfa, localPointA, pointA); float separation = Vec2.Dot(pointA.SubLocal(pointB), normal); return(separation); } default: Debug.Assert(false); return(0f); } }
public override bool TestPoint(Transform transform, Vec2 p) { Vec2 center = pool1; Rot.MulToOutUnsafe(transform.Q, P, center); center.AddLocal(transform.P); Vec2 d = center.SubLocal(p).NegateLocal(); return(Vec2.Dot(d, d) <= Radius * Radius); }
internal void Advance(float t) { // Advance to the new safe time. This doesn't sync the broad-phase. Sweep.Advance(t); Sweep.C.Set(Sweep.C0); Sweep.A = Sweep.A0; Xf.Q.Set(Sweep.A); // m_xf.position = m_sweep.c - Mul(m_xf.R, m_sweep.localCenter); Rot.MulToOutUnsafe(Xf.Q, Sweep.LocalCenter, Xf.P); Xf.P.MulLocal(-1).AddLocal(Sweep.C); }
public override void ComputeAABB(AABB aabb, Transform transform, int childIndex) { Vec2 p = pool1; Rot.MulToOutUnsafe(transform.Q, P, p); p.AddLocal(transform.P); aabb.LowerBound.X = p.X - Radius; aabb.LowerBound.Y = p.Y - Radius; aabb.UpperBound.X = p.X + Radius; aabb.UpperBound.Y = p.Y + Radius; }
public override bool SolvePositionConstraints(SolverData data) { if (FrequencyHz > 0.0f) { return(true); } Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); Vec2 u = Pool.PopVec2(); Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; qA.Set(aA); qB.Set(aB); Rot.MulToOutUnsafe(qA, u.Set(LocalAnchorA).SubLocal(LocalCenterA), rA); Rot.MulToOutUnsafe(qB, u.Set(LocalAnchorB).SubLocal(LocalCenterB), rB); u.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA); float length = u.Normalize(); float C = length - Length; C = MathUtils.Clamp(C, -Settings.MAX_LINEAR_CORRECTION, Settings.MAX_LINEAR_CORRECTION); float impulse = (-Mass) * C; float Px = impulse * u.X; float Py = impulse * u.Y; cA.X -= InvMassA * Px; cA.Y -= InvMassA * Py; aA -= InvIA * (rA.X * Py - rA.Y * Px); cB.X += InvMassB * Px; cB.Y += InvMassB * Py; aB += InvIB * (rB.X * Py - rB.Y * Px); data.Positions[IndexA].C.Set(cA); data.Positions[IndexA].A = aA; data.Positions[IndexB].C.Set(cB); data.Positions[IndexB].A = aB; Pool.PushVec2(3); Pool.PushRot(2); return(MathUtils.Abs(C) < Settings.LINEAR_SLOP); }
public void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index) { Debug.Assert(pc.PointCount > 0); switch (pc.Type) { case Manifold.ManifoldType.Circles: { Transform.MulToOutUnsafe(xfA, pc.LocalPoint, pointA); Transform.MulToOutUnsafe(xfB, pc.LocalPoints[0], pointB); Normal.Set(pointB).SubLocal(pointA); Normal.Normalize(); Point.Set(pointA).AddLocal(pointB).MulLocal(.5f); temp.Set(pointB).SubLocal(pointA); Separation = Vec2.Dot(temp, Normal) - pc.RadiusA - pc.RadiusB; break; } case Manifold.ManifoldType.FaceA: { Rot.MulToOutUnsafe(xfA.Q, pc.LocalNormal, Normal); Transform.MulToOutUnsafe(xfA, pc.LocalPoint, planePoint); Transform.MulToOutUnsafe(xfB, pc.LocalPoints[index], clipPoint); temp.Set(clipPoint).SubLocal(planePoint); Separation = Vec2.Dot(temp, Normal) - pc.RadiusA - pc.RadiusB; Point.Set(clipPoint); break; } case Manifold.ManifoldType.FaceB: { Rot.MulToOutUnsafe(xfB.Q, pc.LocalNormal, Normal); Transform.MulToOutUnsafe(xfB, pc.LocalPoint, planePoint); Transform.MulToOutUnsafe(xfA, pc.LocalPoints[index], clipPoint); temp.Set(clipPoint).SubLocal(planePoint); Separation = Vec2.Dot(temp, Normal) - pc.RadiusA - pc.RadiusB; Point.Set(clipPoint); // Ensure normal points from A to B Normal.NegateLocal(); } break; } }
// Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius public override bool Raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex) { Vec2 position = pool1; Vec2 s = pool2; Vec2 r = pool3; Rot.MulToOutUnsafe(transform.Q, P, position); position.AddLocal(transform.P); s.Set(input.P1).SubLocal(position); float b = Vec2.Dot(s, s) - Radius * Radius; // Solve quadratic equation. r.Set(input.P2).SubLocal(input.P1); float c = Vec2.Dot(s, r); float rr = Vec2.Dot(r, r); float sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < Settings.EPSILON) { return(false); } // Find the point of intersection of the line with the circle. float a = -(c + MathUtils.Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.MaxFraction * rr) { a /= rr; output.Fraction = a; output.Normal.Set(r).MulLocal(a); output.Normal.AddLocal(s); output.Normal.Normalize(); return(true); } return(false); }
public void InitializeVelocityConstraints() { //Console.WriteLine("Initializing velocity constraints for " + m_count + " contacts"); // Warm start. for (int i = 0; i < Count; ++i) { ContactVelocityConstraint vc = VelocityConstraints[i]; ContactPositionConstraint pc = PositionConstraints[i]; float radiusA = pc.RadiusA; float radiusB = pc.RadiusB; Manifold manifold = Contacts[vc.ContactIndex].Manifold; int indexA = vc.IndexA; int indexB = vc.IndexB; float mA = vc.InvMassA; float mB = vc.InvMassB; float iA = vc.InvIA; float iB = vc.InvIB; Vec2 localCenterA = pc.LocalCenterA; Vec2 localCenterB = pc.LocalCenterB; Vec2 cA = Positions[indexA].C; float aA = Positions[indexA].A; Vec2 vA = Velocities[indexA].V; float wA = Velocities[indexA].W; Vec2 cB = Positions[indexB].C; float aB = Positions[indexB].A; Vec2 vB = Velocities[indexB].V; float wB = Velocities[indexB].W; Debug.Assert(manifold.PointCount > 0); xfA.Q.Set(aA); xfB.Q.Set(aB); Rot.MulToOutUnsafe(xfA.Q, localCenterA, temp); xfA.P.Set(cA).SubLocal(temp); Rot.MulToOutUnsafe(xfB.Q, localCenterB, temp); xfB.P.Set(cB).SubLocal(temp); worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB); vc.Normal.Set(worldManifold.Normal); int pointCount = vc.PointCount; for (int j = 0; j < pointCount; ++j) { ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j]; vcp.RA.Set(worldManifold.Points[j]).SubLocal(cA); vcp.RB.Set(worldManifold.Points[j]).SubLocal(cB); float rnA = Vec2.Cross(vcp.RA, vc.Normal); float rnB = Vec2.Cross(vcp.RB, vc.Normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.NormalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; Vec2.CrossToOutUnsafe(vc.Normal, 1.0f, tangent); float rtA = Vec2.Cross(vcp.RA, tangent); float rtB = Vec2.Cross(vcp.RB, tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp.TangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp.VelocityBias = 0.0f; Vec2.CrossToOutUnsafe(wB, vcp.RB, temp1); Vec2.CrossToOutUnsafe(wA, vcp.RA, temp2); temp.Set(vB).AddLocal(temp1).SubLocal(vA).SubLocal(temp2); float vRel = Vec2.Dot(vc.Normal, temp); if (vRel < -Settings.VELOCITY_THRESHOLD) { vcp.VelocityBias = (-vc.Restitution) * vRel; } } // If we have two points, then prepare the block solver. if (vc.PointCount == 2) { ContactVelocityConstraint.VelocityConstraintPoint vcp1 = vc.Points[0]; ContactVelocityConstraint.VelocityConstraintPoint vcp2 = vc.Points[1]; float rn1A = Vec2.Cross(vcp1.RA, vc.Normal); float rn1B = Vec2.Cross(vcp1.RB, vc.Normal); float rn2A = Vec2.Cross(vcp2.RA, vc.Normal); float rn2B = Vec2.Cross(vcp2.RB, vc.Normal); float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; if (k11 * k11 < MAX_CONDITION_NUMBER * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.Ex.Set(k11, k12); vc.K.Ey.Set(k12, k22); vc.K.InvertToOut(vc.NormalMass); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.PointCount = 1; } } } }
/// <seealso cref="Joint.solvePositionConstraints(float)"></seealso> public override bool SolvePositionConstraints(SolverData data) { Vec2 cA = data.Positions[m_indexA].C; float aA = data.Positions[m_indexA].A; Vec2 cB = data.Positions[m_indexB].C; float aB = data.Positions[m_indexB].A; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 temp = Pool.PopVec2(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); qA.Set(aA); qB.Set(aB); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), rA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), rB); float positionError, angularError; Mat33 K = Pool.PopMat33(); Vec2 C1 = Pool.PopVec2(); Vec2 P = Pool.PopVec2(); K.Ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.Ey.X = (-rA.Y) * rA.X * iA - rB.Y * rB.X * iB; K.Ez.X = (-rA.Y) * iA - rB.Y * iB; K.Ex.Y = K.Ey.X; K.Ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.Ez.Y = rA.X * iA + rB.X * iB; K.Ex.Z = K.Ez.X; K.Ey.Z = K.Ez.Y; K.Ez.Z = iA + iB; if (Frequency > 0.0f) { C1.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA); positionError = C1.Length(); angularError = 0.0f; K.Solve22ToOut(C1, P); P.NegateLocal(); cA.X -= mA * P.X; cA.Y -= mA * P.Y; aA -= iA * Vec2.Cross(rA, P); cB.X += mB * P.X; cB.Y += mB * P.Y; aB += iB * Vec2.Cross(rB, P); } else { C1.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA); float C2 = aB - aA - m_referenceAngle; positionError = C1.Length(); angularError = MathUtils.Abs(C2); Vec3 C = Pool.PopVec3(); Vec3 impulse = Pool.PopVec3(); C.Set(C1.X, C1.Y, C2); K.Solve33ToOut(C, impulse); impulse.NegateLocal(); P.Set(impulse.X, impulse.Y); cA.X -= mA * P.X; cA.Y -= mA * P.Y; aA -= iA * (Vec2.Cross(rA, P) + impulse.Z); cB.X += mB * P.X; cB.Y += mB * P.Y; aB += iB * (Vec2.Cross(rB, P) + impulse.Z); } data.Positions[m_indexA].C.Set(cA); data.Positions[m_indexA].A = aA; data.Positions[m_indexB].C.Set(cB); data.Positions[m_indexB].A = aB; Pool.PushVec2(5); Pool.PushRot(2); Pool.PushMat33(1); return(positionError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP); }
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex) { Vec2 p1 = pool1; Vec2 p2 = pool2; Vec2 d = pool3; Vec2 temp = pool4; p1.Set(input.P1).SubLocal(xf.P); Rot.MulTrans(xf.Q, p1, p1); p2.Set(input.P2).SubLocal(xf.P); Rot.MulTrans(xf.Q, p2, p2); d.Set(p2).SubLocal(p1); // if (count == 2) { // } else { float lower = 0, upper = input.MaxFraction; int index = -1; for (int i = 0; i < VertexCount; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 temp.Set(Vertices[i]).SubLocal(p1); float numerator = Vec2.Dot(Normals[i], temp); float denominator = Vec2.Dot(Normals[i], d); if (denominator == 0.0f) { if (numerator < 0.0f) { return(false); } } else { // Note: we want this predicate without division: // lower < numerator / denominator, where denominator < 0 // Since denominator < 0, we have to flip the inequality: // lower < numerator / denominator <==> denominator * lower > // numerator. if (denominator < 0.0f && numerator < lower * denominator) { // Increase lower. // The segment enters this half-space. lower = numerator / denominator; index = i; } else if (denominator > 0.0f && numerator < upper * denominator) { // Decrease upper. // The segment exits this half-space. upper = numerator / denominator; } } if (upper < lower) { return(false); } } Debug.Assert(0.0f <= lower && lower <= input.MaxFraction); if (index >= 0) { output.Fraction = lower; Rot.MulToOutUnsafe(xf.Q, Normals[index], output.Normal); // normal = Mul(xf.R, m_normals[index]); return(true); } return(false); }
public override void InitVelocityConstraints(SolverData data) { IndexA = BodyA.IslandIndex; IndexB = BodyB.IslandIndex; LocalCenterA.Set(BodyA.Sweep.LocalCenter); LocalCenterB.Set(BodyB.Sweep.LocalCenter); InvMassA = BodyA.InvMass; InvMassB = BodyB.InvMass; InvIA = BodyA.InvI; InvIB = BodyB.InvI; Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 d = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); qA.Set(aA); qB.Set(aB); // Compute the effective masses. Rot.MulToOutUnsafe(qA, d.Set(LocalAnchorA).SubLocal(LocalCenterA), rA); Rot.MulToOutUnsafe(qB, d.Set(LocalAnchorB).SubLocal(LocalCenterB), rB); d.Set(cB).SubLocal(cA).AddLocal(rB).SubLocal(rA); float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; // Compute motor Jacobian and effective mass. { Rot.MulToOutUnsafe(qA, LocalXAxisA, Axis); temp.Set(d).AddLocal(rA); A1 = Vec2.Cross(temp, Axis); A2 = Vec2.Cross(rB, Axis); MotorMass = mA + mB + iA * A1 * A1 + iB * A2 * A2; if (MotorMass > 0.0f) { MotorMass = 1.0f / MotorMass; } } // Prismatic constraint. { Rot.MulToOutUnsafe(qA, LocalYAxisA, Perp); temp.Set(d).AddLocal(rA); S1 = Vec2.Cross(temp, Perp); S2 = Vec2.Cross(rB, Perp); float k11 = mA + mB + iA * S1 * S1 + iB * S2 * S2; float k12 = iA * S1 + iB * S2; float k13 = iA * S1 * A1 + iB * S2 * A2; float k22 = iA + iB; if (k22 == 0.0f) { // For bodies with fixed rotation. k22 = 1.0f; } float k23 = iA * A1 + iB * A2; float k33 = mA + mB + iA * A1 * A1 + iB * A2 * A2; K.Ex.Set(k11, k12, k13); K.Ey.Set(k12, k22, k23); K.Ez.Set(k13, k23, k33); } // Compute motor and limit terms. if (m_limitEnabled) { float jointTranslation = Vec2.Dot(Axis, d); if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP) { LimitState = LimitState.Equal; } else if (jointTranslation <= LowerTranslation) { if (LimitState != LimitState.AtLower) { LimitState = LimitState.AtLower; Impulse.Z = 0.0f; } } else if (jointTranslation >= UpperTranslation) { if (LimitState != LimitState.AtUpper) { LimitState = LimitState.AtUpper; Impulse.Z = 0.0f; } } else { LimitState = LimitState.Inactive; Impulse.Z = 0.0f; } } else { LimitState = LimitState.Inactive; Impulse.Z = 0.0f; } if (m_motorEnabled == false) { MotorImpulse = 0.0f; } if (data.Step.WarmStarting) { // Account for variable time step. Impulse.MulLocal(data.Step.DtRatio); MotorImpulse *= data.Step.DtRatio; Vec2 P = Pool.PopVec2(); temp.Set(Axis).MulLocal(MotorImpulse + Impulse.Z); P.Set(Perp).MulLocal(Impulse.X).AddLocal(temp); float LA = Impulse.X * S1 + Impulse.Y + (MotorImpulse + Impulse.Z) * A1; float LB = Impulse.X * S2 + Impulse.Y + (MotorImpulse + Impulse.Z) * A2; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * LA; vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * LB; Pool.PushVec2(1); } else { Impulse.SetZero(); MotorImpulse = 0.0f; } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushRot(2); Pool.PushVec2(4); }
/// <seealso cref="Joint.initVelocityConstraints(TimeStep)"></seealso> public override void InitVelocityConstraints(SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_localCenterA.Set(BodyA.Sweep.LocalCenter); m_localCenterB.Set(BodyB.Sweep.LocalCenter); m_invMassA = BodyA.InvMass; m_invMassB = BodyB.InvMass; m_invIA = BodyA.InvI; m_invIB = BodyB.InvI; // Vec2 cA = data.positions[m_indexA].c; float aA = data.Positions[m_indexA].A; Vec2 vA = data.Velocities[m_indexA].V; float wA = data.Velocities[m_indexA].W; // Vec2 cB = data.positions[m_indexB].c; float aB = data.Positions[m_indexB].A; Vec2 vB = data.Velocities[m_indexB].V; float wB = data.Velocities[m_indexB].W; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 temp = Pool.PopVec2(); qA.Set(aA); qB.Set(aB); // Compute the effective masses. Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), m_rA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), m_rB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Mat33 K = Pool.PopMat33(); K.Ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB; K.Ey.X = (-m_rA.Y) * m_rA.X * iA - m_rB.Y * m_rB.X * iB; K.Ez.X = (-m_rA.Y) * iA - m_rB.Y * iB; K.Ex.Y = K.Ey.X; K.Ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB; K.Ez.Y = m_rA.X * iA + m_rB.X * iB; K.Ex.Z = K.Ez.X; K.Ey.Z = K.Ez.Y; K.Ez.Z = iA + iB; if (Frequency > 0.0f) { K.GetInverse22(m_mass); float invM = iA + iB; float m = invM > 0.0f ? 1.0f / invM : 0.0f; float C = aB - aA - m_referenceAngle; // Frequency float omega = 2.0f * MathUtils.PI * Frequency; // Damping coefficient float d = 2.0f * m * DampingRatio * omega; // Spring stiffness float k = m * omega * omega; // magic formulas float h = data.Step.Dt; m_gamma = h * (d + h * k); m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f; m_bias = C * h * k * m_gamma; invM += m_gamma; m_mass.Ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(m_mass); m_gamma = 0.0f; m_bias = 0.0f; } if (data.Step.WarmStarting) { Vec2 P = Pool.PopVec2(); // Scale impulses to support a variable time step. m_impulse.MulLocal(data.Step.DtRatio); P.Set(m_impulse.X, m_impulse.Y); vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * (Vec2.Cross(m_rA, P) + m_impulse.Z); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * (Vec2.Cross(m_rB, P) + m_impulse.Z); Pool.PushVec2(1); } else { m_impulse.SetZero(); } data.Velocities[m_indexA].V.Set(vA); data.Velocities[m_indexA].W = wA; data.Velocities[m_indexB].V.Set(vB); data.Velocities[m_indexB].W = wB; Pool.PushVec2(1); Pool.PushRot(2); Pool.PushMat33(1); }
public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB) { if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case Manifold.ManifoldType.Circles: { // final Vec2 pointA = pool3; // final Vec2 pointB = pool4; // // normal.set(1, 0); // Transform.mulToOut(xfA, manifold.localPoint, pointA); // Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB); // // if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { // normal.set(pointB).subLocal(pointA); // normal.normalize(); // } // // cA.set(normal).mulLocal(radiusA).addLocal(pointA); // cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal(); // points[0].set(cA).addLocal(cB).mulLocal(0.5f); Vec2 pointA = pool3; Vec2 pointB = pool4; Normal.X = 1; Normal.Y = 0; // pointA.x = xfA.p.x + xfA.q.ex.x * manifold.localPoint.x + xfA.q.ey.x * // manifold.localPoint.y; // pointA.y = xfA.p.y + xfA.q.ex.y * manifold.localPoint.x + xfA.q.ey.y * // manifold.localPoint.y; // pointB.x = xfB.p.x + xfB.q.ex.x * manifold.points[0].localPoint.x + xfB.q.ey.x * // manifold.points[0].localPoint.y; // pointB.y = xfB.p.y + xfB.q.ex.y * manifold.points[0].localPoint.x + xfB.q.ey.y * // manifold.points[0].localPoint.y; Transform.MulToOut(xfA, manifold.LocalPoint, pointA); Transform.MulToOut(xfB, manifold.Points[0].LocalPoint, pointB); if (MathUtils.DistanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) { Normal.X = pointB.X - pointA.X; Normal.Y = pointB.Y - pointA.Y; Normal.Normalize(); } float cAx = Normal.X * radiusA + pointA.X; float cAy = Normal.Y * radiusA + pointA.Y; float cBx = (-Normal.X) * radiusB + pointB.X; float cBy = (-Normal.Y) * radiusB + pointB.Y; Points[0].X = (cAx + cBx) * .5f; Points[0].Y = (cAy + cBy) * .5f; } break; case Manifold.ManifoldType.FaceA: { Vec2 planePoint = pool3; Rot.MulToOutUnsafe(xfA.Q, manifold.LocalNormal, Normal); Transform.MulToOut(xfA, manifold.LocalPoint, planePoint); Vec2 clipPoint = pool4; for (int i = 0; i < manifold.PointCount; i++) { // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint); // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, // normal)) * normal; // b2Vec2 cB = clipPoint - radiusB * normal; // points[i] = 0.5f * (cA + cB); Transform.MulToOut(xfB, manifold.Points[i].LocalPoint, clipPoint); // use cA as temporary for now // cA.set(clipPoint).subLocal(planePoint); // float scalar = radiusA - Vec2.dot(cA, normal); // cA.set(normal).mulLocal(scalar).addLocal(clipPoint); // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal(); // points[i].set(cA).addLocal(cB).mulLocal(0.5f); float scalar = radiusA - ((clipPoint.X - planePoint.X) * Normal.X + (clipPoint.Y - planePoint.Y) * Normal.Y); float cAx = Normal.X * scalar + clipPoint.X; float cAy = Normal.Y * scalar + clipPoint.Y; float cBx = (-Normal.X) * radiusB + clipPoint.X; float cBy = (-Normal.Y) * radiusB + clipPoint.Y; Points[i].X = (cAx + cBx) * .5f; Points[i].Y = (cAy + cBy) * .5f; } } break; case Manifold.ManifoldType.FaceB: Vec2 planePoint2 = pool3; Rot.MulToOutUnsafe(xfB.Q, manifold.LocalNormal, Normal); Transform.MulToOut(xfB, manifold.LocalPoint, planePoint2); // final Mat22 R = xfB.q; // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y; // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y; // final Vec2 v = manifold.localPoint; // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y; // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y; Vec2 clipPoint2 = pool4; for (int i = 0; i < manifold.PointCount; i++) { // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint); // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, // normal)) * normal; // b2Vec2 cA = clipPoint - radiusA * normal; // points[i] = 0.5f * (cA + cB); Transform.MulToOut(xfA, manifold.Points[i].LocalPoint, clipPoint2); // cB.set(clipPoint).subLocal(planePoint); // float scalar = radiusB - Vec2.dot(cB, normal); // cB.set(normal).mulLocal(scalar).addLocal(clipPoint); // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal(); // points[i].set(cA).addLocal(cB).mulLocal(0.5f); // points[i] = 0.5f * (cA + cB); // // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x * // manifold.points[i].localPoint.y; // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y * // manifold.points[i].localPoint.y; float scalar = radiusB - ((clipPoint2.X - planePoint2.X) * Normal.X + (clipPoint2.Y - planePoint2.Y) * Normal.Y); float cBx = Normal.X * scalar + clipPoint2.X; float cBy = Normal.Y * scalar + clipPoint2.Y; float cAx = (-Normal.X) * radiusA + clipPoint2.X; float cAy = (-Normal.Y) * radiusA + clipPoint2.Y; Points[i].X = (cAx + cBx) * .5f; Points[i].Y = (cAy + cBy) * .5f; } // Ensure normal points from A to B. Normal.X = -Normal.X; Normal.Y = -Normal.Y; break; } }
public override void InitVelocityConstraints(SolverData data) { IndexA = BodyA.IslandIndex; IndexB = BodyB.IslandIndex; LocalCenterA.Set(BodyA.Sweep.LocalCenter); LocalCenterB.Set(BodyB.Sweep.LocalCenter); InvMassA = BodyA.InvMass; InvMassB = BodyB.InvMass; InvIA = BodyA.InvI; InvIB = BodyB.InvI; Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); qA.Set(aA); qB.Set(aB); // use m_u as temporary variable Rot.MulToOutUnsafe(qA, U.Set(LocalAnchorA).SubLocal(LocalCenterA), RA); Rot.MulToOutUnsafe(qB, U.Set(LocalAnchorB).SubLocal(LocalCenterB), RB); U.Set(cB).AddLocal(RB).SubLocal(cA).SubLocal(RA); Pool.PushRot(2); // Handle singularity. float length = U.Length(); if (length > Settings.LINEAR_SLOP) { U.X *= 1.0f / length; U.Y *= 1.0f / length; } else { U.Set(0.0f, 0.0f); } float crAu = Vec2.Cross(RA, U); float crBu = Vec2.Cross(RB, U); float invMass = InvMassA + InvIA * crAu * crAu + InvMassB + InvIB * crBu * crBu; // Compute the effective mass matrix. Mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (FrequencyHz > 0.0f) { float C = length - Length; // Frequency float omega = 2.0f * MathUtils.PI * FrequencyHz; // Damping coefficient float d = 2.0f * Mass * DampingRatio * omega; // Spring stiffness float k = Mass * omega * omega; // magic formulas float h = data.Step.Dt; Gamma = h * (d + h * k); Gamma = Gamma != 0.0f ? 1.0f / Gamma : 0.0f; Bias = C * h * k * Gamma; invMass += Gamma; Mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; } else { Gamma = 0.0f; Bias = 0.0f; } if (data.Step.WarmStarting) { // Scale the impulse to support a variable time step. Impulse *= data.Step.DtRatio; Vec2 P = Pool.PopVec2(); P.Set(U).MulLocal(Impulse); vA.X -= InvMassA * P.X; vA.Y -= InvMassA * P.Y; wA -= InvIA * Vec2.Cross(RA, P); vB.X += InvMassB * P.X; vB.Y += InvMassB * P.Y; wB += InvIB * Vec2.Cross(RB, P); Pool.PushVec2(1); } else { Impulse = 0.0f; } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; }
public override void InitVelocityConstraints(SolverData data) { IndexA = BodyA.IslandIndex; IndexB = BodyB.IslandIndex; LocalCenterB.Set(BodyB.Sweep.LocalCenter); InvMassB = BodyB.InvMass; InvIB = BodyB.InvI; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Rot qB = Pool.PopRot(); qB.Set(aB); float mass = BodyB.Mass; // Frequency float omega = 2.0f * MathUtils.PI * Frequency; // Damping coefficient float d = 2.0f * mass * DampingRatio * omega; // Spring stiffness float k = mass * (omega * omega); // magic formulas // gamma has units of inverse mass. // beta has units of inverse time. float h = data.Step.Dt; Debug.Assert(d + h * k > Settings.EPSILON); m_gamma = h * (d + h * k); if (m_gamma != 0.0f) { m_gamma = 1.0f / m_gamma; } m_beta = h * k * m_gamma; Vec2 temp = Pool.PopVec2(); // Compute the effective mass matrix. Rot.MulToOutUnsafe(qB, temp.Set(m_localAnchorB).SubLocal(LocalCenterB), RB); // K = [(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 K = Pool.PopMat22(); K.Ex.X = InvMassB + InvIB * RB.Y * RB.Y + m_gamma; K.Ex.Y = (-InvIB) * RB.X * RB.Y; K.Ey.X = K.Ex.Y; K.Ey.Y = InvMassB + InvIB * RB.X * RB.X + m_gamma; K.InvertToOut(m_mass); m_C.Set(cB).AddLocal(RB).SubLocal(m_targetA); m_C.MulLocal(m_beta); // Cheat with some damping wB *= 0.98f; if (data.Step.WarmStarting) { m_impulse.MulLocal(data.Step.DtRatio); vB.X += InvMassB * m_impulse.X; vB.Y += InvMassB * m_impulse.Y; wB += InvIB * Vec2.Cross(RB, m_impulse); } else { m_impulse.SetZero(); } data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(1); Pool.PushMat22(1); Pool.PushRot(1); }
public override bool SolvePositionConstraints(SolverData data) { Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); Vec2 d = Pool.PopVec2(); Vec2 axis = Pool.PopVec2(); Vec2 perp = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); Vec2 C1 = Pool.PopVec2(); Vec3 impulse = Pool.PopVec3(); Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; qA.Set(aA); qB.Set(aB); float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; // Compute fresh Jacobians Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(LocalCenterA), rA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(LocalCenterB), rB); d.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA); Rot.MulToOutUnsafe(qA, LocalXAxisA, axis); float a1 = Vec2.Cross(temp.Set(d).AddLocal(rA), axis); float a2 = Vec2.Cross(rB, axis); Rot.MulToOutUnsafe(qA, LocalYAxisA, perp); float s1 = Vec2.Cross(temp.Set(d).AddLocal(rA), perp); float s2 = Vec2.Cross(rB, perp); C1.X = Vec2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; float linearError = MathUtils.Abs(C1.X); float angularError = MathUtils.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (m_limitEnabled) { float translation = Vec2.Dot(axis, d); if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MAX_LINEAR_CORRECTION, Settings.MAX_LINEAR_CORRECTION); linearError = MathUtils.Max(linearError, MathUtils.Abs(translation)); active = true; } else if (translation <= LowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - LowerTranslation + Settings.LINEAR_SLOP, -Settings.MAX_LINEAR_CORRECTION, 0.0f); linearError = MathUtils.Max(linearError, LowerTranslation - translation); active = true; } else if (translation >= UpperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - UpperTranslation - Settings.LINEAR_SLOP, 0.0f, Settings.MAX_LINEAR_CORRECTION); linearError = MathUtils.Max(linearError, translation - UpperTranslation); active = true; } } if (active) { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k13 = iA * s1 * a1 + iB * s2 * a2; float k22 = iA + iB; if (k22 == 0.0f) { // For fixed rotation k22 = 1.0f; } float k23 = iA * a1 + iB * a2; float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2; Mat33 K = Pool.PopMat33(); K.Ex.Set(k11, k12, k13); K.Ey.Set(k12, k22, k23); K.Ez.Set(k13, k23, k33); Vec3 C = Pool.PopVec3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; K.Solve33ToOut(C.NegateLocal(), impulse); Pool.PushVec3(1); Pool.PushMat33(1); } else { float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; float k12 = iA * s1 + iB * s2; float k22 = iA + iB; if (k22 == 0.0f) { k22 = 1.0f; } Mat22 K = Pool.PopMat22(); K.Ex.Set(k11, k12); K.Ey.Set(k12, k22); // temp is impulse1 K.SolveToOut(C1.NegateLocal(), temp); C1.NegateLocal(); impulse.X = temp.X; impulse.Y = temp.Y; impulse.Z = 0.0f; Pool.PushMat22(1); } float Px = impulse.X * perp.X + impulse.Z * axis.X; float Py = impulse.X * perp.Y + impulse.Z * axis.Y; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; cA.X -= mA * Px; cA.Y -= mA * Py; aA -= iA * LA; cB.X += mB * Px; cB.Y += mB * Py; aB += iB * LB; data.Positions[IndexA].C.Set(cA); data.Positions[IndexA].A = aA; data.Positions[IndexB].C.Set(cB); data.Positions[IndexB].A = aB; Pool.PushVec2(7); Pool.PushVec3(1); Pool.PushRot(2); return(linearError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP); }
// Sequential position solver for position constraints. public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) { float minSeparation = 0.0f; for (int i = 0; i < Count; ++i) { ContactPositionConstraint pc = PositionConstraints[i]; int indexA = pc.IndexA; int indexB = pc.IndexB; Vec2 localCenterA = pc.LocalCenterA; Vec2 localCenterB = pc.LocalCenterB; int pointCount = pc.PointCount; float mA = 0.0f; float iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.InvMassA; iA = pc.InvIA; } float mB = pc.InvMassB; float iB = pc.InvIB; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.InvMassB; iB = pc.InvIB; } Vec2 cA = Positions[indexA].C; float aA = Positions[indexA].A; Vec2 cB = Positions[indexB].C; float aB = Positions[indexB].A; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { xfA.Q.Set(aA); xfB.Q.Set(aB); Rot.MulToOutUnsafe(xfA.Q, localCenterA, xfA.P); xfA.P.NegateLocal().AddLocal(cA); Rot.MulToOutUnsafe(xfB.Q, localCenterB, xfB.P); xfB.P.NegateLocal().AddLocal(cB); PositionSolverManifold psm = psolver; psm.Initialize(pc, xfA, xfB, j); Vec2 normal = psm.Normal; Vec2 point = psm.Point; float separation = psm.Separation; rA.Set(point).SubLocal(cA); rB.Set(point).SubLocal(cB); // Track max constraint error. minSeparation = MathUtils.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.TOI_BAUGARTE * (separation + Settings.LINEAR_SLOP), -Settings.MAX_LINEAR_CORRECTION, 0.0f); // Compute the effective mass. float rnA = Vec2.Cross(rA, normal); float rnB = Vec2.Cross(rB, normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? (-C) / K : 0.0f; P.Set(normal).MulLocal(impulse); cA.SubLocal(temp.Set(P).MulLocal(mA)); aA -= iA * Vec2.Cross(rA, P); cB.AddLocal(temp.Set(P).MulLocal(mB)); aB += iB * Vec2.Cross(rB, P); } Positions[indexA].C.Set(cA); Positions[indexA].A = aA; Positions[indexB].C.Set(cB); Positions[indexB].A = aB; } // We can't expect minSpeparation >= -_linearSlop because we don't // push the separation above -_linearSlop. return(minSeparation >= (-1.5f) * Settings.LINEAR_SLOP); }
// float FindMinSeparation(int* indexA, int* indexB, float t) const public float FindMinSeparation(int[] indexes, float t) { SweepA.GetTransform(xfa, t); SweepB.GetTransform(xfb, t); switch (Type) { case Type.Points: { Rot.MulTransUnsafe(xfa.Q, Axis, axisA); Rot.MulTransUnsafe(xfb.Q, Axis.NegateLocal(), axisB); Axis.NegateLocal(); indexes[0] = ProxyA.GetSupport(axisA); indexes[1] = ProxyB.GetSupport(axisB); localPointA.Set(ProxyA.GetVertex(indexes[0])); localPointB.Set(ProxyB.GetVertex(indexes[1])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); Transform.MulToOutUnsafe(xfb, localPointB, pointB); float separation = Vec2.Dot(pointB.SubLocal(pointA), Axis); return(separation); } case Type.FaceA: { Rot.MulToOutUnsafe(xfa.Q, Axis, normal); Transform.MulToOutUnsafe(xfa, LocalPoint, pointA); Rot.MulTransUnsafe(xfb.Q, normal.NegateLocal(), axisB); normal.NegateLocal(); indexes[0] = -1; indexes[1] = ProxyB.GetSupport(axisB); localPointB.Set(ProxyB.GetVertex(indexes[1])); Transform.MulToOutUnsafe(xfb, localPointB, pointB); float separation = Vec2.Dot(pointB.SubLocal(pointA), normal); return(separation); } case Type.FaceB: { Rot.MulToOutUnsafe(xfb.Q, Axis, normal); Transform.MulToOutUnsafe(xfb, LocalPoint, pointB); Rot.MulTransUnsafe(xfa.Q, normal.NegateLocal(), axisA); normal.NegateLocal(); indexes[1] = -1; indexes[0] = ProxyA.GetSupport(axisA); localPointA.Set(ProxyA.GetVertex(indexes[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); float separation = Vec2.Dot(pointA.SubLocal(pointB), normal); return(separation); } default: Debug.Assert(false); indexes[0] = -1; indexes[1] = -1; return(0f); } }
// TODO_ERIN might not need to return the separation public float Initialize(Distance.SimplexCache cache, Distance.DistanceProxy proxyA, Sweep sweepA, Distance.DistanceProxy proxyB, Sweep sweepB, float t1) { ProxyA = proxyA; ProxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); SweepA = sweepA; SweepB = sweepB; SweepA.GetTransform(xfa, t1); SweepB.GetTransform(xfb, t1); // log.debug("initializing separation.\n" + // "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n" // "distance: "+proxyA. if (count == 1) { Type = Type.Points; /* * Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = * m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2 * pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize(); */ localPointA.Set(ProxyA.GetVertex(cache.IndexA[0])); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); Transform.MulToOutUnsafe(xfb, localPointB, pointB); Axis.Set(pointB).SubLocal(pointA); float s = Axis.Normalize(); return(s); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. Type = Type.FaceB; localPointB1.Set(ProxyB.GetVertex(cache.IndexB[0])); localPointB2.Set(ProxyB.GetVertex(cache.IndexB[1])); temp.Set(localPointB2).SubLocal(localPointB1); Vec2.CrossToOutUnsafe(temp, 1f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfb.Q, Axis, normal); LocalPoint.Set(localPointB1).AddLocal(localPointB2).MulLocal(.5f); Transform.MulToOutUnsafe(xfb, LocalPoint, pointB); localPointA.Set(proxyA.GetVertex(cache.IndexA[0])); Transform.MulToOutUnsafe(xfa, localPointA, pointA); temp.Set(pointA).SubLocal(pointB); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return(s); } else { // Two points on A and one or two points on B. Type = Type.FaceA; localPointA1.Set(ProxyA.GetVertex(cache.IndexA[0])); localPointA2.Set(ProxyA.GetVertex(cache.IndexA[1])); temp.Set(localPointA2).SubLocal(localPointA1); Vec2.CrossToOutUnsafe(temp, 1.0f, Axis); Axis.Normalize(); Rot.MulToOutUnsafe(xfa.Q, Axis, normal); LocalPoint.Set(localPointA1).AddLocal(localPointA2).MulLocal(.5f); Transform.MulToOutUnsafe(xfa, LocalPoint, pointA); localPointB.Set(ProxyB.GetVertex(cache.IndexB[0])); Transform.MulToOutUnsafe(xfb, localPointB, pointB); temp.Set(pointB).SubLocal(pointA); float s = Vec2.Dot(temp, normal); if (s < 0.0f) { Axis.NegateLocal(); s = -s; } return(s); } }
public override bool SolvePositionConstraints(SolverData data) { Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); Vec2 uA = Pool.PopVec2(); Vec2 uB = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); Vec2 PA = Pool.PopVec2(); Vec2 PB = Pool.PopVec2(); Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; qA.Set(aA); qB.Set(aB); Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), rA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), rB); uA.Set(cA).AddLocal(rA).SubLocal(m_groundAnchorA); uB.Set(cB).AddLocal(rB).SubLocal(m_groundAnchorB); float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings.LINEAR_SLOP) { uA.MulLocal(1.0f / lengthA); } else { uA.SetZero(); } if (lengthB > 10.0f * Settings.LINEAR_SLOP) { uB.MulLocal(1.0f / lengthB); } else { uB.SetZero(); } // Compute effective mass. float ruA = Vec2.Cross(rA, uA); float ruB = Vec2.Cross(rB, uB); float mA = InvMassA + InvIA * ruA * ruA; float mB = InvMassB + InvIB * ruB * ruB; float mass = mA + m_ratio * m_ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = m_constant - lengthA - m_ratio * lengthB; float linearError = MathUtils.Abs(C); float impulse = (-mass) * C; PA.Set(uA).MulLocal(-impulse); PB.Set(uB).MulLocal((-m_ratio) * impulse); cA.X += InvMassA * PA.X; cA.Y += InvMassA * PA.Y; aA += InvIA * Vec2.Cross(rA, PA); cB.X += InvMassB * PB.X; cB.Y += InvMassB * PB.Y; aB += InvIB * Vec2.Cross(rB, PB); data.Positions[IndexA].C.Set(cA); data.Positions[IndexA].A = aA; data.Positions[IndexB].C.Set(cB); data.Positions[IndexB].A = aB; Pool.PushRot(2); Pool.PushVec2(7); return(linearError < Settings.LINEAR_SLOP); }
public override void InitVelocityConstraints(SolverData data) { IndexA = BodyA.IslandIndex; IndexB = BodyB.IslandIndex; m_localCenterA.Set(BodyA.Sweep.LocalCenter); m_localCenterB.Set(BodyB.Sweep.LocalCenter); InvMassA = BodyA.InvMass; InvMassB = BodyB.InvMass; InvIA = BodyA.InvI; InvIB = BodyB.InvI; Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 temp = Pool.PopVec2(); qA.Set(aA); qB.Set(aB); // Compute the effective masses. Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(m_localCenterA), RA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(m_localCenterB), RB); m_uA.Set(cA).AddLocal(RA).SubLocal(m_groundAnchorA); m_uB.Set(cB).AddLocal(RB).SubLocal(m_groundAnchorB); float lengthA = m_uA.Length(); float lengthB = m_uB.Length(); if (lengthA > 10f * Settings.LINEAR_SLOP) { m_uA.MulLocal(1.0f / lengthA); } else { m_uA.SetZero(); } if (lengthB > 10f * Settings.LINEAR_SLOP) { m_uB.MulLocal(1.0f / lengthB); } else { m_uB.SetZero(); } // Compute effective mass. float ruA = Vec2.Cross(RA, m_uA); float ruB = Vec2.Cross(RB, m_uB); float mA = InvMassA + InvIA * ruA * ruA; float mB = InvMassB + InvIB * ruB * ruB; m_mass = mA + m_ratio * m_ratio * mB; if (m_mass > 0.0f) { m_mass = 1.0f / m_mass; } if (data.Step.WarmStarting) { // Scale impulses to support variable time steps. m_impulse *= data.Step.DtRatio; // Warm starting. Vec2 PA = Pool.PopVec2(); Vec2 PB = Pool.PopVec2(); PA.Set(m_uA).MulLocal(-m_impulse); PB.Set(m_uB).MulLocal((-m_ratio) * m_impulse); vA.X += InvMassA * PA.X; vA.Y += InvMassA * PA.Y; wA += InvIA * Vec2.Cross(RA, PA); vB.X += InvMassB * PB.X; vB.Y += InvMassB * PB.Y; wB += InvIB * Vec2.Cross(RB, PB); Pool.PushVec2(2); } else { m_impulse = 0.0f; } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(1); Pool.PushRot(2); }
public override void InitVelocityConstraints(SolverData data) { IndexA = BodyA.IslandIndex; IndexB = BodyB.IslandIndex; LocalCenterA.Set(BodyA.Sweep.LocalCenter); LocalCenterB.Set(BodyB.Sweep.LocalCenter); InvMassA = BodyA.InvMass; InvMassB = BodyB.InvMass; InvIA = BodyA.InvI; InvIB = BodyB.InvI; // Vec2 cA = data.positions[m_indexA].c; float aA = data.Positions[IndexA].A; Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; // Vec2 cB = data.positions[m_indexB].c; float aB = data.Positions[IndexB].A; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 temp = Pool.PopVec2(); qA.Set(aA); qB.Set(aB); // Compute the effective masses. Rot.MulToOutUnsafe(qA, temp.Set(LocalAnchorA).SubLocal(LocalCenterA), RA); Rot.MulToOutUnsafe(qB, temp.Set(LocalAnchorB).SubLocal(LocalCenterB), RB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; bool fixedRotation = (iA + iB == 0.0f); Mass.Ex.X = mA + mB + RA.Y * RA.Y * iA + RB.Y * RB.Y * iB; Mass.Ey.X = (-RA.Y) * RA.X * iA - RB.Y * RB.X * iB; Mass.Ez.X = (-RA.Y) * iA - RB.Y * iB; Mass.Ex.Y = Mass.Ey.X; Mass.Ey.Y = mA + mB + RA.X * RA.X * iA + RB.X * RB.X * iB; Mass.Ez.Y = RA.X * iA + RB.X * iB; Mass.Ex.Z = Mass.Ez.X; Mass.Ey.Z = Mass.Ez.Y; Mass.Ez.Z = iA + iB; MotorMass = iA + iB; if (MotorMass > 0.0f) { MotorMass = 1.0f / MotorMass; } if (m_motorEnabled == false || fixedRotation) { MotorImpulse = 0.0f; } if (m_limitEnabled && fixedRotation == false) { float jointAngle = aB - aA - ReferenceAngle; if (MathUtils.Abs(UpperAngle - LowerAngle) < 2.0f * Settings.ANGULAR_SLOP) { LimitState = LimitState.Equal; } else if (jointAngle <= LowerAngle) { if (LimitState != LimitState.AtLower) { Impulse.Z = 0.0f; } LimitState = LimitState.AtLower; } else if (jointAngle >= UpperAngle) { if (LimitState != LimitState.AtUpper) { Impulse.Z = 0.0f; } LimitState = LimitState.AtUpper; } else { LimitState = LimitState.Inactive; Impulse.Z = 0.0f; } } else { LimitState = LimitState.Inactive; } if (data.Step.WarmStarting) { Vec2 P = Pool.PopVec2(); // Scale impulses to support a variable time step. Impulse.X *= data.Step.DtRatio; Impulse.Y *= data.Step.DtRatio; MotorImpulse *= data.Step.DtRatio; P.X = Impulse.X; P.Y = Impulse.Y; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * (Vec2.Cross(RA, P) + MotorImpulse + Impulse.Z); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * (Vec2.Cross(RB, P) + MotorImpulse + Impulse.Z); Pool.PushVec2(1); } else { Impulse.SetZero(); MotorImpulse = 0.0f; } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(1); Pool.PushRot(2); }
public override bool SolvePositionConstraints(SolverData data) { Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); Vec2 cA = data.Positions[IndexA].C; float aA = data.Positions[IndexA].A; Vec2 cB = data.Positions[IndexB].C; float aB = data.Positions[IndexB].A; qA.Set(aA); qB.Set(aB); float angularError = 0.0f; float positionError = 0.0f; bool fixedRotation = (InvIA + InvIB == 0.0f); // Solve angular limit constraint. if (m_limitEnabled && LimitState != LimitState.Inactive && fixedRotation == false) { float angle = aB - aA - ReferenceAngle; float limitImpulse = 0.0f; if (LimitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - LowerAngle, -Settings.MAX_ANGULAR_CORRECTION, Settings.MAX_ANGULAR_CORRECTION); limitImpulse = (-MotorMass) * C; angularError = MathUtils.Abs(C); } else if (LimitState == LimitState.AtLower) { float C = angle - LowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.ANGULAR_SLOP, -Settings.MAX_ANGULAR_CORRECTION, 0.0f); limitImpulse = (-MotorMass) * C; } else if (LimitState == LimitState.AtUpper) { float C = angle - UpperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.ANGULAR_SLOP, 0.0f, Settings.MAX_ANGULAR_CORRECTION); limitImpulse = (-MotorMass) * C; } aA -= InvIA * limitImpulse; aB += InvIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); Vec2 rA = Pool.PopVec2(); Vec2 rB = Pool.PopVec2(); Vec2 C = Pool.PopVec2(); Vec2 impulse = Pool.PopVec2(); Rot.MulToOutUnsafe(qA, C.Set(LocalAnchorA).SubLocal(LocalCenterA), rA); Rot.MulToOutUnsafe(qB, C.Set(LocalAnchorB).SubLocal(LocalCenterB), rB); C.Set(cB).AddLocal(rB).SubLocal(cA).SubLocal(rA); positionError = C.Length(); float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; Mat22 K = Pool.PopMat22(); K.Ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y; K.Ex.Y = (-iA) * rA.X * rA.Y - iB * rB.X * rB.Y; K.Ey.X = K.Ex.Y; K.Ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X; K.SolveToOut(C, impulse); impulse.NegateLocal(); cA.X -= mA * impulse.X; cA.Y -= mA * impulse.Y; aA -= iA * Vec2.Cross(rA, impulse); cB.X += mB * impulse.X; cB.Y += mB * impulse.Y; aB += iB * Vec2.Cross(rB, impulse); Pool.PushVec2(4); Pool.PushMat22(1); } data.Positions[IndexA].C.Set(cA); data.Positions[IndexA].A = aA; data.Positions[IndexB].C.Set(cB); data.Positions[IndexB].A = aB; Pool.PushRot(2); return(positionError <= Settings.LINEAR_SLOP && angularError <= Settings.ANGULAR_SLOP); }
public void GetWorldVectorToOutUnsafe(Vec2 localVector, Vec2 result) { Rot.MulToOutUnsafe(Xf.Q, localVector, result); }
/// <seealso cref="Joint.initVelocityConstraints(TimeStep)"></seealso> public override void InitVelocityConstraints(SolverData data) { IndexA = BodyA.IslandIndex; IndexB = BodyB.IslandIndex; LocalCenterA.Set(BodyA.Sweep.LocalCenter); LocalCenterB.Set(BodyB.Sweep.LocalCenter); InvMassA = BodyA.InvMass; InvMassB = BodyB.InvMass; InvIA = BodyA.InvI; InvIB = BodyB.InvI; float aA = data.Positions[IndexA].A; Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; float aB = data.Positions[IndexB].A; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; Vec2 temp = Pool.PopVec2(); Rot qA = Pool.PopRot(); Rot qB = Pool.PopRot(); qA.Set(aA); qB.Set(aB); // Compute the effective mass matrix. Rot.MulToOutUnsafe(qA, temp.Set(m_localAnchorA).SubLocal(LocalCenterA), RA); Rot.MulToOutUnsafe(qB, temp.Set(m_localAnchorB).SubLocal(LocalCenterB), RB); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB] // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB] // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB] float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; Mat22 K = Pool.PopMat22(); K.Ex.X = mA + mB + iA * RA.Y * RA.Y + iB * RB.Y * RB.Y; K.Ex.Y = (-iA) * RA.X * RA.Y - iB * RB.X * RB.Y; K.Ey.X = K.Ex.Y; K.Ey.Y = mA + mB + iA * RA.X * RA.X + iB * RB.X * RB.X; K.InvertToOut(LinearMass); AngularMass = iA + iB; if (AngularMass > 0.0f) { AngularMass = 1.0f / AngularMass; } if (data.Step.WarmStarting) { // Scale impulses to support a variable time step. m_linearImpulse.MulLocal(data.Step.DtRatio); m_angularImpulse *= data.Step.DtRatio; Vec2 P = Pool.PopVec2(); P.Set(m_linearImpulse); temp.Set(P).MulLocal(mA); vA.SubLocal(temp); wA -= iA * (Vec2.Cross(RA, P) + m_angularImpulse); temp.Set(P).MulLocal(mB); vB.AddLocal(temp); wB += iB * (Vec2.Cross(RB, P) + m_angularImpulse); Pool.PushVec2(1); } else { m_linearImpulse.SetZero(); m_angularImpulse = 0.0f; } data.Velocities[IndexA].V.Set(vA); if (data.Velocities[IndexA].W != wA) { Debug.Assert(data.Velocities[IndexA].W != wA); } data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushRot(2); Pool.PushVec2(1); Pool.PushMat22(1); }