internal override void SolveVelocityConstraints(ref SolverData data)
{
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FPVector2 vpA = vA + MathUtils.Cross(wA, _rA);
FPVector2 vpB = vB + MathUtils.Cross(wB, _rB);
FP Cdot = -FPVector2.Dot(_uA, vpA) - Ratio * FPVector2.Dot(_uB, vpB);
FP impulse = -_mass * Cdot;
_impulse += impulse;
FPVector2 PA = -impulse * _uA;
FPVector2 PB = -Ratio * impulse * _uB;
vA += _invMassA * PA;
wA += _invIA * MathUtils.Cross(_rA, PA);
vB += _invMassB * PB;
wB += _invIB * MathUtils.Cross(_rB, PB);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
FPVector2 vpA = vA + MathUtils.Cross(wA, _rA);
FPVector2 vpB = vB + MathUtils.Cross(wB, _rB);
FP Cdot = FPVector2.Dot(_u, vpB - vpA);
FP impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
FPVector2 P = impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public override bool TestPoint(ref Transform transform, ref FPVector2 point)
{
FPVector2 center = transform.p + MathUtils.Mul(transform.q, Position);
FPVector2 d = point - center;
return(FPVector2.Dot(d, d) <= _2radius);
}
public override FP ComputeSubmergedArea(ref FPVector2 normal, FP offset, ref Transform xf, out FPVector2 sc)
{
sc = FPVector2.zero;
FPVector2 p = MathUtils.Mul(ref xf, Position);
FP l = -(FPVector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return(0);
}
if (l > Radius)
{
//Completely wet
sc = p;
return(Settings.Pi * _2radius);
}
//Magic
FP l2 = l * l;
FP area = _2radius * (FP)((FPMath.Asin((l / Radius)) + FPMath.PiOver2) + l * FPMath.Sqrt(_2radius - l2));
// TODO - PORT
//FP com = -2.0f / 3.0f * (FP)Math.Pow(_2radius - l2, 1.5f) / area;
FP com = new FP(-2) / new FP(3) * (FP)Math.Pow((_2radius - l2).AsFloat(), 1.5f) / area;
sc.x = p.x + normal.x * com;
sc.y = p.y + normal.y * com;
return(area);
}
public static FP DistanceBetweenPointAndLineSegment(ref FPVector2 point, ref FPVector2 start, ref FPVector2 end)
{
if (start == end)
{
return(FPVector2.Distance(point, start));
}
FPVector2 v = FPVector2.Subtract(end, start);
FPVector2 w = FPVector2.Subtract(point, start);
FP c1 = FPVector2.Dot(w, v);
if (c1 <= 0)
{
return(FPVector2.Distance(point, start));
}
FP c2 = FPVector2.Dot(v, v);
if (c2 <= c1)
{
return(FPVector2.Distance(point, end));
}
FP b = c1 / c2;
FPVector2 pointOnLine = FPVector2.Add(start, FPVector2.Multiply(v, b));
return(FPVector2.Distance(point, pointOnLine));
}
/// <summary>
/// Tests if a point lies on a line segment.
/// </summary>
/// <remarks>Used by method <c>CalculateBeta()</c>.</remarks>
private static bool PointOnLineSegment(FPVector2 start, FPVector2 end, FPVector2 point)
{
FPVector2 segment = end - start;
return(MathUtils.Area(ref start, ref end, ref point) == 0f &&
FPVector2.Dot(point - start, segment) >= 0f &&
FPVector2.Dot(point - end, segment) <= 0f);
}
protected override sealed void ComputeProperties()
{
FP area = Settings.Pi * _2radius;
MassData.Area = area;
MassData.Mass = Density * area;
MassData.Centroid = Position;
// inertia about the local origin
MassData.Inertia = MassData.Mass * (0.5f * _2radius + FPVector2.Dot(Position, Position));
}
public static FP Evaluate(int indexA, int indexB, FP t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
FPVector2 localPointA = _proxyA.Vertices[indexA];
FPVector2 localPointB = _proxyB.Vertices[indexB];
FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
FPVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
FP separation = FPVector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
FPVector2 normal = MathUtils.Mul(ref xfA.q, _axis);
FPVector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
FPVector2 localPointB = _proxyB.Vertices[indexB];
FPVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
FP separation = FPVector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
FPVector2 normal = MathUtils.Mul(ref xfB.q, _axis);
FPVector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
FPVector2 localPointA = _proxyA.Vertices[indexA];
FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
FP separation = FPVector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
return(0.0f);
}
}
public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out FPVector2 normal, out FPVector2 point, out FP separation)
{
Debug.Assert(pc.pointCount > 0);
switch (pc.type)
{
case ManifoldType.Circles:
{
FPVector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
FPVector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
normal = pointB - pointA;
normal.Normalize();
point = 0.5f * (pointA + pointB);
separation = FPVector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, pc.localNormal);
FPVector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);
FPVector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
separation = FPVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, pc.localNormal);
FPVector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);
FPVector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
separation = FPVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
default:
normal = FPVector2.zero;
point = FPVector2.zero;
separation = 0;
break;
}
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 d = (cB - cA) + rB - rA;
FPVector2 ay = MathUtils.Mul(qA, _localYAxis);
FP sAy = MathUtils.Cross(d + rA, ay);
FP sBy = MathUtils.Cross(rB, ay);
FP C = FPVector2.Dot(d, ay);
FP k = _invMassA + _invMassB + _invIA * _sAy * _sAy + _invIB * _sBy * _sBy;
FP impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
FPVector2 P = impulse * ay;
FP LA = impulse * sAy;
FP 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(FP.Abs(C) <= Settings.LinearSlop);
}
public override bool TestPoint(ref Transform transform, ref FPVector2 point)
{
FPVector2 pLocal = MathUtils.MulT(transform.q, point - transform.p);
for (int i = 0; i < Vertices.Count; ++i)
{
FP dot = FPVector2.Dot(Normals[i], pLocal - Vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
/// <summary>
/// Get the supporting vertex in the given direction.
/// </summary>
/// <param name="direction">The direction.</param>
/// <returns></returns>
public FPVector2 GetSupportVertex(FPVector2 direction)
{
int bestIndex = 0;
FP bestValue = FPVector2.Dot(Vertices[0], direction);
for (int i = 1; i < Vertices.Count; ++i)
{
FP value = FPVector2.Dot(Vertices[i], direction);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return(Vertices[bestIndex]);
}
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
internal void Solve2()
{
FPVector2 w1 = V[0].W;
FPVector2 w2 = V[1].W;
FPVector2 e12 = w2 - w1;
// w1 region
FP d12_2 = -FPVector2.Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
SimplexVertex v0 = V[0];
v0.A = 1.0f;
V[0] = v0;
Count = 1;
return;
}
// w2 region
FP d12_1 = FPVector2.Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
SimplexVertex v1 = V[1];
v1.A = 1.0f;
V[1] = v1;
Count = 1;
V[0] = V[1];
return;
}
// Must be in e12 region.
FP inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
}
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();
FPVector2 position = transform.p + MathUtils.Mul(transform.q, Position);
FPVector2 s = input.Point1 - position;
FP b = FPVector2.Dot(s, s) - _2radius;
// Solve quadratic equation.
FPVector2 r = input.Point2 - input.Point1;
FP c = FPVector2.Dot(s, r);
FP rr = FPVector2.Dot(r, r);
FP 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.
FP a = -(c + FP.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 = s + a * r;
output.Normal.Normalize();
return(true);
}
return(false);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FPVector2 vC = data.velocities[_indexC].v;
FP wC = data.velocities[_indexC].w;
FPVector2 vD = data.velocities[_indexD].v;
FP wD = data.velocities[_indexD].w;
FP Cdot = FPVector2.Dot(_JvAC, vA - vC) + FPVector2.Dot(_JvBD, vB - vD);
Cdot += (_JwA * wA - _JwC * wC) + (_JwB * wB - _JwD * wD);
FP impulse = -_mass * Cdot;
_impulse += impulse;
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;
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
data.velocities[_indexC].v = vC;
data.velocities[_indexC].w = wC;
data.velocities[_indexD].v = vD;
data.velocities[_indexD].w = wD;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
FPVector2 vpA = vA + MathUtils.Cross(wA, _rA);
FPVector2 vpB = vB + MathUtils.Cross(wB, _rB);
FP C = _length - MaxLength;
FP Cdot = FPVector2.Dot(_u, vpB - vpA);
// Predictive constraint.
if (C < 0.0f)
{
Cdot += data.step.inv_dt * C;
}
FP impulse = -_mass * Cdot;
FP oldImpulse = _impulse;
_impulse = Spax.FPMath.Min(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
FPVector2 P = impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
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>
// TS - public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, FP ratio = 1f)
public GearJoint(Body bodyA, Body bodyB, Joint2D jointA, Joint2D jointB, FP ratio)
{
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);
FP 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;
FP aA = _bodyA._sweep.A;
Transform xfC = _bodyC._xf;
FP aC = _bodyC._sweep.A;
if (_typeA == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointA;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC = FPVector2.zero;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointA;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxis;
FPVector2 pC = _localAnchorC;
FPVector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
}
_bodyD = JointB.BodyA;
_bodyB = JointB.BodyB;
// Get geometry of joint2
Transform xfB = _bodyB._xf;
FP aB = _bodyB._sweep.A;
Transform xfD = _bodyD._xf;
FP aD = _bodyD._sweep.A;
if (_typeB == JointType.Revolute)
{
RevoluteJoint revolute = (RevoluteJoint)jointB;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD = FPVector2.zero;
coordinateB = aB - aD - _referenceAngleB;
}
else
{
PrismaticJoint prismatic = (PrismaticJoint)jointB;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxis;
FPVector2 pD = _localAnchorD;
FPVector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
}
_ratio = ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
//Cutting a shape into two is based on the work of Daid and his prototype BoxCutter: http://www.box2d.org/forum/viewtopic.php?f=3&t=1473
/// <summary>
/// Split a fixture into 2 vertice collections using the given entry and exit-point.
/// </summary>
/// <param name="fixture">The Fixture to split</param>
/// <param name="entryPoint">The entry point - The start point</param>
/// <param name="exitPoint">The exit point - The end point</param>
/// <param name="first">The first collection of vertexes</param>
/// <param name="second">The second collection of vertexes</param>
public static void SplitShape(Fixture fixture, FPVector2 entryPoint, FPVector2 exitPoint, out Vertices first, out Vertices second)
{
FPVector2 localEntryPoint = fixture.Body.GetLocalPoint(ref entryPoint);
FPVector2 localExitPoint = fixture.Body.GetLocalPoint(ref exitPoint);
PolygonShape shape = fixture.Shape as PolygonShape;
//We can only cut polygons at the moment
if (shape == null)
{
first = new Vertices();
second = new Vertices();
return;
}
//Offset the entry and exit points if they are too close to the vertices
foreach (FPVector2 vertex in shape.Vertices)
{
if (vertex.Equals(localEntryPoint))
{
localEntryPoint -= new FPVector2(0, Settings.Epsilon);
}
if (vertex.Equals(localExitPoint))
{
localExitPoint += new FPVector2(0, Settings.Epsilon);
}
}
Vertices vertices = new Vertices(shape.Vertices);
Vertices[] newPolygon = new Vertices[2];
for (int i = 0; i < newPolygon.Length; i++)
{
newPolygon[i] = new Vertices(vertices.Count);
}
int[] cutAdded = { -1, -1 };
int last = -1;
for (int i = 0; i < vertices.Count; i++)
{
int n;
//Find out if this vertex is on the old or new shape.
if (FPVector2.Dot(MathUtils.Cross(localExitPoint - localEntryPoint, 1), vertices[i] - localEntryPoint) > Settings.Epsilon)
{
n = 0;
}
else
{
n = 1;
}
if (last != n)
{
//If we switch from one shape to the other add the cut vertices.
if (last == 0)
{
Debug.Assert(cutAdded[0] == -1);
cutAdded[0] = newPolygon[last].Count;
newPolygon[last].Add(localExitPoint);
newPolygon[last].Add(localEntryPoint);
}
if (last == 1)
{
Debug.Assert(cutAdded[last] == -1);
cutAdded[last] = newPolygon[last].Count;
newPolygon[last].Add(localEntryPoint);
newPolygon[last].Add(localExitPoint);
}
}
newPolygon[n].Add(vertices[i]);
last = n;
}
//Add the cut in case it has not been added yet.
if (cutAdded[0] == -1)
{
cutAdded[0] = newPolygon[0].Count;
newPolygon[0].Add(localExitPoint);
newPolygon[0].Add(localEntryPoint);
}
if (cutAdded[1] == -1)
{
cutAdded[1] = newPolygon[1].Count;
newPolygon[1].Add(localEntryPoint);
newPolygon[1].Add(localExitPoint);
}
for (int n = 0; n < 2; n++)
{
FPVector2 offset;
if (cutAdded[n] > 0)
{
offset = (newPolygon[n][cutAdded[n] - 1] - newPolygon[n][cutAdded[n]]);
}
else
{
offset = (newPolygon[n][newPolygon[n].Count - 1] - newPolygon[n][0]);
}
offset.Normalize();
if (!offset.IsValid())
{
offset = FPVector2.one;
}
newPolygon[n][cutAdded[n]] += Settings.Epsilon * offset;
if (cutAdded[n] < newPolygon[n].Count - 2)
{
offset = (newPolygon[n][cutAdded[n] + 2] - newPolygon[n][cutAdded[n] + 1]);
}
else
{
offset = (newPolygon[n][0] - newPolygon[n][newPolygon[n].Count - 1]);
}
offset.Normalize();
if (!offset.IsValid())
{
offset = FPVector2.one;
}
newPolygon[n][cutAdded[n] + 1] += Settings.Epsilon * offset;
}
first = newPolygon[0];
second = newPolygon[1];
}
public void Solve(ref TimeStep step, ref FPVector2 gravity)
{
FP h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
FPVector2 c = b._sweep.C;
FP a = b._sweep.A;
FPVector2 v = b._linearVelocity;
FP w = b._angularVelocity;
// Store positions for continuous collision.
b._sweep.C0 = b._sweep.C;
b._sweep.A0 = b._sweep.A;
if (b.BodyType == BodyType.Dynamic)
{
// Integrate velocities.
// FPE: Only apply gravity if the body wants it.
if (b.IgnoreGravity)
{
v += h * (b._invMass * b._force);
}
else
{
v += h * (b.GravityScale * gravity + b._invMass * b._force);
}
w += h * b._invI * b._torque;
// Apply damping.
// ODE: dv/dt + c * v = 0
// Solution: v(t) = v0 * exp(-c * t)
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
// v2 = exp(-c * dt) * v1
// Taylor expansion:
// v2 = (1.0f - c * dt) * v1
//v *= MathUtils.Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f);
//w *= MathUtils.Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f);
v *= 1 / (1 + h * b.LinearDamping);
w *= 1 / (1 + h * b.AngularDamping);
}
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
// Solver data
SolverData solverData = new SolverData();
solverData.step = step;
solverData.positions = _positions;
solverData.velocities = _velocities;
_contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
_contactSolver.InitializeVelocityConstraints();
if (Settings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
// Solve velocity constraints.
for (int i = 0; i < Settings.VelocityIterations; ++i)
{
for (int j = 0; j < JointCount; ++j)
{
Joint2D joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref solverData);
joint.Validate(step.inv_dt);
}
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
FPVector2 c = _positions[i].c;
FP a = _positions[i].a;
FPVector2 v = _velocities[i].v;
FP w = _velocities[i].w;
// Check for large velocities
FPVector2 translation = h * v;
if (FPVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
FP ratio = Settings.MaxTranslation / translation.magnitude;
v *= ratio;
}
FP rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
FP ratio = Settings.MaxRotation / FP.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
}
// Solve position constraints
bool positionSolved = false;
for (int i = 0; i < Settings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
for (int j = 0; j < JointCount; ++j)
{
Joint2D joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
jointsOkay = jointsOkay && jointOkay;
}
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
Body body = Bodies[i];
body._sweep.C = _positions[i].c;
body._sweep.A = _positions[i].a;
body._linearVelocity = _velocities[i].v;
body._angularVelocity = _velocities[i].w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
if (Settings.AllowSleep)
{
FP minSleepTime = Settings.MaxFP;
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if (!b.SleepingAllowed || b._angularVelocity * b._angularVelocity > AngTolSqr || FPVector2.Dot(b._linearVelocity, b._linearVelocity) > LinTolSqr)
{
b._sleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b._sleepTime += h;
minSleepTime = Spax.FPMath.Min(minSleepTime, b._sleepTime);
}
}
if (minSleepTime >= Settings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
b.Awake = false;
}
}
}
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
FP Cdot = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
FP impulse = _motorMass * (_motorSpeed - Cdot);
FP oldImpulse = MotorImpulse;
FP maxImpulse = data.step.dt * _maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
FPVector2 P = impulse * _axis;
FP LA = impulse * _a1;
FP LB = impulse * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
FPVector2 Cdot1 = new FPVector2();
Cdot1.x = FPVector2.Dot(_perp, vB - vA) + _s2 * wB - _s1 * wA;
Cdot1.y = wB - wA;
if (_enableLimit && _limitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
FP Cdot2;
Cdot2 = FPVector2.Dot(_axis, vB - vA) + _a2 * wB - _a1 * wA;
FPVector Cdot = new FPVector(Cdot1.x, Cdot1.y, Cdot2);
FPVector f1 = _impulse;
FPVector df = _K.Solve33(Cdot * -1);
_impulse += df;
if (_limitState == LimitState.AtLower)
{
_impulse.z = KBEngine.FPMath.Max(_impulse.z, 0.0f);
}
else if (_limitState == LimitState.AtUpper)
{
_impulse.z = KBEngine.FPMath.Min(_impulse.z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
FPVector2 b = -Cdot1 - (_impulse.z - f1.z) * new FPVector2(_K.ez.x, _K.ez.y);
FPVector2 f2r = _K.Solve22(b) + new FPVector2(f1.x, f1.y);
_impulse.x = f2r.x;
_impulse.y = f2r.y;
df = _impulse - f1;
FPVector2 P = df.x * _perp + df.z * _axis;
FP LA = df.x * _s1 + df.y + df.z * _a1;
FP LB = df.x * _s2 + df.y + df.z * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
FPVector2 df = _K.Solve22(-Cdot1);
_impulse.x += df.x;
_impulse.y += df.y;
FPVector2 P = df.x * _perp;
FP LA = df.x * _s1 + df.y;
FP LB = df.x * _s2 + df.y;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(Func<RayCastInput, int, FP> callback, ref RayCastInput input)
{
FPVector2 p1 = input.Point1;
FPVector2 p2 = input.Point2;
FPVector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
FPVector2 absV = MathUtils.Abs(new FPVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FP maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
FPVector2 t = p1 + maxFraction * (p2 - p1);
FPVector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
FPVector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_raycastStack.Clear();
_raycastStack.Push(_root);
while (_raycastStack.Count > 0)
{
int nodeId = _raycastStack.Pop();
if (nodeId == NullNode)
{
continue;
}
TreeNode<T> node = _nodes[nodeId];
if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FPVector2 c = node.AABB.Center;
FPVector2 h = node.AABB.Extents;
FP separation = FP.Abs(FPVector2.Dot(new FPVector2(-r.y, r.x), p1 - c)) - FPVector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
FP value = callback(subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
FPVector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = FPVector2.Min(p1, t);
segmentAABB.UpperBound = FPVector2.Max(p1, t);
}
}
else
{
_raycastStack.Push(node.Child1);
_raycastStack.Push(node.Child2);
}
}
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
internal void Solve3()
{
FPVector2 w1 = V[0].W;
FPVector2 w2 = V[1].W;
FPVector2 w3 = V[2].W;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
FPVector2 e12 = w2 - w1;
FP w1e12 = FPVector2.Dot(w1, e12);
FP w2e12 = FPVector2.Dot(w2, e12);
FP d12_1 = w2e12;
FP d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
FPVector2 e13 = w3 - w1;
FP w1e13 = FPVector2.Dot(w1, e13);
FP w3e13 = FPVector2.Dot(w3, e13);
FP d13_1 = w3e13;
FP d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
FPVector2 e23 = w3 - w2;
FP w2e23 = FPVector2.Dot(w2, e23);
FP w3e23 = FPVector2.Dot(w3, e23);
FP d23_1 = w3e23;
FP d23_2 = -w2e23;
// Triangle123
FP n123 = MathUtils.Cross(e12, e13);
FP d123_1 = n123 * MathUtils.Cross(w2, w3);
FP d123_2 = n123 * MathUtils.Cross(w3, w1);
FP d123_3 = n123 * MathUtils.Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
SimplexVertex v0_1 = V[0];
v0_1.A = 1.0f;
V[0] = v0_1;
Count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
FP inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
FP inv_d13 = 1.0f / (d13_1 + d13_2);
SimplexVertex v0_3 = V[0];
SimplexVertex v2_3 = V[2];
v0_3.A = d13_1 * inv_d13;
v2_3.A = d13_2 * inv_d13;
V[0] = v0_3;
V[2] = v2_3;
Count = 2;
V[1] = V[2];
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
SimplexVertex v1_4 = V[1];
v1_4.A = 1.0f;
V[1] = v1_4;
Count = 1;
V[0] = V[1];
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
SimplexVertex v2_5 = V[2];
v2_5.A = 1.0f;
V[2] = v2_5;
Count = 1;
V[0] = V[2];
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
FP inv_d23 = 1.0f / (d23_1 + d23_2);
SimplexVertex v1_6 = V[1];
SimplexVertex v2_6 = V[2];
v1_6.A = d23_1 * inv_d23;
v2_6.A = d23_2 * inv_d23;
V[1] = v1_6;
V[2] = v2_6;
Count = 2;
V[0] = V[2];
return;
}
// Must be in triangle123
FP inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
SimplexVertex v0_7 = V[0];
SimplexVertex v1_7 = V[1];
SimplexVertex v2_7 = V[2];
v0_7.A = d123_1 * inv_d123;
v1_7.A = d123_2 * inv_d123;
v2_7.A = d123_3 * inv_d123;
V[0] = v0_7;
V[1] = v1_7;
V[2] = v2_7;
Count = 3;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
// Solve spring constraint
{
FP Cdot = FPVector2.Dot(_ax, vB - vA) + _sBx * wB - _sAx * wA;
FP impulse = -_springMass * (Cdot + _bias + _gamma * _springImpulse);
_springImpulse += impulse;
FPVector2 P = impulse * _ax;
FP LA = impulse * _sAx;
FP LB = impulse * _sBx;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
// Solve rotational motor constraint
{
FP Cdot = wB - wA - _motorSpeed;
FP impulse = -_motorMass * Cdot;
FP oldImpulse = _motorImpulse;
FP maxImpulse = data.step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line constraint
{
FP Cdot = FPVector2.Dot(_ay, vB - vA) + _sBy * wB - _sAy * wA;
FP impulse = -_mass * Cdot;
_impulse += impulse;
FPVector2 P = impulse * _ay;
FP LA = impulse * _sAy;
FP LB = impulse * _sBy;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
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;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 d1 = cB + rB - cA - rA;
// Point to line constraint
{
_ay = MathUtils.Mul(qA, _localYAxis);
_sAy = MathUtils.Cross(d1 + rA, _ay);
_sBy = MathUtils.Cross(rB, _ay);
_mass = mA + mB + iA * _sAy * _sAy + iB * _sBy * _sBy;
if (_mass > 0.0f)
{
_mass = 1.0f / _mass;
}
}
// Spring constraint
_springMass = 0.0f;
_bias = 0.0f;
_gamma = 0.0f;
if (Frequency > 0.0f)
{
_ax = MathUtils.Mul(qA, LocalXAxis);
_sAx = MathUtils.Cross(d1 + rA, _ax);
_sBx = MathUtils.Cross(rB, _ax);
FP invMass = mA + mB + iA * _sAx * _sAx + iB * _sBx * _sBx;
if (invMass > 0.0f)
{
_springMass = 1.0f / invMass;
FP C = FPVector2.Dot(d1, _ax);
// Frequency
FP omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
FP d = 2.0f * _springMass * DampingRatio * omega;
// Spring stiffness
FP k = _springMass * omega * omega;
// magic formulas
FP h = data.step.dt;
_gamma = h * (d + h * k);
if (_gamma > 0.0f)
{
_gamma = 1.0f / _gamma;
}
_bias = C * h * k * _gamma;
_springMass = invMass + _gamma;
if (_springMass > 0.0f)
{
_springMass = 1.0f / _springMass;
}
}
}
else
{
_springImpulse = 0.0f;
}
// Rotational motor
if (_enableMotor)
{
_motorMass = iA + iB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
else
{
_motorMass = 0.0f;
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
_springImpulse *= data.step.dtRatio;
_motorImpulse *= data.step.dtRatio;
FPVector2 P = _impulse * _ay + _springImpulse * _ax;
FP LA = _impulse * _sAy + _springImpulse * _sAx + _motorImpulse;
FP 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;
}
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.
FPVector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
FPVector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
FPVector2 d = p2 - p1;
FPVector2 v1 = _vertex1;
FPVector2 v2 = _vertex2;
FPVector2 e = v2 - v1;
FPVector2 normal = new FPVector2(e.y, -e.x); //TODO: Could possibly cache the normal.
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
FP numerator = FPVector2.Dot(normal, v1 - p1);
FP denominator = FPVector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
FP t = numerator / denominator;
if (t < 0.0f || input.MaxFraction < t)
{
return(false);
}
FPVector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
FPVector2 r = v2 - v1;
FP rr = FPVector2.Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
FP s = FPVector2.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);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 cC = data.positions[_indexC].c;
FP aC = data.positions[_indexC].a;
FPVector2 cD = data.positions[_indexD].c;
FP aD = data.positions[_indexD].a;
Rot qA = new Rot(aA), qB = new Rot(aB), qC = new Rot(aC), qD = new Rot(aD);
FP linearError = 0.0f;
FP coordinateA, coordinateB;
FPVector2 JvAC, JvBD;
FP JwA, JwB, JwC, JwD;
FP mass = 0.0f;
if (_typeA == JointType.Revolute)
{
JvAC = FPVector2.zero;
JwA = 1.0f;
JwC = 1.0f;
mass += _iA + _iC;
coordinateA = aA - aC - _referenceAngleA;
}
else
{
FPVector2 u = MathUtils.Mul(qC, _localAxisC);
FPVector2 rC = MathUtils.Mul(qC, _localAnchorC - _lcC);
FPVector2 rA = MathUtils.Mul(qA, _localAnchorA - _lcA);
JvAC = u;
JwC = MathUtils.Cross(rC, u);
JwA = MathUtils.Cross(rA, u);
mass += _mC + _mA + _iC * JwC * JwC + _iA * JwA * JwA;
FPVector2 pC = _localAnchorC - _lcC;
FPVector2 pA = MathUtils.MulT(qC, rA + (cA - cC));
coordinateA = FPVector2.Dot(pA - pC, _localAxisC);
}
if (_typeB == JointType.Revolute)
{
JvBD = FPVector2.zero;
JwB = _ratio;
JwD = _ratio;
mass += _ratio * _ratio * (_iB + _iD);
coordinateB = aB - aD - _referenceAngleB;
}
else
{
FPVector2 u = MathUtils.Mul(qD, _localAxisD);
FPVector2 rD = MathUtils.Mul(qD, _localAnchorD - _lcD);
FPVector2 rB = MathUtils.Mul(qB, _localAnchorB - _lcB);
JvBD = _ratio * u;
JwD = _ratio * MathUtils.Cross(rD, u);
JwB = _ratio * MathUtils.Cross(rB, u);
mass += _ratio * _ratio * (_mD + _mB) + _iD * JwD * JwD + _iB * JwB * JwB;
FPVector2 pD = _localAnchorD - _lcD;
FPVector2 pB = MathUtils.MulT(qD, rB + (cB - cD));
coordinateB = FPVector2.Dot(pB - pD, _localAxisD);
}
FP C = (coordinateA + _ratio * coordinateB) - _constant;
FP 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;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 d = (cB - cA) + rB - rA;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Mul(qA, LocalXAxis);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
FP k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
FP k12 = iA * _s1 + iB * _s2;
FP k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
FP k23 = iA * _a1 + iB * _a2;
FP k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_K.ex = new FPVector(k11, k12, k13);
_K.ey = new FPVector(k12, k22, k23);
_K.ez = new FPVector(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
FP jointTranslation = FPVector2.Dot(_axis, d);
if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
if (_enableMotor == false)
{
MotorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
MotorImpulse *= data.step.dtRatio;
FPVector2 P = _impulse.x * _perp + (MotorImpulse + _impulse.z) * _axis;
FP LA = _impulse.x * _s1 + _impulse.y + (MotorImpulse + _impulse.z) * _a1;
FP LB = _impulse.x * _s2 + _impulse.y + (MotorImpulse + _impulse.z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse = FPVector.zero;
MotorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1)
{
_localPoint = FPVector2.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;
FPVector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
FPVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
FPVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
FPVector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
FPVector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
FPVector2 a = localPointB2 - localPointB1;
_axis = new FPVector2(a.y, -a.x);
_axis.Normalize();
FPVector2 normal = MathUtils.Mul(ref xfB.q, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
FPVector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
FPVector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
FPVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
FP s = FPVector2.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;
FPVector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
FPVector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
FPVector2 a = localPointA2 - localPointA1;
_axis = new FPVector2(a.y, -a.x);
_axis.Normalize();
FPVector2 normal = MathUtils.Mul(ref xfA.q, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
FPVector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
FPVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
FPVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
FP s = FPVector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
//FPE note: the returned value that used to be here has been removed, as it was not used.
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 d = cB + rB - cA - rA;
FPVector2 axis = MathUtils.Mul(qA, LocalXAxis);
FP a1 = MathUtils.Cross(d + rA, axis);
FP a2 = MathUtils.Cross(rB, axis);
FPVector2 perp = MathUtils.Mul(qA, _localYAxisA);
FP s1 = MathUtils.Cross(d + rA, perp);
FP s2 = MathUtils.Cross(rB, perp);
FPVector impulse;
FPVector2 C1 = new FPVector2();
C1.x = FPVector2.Dot(perp, d);
C1.y = aB - aA - ReferenceAngle;
FP linearError = FP.Abs(C1.x);
FP angularError = FP.Abs(C1.y);
bool active = false;
FP C2 = 0.0f;
if (_enableLimit)
{
FP translation = FPVector2.Dot(axis, d);
if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = KBEngine.FPMath.Max(linearError, FP.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = KBEngine.FPMath.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = KBEngine.FPMath.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
FP k12 = iA * s1 + iB * s2;
FP k13 = iA * s1 * a1 + iB * s2 * a2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
// For fixed rotation
k22 = 1.0f;
}
FP k23 = iA * a1 + iB * a2;
FP k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new FPVector(k11, k12, k13);
K.ey = new FPVector(k12, k22, k23);
K.ez = new FPVector(k13, k23, k33);
FPVector C = new FPVector();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
impulse = K.Solve33(C * -1);
}
else
{
FP k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
FP k12 = iA * s1 + iB * s2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new FPVector2(k11, k12);
K.ey = new FPVector2(k12, k22);
FPVector2 impulse1 = K.Solve(-C1);
impulse = new FPVector();
impulse.x = impulse1.x;
impulse.y = impulse1.y;
impulse.z = 0.0f;
}
FPVector2 P = impulse.x * perp + impulse.z * axis;
FP LA = impulse.x * s1 + impulse.y + impulse.z * a1;
FP LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal void SolveTOI(ref TimeStep subStep, int toiIndexA, int toiIndexB, bool warmstarting)
{
Debug.Assert(toiIndexA < BodyCount);
Debug.Assert(toiIndexB < BodyCount);
// Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
Body b = Bodies[i];
_positions[i].c = b._sweep.C;
_positions[i].a = b._sweep.A;
_velocities[i].v = b._linearVelocity;
_velocities[i].w = b._angularVelocity;
}
_contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities, warmstarting);
// Solve position constraints.
for (int i = 0; i < Settings.TOIPositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
if (contactsOkay)
{
break;
}
}
// Leap of faith to new safe state.
Bodies[toiIndexA]._sweep.C0 = _positions[toiIndexA].c;
Bodies[toiIndexA]._sweep.A0 = _positions[toiIndexA].a;
Bodies[toiIndexB]._sweep.C0 = _positions[toiIndexB].c;
Bodies[toiIndexB]._sweep.A0 = _positions[toiIndexB].a;
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
_contactSolver.InitializeVelocityConstraints();
// Solve velocity constraints.
for (int i = 0; i < Settings.TOIVelocityIterations; ++i)
{
_contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
FP h = subStep.dt;
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
FPVector2 c = _positions[i].c;
FP a = _positions[i].a;
FPVector2 v = _velocities[i].v;
FP w = _velocities[i].w;
// Check for large velocities
FPVector2 translation = h * v;
if (FPVector2.Dot(translation, translation) > Settings.MaxTranslationSquared)
{
FP ratio = Settings.MaxTranslation / translation.magnitude;
v *= ratio;
}
FP rotation = h * w;
if (rotation * rotation > Settings.MaxRotationSquared)
{
FP ratio = Settings.MaxRotation / FP.Abs(rotation);
w *= ratio;
}
// Integrate
c += h * v;
a += h * w;
_positions[i].c = c;
_positions[i].a = a;
_velocities[i].v = v;
_velocities[i].w = w;
// Sync bodies
Body body = Bodies[i];
body._sweep.C = c;
body._sweep.A = a;
body._linearVelocity = v;
body._angularVelocity = w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
}
