public static Vector3 operator *( Vector3 V, Rot rot ) { Rot InverseInRot = rot.Inverse(); Rot VectorRot = new Rot( V.x, V.y, V.z, 0 ); Rot IntRot = VectorRot * rot; Rot ResultRot = InverseInRot * IntRot; return new Vector3( ResultRot.x, ResultRot.y, ResultRot.z ); }
public static void ApplyRotToGLMatrix4d( ref GLMatrix4d matrix, Rot rot ) { double fRotAngle = 0; Vector3 vAxis = new Vector3(); mvMath.Rot2AxisAngle( ref vAxis, ref fRotAngle, rot ); matrix.applyRotate( (float)( fRotAngle / Math.PI * 180 ), (float)vAxis.x, (float)vAxis.y, (float)vAxis.z ); }
public static Rot operator *( Rot R1, Rot R2 ) { Rot result = new Rot(); result.s = R1.s * R2.s - R1.x * R2.x - R1.y * R2.y - R1.z * R2.z; result.x = R1.s * R2.x + R1.x * R2.s + R1.y * R2.z -R1.z * R2.y; result.y = R1.s * R2.y + R1.y * R2.s + R1.z * R2.x - R1.x * R2.z; result.z = R1.s * R2.z + R1.z * R2.s + R1.x * R2.y - R1.y * R2.x; //Test.Debug( "RotMULITPLY in=" << Q1 << " " << Q2 << " out=" << Qr <<endl; return result; }
public bool Clip(Vertices clipVertices, Vector2 position) { Rot rot = new Rot(0); Transform t = new Transform(ref position, ref rot); //Transform shape Transform thistransform; Body.GetTransform(out thistransform); //Transform the shape Vertices transformedshape = new Vertices(clipVertices.Count); foreach (Vector2 v in clipVertices) { Vector2 newv = v; newv = MathUtils.Mul(ref t, ref newv); newv = MathUtils.MulT(ref thistransform, ref newv); transformedshape.Add(newv); } PolyClipError error; List<Vertices> result = YuPengClipper.Difference(Vertices, transformedshape, out error); //Need to check if the entire shape was cut, //so we can destroy/erase it if (result.Count == 0) return false; //The shape was split up, //so create a new DestructableBody for each piece if (result.Count > 1) { //Create a new destructable body for each extra shape for (int i = 1; i < result.Count; i++) { DestructableBody db = new DestructableBody(_world, result[i]); db.Body.Position = Body.Position; } } //Set Shape Vertices newshape = result[0]; SetShape(newshape); return true; }
public static void Rot2AxisAngle( ref Vector3 Vr, ref double Thetar, Rot R ) { //QuaternionNormalize( |X,Y,Z,W| ); Vr = new Vector3(); double cos_a = R.s; Thetar = Math.Acos( cos_a ) * 2; double sin_a = Math.Sqrt( 1.0 - cos_a * cos_a ); if ( Math.Abs( sin_a )> 0.0005 ) { Vr.x = R.x / sin_a; Vr.y = R.y / sin_a; Vr.z = R.z / sin_a; } else { Vr.x = 1; Vr.y = 0; Vr.z = 0; } }
public void Rotate( Rot rot ) { double fRotAngle = 0; Vector3 vAxis = new Vector3(); mvMath.Rot2AxisAngle( ref vAxis, ref fRotAngle, rot ); Gl.glRotatef( (float)( fRotAngle / Math.PI * 180 ), (float)vAxis.x, (float)vAxis.y, (float)vAxis.z ); }
/// <summary> /// Compute the closest points between two shapes. Supports any combination of: CircleShape and /// PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to /// zero. /// </summary> /// <param name="output"></param> /// <param name="cache"></param> /// <param name="input"></param> public void GetDistance(DistanceOutput output, SimplexCache cache, DistanceInput input) { GJK_CALLS++; DistanceProxy proxyA = input.ProxyA; DistanceProxy proxyB = input.ProxyB; Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.Vertices; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. // (pooled above) simplex.GetClosestPoint(closestPoint); float distanceSqr1 = closestPoint.LengthSquared(); // Main iteration loop int iter = 0; while (iter < GJK_MAX_ITERS) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; i++) { saveA[i] = vertices[i].IndexA; saveB[i] = vertices[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Compute closest point. simplex.GetClosestPoint(closestPoint); float distanceSqr2 = closestPoint.LengthSquared(); // ensure progress if (distanceSqr2 >= distanceSqr1) { // break; } distanceSqr1 = distanceSqr2; // get search direction; simplex.GetSearchDirection(d); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.EPSILON * Settings.EPSILON) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } /* * SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA = * proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA, * proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB = * proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB, * proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; */ // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = vertices[simplex.Count]; Rot.MulTransUnsafe(transformA.Q, d.NegateLocal(), temp); vertex.IndexA = proxyA.GetSupport(temp); Transform.MulToOutUnsafe(transformA, proxyA.GetVertex(vertex.IndexA), vertex.WA); // Vec2 wBLocal; Rot.MulTransUnsafe(transformB.Q, d.NegateLocal(), temp); vertex.IndexB = proxyB.GetSupport(temp); Transform.MulToOutUnsafe(transformB, proxyB.GetVertex(vertex.IndexB), vertex.WB); vertex.W.Set(vertex.WB).SubLocal(vertex.WA); // Iteration count is equated to the number of support point calls. ++iter; ++GJK_ITERS; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } GJK_MAX_ITERS = MathUtils.Max(GJK_MAX_ITERS, iter); // Prepare output. simplex.GetWitnessPoints(output.PointA, output.PointB); output.Distance = MathUtils.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input.UseRadii) { float rA = proxyA.Radius; float rB = proxyB.Radius; if (output.Distance > rA + rB && output.Distance > Settings.EPSILON) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= (rA + rB); normal.Set(output.PointB).SubLocal(output.PointA); normal.Normalize(); temp.Set(normal).MulLocal(rA); output.PointA.AddLocal(temp); temp.Set(normal).MulLocal(rB); output.PointB.SubLocal(temp); } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. // Vec2 p = 0.5f * (output.pointA + output.pointB); output.PointA.AddLocal(output.PointB).MulLocal(.5f); output.PointB.Set(output.PointA); output.Distance = 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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; 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); _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 = Vector2.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; Vector2 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; }
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; Vec2 cC = data.positions[m_indexC].c; double aC = data.positions[m_indexC].a; Vec2 cD = data.positions[m_indexD].c; double aD = data.positions[m_indexD].a; Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot(); qA.set(aA); qB.set(aB); qC.set(aC); qD.set(aD); double linearError = 0.0d; double coordinateA, coordinateB; Vec2 temp = pool.popVec2(); Vec2 JvAC = pool.popVec2(); Vec2 JvBD = pool.popVec2(); double JwA, JwB, JwC, JwD; double mass = 0.0d; if (m_typeA == JointType.REVOLUTE) { JvAC.setZero(); JwA = 1.0d; JwC = 1.0d; mass += m_iA + m_iC; coordinateA = aA - aC - m_referenceAngleA; } else { Vec2 rC = pool.popVec2(); Vec2 rA = pool.popVec2(); Vec2 pC = pool.popVec2(); Vec2 pA = pool.popVec2(); Rot.mulToOutUnsafe(qC, m_localAxisC, JvAC); Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC); Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA); JwC = Vec2.cross(rC, JvAC); JwA = Vec2.cross(rA, JvAC); mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA; pC.set(m_localAnchorC).subLocal(m_lcC); Rot.mulTransUnsafe(qC, temp.set(rA).addLocal(cA).subLocal(cC), pA); coordinateA = Vec2.dot(pA.subLocal(pC), m_localAxisC); pool.pushVec2(4); } if (m_typeB == JointType.REVOLUTE) { JvBD.setZero(); JwB = m_ratio; JwD = m_ratio; mass += m_ratio * m_ratio * (m_iB + m_iD); coordinateB = aB - aD - m_referenceAngleB; } else { Vec2 u = pool.popVec2(); Vec2 rD = pool.popVec2(); Vec2 rB = pool.popVec2(); Vec2 pD = pool.popVec2(); Vec2 pB = pool.popVec2(); Rot.mulToOutUnsafe(qD, m_localAxisD, u); Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB); JvBD.set(u).mulLocal(m_ratio); JwD = Vec2.cross(rD, u); JwB = Vec2.cross(rB, u); mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB; pD.set(m_localAnchorD).subLocal(m_lcD); Rot.mulTransUnsafe(qD, temp.set(rB).addLocal(cB).subLocal(cD), pB); coordinateB = Vec2.dot(pB.subLocal(pD), m_localAxisD); pool.pushVec2(5); } double C = (coordinateA + m_ratio * coordinateB) - m_constant; double impulse = 0.0d; if (mass > 0.0d) { impulse = -C / mass; } pool.pushVec2(3); pool.pushRot(4); cA.x += (m_mA * impulse) * JvAC.x; cA.y += (m_mA * impulse) * JvAC.y; aA += m_iA * impulse * JwA; cB.x += (m_mB * impulse) * JvBD.x; cB.y += (m_mB * impulse) * JvBD.y; aB += m_iB * impulse * JwB; cC.x -= (m_mC * impulse) * JvAC.x; cC.y -= (m_mC * impulse) * JvAC.y; aC -= m_iC * impulse * JwC; cD.x -= (m_mD * impulse) * JvBD.x; cD.y -= (m_mD * impulse) * JvBD.y; aD -= m_iD * impulse * JwD; // data.positions[m_indexA].c = cA; data.positions[m_indexA].a = aA; // data.positions[m_indexB].c = cB; data.positions[m_indexB].a = aB; // data.positions[m_indexC].c = cC; data.positions[m_indexC].a = aC; // data.positions[m_indexD].c = cD; data.positions[m_indexD].a = aD; // TODO_ERIN not implemented return(linearError < Settings.linearSlop); }
internal override bool SolvePositionConstraints(ref SolverData data) { if (Frequency > 0.0f) { // There is no position correction for soft distance constraints. return true; } 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); Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 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; Vector2 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 Math.Abs(C) < Settings.LinearSlop; }
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 mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; 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); // Compute the effective masses. Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d1 = cB + rB - cA - rA; // Point to line constraint { _ay = MathUtils.Mul(qA, _localYAxis); _sAy = MathUtils.Cross(d1 + rA, _ay); _sBy = MathUtils.Cross(rB, _ay); _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; if (_mass > 0.0f) { _mass = 1.0f / _mass; } } // Spring constraint _springMass = 0.0f; _bias = 0.0f; _gamma = 0.0f; if (Frequency > 0.0f) { _ax = MathUtils.Mul(qA, LocalXAxis); _sAx = MathUtils.Cross(d1 + rA, _ax); _sBx = MathUtils.Cross(rB, _ax); float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx; if (invMass > 0.0f) { _springMass = 1.0f / invMass; float C = Vector2.Dot(d1, _ax); // Frequency float omega = 2.0f * Settings.Pi * Frequency; // Damping coefficient float d = 2.0f * _springMass * DampingRatio * omega; // Spring stiffness float k = _springMass * omega * omega; // magic formulas float h = data.step.dt; _gamma = h * (d + h * k); if (_gamma > 0.0f) { _gamma = 1.0f / _gamma; } _bias = C * h * k * _gamma; _springMass = invMass + _gamma; if (_springMass > 0.0f) { _springMass = 1.0f / _springMass; } } } else { _springImpulse = 0.0f; } // Rotational motor if (_enableMotor) { _motorMass = iA + iB; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } } else { _motorMass = 0.0f; _motorImpulse = 0.0f; } if (Settings.EnableWarmstarting) { // Account for variable time step. _impulse *= data.step.dtRatio; _springImpulse *= data.step.dtRatio; _motorImpulse *= data.step.dtRatio; Vector2 P = _impulse * _ay + _springImpulse * _ax; float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; vA -= _invMassA * P; wA -= _invIA * LA; vB += _invMassB * P; wB += _invIB * LB; } else { _impulse = 0.0f; _springImpulse = 0.0f; _motorImpulse = 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) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 cC = data.positions[_indexC].c; float aC = data.positions[_indexC].a; Vector2 cD = data.positions[_indexD].c; float aD = data.positions[_indexD].a; Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); const float linearError = 0.0f; float coordinateA, coordinateB; Vector2 JvAC, JvBD; float JwA, JwB, JwC, JwD; float mass = 0.0f; if (_typeA == JointType.Revolute) { JvAC = Vector2.Zero; JwA = 1.0f; JwC = 1.0f; mass += _iA + _iC; coordinateA = aA - aC - _referenceAngleA; } else { Vector2 u = MathUtils.Mul(qC, _localAxisC); Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); JvAC = u; JwC = MathUtils.Cross(rC, u); JwA = MathUtils.Cross(rA, u); mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA; Vector2 pC = _localAnchorC - _lcC; Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC)); coordinateA = Vector2.Dot(pA - pC, _localAxisC); } if (_typeB == JointType.Revolute) { JvBD = Vector2.Zero; JwB = _ratio; JwD = _ratio; mass += _ratio * _ratio * (_iB + _iD); coordinateB = aB - aD - _referenceAngleB; } else { Vector2 u = MathUtils.Mul(qD, _localAxisD); Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); JvBD = _ratio * u; JwD = _ratio * MathUtils.Cross(rD, u); JwB = _ratio * MathUtils.Cross(rB, u); mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; Vector2 pD = _localAnchorD - _lcD; Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD)); coordinateB = Vector2.Dot(pB - pD, _localAxisD); } float C = (coordinateA + _ratio * coordinateB) - _constant; float impulse = 0.0f; if (mass > 0.0f) { impulse = -C / mass; } cA += _mA * impulse * JvAC; aA += _iA * impulse * JwA; cB += _mB * impulse * JvBD; aB += _iB * impulse * JwB; cC -= _mC * impulse * JvAC; aC -= _iC * impulse * JwC; cD -= _mD * impulse * JvBD; aD -= _iD * impulse * JwD; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; data.positions[_indexC].c = cC; data.positions[_indexC].a = aC; data.positions[_indexD].c = cD; data.positions[_indexD].a = aD; // TODO_ERIN not implemented return(linearError < Settings.LinearSlop); }
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, -Settings.MaxAngularCorrection, Settings.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 + Settings.AngularSlop, -Settings.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 - Settings.AngularSlop, 0.0f, Settings.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 <= Settings.LinearSlop && angularError <= Settings.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; float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; Vector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vector2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; Vector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Vector2 vB = data.velocities[m_indexB].v; float wB = data.velocities[m_indexB].w; Rot qA = new Rot(aA), qB = new Rot(aB); // Compute the effective masses. Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); Vector2 d1 = cB + rB - cA - rA; // Point to line constraint { m_ay = MathUtils.Mul(qA, m_localYAxisA); m_sAy = MathUtils.Cross(d1 + rA, m_ay); m_sBy = MathUtils.Cross(rB, m_ay); m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; if (m_mass > 0.0f) { m_mass = 1.0f / m_mass; } } // Spring constraint m_springMass = 0.0f; m_bias = 0.0f; m_gamma = 0.0f; if (SpringFrequencyHz > 0.0f) { m_ax = MathUtils.Mul(qA, m_localXAxisA); m_sAx = MathUtils.Cross(d1 + rA, m_ax); m_sBx = MathUtils.Cross(rB, m_ax); float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; if (invMass > 0.0f) { m_springMass = 1.0f / invMass; float C = Vector2.Dot(d1, m_ax); // Frequency float omega = 2.0f * Settings.Pi * SpringFrequencyHz; // Damping coefficient float d = 2.0f * m_springMass * SpringDampingRatio * omega; // Spring stiffness float k = m_springMass * omega * omega; // magic formulas float h = data.step.dt; m_gamma = h * (d + h * k); if (m_gamma > 0.0f) { m_gamma = 1.0f / m_gamma; } m_bias = C * h * k * m_gamma; m_springMass = invMass + m_gamma; if (m_springMass > 0.0f) { m_springMass = 1.0f / m_springMass; } } } else { m_springImpulse = 0.0f; } // Rotational motor if (m_enableMotor) { m_motorMass = iA + iB; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } } else { m_motorMass = 0.0f; m_motorImpulse = 0.0f; } if (Settings.EnableWarmstarting) { // Account for variable time step. m_impulse *= data.step.dtRatio; m_springImpulse *= data.step.dtRatio; m_motorImpulse *= data.step.dtRatio; Vector2 P = m_impulse * m_ay + m_springImpulse * m_ax; float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; vA -= m_invMassA * P; wA -= m_invIA * LA; vB += m_invMassB * P; wB += m_invIB * LB; } else { m_impulse = 0.0f; m_springImpulse = 0.0f; m_motorImpulse = 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; _indexC = _bodyC.IslandIndex; _indexD = _bodyD.IslandIndex; _lcA = _bodyA._sweep.LocalCenter; _lcB = _bodyB._sweep.LocalCenter; _lcC = _bodyC._sweep.LocalCenter; _lcD = _bodyD._sweep.LocalCenter; _mA = _bodyA._invMass; _mB = _bodyB._invMass; _mC = _bodyC._invMass; _mD = _bodyD._invMass; _iA = _bodyA._invI; _iB = _bodyB._invI; _iC = _bodyC._invI; _iD = _bodyD._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; float aC = data.positions[_indexC].a; Vector2 vC = data.velocities[_indexC].v; float wC = data.velocities[_indexC].w; float aD = data.positions[_indexD].a; Vector2 vD = data.velocities[_indexD].v; float wD = data.velocities[_indexD].w; Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); _mass = 0.0f; if (_typeA == JointType.Revolute) { _JvAC = Vector2.Zero; _JwA = 1.0f; _JwC = 1.0f; _mass += _iA + _iC; } else { Vector2 u = MathUtils.Mul(qC, _localAxisC); Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); _JvAC = u; _JwC = MathUtils.Cross(rC, u); _JwA = MathUtils.Cross(rA, u); _mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA; } if (_typeB == JointType.Revolute) { _JvBD = Vector2.Zero; _JwB = _ratio; _JwD = _ratio; _mass += _ratio * _ratio * (_iB + _iD); } else { Vector2 u = MathUtils.Mul(qD, _localAxisD); Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); _JvBD = _ratio * u; _JwD = _ratio * MathUtils.Cross(rD, u); _JwB = _ratio * MathUtils.Cross(rB, u); _mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB; } // Compute effective mass. _mass = _mass > 0.0f ? 1.0f / _mass : 0.0f; if (Settings.EnableWarmstarting) { vA += (_mA * _impulse) * _JvAC; wA += _iA * _impulse * _JwA; vB += (_mB * _impulse) * _JvBD; wB += _iB * _impulse * _JwB; vC -= (_mC * _impulse) * _JvAC; wC -= _iC * _impulse * _JwC; vD -= (_mD * _impulse) * _JvBD; wD -= _iD * _impulse * _JwD; } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; data.velocities[_indexC].v = vC; data.velocities[_indexC].w = wC; data.velocities[_indexD].v = vD; data.velocities[_indexD].w = wD; }
/// <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); }
public bool FuzzyEquals(CG_Transform trans) { return(Pos.FuzzyEquals(trans.Pos) && Rot.FuzzyEquals(trans.Rot) && Scl.FuzzyEquals(trans.Scl)); }
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 mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; Vector2 cB = data.Positions[_indexB].C; 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); // Compute the effective masses. Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d1 = cB + rB - cA - rA; // Point to line constraint { _ay = MathUtils.Mul(qA, _localYAxis); _sAy = MathUtils.Cross(d1 + rA, _ay); _sBy = MathUtils.Cross(rB, _ay); _mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy; if (_mass > 0.0f) { _mass = 1.0f / _mass; } } // Spring constraint _springMass = 0.0f; _bias = 0.0f; _gamma = 0.0f; if (Frequency > 0.0f) { _ax = MathUtils.Mul(qA, LocalXAxis); _sAx = MathUtils.Cross(d1 + rA, _ax); _sBx = MathUtils.Cross(rB, _ax); float invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx; if (invMass > 0.0f) { _springMass = 1.0f / invMass; float C = Vector2.Dot(d1, _ax); // Frequency float omega = 2.0f * Settings.Pi * Frequency; // Damping coefficient float d = 2.0f * _springMass * DampingRatio * omega; // Spring stiffness float k = _springMass * omega * omega; // magic formulas float h = data.Step.dt; _gamma = h * (d + h * k); if (_gamma > 0.0f) { _gamma = 1.0f / _gamma; } _bias = C * h * k * _gamma; _springMass = invMass + _gamma; if (_springMass > 0.0f) { _springMass = 1.0f / _springMass; } } } else { _springImpulse = 0.0f; } // Rotational motor if (_enableMotor) { _motorMass = iA + iB; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } } else { _motorMass = 0.0f; _motorImpulse = 0.0f; } #pragma warning disable 162 // ReSharper disable once ConditionIsAlwaysTrueOrFalse if (Settings.EnableWarmstarting) { // Account for variable time step. _impulse *= data.Step.dtRatio; _springImpulse *= data.Step.dtRatio; _motorImpulse *= data.Step.dtRatio; Vector2 P = _impulse * _ay + _springImpulse * _ax; float LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse; float LB = _impulse * _sBy + _springImpulse * _sBx + _motorImpulse; vA -= _invMassA * P; wA -= _invIA * LA; vB += _invMassB * P; wB += _invIB * LB; } else { _impulse = 0.0f; _springImpulse = 0.0f; _motorImpulse = 0.0f; } #pragma warning restore 162 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) { 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 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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 vB = data.velocities[_indexB].v; float wB = data.velocities[_indexB].w; Rot qA = new Rot(aA); Rot qB = new Rot(aB); // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, -_localCenterA); _rB = MathUtils.Mul(qB, -_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; 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; _linearMass = K.Inverse; _angularMass = iA + iB; if (_angularMass > 0.0f) { _angularMass = 1.0f / _angularMass; } _linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset); _angularError = aB - aA - _angularOffset; if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= data.step.dtRatio; _angularImpulse *= data.step.dtRatio; Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); } else { _linearImpulse = Vector2.Zero; _angularImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _invMassA = BodyA._invMass; _invIA = BodyA._invI; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Rot qA = new Rot(aA); float mass = BodyA.Mass; // 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 has units of inverse mass. // beta has units of inverse time. float h = data.step.dt; Debug.Assert(d + h * k > Settings.Epsilon); _gamma = h * (d + h * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = h * k * _gamma; // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); // 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 = new Mat22(); K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; K.ex.Y = -_invIA * _rA.X * _rA.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; _mass = K.Inverse; _C = cA + _rA - _worldAnchor; _C *= _beta; // Cheat with some damping wA *= 0.98f; if (Settings.EnableWarmstarting) { _impulse *= data.step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.Cross(_rA, _impulse); } else { _impulse = Vector2.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
/// Inverse rotate a vector public static Vector2 MulT(Rot q, Vector2 v) { return(new Vector2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * 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); Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // Get the pulley axes. Vector2 uA = cA + rA - WorldAnchorA; Vector2 uB = cB + rB - WorldAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { uA *= 1.0f / lengthA; } else { uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { uB *= 1.0f / lengthB; } else { uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(rA, uA); float ruB = MathUtils.Cross(rB, uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; float mass = mA + Ratio * Ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = Constant - lengthA - Ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; Vector2 PA = -impulse * uA; Vector2 PB = -Ratio * impulse * uB; cA += _invMassA * PA; aA += _invIA * MathUtils.Cross(rA, PA); cB += _invMassB * PB; aB += _invIB * MathUtils.Cross(rB, PB); data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return linearError < Settings.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; Vector2 cA = data.positions[m_indexA].c; float aA = data.positions[m_indexA].a; Vector2 vA = data.velocities[m_indexA].v; float wA = data.velocities[m_indexA].w; Vector2 cB = data.positions[m_indexB].c; float aB = data.positions[m_indexB].a; Vector2 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 = Vector2.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; Vector2 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) { _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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; 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); // Compute the effective masses. Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d = (cB - cA) + rB - rA; float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; // Compute motor Jacobian and effective mass. { _axis = MathUtils.Mul(qA, LocalXAxis); _a1 = MathUtils.Cross(d + rA, _axis); _a2 = MathUtils.Cross(rB, _axis); _motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2; if (_motorMass > 0.0f) { _motorMass = 1.0f / _motorMass; } } // Prismatic constraint. { _perp = MathUtils.Mul(qA, _localYAxisA); _s1 = MathUtils.Cross(d + rA, _perp); _s2 = MathUtils.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 = new Vector3(k11, k12, k13); _K.ey = new Vector3(k12, k22, k23); _K.ez = new Vector3(k13, k23, k33); } // Compute motor and limit terms. if (_enableLimit) { float jointTranslation = Vector2.Dot(_axis, d); if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { _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 (_enableMotor == false) { MotorImpulse = 0.0f; } if (Settings.EnableWarmstarting) { // Account for variable time step. _impulse *= data.step.dtRatio; MotorImpulse *= data.step.dtRatio; Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis; float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1; float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2; vA -= mA * P; wA -= iA * LA; vB += mB * P; wB += iB * LB; } else { _impulse = Vector3.Zero; MotorImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = BodyA.IslandIndex; _localCenterA = BodyA._sweep.LocalCenter; _invMassA = BodyA._invMass; _invIA = BodyA._invI; Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; Rot qA = new Rot(aA); float mass = BodyA.Mass; // 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 has units of inverse mass. // beta has units of inverse time. float h = data.Step.dt; Debug.Assert(d + h * k > Settings.Epsilon); _gamma = h * (d + h * k); if (_gamma != 0.0f) { _gamma = 1.0f / _gamma; } _beta = h * k * _gamma; // Compute the effective mass matrix. _rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); // 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 = new Mat22(); K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma; K.ex.Y = -_invIA * _rA.X * _rA.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma; _mass = K.Inverse; _C = cA + _rA - _worldAnchor; _C *= _beta; // Cheat with some damping wA *= 0.98f; if (Settings.EnableWarmstarting) { _impulse *= data.Step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.Cross(_rA, _impulse); } else { _impulse = Vector2.Zero; } data.Velocities[_indexA].V = vA; data.Velocities[_indexA].W = wA; }
internal override void InitVelocityConstraints(ref SolverData data) { _indexA = _bodyA.IslandIndex; _indexB = _bodyB.IslandIndex; _indexC = _bodyC.IslandIndex; _indexD = _bodyD.IslandIndex; _lcA = _bodyA.Sweep.LocalCenter; _lcB = _bodyB.Sweep.LocalCenter; _lcC = _bodyC.Sweep.LocalCenter; _lcD = _bodyD.Sweep.LocalCenter; _mA = _bodyA.InvMass; _mB = _bodyB.InvMass; _mC = _bodyC.InvMass; _mD = _bodyD.InvMass; _iA = _bodyA.InvI; _iB = _bodyB.InvI; _iC = _bodyC.InvI; _iD = _bodyD.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; float aC = data.positions[_indexC].a; Vector2 vC = data.velocities[_indexC].v; float wC = data.velocities[_indexC].w; float aD = data.positions[_indexD].a; Vector2 vD = data.velocities[_indexD].v; float wD = data.velocities[_indexD].w; Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); _mass = 0.0f; if (_typeA == JointType.Revolute) { _JvAC = Vector2.Zero; _JwA = 1.0f; _JwC = 1.0f; _mass += _iA + _iC; } else { Vector2 u = MathUtils.Mul(qC, _localAxisC); Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); _JvAC = u; _JwC = MathUtils.Cross(rC, u); _JwA = MathUtils.Cross(rA, u); _mass += _mC + _mA + _iC * _JwC * _JwC + _iA * _JwA * _JwA; } if (_typeB == JointType.Revolute) { _JvBD = Vector2.Zero; _JwB = _ratio; _JwD = _ratio; _mass += _ratio * _ratio * (_iB + _iD); } else { Vector2 u = MathUtils.Mul(qD, _localAxisD); Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); _JvBD = _ratio * u; _JwD = _ratio * MathUtils.Cross(rD, u); _JwB = _ratio * MathUtils.Cross(rB, u); _mass += _ratio * _ratio * (_mD + _mB) + _iD * _JwD * _JwD + _iB * _JwB * _JwB; } // Compute effective mass. _mass = _mass > 0.0f ? 1.0f / _mass : 0.0f; if (Settings.EnableWarmstarting) { vA += (_mA * _impulse) * _JvAC; wA += _iA * _impulse * _JwA; vB += (_mB * _impulse) * _JvBD; wB += _iB * _impulse * _JwB; vC -= (_mC * _impulse) * _JvAC; wC -= _iC * _impulse * _JwC; vD -= (_mD * _impulse) * _JvBD; wD -= _iD * _impulse * _JwD; } else { _impulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_indexB].w = wB; data.velocities[_indexC].v = vC; data.velocities[_indexC].w = wC; data.velocities[_indexD].v = vD; data.velocities[_indexD].w = wD; }
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; Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; Vector2 cB = data.Positions[_indexB].C; 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); // Get the pulley axes. _uA = cA + _rA - WorldAnchorA; _uB = cB + _rB - WorldAnchorB; float lengthA = _uA.Length(); float lengthB = _uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { _uA *= 1.0f / lengthA; } else { _uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { _uB *= 1.0f / lengthB; } else { _uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(_rA, _uA); float ruB = MathUtils.Cross(_rB, _uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; _mass = mA + Ratio * Ratio * mB; if (_mass > 0.0f) { _mass = 1.0f / _mass; } if (Settings.EnableWarmstarting) { // Scale impulses to support variable time steps. _impulse *= data.Step.DtRatio; // Warm starting. Vector2 PA = -(_impulse) * _uA; Vector2 PB = (-Ratio * _impulse) * _uB; vA += _invMassA * PA; wA += _invIA * MathUtils.Cross(_rA, PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(_rB, PB); } 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 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 static Rot RotBetween( Vector3 v1, Vector3 v2 ) { Rot rResult = new Rot(); Vector3 VectorNorm1 = new Vector3( v1 ).Normalize(); Vector3 VectorNorm2 = new Vector3( v2 ).Normalize(); Vector3 RotationAxis = Vector3.CrossProduct( VectorNorm1, VectorNorm2 ); //Test.Debug( "math: " << RotationAxis ); // Test.Debug //Test.Debug( Math.Abs( RotationAxis.x ) << " " << Math.Abs( RotationAxis.y ) << " " << Math.Abs( RotationAxis.z ) ); // Test.Debug if( Math.Abs( RotationAxis.x ) < 0.0005 && Math.Abs( RotationAxis.y ) < 0.0005 && Math.Abs( RotationAxis.z ) < 0.0005 ) { Vector3 RandomVector = new Vector3( VectorNorm1 ); RandomVector.x += 0.5; RandomVector.Normalize(); RotationAxis = Vector3.CrossProduct( VectorNorm1, RandomVector ); rResult = mvMath.AxisAngle2Rot( RotationAxis, Math.PI ); } else { double DotProduct = Vector3.DotProduct( VectorNorm1, VectorNorm2 ); Diag.Debug( "DotProduct: " + DotProduct.ToString() ); // Test.Debug double Vangle = Math.Acos( DotProduct ); Diag.Debug( "math: " + Vangle.ToString() ); // Test.Debug rResult = AxisAngle2Rot( RotationAxis, Vangle ); } return rResult; }
//! Feedback line buffer for OpenGL feedback, used by mvgraphics.cpp /* class FeedbackLineBufferItem { public double type; public Vector2[] vertices = new Vector2[2]; } */ public Vector3 GetMouseVector(Vector3 OurPos, Rot OurRot, int mousex, int mousey) { IRenderer renderer = RendererFactory.GetInstance(); Vector3 MouseVectorObserverAxes = new Vector3( - renderer.InnerWindowWidth / 2 + mousex, - renderer.ScreenDistanceScreenCoords, renderer.InnerWindowHeight / 2 - mousey); //Console.WriteLine("MouseVectorObserverAxes: " + MouseVectorObserverAxes); MouseVectorObserverAxes.Normalize(); //Console.WriteLine("MouseVectorObserverAxes (normalized): " + MouseVectorObserverAxes); Vector3 MouseVectorWorldAxes = MouseVectorObserverAxes * OurRot.Inverse(); //Console.WriteLine("MouseVectorWorldAxes: " + MouseVectorWorldAxes.ToString()); MouseVectorWorldAxes.Normalize(); return MouseVectorWorldAxes; }
internal rotOpt(float SAH, Rot rot) { this.SAH = SAH; this.rot = rot; }
internal override void InitVelocityConstraints(ref SolverData data) { m_indexA = BodyA.IslandIndex; m_indexB = BodyB.IslandIndex; m_indexC = m_bodyC.IslandIndex; m_indexD = m_bodyD.IslandIndex; m_lcA = BodyA.Sweep.LocalCenter; m_lcB = BodyB.Sweep.LocalCenter; m_lcC = m_bodyC.Sweep.LocalCenter; m_lcD = m_bodyD.Sweep.LocalCenter; m_mA = BodyA.InvMass; m_mB = BodyB.InvMass; m_mC = m_bodyC.InvMass; m_mD = m_bodyD.InvMass; m_iA = BodyA.InvI; m_iB = BodyB.InvI; m_iC = m_bodyC.InvI; m_iD = m_bodyD.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; //FVector2 cC = data.positions[m_indexC].c; float aC = data.positions[m_indexC].a; FVector2 vC = data.velocities[m_indexC].v; float wC = data.velocities[m_indexC].w; //FVector2 cD = data.positions[m_indexD].c; float aD = data.positions[m_indexD].a; FVector2 vD = data.velocities[m_indexD].v; float wD = data.velocities[m_indexD].w; Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); m_mass = 0.0f; if (m_typeA == JointType.Revolute) { m_JvAC = FVector2.Zero; m_JwA = 1.0f; m_JwC = 1.0f; m_mass += m_iA + m_iC; } else { FVector2 u = MathUtils.Mul(qC, m_localAxisC); FVector2 rC = MathUtils.Mul(qC, m_localAnchorC - m_lcC); FVector2 rA = MathUtils.Mul(qA, m_localAnchorA - m_lcA); m_JvAC = u; m_JwC = MathUtils.Cross(rC, u); m_JwA = MathUtils.Cross(rA, u); m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; } if (m_typeB == JointType.Revolute) { m_JvBD = FVector2.Zero; m_JwB = _ratio; m_JwD = _ratio; m_mass += _ratio * _ratio * (m_iB + m_iD); } else { FVector2 u = MathUtils.Mul(qD, m_localAxisD); FVector2 rD = MathUtils.Mul(qD, m_localAnchorD - m_lcD); FVector2 rB = MathUtils.Mul(qB, m_localAnchorB - m_lcB); m_JvBD = _ratio * u; m_JwD = _ratio * MathUtils.Cross(rD, u); m_JwB = _ratio * MathUtils.Cross(rB, u); m_mass += _ratio * _ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; } // Compute effective mass. m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f; if (Settings.EnableWarmstarting) { vA += (m_mA * m_impulse) * m_JvAC; wA += m_iA * m_impulse * m_JwA; vB += (m_mB * m_impulse) * m_JvBD; wB += m_iB * m_impulse * m_JwB; vC -= (m_mC * m_impulse) * m_JvAC; wC -= m_iC * m_impulse * m_JwC; vD -= (m_mD * m_impulse) * m_JvBD; wD -= m_iD * m_impulse * m_JwD; } else { m_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; data.velocities[m_indexC].v = vC; data.velocities[m_indexC].w = wC; data.velocities[m_indexD].v = vD; data.velocities[m_indexD].w = wD; }
// Requires Using System.Runtime.InteropServices.ComTypes // Get all running instance by querying ROT public static IEnumerable <RunningObjectTableEntry> GetActiveEntries() { // get Running Object Table ... GetRunningObjectTable(0, out var Rot); if (Rot == null) { yield break; } // get enumerator for ROT entries Rot.EnumRunning(out var monikerEnumerator); if (monikerEnumerator == null) { yield break; } monikerEnumerator.Reset(); var pNumFetched = new IntPtr(); var monikers = new IMoniker[1]; // go through all entries and identifies app instances while (monikerEnumerator.Next(1, monikers, pNumFetched) == 0) { var ret = default(RunningObjectTableEntry); try { CreateBindCtx(0, out var bindCtx); if (bindCtx == null) { continue; } Rot.GetObject(monikers[0], out var ComObject); monikers[0].GetDisplayName(bindCtx, null, out var displayName); monikers[0].GetClassID(out var ClassId); ret = new RunningObjectTableEntry() { Object = ComObject, DisplayName = displayName, ClassID = ClassId, }; } catch (Exception ex) { ex.Ignore(); } if (ret != null) { yield return(ret); } } }
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); Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // Get the pulley axes. Vector2 uA = cA + rA - WorldAnchorA; Vector2 uB = cB + rB - WorldAnchorB; float lengthA = uA.Length(); float lengthB = uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { uA *= 1.0f / lengthA; } else { uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { uB *= 1.0f / lengthB; } else { uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(rA, uA); float ruB = MathUtils.Cross(rB, uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; float mass = mA + Ratio * Ratio * mB; if (mass > 0.0f) { mass = 1.0f / mass; } float C = constant - lengthA - Ratio * lengthB; float linearError = Math.Abs(C); float impulse = -mass * C; Vector2 PA = -impulse * uA; Vector2 PB = -Ratio * impulse * uB; cA += _invMassA * PA; aA += _invIA * MathUtils.Cross(rA, PA); cB += _invMassB * PB; aB += _invIB * MathUtils.Cross(rB, PB); data.Positions[_indexA].C = cA; data.Positions[_indexA].A = aA; data.Positions[_indexB].C = cB; data.Positions[_indexB].A = aB; return(linearError < Settings.LinearSlop); }
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); }
internal override void InitVelocityConstraints(ref SolverData data) { _indexB = BodyA.IslandIndex; _localCenterB = BodyA.Sweep.LocalCenter; _invMassB = BodyA.InvMass; _invIB = 0; 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 qB = new Rot(aB); float mass = BodyA.Mass; // 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 has units of inverse mass. // beta has units of inverse time. float h = data.step.dt; Debug.Assert(d + h * k > FSSettings.Epsilon); _gamma = h * (d + h * k); if (!Mathf.Approximately(_gamma, 0.0f)) _gamma = 1.0f / _gamma; _beta = h * k * _gamma; // Compute the effective mass matrix. _rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); // 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] var K = new Mat22(); K.ex.X = _invMassB + _invIB * _rB.Y * _rB.Y + _gamma; K.ex.Y = -_invIB * _rB.X * _rB.Y; K.ey.X = K.ex.Y; K.ey.Y = _invMassB + _invIB * _rB.X * _rB.X + _gamma; _mass = K.Inverse; _C = cB + _rB - _targetA; _C *= _beta; // Cheat with some damping wB *= 0.98f; // if (Settings.EnableWarmstarting) // { _impulse *= data.step.dtRatio; vB += _invMassB * _impulse; wB += _invIB * MathUtils.Cross(_rB, _impulse); // } // else // _impulse = FVector2.Zero; data.velocities[_indexB].v = vB; data.velocities[_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; float mA = m_invMassA, mB = m_invMassB; float iA = m_invIA, iB = m_invIB; 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); // Compute the effective masses. FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA); FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB); FVector2 d1 = cB + rB - cA - rA; // Point to line constraint { m_ay = MathUtils.Mul(qA, m_localYAxisA); m_sAy = MathUtils.Cross(d1 + rA, m_ay); m_sBy = MathUtils.Cross(rB, m_ay); m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy; if (m_mass > 0.0f) { m_mass = 1.0f / m_mass; } } // Spring constraint m_springMass = 0.0f; m_bias = 0.0f; m_gamma = 0.0f; if (SpringFrequencyHz > 0.0f) { m_ax = MathUtils.Mul(qA, m_localXAxisA); m_sAx = MathUtils.Cross(d1 + rA, m_ax); m_sBx = MathUtils.Cross(rB, m_ax); float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx; if (invMass > 0.0f) { m_springMass = 1.0f / invMass; float C = FVector2.Dot(d1, m_ax); // Frequency float omega = 2.0f * Settings.Pi * SpringFrequencyHz; // Damping coefficient float d = 2.0f * m_springMass * SpringDampingRatio * omega; // Spring stiffness float k = m_springMass * omega * omega; // magic formulas float h = data.step.dt; m_gamma = h * (d + h * k); if (m_gamma > 0.0f) { m_gamma = 1.0f / m_gamma; } m_bias = C * h * k * m_gamma; m_springMass = invMass + m_gamma; if (m_springMass > 0.0f) { m_springMass = 1.0f / m_springMass; } } } else { m_springImpulse = 0.0f; } // Rotational motor if (m_enableMotor) { m_motorMass = iA + iB; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } } else { m_motorMass = 0.0f; m_motorImpulse = 0.0f; } if (Settings.EnableWarmstarting) { // Account for variable time step. m_impulse *= data.step.dtRatio; m_springImpulse *= data.step.dtRatio; m_motorImpulse *= data.step.dtRatio; FVector2 P = m_impulse * m_ay + m_springImpulse * m_ax; float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse; float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse; vA -= m_invMassA * P; wA -= m_invIA * LA; vB += m_invMassB * P; wB += m_invIB * LB; } else { m_impulse = 0.0f; m_springImpulse = 0.0f; m_motorImpulse = 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 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); }
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; }
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); }
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); // 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; bool fixedRotation = (iA + iB == 0.0f); m_mass.ex.X = mA + mB + m_rA.Y * m_rA.Y * iA + m_rB.Y * m_rB.Y * iB; m_mass.ey.X = -m_rA.Y * m_rA.X * iA - m_rB.Y * m_rB.X * iB; m_mass.ez.X = -m_rA.Y * iA - m_rB.Y * iB; m_mass.ex.Y = m_mass.ey.X; m_mass.ey.Y = mA + mB + m_rA.X * m_rA.X * iA + m_rB.X * m_rB.X * iB; m_mass.ez.Y = m_rA.X * iA + m_rB.X * iB; m_mass.ex.Z = m_mass.ez.X; m_mass.ey.Z = m_mass.ez.Y; m_mass.ez.Z = iA + iB; m_motorMass = iA + iB; if (m_motorMass > 0.0f) { m_motorMass = 1.0f / m_motorMass; } if (_enableMotor == false || fixedRotation) { _motorImpulse = 0.0f; } if (_enableLimit && fixedRotation == false) { float jointAngle = aB - aA - ReferenceAngle; if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) { _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 (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; _motorImpulse *= data.step.dtRatio; FVector2 P = new FVector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(m_rA, P) + MotorImpulse + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(m_rB, P) + MotorImpulse + _impulse.Z); } else { _impulse = FVector3.Zero; _motorImpulse = 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; }
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); }
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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; 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); // Get the pulley axes. _uA = cA + _rA - WorldAnchorA; _uB = cB + _rB - WorldAnchorB; float lengthA = _uA.Length(); float lengthB = _uB.Length(); if (lengthA > 10.0f * Settings.LinearSlop) { _uA *= 1.0f / lengthA; } else { _uA = Vector2.Zero; } if (lengthB > 10.0f * Settings.LinearSlop) { _uB *= 1.0f / lengthB; } else { _uB = Vector2.Zero; } // Compute effective mass. float ruA = MathUtils.Cross(_rA, _uA); float ruB = MathUtils.Cross(_rB, _uB); float mA = _invMassA + _invIA * ruA * ruA; float mB = _invMassB + _invIB * ruB * ruB; _mass = mA + Ratio * Ratio * mB; if (_mass > 0.0f) { _mass = 1.0f / _mass; } if (Settings.EnableWarmstarting) { // Scale impulses to support variable time steps. _impulse *= data.step.dtRatio; // Warm starting. Vector2 PA = -(_impulse) * _uA; Vector2 PB = (-Ratio * _impulse) * _uB; vA += _invMassA * PA; wA += _invIA * MathUtils.Cross(_rA, PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(_rB, PB); } 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 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; 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 (_enableMotor == false || fixedRotation) { _motorImpulse = 0.0f; } if (_enableLimit && fixedRotation == false) { float jointAngle = aB - aA - ReferenceAngle; if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop) { _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 (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _impulse *= data.step.dtRatio; _motorImpulse *= data.step.dtRatio; Vector2 P = new Vector2(_impulse.X, _impulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z); } else { _impulse = Vector3.Zero; _motorImpulse = 0.0f; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; data.velocities[_indexB].v = vB; data.velocities[_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; Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 vA = data.velocities[_indexA].v; float wA = data.velocities[_indexA].w; Vector2 cB = data.positions[_indexB].c; 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); _u = cB + _rB - cA - _rA; // Handle singularity. float length = _u.Length(); if (length > Settings.LinearSlop) { _u *= 1.0f / length; } else { _u = Vector2.Zero; } float crAu = MathUtils.Cross(_rA, _u); float crBu = MathUtils.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 (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; Vector2 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) { 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 angularError = 0.0f; float positionError; bool fixedRotation = (_invIA + _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, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = (float)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 + Settings.AngularSlop, -Settings.MaxAngularCorrection, 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.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } aA -= _invIA * limitImpulse; aB += _invIB * limitImpulse; } // Solve point-to-point constraint. { qA.Set(aA); qB.Set(aB); Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 C = cB + rB - cA - rA; positionError = C.Length(); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _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; Vector2 impulse = -K.Solve(C); cA -= mA * impulse; aA -= iA * MathUtils.Cross(rA, impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(rB, impulse); } 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); }
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); // Compute the effective mass matrix. _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; 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; _linearMass = K.Inverse; _angularMass = iA + iB; if (_angularMass > 0.0f) { _angularMass = 1.0f / _angularMass; } if (Settings.EnableWarmstarting) { // Scale impulses to support a variable time step. _linearImpulse *= data.step.dtRatio; _angularImpulse *= data.step.dtRatio; Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y); vA -= mA * P; wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse); vB += mB * P; wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse); } else { _linearImpulse = Vector2.Zero; _angularImpulse = 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) { 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; }
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); }
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); Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 d = (cB - cA) + rB - rA; Vector2 ay = MathUtils.Mul(qA, _localYAxis); float sAy = MathUtils.Cross(d + rA, ay); float sBy = MathUtils.Cross(rB, ay); float C = Vector2.Dot(d, ay); float k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy; float impulse; if (k != 0.0f) { impulse = -C / k; } else { impulse = 0.0f; } Vector2 P = impulse * ay; float LA = impulse * sAy; float LB = impulse * sBy; cA -= _invMassA * P; aA -= _invIA * LA; cB += _invMassB * P; aB += _invIB * LB; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return Math.Abs(C) <= Settings.LinearSlop; }
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; Vector2 cA = data.Positions[_indexA].C; float aA = data.Positions[_indexA].A; Vector2 vA = data.Velocities[_indexA].V; float wA = data.Velocities[_indexA].W; Vector2 cB = data.Positions[_indexB].C; 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); _u = cB + _rB - cA - _rA; // Handle singularity. float length = _u.Length(); if (length > Settings.LinearSlop) { _u *= 1.0f / length; } else { _u = Vector2.Zero; } float crAu = MathUtils.Cross(_rA, _u); float crBu = MathUtils.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 (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; } #pragma warning disable 162 // ReSharper disable once ConditionIsAlwaysTrueOrFalse if (Settings.EnableWarmstarting) { // Scale the impulse to support a variable time step. _impulse *= data.Step.dtRatio; Vector2 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; } #pragma warning restore 162 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) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Vector2 cC = data.positions[_indexC].c; float aC = data.positions[_indexC].a; Vector2 cD = data.positions[_indexD].c; float aD = data.positions[_indexD].a; Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD); const float linearError = 0.0f; float coordinateA, coordinateB; Vector2 JvAC, JvBD; float JwA, JwB, JwC, JwD; float mass = 0.0f; if (_typeA == JointType.Revolute) { JvAC = Vector2.Zero; JwA = 1.0f; JwC = 1.0f; mass += _iA + _iC; coordinateA = aA - aC - _referenceAngleA; } else { Vector2 u = MathUtils.Mul(qC, _localAxisC); Vector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC); Vector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA); JvAC = u; JwC = MathUtils.Cross(rC, u); JwA = MathUtils.Cross(rA, u); mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA; Vector2 pC = _localAnchorC - _lcC; Vector2 pA = MathUtils.MulT(qC, rA + (cA - cC)); coordinateA = Vector2.Dot(pA - pC, _localAxisC); } if (_typeB == JointType.Revolute) { JvBD = Vector2.Zero; JwB = _ratio; JwD = _ratio; mass += _ratio * _ratio * (_iB + _iD); coordinateB = aB - aD - _referenceAngleB; } else { Vector2 u = MathUtils.Mul(qD, _localAxisD); Vector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD); Vector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB); JvBD = _ratio * u; JwD = _ratio * MathUtils.Cross(rD, u); JwB = _ratio * MathUtils.Cross(rB, u); mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; Vector2 pD = _localAnchorD - _lcD; Vector2 pB = MathUtils.MulT(qD, rB + (cB - cD)); coordinateB = Vector2.Dot(pB - pD, _localAxisD); } float C = (coordinateA + _ratio * coordinateB) - _constant; float impulse = 0.0f; if (mass > 0.0f) { impulse = -C / mass; } cA += _mA * impulse * JvAC; aA += _iA * impulse * JwA; cB += _mB * impulse * JvBD; aB += _iB * impulse * JwB; cC -= _mC * impulse * JvAC; aC -= _iC * impulse * JwC; cD -= _mD * impulse * JvBD; aD -= _iD * impulse * JwD; data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; data.positions[_indexC].c = cC; data.positions[_indexC].a = aC; data.positions[_indexD].c = cD; data.positions[_indexD].a = aD; // TODO_ERIN not implemented return linearError < Settings.LinearSlop; }
/// <summary>Inverse rotate a vector</summary> /// <param name="q">The rotation matrix</param> /// <param name="v">The value</param> public static Vector2 MulT(Rot q, Vector2 v) { return(new Vector2(q.c * v.X + q.s * v.Y, -q.s * v.X + q.c * 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); Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA); Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB); Vector2 u = cB + rB - cA - rA; float length = u.Length(); u.Normalize(); float C = length - MaxLength; C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection); float impulse = -_mass * C; Vector2 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 < Settings.LinearSlop; }
public static Vector2 MulT(ref Rot rot, Vector2 axis) { return(MulT(rot, axis)); }
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_indexC = m_bodyC.m_islandIndex; m_indexD = m_bodyD.m_islandIndex; m_lcA.set(m_bodyA.m_sweep.localCenter); m_lcB.set(m_bodyB.m_sweep.localCenter); m_lcC.set(m_bodyC.m_sweep.localCenter); m_lcD.set(m_bodyD.m_sweep.localCenter); m_mA = m_bodyA.m_invMass; m_mB = m_bodyB.m_invMass; m_mC = m_bodyC.m_invMass; m_mD = m_bodyD.m_invMass; m_iA = m_bodyA.m_invI; m_iB = m_bodyB.m_invI; m_iC = m_bodyC.m_invI; m_iD = m_bodyD.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; // Vec2 cC = data.positions[m_indexC].c; double aC = data.positions[m_indexC].a; Vec2 vC = data.velocities[m_indexC].v; double wC = data.velocities[m_indexC].w; // Vec2 cD = data.positions[m_indexD].c; double aD = data.positions[m_indexD].a; Vec2 vD = data.velocities[m_indexD].v; double wD = data.velocities[m_indexD].w; Rot qA = pool.popRot(), qB = pool.popRot(), qC = pool.popRot(), qD = pool.popRot(); qA.set(aA); qB.set(aB); qC.set(aC); qD.set(aD); m_mass = 0.0d; Vec2 temp = pool.popVec2(); if (m_typeA == JointType.REVOLUTE) { m_JvAC.setZero(); m_JwA = 1.0d; m_JwC = 1.0d; m_mass += m_iA + m_iC; } else { Vec2 rC = pool.popVec2(); Vec2 rA = pool.popVec2(); Rot.mulToOutUnsafe(qC, m_localAxisC, m_JvAC); Rot.mulToOutUnsafe(qC, temp.set(m_localAnchorC).subLocal(m_lcC), rC); Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_lcA), rA); m_JwC = Vec2.cross(rC, m_JvAC); m_JwA = Vec2.cross(rA, m_JvAC); m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA; pool.pushVec2(2); } if (m_typeB == JointType.REVOLUTE) { m_JvBD.setZero(); m_JwB = m_ratio; m_JwD = m_ratio; m_mass += m_ratio * m_ratio * (m_iB + m_iD); } else { Vec2 u = pool.popVec2(); Vec2 rD = pool.popVec2(); Vec2 rB = pool.popVec2(); Rot.mulToOutUnsafe(qD, m_localAxisD, u); Rot.mulToOutUnsafe(qD, temp.set(m_localAnchorD).subLocal(m_lcD), rD); Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_lcB), rB); m_JvBD.set(u).mulLocal(m_ratio); m_JwD = m_ratio * Vec2.cross(rD, u); m_JwB = m_ratio * Vec2.cross(rB, u); m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB; pool.pushVec2(3); } // Compute effective mass. m_mass = m_mass > 0.0d ? 1.0d / m_mass : 0.0d; if (data.step.warmStarting) { vA.x += (m_mA * m_impulse) * m_JvAC.x; vA.y += (m_mA * m_impulse) * m_JvAC.y; wA += m_iA * m_impulse * m_JwA; vB.x += (m_mB * m_impulse) * m_JvBD.x; vB.y += (m_mB * m_impulse) * m_JvBD.y; wB += m_iB * m_impulse * m_JwB; vC.x -= (m_mC * m_impulse) * m_JvAC.x; vC.y -= (m_mC * m_impulse) * m_JvAC.y; wC -= m_iC * m_impulse * m_JwC; vD.x -= (m_mD * m_impulse) * m_JvBD.x; vD.y -= (m_mD * m_impulse) * m_JvBD.y; wD -= m_iD * m_impulse * m_JwD; } else { m_impulse = 0.0d; } pool.pushVec2(1); pool.pushRot(4); // data.velocities[m_indexA].v = vA; data.velocities[m_indexA].w = wA; // data.velocities[m_indexB].v = vB; data.velocities[m_indexB].w = wB; // data.velocities[m_indexC].v = vC; data.velocities[m_indexC].w = wC; // data.velocities[m_indexD].v = vD; data.velocities[m_indexD].w = wD; }