/// <summary> /// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors. /// This requires two ground anchors, /// two dynamic body anchor points, max lengths for each side, /// and a pulley ratio. /// </summary> /// <param name="bA">The first body.</param> /// <param name="bB">The second body.</param> /// <param name="groundA">The ground anchor for the first body.</param> /// <param name="groundB">The ground anchor for the second body.</param> /// <param name="anchorA">The first body anchor.</param> /// <param name="anchorB">The second body anchor.</param> /// <param name="ratio">The ratio.</param> public FSPulleyJoint(FSBody bA, FSBody bB, FVector2 groundA, FVector2 groundB, FVector2 anchorA, FVector2 anchorB, float ratio) : base(bA, bB) { JointType = JointType.Pulley; GroundAnchorA = groundA; GroundAnchorB = groundB; LocalAnchorA = anchorA; LocalAnchorB = anchorB; Debug.Assert(ratio != 0.0f); Debug.Assert(ratio > FSSettings.Epsilon); Ratio = ratio; FVector2 dA = BodyA.GetWorldPoint(anchorA) - groundA; LengthA = dA.Length(); FVector2 dB = BodyB.GetWorldPoint(anchorB) - groundB; LengthB = dB.Length(); m_constant = LengthA + ratio * LengthB; _impulse = 0.0f; }
internal override void SolveVelocityConstraints(ref SolverData data) { FVector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; // Cdot = v + cross(w, r) FVector2 Cdot = vB + MathUtils.Cross(wB, _rB); FVector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); FVector2 oldImpulse = _impulse; _impulse += impulse; float maxImpulse = data.step.dt * MaxForce; if (_impulse.LengthSquared() > maxImpulse * maxImpulse) { _impulse *= maxImpulse / _impulse.Length(); } impulse = _impulse - oldImpulse; vB += _invMassB * impulse; wB += _invIB * MathUtils.Cross(_rB, impulse); data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
public PolyNode GetRightestConnection(PolyNode incoming) { if (NConnected == 0) { Debug.Assert(false); // This means the connection graph is inconsistent } if (NConnected == 1) { //b2Assert(false); // Because of the possibility of collapsing nearby points, // we may end up with "spider legs" dangling off of a region. // The correct behavior here is to turn around. return(incoming); } FVector2 inDir = Position - incoming.Position; float inLength = inDir.Length(); inDir.Normalize(); Debug.Assert(inLength > Settings.Epsilon); PolyNode result = null; for (int i = 0; i < NConnected; ++i) { if (Connected[i] == incoming) { continue; } FVector2 testDir = Connected[i].Position - Position; float testLengthSqr = testDir.LengthSquared(); testDir.Normalize(); Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon); float myCos = FVector2.Dot(inDir, testDir); float mySin = MathUtils.Cross(inDir, testDir); if (result != null) { FVector2 resultDir = result.Position - Position; resultDir.Normalize(); float resCos = FVector2.Dot(inDir, resultDir); float resSin = MathUtils.Cross(inDir, resultDir); if (IsRighter(mySin, myCos, resSin, resCos)) { result = Connected[i]; } } else { result = Connected[i]; } } Debug.Assert(result != null); return(result); }
internal override bool SolvePositionConstraints(ref SolverData data) { if (Frequency > 0.0f) { // There is no position correction for soft distance constraints. return(true); } FSBody b1 = BodyA; FSBody b2 = BodyB; Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter); FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter); FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; float length = d.Length(); if (length < MaxLength && length > MinLength) { return(true); } if (length == 0.0f) { return(true); } d /= length; float C = length - MaxLength; C = MathUtils.Clamp(C, -FSSettings.MaxLinearCorrection, FSSettings.MaxLinearCorrection); float impulse = -_mass * C; _u = d; FVector2 P = impulse * _u; b1.Sweep.C -= b1.InvMass * P; b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, P); b2.Sweep.C += b2.InvMass * P; b2.Sweep.A += b2.InvI * MathUtils.Cross(r2, P); b1.SynchronizeTransform(); b2.SynchronizeTransform(); return(Math.Abs(C) < FSSettings.LinearSlop); }
public FSRopeJoint(FSBody bodyA, FSBody bodyB, FVector2 localAnchorA, FVector2 localAnchorB) : base(bodyA, bodyB) { JointType = JointType.Rope; LocalAnchorA = localAnchorA; LocalAnchorB = localAnchorB; //FPE: Setting default MaxLength FVector2 d = WorldAnchorB - WorldAnchorA; MaxLength = d.Length(); }
/// <summary> /// This requires defining an /// anchor point on both bodies and the non-zero length of the /// distance joint. If you don't supply a length, the local anchor points /// is used so that the initial configuration can violate the constraint /// slightly. This helps when saving and loading a game. /// @warning Do not use a zero or short length. /// </summary> /// <param name="bodyA">The first body</param> /// <param name="bodyB">The second body</param> /// <param name="localAnchorA">The first body anchor</param> /// <param name="localAnchorB">The second body anchor</param> public DistanceJoint(Body bodyA, Body bodyB, FVector2 localAnchorA, FVector2 localAnchorB) : base(bodyA, bodyB) { JointType = JointType.Distance; LocalAnchorA = localAnchorA; LocalAnchorB = localAnchorB; FVector2 d = WorldAnchorB - WorldAnchorA; Length = d.Length(); }
/// <summary> /// tests if ray intersects AABB /// </summary> /// <param name="aabb"></param> /// <returns></returns> public static bool RayCastAABB(AABB aabb, FVector2 p1, FVector2 p2) { AABB segmentAABB = new AABB(); { FVector2.Min(ref p1, ref p2, out segmentAABB.LowerBound); FVector2.Max(ref p1, ref p2, out segmentAABB.UpperBound); } if (!AABB.TestOverlap(aabb, segmentAABB)) { return(false); } FVector2 rayDir = p2 - p1; FVector2 rayPos = p1; FVector2 norm = new FVector2(-rayDir.Y, rayDir.X); //normal to ray if (norm.Length() == 0.0) { return(true); //if ray is just a point, return true (iff point is within aabb, as tested earlier) } norm.Normalize(); float dPos = FVector2.Dot(rayPos, norm); FVector2[] verts = aabb.GetVertices(); float d0 = FVector2.Dot(verts[0], norm) - dPos; for (int i = 1; i < 4; i++) { float d = FVector2.Dot(verts[i], norm) - dPos; if (Math.Sign(d) != Math.Sign(d0)) { //return true if the ray splits the vertices (ie: sign of dot products with normal are not all same) return(true); } } return(false); }
internal override bool SolvePositionConstraints(ref SolverData data) { if (Frequency > 0.0f) { // There is no position correction for soft distance constraints. return(true); } FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); FVector2 u = cB + rB - cA - rA; float length = u.Length(); u.Normalize(); float C = length - Length; C = MathUtils.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); float impulse = -_mass * C; FVector2 P = impulse * u; cA -= m_invMassA * P; aA -= m_invIA * MathUtils.Cross(rA, P); cB += m_invMassB * P; aB += m_invIB * MathUtils.Cross(rB, P); data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(Math.Abs(C) < Settings.LinearSlop); }
internal override void SolveVelocityConstraints(ref SolverData data) { FSBody b1 = BodyA; FSBody b2 = BodyB; Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter); FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter); FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1; float length = d.Length(); if (length < MaxLength && length > MinLength) { return; } // Cdot = dot(u, v + cross(w, r)) FVector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1); FVector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2); float Cdot = FVector2.Dot(_u, v2 - v1); float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); _impulse += impulse; FVector2 P = impulse * _u; b1.LinearVelocityInternal -= b1.InvMass * P; b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); b2.LinearVelocityInternal += b2.InvMass * P; b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P); }
internal override bool SolvePositionConstraints(ref SolverData data) { FVector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; FVector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); FVector2 u = cB + rB - cA - rA; float length = u.Length(); u.Normalize(); float C = length - MaxLength; C = MathUtils.Clamp(C, 0.0f, FSSettings.MaxLinearCorrection); float impulse = -_mass * C; FVector2 P = impulse * u; cA -= _invMassA * P; aA -= _invIA * MathUtils.Cross(rA, P); cB += _invMassB * P; aB += _invIB * MathUtils.Cross(rB, P); data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(length - MaxLength < FSSettings.LinearSlop); }
internal override void InitVelocityConstraints(ref SolverData data) { Body b1 = BodyA; Body b2 = BodyB; Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); // Compute the effective mass matrix. FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter); FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter); _u = b2.Sweep.C + r2 - b1.Sweep.C - r1; // Handle singularity. float length = _u.Length(); if (length < MaxLength && length > MinLength) { return; } if (length > Settings.LinearSlop) { _u *= 1.0f / length; } else { _u = FVector2.Zero; } float cr1u = MathUtils.Cross(r1, _u); float cr2u = MathUtils.Cross(r2, _u); float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u; Debug.Assert(invMass > Settings.Epsilon); _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (Frequency > 0.0f) { float C = length - MaxLength; // Frequency float omega = 2.0f * Settings.Pi * Frequency; // Damping coefficient float d = 2.0f * _mass * DampingRatio * omega; // Spring stiffness float k = _mass * omega * omega; // magic formulas _gamma = data.step.dt * (d + data.step.dt * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * data.step.dt * k * _gamma; _mass = invMass + _gamma; _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; } if (Settings.EnableWarmstarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; FVector2 P = _impulse * _u; b1.LinearVelocityInternal -= b1.InvMass * P; b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); b2.LinearVelocityInternal += b2.InvMass * P; b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P); } else { _impulse = 0.0f; } }
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; FVector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; FVector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; FVector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; FVector2 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); _u = cB + _rB - cA - _rA; _length = _u.Length(); float C = _length - MaxLength; if (C > 0.0f) { _state = LimitState.AtUpper; } else { _state = LimitState.Inactive; } if (_length > Settings.LinearSlop) { _u *= 1.0f / _length; } else { _u = FVector2.Zero; _mass = 0.0f; _impulse = 0.0f; return; } // Compute effective mass. float crA = MathUtils.Cross(_rA, _u); float crB = MathUtils.Cross(_rB, _u); float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB; _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (Settings.EnableWarmstarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; FVector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(_rA, P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(_rB, P); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override bool SolvePositionConstraints(ref SolverData data) { FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); // Get the pulley axes. FVector2 uA = cA + rA - GroundAnchorA; FVector2 uB = cB + rB - GroundAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * FSSettings.LinearSlop) { uA *= 1.0f / lengthA; } else { uA = FVector2.Zero; } if (lengthB > 10.0f * FSSettings.LinearSlop) { uB *= 1.0f / lengthB; } else { uB = FVector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(rA, uA); float ruB = MathUtils.Cross(rB, uB); float mA = m_invMassA + m_invIA * ruA * ruA; float mB = m_invMassB + m_invIB * ruB * ruB; float mass = mA + Ratio * Ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = m_constant - lengthA - Ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; FVector2 PA = -impulse * uA; FVector2 PB = -Ratio * impulse * uB; cA += m_invMassA * PA; aA += m_invIA * MathUtils.Cross(rA, PA); cB += m_invMassB * PB; aB += m_invIB * MathUtils.Cross(rB, PB); data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(linearError < FSSettings.LinearSlop); }
internal override void InitVelocityConstraints(ref SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_localCenterA = BodyA.Sweep.LocalCenter; m_localCenterB = BodyB.Sweep.LocalCenter; m_invMassA = BodyA.InvMass; m_invMassB = BodyB.InvMass; m_invIA = BodyA.InvI; m_invIB = BodyB.InvI; FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; FVector2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); m_rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); m_rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); // Get the pulley axes. m_uA = cA + m_rA - GroundAnchorA; m_uB = cB + m_rB - GroundAnchorB; float lengthA = m_uA.Length(); float lengthB = m_uB.Length(); if (lengthA > 10.0f * FSSettings.LinearSlop) { m_uA *= 1.0f / lengthA; } else { m_uA = FVector2.Zero; } if (lengthB > 10.0f * FSSettings.LinearSlop) { m_uB *= 1.0f / lengthB; } else { m_uB = FVector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(m_rA, m_uA); float ruB = MathUtils.Cross(m_rB, m_uB); float mA = m_invMassA + m_invIA * ruA * ruA; float mB = m_invMassB + m_invIB * ruB * ruB; m_mass = mA + Ratio * Ratio * mB; if (m_mass > 0.0f) { m_mass = 1.0f / m_mass; } if (FSSettings.EnableWarmstarting) { // Scale impulses to support variable time steps. _impulse *= data.step.dtRatio; // Warm starting. FVector2 PA = -(_impulse + _limitImpulse1) * m_uA; FVector2 PB = (-Ratio * _impulse - _limitImpulse2) * m_uB; vA += m_invMassA * PA; wA += m_invIA * MathUtils.Cross(m_rA, PA); vB += m_invMassB * PB; wB += m_invIB * MathUtils.Cross(m_rB, PB); } else { _impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_localCenterA = BodyA.Sweep.LocalCenter; m_localCenterB = BodyB.Sweep.LocalCenter; m_invMassA = BodyA.InvMass; m_invMassB = BodyB.InvMass; m_invIA = BodyA.InvI; m_invIB = BodyB.InvI; FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; FVector2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); m_rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); m_rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); _u = cB + m_rB - cA - m_rA; // Handle singularity. float length = _u.Length(); if (length > Settings.LinearSlop) { _u *= 1.0f / length; } else { _u = FVector2.Zero; } float crAu = MathUtils.Cross(m_rA, _u); float crBu = MathUtils.Cross(m_rB, _u); float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; // Compute the effective mass matrix. _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (Frequency > 0.0f) { float C = length - Length; // Frequency float omega = 2.0f * Settings.Pi * Frequency; // 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 (Settings.EnableWarmstarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; FVector2 P = _impulse * _u; vA -= m_invMassA * P; wA -= m_invIA * MathUtils.Cross(m_rA, P); vB += m_invMassB * P; wB += m_invIB * MathUtils.Cross(m_rB, P); } else { _impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
internal override void SolveVelocityConstraints(ref SolverData data) { FVector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; // Cdot = v + cross(w, r) FVector2 Cdot = vB + MathUtils.Cross(wB, _rB); FVector2 impulse = MathUtils.Mul(ref _mass, -(Cdot + _C + _gamma * _impulse)); FVector2 oldImpulse = _impulse; _impulse += impulse; float maxImpulse = data.step.dt * MaxForce; if (_impulse.LengthSquared() > maxImpulse * maxImpulse) { _impulse *= maxImpulse / _impulse.Length(); } impulse = _impulse - oldImpulse; vB += _invMassB * impulse; wB += _invIB * MathUtils.Cross(_rB, impulse); data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override bool SolvePositionConstraints(ref SolverData data) { FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_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 (m_frequencyHz > 0.0f) { FVector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; FVector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(rA, P); cB += mB * P; aB += iB * MathUtils.Cross(rB, P); } else { FVector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); FVector3 C = new FVector3(C1.X, C1.Y, C2); FVector3 impulse = -K.Solve33(C); FVector2 P = new FVector2(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[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints(ref SolverData data) { FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Rot qA = new Rot(aA), qB = new Rot(aB); float angularError = 0.0f; float positionError; bool fixedRotation = (m_invIA + m_invIB == 0.0f); // Solve angular limit constraint. if (_enableLimit && _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, -FSSettings.MaxAngularCorrection, FSSettings.MaxAngularCorrection); limitImpulse = -m_motorMass * C; angularError = Math.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 + FSSettings.AngularSlop, -FSSettings.MaxAngularCorrection, 0.0f); limitImpulse = -m_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 - FSSettings.AngularSlop, 0.0f, FSSettings.MaxAngularCorrection); limitImpulse = -m_motorMass * C; } aA -= m_invIA * limitImpulse; aB += m_invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); FVector2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Mat22 K = new Mat22(); 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; FVector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(rA, impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(rB, impulse); } data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; return(positionError <= FSSettings.LinearSlop && angularError <= FSSettings.AngularSlop); }
internal override void InitVelocityConstraints(ref SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_localCenterA = BodyA.Sweep.LocalCenter; m_localCenterB = BodyB.Sweep.LocalCenter; m_invMassA = BodyA.InvMass; m_invMassB = BodyB.InvMass; m_invIA = BodyA.InvI; m_invIB = BodyB.InvI; FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; FVector2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); m_rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); m_rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); // Get the pulley axes. m_uA = cA + m_rA - GroundAnchorA; m_uB = cB + m_rB - GroundAnchorB; float lengthA = m_uA.Length(); float lengthB = m_uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { m_uA *= 1.0f / lengthA; } else { m_uA = FVector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { m_uB *= 1.0f / lengthB; } else { m_uB = FVector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(m_rA, m_uA); float ruB = MathUtils.Cross(m_rB, m_uB); float mA = m_invMassA + m_invIA * ruA * ruA; float mB = m_invMassB + m_invIB * ruB * ruB; m_mass = mA + Ratio * Ratio * mB; if (m_mass > 0.0f) { m_mass = 1.0f / m_mass; } if (Settings.EnableWarmstarting) { // Scale impulses to support variable time steps. _impulse *= data.step.dtRatio; // Warm starting. FVector2 PA = -(_impulse + _limitImpulse1) * m_uA; FVector2 PB = (-Ratio * _impulse - _limitImpulse2) * m_uB; vA += m_invMassA * PA; wA += m_invIA * MathUtils.Cross(m_rA, PA); vB += m_invMassB * PB; wB += m_invIB * MathUtils.Cross(m_rB, PB); } else { _impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
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; FVector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; FVector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; FVector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; FVector2 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); _u = cB + _rB - cA - _rA; _length = _u.Length(); float C = _length - MaxLength; if (C > 0.0f) { _state = LimitState.AtUpper; } else { _state = LimitState.Inactive; } if (_length > FSSettings.LinearSlop) { _u *= 1.0f / _length; } else { _u = FVector2.Zero; _mass = 0.0f; _impulse = 0.0f; return; } // Compute effective mass. float crA = MathUtils.Cross(_rA, _u); float crB = MathUtils.Cross(_rB, _u); float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB; _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (FSSettings.EnableWarmstarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; FVector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(_rA, P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(_rB, P); } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal void SolveTOI(ref FSTimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < BodyCount); Debug.Assert(toiIndexB < BodyCount); // Initialize the body state. for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; _positions[i].c = b.Sweep.C; _positions[i].a = b.Sweep.A; _velocities[i].v = b.LinearVelocity; _velocities[i].w = b.AngularVelocity; } //b2ContactSolverDef contactSolverDef; //contactSolverDef.contacts = _contacts; //contactSolverDef.count = _contactCount; //contactSolverDef.allocator = _allocator; //contactSolverDef.step = subStep; //contactSolverDef.positions = _positions; //contactSolverDef.velocities = _velocities; //b2ContactSolver contactSolver(&contactSolverDef); _contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities); // Solve position constraints. for (int i = 0; i < FSSettings.TOIPositionIterations; ++i) { bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // Leap of faith to new safe state. Bodies[toiIndexA].Sweep.C0 = _positions[toiIndexA].c; Bodies[toiIndexA].Sweep.A0 = _positions[toiIndexA].a; Bodies[toiIndexB].Sweep.C0 = _positions[toiIndexB].c; Bodies[toiIndexB].Sweep.A0 = _positions[toiIndexB].a; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. _contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < FSSettings.TOIVelocityIterations; ++i) { _contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. float h = subStep.dt; // Integrate positions. for (int i = 0; i < BodyCount; ++i) { FVector2 c = _positions[i].c; float a = _positions[i].a; FVector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities FVector2 translation = h * v; if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared) { float ratio = FSSettings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > FSSettings.MaxRotationSquared) { float ratio = FSSettings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; // Sync bodies FSBody body = Bodies[i]; body.Sweep.C = c; body.Sweep.A = a; body.LinearVelocity = v; body.AngularVelocity = w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); }
internal override void InitVelocityConstraints(ref SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_localCenterA = BodyA.Sweep.LocalCenter; m_localCenterB = BodyB.Sweep.LocalCenter; m_invMassA = BodyA.InvMass; m_invMassB = BodyB.InvMass; m_invIA = BodyA.InvI; m_invIB = BodyB.InvI; FVector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; FVector2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; FVector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; FVector2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); m_rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); m_rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); _u = cB + m_rB - cA - m_rA; // Handle singularity. float length = _u.Length(); if (length > Settings.LinearSlop) { _u *= 1.0f / length; } else { _u = FVector2.Zero; } float crAu = MathUtils.Cross(m_rA, _u); float crBu = MathUtils.Cross(m_rB, _u); float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu; // Compute the effective mass matrix. _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (Frequency > 0.0f) { float C = length - Length; // Frequency float omega = 2.0f * Settings.Pi * Frequency; // 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 (Settings.EnableWarmstarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; FVector2 P = _impulse * _u; vA -= m_invMassA * P; wA -= m_invIA * MathUtils.Cross(m_rA, P); vB += m_invMassB * P; wB += m_invIB * MathUtils.Cross(m_rB, P); } else { _impulse = 0.0f; } data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { FSBody b1 = BodyA; FSBody b2 = BodyB; Transform xf1, xf2; b1.GetTransform(out xf1); b2.GetTransform(out xf2); // Compute the effective mass matrix. FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter); FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter); _u = b2.Sweep.C + r2 - b1.Sweep.C - r1; // Handle singularity. float length = _u.Length(); if (length < MaxLength && length > MinLength) { return; } if (length > FSSettings.LinearSlop) { _u *= 1.0f / length; } else { _u = FVector2.Zero; } float cr1u = MathUtils.Cross(r1, _u); float cr2u = MathUtils.Cross(r2, _u); float invMass = b1.InvMass + b1.InvI * cr1u * cr1u + b2.InvMass + b2.InvI * cr2u * cr2u; Debug.Assert(invMass > FSSettings.Epsilon); _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (Frequency > 0.0f) { float C = length - MaxLength; // Frequency float omega = 2.0f * FSSettings.Pi * Frequency; // Damping coefficient float d = 2.0f * _mass * DampingRatio * omega; // Spring stiffness float k = _mass * omega * omega; // magic formulas _gamma = data.step.dt * (d + data.step.dt * k); _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f; _bias = C * data.step.dt * k * _gamma; _mass = invMass + _gamma; _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f; } if (FSSettings.EnableWarmstarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; FVector2 P = _impulse * _u; b1.LinearVelocityInternal -= b1.InvMass * P; b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P); b2.LinearVelocityInternal += b2.InvMass * P; b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P); } else { _impulse = 0.0f; } }
public void Solve(ref FSTimeStep step, ref FVector2 gravity) { float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; FVector2 c = b.Sweep.C; float a = b.Sweep.A; FVector2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.C0 = b.Sweep.C; b.Sweep.A0 = b.Sweep.A; if (b.BodyType == BodyType.Dynamic) { // Integrate velocities. v += h * (b.GravityScale * gravity + b.InvMass * b.Force); w += h * b.InvI * b.Torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solver data SolverData solverData = new SolverData(); solverData.step = step; solverData.positions = _positions; solverData.velocities = _velocities; // Initialize velocity constraints. //b2ContactSolverDef contactSolverDef; //contactSolverDef.step = step; //contactSolverDef.contacts = m_contacts; //contactSolverDef.count = m_contactCount; //contactSolverDef.positions = m_positions; //contactSolverDef.velocities = m_velocities; //contactSolverDef.allocator = m_allocator; _contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities); _contactSolver.InitializeVelocityConstraints(); if (FSSettings.EnableWarmstarting) { _contactSolver.WarmStart(); } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Start(); _tmpTime = 0; } #endif for (int i = 0; i < JointCount; ++i) { if (_joints[i].Enabled) { _joints[i].InitVelocityConstraints(ref solverData); } } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _tmpTime += _watch.ElapsedTicks; } #endif // Solve velocity constraints. for (int i = 0; i < FSSettings.VelocityIterations; ++i) { #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) { continue; } joint.SolveVelocityConstraints(ref solverData); //TODO: Move up before solve? joint.Validate(step.inv_dt); } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif _contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting. _contactSolver.StoreImpulses(); // Integrate positions for (int i = 0; i < BodyCount; ++i) { FVector2 c = _positions[i].c; float a = _positions[i].a; FVector2 v = _velocities[i].v; float w = _velocities[i].w; // Check for large velocities FVector2 translation = h * v; if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared) { float ratio = FSSettings.MaxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > FSSettings.MaxRotationSquared) { float ratio = FSSettings.MaxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; _positions[i].c = c; _positions[i].a = a; _velocities[i].v = v; _velocities[i].w = w; } // Solve position constraints bool positionSolved = false; for (int i = 0; i < FSSettings.PositionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(); bool jointsOkay = true; #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Start(); } #endif for (int j = 0; j < JointCount; ++j) { FarseerJoint joint = _joints[j]; if (!joint.Enabled) { continue; } bool jointOkay = joint.SolvePositionConstraints(ref solverData); jointsOkay = jointsOkay && jointOkay; } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { _watch.Stop(); _tmpTime += _watch.ElapsedTicks; _watch.Reset(); } #endif if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } #if (!SILVERLIGHT) if (FSSettings.EnableDiagnostics) { JointUpdateTime = _tmpTime; } #endif // Copy state buffers back to the bodies for (int i = 0; i < BodyCount; ++i) { FSBody body = Bodies[i]; body.Sweep.C = _positions[i].c; body.Sweep.A = _positions[i].a; body.LinearVelocity = _velocities[i].v; body.AngularVelocity = _velocities[i].w; body.SynchronizeTransform(); } Report(_contactSolver._velocityConstraints); if (FSSettings.AllowSleep) { float minSleepTime = FSSettings.MaxFloat; for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; if (b.BodyType == BodyType.Static) { continue; } if ((b.Flags & BodyFlags.AutoSleep) == 0 || b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr || FVector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= FSSettings.TimeToSleep && positionSolved) { for (int i = 0; i < BodyCount; ++i) { FSBody b = Bodies[i]; b.Awake = false; } } } }
public override void Update(float dt) { FVector2 f = FVector2.Zero; foreach (FSBody body1 in World.BodyList) { if (!IsActiveOn(body1)) { continue; } foreach (FSBody body2 in Bodies) { if (body1 == body2 || (body1.IsStatic && body2.IsStatic) || !body2.Enabled) { continue; } FVector2 d = body2.WorldCenter - body1.WorldCenter; float r2 = d.LengthSquared(); if (r2 < FSSettings.Epsilon) { continue; } float r = d.Length(); if (r >= MaxRadius || r <= MinRadius) { continue; } switch (GravityType) { case GravityType.DistanceSquared: f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * body2.Mass * d; break; case GravityType.Linear: f = Strength / r2 * body1.Mass * body2.Mass * d; break; } body1.ApplyForce(ref f); FVector2.Negate(ref f, out f); body2.ApplyForce(ref f); } foreach (FVector2 point in Points) { FVector2 d = point - body1.Position; float r2 = d.LengthSquared(); if (r2 < FSSettings.Epsilon) { continue; } float r = d.Length(); if (r >= MaxRadius || r <= MinRadius) { continue; } switch (GravityType) { case GravityType.DistanceSquared: f = Strength / r2 / (float)Math.Sqrt(r2) * body1.Mass * d; break; case GravityType.Linear: f = Strength / r2 * body1.Mass * d; break; } body1.ApplyForce(ref f); } } }