/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref FPVector direction, out FPVector result)
{
FPVector expandVector;
FPVector.Normalize(ref direction, out expandVector);
FPVector.Multiply(ref expandVector, sphericalExpansion, out expandVector);
int minIndex = 0;
FP min = FPVector.Dot(ref points[0], ref direction);
FP dot = FPVector.Dot(ref points[1], ref direction);
if (dot > min)
{
min = dot;
minIndex = 1;
}
dot = FPVector.Dot(ref points[2], ref direction);
if (dot > min)
{
min = dot;
minIndex = 2;
}
FPVector.Add(ref points[minIndex], ref expandVector, out result);
}
/// <summary>
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
/// </summary>
/// <param name="b">The b.</param>
/// <returns></returns>
public FPVector Solve33(FPVector b)
{
FP det = FPVector.Dot(ex, FPVector.Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
return(new FPVector(det * FPVector.Dot(b, FPVector.Cross(ey, ez)), det * FPVector.Dot(ex, FPVector.Cross(b, ez)), det * FPVector.Dot(ex, FPVector.Cross(ey, b))));
}
/// Test if point p and d lie on opposite sides of plane through abc
public int PointOutsideOfPlane(FPVector p, FPVector a, FPVector b, FPVector c, FPVector d)
{
FPVector normal = FPVector.Cross(b - a, c - a);
FP signp = FPVector.Dot(p - a, normal); // [AP AB AC]
FP signd = FPVector.Dot(d - a, normal); // [AD AB AC]
//if (CatchDegenerateTetrahedron)
if (signd * signd < (FP.EN8))
{
return(-1);
}
// Points on opposite sides if expression signs are opposite
return(signp * signd < FP.Zero ? 1 : 0);
}
/// <summary>
/// Iteratively solve this constraint.
/// </summary>
public override void Iterate()
{
if (skipConstraint)
{
return;
}
FP jv = FPVector.Dot(ref body1.linearVelocity, ref jacobian[0]);
jv += FPVector.Dot(ref body2.linearVelocity, ref jacobian[1]);
FP softnessScalar = accumulatedImpulse * softnessOverDt;
FP lambda = -effectiveMass * (jv + bias + softnessScalar);
if (behavior == DistanceBehavior.LimitMinimumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = FPMath.Max(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else if (behavior == DistanceBehavior.LimitMaximumDistance)
{
FP previousAccumulatedImpulse = accumulatedImpulse;
accumulatedImpulse = FPMath.Min(accumulatedImpulse + lambda, 0);
lambda = accumulatedImpulse - previousAccumulatedImpulse;
}
else
{
accumulatedImpulse += lambda;
}
FPVector temp;
if (!body1.isStatic)
{
FPVector.Multiply(ref jacobian[0], lambda * body1.inverseMass, out temp);
FPVector.Add(ref temp, ref body1.linearVelocity, out body1.linearVelocity);
}
if (!body2.isStatic)
{
FPVector.Multiply(ref jacobian[1], lambda * body2.inverseMass, out temp);
FPVector.Add(ref temp, ref body2.linearVelocity, out body2.linearVelocity);
}
}
/// <summary>
/// SupportMapping. Finds the point in the shape furthest away from the given direction.
/// Imagine a plane with a normal in the search direction. Now move the plane along the normal
/// until the plane does not intersect the shape. The last intersection point is the result.
/// </summary>
/// <param name="direction">The direction.</param>
/// <param name="result">The result.</param>
public override void SupportMapping(ref FPVector direction, out FPVector result)
{
FP maxDotProduct = FP.MinValue;
int maxIndex = 0;
FP dotProduct;
for (int i = 0; i < vertices.Count; i++)
{
dotProduct = FPVector.Dot(vertices[i], direction);
if (dotProduct > maxDotProduct)
{
maxDotProduct = dotProduct;
maxIndex = i;
}
}
result = vertices[maxIndex] - this.shifted;
}
private static int FindExtremePoint(List <FPVector> points, ref FPVector dir)
{
int index = 0;
FP current = FP.MinValue;
FPVector point; FP value;
for (int i = 1; i < points.Count; i++)
{
point = points[i];
value = FPVector.Dot(ref point, ref dir);
if (value > current)
{
current = value; index = i;
}
}
return(index);
}
private void FindSupportPoints(RigidBody body1, RigidBody body2,
Shape shape1, Shape shape2, ref FPVector point, ref FPVector normal,
out FPVector point1, out FPVector point2)
{
FPVector mn; FPVector.Negate(ref normal, out mn);
FPVector sA; SupportMapping(body1, shape1, ref mn, out sA);
FPVector sB; SupportMapping(body2, shape2, ref normal, out sB);
FPVector.Subtract(ref sA, ref point, out sA);
FPVector.Subtract(ref sB, ref point, out sB);
FP dot1 = FPVector.Dot(ref sA, ref normal);
FP dot2 = FPVector.Dot(ref sB, ref normal);
FPVector.Multiply(ref normal, dot1, out sA);
FPVector.Multiply(ref normal, dot2, out sB);
FPVector.Add(ref point, ref sA, out point1);
FPVector.Add(ref point, ref sB, out point2);
}
private void UpdateArbiterContacts(Arbiter arbiter)
{
if (arbiter.contactList.Count == 0)
{
lock (removedArbiterStack) { removedArbiterStack.Push(arbiter); }
return;
}
for (int i = arbiter.contactList.Count - 1; i >= 0; i--)
{
Contact c = arbiter.contactList[i];
c.UpdatePosition();
if (c.penetration < -contactSettings.breakThreshold)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
else
{
FPVector diff; FPVector.Subtract(ref c.p1, ref c.p2, out diff);
FP distance = FPVector.Dot(ref diff, ref c.normal);
diff = diff - distance * c.normal;
distance = diff.sqrMagnitude;
// hack (multiplication by factor 100) in the
// following line.
if (distance > contactSettings.breakThreshold * contactSettings.breakThreshold * 100)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
}
}
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref FPMatrix orientation1,
ref FPMatrix orientation2, ref FPVector position1, ref FPVector position2,
out FPVector point, out FPVector normal, out FP penetration)
{
// Used variables
FPVector temp1, temp2;
FPVector v01, v02, v0;
FPVector v11, v12, v1;
FPVector v21, v22, v2;
FPVector v31, v32, v3;
FPVector v41 = FPVector.zero, v42 = FPVector.zero, v4 = FPVector.zero;
FPVector mn;
// Initialization of the output
point = normal = FPVector.zero;
penetration = FP.Zero;
//JVector right = JVector.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
FPVector.Transform(ref v01, ref orientation1, out v01);
FPVector.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
FPVector.Transform(ref v02, ref orientation2, out v02);
FPVector.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
FPVector.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.IsNearlyZero())
{
v0 = new FPVector(FP.EN4, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
FPVector.Negate(ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
FPVector.Subtract(ref v12, ref v11, out v1);
if (FPVector.Dot(ref v1, ref normal) <= FP.Zero)
{
return(false);
}
// v2 = support perpendicular to v1,v0
FPVector.Cross(ref v1, ref v0, out normal);
if (normal.IsNearlyZero())
{
FPVector.Subtract(ref v1, ref v0, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
normal.Normalize();
point = v11;
FPVector.Add(ref point, ref v12, out point);
FPVector.Multiply(ref point, FP.Half, out point);
FPVector.Subtract(ref v12, ref v11, out temp1);
penetration = FPVector.Dot(ref temp1, ref normal);
//point = v11;
//point2 = v12;
return(true);
}
FPVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
FPVector.Subtract(ref v22, ref v21, out v2);
if (FPVector.Dot(ref v2, ref normal) <= FP.Zero)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
FP dist = FPVector.Dot(ref normal, ref v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > FP.Zero)
{
FPVector.Swap(ref v1, ref v2);
FPVector.Swap(ref v11, ref v21);
FPVector.Swap(ref v12, ref v22);
FPVector.Negate(ref normal, out normal);
UnityEngine.Debug.Log("normal: " + normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
FPVector.Negate(ref normal, out mn);
//UnityEngine.Debug.Log("mn: " + mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
FPVector.Subtract(ref v32, ref v31, out v3);
if (FPVector.Dot(ref v3, ref normal) <= FP.Zero)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
FPVector.Cross(ref v1, ref v3, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v2 = v3;
v21 = v31;
v22 = v32;
FPVector.Subtract(ref v1, ref v0, out temp1);
FPVector.Subtract(ref v3, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
FPVector.Cross(ref v3, ref v2, out temp1);
if (FPVector.Dot(ref temp1, ref v0) < FP.Zero)
{
v1 = v3;
v11 = v31;
v12 = v32;
FPVector.Subtract(ref v3, ref v0, out temp1);
FPVector.Subtract(ref v2, ref v0, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
//UnityEngine.Debug.Log("normal: " + normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
/*
* UnityEngine.Debug.LogError(" ::Start STATE");
* UnityEngine.Debug.Log(temp1 + " " + temp2);
* UnityEngine.Debug.Log( v01 + " " + v02 + " "+ v0);
* UnityEngine.Debug.Log( v11+" "+ v12 +" "+ v1);
* UnityEngine.Debug.Log( v21 +" "+ v22 +" "+ v2);
* UnityEngine.Debug.Log( v31 +" "+ v32 +" "+ v3);
* UnityEngine.Debug.Log( v41 +" "+ v42 +" "+ v4);
* UnityEngine.Debug.Log( mn);
*
* UnityEngine.Debug.LogError(" ::END STATE");
*/
// Compute normal of the wedge face
FPVector.Subtract(ref v2, ref v1, out temp1);
FPVector.Subtract(ref v3, ref v1, out temp2);
FPVector.Cross(ref temp1, ref temp2, out normal);
// Beginer
// UnityEngine.Debug.Log("normal: " + normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.IsNearlyZero())
{
return(true);
}
normal.Normalize();
//UnityEngine.Debug.Log("normal: " + normal);
// Compute distance from origin to wedge face
FP d = FPVector.Dot(ref normal, ref v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
FPVector.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
FPVector.Subtract(ref v42, ref v41, out v4);
FPVector.Subtract(ref v4, ref v3, out temp1);
FP delta = FPVector.Dot(ref temp1, ref normal);
penetration = FPVector.Dot(ref v4, ref normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= FP.Zero || phase2 > MaximumIterations)
{
if (hit)
{
FPVector.Cross(ref v1, ref v2, out temp1);
FP b0 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v3, ref v2, out temp1);
FP b1 = FPVector.Dot(ref temp1, ref v0);
FPVector.Cross(ref v0, ref v1, out temp1);
FP b2 = FPVector.Dot(ref temp1, ref v3);
FPVector.Cross(ref v2, ref v1, out temp1);
FP b3 = FPVector.Dot(ref temp1, ref v0);
FP sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
FPVector.Cross(ref v2, ref v3, out temp1);
b1 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v3, ref v1, out temp1);
b2 = FPVector.Dot(ref temp1, ref normal);
FPVector.Cross(ref v1, ref v2, out temp1);
b3 = FPVector.Dot(ref temp1, ref normal);
sum = b1 + b2 + b3;
}
FP inv = FP.One / sum;
FPVector.Multiply(ref v01, b0, out point);
FPVector.Multiply(ref v11, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v21, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v31, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v02, b0, out temp2);
FPVector.Add(ref temp2, ref point, out point);
FPVector.Multiply(ref v12, b1, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v22, b2, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref v32, b3, out temp1);
FPVector.Add(ref point, ref temp1, out point);
FPVector.Multiply(ref point, inv * FP.Half, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
//// Compute the tetrahedron dividing face (v4,v0,v1)
//JVector.Cross(ref v4, ref v1, out temp1);
//FP d1 = JVector.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//JVector.Cross(ref v4, ref v2, out temp1);
//FP d2 = JVector.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
//UnityEngine.Debug.LogError("v4:" + v4 + " v0:" + v0);
FPVector.Cross(ref v4, ref v0, out temp1);
//UnityEngine.Debug.LogError("temp1:"+ temp1);
//Ender
// UnityEngine.Debug.Log("normal: " + normal);
FP dot = FPVector.Dot(ref temp1, ref v1);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 temp1:" + temp1 + " v2:" + v2 );
dot = FPVector.Dot(ref temp1, ref v2);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v1->v4");
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < v3->v4");
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
// UnityEngine.Debug.Log("dot < 0 temp1:" + temp1 + " v3:" + v3 );
dot = FPVector.Dot(ref temp1, ref v3);
if (dot >= FP.Zero)
{
// UnityEngine.Debug.Log("dot >= 0 v2 => v4");
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// UnityEngine.Debug.Log("dot < 0 v1 => v4");
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
public bool ClosestPtPointTriangle(FPVector p, FPVector a, FPVector b, FPVector c,
ref SubSimplexClosestResult result)
{
result.UsedVertices.Reset();
FP v, w;
// Check if P in vertex region outside A
FPVector ab = b - a;
FPVector ac = c - a;
FPVector ap = p - a;
FP d1 = FPVector.Dot(ab, ap);
FP d2 = FPVector.Dot(ac, ap);
if (d1 <= FP.Zero && d2 <= FP.Zero)
{
result.ClosestPointOnSimplex = a;
result.UsedVertices.UsedVertexA = true;
result.SetBarycentricCoordinates(1, 0, 0, 0);
return(true); // a; // barycentric coordinates (1,0,0)
}
// Check if P in vertex region outside B
FPVector bp = p - b;
FP d3 = FPVector.Dot(ab, bp);
FP d4 = FPVector.Dot(ac, bp);
if (d3 >= FP.Zero && d4 <= d3)
{
result.ClosestPointOnSimplex = b;
result.UsedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(0, 1, 0, 0);
return(true); // b; // barycentric coordinates (0,1,0)
}
// Check if P in edge region of AB, if so return projection of P onto AB
FP vc = d1 * d4 - d3 * d2;
if (vc <= FP.Zero && d1 >= FP.Zero && d3 <= FP.Zero)
{
v = d1 / (d1 - d3);
result.ClosestPointOnSimplex = a + v * ab;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(1 - v, v, 0, 0);
return(true);
//return a + v * ab; // barycentric coordinates (1-v,v,0)
}
// Check if P in vertex region outside C
FPVector cp = p - c;
FP d5 = FPVector.Dot(ab, cp);
FP d6 = FPVector.Dot(ac, cp);
if (d6 >= FP.Zero && d5 <= d6)
{
result.ClosestPointOnSimplex = c;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 0, 1, 0);
return(true);//c; // barycentric coordinates (0,0,1)
}
// Check if P in edge region of AC, if so return projection of P onto AC
FP vb = d5 * d2 - d1 * d6;
if (vb <= FP.Zero && d2 >= FP.Zero && d6 <= FP.Zero)
{
w = d2 / (d2 - d6);
result.ClosestPointOnSimplex = a + w * ac;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(1 - w, 0, w, 0);
return(true);
//return a + w * ac; // barycentric coordinates (1-w,0,w)
}
// Check if P in edge region of BC, if so return projection of P onto BC
FP va = d3 * d6 - d5 * d4;
if (va <= FP.Zero && (d4 - d3) >= FP.Zero && (d5 - d6) >= FP.Zero)
{
w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
result.ClosestPointOnSimplex = b + w * (c - b);
result.UsedVertices.UsedVertexB = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 1 - w, w, 0);
return(true);
// return b + w * (c - b); // barycentric coordinates (0,1-w,w)
}
// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
FP denom = FP.One / (va + vb + vc);
v = vb * denom;
w = vc * denom;
result.ClosestPointOnSimplex = a + ab * v + ac * w;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexB = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(1 - v - w, v, w, 0);
return(true);
}
/// <summary>
/// Checks if given point is within a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="point">The point to check.</param>
/// <returns>Returns true if the point is within the shape, otherwise false.</returns>
public static bool Pointcast(ISupportMappable support, ref FPMatrix orientation, ref FPVector position, ref FPVector point)
{
FPVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref point, out arbitraryPoint);
FPVector.Subtract(ref point, ref arbitraryPoint, out arbitraryPoint);
FPVector r; support.SupportCenter(out r);
FPVector.Transform(ref r, ref orientation, out r);
FPVector.Add(ref position, ref r, out r);
FPVector.Subtract(ref point, ref r, out r);
FPVector x = point;
FPVector w, p;
FP VdotR;
FPVector v; FPVector.Subtract(ref x, ref arbitraryPoint, out v);
FP dist = v.sqrMagnitude;
FP epsilon = CollideEpsilon;
int maxIter = MaxIterations;
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
while ((dist > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
FPVector.Subtract(ref x, ref p, out w);
FP VdotW = FPVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = FPVector.Dot(ref v, ref r);
if (VdotR >= -(FPMath.Epsilon * FPMath.Epsilon))
{
simplexSolverPool.GiveBack(simplexSolver); return(false);
}
else
{
simplexSolver.Reset();
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
dist = v.sqrMagnitude;
}
else
{
dist = FP.Zero;
}
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
public bool UpdateClosestVectorAndPoints()
{
if (_needsUpdate)
{
_cachedBC.Reset();
_needsUpdate = false;
FPVector p, a, b, c, d;
switch (NumVertices)
{
case 0:
_cachedValidClosest = false;
break;
case 1:
_cachedPA = _simplexPointsP[0];
_cachedPB = _simplexPointsQ[0];
_cachedV = _cachedPA - _cachedPB;
_cachedBC.Reset();
_cachedBC.SetBarycentricCoordinates(1f, FP.Zero, FP.Zero, FP.Zero);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 2:
//closest point origin from line segment
FPVector from = _simplexVectorW[0];
FPVector to = _simplexVectorW[1];
//FPVector nearest;
FPVector diff = from * (-1);
FPVector v = to - from;
FP t = FPVector.Dot(v, diff);
if (t > 0)
{
FP dotVV = v.sqrMagnitude;
if (t < dotVV)
{
t /= dotVV;
diff -= t * v;
_cachedBC.UsedVertices.UsedVertexA = true;
_cachedBC.UsedVertices.UsedVertexB = true;
}
else
{
t = 1;
diff -= v;
//reduce to 1 point
_cachedBC.UsedVertices.UsedVertexB = true;
}
}
else
{
t = 0;
//reduce to 1 point
_cachedBC.UsedVertices.UsedVertexA = true;
}
_cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);
//nearest = from + t * v;
_cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
_cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 3:
//closest point origin from triangle
p = new FPVector();
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);
_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
_cachedValidClosest = _cachedBC.IsValid;
break;
case 4:
p = new FPVector();
a = _simplexVectorW[0];
b = _simplexVectorW[1];
c = _simplexVectorW[2];
d = _simplexVectorW[3];
bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);
if (hasSeperation)
{
_cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
_cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
_simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
_simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
_simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
_cachedV = _cachedPA - _cachedPB;
ReduceVertices(_cachedBC.UsedVertices);
}
else
{
if (_cachedBC.Degenerate)
{
_cachedValidClosest = false;
}
else
{
_cachedValidClosest = true;
//degenerate case == false, penetration = true + zero
_cachedV.x = _cachedV.y = _cachedV.z = FP.Zero;
}
break; // !!!!!!!!!!!! proverit na vsakiy sluchai
}
_cachedValidClosest = _cachedBC.IsValid;
//closest point origin from tetrahedron
break;
default:
_cachedValidClosest = false;
break;
}
}
return(_cachedValidClosest);
}
// public static bool TimeOfImpact(ISupportMappable support1, ISupportMappable support2, ref JMatrix orientation1,
//ref JMatrix orientation2, ref JVector position1, ref JVector position2, ref JVector sweptA, ref JVector sweptB,
//out JVector p1, out JVector p2, out JVector normal)
// {
// VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
// simplexSolver.Reset();
// FP lambda = FP.Zero;
// p1 = p2 = JVector.Zero;
// JVector x1 = position1;
// JVector x2 = position2;
// JVector r = sweptA - sweptB;
// JVector w, v;
// JVector supVertexA;
// JVector rn = JVector.Negate(r);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref rn, out supVertexA);
// JVector supVertexB;
// SupportMapTransformed(support2, ref orientation2, ref x2, ref r, out supVertexB);
// v = supVertexA - supVertexB;
// bool hasResult = false;
// normal = JVector.Zero;
// FP lastLambda = lambda;
// int maxIter = MaxIterations;
// FP distSq = v.LengthSquared();
// FP epsilon = FP.EN5;
// FP VdotR;
// while ((distSq > epsilon) && (maxIter-- != 0))
// {
// JVector vn = JVector.Negate(v);
// SupportMapTransformed(support1, ref orientation1, ref x1, ref vn, out supVertexA);
// SupportMapTransformed(support2, ref orientation2, ref x2, ref v, out supVertexB);
// w = supVertexA - supVertexB;
// FP VdotW = JVector.Dot(ref v, ref w);
// if (VdotW > FP.Zero)
// {
// VdotR = JVector.Dot(ref v, ref r);
// if (VdotR >= -JMath.Epsilon)
// {
// simplexSolverPool.GiveBack(simplexSolver);
// return false;
// }
// else
// {
// lambda = lambda - VdotW / VdotR;
// x1 = position1 + lambda * sweptA;
// x2 = position2 + lambda * sweptB;
// w = supVertexA - supVertexB;
// normal = v;
// hasResult = true;
// }
// }
// if (!simplexSolver.InSimplex(w)) simplexSolver.AddVertex(w, supVertexA, supVertexB);
// if (simplexSolver.Closest(out v))
// {
// distSq = v.LengthSquared();
// normal = v;
// hasResult = true;
// }
// else distSq = FP.Zero;
// }
// simplexSolver.ComputePoints(out p1, out p2);
// if (normal.LengthSquared() > JMath.Epsilon * JMath.Epsilon)
// normal.Normalize();
// p1 = p1 - lambda * sweptA;
// p2 = p2 - lambda * sweptB;
// simplexSolverPool.GiveBack(simplexSolver);
// return true;
// }
#endregion
// see: btSubSimplexConvexCast.cpp
/// <summary>
/// Checks if a ray definied through it's origin and direction collides
/// with a shape.
/// </summary>
/// <param name="support">The supportmap implementation representing the shape.</param>
/// <param name="orientation">The orientation of the shape.</param>
/// <param name="invOrientation">The inverse orientation of the shape.</param>
/// <param name="position">The position of the shape.</param>
/// <param name="origin">The origin of the ray.</param>
/// <param name="direction">The direction of the ray.</param>
/// <param name="fraction">The fraction which gives information where at the
/// ray the collision occured. The hitPoint is calculated by: origin+friction*direction.</param>
/// <param name="normal">The normal from the ray collision.</param>
/// <returns>Returns true if the ray hit the shape, false otherwise.</returns>
public static bool Raycast(ISupportMappable support, ref FPMatrix orientation, ref FPMatrix invOrientation,
ref FPVector position, ref FPVector origin, ref FPVector direction, out FP fraction, out FPVector normal)
{
VoronoiSimplexSolver simplexSolver = simplexSolverPool.GetNew();
simplexSolver.Reset();
normal = FPVector.zero;
fraction = FP.MaxValue;
FP lambda = FP.Zero;
FPVector r = direction;
FPVector x = origin;
FPVector w, p, v;
FPVector arbitraryPoint;
SupportMapTransformed(support, ref orientation, ref position, ref r, out arbitraryPoint);
FPVector.Subtract(ref x, ref arbitraryPoint, out v);
int maxIter = MaxIterations;
FP distSq = v.sqrMagnitude;
FP epsilon = FP.EN6;
FP VdotR;
while ((distSq > epsilon) && (maxIter-- != 0))
{
SupportMapTransformed(support, ref orientation, ref position, ref v, out p);
FPVector.Subtract(ref x, ref p, out w);
FP VdotW = FPVector.Dot(ref v, ref w);
if (VdotW > FP.Zero)
{
VdotR = FPVector.Dot(ref v, ref r);
if (VdotR >= -FPMath.Epsilon)
{
simplexSolverPool.GiveBack(simplexSolver);
return(false);
}
else
{
lambda = lambda - VdotW / VdotR;
FPVector.Multiply(ref r, lambda, out x);
FPVector.Add(ref origin, ref x, out x);
FPVector.Subtract(ref x, ref p, out w);
normal = v;
}
}
if (!simplexSolver.InSimplex(w))
{
simplexSolver.AddVertex(w, x, p);
}
if (simplexSolver.Closest(out v))
{
distSq = v.sqrMagnitude;
}
else
{
distSq = FP.Zero;
}
}
#region Retrieving hitPoint
// Giving back the fraction like this *should* work
// but is inaccurate against large objects:
// fraction = lambda;
FPVector p1, p2;
simplexSolver.ComputePoints(out p1, out p2);
p2 = p2 - origin;
fraction = p2.magnitude / direction.magnitude;
#endregion
if (normal.sqrMagnitude > FPMath.Epsilon * FPMath.Epsilon)
{
normal.Normalize();
}
simplexSolverPool.GiveBack(simplexSolver);
return(true);
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
FP dvx, dvy, dvz;
dvx = (body2.angularVelocity.y * relativePos2.z) - (body2.angularVelocity.z * relativePos2.y) + body2.linearVelocity.x;
dvy = (body2.angularVelocity.z * relativePos2.x) - (body2.angularVelocity.x * relativePos2.z) + body2.linearVelocity.y;
dvz = (body2.angularVelocity.x * relativePos2.y) - (body2.angularVelocity.y * relativePos2.x) + body2.linearVelocity.z;
dvx = dvx - (body1.angularVelocity.y * relativePos1.z) + (body1.angularVelocity.z * relativePos1.y) - body1.linearVelocity.x;
dvy = dvy - (body1.angularVelocity.z * relativePos1.x) + (body1.angularVelocity.x * relativePos1.z) - body1.linearVelocity.y;
dvz = dvz - (body1.angularVelocity.x * relativePos1.y) + (body1.angularVelocity.y * relativePos1.x) - body1.linearVelocity.z;
FP kNormal = FP.Zero;
FPVector rantra = FPVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * normal.z) - (relativePos1.z * normal.y);
rantra.y = (relativePos1.z * normal.x) - (relativePos1.x * normal.z);
rantra.z = (relativePos1.x * normal.y) - (relativePos1.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
FPVector rbntrb = FPVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * normal.z) - (relativePos2.z * normal.y);
rbntrb.y = (relativePos2.z * normal.x) - (relativePos2.x * normal.z);
rbntrb.z = (relativePos2.x * normal.y) - (relativePos2.y * normal.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kNormal += rantra.x * normal.x + rantra.y * normal.y + rantra.z * normal.z;
}
if (!treatBody2AsStatic)
{
kNormal += rbntrb.x * normal.x + rbntrb.y * normal.y + rbntrb.z * normal.z;
}
massNormal = FP.One / kNormal;
FP num = dvx * normal.x + dvy * normal.y + dvz * normal.z;
tangent.x = dvx - normal.x * num;
tangent.y = dvy - normal.y * num;
tangent.z = dvz - normal.z * num;
num = tangent.x * tangent.x + tangent.y * tangent.y + tangent.z * tangent.z;
if (num != FP.Zero)
{
num = FP.Sqrt(num);
tangent.x /= num;
tangent.y /= num;
tangent.z /= num;
}
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rantra.x = (relativePos1.y * tangent.z) - (relativePos1.z * tangent.y);
rantra.y = (relativePos1.z * tangent.x) - (relativePos1.x * tangent.z);
rantra.z = (relativePos1.x * tangent.y) - (relativePos1.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rantra.x * body1.invInertiaWorld.M11) + (rantra.y * body1.invInertiaWorld.M21)) + (rantra.z * body1.invInertiaWorld.M31);
FP num1 = ((rantra.x * body1.invInertiaWorld.M12) + (rantra.y * body1.invInertiaWorld.M22)) + (rantra.z * body1.invInertiaWorld.M32);
FP num2 = ((rantra.x * body1.invInertiaWorld.M13) + (rantra.y * body1.invInertiaWorld.M23)) + (rantra.z * body1.invInertiaWorld.M33);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rantra.y * relativePos1.z) - (rantra.z * relativePos1.y);
num1 = (rantra.z * relativePos1.x) - (rantra.x * relativePos1.z);
num2 = (rantra.x * relativePos1.y) - (rantra.y * relativePos1.x);
rantra.x = num0; rantra.y = num1; rantra.z = num2;
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
// JVector.Cross(ref relativePos1, ref normal, out rantra);
rbntrb.x = (relativePos2.y * tangent.z) - (relativePos2.z * tangent.y);
rbntrb.y = (relativePos2.z * tangent.x) - (relativePos2.x * tangent.z);
rbntrb.z = (relativePos2.x * tangent.y) - (relativePos2.y * tangent.x);
// JVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FP num0 = ((rbntrb.x * body2.invInertiaWorld.M11) + (rbntrb.y * body2.invInertiaWorld.M21)) + (rbntrb.z * body2.invInertiaWorld.M31);
FP num1 = ((rbntrb.x * body2.invInertiaWorld.M12) + (rbntrb.y * body2.invInertiaWorld.M22)) + (rbntrb.z * body2.invInertiaWorld.M32);
FP num2 = ((rbntrb.x * body2.invInertiaWorld.M13) + (rbntrb.y * body2.invInertiaWorld.M23)) + (rbntrb.z * body2.invInertiaWorld.M33);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
//JVector.Cross(ref rantra, ref relativePos1, out rantra);
num0 = (rbntrb.y * relativePos2.z) - (rbntrb.z * relativePos2.y);
num1 = (rbntrb.z * relativePos2.x) - (rbntrb.x * relativePos2.z);
num2 = (rbntrb.x * relativePos2.y) - (rbntrb.y * relativePos2.x);
rbntrb.x = num0; rbntrb.y = num1; rbntrb.z = num2;
}
}
if (!treatBody1AsStatic)
{
kTangent += FPVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += FPVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = normal.x * dvx + normal.y * dvy + normal.z * dvz; //JVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * FPMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = FPMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -(tangent.x * dvx + tangent.y * dvy + tangent.z * dvz);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
FPVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
/*if (relNormalVel < -FP.One && newContact)
* {
* restitutionBias = TSMath.Max(-restitution * relNormalVel, restitutionBias);
* }*/
restitutionBias = FPMath.Max(-restitution * relNormalVel, restitutionBias);
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse.x = normal.x * accumulatedNormalImpulse + tangent.x * accumulatedTangentImpulse;
impulse.y = normal.y * accumulatedNormalImpulse + tangent.y * accumulatedTangentImpulse;
impulse.z = normal.z * accumulatedNormalImpulse + tangent.z * accumulatedTangentImpulse;
if (!treatBody1AsStatic)
{
body1.linearVelocity.x -= (impulse.x * body1.inverseMass);
body1.linearVelocity.y -= (impulse.y * body1.inverseMass);
body1.linearVelocity.z -= (impulse.z * body1.inverseMass);
if (!body1IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos1.y * impulse.z - relativePos1.z * impulse.y;
num1 = relativePos1.z * impulse.x - relativePos1.x * impulse.z;
num2 = relativePos1.x * impulse.y - relativePos1.y * impulse.x;
FP num3 =
(((num0 * body1.invInertiaWorld.M11) +
(num1 * body1.invInertiaWorld.M21)) +
(num2 * body1.invInertiaWorld.M31));
FP num4 =
(((num0 * body1.invInertiaWorld.M12) +
(num1 * body1.invInertiaWorld.M22)) +
(num2 * body1.invInertiaWorld.M32));
FP num5 =
(((num0 * body1.invInertiaWorld.M13) +
(num1 * body1.invInertiaWorld.M23)) +
(num2 * body1.invInertiaWorld.M33));
body1.angularVelocity.x -= num3;
body1.angularVelocity.y -= num4;
body1.angularVelocity.z -= num5;
}
}
if (!treatBody2AsStatic)
{
body2.linearVelocity.x += (impulse.x * body2.inverseMass);
body2.linearVelocity.y += (impulse.y * body2.inverseMass);
body2.linearVelocity.z += (impulse.z * body2.inverseMass);
if (!body2IsMassPoint)
{
FP num0, num1, num2;
num0 = relativePos2.y * impulse.z - relativePos2.z * impulse.y;
num1 = relativePos2.z * impulse.x - relativePos2.x * impulse.z;
num2 = relativePos2.x * impulse.y - relativePos2.y * impulse.x;
FP num3 =
(((num0 * body2.invInertiaWorld.M11) +
(num1 * body2.invInertiaWorld.M21)) +
(num2 * body2.invInertiaWorld.M31));
FP num4 =
(((num0 * body2.invInertiaWorld.M12) +
(num1 * body2.invInertiaWorld.M22)) +
(num2 * body2.invInertiaWorld.M32));
FP num5 =
(((num0 * body2.invInertiaWorld.M13) +
(num1 * body2.invInertiaWorld.M23)) +
(num2 * body2.invInertiaWorld.M33));
body2.angularVelocity.x += num3;
body2.angularVelocity.y += num4;
body2.angularVelocity.z += num5;
}
}
lastTimeStep = timestep;
newContact = false;
}
/// <summary>
/// PrepareForIteration has to be called before <see cref="Iterate"/>.
/// </summary>
/// <param name="timestep">The timestep of the simulation.</param>
public void PrepareForIteration(FP timestep)
{
FPVector dv = CalculateRelativeVelocity();
FP kNormal = FP.Zero;
FPVector rantra = FPVector.zero;
if (!treatBody1AsStatic)
{
kNormal += body1.inverseMass;
if (!body1IsMassPoint)
{
FPVector.Cross(ref relativePos1, ref normal, out rantra);
FPVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FPVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
FPVector rbntrb = FPVector.zero;
if (!treatBody2AsStatic)
{
kNormal += body2.inverseMass;
if (!body2IsMassPoint)
{
FPVector.Cross(ref relativePos2, ref normal, out rbntrb);
FPVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
FPVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kNormal += FPVector.Dot(ref rantra, ref normal);
}
if (!treatBody2AsStatic)
{
kNormal += FPVector.Dot(ref rbntrb, ref normal);
}
massNormal = FP.One / kNormal;
tangent = dv - FPVector.Dot(dv, normal) * normal;
tangent.Normalize();
FP kTangent = FP.Zero;
if (treatBody1AsStatic)
{
rantra.MakeZero();
}
else
{
kTangent += body1.inverseMass;
if (!body1IsMassPoint)
{
FPVector.Cross(ref relativePos1, ref normal, out rantra);
FPVector.Transform(ref rantra, ref body1.invInertiaWorld, out rantra);
FPVector.Cross(ref rantra, ref relativePos1, out rantra);
}
}
if (treatBody2AsStatic)
{
rbntrb.MakeZero();
}
else
{
kTangent += body2.inverseMass;
if (!body2IsMassPoint)
{
FPVector.Cross(ref relativePos2, ref tangent, out rbntrb);
FPVector.Transform(ref rbntrb, ref body2.invInertiaWorld, out rbntrb);
FPVector.Cross(ref rbntrb, ref relativePos2, out rbntrb);
}
}
if (!treatBody1AsStatic)
{
kTangent += FPVector.Dot(ref rantra, ref tangent);
}
if (!treatBody2AsStatic)
{
kTangent += FPVector.Dot(ref rbntrb, ref tangent);
}
massTangent = FP.One / kTangent;
restitutionBias = lostSpeculativeBounce;
speculativeVelocity = FP.Zero;
FP relNormalVel = FPVector.Dot(ref normal, ref dv);
if (Penetration > settings.allowedPenetration)
{
restitutionBias = settings.bias * (FP.One / timestep) * FPMath.Max(FP.Zero, Penetration - settings.allowedPenetration);
restitutionBias = FPMath.Clamp(restitutionBias, FP.Zero, settings.maximumBias);
// body1IsMassPoint = body2IsMassPoint = false;
}
FP timeStepRatio = timestep / lastTimeStep;
accumulatedNormalImpulse *= timeStepRatio;
accumulatedTangentImpulse *= timeStepRatio;
{
// Static/Dynamic friction
FP relTangentVel = -FPVector.Dot(ref tangent, ref dv);
FP tangentImpulse = massTangent * relTangentVel;
FP maxTangentImpulse = -staticFriction * accumulatedNormalImpulse;
if (tangentImpulse < maxTangentImpulse)
{
friction = dynamicFriction;
}
else
{
friction = staticFriction;
}
}
FPVector impulse;
// Simultaneos solving and restitution is simply not possible
// so fake it a bit by just applying restitution impulse when there
// is a new contact.
if (relNormalVel < -FP.One && newContact)
{
restitutionBias = FPMath.Max(-restitution * relNormalVel, restitutionBias);
}
// Speculative Contacts!
// if the penetration is negative (which means the bodies are not already in contact, but they will
// be in the future) we store the current bounce bias in the variable 'lostSpeculativeBounce'
// and apply it the next frame, when the speculative contact was already solved.
if (penetration < -settings.allowedPenetration)
{
speculativeVelocity = penetration / timestep;
lostSpeculativeBounce = restitutionBias;
restitutionBias = FP.Zero;
}
else
{
lostSpeculativeBounce = FP.Zero;
}
impulse = normal * accumulatedNormalImpulse + tangent * accumulatedTangentImpulse;
ApplyImpulse(ref impulse);
lastTimeStep = timestep;
newContact = false;
}