public static bool ClosestPoints(ISupportMappable support1, ISupportMappable support2, ref TSMatrix orientation1, ref TSMatrix orientation2, ref TSVector position1, ref TSVector position2, out TSVector p1, out TSVector p2, out TSVector normal)
{
VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew();
@new.Reset();
p1 = (p2 = TSVector.zero);
TSVector value = position1 - position2;
TSVector tSVector = TSVector.Negate(value);
TSVector tSVector2;
GJKCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector, out tSVector2);
TSVector tSVector3;
GJKCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref value, out tSVector3);
TSVector tSVector4 = tSVector2 - tSVector3;
normal = TSVector.zero;
int num = 15;
FP x = tSVector4.sqrMagnitude;
FP eN = FP.EN5;
while (x > eN && num-- != 0)
{
TSVector tSVector5 = TSVector.Negate(tSVector4);
GJKCollide.SupportMapTransformed(support1, ref orientation1, ref position1, ref tSVector5, out tSVector2);
GJKCollide.SupportMapTransformed(support2, ref orientation2, ref position2, ref tSVector4, out tSVector3);
TSVector w = tSVector2 - tSVector3;
bool flag = [email protected](w);
if (flag)
{
@new.AddVertex(w, tSVector2, tSVector3);
}
bool flag2 = @new.Closest(out tSVector4);
if (flag2)
{
x = tSVector4.sqrMagnitude;
normal = tSVector4;
}
else
{
x = FP.Zero;
}
}
@new.ComputePoints(out p1, out p2);
bool flag3 = normal.sqrMagnitude > TSMath.Epsilon * TSMath.Epsilon;
if (flag3)
{
normal.Normalize();
}
GJKCollide.simplexSolverPool.GiveBack(@new);
return(true);
}
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);
}
private void DetectRigidRigid(RigidBody body1, RigidBody body2)
{
bool flag = body1.Shape is Multishape;
bool flag2 = body2.Shape is Multishape;
bool flag3 = this.speculativeContacts || body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts;
bool flag4 = !flag && !flag2;
if (flag4)
{
TSVector tSVector;
TSVector value;
FP fP;
bool flag5 = XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out tSVector, out value, out fP);
if (flag5)
{
TSVector tSVector2;
TSVector tSVector3;
this.FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref tSVector, ref value, out tSVector2, out tSVector3);
this.RaiseCollisionDetected(body1, body2, ref tSVector2, ref tSVector3, ref value, fP);
}
else
{
bool flag6 = flag3;
if (flag6)
{
TSVector value2;
TSVector value3;
bool flag7 = GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out value2, out value3, out value);
if (flag7)
{
TSVector value4 = value3 - value2;
bool flag8 = value4.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude;
if (flag8)
{
fP = value4 * value;
bool flag9 = fP < FP.Zero;
if (flag9)
{
this.RaiseCollisionDetected(body1, body2, ref value2, ref value3, ref value, fP);
}
}
}
}
}
}
else
{
bool flag10 = flag & flag2;
if (flag10)
{
Multishape multishape = body1.Shape as Multishape;
Multishape multishape2 = body2.Shape as Multishape;
multishape = multishape.RequestWorkingClone();
multishape2 = multishape2.RequestWorkingClone();
TSBBox boundingBox = body2.boundingBox;
boundingBox.InverseTransform(ref body1.position, ref body1.orientation);
int num = multishape.Prepare(ref boundingBox);
boundingBox = body1.boundingBox;
boundingBox.InverseTransform(ref body2.position, ref body2.orientation);
int num2 = multishape2.Prepare(ref boundingBox);
bool flag11 = num == 0 || num2 == 0;
if (flag11)
{
multishape.ReturnWorkingClone();
multishape2.ReturnWorkingClone();
}
else
{
for (int i = 0; i < num; i++)
{
multishape.SetCurrentShape(i);
for (int j = 0; j < num2; j++)
{
multishape2.SetCurrentShape(j);
TSVector tSVector;
TSVector value;
FP fP;
bool flag12 = XenoCollide.Detect(multishape, multishape2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out tSVector, out value, out fP);
if (flag12)
{
TSVector tSVector4;
TSVector tSVector5;
this.FindSupportPoints(body1, body2, multishape, multishape2, ref tSVector, ref value, out tSVector4, out tSVector5);
this.RaiseCollisionDetected(body1, body2, ref tSVector4, ref tSVector5, ref value, fP);
}
else
{
bool flag13 = flag3;
if (flag13)
{
TSVector value5;
TSVector value6;
bool flag14 = GJKCollide.ClosestPoints(multishape, multishape2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out value5, out value6, out value);
if (flag14)
{
TSVector value7 = value6 - value5;
bool flag15 = value7.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude;
if (flag15)
{
fP = value7 * value;
bool flag16 = fP < FP.Zero;
if (flag16)
{
this.RaiseCollisionDetected(body1, body2, ref value5, ref value6, ref value, fP);
}
}
}
}
}
}
}
multishape.ReturnWorkingClone();
multishape2.ReturnWorkingClone();
}
}
else
{
bool flag17 = body2.Shape is Multishape;
RigidBody rigidBody;
RigidBody rigidBody2;
if (flag17)
{
rigidBody = body2;
rigidBody2 = body1;
}
else
{
rigidBody2 = body2;
rigidBody = body1;
}
Multishape multishape3 = rigidBody.Shape as Multishape;
multishape3 = multishape3.RequestWorkingClone();
TSBBox boundingBox2 = rigidBody2.boundingBox;
boundingBox2.InverseTransform(ref rigidBody.position, ref rigidBody.orientation);
int num3 = multishape3.Prepare(ref boundingBox2);
bool flag18 = num3 == 0;
if (flag18)
{
multishape3.ReturnWorkingClone();
}
else
{
for (int k = 0; k < num3; k++)
{
multishape3.SetCurrentShape(k);
TSVector tSVector;
TSVector value;
FP fP;
bool flag19 = XenoCollide.Detect(multishape3, rigidBody2.Shape, ref rigidBody.orientation, ref rigidBody2.orientation, ref rigidBody.position, ref rigidBody2.position, out tSVector, out value, out fP);
if (flag19)
{
TSVector tSVector6;
TSVector tSVector7;
this.FindSupportPoints(rigidBody, rigidBody2, multishape3, rigidBody2.Shape, ref tSVector, ref value, out tSVector6, out tSVector7);
bool flag20 = this.useTerrainNormal && multishape3 is TerrainShape;
if (flag20)
{
(multishape3 as TerrainShape).CollisionNormal(out value);
TSVector.Transform(ref value, ref rigidBody.orientation, out value);
}
else
{
bool flag21 = this.useTriangleMeshNormal && multishape3 is TriangleMeshShape;
if (flag21)
{
(multishape3 as TriangleMeshShape).CollisionNormal(out value);
TSVector.Transform(ref value, ref rigidBody.orientation, out value);
}
}
this.RaiseCollisionDetected(rigidBody, rigidBody2, ref tSVector6, ref tSVector7, ref value, fP);
}
else
{
bool flag22 = flag3;
if (flag22)
{
TSVector value8;
TSVector value9;
bool flag23 = GJKCollide.ClosestPoints(multishape3, rigidBody2.Shape, ref rigidBody.orientation, ref rigidBody2.orientation, ref rigidBody.position, ref rigidBody2.position, out value8, out value9, out value);
if (flag23)
{
TSVector value10 = value9 - value8;
bool flag24 = value10.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude;
if (flag24)
{
fP = value10 * value;
bool flag25 = fP < FP.Zero;
if (flag25)
{
this.RaiseCollisionDetected(rigidBody, rigidBody2, ref value8, ref value9, ref value, fP);
}
}
}
}
}
}
multishape3.ReturnWorkingClone();
}
}
}
}
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 override bool Raycast(RigidBody body, TSVector rayOrigin, TSVector rayDirection, out TSVector normal, out FP fraction)
{
fraction = FP.MaxValue;
normal = TSVector.zero;
bool flag = !body.BoundingBox.RayIntersect(ref rayOrigin, ref rayDirection);
bool result;
if (flag)
{
result = false;
}
else
{
bool flag2 = body.Shape is Multishape;
if (flag2)
{
Multishape multishape = (body.Shape as Multishape).RequestWorkingClone();
bool flag3 = false;
TSVector tSVector;
TSVector.Subtract(ref rayOrigin, ref body.position, out tSVector);
TSVector.Transform(ref tSVector, ref body.invOrientation, out tSVector);
TSVector tSVector2;
TSVector.Transform(ref rayDirection, ref body.invOrientation, out tSVector2);
int num = multishape.Prepare(ref tSVector, ref tSVector2);
for (int i = 0; i < num; i++)
{
multishape.SetCurrentShape(i);
FP fP;
TSVector tSVector3;
bool flag4 = GJKCollide.Raycast(multishape, ref body.orientation, ref body.invOrientation, ref body.position, ref rayOrigin, ref rayDirection, out fP, out tSVector3);
if (flag4)
{
bool flag5 = fP < fraction;
if (flag5)
{
bool flag6 = this.useTerrainNormal && multishape is TerrainShape;
if (flag6)
{
(multishape as TerrainShape).CollisionNormal(out tSVector3);
TSVector.Transform(ref tSVector3, ref body.orientation, out tSVector3);
tSVector3.Negate();
}
else
{
bool flag7 = this.useTriangleMeshNormal && multishape is TriangleMeshShape;
if (flag7)
{
(multishape as TriangleMeshShape).CollisionNormal(out tSVector3);
TSVector.Transform(ref tSVector3, ref body.orientation, out tSVector3);
tSVector3.Negate();
}
}
normal = tSVector3;
fraction = fP;
flag3 = true;
}
}
}
multishape.ReturnWorkingClone();
result = flag3;
}
else
{
result = GJKCollide.Raycast(body.Shape, ref body.orientation, ref body.invOrientation, ref body.position, ref rayOrigin, ref rayDirection, out fraction, out normal);
}
}
return(result);
}
