public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector tSVector;
TSVector.Normalize(ref direction, out tSVector);
tSVector *= this.sphericalExpansion;
FP y = TSVector.Dot(ref this.vecs[0], ref direction);
int num = 0;
FP fP = TSVector.Dot(ref this.vecs[1], ref direction);
bool flag = fP > y;
if (flag)
{
y = fP;
num = 1;
}
fP = TSVector.Dot(ref this.vecs[2], ref direction);
bool flag2 = fP > y;
if (flag2)
{
num = 2;
}
result = this.vecs[num] + tSVector;
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector tSVector;
TSVector.Normalize(ref direction, out tSVector);
TSVector.Multiply(ref tSVector, this.sphericalExpansion, out tSVector);
int num = 0;
FP y = TSVector.Dot(ref this.points[0], ref direction);
FP fP = TSVector.Dot(ref this.points[1], ref direction);
bool flag = fP > y;
if (flag)
{
y = fP;
num = 1;
}
fP = TSVector.Dot(ref this.points[2], ref direction);
bool flag2 = fP > y;
if (flag2)
{
num = 2;
}
TSVector.Add(ref this.points[num], ref tSVector, out result);
}
public void SupportMapping(ref TSVector direction, out TSVector result)
{
FP y = TSVector.Dot(ref this.owner.points[this.indices.I0].position, ref direction);
FP fP = TSVector.Dot(ref this.owner.points[this.indices.I1].position, ref direction);
TSVector position = this.owner.points[this.indices.I0].position;
bool flag = fP > y;
if (flag)
{
y = fP;
position = this.owner.points[this.indices.I1].position;
}
fP = TSVector.Dot(ref this.owner.points[this.indices.I2].position, ref direction);
bool flag2 = fP > y;
if (flag2)
{
position = this.owner.points[this.indices.I2].position;
}
TSVector tSVector;
TSVector.Normalize(ref direction, out tSVector);
tSVector *= this.owner.triangleExpansion;
result = position + tSVector;
}
public void UpdatePosition()
{
bool flag = this.body1IsMassPoint;
if (flag)
{
TSVector.Add(ref this.realRelPos1, ref this.body1.position, out this.p1);
}
else
{
TSVector.Transform(ref this.realRelPos1, ref this.body1.orientation, out this.p1);
TSVector.Add(ref this.p1, ref this.body1.position, out this.p1);
}
bool flag2 = this.body2IsMassPoint;
if (flag2)
{
TSVector.Add(ref this.realRelPos2, ref this.body2.position, out this.p2);
}
else
{
TSVector.Transform(ref this.realRelPos2, ref this.body2.orientation, out this.p2);
TSVector.Add(ref this.p2, ref this.body2.position, out this.p2);
}
TSVector tSVector;
TSVector.Subtract(ref this.p1, ref this.p2, out tSVector);
this.penetration = TSVector.Dot(ref tSVector, ref this.normal);
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector w = TSVector.Cross(fromVector, toVector);
TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
public override void Iterate()
{
bool flag = this.skipConstraint;
if (!flag)
{
FP x = TSVector.Dot(ref this.body1.linearVelocity, ref this.jacobian[0]);
x += TSVector.Dot(ref this.body2.linearVelocity, ref this.jacobian[1]);
FP y = this.accumulatedImpulse * this.softnessOverDt;
FP fP = -this.effectiveMass * (x + this.bias + y);
bool flag2 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMinimumDistance;
if (flag2)
{
FP y2 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Max(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y2;
}
else
{
bool flag3 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMaximumDistance;
if (flag3)
{
FP y3 = this.accumulatedImpulse;
this.accumulatedImpulse = TSMath.Min(this.accumulatedImpulse + fP, 0);
fP = this.accumulatedImpulse - y3;
}
else
{
this.accumulatedImpulse += fP;
}
}
bool flag4 = !this.body1.isStatic;
if (flag4)
{
TSVector tSVector;
TSVector.Multiply(ref this.jacobian[0], fP * this.body1.inverseMass, out tSVector);
TSVector.Add(ref tSVector, ref this.body1.linearVelocity, out this.body1.linearVelocity);
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
TSVector tSVector;
TSVector.Multiply(ref this.jacobian[1], fP * this.body2.inverseMass, out tSVector);
TSVector.Add(ref tSVector, ref this.body2.linearVelocity, out this.body2.linearVelocity);
}
}
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
FP y = FP.MinValue;
int index = 0;
for (int i = 0; i < this.vertices.Count; i++)
{
FP fP = TSVector.Dot(this.vertices[i], direction);
bool flag = fP > y;
if (flag)
{
y = fP;
index = i;
}
}
result = this.vertices[index] - this.shifted;
}
public int PointOutsideOfPlane(TSVector p, TSVector a, TSVector b, TSVector c, TSVector d)
{
TSVector vector = TSVector.Cross(b - a, c - a);
FP x = TSVector.Dot(p - a, vector);
FP fP = TSVector.Dot(d - a, vector);
bool flag = fP * fP < FP.EN8;
int result;
if (flag)
{
result = -1;
}
else
{
result = ((x * fP < FP.Zero) ? 1 : 0);
}
return(result);
}
private static int FindExtremePoint(List <TSVector> points, ref TSVector dir)
{
int result = 0;
FP y = FP.MinValue;
for (int i = 1; i < points.Count; i++)
{
TSVector tSVector = points[i];
FP fP = TSVector.Dot(ref tSVector, ref dir);
bool flag = fP > y;
if (flag)
{
y = fP;
result = i;
}
}
return(result);
}
private void UpdateArbiterContacts(Arbiter arbiter)
{
bool flag = arbiter.contactList.Count == 0;
if (flag)
{
Stack <Arbiter> obj = this.removedArbiterStack;
lock (obj)
{
this.removedArbiterStack.Push(arbiter);
}
}
else
{
for (int i = arbiter.contactList.Count - 1; i >= 0; i--)
{
Contact contact = arbiter.contactList[i];
contact.UpdatePosition();
bool flag2 = contact.penetration < -this.contactSettings.breakThreshold;
if (flag2)
{
Contact.Pool.GiveBack(contact);
arbiter.contactList.RemoveAt(i);
}
else
{
TSVector value;
TSVector.Subtract(ref contact.p1, ref contact.p2, out value);
FP fP = TSVector.Dot(ref value, ref contact.normal);
value -= fP * contact.normal;
fP = value.sqrMagnitude;
bool flag3 = fP > this.contactSettings.breakThreshold * this.contactSettings.breakThreshold * 100;
if (flag3)
{
Contact.Pool.GiveBack(contact);
arbiter.contactList.RemoveAt(i);
}
}
}
}
}
private void FindSupportPoints(RigidBody body1, RigidBody body2, Shape shape1, Shape shape2, ref TSVector point, ref TSVector normal, out TSVector point1, out TSVector point2)
{
TSVector tSVector;
TSVector.Negate(ref normal, out tSVector);
TSVector tSVector2;
this.SupportMapping(body1, shape1, ref tSVector, out tSVector2);
TSVector tSVector3;
this.SupportMapping(body2, shape2, ref normal, out tSVector3);
TSVector.Subtract(ref tSVector2, ref point, out tSVector2);
TSVector.Subtract(ref tSVector3, ref point, out tSVector3);
FP scaleFactor = TSVector.Dot(ref tSVector2, ref normal);
FP scaleFactor2 = TSVector.Dot(ref tSVector3, ref normal);
TSVector.Multiply(ref normal, scaleFactor, out tSVector2);
TSVector.Multiply(ref normal, scaleFactor2, out tSVector3);
TSVector.Add(ref point, ref tSVector2, out point1);
TSVector.Add(ref point, ref tSVector3, out point2);
}
public bool ClosestPtPointTriangle(TSVector p, TSVector a, TSVector b, TSVector c, ref SubSimplexClosestResult result)
{
result.UsedVertices.Reset();
TSVector tSVector = b - a;
TSVector tSVector2 = c - a;
TSVector vector = p - a;
FP x = TSVector.Dot(tSVector, vector);
FP fP = TSVector.Dot(tSVector2, vector);
bool flag = x <= FP.Zero && fP <= FP.Zero;
bool result2;
if (flag)
{
result.ClosestPointOnSimplex = a;
result.UsedVertices.UsedVertexA = true;
result.SetBarycentricCoordinates(1, 0, 0, 0);
result2 = true;
}
else
{
TSVector vector2 = p - b;
FP fP2 = TSVector.Dot(tSVector, vector2);
FP fP3 = TSVector.Dot(tSVector2, vector2);
bool flag2 = fP2 >= FP.Zero && fP3 <= fP2;
if (flag2)
{
result.ClosestPointOnSimplex = b;
result.UsedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(0, 1, 0, 0);
result2 = true;
}
else
{
FP fP4 = x * fP3 - fP2 * fP;
bool flag3 = fP4 <= FP.Zero && x >= FP.Zero && fP2 <= FP.Zero;
if (flag3)
{
FP fP5 = x / (x - fP2);
result.ClosestPointOnSimplex = a + fP5 * tSVector;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexB = true;
result.SetBarycentricCoordinates(1 - fP5, fP5, 0, 0);
result2 = true;
}
else
{
TSVector vector3 = p - c;
FP x2 = TSVector.Dot(tSVector, vector3);
FP fP6 = TSVector.Dot(tSVector2, vector3);
bool flag4 = fP6 >= FP.Zero && x2 <= fP6;
if (flag4)
{
result.ClosestPointOnSimplex = c;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 0, 1, 0);
result2 = true;
}
else
{
FP fP7 = x2 * fP - x * fP6;
bool flag5 = fP7 <= FP.Zero && fP >= FP.Zero && fP6 <= FP.Zero;
if (flag5)
{
FP fP8 = fP / (fP - fP6);
result.ClosestPointOnSimplex = a + fP8 * tSVector2;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(1 - fP8, 0, fP8, 0);
result2 = true;
}
else
{
FP x3 = fP2 * fP6 - x2 * fP3;
bool flag6 = x3 <= FP.Zero && fP3 - fP2 >= FP.Zero && x2 - fP6 >= FP.Zero;
if (flag6)
{
FP fP8 = (fP3 - fP2) / (fP3 - fP2 + (x2 - fP6));
result.ClosestPointOnSimplex = b + fP8 * (c - b);
result.UsedVertices.UsedVertexB = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(0, 1 - fP8, fP8, 0);
result2 = true;
}
else
{
FP y = FP.One / (x3 + fP7 + fP4);
FP fP5 = fP7 * y;
FP fP8 = fP4 * y;
result.ClosestPointOnSimplex = a + tSVector * fP5 + tSVector2 * fP8;
result.UsedVertices.UsedVertexA = true;
result.UsedVertices.UsedVertexB = true;
result.UsedVertices.UsedVertexC = true;
result.SetBarycentricCoordinates(1 - fP5 - fP8, fP5, fP8, 0);
result2 = true;
}
}
}
}
}
}
return(result2);
}
public bool UpdateClosestVectorAndPoints()
{
bool needsUpdate = this._needsUpdate;
if (needsUpdate)
{
this._cachedBC.Reset();
this._needsUpdate = false;
switch (this.NumVertices)
{
case 0:
this._cachedValidClosest = false;
break;
case 1:
this._cachedPA = this._simplexPointsP[0];
this._cachedPB = this._simplexPointsQ[0];
this._cachedV = this._cachedPA - this._cachedPB;
this._cachedBC.Reset();
this._cachedBC.SetBarycentricCoordinates(1f, FP.Zero, FP.Zero, FP.Zero);
this._cachedValidClosest = this._cachedBC.IsValid;
break;
case 2:
{
TSVector tSVector = this._simplexVectorW[0];
TSVector value = this._simplexVectorW[1];
TSVector tSVector2 = tSVector * -1;
TSVector tSVector3 = value - tSVector;
FP fP = TSVector.Dot(tSVector3, tSVector2);
bool flag = fP > 0;
if (flag)
{
FP sqrMagnitude = tSVector3.sqrMagnitude;
bool flag2 = fP < sqrMagnitude;
if (flag2)
{
fP /= sqrMagnitude;
tSVector2 -= fP * tSVector3;
this._cachedBC.UsedVertices.UsedVertexA = true;
this._cachedBC.UsedVertices.UsedVertexB = true;
}
else
{
fP = 1;
tSVector2 -= tSVector3;
this._cachedBC.UsedVertices.UsedVertexB = true;
}
}
else
{
fP = 0;
this._cachedBC.UsedVertices.UsedVertexA = true;
}
this._cachedBC.SetBarycentricCoordinates(1 - fP, fP, 0, 0);
this._cachedPA = this._simplexPointsP[0] + fP * (this._simplexPointsP[1] - this._simplexPointsP[0]);
this._cachedPB = this._simplexPointsQ[0] + fP * (this._simplexPointsQ[1] - this._simplexPointsQ[0]);
this._cachedV = this._cachedPA - this._cachedPB;
this.ReduceVertices(this._cachedBC.UsedVertices);
this._cachedValidClosest = this._cachedBC.IsValid;
break;
}
case 3:
{
TSVector p = default(TSVector);
TSVector a = this._simplexVectorW[0];
TSVector b = this._simplexVectorW[1];
TSVector c = this._simplexVectorW[2];
this.ClosestPtPointTriangle(p, a, b, c, ref this._cachedBC);
this._cachedPA = this._simplexPointsP[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsP[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsP[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsP[3] * this._cachedBC.BarycentricCoords[3];
this._cachedPB = this._simplexPointsQ[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsQ[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsQ[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsQ[3] * this._cachedBC.BarycentricCoords[3];
this._cachedV = this._cachedPA - this._cachedPB;
this.ReduceVertices(this._cachedBC.UsedVertices);
this._cachedValidClosest = this._cachedBC.IsValid;
break;
}
case 4:
{
TSVector p = default(TSVector);
TSVector a = this._simplexVectorW[0];
TSVector b = this._simplexVectorW[1];
TSVector c = this._simplexVectorW[2];
TSVector d = this._simplexVectorW[3];
bool flag3 = this.ClosestPtPointTetrahedron(p, a, b, c, d, ref this._cachedBC);
bool flag4 = flag3;
if (flag4)
{
this._cachedPA = this._simplexPointsP[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsP[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsP[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsP[3] * this._cachedBC.BarycentricCoords[3];
this._cachedPB = this._simplexPointsQ[0] * this._cachedBC.BarycentricCoords[0] + this._simplexPointsQ[1] * this._cachedBC.BarycentricCoords[1] + this._simplexPointsQ[2] * this._cachedBC.BarycentricCoords[2] + this._simplexPointsQ[3] * this._cachedBC.BarycentricCoords[3];
this._cachedV = this._cachedPA - this._cachedPB;
this.ReduceVertices(this._cachedBC.UsedVertices);
this._cachedValidClosest = this._cachedBC.IsValid;
}
else
{
bool degenerate = this._cachedBC.Degenerate;
if (degenerate)
{
this._cachedValidClosest = false;
}
else
{
this._cachedValidClosest = true;
this._cachedV.x = (this._cachedV.y = (this._cachedV.z = FP.Zero));
}
}
break;
}
default:
this._cachedValidClosest = false;
break;
}
}
return(this._cachedValidClosest);
}
/// <summary>
/// Calculates the dot product of two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>Returns the dot product of both.</returns>
#region public static FP operator *(JVector value1, JVector value2)
public static FP operator *(TSVector value1, TSVector value2)
{
return(TSVector.Dot(ref value1, ref value2));
}
/// <summary>
/// Calculates the dot product of two vectors.
/// </summary>
/// <param name="vector1">The first vector.</param>
/// <param name="vector2">The second vector.</param>
/// <returns>Returns the dot product of both vectors.</returns>
#region public static FP Dot(JVector vector1, JVector vector2)
public static FP Dot(TSVector vector1, TSVector vector2)
{
return(TSVector.Dot(ref vector1, ref vector2));
}
public static bool Pointcast(ISupportMappable support, ref TSMatrix orientation, ref TSVector position, ref TSVector point)
{
TSVector tSVector;
GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref point, out tSVector);
TSVector.Subtract(ref point, ref tSVector, out tSVector);
TSVector tSVector2;
support.SupportCenter(out tSVector2);
TSVector.Transform(ref tSVector2, ref orientation, out tSVector2);
TSVector.Add(ref position, ref tSVector2, out tSVector2);
TSVector.Subtract(ref point, ref tSVector2, out tSVector2);
TSVector p = point;
TSVector tSVector3;
TSVector.Subtract(ref p, ref tSVector, out tSVector3);
FP x = tSVector3.sqrMagnitude;
FP eN = FP.EN4;
int num = 15;
VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew();
@new.Reset();
bool result;
while (x > eN && num-- != 0)
{
TSVector q;
GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref tSVector3, out q);
TSVector w;
TSVector.Subtract(ref p, ref q, out w);
FP x2 = TSVector.Dot(ref tSVector3, ref w);
bool flag = x2 > FP.Zero;
if (flag)
{
FP x3 = TSVector.Dot(ref tSVector3, ref tSVector2);
bool flag2 = x3 >= -(TSMath.Epsilon * TSMath.Epsilon);
if (flag2)
{
GJKCollide.simplexSolverPool.GiveBack(@new);
result = false;
return(result);
}
@new.Reset();
}
bool flag3 = [email protected](w);
if (flag3)
{
@new.AddVertex(w, p, q);
}
bool flag4 = @new.Closest(out tSVector3);
if (flag4)
{
x = tSVector3.sqrMagnitude;
}
else
{
x = FP.Zero;
}
}
GJKCollide.simplexSolverPool.GiveBack(@new);
result = true;
return(result);
}
public static bool Raycast(ISupportMappable support, ref TSMatrix orientation, ref TSMatrix invOrientation, ref TSVector position, ref TSVector origin, ref TSVector direction, out FP fraction, out TSVector normal)
{
VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew();
@new.Reset();
normal = TSVector.zero;
fraction = FP.MaxValue;
FP fP = FP.Zero;
TSVector tSVector = direction;
TSVector p = origin;
TSVector tSVector2;
GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref tSVector, out tSVector2);
TSVector tSVector3;
TSVector.Subtract(ref p, ref tSVector2, out tSVector3);
int num = 15;
FP x = tSVector3.sqrMagnitude;
FP eN = FP.EN6;
bool result;
while (x > eN && num-- != 0)
{
TSVector q;
GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref tSVector3, out q);
TSVector w;
TSVector.Subtract(ref p, ref q, out w);
FP x2 = TSVector.Dot(ref tSVector3, ref w);
bool flag = x2 > FP.Zero;
if (flag)
{
FP fP2 = TSVector.Dot(ref tSVector3, ref tSVector);
bool flag2 = fP2 >= -TSMath.Epsilon;
if (flag2)
{
GJKCollide.simplexSolverPool.GiveBack(@new);
result = false;
return(result);
}
fP -= x2 / fP2;
TSVector.Multiply(ref tSVector, fP, out p);
TSVector.Add(ref origin, ref p, out p);
TSVector.Subtract(ref p, ref q, out w);
normal = tSVector3;
}
bool flag3 = [email protected](w);
if (flag3)
{
@new.AddVertex(w, p, q);
}
bool flag4 = @new.Closest(out tSVector3);
if (flag4)
{
x = tSVector3.sqrMagnitude;
}
else
{
x = FP.Zero;
}
}
TSVector tSVector4;
TSVector value;
@new.ComputePoints(out tSVector4, out value);
value -= origin;
fraction = value.magnitude / direction.magnitude;
bool flag5 = normal.sqrMagnitude > TSMath.Epsilon * TSMath.Epsilon;
if (flag5)
{
normal.Normalize();
}
GJKCollide.simplexSolverPool.GiveBack(@new);
result = true;
return(result);
}
public static bool Detect(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector point, out TSVector normal, out FP penetration)
{
TSVector zero = TSVector.zero;
TSVector zero2 = TSVector.zero;
TSVector zero3 = TSVector.zero;
point = (normal = TSVector.zero);
penetration = FP.Zero;
TSVector tSVector;
support1.SupportCenter(out tSVector);
TSVector.Transform(ref tSVector, ref orientation1, out tSVector);
TSVector.Add(ref position1, ref tSVector, out tSVector);
TSVector tSVector2;
support2.SupportCenter(out tSVector2);
TSVector.Transform(ref tSVector2, ref orientation2, out tSVector2);
TSVector.Add(ref position2, ref tSVector2, out tSVector2);
TSVector tSVector3;
TSVector.Subtract(ref tSVector2, ref tSVector, out tSVector3);
bool flag = tSVector3.IsNearlyZero();
if (flag)
{
tSVector3 = new TSVector(FP.EN4, 0, 0);
}
TSVector tSVector4 = tSVector3;
TSVector.Negate(ref tSVector3, out normal);
TSVector tSVector5;
XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector5);
TSVector tSVector6;
XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector6);
TSVector tSVector7;
TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector7);
bool flag2 = TSVector.Dot(ref tSVector7, ref normal) <= FP.Zero;
bool result;
if (flag2)
{
result = false;
}
else
{
TSVector.Cross(ref tSVector7, ref tSVector3, out normal);
bool flag3 = normal.IsNearlyZero();
if (flag3)
{
TSVector.Subtract(ref tSVector7, ref tSVector3, out normal);
normal.Normalize();
point = tSVector5;
TSVector.Add(ref point, ref tSVector6, out point);
TSVector.Multiply(ref point, FP.Half, out point);
TSVector tSVector8;
TSVector.Subtract(ref tSVector6, ref tSVector5, out tSVector8);
penetration = TSVector.Dot(ref tSVector8, ref normal);
result = true;
}
else
{
TSVector.Negate(ref normal, out tSVector4);
TSVector tSVector9;
XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector9);
TSVector tSVector10;
XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector10);
TSVector tSVector11;
TSVector.Subtract(ref tSVector10, ref tSVector9, out tSVector11);
bool flag4 = TSVector.Dot(ref tSVector11, ref normal) <= FP.Zero;
if (flag4)
{
result = false;
}
else
{
TSVector tSVector8;
TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8);
TSVector tSVector12;
TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12);
TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
FP x = TSVector.Dot(ref normal, ref tSVector3);
bool flag5 = x > FP.Zero;
if (flag5)
{
TSVector.Swap(ref tSVector7, ref tSVector11);
TSVector.Swap(ref tSVector5, ref tSVector9);
TSVector.Swap(ref tSVector6, ref tSVector10);
TSVector.Negate(ref normal, out normal);
Debug.Log("normal: " + normal);
}
int num = 0;
int num2 = 0;
bool flag6 = false;
while (true)
{
bool flag7 = num2 > 34;
if (flag7)
{
break;
}
num2++;
TSVector.Negate(ref normal, out tSVector4);
TSVector tSVector13;
XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out tSVector13);
TSVector tSVector14;
XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out tSVector14);
TSVector tSVector15;
TSVector.Subtract(ref tSVector14, ref tSVector13, out tSVector15);
bool flag8 = TSVector.Dot(ref tSVector15, ref normal) <= FP.Zero;
if (flag8)
{
goto Block_7;
}
TSVector.Cross(ref tSVector7, ref tSVector15, out tSVector8);
bool flag9 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero;
if (flag9)
{
tSVector11 = tSVector15;
tSVector9 = tSVector13;
tSVector10 = tSVector14;
TSVector.Subtract(ref tSVector7, ref tSVector3, out tSVector8);
TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector12);
TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
}
else
{
TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8);
bool flag10 = TSVector.Dot(ref tSVector8, ref tSVector3) < FP.Zero;
if (!flag10)
{
goto IL_385;
}
tSVector7 = tSVector15;
tSVector5 = tSVector13;
tSVector6 = tSVector14;
TSVector.Subtract(ref tSVector15, ref tSVector3, out tSVector8);
TSVector.Subtract(ref tSVector11, ref tSVector3, out tSVector12);
TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
}
}
result = false;
return(result);
Block_7:
result = false;
return(result);
IL_385:
while (true)
{
num++;
TSVector.Subtract(ref tSVector11, ref tSVector7, out tSVector8);
TSVector tSVector15;
TSVector.Subtract(ref tSVector15, ref tSVector7, out tSVector12);
TSVector.Cross(ref tSVector8, ref tSVector12, out normal);
bool flag11 = normal.IsNearlyZero();
if (flag11)
{
break;
}
normal.Normalize();
FP x2 = TSVector.Dot(ref normal, ref tSVector7);
bool flag12 = x2 >= 0 && !flag6;
if (flag12)
{
flag6 = true;
}
TSVector.Negate(ref normal, out tSVector4);
XenoCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector4, out zero);
XenoCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out zero2);
TSVector.Subtract(ref zero2, ref zero, out zero3);
TSVector.Subtract(ref zero3, ref tSVector15, out tSVector8);
FP x3 = TSVector.Dot(ref tSVector8, ref normal);
penetration = TSVector.Dot(ref zero3, ref normal);
bool flag13 = x3 <= XenoCollide.CollideEpsilon || penetration <= FP.Zero || num > 34;
if (flag13)
{
goto Block_15;
}
TSVector.Cross(ref zero3, ref tSVector3, out tSVector8);
FP x4 = TSVector.Dot(ref tSVector8, ref tSVector7);
bool flag14 = x4 >= FP.Zero;
if (flag14)
{
x4 = TSVector.Dot(ref tSVector8, ref tSVector11);
bool flag15 = x4 >= FP.Zero;
if (flag15)
{
tSVector7 = zero3;
tSVector5 = zero;
tSVector6 = zero2;
}
else
{
tSVector15 = zero3;
TSVector tSVector13 = zero;
TSVector tSVector14 = zero2;
}
}
else
{
x4 = TSVector.Dot(ref tSVector8, ref tSVector15);
bool flag16 = x4 >= FP.Zero;
if (flag16)
{
tSVector11 = zero3;
tSVector9 = zero;
tSVector10 = zero2;
}
else
{
tSVector7 = zero3;
tSVector5 = zero;
tSVector6 = zero2;
}
}
}
result = true;
return(result);
Block_15:
bool flag17 = flag6;
if (flag17)
{
TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8);
TSVector tSVector15;
FP fP = TSVector.Dot(ref tSVector8, ref tSVector15);
TSVector.Cross(ref tSVector15, ref tSVector11, out tSVector8);
FP fP2 = TSVector.Dot(ref tSVector8, ref tSVector3);
TSVector.Cross(ref tSVector3, ref tSVector7, out tSVector8);
FP fP3 = TSVector.Dot(ref tSVector8, ref tSVector15);
TSVector.Cross(ref tSVector11, ref tSVector7, out tSVector8);
FP fP4 = TSVector.Dot(ref tSVector8, ref tSVector3);
FP fP5 = fP + fP2 + fP3 + fP4;
bool flag18 = fP5 <= 0;
if (flag18)
{
fP = 0;
TSVector.Cross(ref tSVector11, ref tSVector15, out tSVector8);
fP2 = TSVector.Dot(ref tSVector8, ref normal);
TSVector.Cross(ref tSVector15, ref tSVector7, out tSVector8);
fP3 = TSVector.Dot(ref tSVector8, ref normal);
TSVector.Cross(ref tSVector7, ref tSVector11, out tSVector8);
fP4 = TSVector.Dot(ref tSVector8, ref normal);
fP5 = fP2 + fP3 + fP4;
}
FP x5 = FP.One / fP5;
TSVector.Multiply(ref tSVector, fP, out point);
TSVector.Multiply(ref tSVector5, fP2, out tSVector8);
TSVector.Add(ref point, ref tSVector8, out point);
TSVector.Multiply(ref tSVector9, fP3, out tSVector8);
TSVector.Add(ref point, ref tSVector8, out point);
TSVector tSVector13;
TSVector.Multiply(ref tSVector13, fP4, out tSVector8);
TSVector.Add(ref point, ref tSVector8, out point);
TSVector.Multiply(ref tSVector2, fP, out tSVector12);
TSVector.Add(ref tSVector12, ref point, out point);
TSVector.Multiply(ref tSVector6, fP2, out tSVector8);
TSVector.Add(ref point, ref tSVector8, out point);
TSVector.Multiply(ref tSVector10, fP3, out tSVector8);
TSVector.Add(ref point, ref tSVector8, out point);
TSVector tSVector14;
TSVector.Multiply(ref tSVector14, fP4, out tSVector8);
TSVector.Add(ref point, ref tSVector8, out point);
TSVector.Multiply(ref point, x5 * FP.Half, out point);
}
result = flag6;
}
}
}
return(result);
}
public void PrepareForIteration(FP timestep)
{
FP fP = this.body2.angularVelocity.y * this.relativePos2.z - this.body2.angularVelocity.z * this.relativePos2.y + this.body2.linearVelocity.x;
FP fP2 = this.body2.angularVelocity.z * this.relativePos2.x - this.body2.angularVelocity.x * this.relativePos2.z + this.body2.linearVelocity.y;
FP fP3 = this.body2.angularVelocity.x * this.relativePos2.y - this.body2.angularVelocity.y * this.relativePos2.x + this.body2.linearVelocity.z;
fP = fP - this.body1.angularVelocity.y * this.relativePos1.z + this.body1.angularVelocity.z * this.relativePos1.y - this.body1.linearVelocity.x;
fP2 = fP2 - this.body1.angularVelocity.z * this.relativePos1.x + this.body1.angularVelocity.x * this.relativePos1.z - this.body1.linearVelocity.y;
fP3 = fP3 - this.body1.angularVelocity.x * this.relativePos1.y + this.body1.angularVelocity.y * this.relativePos1.x - this.body1.linearVelocity.z;
FP fP4 = FP.Zero;
TSVector zero = TSVector.zero;
bool flag = !this.treatBody1AsStatic;
if (flag)
{
fP4 += this.body1.inverseMass;
bool flag2 = !this.body1IsMassPoint;
if (flag2)
{
zero.x = this.relativePos1.y * this.normal.z - this.relativePos1.z * this.normal.y;
zero.y = this.relativePos1.z * this.normal.x - this.relativePos1.x * this.normal.z;
zero.z = this.relativePos1.x * this.normal.y - this.relativePos1.y * this.normal.x;
FP x = zero.x * this.body1.invInertiaWorld.M11 + zero.y * this.body1.invInertiaWorld.M21 + zero.z * this.body1.invInertiaWorld.M31;
FP y = zero.x * this.body1.invInertiaWorld.M12 + zero.y * this.body1.invInertiaWorld.M22 + zero.z * this.body1.invInertiaWorld.M32;
FP z = zero.x * this.body1.invInertiaWorld.M13 + zero.y * this.body1.invInertiaWorld.M23 + zero.z * this.body1.invInertiaWorld.M33;
zero.x = x;
zero.y = y;
zero.z = z;
x = zero.y * this.relativePos1.z - zero.z * this.relativePos1.y;
y = zero.z * this.relativePos1.x - zero.x * this.relativePos1.z;
z = zero.x * this.relativePos1.y - zero.y * this.relativePos1.x;
zero.x = x;
zero.y = y;
zero.z = z;
}
}
TSVector zero2 = TSVector.zero;
bool flag3 = !this.treatBody2AsStatic;
if (flag3)
{
fP4 += this.body2.inverseMass;
bool flag4 = !this.body2IsMassPoint;
if (flag4)
{
zero2.x = this.relativePos2.y * this.normal.z - this.relativePos2.z * this.normal.y;
zero2.y = this.relativePos2.z * this.normal.x - this.relativePos2.x * this.normal.z;
zero2.z = this.relativePos2.x * this.normal.y - this.relativePos2.y * this.normal.x;
FP x2 = zero2.x * this.body2.invInertiaWorld.M11 + zero2.y * this.body2.invInertiaWorld.M21 + zero2.z * this.body2.invInertiaWorld.M31;
FP y2 = zero2.x * this.body2.invInertiaWorld.M12 + zero2.y * this.body2.invInertiaWorld.M22 + zero2.z * this.body2.invInertiaWorld.M32;
FP z2 = zero2.x * this.body2.invInertiaWorld.M13 + zero2.y * this.body2.invInertiaWorld.M23 + zero2.z * this.body2.invInertiaWorld.M33;
zero2.x = x2;
zero2.y = y2;
zero2.z = z2;
x2 = zero2.y * this.relativePos2.z - zero2.z * this.relativePos2.y;
y2 = zero2.z * this.relativePos2.x - zero2.x * this.relativePos2.z;
z2 = zero2.x * this.relativePos2.y - zero2.y * this.relativePos2.x;
zero2.x = x2;
zero2.y = y2;
zero2.z = z2;
}
}
bool flag5 = !this.treatBody1AsStatic;
if (flag5)
{
fP4 += zero.x * this.normal.x + zero.y * this.normal.y + zero.z * this.normal.z;
}
bool flag6 = !this.treatBody2AsStatic;
if (flag6)
{
fP4 += zero2.x * this.normal.x + zero2.y * this.normal.y + zero2.z * this.normal.z;
}
this.massNormal = FP.One / fP4;
FP fP5 = fP * this.normal.x + fP2 * this.normal.y + fP3 * this.normal.z;
this.tangent.x = fP - this.normal.x * fP5;
this.tangent.y = fP2 - this.normal.y * fP5;
this.tangent.z = fP3 - this.normal.z * fP5;
fP5 = this.tangent.x * this.tangent.x + this.tangent.y * this.tangent.y + this.tangent.z * this.tangent.z;
bool flag7 = fP5 != FP.Zero;
if (flag7)
{
fP5 = FP.Sqrt(fP5);
this.tangent.x = this.tangent.x / fP5;
this.tangent.y = this.tangent.y / fP5;
this.tangent.z = this.tangent.z / fP5;
}
FP fP6 = FP.Zero;
bool flag8 = this.treatBody1AsStatic;
if (flag8)
{
zero.MakeZero();
}
else
{
fP6 += this.body1.inverseMass;
bool flag9 = !this.body1IsMassPoint;
if (flag9)
{
zero.x = this.relativePos1.y * this.tangent.z - this.relativePos1.z * this.tangent.y;
zero.y = this.relativePos1.z * this.tangent.x - this.relativePos1.x * this.tangent.z;
zero.z = this.relativePos1.x * this.tangent.y - this.relativePos1.y * this.tangent.x;
FP x3 = zero.x * this.body1.invInertiaWorld.M11 + zero.y * this.body1.invInertiaWorld.M21 + zero.z * this.body1.invInertiaWorld.M31;
FP y3 = zero.x * this.body1.invInertiaWorld.M12 + zero.y * this.body1.invInertiaWorld.M22 + zero.z * this.body1.invInertiaWorld.M32;
FP z3 = zero.x * this.body1.invInertiaWorld.M13 + zero.y * this.body1.invInertiaWorld.M23 + zero.z * this.body1.invInertiaWorld.M33;
zero.x = x3;
zero.y = y3;
zero.z = z3;
x3 = zero.y * this.relativePos1.z - zero.z * this.relativePos1.y;
y3 = zero.z * this.relativePos1.x - zero.x * this.relativePos1.z;
z3 = zero.x * this.relativePos1.y - zero.y * this.relativePos1.x;
zero.x = x3;
zero.y = y3;
zero.z = z3;
}
}
bool flag10 = this.treatBody2AsStatic;
if (flag10)
{
zero2.MakeZero();
}
else
{
fP6 += this.body2.inverseMass;
bool flag11 = !this.body2IsMassPoint;
if (flag11)
{
zero2.x = this.relativePos2.y * this.tangent.z - this.relativePos2.z * this.tangent.y;
zero2.y = this.relativePos2.z * this.tangent.x - this.relativePos2.x * this.tangent.z;
zero2.z = this.relativePos2.x * this.tangent.y - this.relativePos2.y * this.tangent.x;
FP x4 = zero2.x * this.body2.invInertiaWorld.M11 + zero2.y * this.body2.invInertiaWorld.M21 + zero2.z * this.body2.invInertiaWorld.M31;
FP y4 = zero2.x * this.body2.invInertiaWorld.M12 + zero2.y * this.body2.invInertiaWorld.M22 + zero2.z * this.body2.invInertiaWorld.M32;
FP z4 = zero2.x * this.body2.invInertiaWorld.M13 + zero2.y * this.body2.invInertiaWorld.M23 + zero2.z * this.body2.invInertiaWorld.M33;
zero2.x = x4;
zero2.y = y4;
zero2.z = z4;
x4 = zero2.y * this.relativePos2.z - zero2.z * this.relativePos2.y;
y4 = zero2.z * this.relativePos2.x - zero2.x * this.relativePos2.z;
z4 = zero2.x * this.relativePos2.y - zero2.y * this.relativePos2.x;
zero2.x = x4;
zero2.y = y4;
zero2.z = z4;
}
}
bool flag12 = !this.treatBody1AsStatic;
if (flag12)
{
fP6 += TSVector.Dot(ref zero, ref this.tangent);
}
bool flag13 = !this.treatBody2AsStatic;
if (flag13)
{
fP6 += TSVector.Dot(ref zero2, ref this.tangent);
}
this.massTangent = FP.One / fP6;
this.restitutionBias = this.lostSpeculativeBounce;
this.speculativeVelocity = FP.Zero;
FP y5 = this.normal.x * fP + this.normal.y * fP2 + this.normal.z * fP3;
bool flag14 = this.Penetration > this.settings.allowedPenetration;
if (flag14)
{
this.restitutionBias = this.settings.bias * (FP.One / timestep) * TSMath.Max(FP.Zero, this.Penetration - this.settings.allowedPenetration);
this.restitutionBias = TSMath.Clamp(this.restitutionBias, FP.Zero, this.settings.maximumBias);
}
FP y6 = timestep / this.lastTimeStep;
this.accumulatedNormalImpulse *= y6;
this.accumulatedTangentImpulse *= y6;
FP y7 = -(this.tangent.x * fP + this.tangent.y * fP2 + this.tangent.z * fP3);
FP x5 = this.massTangent * y7;
FP y8 = -this.staticFriction * this.accumulatedNormalImpulse;
bool flag15 = x5 < y8;
if (flag15)
{
this.friction = this.dynamicFriction;
}
else
{
this.friction = this.staticFriction;
}
this.restitutionBias = TSMath.Max(-this.restitution * y5, this.restitutionBias);
bool flag16 = this.penetration < -this.settings.allowedPenetration;
if (flag16)
{
this.speculativeVelocity = this.penetration / timestep;
this.lostSpeculativeBounce = this.restitutionBias;
this.restitutionBias = FP.Zero;
}
else
{
this.lostSpeculativeBounce = FP.Zero;
}
TSVector tSVector;
tSVector.x = this.normal.x * this.accumulatedNormalImpulse + this.tangent.x * this.accumulatedTangentImpulse;
tSVector.y = this.normal.y * this.accumulatedNormalImpulse + this.tangent.y * this.accumulatedTangentImpulse;
tSVector.z = this.normal.z * this.accumulatedNormalImpulse + this.tangent.z * this.accumulatedTangentImpulse;
bool flag17 = !this.treatBody1AsStatic;
if (flag17)
{
RigidBody expr_13D4_cp_0_cp_0 = this.body1;
expr_13D4_cp_0_cp_0.linearVelocity.x = expr_13D4_cp_0_cp_0.linearVelocity.x - tSVector.x * this.body1.inverseMass;
RigidBody expr_140B_cp_0_cp_0 = this.body1;
expr_140B_cp_0_cp_0.linearVelocity.y = expr_140B_cp_0_cp_0.linearVelocity.y - tSVector.y * this.body1.inverseMass;
RigidBody expr_1442_cp_0_cp_0 = this.body1;
expr_1442_cp_0_cp_0.linearVelocity.z = expr_1442_cp_0_cp_0.linearVelocity.z - tSVector.z * this.body1.inverseMass;
bool flag18 = !this.body1IsMassPoint;
if (flag18)
{
FP x6 = this.relativePos1.y * tSVector.z - this.relativePos1.z * tSVector.y;
FP x7 = this.relativePos1.z * tSVector.x - this.relativePos1.x * tSVector.z;
FP x8 = this.relativePos1.x * tSVector.y - this.relativePos1.y * tSVector.x;
FP y9 = x6 * this.body1.invInertiaWorld.M11 + x7 * this.body1.invInertiaWorld.M21 + x8 * this.body1.invInertiaWorld.M31;
FP y10 = x6 * this.body1.invInertiaWorld.M12 + x7 * this.body1.invInertiaWorld.M22 + x8 * this.body1.invInertiaWorld.M32;
FP y11 = x6 * this.body1.invInertiaWorld.M13 + x7 * this.body1.invInertiaWorld.M23 + x8 * this.body1.invInertiaWorld.M33;
RigidBody expr_161E_cp_0_cp_0 = this.body1;
expr_161E_cp_0_cp_0.angularVelocity.x = expr_161E_cp_0_cp_0.angularVelocity.x - y9;
RigidBody expr_1640_cp_0_cp_0 = this.body1;
expr_1640_cp_0_cp_0.angularVelocity.y = expr_1640_cp_0_cp_0.angularVelocity.y - y10;
RigidBody expr_1662_cp_0_cp_0 = this.body1;
expr_1662_cp_0_cp_0.angularVelocity.z = expr_1662_cp_0_cp_0.angularVelocity.z - y11;
}
}
bool flag19 = !this.treatBody2AsStatic;
if (flag19)
{
RigidBody expr_1699_cp_0_cp_0 = this.body2;
expr_1699_cp_0_cp_0.linearVelocity.x = expr_1699_cp_0_cp_0.linearVelocity.x + tSVector.x * this.body2.inverseMass;
RigidBody expr_16D0_cp_0_cp_0 = this.body2;
expr_16D0_cp_0_cp_0.linearVelocity.y = expr_16D0_cp_0_cp_0.linearVelocity.y + tSVector.y * this.body2.inverseMass;
RigidBody expr_1707_cp_0_cp_0 = this.body2;
expr_1707_cp_0_cp_0.linearVelocity.z = expr_1707_cp_0_cp_0.linearVelocity.z + tSVector.z * this.body2.inverseMass;
bool flag20 = !this.body2IsMassPoint;
if (flag20)
{
FP x9 = this.relativePos2.y * tSVector.z - this.relativePos2.z * tSVector.y;
FP x10 = this.relativePos2.z * tSVector.x - this.relativePos2.x * tSVector.z;
FP x11 = this.relativePos2.x * tSVector.y - this.relativePos2.y * tSVector.x;
FP y12 = x9 * this.body2.invInertiaWorld.M11 + x10 * this.body2.invInertiaWorld.M21 + x11 * this.body2.invInertiaWorld.M31;
FP y13 = x9 * this.body2.invInertiaWorld.M12 + x10 * this.body2.invInertiaWorld.M22 + x11 * this.body2.invInertiaWorld.M32;
FP y14 = x9 * this.body2.invInertiaWorld.M13 + x10 * this.body2.invInertiaWorld.M23 + x11 * this.body2.invInertiaWorld.M33;
RigidBody expr_18E3_cp_0_cp_0 = this.body2;
expr_18E3_cp_0_cp_0.angularVelocity.x = expr_18E3_cp_0_cp_0.angularVelocity.x + y12;
RigidBody expr_1905_cp_0_cp_0 = this.body2;
expr_1905_cp_0_cp_0.angularVelocity.y = expr_1905_cp_0_cp_0.angularVelocity.y + y13;
RigidBody expr_1927_cp_0_cp_0 = this.body2;
expr_1927_cp_0_cp_0.angularVelocity.z = expr_1927_cp_0_cp_0.angularVelocity.z + y14;
}
}
this.lastTimeStep = timestep;
this.newContact = false;
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector tSVector = TSVector.Cross(fromVector, toVector);
TSQuaternion result = new TSQuaternion(tSVector.x, tSVector.y, tSVector.z, TSVector.Dot(fromVector, toVector));
result.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
result.Normalize();
return(result);
}
