internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
FVector2 vpA = vA + MathUtils.Cross(wA, m_rA);
FVector2 vpB = vB + MathUtils.Cross(wB, m_rB);
float Cdot = -FVector2.Dot(m_uA, vpA) - Ratio * FVector2.Dot(m_uB, vpB);
float impulse = -m_mass * Cdot;
_impulse += impulse;
FVector2 PA = -impulse * m_uA;
FVector2 PB = -Ratio * impulse * m_uB;
vA += m_invMassA * PA;
wA += m_invIA * MathUtils.Cross(m_rA, PA);
vB += m_invMassB * PB;
wB += m_invIB * MathUtils.Cross(m_rB, PB);
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
/// <summary>
/// Method used by the BuoyancyController
/// </summary>
public override float ComputeSubmergedArea(FVector2 normal, float offset, Transform xf, out FVector2 sc)
{
sc = FVector2.Zero;
FVector2 p = MathUtils.Mul(ref xf, Position);
float l = -(FVector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return(0);
}
if (l > Radius)
{
//Completely wet
sc = p;
return(Settings.Pi * Radius * Radius);
}
//Magic
float r2 = Radius * Radius;
float l2 = l * l;
float area = r2 * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(r2 - l2));
float com = -2.0f / 3.0f * (float)Math.Pow(r2 - l2, 1.5f) / area;
sc.X = p.X + normal.X * com;
sc.Y = p.Y + normal.Y * com;
return(area);
}
public static float DistanceBetweenPointAndLineSegment(ref FVector2 point, ref FVector2 lineEndPoint1,
ref FVector2 lineEndPoint2)
{
FVector2 v = FVector2.Subtract(lineEndPoint2, lineEndPoint1);
FVector2 w = FVector2.Subtract(point, lineEndPoint1);
float c1 = FVector2.Dot(w, v);
if (c1 <= 0)
{
return(DistanceBetweenPointAndPoint(ref point, ref lineEndPoint1));
}
float c2 = FVector2.Dot(v, v);
if (c2 <= c1)
{
return(DistanceBetweenPointAndPoint(ref point, ref lineEndPoint2));
}
float b = c1 / c2;
FVector2 pointOnLine = FVector2.Add(lineEndPoint1, FVector2.Multiply(v, b));
return(DistanceBetweenPointAndPoint(ref point, ref pointOnLine));
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
// Cdot = dot(u, v + cross(w, r))
FVector2 vpA = vA + MathUtils.Cross(wA, m_rA);
FVector2 vpB = vB + MathUtils.Cross(wB, m_rB);
float Cdot = FVector2.Dot(_u, vpB - vpA);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
FVector2 P = impulse * _u;
vA -= m_invMassA * P;
wA -= m_invIA * MathUtils.Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * MathUtils.Cross(m_rB, P);
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref FVector2 point)
{
FVector2 center = transform.p + MathUtils.Mul(transform.q, Position);
FVector2 d = point - center;
return(FVector2.Dot(d, d) <= Radius * Radius);
}
/// <summary>
/// Tests if a point lies on a line segment.
/// </summary>
/// <remarks>Used by method <c>CalculateBeta()</c>.</remarks>
private static bool PointOnLineSegment(FVector2 start, FVector2 end, FVector2 point)
{
FVector2 segment = end - start;
return(MathUtils.Area(ref start, ref end, ref point) == 0f &&
FVector2.Dot(point - start, segment) >= 0f &&
FVector2.Dot(point - end, segment) <= 0f);
}
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:
{
//FVector2 axisA = MathUtils.MulT(ref xfA.q, _axis);
//FVector2 axisB = MathUtils.MulT(ref xfB.q, -_axis);
FVector2 localPointA = _proxyA.Vertices[indexA];
FVector2 localPointB = _proxyB.Vertices[indexB];
FVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
FVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float separation = FVector2.Dot(pointB - pointA, _axis);
return(separation);
}
case SeparationFunctionType.FaceA:
{
FVector2 normal = MathUtils.Mul(ref xfA.q, _axis);
FVector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
//FVector2 axisB = MathUtils.MulT(ref xfB.q, -normal);
FVector2 localPointB = _proxyB.Vertices[indexB];
FVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float separation = FVector2.Dot(pointB - pointA, normal);
return(separation);
}
case SeparationFunctionType.FaceB:
{
FVector2 normal = MathUtils.Mul(ref xfB.q, _axis);
FVector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
//FVector2 axisA = MathUtils.MulT(ref xfA.q, -normal);
FVector2 localPointA = _proxyA.Vertices[indexA];
FVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
float separation = FVector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Debug.Assert(false);
return(0.0f);
}
}
public PolyNode GetRightestConnection(PolyNode incoming)
{
if (NConnected == 0)
{
Debug.Assert(false); // This means the connection graph is inconsistent
}
if (NConnected == 1)
{
//b2Assert(false);
// Because of the possibility of collapsing nearby points,
// we may end up with "spider legs" dangling off of a region.
// The correct behavior here is to turn around.
return(incoming);
}
FVector2 inDir = Position - incoming.Position;
float inLength = inDir.Length();
inDir.Normalize();
Debug.Assert(inLength > Settings.Epsilon);
PolyNode result = null;
for (int i = 0; i < NConnected; ++i)
{
if (Connected[i] == incoming)
{
continue;
}
FVector2 testDir = Connected[i].Position - Position;
float testLengthSqr = testDir.LengthSquared();
testDir.Normalize();
Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon);
float myCos = FVector2.Dot(inDir, testDir);
float mySin = MathUtils.Cross(inDir, testDir);
if (result != null)
{
FVector2 resultDir = result.Position - Position;
resultDir.Normalize();
float resCos = FVector2.Dot(inDir, resultDir);
float resSin = MathUtils.Cross(inDir, resultDir);
if (IsRighter(mySin, myCos, resSin, resCos))
{
result = Connected[i];
}
}
else
{
result = Connected[i];
}
}
Debug.Assert(result != null);
return(result);
}
/// <summary>
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin, not the centroid.
/// </summary>
protected override sealed void ComputeProperties()
{
float area = Settings.Pi * Radius * Radius;
MassData.Area = area;
MassData.Mass = Density * area;
MassData.Centroid = Position;
// inertia about the local origin
MassData.Inertia = MassData.Mass * (0.5f * Radius * Radius + FVector2.Dot(Position, Position));
}
public float GetAngleCos(Vector2 apos, Vector2 bpos)
{
Vector2 mpos = m.position;
float lengtha = (apos - mpos).magnitude;
float lengthb = (bpos - mpos).magnitude;
float lengthMultiplar = lengtha * lengthb;
Vector2 av = apos - mpos;
Vector2 bv = bpos - mpos;
float dotP = FVector2.Dot(av, bv);
float crossP = FVector2.Cross(av, bv);
return((dotP / lengthMultiplar + 1f) * Math.Sign(crossP));
}
public static void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index, out FVector2 normal, out FVector2 point, out float separation)
{
Debug.Assert(pc.pointCount > 0);
switch (pc.type)
{
case ManifoldType.Circles:
{
FVector2 pointA = MathUtils.Mul(ref xfA, pc.localPoint);
FVector2 pointB = MathUtils.Mul(ref xfB, pc.localPoints[0]);
normal = pointB - pointA;
normal.Normalize();
point = 0.5f * (pointA + pointB);
separation = FVector2.Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, pc.localNormal);
FVector2 planePoint = MathUtils.Mul(ref xfA, pc.localPoint);
FVector2 clipPoint = MathUtils.Mul(ref xfB, pc.localPoints[index]);
separation = FVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, pc.localNormal);
FVector2 planePoint = MathUtils.Mul(ref xfB, pc.localPoint);
FVector2 clipPoint = MathUtils.Mul(ref xfA, pc.localPoints[index]);
separation = FVector2.Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
default:
normal = FVector2.Zero;
point = FVector2.Zero;
separation = 0;
break;
}
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FVector2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
FVector2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB);
FVector2 d = (cB - cA) + rB - rA;
FVector2 ay = MathUtils.Mul(qA, m_localYAxisA);
float sAy = MathUtils.Cross(d + rA, ay);
float sBy = MathUtils.Cross(rB, ay);
float C = FVector2.Dot(d, ay);
float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
float impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
FVector2 P = impulse * ay;
float LA = impulse * sAy;
float LB = impulse * sBy;
cA -= m_invMassA * P;
aA -= m_invIA * LA;
cB += m_invMassB * P;
aB += m_invIB * LB;
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(Math.Abs(C) <= FSSettings.LinearSlop);
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref FVector2 point)
{
FVector2 pLocal = MathUtils.MulT(transform.q, point - transform.p);
for (int i = 0; i < Vertices.Count; ++i)
{
float dot = FVector2.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 FVector2 GetSupportVertex(FVector2 direction)
{
int bestIndex = 0;
float bestValue = FVector2.Dot(Vertices[0], direction);
for (int i = 1; i < Vertices.Count; ++i)
{
float value = FVector2.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()
{
FVector2 w1 = V[0].W;
FVector2 w2 = V[1].W;
FVector2 e12 = w2 - w1;
// w1 region
float d12_2 = -FVector2.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
float d12_1 = FVector2.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.
float 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;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
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();
FVector2 position = transform.p + MathUtils.Mul(transform.q, Position);
FVector2 s = input.Point1 - position;
float b = FVector2.Dot(s, s) - Radius * Radius;
// Solve quadratic equation.
FVector2 r = input.Point2 - input.Point1;
float c = FVector2.Dot(s, r);
float rr = FVector2.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 = s + a * r;
output.Normal.Normalize();
return(true);
}
return(false);
}
/// <summary>
/// tests if ray intersects AABB
/// </summary>
/// <param name="aabb"></param>
/// <returns></returns>
public static bool RayCastAABB(AABB aabb, FVector2 p1, FVector2 p2)
{
AABB segmentAABB = new AABB();
{
FVector2.Min(ref p1, ref p2, out segmentAABB.LowerBound);
FVector2.Max(ref p1, ref p2, out segmentAABB.UpperBound);
}
if (!AABB.TestOverlap(aabb, segmentAABB))
{
return(false);
}
FVector2 rayDir = p2 - p1;
FVector2 rayPos = p1;
FVector2 norm = new FVector2(-rayDir.Y, rayDir.X); //normal to ray
if (norm.Length() == 0.0)
{
return(true); //if ray is just a point, return true (iff point is within aabb, as tested earlier)
}
norm.Normalize();
float dPos = FVector2.Dot(rayPos, norm);
FVector2[] verts = aabb.GetVertices();
float d0 = FVector2.Dot(verts[0], norm) - dPos;
for (int i = 1; i < 4; i++)
{
float d = FVector2.Dot(verts[i], norm) - dPos;
if (Math.Sign(d) != Math.Sign(d0))
{
//return true if the ray splits the vertices (ie: sign of dot products with normal are not all same)
return(true);
}
}
return(false);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
FVector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
// Cdot = dot(u, v + cross(w, r))
FVector2 vpA = vA + MathUtils.Cross(wA, _rA);
FVector2 vpB = vB + MathUtils.Cross(wB, _rB);
float C = _length - MaxLength;
float Cdot = FVector2.Dot(_u, vpB - vpA);
// Predictive constraint.
if (C < 0.0f)
{
Cdot += data.step.inv_dt * C;
}
float impulse = -_mass * Cdot;
float oldImpulse = _impulse;
_impulse = Math.Min(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
FVector2 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;
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FSBody b1 = BodyA;
FSBody b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);
FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length < MaxLength && length > MinLength)
{
return;
}
// Cdot = dot(u, v + cross(w, r))
FVector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
FVector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = FVector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
FVector2 P = impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
b2.LinearVelocityInternal += b2.InvMass * P;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
FVector2 vC = data.velocities[m_indexC].v;
float wC = data.velocities[m_indexC].w;
FVector2 vD = data.velocities[m_indexD].v;
float wD = data.velocities[m_indexD].w;
float Cdot = FVector2.Dot(m_JvAC, vA - vC) + FVector2.Dot(m_JvBD, vB - vD);
Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
float impulse = -m_mass * Cdot;
m_impulse += impulse;
vA += (m_mA * impulse) * m_JvAC;
wA += m_iA * impulse * m_JwA;
vB += (m_mB * impulse) * m_JvBD;
wB += m_iB * impulse * m_JwB;
vC -= (m_mC * impulse) * m_JvAC;
wC -= m_iC * impulse * m_JwC;
vD -= (m_mD * impulse) * m_JvBD;
wD -= m_iD * impulse * m_JwD;
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
data.velocities[m_indexC].v = vC;
data.velocities[m_indexC].w = wC;
data.velocities[m_indexD].v = vD;
data.velocities[m_indexD].w = wD;
}
public override float ComputeSubmergedArea(FVector2 normal, float offset, Transform xf, out FVector2 sc)
{
sc = FVector2.Zero;
//Transform plane into shape co-ordinates
FVector2 normalL = MathUtils.MulT(xf.q, normal);
float offsetL = offset - FVector2.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] = FVector2.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 = MathUtils.Mul(ref xf, MassData.Centroid);
return(MassData.Mass / Density);
}
else
{
//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]);
FVector2 intoVec = new FVector2(
Vertices[intoIndex].X * (1 - intoLambda) + Vertices[intoIndex2].X * intoLambda,
Vertices[intoIndex].Y * (1 - intoLambda) + Vertices[intoIndex2].Y * intoLambda);
FVector2 outoVec = new FVector2(
Vertices[outoIndex].X * (1 - outoLambda) + Vertices[outoIndex2].X * outoLambda,
Vertices[outoIndex].Y * (1 - outoLambda) + Vertices[outoIndex2].Y * outoLambda);
//Initialize accumulator
float area = 0;
FVector2 center = new FVector2(0, 0);
FVector2 p2 = Vertices[intoIndex2];
FVector2 p3;
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;
if (i == outoIndex2)
{
p3 = outoVec;
}
else
{
p3 = Vertices[i];
}
//Add the triangle formed by intoVec,p2,p3
{
FVector2 e1 = p2 - intoVec;
FVector2 e2 = p3 - intoVec;
float D = MathUtils.Cross(e1, 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 = MathUtils.Mul(ref xf, center);
return(area);
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
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.
FVector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
FVector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
FVector2 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 = FVector2.Dot(Normals[i], Vertices[i] - p1);
float denominator = FVector2.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 = MathUtils.Mul(transform.q, Normals[index]);
return(true);
}
return(false);
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < _count; ++i)
{
ContactVelocityConstraint vc = _velocityConstraints[i];
int indexA = vc.indexA;
int indexB = vc.indexB;
float mA = vc.invMassA;
float iA = vc.invIA;
float mB = vc.invMassB;
float iB = vc.invIB;
int pointCount = vc.pointCount;
FVector2 vA = _velocities[indexA].v;
float wA = _velocities[indexA].w;
FVector2 vB = _velocities[indexB].v;
float wB = _velocities[indexB].w;
FVector2 normal = vc.normal;
FVector2 tangent = MathUtils.Cross(normal, 1.0f);
float friction = vc.friction;
//Debug.Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints first because non-penetration is more important
// than friction.
for (int j = 0; j < pointCount; ++j)
{
VelocityConstraintPoint vcp = vc.points[j];
// Relative velocity at contact
FVector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
// Compute tangent force
float vt = FVector2.Dot(dv, tangent) - vc.tangentSpeed;
float lambda = vcp.tangentMass * (-vt);
// b2Clamp the accumulated force
float maxFriction = friction * vcp.normalImpulse;
float newImpulse = MathUtils.Clamp(vcp.tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp.tangentImpulse;
vcp.tangentImpulse = newImpulse;
// Apply contact impulse
FVector2 P = lambda * tangent;
vA -= mA * P;
wA -= iA * MathUtils.Cross(vcp.rA, P);
vB += mB * P;
wB += iB * MathUtils.Cross(vcp.rB, P);
}
// Solve normal constraints
if (vc.pointCount == 1)
{
VelocityConstraintPoint vcp = vc.points[0];
// Relative velocity at contact
FVector2 dv = vB + MathUtils.Cross(wB, vcp.rB) - vA - MathUtils.Cross(wA, vcp.rA);
// Compute normal impulse
float vn = FVector2.Dot(dv, normal);
float lambda = -vcp.normalMass * (vn - vcp.velocityBias);
// b2Clamp the accumulated impulse
float newImpulse = Math.Max(vcp.normalImpulse + lambda, 0.0f);
lambda = newImpulse - vcp.normalImpulse;
vcp.normalImpulse = newImpulse;
// Apply contact impulse
FVector2 P = lambda * normal;
vA -= mA * P;
wA -= iA * MathUtils.Cross(vcp.rA, P);
vB += mB * P;
wB += iB * MathUtils.Cross(vcp.rB, P);
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
VelocityConstraintPoint cp1 = vc.points[0];
VelocityConstraintPoint cp2 = vc.points[1];
FVector2 a = new FVector2(cp1.normalImpulse, cp2.normalImpulse);
//Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
FVector2 dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
FVector2 dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
float vn1 = FVector2.Dot(dv1, normal);
float vn2 = FVector2.Dot(dv2, normal);
FVector2 b = new FVector2();
b.X = vn1 - cp1.velocityBias;
b.Y = vn2 - cp2.velocityBias;
// Compute b'
b -= MathUtils.Mul(ref vc.K, a);
//const float k_errorTol = 1e-3f;
//B2_NOT_USED(k_errorTol);
while (true)
{
//
// Case 1: vn = 0
//
// 0 = A * x + b'
//
// Solve for x:
//
// x = - inv(A) * b'
//
FVector2 x = -MathUtils.Mul(ref vc.normalMass, b);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
// Get the incremental impulse
FVector2 d = x - a;
// Apply incremental impulse
FVector2 P1 = d.X * normal;
FVector2 P2 = d.Y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.X;
cp2.normalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn1 = FVector2.Dot(dv1, normal);
vn2 = FVector2.Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1 + a12 * 0 + b1'
// vn2 = a21 * x1 + a22 * 0 + b2'
//
x.X = -cp1.normalMass * b.X;
x.Y = 0.0f;
vn1 = 0.0f;
vn2 = vc.K.ex.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
// Get the incremental impulse
FVector2 d = x - a;
// Apply incremental impulse
FVector2 P1 = d.X * normal;
FVector2 P2 = d.Y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.X;
cp2.normalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv1 = vB + MathUtils.Cross(wB, cp1.rB) - vA - MathUtils.Cross(wA, cp1.rA);
// Compute normal velocity
vn1 = FVector2.Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2 + b1'
// 0 = a21 * 0 + a22 * x2 + b2'
//
x.X = 0.0f;
x.Y = -cp2.normalMass * b.Y;
vn1 = vc.K.ey.X * x.Y + b.X;
vn2 = 0.0f;
if (x.Y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
FVector2 d = x - a;
// Apply incremental impulse
FVector2 P1 = d.X * normal;
FVector2 P2 = d.Y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.X;
cp2.normalImpulse = x.Y;
#if B2_DEBUG_SOLVER
// Postconditions
dv2 = vB + MathUtils.Cross(wB, cp2.rB) - vA - MathUtils.Cross(wA, cp2.rA);
// Compute normal velocity
vn2 = FVector2.Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2.velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.X = 0.0f;
x.Y = 0.0f;
vn1 = b.X;
vn2 = b.Y;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
// Resubstitute for the incremental impulse
FVector2 d = x - a;
// Apply incremental impulse
FVector2 P1 = d.X * normal;
FVector2 P2 = d.Y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (MathUtils.Cross(cp1.rA, P1) + MathUtils.Cross(cp2.rA, P2));
vB += mB * (P1 + P2);
wB += iB * (MathUtils.Cross(cp1.rB, P1) + MathUtils.Cross(cp2.rB, P2));
// Accumulate
cp1.normalImpulse = x.X;
cp2.normalImpulse = x.Y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
_velocities[indexA].v = vA;
_velocities[indexA].w = wA;
_velocities[indexB].v = vB;
_velocities[indexB].w = wB;
}
}
/// <summary>
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin, not the centroid.
/// </summary>
protected override void ComputeProperties()
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.X = (1/mass) * rho * int(x * dA)
// centroid.Y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
Debug.Assert(Vertices.Count >= 3);
if (_density <= 0)
{
return;
}
FVector2 center = FVector2.Zero;
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
FVector2 s = FVector2.Zero;
// This code would put the reference point inside the polygon.
for (int i = 0; i < Vertices.Count; ++i)
{
s += Vertices[i];
}
s *= 1.0f / Vertices.Count;
const float k_inv3 = 1.0f / 3.0f;
for (int i = 0; i < Vertices.Count; ++i)
{
// Triangle vertices.
FVector2 e1 = Vertices[i] - s;
FVector2 e2 = i + 1 < Vertices.Count ? Vertices[i + 1] - s : Vertices[0] - s;
float D = MathUtils.Cross(e1, e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (e1 + e2);
float ex1 = e1.X, ey1 = e1.Y;
float ex2 = e2.X, ey2 = e2.Y;
float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2;
float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
//The area is too small for the engine to handle.
Debug.Assert(area > Settings.Epsilon);
// We save the area
MassData.Area = area;
// Total mass
MassData.Mass = _density * area;
// Center of mass
center *= 1.0f / area;
MassData.Centroid = center + s;
// Inertia tensor relative to the local origin (point s).
MassData.Inertia = _density * I;
// Shift to center of mass then to original body origin.
MassData.Inertia += MassData.Mass * (FVector2.Dot(MassData.Centroid, MassData.Centroid) - FVector2.Dot(center, center));
}
/// <summary>
/// This resets the mass properties to the sum of the mass properties of the fixtures.
/// This normally does not need to be called unless you called SetMassData to override
/// the mass and you later want to reset the mass.
/// </summary>
public void ResetMassData()
{
// Compute mass data from shapes. Each shape has its own density.
_mass = 0.0f;
InvMass = 0.0f;
_inertia = 0.0f;
InvI = 0.0f;
Sweep.LocalCenter = FVector2.Zero;
// Kinematic bodies have zero mass.
if (BodyType == BodyType.Kinematic)
{
Sweep.C0 = Xf.p;
Sweep.C = Xf.p;
Sweep.A0 = Sweep.A;
return;
}
Debug.Assert(BodyType == BodyType.Dynamic || BodyType == BodyType.Static);
// Accumulate mass over all fixtures.
FVector2 localCenter = FVector2.Zero;
foreach (Fixture f in FixtureList)
{
if (f.Shape._density == 0)
{
continue;
}
MassData massData = f.Shape.MassData;
_mass += massData.Mass;
localCenter += massData.Mass * massData.Centroid;
_inertia += massData.Inertia;
}
//Static bodies only have mass, they don't have other properties. A little hacky tho...
if (BodyType == BodyType.Static)
{
Sweep.C0 = Sweep.C = Xf.p;
return;
}
// Compute center of mass.
if (_mass > 0.0f)
{
InvMass = 1.0f / _mass;
localCenter *= InvMass;
}
else
{
// Force all dynamic bodies to have a positive mass.
_mass = 1.0f;
InvMass = 1.0f;
}
if (_inertia > 0.0f && (Flags & BodyFlags.FixedRotation) == 0)
{
// Center the inertia about the center of mass.
_inertia -= _mass * FVector2.Dot(localCenter, localCenter);
Debug.Assert(_inertia > 0.0f);
InvI = 1.0f / _inertia;
}
else
{
_inertia = 0.0f;
InvI = 0.0f;
}
// Move center of mass.
FVector2 oldCenter = Sweep.C;
Sweep.LocalCenter = localCenter;
Sweep.C0 = Sweep.C = MathUtils.Mul(ref Xf, ref Sweep.LocalCenter);
// Update center of mass velocity.
FVector2 a = Sweep.C - oldCenter;
LinearVelocityInternal += new FVector2(-AngularVelocityInternal * a.Y, AngularVelocityInternal * a.X);
}
/// <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 FVector2 normal, out FixedArray2 <FVector2> points)
{
normal = FVector2.Zero;
points = new FixedArray2 <FVector2>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new FVector2(1.0f, 0.0f);
FVector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
FVector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
if (FVector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
FVector2 cA = pointA + radiusA * normal;
FVector2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
FVector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
FVector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
FVector2 cA = clipPoint + (radiusA - FVector2.Dot(clipPoint - planePoint, normal)) * normal;
FVector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
FVector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
FVector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
FVector2 cB = clipPoint + (radiusB - FVector2.Dot(clipPoint - planePoint, normal)) * normal;
FVector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
// Solve spring constraint
{
float Cdot = FVector2.Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
m_springImpulse += impulse;
FVector2 P = impulse * m_ax;
float LA = impulse * m_sAx;
float LB = impulse * m_sBx;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
// Solve rotational motor constraint
{
float Cdot = wB - wA - m_motorSpeed;
float impulse = -m_motorMass * Cdot;
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = MathUtils.Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line constraint
{
float Cdot = FVector2.Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
float impulse = -m_mass * Cdot;
m_impulse += impulse;
FVector2 P = impulse * m_ay;
float LA = impulse * m_sAy;
float LB = impulse * m_sBy;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
internal void SolveTOI(ref FSTimeStep subStep, int toiIndexA, int toiIndexB)
{
Debug.Assert(toiIndexA < BodyCount);
Debug.Assert(toiIndexB < BodyCount);
// Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
FSBody 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;
}
//b2ContactSolverDef contactSolverDef;
//contactSolverDef.contacts = _contacts;
//contactSolverDef.count = _contactCount;
//contactSolverDef.allocator = _allocator;
//contactSolverDef.step = subStep;
//contactSolverDef.positions = _positions;
//contactSolverDef.velocities = _velocities;
//b2ContactSolver contactSolver(&contactSolverDef);
_contactSolver.Reset(subStep, ContactCount, _contacts, _positions, _velocities);
// Solve position constraints.
for (int i = 0; i < FSSettings.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 < FSSettings.TOIVelocityIterations; ++i)
{
_contactSolver.SolveVelocityConstraints();
}
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
float h = subStep.dt;
// Integrate positions.
for (int i = 0; i < BodyCount; ++i)
{
FVector2 c = _positions[i].c;
float a = _positions[i].a;
FVector2 v = _velocities[i].v;
float w = _velocities[i].w;
// Check for large velocities
FVector2 translation = h * v;
if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared)
{
float ratio = FSSettings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > FSSettings.MaxRotationSquared)
{
float ratio = FSSettings.MaxRotation / Math.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
FSBody body = Bodies[i];
body.Sweep.C = c;
body.Sweep.A = a;
body.LinearVelocity = v;
body.AngularVelocity = w;
body.SynchronizeTransform();
}
Report(_contactSolver._velocityConstraints);
}
public void Solve(ref FSTimeStep step, ref FVector2 gravity)
{
float h = step.dt;
// Integrate velocities and apply damping. Initialize the body state.
for (int i = 0; i < BodyCount; ++i)
{
FSBody b = Bodies[i];
FVector2 c = b.Sweep.C;
float a = b.Sweep.A;
FVector2 v = b.LinearVelocity;
float 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.
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);
}
_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;
// Initialize velocity constraints.
//b2ContactSolverDef contactSolverDef;
//contactSolverDef.step = step;
//contactSolverDef.contacts = m_contacts;
//contactSolverDef.count = m_contactCount;
//contactSolverDef.positions = m_positions;
//contactSolverDef.velocities = m_velocities;
//contactSolverDef.allocator = m_allocator;
_contactSolver.Reset(step, ContactCount, _contacts, _positions, _velocities);
_contactSolver.InitializeVelocityConstraints();
if (FSSettings.EnableWarmstarting)
{
_contactSolver.WarmStart();
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Start();
_tmpTime = 0;
}
#endif
for (int i = 0; i < JointCount; ++i)
{
if (_joints[i].Enabled)
{
_joints[i].InitVelocityConstraints(ref solverData);
}
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_tmpTime += _watch.ElapsedTicks;
}
#endif
// Solve velocity constraints.
for (int i = 0; i < FSSettings.VelocityIterations; ++i)
{
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
FarseerJoint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
joint.SolveVelocityConstraints(ref solverData);
//TODO: Move up before solve?
joint.Validate(step.inv_dt);
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
_contactSolver.SolveVelocityConstraints();
}
// Store impulses for warm starting.
_contactSolver.StoreImpulses();
// Integrate positions
for (int i = 0; i < BodyCount; ++i)
{
FVector2 c = _positions[i].c;
float a = _positions[i].a;
FVector2 v = _velocities[i].v;
float w = _velocities[i].w;
// Check for large velocities
FVector2 translation = h * v;
if (FVector2.Dot(translation, translation) > FSSettings.MaxTranslationSquared)
{
float ratio = FSSettings.MaxTranslation / translation.Length();
v *= ratio;
}
float rotation = h * w;
if (rotation * rotation > FSSettings.MaxRotationSquared)
{
float ratio = FSSettings.MaxRotation / Math.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 < FSSettings.PositionIterations; ++i)
{
bool contactsOkay = _contactSolver.SolvePositionConstraints();
bool jointsOkay = true;
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Start();
}
#endif
for (int j = 0; j < JointCount; ++j)
{
FarseerJoint joint = _joints[j];
if (!joint.Enabled)
{
continue;
}
bool jointOkay = joint.SolvePositionConstraints(ref solverData);
jointsOkay = jointsOkay && jointOkay;
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
_watch.Stop();
_tmpTime += _watch.ElapsedTicks;
_watch.Reset();
}
#endif
if (contactsOkay && jointsOkay)
{
// Exit early if the position errors are small.
positionSolved = true;
break;
}
}
#if (!SILVERLIGHT)
if (FSSettings.EnableDiagnostics)
{
JointUpdateTime = _tmpTime;
}
#endif
// Copy state buffers back to the bodies
for (int i = 0; i < BodyCount; ++i)
{
FSBody 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 (FSSettings.AllowSleep)
{
float minSleepTime = FSSettings.MaxFloat;
for (int i = 0; i < BodyCount; ++i)
{
FSBody b = Bodies[i];
if (b.BodyType == BodyType.Static)
{
continue;
}
if ((b.Flags & BodyFlags.AutoSleep) == 0 ||
b.AngularVelocityInternal * b.AngularVelocityInternal > AngTolSqr ||
FVector2.Dot(b.LinearVelocityInternal, b.LinearVelocityInternal) > LinTolSqr)
{
b.SleepTime = 0.0f;
minSleepTime = 0.0f;
}
else
{
b.SleepTime += h;
minSleepTime = Math.Min(minSleepTime, b.SleepTime);
}
}
if (minSleepTime >= FSSettings.TimeToSleep && positionSolved)
{
for (int i = 0; i < BodyCount; ++i)
{
FSBody b = Bodies[i];
b.Awake = false;
}
}
}
}
internal override void InitVelocityConstraints(ref SolverData data)
{
m_indexA = BodyA.IslandIndex;
m_indexB = BodyB.IslandIndex;
m_localCenterA = BodyA.Sweep.LocalCenter;
m_localCenterB = BodyB.Sweep.LocalCenter;
m_invMassA = BodyA.InvMass;
m_invMassB = BodyB.InvMass;
m_invIA = BodyA.InvI;
m_invIB = BodyB.InvI;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
FVector2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
FVector2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
FVector2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
FVector2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB);
FVector2 d1 = cB + rB - cA - rA;
// Point to line constraint
{
m_ay = MathUtils.Mul(qA, m_localYAxisA);
m_sAy = MathUtils.Cross(d1 + rA, m_ay);
m_sBy = MathUtils.Cross(rB, m_ay);
m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
if (m_mass > 0.0f)
{
m_mass = 1.0f / m_mass;
}
}
// Spring constraint
m_springMass = 0.0f;
m_bias = 0.0f;
m_gamma = 0.0f;
if (SpringFrequencyHz > 0.0f)
{
m_ax = MathUtils.Mul(qA, m_localXAxisA);
m_sAx = MathUtils.Cross(d1 + rA, m_ax);
m_sBx = MathUtils.Cross(rB, m_ax);
float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
if (invMass > 0.0f)
{
m_springMass = 1.0f / invMass;
float C = FVector2.Dot(d1, m_ax);
// Frequency
float omega = 2.0f * FSSettings.Pi * SpringFrequencyHz;
// Damping coefficient
float d = 2.0f * m_springMass * SpringDampingRatio * omega;
// Spring stiffness
float k = m_springMass * omega * omega;
// magic formulas
float h = data.step.dt;
m_gamma = h * (d + h * k);
if (m_gamma > 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_bias = C * h * k * m_gamma;
m_springMass = invMass + m_gamma;
if (m_springMass > 0.0f)
{
m_springMass = 1.0f / m_springMass;
}
}
}
else
{
m_springImpulse = 0.0f;
}
// Rotational motor
if (m_enableMotor)
{
m_motorMass = iA + iB;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
else
{
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
}
if (FSSettings.EnableWarmstarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_springImpulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
FVector2 P = m_impulse * m_ay + m_springImpulse * m_ax;
float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
vA -= m_invMassA * P;
wA -= m_invIA * LA;
vB += m_invMassB * P;
wB += m_invIB * LB;
}
else
{
m_impulse = 0.0f;
m_springImpulse = 0.0f;
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}