public static TSVector operator %(TSVector value1, TSVector value2)
{
TSVector result;
TSVector.Cross(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// The cross product of two vectors.
/// </summary>
/// <param name="vector1">The first vector.</param>
/// <param name="vector2">The second vector.</param>
/// <returns>The cross product of both vectors.</returns>
#region public static JVector Cross(JVector vector1, JVector vector2)
public static TSVector Cross(TSVector vector1, TSVector vector2)
{
TSVector result;
TSVector.Cross(ref vector1, ref vector2, out result);
return(result);
}
public void AddForce(TSVector force, TSVector pos)
{
TSVector.Add(ref this.force, ref force, out this.force);
TSVector.Subtract(ref pos, ref this.position, out pos);
TSVector.Cross(ref pos, ref force, out pos);
TSVector.Add(ref pos, ref this.torque, out this.torque);
}
public void GetNormal(out TSVector normal)
{
TSVector tSVector;
TSVector.Subtract(ref this.owner.points[this.indices.I1].position, ref this.owner.points[this.indices.I0].position, out tSVector);
TSVector.Subtract(ref this.owner.points[this.indices.I2].position, ref this.owner.points[this.indices.I0].position, out normal);
TSVector.Cross(ref tSVector, ref normal, out normal);
}
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);
}
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 void ApplyImpulse(TSVector impulse, TSVector relativePosition)
{
bool flag = this.isStatic;
if (flag)
{
throw new InvalidOperationException("Can't apply an impulse to a static body.");
}
TSVector tSVector;
TSVector.Multiply(ref impulse, this.inverseMass, out tSVector);
TSVector.Add(ref this.linearVelocity, ref tSVector, out this.linearVelocity);
TSVector.Cross(ref relativePosition, ref impulse, out tSVector);
TSVector.Transform(ref tSVector, ref this.invInertiaWorld, out tSVector);
TSVector.Add(ref this.angularVelocity, ref tSVector, out this.angularVelocity);
}
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);
}
public static void LookAt(out TSMatrix result, TSVector position, TSVector target)
{
TSVector tSVector = target - position;
tSVector.Normalize();
TSVector tSVector2 = TSVector.Cross(TSVector.up, tSVector);
tSVector2.Normalize();
TSVector tSVector3 = TSVector.Cross(tSVector, tSVector2);
result.M11 = tSVector2.x;
result.M21 = tSVector3.x;
result.M31 = tSVector.x;
result.M12 = tSVector2.y;
result.M22 = tSVector3.y;
result.M32 = tSVector.y;
result.M13 = tSVector2.z;
result.M23 = tSVector3.z;
result.M33 = tSVector.z;
}
public override void SetCurrentShape(int index)
{
this.vecs[0] = this.octree.GetVertex(this.octree.tris[this.potentialTriangles[index]].I0);
this.vecs[1] = this.octree.GetVertex(this.octree.tris[this.potentialTriangles[index]].I1);
this.vecs[2] = this.octree.GetVertex(this.octree.tris[this.potentialTriangles[index]].I2);
TSVector geomCen = this.vecs[0];
TSVector.Add(ref geomCen, ref this.vecs[1], out geomCen);
TSVector.Add(ref geomCen, ref this.vecs[2], out geomCen);
TSVector.Multiply(ref geomCen, FP.One / (3 * FP.One), out geomCen);
this.geomCen = geomCen;
TSVector.Subtract(ref this.vecs[1], ref this.vecs[0], out geomCen);
TSVector.Subtract(ref this.vecs[2], ref this.vecs[0], out this.normal);
TSVector.Cross(ref geomCen, ref this.normal, out this.normal);
bool flag = this.flipNormal;
if (flag)
{
this.normal.Negate();
}
}
public override void SetCurrentShape(int index)
{
bool flag = false;
bool flag2 = index >= this.numX * this.numZ;
if (flag2)
{
flag = true;
index -= this.numX * this.numZ;
}
int num = index % this.numX;
int num2 = index / this.numX;
bool flag3 = flag;
if (flag3)
{
this.points[0].Set((this.minX + num) * this.scaleX, this.heights[this.minX + num, this.minZ + num2], (this.minZ + num2) * this.scaleZ);
this.points[1].Set((this.minX + num + 1) * this.scaleX, this.heights[this.minX + num + 1, this.minZ + num2], (this.minZ + num2) * this.scaleZ);
this.points[2].Set((this.minX + num) * this.scaleX, this.heights[this.minX + num, this.minZ + num2 + 1], (this.minZ + num2 + 1) * this.scaleZ);
}
else
{
this.points[0].Set((this.minX + num + 1) * this.scaleX, this.heights[this.minX + num + 1, this.minZ + num2], (this.minZ + num2) * this.scaleZ);
this.points[1].Set((this.minX + num + 1) * this.scaleX, this.heights[this.minX + num + 1, this.minZ + num2 + 1], (this.minZ + num2 + 1) * this.scaleZ);
this.points[2].Set((this.minX + num) * this.scaleX, this.heights[this.minX + num, this.minZ + num2 + 1], (this.minZ + num2 + 1) * this.scaleZ);
}
TSVector geomCen = this.points[0];
TSVector.Add(ref geomCen, ref this.points[1], out geomCen);
TSVector.Add(ref geomCen, ref this.points[2], out geomCen);
TSVector.Multiply(ref geomCen, FP.One / (3 * FP.One), out geomCen);
this.geomCen = geomCen;
TSVector.Subtract(ref this.points[1], ref this.points[0], out geomCen);
TSVector.Subtract(ref this.points[2], ref this.points[0], out this.normal);
TSVector.Cross(ref geomCen, ref this.normal, out this.normal);
}
/**
* @brief Returns the velocity of the body at some position in world space.
**/
public TSVector GetPointVelocity(TSVector worldPoint)
{
TSVector directionPoint = position - tsCollider.Body.TSPosition;
return(TSVector.Cross(tsCollider.Body.TSAngularVelocity, directionPoint) + tsCollider.Body.TSLinearVelocity);
}
private int SortCachedPoints(ref TSVector realRelPos1, FP pen)
{
int num = -1;
FP y = pen;
for (int i = 0; i < 4; i++)
{
bool flag = this.contactList[i].penetration > y;
if (flag)
{
num = i;
y = this.contactList[i].penetration;
}
}
FP x = 0;
FP y2 = 0;
FP z = 0;
FP w = 0;
bool flag2 = num != 0;
if (flag2)
{
TSVector tSVector;
TSVector.Subtract(ref realRelPos1, ref this.contactList[1].relativePos1, out tSVector);
TSVector tSVector2;
TSVector.Subtract(ref this.contactList[3].relativePos1, ref this.contactList[2].relativePos1, out tSVector2);
TSVector tSVector3;
TSVector.Cross(ref tSVector, ref tSVector2, out tSVector3);
x = tSVector3.sqrMagnitude;
}
bool flag3 = num != 1;
if (flag3)
{
TSVector tSVector4;
TSVector.Subtract(ref realRelPos1, ref this.contactList[0].relativePos1, out tSVector4);
TSVector tSVector5;
TSVector.Subtract(ref this.contactList[3].relativePos1, ref this.contactList[2].relativePos1, out tSVector5);
TSVector tSVector6;
TSVector.Cross(ref tSVector4, ref tSVector5, out tSVector6);
y2 = tSVector6.sqrMagnitude;
}
bool flag4 = num != 2;
if (flag4)
{
TSVector tSVector7;
TSVector.Subtract(ref realRelPos1, ref this.contactList[0].relativePos1, out tSVector7);
TSVector tSVector8;
TSVector.Subtract(ref this.contactList[3].relativePos1, ref this.contactList[1].relativePos1, out tSVector8);
TSVector tSVector9;
TSVector.Cross(ref tSVector7, ref tSVector8, out tSVector9);
z = tSVector9.sqrMagnitude;
}
bool flag5 = num != 3;
if (flag5)
{
TSVector tSVector10;
TSVector.Subtract(ref realRelPos1, ref this.contactList[0].relativePos1, out tSVector10);
TSVector tSVector11;
TSVector.Subtract(ref this.contactList[2].relativePos1, ref this.contactList[1].relativePos1, out tSVector11);
TSVector tSVector12;
TSVector.Cross(ref tSVector10, ref tSVector11, out tSVector12);
w = tSVector12.sqrMagnitude;
}
return(Arbiter.MaxAxis(x, y2, z, w));
}
/**
* @brief Returns the velocity of the body at some position in world space.
**/
public TSVector2 GetPointVelocity(TSVector2 worldPoint)
{
TSVector directionPoint = (position - tsCollider.Body.TSPosition).ToTSVector();
return(TSVector.Cross(new TSVector(0, 0, tsCollider.Body.TSAngularVelocity), directionPoint).ToTSVector2() + tsCollider.Body.TSLinearVelocity);
}
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);
}
