public override void solveVelocityConstraints(SolverData data) { Vec2 vA = data.velocities[m_indexA].v; double wA = data.velocities[m_indexA].w; Vec2 vB = data.velocities[m_indexB].v; double wB = data.velocities[m_indexB].w; double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; Vec2 Cdot1 = pool.popVec2(); Vec2 P = pool.popVec2(); Vec2 temp = pool.popVec2(); if (m_frequencyHz > 0.0d) { double Cdot2 = wB - wA; double impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z); m_impulse.z += impulse2; wA -= iA * impulse2; wB += iB * impulse2; Vec2.crossToOutUnsafe(wB, m_rB, Cdot1); Vec2.crossToOutUnsafe(wA, m_rA, temp); Cdot1.addLocal(vB).subLocal(vA).subLocal(temp); Vec2 impulse1 = P; Mat33.mul22ToOutUnsafe(m_mass, Cdot1, impulse1); impulse1.negateLocal(); m_impulse.x += impulse1.x; m_impulse.y += impulse1.y; vA.x -= mA * P.x; vA.y -= mA * P.y; wA -= iA * Vec2.cross(m_rA, P); vB.x += mB * P.x; vB.y += mB * P.y; wB += iB * Vec2.cross(m_rB, P); } else { Vec2.crossToOutUnsafe(wA, m_rA, temp); Vec2.crossToOutUnsafe(wB, m_rB, Cdot1); Cdot1.addLocal(vB).subLocal(vA).subLocal(temp); double Cdot2 = wB - wA; Vec3 Cdot = pool.popVec3(); Cdot.set(Cdot1.x, Cdot1.y, Cdot2); Vec3 impulse = pool.popVec3(); Mat33.mulToOutUnsafe(m_mass, Cdot, impulse); impulse.negateLocal(); m_impulse.addLocal(impulse); P.set(impulse.x, impulse.y); vA.x -= mA * P.x; vA.y -= mA * P.y; wA -= iA * (Vec2.cross(m_rA, P) + impulse.z); vB.x += mB * P.x; vB.y += mB * P.y; wB += iB * (Vec2.cross(m_rB, P) + impulse.z); pool.pushVec3(2); } // 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(3); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _indexB = BodyB.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _localCenterB = BodyB._sweep.LocalCenter; _invMassA = BodyA._invMass; _invMassB = BodyB._invMass; _invIA = BodyA._invI; _invIB = BodyB._invI; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // 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; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { K.GetInverse22(ref _mass); float invM = iA + iB; float m = invM > 0.0f ? 1.0f / invM : 0.0f; float C = aB - aA - ReferenceAngle; // Frequency float omega = 2.0f * Settings.Pi * FrequencyHz; // Damping coefficient float d = 2.0f * m * DampingRatio * omega; // Spring stiffness float k = m * 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; invM += _gamma; _mass.ez.Z = invM != 0.0f ? 1.0f / invM : 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = new Vector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _impulse.Z); } else { _impulse = Vector3.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
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[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; double aB = data.positions[m_indexB].a; qA.set(aA); qB.set(aB); double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; // Compute fresh Jacobians Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); Rot.mulToOutUnsafe(qA, m_localXAxisA, axis); double a1 = Vec2.cross(temp.set(d).addLocal(rA), axis); double a2 = Vec2.cross(rB, axis); Rot.mulToOutUnsafe(qA, m_localYAxisA, perp); double s1 = Vec2.cross(temp.set(d).addLocal(rA), perp); double s2 = Vec2.cross(rB, perp); C1.x = Vec2.dot(perp, d); C1.y = aB - aA - m_referenceAngle; double linearError = MathUtils.abs(C1.x); double angularError = MathUtils.abs(C1.y); bool active = false; double C2 = 0.0d; if (m_enableLimit) { double translation = Vec2.dot(axis, d); if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0d * Settings.linearSlop) { // Prevent large angular corrections C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, Settings.maxLinearCorrection); linearError = MathUtils.max(linearError, MathUtils.abs(translation)); active = true; } else if (translation <= m_lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0d); linearError = MathUtils.max(linearError, m_lowerTranslation - translation); active = true; } else if (translation >= m_upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0d, Settings.maxLinearCorrection); linearError = MathUtils.max(linearError, translation - m_upperTranslation); active = true; } } if (active) { double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; double k12 = iA * s1 + iB * s2; double k13 = iA * s1 * a1 + iB * s2 * a2; double k22 = iA + iB; if (k22 == 0.0d) { // For fixed rotation k22 = 1.0d; } double k23 = iA * a1 + iB * a2; double 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 { double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; double k12 = iA * s1 + iB * s2; double k22 = iA + iB; if (k22 == 0.0d) { k22 = 1.0d; } 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.0d; pool.pushMat22(1); } double Px = impulse.x * perp.x + impulse.z * axis.x; double Py = impulse.x * perp.y + impulse.z * axis.y; double LA = impulse.x * s1 + impulse.y + impulse.z * a1; double 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[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(7); pool.pushVec3(1); pool.pushRot(2); return(linearError <= Settings.linearSlop && angularError <= Settings.angularSlop); }
/// <summary>Multiply a matrix times a vector.</summary> public static Vector2 Mul22(Mat33 a, Vector2 v) => new Vector2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y);
/// <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); }
/// <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); }
// / Multiply a matrix times a vector. public static Vec3 mul(Mat33 A, Vec3 v) { return(new Vec3(v.x * A.ex.x + v.y * A.ey.x + v.z + A.ez.x, v.x * A.ex.y + v.y * A.ey.y + v.z * A.ez.y, v.x * A.ex.z + v.y * A.ey.z + v.z * A.ez.z)); }
public static Vec2 mul22(Mat33 A, Vec2 v) { return(new Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y)); }
/// <summary>Multiply a matrix times a vector.</summary> public static Vector3 Mul(Mat33 a, Vector3 v) { return(v.X * a.ex + v.Y * a.ey + v.Z * a.ez); }
/// <summary>Multiply a matrix times a vector.</summary> public static Vector2 Mul22(Mat33 a, Vector2 v) { return(new Vector2(a.ex.X * v.X + a.ey.X * v.Y, a.ex.Y * v.X + a.ey.Y * v.Y)); }
internal override bool SolvePositionConstraints() { Vector2 cA = _bodyA.GetRelativePoint(m_LocalCenterA); Vector2 cA0 = cA; float aA = _bodyA.rotation * Mathf.Deg2Rad; Vector2 cB = _bodyB.GetRelativePoint(m_LocalCenterB); Vector2 cB0 = cB; float aB = _bodyB.rotation * Mathf.Deg2Rad; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = m_InvMassA, mB = m_InvMassB; float iA = m_InvIA, iB = m_InvIB; // Compute fresh Jacobians Vector2 rA = MathUtils.Mul(qA, _localAnchorA - m_LocalCenterA); Vector2 rB = MathUtils.Mul(qB, _localAnchorB - m_LocalCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.Mul(qA, localXAxisA); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(rB, axis); Vector2 perp = MathUtils.Mul(qA, m_LocalYAxisA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.x = Vector2.Dot(perp, d); C1.y = aB - aA - referenceAngle; float linearError = Mathf.Abs(C1.x); float angularError = Mathf.Abs(C1.y); bool active = false; float C2 = 0.0f; if (m_EnableLimit) { float translation = Vector2.Dot(axis, d); m_Translation = translation; if (Mathf.Abs(m_UpperTranslation - m_LowerTranslation) < 2.0f * Constants.linearSlop) { // Prevent large angular corrections C2 = Mathf.Clamp(translation, -Constants.maxLinearCorrection, Constants.maxLinearCorrection); linearError = Mathf.Max(linearError, Mathf.Abs(translation)); active = true; } else if (translation <= m_LowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = Mathf.Clamp(translation - m_LowerTranslation + Constants.linearSlop, -Constants.maxLinearCorrection, 0.0f); linearError = Mathf.Max(linearError, m_LowerTranslation - translation); active = true; } else if (translation >= m_UpperTranslation) { // Prevent large linear corrections and allow some slop. C2 = Mathf.Clamp(translation - m_UpperTranslation - Constants.linearSlop, 0.0f, Constants.maxLinearCorrection); linearError = Mathf.Max(linearError, translation - m_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 = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.x = C1.x; C.y = C1.y; C.z = C2; impulse = K.Solve33(-C); } 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 = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.x = impulse1.x; impulse.y = impulse1.y; impulse.z = 0.0f; } Vector2 P = impulse.x * perp + impulse.z * axis; float LA = impulse.x * s1 + impulse.y + impulse.z * a1; float LB = impulse.x * s2 + impulse.y + impulse.z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; if (!_bodyA.isKinematic) { var dcA = cA0 - cA; _bodyA.position -= dcA; if (!_bodyA.fixedAngle) { _bodyA.rotation = aA * Mathf.Rad2Deg; } } if (!_bodyB.isKinematic) { var dcB = cB0 - cB; _bodyB.position -= dcB; if (!_bodyB.fixedAngle) { _bodyB.rotation = aB * Mathf.Rad2Deg; } } return(linearError < Constants.linearSlop && angularError < Constants.angularSlop); }
/// Multiply a matrix times a vector. public static Vector3 Mul(Mat33 A, Vector3 v) { return(v.x * A.ex + v.y * A.ey + v.z * A.ez); }
public override bool solvePositionConstraints(SolverData data) { Vec2 cA = data.positions[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 cB = data.positions[m_indexB].c; double 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); double mA = m_invMassA, mB = m_invMassB; double iA = m_invIA, iB = m_invIB; Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB); double 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 (m_frequencyHz > 0.0d) { C1.set(cB).addLocal(rB).subLocal(cA).subLocal(rA); positionError = C1.length(); angularError = 0.0d; 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); double 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); pool.pushVec3(2); } // 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.linearSlop && angularError <= Settings.angularSlop); }
internal override bool SolvePositionConstraints(ref SolverData data) { TSVector2 cA = data.positions[_indexA].c; FP aA = data.positions[_indexA].a; TSVector2 cB = data.positions[_indexB].c; FP aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FP mA = _invMassA, mB = _invMassB; FP iA = _invIA, iB = _invIB; TSVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); TSVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FP positionError, angularError; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { TSVector2 C1 = cB + rB - cA - rA; positionError = C1.magnitude; angularError = 0.0f; TSVector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { TSVector2 C1 = cB + rB - cA - rA; FP C2 = aB - aA - ReferenceAngle; positionError = C1.magnitude; angularError = FP.Abs(C2); TSVector C = new TSVector(C1.x, C1.y, C2); TSVector impulse = K.Solve33(C) * -1; TSVector2 P = new TSVector2(impulse.x, impulse.y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
public static void mul22ToOutUnsafe(Mat33 A, Vec2 v, Vec2 out_) { out_.y = A.ex.y * v.x + A.ey.y * v.y; out_.x = A.ex.x * v.x + A.ey.x * v.y; }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.Mul(qA, LocalXAxis); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(rB, axis); Vector2 perp = MathUtils.Mul(qA, _localYAxisA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.X = Vector2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; float linearError = Math.Abs(C1.X); float angularError = Math.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (_enableLimit) { float translation = Vector2.Dot(axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Max(linearError, Math.Abs(translation)); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = Math.Max(linearError, _lowerTranslation - translation); active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = Math.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 = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; impulse = K.Solve33(-C); } 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 = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * perp + impulse.Z * axis; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
public static void mulToOutUnsafe(Mat33 A, Vec3 v, Vec3 out_) { out_.x = v.x * A.ex.x + v.y * A.ey.x + v.z * A.ez.x; out_.y = v.x * A.ex.y + v.y * A.ey.y + v.z * A.ez.y; out_.z = v.x * A.ex.z + v.y * A.ey.z + v.z * A.ez.z; }
/// <seealso cref="Joint.solveVelocityConstraints(TimeStep)"></seealso> public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[m_indexA].V; float wA = data.Velocities[m_indexA].W; Vec2 vB = data.Velocities[m_indexB].V; float wB = data.Velocities[m_indexB].W; float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Vec2 Cdot1 = Pool.PopVec2(); Vec2 P = Pool.PopVec2(); Vec2 temp = Pool.PopVec2(); if (Frequency > 0.0f) { float Cdot2 = wB - wA; float impulse2 = (-m_mass.Ez.Z) * (Cdot2 + m_bias + m_gamma * m_impulse.Z); m_impulse.Z += impulse2; wA -= iA * impulse2; wB += iB * impulse2; Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1); Vec2.CrossToOutUnsafe(wA, m_rA, temp); Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp); Vec2 impulse1 = P; Mat33.Mul22ToOutUnsafe(m_mass, Cdot1, impulse1); impulse1.NegateLocal(); m_impulse.X += impulse1.X; m_impulse.Y += impulse1.Y; vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * Vec2.Cross(m_rA, P); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * Vec2.Cross(m_rB, P); } else { Vec2.CrossToOutUnsafe(wA, m_rA, temp); Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1); Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp); float Cdot2 = wB - wA; Vec3 Cdot = Pool.PopVec3(); Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2); Vec3 impulse = Pool.PopVec3(); Mat33.MulToOutUnsafe(m_mass, Cdot, impulse); impulse.NegateLocal(); m_impulse.AddLocal(impulse); P.Set(impulse.X, impulse.Y); vA.X -= mA * P.X; vA.Y -= mA * P.Y; wA -= iA * (Vec2.Cross(m_rA, P) + impulse.Z); vB.X += mB * P.X; vB.Y += mB * P.Y; wB += iB * (Vec2.Cross(m_rB, P) + impulse.Z); Pool.PushVec3(2); } 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(3); }
/// <summary> /// Inits the velocity constraints using the specified step /// </summary> /// <param name="step">The step</param> internal override void InitVelocityConstraints(TimeStep step) { Body body1 = Body1; Body body2 = Body2; if (IsMotorEnabled || IsLimitEnabled) { // You cannot create a rotation limit between bodies that // both have fixed rotation. Box2DxDebug.Assert(body1.InvI > 0.0f || body2.InvI > 0.0f); } // Compute the effective mass matrix. Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter()); Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter()); // J = [-I -r1_skew I r2_skew] // [ 0 -1 0 1] // r_skew = [-ry; rx] // Matlab // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2] // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2] // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2] float m1 = body1.InvMass, m2 = body2.InvMass; float i1 = body1.InvI, i2 = body2.InvI; float col1X = m1 + m2 + mulR1.Y * mulR1.Y * i1 + mulR2.Y * mulR2.Y * i2; float col2X = -mulR1.Y * mulR1.X * i1 - mulR2.Y * mulR2.X * i2; float col3X = -mulR1.Y * i1 - mulR2.Y * i2; float col1Y = Mass.Col2.X; float col2Y = m1 + m2 + mulR1.X * mulR1.X * i1 + mulR2.X * mulR2.X * i2; float col3Y = mulR1.X * i1 + mulR2.X * i2; float col1Z = Mass.Col3.X; float col2Z = Mass.Col3.Y; float col3Z = i1 + i2; Mass = new Mat33(new Vec3(col1X, col1Y, col1Z), new Vec3(col2X, col2Y, col2Z), new Vec3(col3X, col3Y, col3Z)); /* * _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2; * _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2; * _mass.Col3.X = -r1.Y * i1 - r2.Y * i2; * _mass.Col1.Y = _mass.Col2.X; * _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2; * _mass.Col3.Y = r1.X * i1 + r2.X * i2; * _mass.Col1.Z = _mass.Col3.X; * _mass.Col2.Z = _mass.Col3.Y; * _mass.Col3.Z = i1 + i2; */ MotorMass = 1.0f / (i1 + i2); if (IsMotorEnabled == false) { MotorTorque = 0.0f; } if (IsLimitEnabled) { float jointAngle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle; if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.AngularSlop) { State = LimitState.EqualLimits; } else if (jointAngle <= LowerLimit) { if (State != LimitState.AtLowerLimit) { Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f); } State = LimitState.AtLowerLimit; } else if (jointAngle >= UpperLimit) { if (State != LimitState.AtUpperLimit) { Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f); } State = LimitState.AtUpperLimit; } else { State = LimitState.InactiveLimit; Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f); } } else { State = LimitState.InactiveLimit; } if (step.WarmStarting) { // Scale impulses to support a variable time step. Impulse *= step.DtRatio; MotorTorque *= step.DtRatio; Vec2 p = new Vec2(Impulse.X, Impulse.Y); body1.LinearVelocity -= m1 * p; body1.AngularVelocity -= i1 * (Vec2.Cross(mulR1, p) + MotorTorque + Impulse.Z); body2.LinearVelocity += m2 * p; body2.AngularVelocity += i2 * (Vec2.Cross(mulR2, p) + MotorTorque + Impulse.Z); } else { Impulse.SetZero(); MotorTorque = 0.0f; } }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); float positionError, angularError; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse; if (K.ez.Z <= 0.0f) { Vector2 impulse2 = -K.Solve22(C1); impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); } else { impulse = -K.Solve33(C); } Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(ref rB, ref P) + impulse.Z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
public override void SolveVelocityConstraints(SolverData data) { Vec2 vA = data.Velocities[IndexA].V; float wA = data.Velocities[IndexA].W; Vec2 vB = data.Velocities[IndexB].V; float wB = data.Velocities[IndexB].W; float mA = InvMassA, mB = InvMassB; float iA = InvIA, iB = InvIB; Vec2 temp = Pool.PopVec2(); // Solve linear motor constraint. if (m_motorEnabled && LimitState != LimitState.Equal) { temp.Set(vB).SubLocal(vA); float Cdot = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA; float impulse = MotorMass * (m_motorSpeed - Cdot); float oldImpulse = MotorImpulse; float maxImpulse = data.Step.Dt * m_maxMotorForce; MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse); impulse = MotorImpulse - oldImpulse; Vec2 P = Pool.PopVec2(); P.Set(Axis).MulLocal(impulse); float LA = impulse * A1; float LB = impulse * 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); } Vec2 Cdot1 = Pool.PopVec2(); temp.Set(vB).SubLocal(vA); Cdot1.X = Vec2.Dot(Perp, temp) + S2 * wB - S1 * wA; Cdot1.Y = wB - wA; // System.out.println(Cdot1); if (m_limitEnabled && LimitState != LimitState.Inactive) { // Solve prismatic and limit constraint in block form. float Cdot2; temp.Set(vB).SubLocal(vA); Cdot2 = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA; Vec3 Cdot = Pool.PopVec3(); Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2); Cdot.NegateLocal(); Vec3 f1 = Pool.PopVec3(); Vec3 df = Pool.PopVec3(); f1.Set(Impulse); K.Solve33ToOut(Cdot.NegateLocal(), df); //Cdot.negateLocal(); not used anymore Impulse.AddLocal(df); if (LimitState == LimitState.AtLower) { Impulse.Z = MathUtils.Max(Impulse.Z, 0.0f); } else if (LimitState == LimitState.AtUpper) { Impulse.Z = MathUtils.Min(Impulse.Z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + // f1(1:2) Vec2 b = Pool.PopVec2(); Vec2 f2r = Pool.PopVec2(); temp.Set(K.Ez.X, K.Ez.Y).MulLocal(Impulse.Z - f1.Z); b.Set(Cdot1).NegateLocal().SubLocal(temp); temp.Set(f1.X, f1.Y); K.Solve22ToOut(b, f2r); f2r.AddLocal(temp); Impulse.X = f2r.X; Impulse.Y = f2r.Y; df.Set(Impulse).SubLocal(f1); Vec2 P = Pool.PopVec2(); temp.Set(Axis).MulLocal(df.Z); P.Set(Perp).MulLocal(df.X).AddLocal(temp); float LA = df.X * S1 + df.Y + df.Z * A1; float LB = df.X * S2 + df.Y + df.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(3); Pool.PushVec3(3); } else { // Limit is inactive, just solve the prismatic constraint in block form. Vec2 df = Pool.PopVec2(); K.Solve22ToOut(Cdot1.NegateLocal(), df); Cdot1.NegateLocal(); Impulse.X += df.X; Impulse.Y += df.Y; Vec2 P = Pool.PopVec2(); P.Set(Perp).MulLocal(df.X); float LA = df.X * S1 + df.Y; float LB = df.X * S2 + df.Y; 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; Vec2 Cdot10 = Pool.PopVec2(); Cdot10.Set(Cdot1); Cdot1.X = Vec2.Dot(Perp, temp.Set(vB).SubLocal(vA)) + S2 * wB - S1 * wA; Cdot1.Y = wB - wA; if (MathUtils.Abs(Cdot1.X) > 0.01f || MathUtils.Abs(Cdot1.Y) > 0.01f) { // djm note: what's happening here? Mat33.Mul22ToOutUnsafe(K, df, temp); Cdot1.X += 0.0f; } Pool.PushVec2(3); } data.Velocities[IndexA].V.Set(vA); data.Velocities[IndexA].W = wA; data.Velocities[IndexB].V.Set(vB); data.Velocities[IndexB].W = wB; Pool.PushVec2(2); }
/// <summary>Multiply a matrix times a vector.</summary> public static Vector3 Mul(Mat33 a, Vector3 v) => v.X * a.Ex + v.Y * a.Ey + v.Z * a.Ez;
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); }
internal override bool SolvePositionConstraints(ref SolverData data) { var cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; var cB = data.Positions[_indexB].C; float aB = data.Positions[_indexB].A; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; var rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); var rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); float positionError, angularError; var K = new Mat33(); 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 (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse = -K.Solve33(C); Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); } data.Positions[_indexA].C = cA; data.Positions[_indexA].A = aA; data.Positions[_indexB].C = cB; data.Positions[_indexB].A = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
/// Multiply a matrix times a vector. public static Vector3 Mul(Mat33 A, Vector3 v) { return(v.X * A.ex + v.Y * A.ey + v.Z * A.ez); }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute fresh Jacobians Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d = cB + rB - cA - rA; Vector2 axis = MathUtils.Mul(qA, LocalXAxis); float a1 = MathUtils.Cross(d + rA, axis); float a2 = MathUtils.Cross(rB, axis); Vector2 perp = MathUtils.Mul(qA, _localYAxisA); float s1 = MathUtils.Cross(d + rA, perp); float s2 = MathUtils.Cross(rB, perp); Vector3 impulse; Vector2 C1 = new Vector2(); C1.X = Vector2.Dot(perp, d); C1.Y = aB - aA - ReferenceAngle; float linearError = Math.Abs(C1.X); float angularError = Math.Abs(C1.Y); bool active = false; float C2 = 0.0f; if (_enableLimit) { float translation = Vector2.Dot(axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Math.Max(linearError, Math.Abs(translation)); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = Math.Max(linearError, _lowerTranslation - translation); active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = Math.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 = new Mat33(); K.ex = new Vector3(k11, k12, k13); K.ey = new Vector3(k12, k22, k23); K.ez = new Vector3(k13, k23, k33); Vector3 C = new Vector3(); C.X = C1.X; C.Y = C1.Y; C.Z = C2; impulse = K.Solve33(-C); } 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 = new Mat22(); K.ex = new Vector2(k11, k12); K.ey = new Vector2(k12, k22); Vector2 impulse1 = K.Solve(-C1); impulse = new Vector3(); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vector2 P = impulse.X * perp + impulse.Z * axis; float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1; float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2; cA -= mA * P; aA -= iA * LA; cB += mB * P; aB += iB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
/// Multiply a matrix times a vector. public static Vector2 Mul22(Mat33 A, Vector2 v) { return(new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y)); }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); float positionError, angularError; Mat33 K = new Mat33(); 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 (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse = -K.Solve33(C); Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(rA, P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(rB, P) + impulse.Z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop; }
public override void initVelocityConstraints(SolverData data) { m_indexA = m_bodyA.m_islandIndex; m_indexB = m_bodyB.m_islandIndex; m_localCenterA.set(m_bodyA.m_sweep.localCenter); m_localCenterB.set(m_bodyB.m_sweep.localCenter); m_invMassA = m_bodyA.m_invMass; m_invMassB = m_bodyB.m_invMass; m_invIA = m_bodyA.m_invI; m_invIB = m_bodyB.m_invI; // Vec2 cA = data.positions[m_indexA].c; double aA = data.positions[m_indexA].a; Vec2 vA = data.velocities[m_indexA].v; double wA = data.velocities[m_indexA].w; // Vec2 cB = data.positions[m_indexB].c; double aB = data.positions[m_indexB].a; Vec2 vB = data.velocities[m_indexB].v; double 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(m_localAnchorA).subLocal(m_localCenterA), m_rA); Rot.mulToOutUnsafe(qB, temp.set(m_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] double mA = m_invMassA, mB = m_invMassB; double 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 (m_frequencyHz > 0.0d) { K.getInverse22(m_mass); double invM = iA + iB; double m = invM > 0.0d ? 1.0d / invM : 0.0d; double C = aB - aA - m_referenceAngle; // Frequency double omega = 2.0d * MathUtils.PI * m_frequencyHz; // Damping coefficient double d = 2.0d * m * m_dampingRatio * omega; // Spring stiffness double k = m * omega * omega; // magic formulas double h = data.step.dt; m_gamma = h * (d + h * k); m_gamma = m_gamma != 0.0d ? 1.0d / m_gamma : 0.0d; m_bias = C * h * k * m_gamma; invM += m_gamma; m_mass.ez.z = invM != 0.0d ? 1.0d / invM : 0.0d; } else { K.getSymInverse33(m_mass); m_gamma = 0.0d; m_bias = 0.0d; } 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); }