public override bool TestPoint(ref Transform transform, ref Vector2 point) { Vector2 center = transform.p + Complex.Multiply(ref _position, ref transform.q); Vector2 d = point - center; return(Vector2.Dot(d, d) <= _2radius); }
/// <summary> /// Rotate the control points by the defined value in radians. /// </summary> /// <param name="value">The amount to rotate by in radians.</param> public void Rotate(float value) { var rotation = Complex.FromAngle(value); for (int i = 0; i < ControlPoints.Count; i++) { ControlPoints[i] = Complex.Multiply(ControlPoints[i], ref rotation); } }
public static void Initialize(ContactPositionConstraint pc, ref Transform xfA, ref Transform xfB, int index, out Vector2 normal, out Vector2 point, out float separation) { Debug.Assert(pc.pointCount > 0); switch (pc.type) { case ManifoldType.Circles: { Vector2 pointA = Transform.Multiply(ref pc.localPoint, ref xfA); Vector2 pointB = Transform.Multiply(pc.localPoints[0], ref xfB); normal = pointB - pointA; // Handle zero normalization if (normal != Vector2.Zero) { normal = Vector2.Normalize(normal); } point = 0.5f * (pointA + pointB); separation = Vector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB; } break; case ManifoldType.FaceA: { Complex.Multiply(ref pc.localNormal, ref xfA.q, out normal); Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfA); Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfB); separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; } break; case ManifoldType.FaceB: { Complex.Multiply(ref pc.localNormal, ref xfB.q, out normal); Vector2 planePoint = Transform.Multiply(ref pc.localPoint, ref xfB); Vector2 clipPoint = Transform.Multiply(pc.localPoints[index], ref xfA); separation = Vector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; // Ensure normal points from A to B normal = -normal; } break; default: normal = Vector2.Zero; point = Vector2.Zero; separation = 0; break; } }
/// <summary> /// Get the interpolated transform at a specific time. /// </summary> /// <param name="xfb">The transform.</param> /// <param name="beta">beta is a factor in [0,1], where 0 indicates alpha0.</param> public void GetTransform(out Transform xfb, float beta) { xfb = new Transform(); xfb.p.X = (1.0f - beta) * C0.X + beta * C.X; xfb.p.Y = (1.0f - beta) * C0.Y + beta * C.Y; float angle = (1.0f - beta) * A0 + beta * A; xfb.q.Phase = angle; // Shift to origin xfb.p -= Complex.Multiply(ref LocalCenter, ref xfb.q); }
public static float Evaluate(int indexA, int indexB, float t) { Transform xfA, xfB; _sweepA.GetTransform(out xfA, t); _sweepB.GetTransform(out xfB, t); switch (_type) { case SeparationFunctionType.Points: { Vector2 localPointA = _proxyA.Vertices[indexA]; Vector2 localPointB = _proxyB.Vertices[indexB]; Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA); Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB); float separation = Vector2.Dot(pointB - pointA, _axis); return(separation); } case SeparationFunctionType.FaceA: { Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q); Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA); Vector2 localPointB = _proxyB.Vertices[indexB]; Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB); float separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case SeparationFunctionType.FaceB: { Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q); Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB); Vector2 localPointA = _proxyA.Vertices[indexA]; Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA); float separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: Debug.Assert(false); return(0.0f); } }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 d = (cB - cA) + rB - rA; Vector2 ay = Complex.Multiply(ref _localYAxis, ref qA); float sAy = MathUtils.Cross(d + rA, ay); float sBy = MathUtils.Cross(ref rB, ref 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 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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 u = cB + rB - cA - rA; float length = u.Length(); u = Vector2.Normalize(u); 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(ref rA, ref P); cB += _invMassB * P; aB += _invIB * MathUtils.Cross(ref rB, ref 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); }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { // Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius output = new RayCastOutput(); Vector2 position = transform.p + Complex.Multiply(ref _position, ref transform.q); Vector2 s = input.Point1 - position; float b = Vector2.Dot(s, s) - _2radius; // Solve quadratic equation. Vector2 r = input.Point2 - input.Point1; float c = Vector2.Dot(s, r); float rr = Vector2.Dot(r, r); float sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < Settings.Epsilon) { return(false); } // Find the point of intersection of the line with the circle. float a = -(c + (float)Math.Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.MaxFraction * rr) { a /= rr; output.Fraction = a; //TODO: Check results here output.Normal = Vector2.Normalize(s + a * r); return(true); } return(false); }
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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // 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 = Constant.Tau * 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 if (K.ez.Z == 0.0f) { K.GetInverse22(ref _mass); _gamma = 0.0f; _bias = 0.0f; } else { K.GetSymInverse33(ref _mass); _gamma = 0.0f; _bias = 0.0f; } if (data.step.warmStarting) { // 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(ref _rA, ref P) + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(ref _rB, ref 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 void Multiply(ref Transform left, Complex right, out Transform result) { result.p = Complex.Multiply(ref left.p, ref right); result.q = Complex.Multiply(ref left.q, ref right); }
public static Transform Multiply(ref Transform left, ref Transform right) { return(new Transform( Complex.Multiply(ref left.p, ref right.q) + right.p, Complex.Multiply(ref left.q, ref right.q))); }
internal override bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); float mA = _invMassA, mB = _invMassB; float iA = _invIA, iB = _invIB; Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); float positionError, angularError; Mat33 K = new Mat33(); K.ex.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; K.ey.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; K.ez.X = -rA.Y * iA - rB.Y * iB; K.ex.Y = K.ey.X; K.ey.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; K.ez.Y = rA.X * iA + rB.X * iB; K.ex.Z = K.ez.X; K.ey.Z = K.ez.Y; K.ez.Z = iA + iB; if (FrequencyHz > 0.0f) { Vector2 C1 = cB + rB - cA - rA; positionError = C1.Length(); angularError = 0.0f; Vector2 P = -K.Solve22(C1); cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } else { Vector2 C1 = cB + rB - cA - rA; float C2 = aB - aA - ReferenceAngle; positionError = C1.Length(); angularError = Math.Abs(C2); Vector3 C = new Vector3(C1.X, C1.Y, C2); Vector3 impulse; if (K.ez.Z <= 0.0f) { Vector2 impulse2 = -K.Solve22(C1); impulse = new Vector3(impulse2.X, impulse2.Y, 0.0f); } else { impulse = -K.Solve33(C); } Vector2 P = new Vector2(impulse.X, impulse.Y); cA -= mA * P; aA -= iA * (MathUtils.Cross(ref rA, ref P) + impulse.Z); cB += mB * P; aB += iB * (MathUtils.Cross(ref rB, ref P) + impulse.Z); } data.positions[_indexA].c = cA; data.positions[_indexA].a = aA; data.positions[_indexB].c = cB; data.positions[_indexB].a = aB; return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Complex qC = Complex.FromAngle(aC); Complex qD = Complex.FromAngle(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 = Complex.Multiply(ref _localAxisC, ref qC); Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC); Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA); JvAC = u; JwC = MathUtils.Cross(ref rC, ref u); JwA = MathUtils.Cross(ref rA, ref u); mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA; Vector2 pC = _localAnchorC - _lcC; Vector2 pA = Complex.Divide(rA + (cA - cC), ref qC); 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 = Complex.Multiply(ref _localAxisD, ref qD); Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD); Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB); JvBD = _ratio * u; JwD = _ratio * MathUtils.Cross(ref rD, ref u); JwB = _ratio * MathUtils.Cross(ref rB, ref u); mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB; Vector2 pD = _localAnchorD - _lcD; Vector2 pB = Complex.Divide(rB + (cB - cD), ref qD); 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 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; Complex qA = Complex.FromAngle(aA); float mass = BodyA.Mass; // Frequency float omega = Constant.Tau * 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 = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); // 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 (data.step.warmStarting) { _impulse *= data.step.dtRatio; vA += _invMassA * _impulse; wA += _invIA * MathUtils.Cross(ref _rA, ref _impulse); } else { _impulse = Vector2.Zero; } data.velocities[_indexA].v = vA; data.velocities[_indexA].w = wA; }
/// <summary> /// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the Shapes /// that generated the manifold. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="xfA">The transform for A.</param> /// <param name="radiusA">The radius for A.</param> /// <param name="xfB">The transform for B.</param> /// <param name="radiusB">The radius for B.</param> /// <param name="normal">World vector pointing from A to B</param> /// <param name="points">Torld contact point (point of intersection).</param> public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 <Vector2> points) { normal = Vector2.Zero; points = new FixedArray2 <Vector2>(); if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { normal = new Vector2(1.0f, 0.0f); Vector2 pointA = Transform.Multiply(ref manifold.LocalPoint, ref xfA); Vector2 pointB = Transform.Multiply(manifold.Points[0].LocalPoint, ref xfB); if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon) { normal = Vector2.Normalize(pointB - pointA); } Vector2 cA = pointA + radiusA * normal; Vector2 cB = pointB - radiusB * normal; points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { normal = Complex.Multiply(ref manifold.LocalNormal, ref xfA.q); Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfA); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfB); Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { normal = Complex.Multiply(ref manifold.LocalNormal, ref xfB.q); Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfB); for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfA); Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); } // Ensure normal points from A to B. normal = -normal; } break; } }
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; 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 = 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. { Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); 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(ref rA, ref impulse); cB += mB * impulse; aB += iB * MathUtils.Cross(ref rB, ref 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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // 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 (data.step.warmStarting) { // 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(ref _rA, ref P) + MotorImpulse + _impulse.Z); vB += mB * P; wB += iB * (MathUtils.Cross(ref _rB, ref 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 bool SolvePositionConstraints(ref SolverData data) { Vector2 cA = data.positions[_indexA].c; float aA = data.positions[_indexA].a; Vector2 cB = data.positions[_indexB].c; float aB = data.positions[_indexB].a; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // 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(ref rA, ref uA); float ruB = MathUtils.Cross(ref rB, ref 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(ref rA, ref PA); cB += _invMassB * PB; aB += _invIB * MathUtils.Cross(ref rB, ref 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) { _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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); // 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(ref _rA, ref _uA); float ruB = MathUtils.Cross(ref _rB, ref _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 (data.step.warmStarting) { // 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(ref _rA, ref PA); vB += _invMassB * PB; wB += _invIB * MathUtils.Cross(ref _rB, ref 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; }
/// <summary> /// Requires two existing revolute or prismatic joints (any combination will work). /// The provided joints must attach a dynamic body to a static body. /// </summary> /// <param name="jointA">The first joint.</param> /// <param name="jointB">The second joint.</param> /// <param name="ratio">The ratio.</param> /// <param name="bodyA">The first body</param> /// <param name="bodyB">The second body</param> public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f) { JointType = JointType.Gear; BodyA = bodyA; BodyB = bodyB; JointA = jointA; JointB = jointB; Ratio = ratio; _typeA = jointA.JointType; _typeB = jointB.JointType; Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic); Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic); float coordinateA, coordinateB; // TODO_ERIN there might be some problem with the joint edges in b2Joint. _bodyC = JointA.BodyA; _bodyA = JointA.BodyB; // Get geometry of joint1 Transform xfA = _bodyA._xf; float aA = _bodyA._sweep.A; Transform xfC = _bodyC._xf; float aC = _bodyC._sweep.A; if (_typeA == JointType.Revolute) { RevoluteJoint revolute = (RevoluteJoint)jointA; _localAnchorC = revolute.LocalAnchorA; _localAnchorA = revolute.LocalAnchorB; _referenceAngleA = revolute.ReferenceAngle; _localAxisC = Vector2.Zero; coordinateA = aA - aC - _referenceAngleA; } else { PrismaticJoint prismatic = (PrismaticJoint)jointA; _localAnchorC = prismatic.LocalAnchorA; _localAnchorA = prismatic.LocalAnchorB; _referenceAngleA = prismatic.ReferenceAngle; _localAxisC = prismatic.LocalXAxis; Vector2 pC = _localAnchorC; Vector2 pA = Complex.Divide(Complex.Multiply(ref _localAnchorA, ref xfA.q) + (xfA.p - xfC.p), ref xfC.q); coordinateA = Vector2.Dot(pA - pC, _localAxisC); } _bodyD = JointB.BodyA; _bodyB = JointB.BodyB; // Get geometry of joint2 Transform xfB = _bodyB._xf; float aB = _bodyB._sweep.A; Transform xfD = _bodyD._xf; float aD = _bodyD._sweep.A; if (_typeB == JointType.Revolute) { RevoluteJoint revolute = (RevoluteJoint)jointB; _localAnchorD = revolute.LocalAnchorA; _localAnchorB = revolute.LocalAnchorB; _referenceAngleB = revolute.ReferenceAngle; _localAxisD = Vector2.Zero; coordinateB = aB - aD - _referenceAngleB; } else { PrismaticJoint prismatic = (PrismaticJoint)jointB; _localAnchorD = prismatic.LocalAnchorA; _localAnchorB = prismatic.LocalAnchorB; _referenceAngleB = prismatic.ReferenceAngle; _localAxisD = prismatic.LocalXAxis; Vector2 pD = _localAnchorD; Vector2 pB = Complex.Divide(Complex.Multiply(ref _localAnchorB, ref xfB.q) + (xfB.p - xfD.p), ref xfD.q); coordinateB = Vector2.Dot(pB - pD, _localAxisD); } _ratio = ratio; _constant = coordinateA + _ratio * coordinateB; _impulse = 0.0f; }
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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); Complex qC = Complex.FromAngle(aC); Complex qD = Complex.FromAngle(aD); _mass = 0.0f; if (_typeA == JointType.Revolute) { _JvAC = Vector2.Zero; _JwA = 1.0f; _JwC = 1.0f; _mass += _iA + _iC; } else { Vector2 u = Complex.Multiply(ref _localAxisC, ref qC); Vector2 rC = Complex.Multiply(_localAnchorC - _lcC, ref qC); Vector2 rA = Complex.Multiply(_localAnchorA - _lcA, ref qA); _JvAC = u; _JwC = MathUtils.Cross(ref rC, ref u); _JwA = MathUtils.Cross(ref rA, ref 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 = Complex.Multiply(ref _localAxisD, ref qD); Vector2 rD = Complex.Multiply(_localAnchorD - _lcD, ref qD); Vector2 rB = Complex.Multiply(_localAnchorB - _lcB, ref qB); _JvBD = _ratio * u; _JwD = _ratio * MathUtils.Cross(ref rD, ref u); _JwB = _ratio * MathUtils.Cross(ref rB, ref 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 (data.step.warmStarting) { 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; }
public static void Set(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1) { _localPoint = Vector2.Zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA); Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB); _axis = Vector2.Normalize(pointB - pointA); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; Vector2 a = localPointB2 - localPointB1; _axis = new Vector2(a.Y, -a.X); _axis = Vector2.Normalize(_axis); Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q); _localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB); Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; } } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; Vector2 a = localPointA2 - localPointA1; _axis = new Vector2(a.Y, -a.X); _axis = Vector2.Normalize(_axis); Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q); _localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA); Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; } } }
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { output = new RayCastOutput(); // Put the ray into the polygon's frame of reference. Vector2 p1 = Complex.Divide(input.Point1 - transform.p, ref transform.q); Vector2 p2 = Complex.Divide(input.Point2 - transform.p, ref transform.q); Vector2 d = p2 - p1; float lower = 0.0f, upper = input.MaxFraction; int index = -1; for (int i = 0; i < Vertices.Count; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 float numerator = Vector2.Dot(Normals[i], Vertices[i] - p1); float denominator = Vector2.Dot(Normals[i], d); if (denominator == 0.0f) { if (numerator < 0.0f) { return(false); } } else { // Note: we want this predicate without division: // lower < numerator / denominator, where denominator < 0 // Since denominator < 0, we have to flip the inequality: // lower < numerator / denominator <==> denominator * lower > numerator. if (denominator < 0.0f && numerator < lower * denominator) { // Increase lower. // The segment enters this half-space. lower = numerator / denominator; index = i; } else if (denominator > 0.0f && numerator < upper * denominator) { // Decrease upper. // The segment exits this half-space. upper = numerator / denominator; } } // The use of epsilon here causes the assert on lower to trip // in some cases. Apparently the use of epsilon was to make edge // shapes work, but now those are handled separately. //if (upper < lower - b2_epsilon) if (upper < lower) { return(false); } } Debug.Assert(0.0f <= lower && lower <= input.MaxFraction); if (index >= 0) { output.Fraction = lower; output.Normal = Complex.Multiply(Normals[index], ref transform.q); return(true); } return(false); }
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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); _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(ref _rA, ref _u); float crBu = MathUtils.Cross(ref _rB, ref _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 = Constant.Tau * 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 (data.step.warmStarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(ref _rA, ref P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(ref _rB, ref 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 void InitializeVelocityConstraints() { for (int i = 0; i < _count; ++i) { ContactVelocityConstraint vc = _velocityConstraints[i]; ContactPositionConstraint pc = _positionConstraints[i]; float radiusA = pc.radiusA; float radiusB = pc.radiusB; Manifold manifold = _contacts[vc.contactIndex].Manifold; int indexA = vc.indexA; int indexB = vc.indexB; float mA = vc.invMassA; float mB = vc.invMassB; float iA = vc.invIA; float iB = vc.invIB; Vector2 localCenterA = pc.localCenterA; Vector2 localCenterB = pc.localCenterB; Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 vA = _velocities[indexA].v; float wA = _velocities[indexA].w; Vector2 cB = _positions[indexB].c; float aB = _positions[indexB].a; Vector2 vB = _velocities[indexB].v; float wB = _velocities[indexB].w; Debug.Assert(manifold.PointCount > 0); Transform xfA = new Transform(Vector2.Zero, aA); Transform xfB = new Transform(Vector2.Zero, aB); xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q); xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q); Vector2 normal; FixedArray2 <Vector2> points; WorldManifold.Initialize(ref manifold, ref xfA, radiusA, ref xfB, radiusB, out normal, out points); vc.normal = normal; Vector2 tangent = MathUtils.Rot270(ref vc.normal); int pointCount = vc.pointCount; for (int j = 0; j < pointCount; ++j) { VelocityConstraintPoint vcp = vc.points[j]; vcp.rA = points[j] - cA; vcp.rB = points[j] - cB; float rnA = MathUtils.Cross(ref vcp.rA, ref vc.normal); float rnB = MathUtils.Cross(ref vcp.rB, ref vc.normal); float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; vcp.normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; float rtA = MathUtils.Cross(ref vcp.rA, ref tangent); float rtB = MathUtils.Cross(ref vcp.rB, ref tangent); float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; vcp.tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; // Setup a velocity bias for restitution. vcp.velocityBias = 0.0f; float vRel = Vector2.Dot(vc.normal, vB + MathUtils.Cross(wB, ref vcp.rB) - vA - MathUtils.Cross(wA, ref vcp.rA)); if (vRel < -Settings.VelocityThreshold) { vcp.velocityBias = -vc.restitution * vRel; } } // If we have two points, then prepare the block solver. if (vc.pointCount == 2) { VelocityConstraintPoint vcp1 = vc.points[0]; VelocityConstraintPoint vcp2 = vc.points[1]; float rn1A = MathUtils.Cross(ref vcp1.rA, ref vc.normal); float rn1B = MathUtils.Cross(ref vcp1.rB, ref vc.normal); float rn2A = MathUtils.Cross(ref vcp2.rA, ref vc.normal); float rn2B = MathUtils.Cross(ref vcp2.rB, ref vc.normal); float k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B; float k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B; float k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 1000.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. vc.K.ex = new Vector2(k11, k12); vc.K.ey = new Vector2(k12, k22); vc.normalMass = vc.K.Inverse; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? vc.pointCount = 1; } } } }
private bool SolvePositionConstraints(int start, int end) { float minSeparation = 0.0f; for (int i = start; i < end; ++i) { ContactPositionConstraint pc = _positionConstraints[i]; #if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // Find lower order item. int orderedIndexA = pc.indexA; int orderedIndexB = pc.indexB; if (orderedIndexB < orderedIndexA) { orderedIndexA = pc.indexB; orderedIndexB = pc.indexA; } // Lock bodies. for (; ;) { if (Interlocked.CompareExchange(ref _locks[orderedIndexA], 1, 0) == 0) { if (Interlocked.CompareExchange(ref _locks[orderedIndexB], 1, 0) == 0) { break; } System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0); } #if NET40 || NET45 || NETSTANDARD2_0 Thread.Sleep(0); #endif } #endif int indexA = pc.indexA; int indexB = pc.indexB; Vector2 localCenterA = pc.localCenterA; float mA = pc.invMassA; float iA = pc.invIA; Vector2 localCenterB = pc.localCenterB; float mB = pc.invMassB; float iB = pc.invIB; int pointCount = pc.pointCount; Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 cB = _positions[indexB].c; float aB = _positions[indexB].a; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(Vector2.Zero, aA); Transform xfB = new Transform(Vector2.Zero, aB); xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q); xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q); Vector2 normal; Vector2 point; float separation; PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(ref rA, ref normal); float rnB = MathUtils.Cross(ref rB, ref normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } _positions[indexA].c = cA; _positions[indexA].a = aA; _positions[indexB].c = cB; _positions[indexB].a = aB; #if NET40 || NET45 || NETSTANDARD2_0 || PORTABLE40 || PORTABLE45 || W10 || W8_1 || WP8_1 // Unlock bodies. System.Threading.Interlocked.Exchange(ref _locks[orderedIndexB], 0); System.Threading.Interlocked.Exchange(ref _locks[orderedIndexA], 0); #endif } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -3.0f * 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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); // Compute the effective mass matrix. _rA = -Complex.Multiply(ref _localCenterA, ref qA); _rB = -Complex.Multiply(ref _localCenterB, ref qB); // 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 - Complex.Multiply(ref _linearOffset, ref qA); _angularError = aB - aA - _angularOffset; if (data.step.warmStarting) { // 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(ref _rA, ref P) + _angularImpulse); vB += mB * P; wB += iB * (MathUtils.Cross(ref _rB, ref 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; }
// Sequential position solver for position constraints. public bool SolveTOIPositionConstraints(int toiIndexA, int toiIndexB) { float minSeparation = 0.0f; for (int i = 0; i < _count; ++i) { ContactPositionConstraint pc = _positionConstraints[i]; int indexA = pc.indexA; int indexB = pc.indexB; Vector2 localCenterA = pc.localCenterA; Vector2 localCenterB = pc.localCenterB; int pointCount = pc.pointCount; float mA = 0.0f; float iA = 0.0f; if (indexA == toiIndexA || indexA == toiIndexB) { mA = pc.invMassA; iA = pc.invIA; } float mB = 0.0f; float iB = 0.0f; if (indexB == toiIndexA || indexB == toiIndexB) { mB = pc.invMassB; iB = pc.invIB; } Vector2 cA = _positions[indexA].c; float aA = _positions[indexA].a; Vector2 cB = _positions[indexB].c; float aB = _positions[indexB].a; // Solve normal constraints for (int j = 0; j < pointCount; ++j) { Transform xfA = new Transform(Vector2.Zero, aA); Transform xfB = new Transform(Vector2.Zero, aB); xfA.p = cA - Complex.Multiply(ref localCenterA, ref xfA.q); xfB.p = cB - Complex.Multiply(ref localCenterB, ref xfB.q); Vector2 normal; Vector2 point; float separation; PositionSolverManifold.Initialize(pc, ref xfA, ref xfB, j, out normal, out point, out separation); Vector2 rA = point - cA; Vector2 rB = point - cB; // Track max constraint error. minSeparation = Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = MathUtils.Clamp(Settings.Baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute the effective mass. float rnA = MathUtils.Cross(ref rA, ref normal); float rnB = MathUtils.Cross(ref rB, ref normal); float K = mA + mB + iA * rnA * rnA + iB * rnB * rnB; // Compute normal impulse float impulse = K > 0.0f ? -C / K : 0.0f; Vector2 P = impulse * normal; cA -= mA * P; aA -= iA * MathUtils.Cross(ref rA, ref P); cB += mB * P; aB += iB * MathUtils.Cross(ref rB, ref P); } _positions[indexA].c = cA; _positions[indexA].a = aA; _positions[indexB].c = cB; _positions[indexB].a = aB; } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * 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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); _rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); _rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); _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(ref _rA, ref _u); float crB = MathUtils.Cross(ref _rB, ref _u); float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB; _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f; if (data.step.warmStarting) { // Scale the impulse to support a variable time step. _impulse *= data.step.dtRatio; Vector2 P = _impulse * _u; vA -= _invMassA * P; wA -= _invIA * MathUtils.Cross(ref _rA, ref P); vB += _invMassB * P; wB += _invIB * MathUtils.Cross(ref _rB, ref 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 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; Complex qA = Complex.FromAngle(aA); Complex qB = Complex.FromAngle(aB); // Compute the effective masses. Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA); Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB); Vector2 d1 = cB + rB - cA - rA; // Point to line constraint { _ay = Complex.Multiply(ref _localYAxis, ref qA); _sAy = MathUtils.Cross(d1 + rA, _ay); _sBy = MathUtils.Cross(ref rB, ref _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 = Complex.Multiply(ref _localXAxis, ref qA); _sAx = MathUtils.Cross(d1 + rA, _ax); _sBx = MathUtils.Cross(ref rB, ref _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 = Constant.Tau * 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 (data.step.warmStarting) { // 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; }