/// <summary>
/// Inverses the direction of a vector.
/// </summary>
/// <param name="value">The vector to inverse.</param>
/// <returns>The negated vector.</returns>
public static TSVector Negate(TSVector value)
{
TSVector result;
TSVector.Negate(ref value, out result);
return(result);
}
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector max;
TSVector.Transform(ref this.halfSize, ref tSMatrix, out max);
box.max = max;
TSVector.Negate(ref max, out box.min);
}
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);
}
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);
}
private void CollisionDetected(RigidBody body1, RigidBody body2, TSVector point1, TSVector point2, TSVector normal, FP penetration)
{
bool flag = body1.IsColliderOnly || body2.IsColliderOnly;
Arbiter arbiter = null;
bool flag2 = flag;
ArbiterMap arbiterMap;
if (flag2)
{
arbiterMap = this.arbiterTriggerMap;
}
else
{
arbiterMap = this.arbiterMap;
}
bool flag3 = false;
ArbiterMap obj = arbiterMap;
lock (obj)
{
arbiterMap.LookUpArbiter(body1, body2, out arbiter);
bool flag4 = arbiter == null;
if (flag4)
{
arbiter = Arbiter.Pool.GetNew();
arbiter.body1 = body1;
arbiter.body2 = body2;
arbiterMap.Add(new ArbiterKey(body1, body2), arbiter);
flag3 = true;
}
}
bool flag5 = arbiter.body1 == body1;
Contact contact;
if (flag5)
{
TSVector.Negate(ref normal, out normal);
contact = arbiter.AddContact(point1, point2, normal, penetration, this.contactSettings);
}
else
{
contact = arbiter.AddContact(point2, point1, normal, penetration, this.contactSettings);
}
bool flag6 = flag3;
if (flag6)
{
bool flag7 = flag;
if (flag7)
{
this.events.RaiseTriggerBeginCollide(contact);
body1.arbitersTrigger.Add(arbiter);
body2.arbitersTrigger.Add(arbiter);
OverlapPairContact overlapPairContact = new OverlapPairContact(body1, body2);
overlapPairContact.contact = contact;
this.initialTriggers.Add(overlapPairContact);
}
else
{
this.events.RaiseBodiesBeginCollide(contact);
this.addedArbiterQueue.Enqueue(arbiter);
OverlapPairContact overlapPairContact2 = new OverlapPairContact(body1, body2);
overlapPairContact2.contact = contact;
this.initialCollisions.Add(overlapPairContact2);
}
}
bool flag8 = !flag && contact != null;
if (flag8)
{
this.events.RaiseContactCreated(contact);
}
}
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);
}
