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);
}
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;
}
}
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);
}
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
Vector2 pLocal = Complex.Divide(point - transform.p, ref transform.q);
for (int i = 0; i < Vertices.Count; ++i)
{
float dot = Vector2.Dot(Normals[i], pLocal - Vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
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 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);
}
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;
}
}
}
/// <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;
}
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);
}
// 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);
}
public override float ComputeSubmergedArea(ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc)
{
sc = Vector2.Zero;
//Transform plane into shape co-ordinates
Vector2 normalL = Complex.Divide(ref normal, ref xf.q);
float offsetL = offset - Vector2.Dot(normal, xf.p);
float[] depths = new float[Settings.MaxPolygonVertices];
int diveCount = 0;
int intoIndex = -1;
int outoIndex = -1;
bool lastSubmerged = false;
int i;
for (i = 0; i < Vertices.Count; i++)
{
depths[i] = Vector2.Dot(normalL, Vertices[i]) - offsetL;
bool isSubmerged = depths[i] < -Settings.Epsilon;
if (i > 0)
{
if (isSubmerged)
{
if (!lastSubmerged)
{
intoIndex = i - 1;
diveCount++;
}
}
else
{
if (lastSubmerged)
{
outoIndex = i - 1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch (diveCount)
{
case 0:
if (lastSubmerged)
{
//Completely submerged
sc = Transform.Multiply(MassData.Centroid, ref xf);
return(MassData.Mass / Density);
}
//Completely dry
return(0);
case 1:
if (intoIndex == -1)
{
intoIndex = Vertices.Count - 1;
}
else
{
outoIndex = Vertices.Count - 1;
}
break;
}
int intoIndex2 = (intoIndex + 1) % Vertices.Count;
int outoIndex2 = (outoIndex + 1) % Vertices.Count;
float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
Vector2 intoVec = new Vector2(Vertices[intoIndex].X * (1 - intoLambda) + Vertices[intoIndex2].X * intoLambda, Vertices[intoIndex].Y * (1 - intoLambda) + Vertices[intoIndex2].Y * intoLambda);
Vector2 outoVec = new Vector2(Vertices[outoIndex].X * (1 - outoLambda) + Vertices[outoIndex2].X * outoLambda, Vertices[outoIndex].Y * (1 - outoLambda) + Vertices[outoIndex2].Y * outoLambda);
//Initialize accumulator
float area = 0;
Vector2 center = new Vector2(0, 0);
Vector2 p2 = Vertices[intoIndex2];
const float k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while (i != outoIndex2)
{
i = (i + 1) % Vertices.Count;
Vector2 p3;
if (i == outoIndex2)
{
p3 = outoVec;
}
else
{
p3 = Vertices[i];
}
//Add the triangle formed by intoVec,p2,p3
{
Vector2 e1 = p2 - intoVec;
Vector2 e2 = p3 - intoVec;
float D = MathUtils.Cross(ref e1, ref e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (intoVec + p2 + p3);
}
p2 = p3;
}
//Normalize and transform centroid
center *= 1.0f / area;
sc = Transform.Multiply(ref center, ref xf);
return(area);
}
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;
}
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;
}
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;
}
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;
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
output = new RayCastOutput();
// Put the ray into the edge'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;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 normal = new Vector2(e.Y, -e.X); //TODO: Could possibly cache the normal.
normal = Vector2.Normalize(normal);
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.MaxFraction < t)
{
return(false);
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.Fraction = t;
if (numerator > 0.0f)
{
output.Normal = -normal;
}
else
{
output.Normal = normal;
}
return(true);
}
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = new SimplexCache();
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++GJKCalls;
}
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, ref input.ProxyA, ref input.TransformA, ref input.ProxyB, ref input.TransformB);
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3 <int> saveA = new FixedArray3 <int>();
FixedArray3 <int> saveB = new FixedArray3 <int>();
//float distanceSqr1 = Settings.MaxFloat;
// Main iteration loop.
int iter = 0;
while (iter < Settings.MaxGJKIterations)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = simplex.V[i].IndexA;
saveB[i] = simplex.V[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;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//float distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// 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;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = simplex.V[simplex.Count];
vertex.IndexA = input.ProxyA.GetSupport(-Complex.Divide(ref d, ref input.TransformA.q));
vertex.WA = Transform.Multiply(input.ProxyA.Vertices[vertex.IndexA], ref input.TransformA);
vertex.IndexB = input.ProxyB.GetSupport(Complex.Divide(ref d, ref input.TransformB.q));
vertex.WB = Transform.Multiply(input.ProxyB.Vertices[vertex.IndexB], ref input.TransformB);
vertex.W = vertex.WB - vertex.WA;
simplex.V[simplex.Count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++GJKIters;
}
// 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;
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
GJKMaxIters = Math.Max(GJKMaxIters, iter);
}
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).Length();
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float rB = input.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;
Vector2 normal = Vector2.Normalize(output.PointB - output.PointA);
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 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;
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 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 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;
}
/// <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;
}
}
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);
}
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;
}
}
}
}
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;
}
