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);
}
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);
}
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 static float FindMinSeparation(out int indexA, out int indexB, float t)
{
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t);
_sweepB.GetTransform(out xfB, t);
switch (_type)
{
case SeparationFunctionType.Points:
{
Vector2 axisA = Complex.Divide(ref _axis, ref xfA.q);
Vector2 axisB = -Complex.Divide(ref _axis, ref xfB.q);
indexA = _proxyA.GetSupport(axisA);
indexB = _proxyB.GetSupport(axisB);
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 axisB = -Complex.Divide(ref normal, ref xfB.q);
indexA = -1;
indexB = _proxyB.GetSupport(axisB);
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 axisA = -Complex.Divide(ref normal, ref xfA.q);
indexB = -1;
indexA = _proxyA.GetSupport(axisA);
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);
indexA = -1;
indexB = -1;
return(0.0f);
}
}
/// <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 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 Divide(ref Transform left, Complex right, out Transform result)
{
result.p = Complex.Divide(ref left.p, ref right);
result.q = Complex.Divide(ref left.q, ref right);
}
public static void Divide(ref Transform left, ref Transform right, out Transform result)
{
Complex.Divide(left.p - right.p, ref right.q, out result.p);
Complex.Divide(ref left.q, ref right.q, out result.q);
}
public static Transform Divide(ref Transform left, ref Transform right)
{
return(new Transform(
Complex.Divide(left.p - right.p, ref right.q),
Complex.Divide(ref left.q, ref right.q)));
}
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;
}
}
}