/// <summary>
/// Subtracts two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>The difference of both vectors.</returns>
#region public static JVector Subtract(JVector value1, JVector value2)
public static TSVector Subtract(TSVector value1, TSVector value2)
{
TSVector result;
TSVector.Subtract(ref value1, ref value2, out result);
return(result);
}
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 PointOnPoint(RigidBody body1, RigidBody body2, TSVector anchor) : base(body1, body2)
{
TSVector.Subtract(ref anchor, ref body1.position, out this.localAnchor1);
TSVector.Subtract(ref anchor, ref body2.position, out this.localAnchor2);
TSVector.Transform(ref this.localAnchor1, ref body1.invOrientation, out this.localAnchor1);
TSVector.Transform(ref this.localAnchor2, ref body2.invOrientation, out this.localAnchor2);
}
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 override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref this.localAnchor1, ref this.body1.orientation, out this.r1);
TSVector.Transform(ref this.localAnchor2, ref this.body2.orientation, out this.r2);
TSVector value;
TSVector.Add(ref this.body1.position, ref this.r1, out value);
TSVector value2;
TSVector.Add(ref this.body2.position, ref this.r2, out value2);
TSVector tSVector;
TSVector.Subtract(ref value2, ref value, out tSVector);
FP x = tSVector.magnitude - this.distance;
bool flag = this.behavior == PointPointDistance.DistanceBehavior.LimitMaximumDistance && x <= FP.Zero;
if (flag)
{
this.skipConstraint = true;
}
else
{
bool flag2 = this.behavior == PointPointDistance.DistanceBehavior.LimitMinimumDistance && x >= FP.Zero;
if (flag2)
{
this.skipConstraint = true;
}
else
{
this.skipConstraint = false;
TSVector value3 = value2 - value;
bool flag3 = value3.sqrMagnitude != FP.Zero;
if (flag3)
{
value3.Normalize();
}
this.jacobian[0] = -FP.One * value3;
this.jacobian[1] = -FP.One * (this.r1 % value3);
this.jacobian[2] = FP.One * value3;
this.jacobian[3] = this.r2 % value3;
this.effectiveMass = this.body1.inverseMass + this.body2.inverseMass + TSVector.Transform(this.jacobian[1], this.body1.invInertiaWorld) * this.jacobian[1] + TSVector.Transform(this.jacobian[3], this.body2.invInertiaWorld) * this.jacobian[3];
this.softnessOverDt = this.softness / timestep;
this.effectiveMass += this.softnessOverDt;
this.effectiveMass = FP.One / this.effectiveMass;
this.bias = x * this.biasFactor * (FP.One / timestep);
bool flag4 = !this.body1.isStatic;
if (flag4)
{
this.body1.linearVelocity += this.body1.inverseMass * this.accumulatedImpulse * this.jacobian[0];
this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse * this.jacobian[1], this.body1.invInertiaWorld);
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
this.body2.linearVelocity += this.body2.inverseMass * this.accumulatedImpulse * this.jacobian[2];
this.body2.angularVelocity += TSVector.Transform(this.accumulatedImpulse * this.jacobian[3], this.body2.invInertiaWorld);
}
}
}
}
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 PointOnLine(RigidBody body1, RigidBody body2, TSVector lineStartPointBody1, TSVector pointBody2) : base(body1, body2)
{
TSVector.Subtract(ref lineStartPointBody1, ref body1.position, out this.localAnchor1);
TSVector.Subtract(ref pointBody2, ref body2.position, out this.localAnchor2);
TSVector.Transform(ref this.localAnchor1, ref body1.invOrientation, out this.localAnchor1);
TSVector.Transform(ref this.localAnchor2, ref body2.invOrientation, out this.localAnchor2);
this.lineNormal = TSVector.Normalize(lineStartPointBody1 - pointBody2);
}
public PointPointDistance(RigidBody body1, RigidBody body2, TSVector anchor1, TSVector anchor2) : base(body1, body2)
{
TSVector.Subtract(ref anchor1, ref body1.position, out this.localAnchor1);
TSVector.Subtract(ref anchor2, ref body2.position, out this.localAnchor2);
TSVector.Transform(ref this.localAnchor1, ref body1.invOrientation, out this.localAnchor1);
TSVector.Transform(ref this.localAnchor2, ref body2.invOrientation, out this.localAnchor2);
this.distance = (anchor1 - anchor2).magnitude;
}
private void CreateAABox(ref TSBBox aabb, Octree.EChild child, out TSBBox result)
{
TSVector tSVector;
TSVector.Subtract(ref aabb.max, ref aabb.min, out tSVector);
TSVector.Multiply(ref tSVector, FP.Half, out tSVector);
TSVector zero = TSVector.zero;
switch (child)
{
case Octree.EChild.MMM:
zero = new TSVector(0, 0, 0);
break;
case Octree.EChild.XP:
zero = new TSVector(1, 0, 0);
break;
case Octree.EChild.YP:
zero = new TSVector(0, 1, 0);
break;
case Octree.EChild.PPM:
zero = new TSVector(1, 1, 0);
break;
case Octree.EChild.ZP:
zero = new TSVector(0, 0, 1);
break;
case Octree.EChild.PMP:
zero = new TSVector(1, 0, 1);
break;
case Octree.EChild.MPP:
zero = new TSVector(0, 1, 1);
break;
case Octree.EChild.PPP:
zero = new TSVector(1, 1, 1);
break;
default:
Debug.WriteLine("Octree.CreateAABox got impossible child");
break;
}
result = default(TSBBox);
result.min = new TSVector(zero.x * tSVector.x, zero.y * tSVector.y, zero.z * tSVector.z);
TSVector.Add(ref result.min, ref aabb.min, out result.min);
TSVector.Add(ref result.min, ref tSVector, out result.max);
FP eN = FP.EN5;
TSVector tSVector2;
TSVector.Multiply(ref tSVector, eN, out tSVector2);
TSVector.Subtract(ref result.min, ref tSVector2, out result.min);
TSVector.Add(ref result.max, ref tSVector2, out result.max);
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector zero = TSVector.zero;
for (int i = 0; i < this.shapes.Count; i++)
{
TSVector tSVector;
this.shapes[i].SupportMapping(ref direction, out tSVector);
TSVector.Add(ref tSVector, ref zero, out zero);
}
TSVector.Subtract(ref zero, ref this.shifted, out result);
}
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref this.localAnchor1, ref this.body1.orientation, out this.r1);
TSVector.Transform(ref this.localAnchor2, ref this.body2.orientation, out this.r2);
TSVector tSVector;
TSVector.Add(ref this.body1.position, ref this.r1, out tSVector);
TSVector tSVector2;
TSVector.Add(ref this.body2.position, ref this.r2, out tSVector2);
TSVector tSVector3;
TSVector.Subtract(ref tSVector2, ref tSVector, out tSVector3);
TSVector tSVector4 = TSVector.Transform(this.lineNormal, this.body1.orientation);
tSVector4.Normalize();
TSVector tSVector5 = (tSVector - tSVector2) % tSVector4;
bool flag = tSVector5.sqrMagnitude != FP.Zero;
if (flag)
{
tSVector5.Normalize();
}
tSVector5 %= tSVector4;
this.jacobian[0] = tSVector5;
this.jacobian[1] = (this.r1 + tSVector2 - tSVector) % tSVector5;
this.jacobian[2] = -FP.One * tSVector5;
this.jacobian[3] = -FP.One * this.r2 % tSVector5;
this.effectiveMass = this.body1.inverseMass + this.body2.inverseMass + TSVector.Transform(this.jacobian[1], this.body1.invInertiaWorld) * this.jacobian[1] + TSVector.Transform(this.jacobian[3], this.body2.invInertiaWorld) * this.jacobian[3];
this.softnessOverDt = this.softness / timestep;
this.effectiveMass += this.softnessOverDt;
bool flag2 = this.effectiveMass != 0;
if (flag2)
{
this.effectiveMass = FP.One / this.effectiveMass;
}
this.bias = -(tSVector4 % (tSVector2 - tSVector)).magnitude * this.biasFactor * (FP.One / timestep);
bool flag3 = !this.body1.isStatic;
if (flag3)
{
this.body1.linearVelocity += this.body1.inverseMass * this.accumulatedImpulse * this.jacobian[0];
this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse * this.jacobian[1], this.body1.invInertiaWorld);
}
bool flag4 = !this.body2.isStatic;
if (flag4)
{
this.body2.linearVelocity += this.body2.inverseMass * this.accumulatedImpulse * this.jacobian[2];
this.body2.angularVelocity += TSVector.Transform(this.accumulatedImpulse * this.jacobian[3], this.body2.invInertiaWorld);
}
}
public override void CalculateMassInertia()
{
this.geomCen = TSVector.zero;
this.inertia = TSMatrix.Identity;
TSVector tSVector;
TSVector.Subtract(ref this.boundingBox.max, ref this.boundingBox.min, out tSVector);
this.mass = tSVector.x * tSVector.y * tSVector.z;
this.inertia.M11 = FP.One / (12 * FP.One) * this.mass * (tSVector.y * tSVector.y + tSVector.z * tSVector.z);
this.inertia.M22 = FP.One / (12 * FP.One) * this.mass * (tSVector.x * tSVector.x + tSVector.z * tSVector.z);
this.inertia.M33 = FP.One / (12 * FP.One) * this.mass * (tSVector.x * tSVector.x + tSVector.y * tSVector.y);
}
public HingeJoint(World world, RigidBody body1, RigidBody body2, TSVector position, TSVector hingeAxis) : base(world)
{
this.worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector anchor = position;
TSVector.Add(ref anchor, ref hingeAxis, out anchor);
TSVector anchor2 = position;
TSVector.Subtract(ref anchor2, ref hingeAxis, out anchor2);
this.worldPointConstraint[0] = new PointOnPoint(body1, body2, anchor);
this.worldPointConstraint[1] = new PointOnPoint(body1, body2, anchor2);
}
public override void PrepareForIteration(FP timestep)
{
TSVector tSVector;
TSVector.Subtract(ref this.body2.position, ref this.body1.position, out tSVector);
FP x = tSVector.magnitude - this.distance;
bool flag = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMaximumDistance && x <= FP.Zero;
if (flag)
{
this.skipConstraint = true;
}
else
{
bool flag2 = this.behavior == SoftBody.Spring.DistanceBehavior.LimitMinimumDistance && x >= FP.Zero;
if (flag2)
{
this.skipConstraint = true;
}
else
{
this.skipConstraint = false;
TSVector value = tSVector;
bool flag3 = value.sqrMagnitude != FP.Zero;
if (flag3)
{
value.Normalize();
}
this.jacobian[0] = -FP.One * value;
this.jacobian[1] = FP.One * value;
this.effectiveMass = this.body1.inverseMass + this.body2.inverseMass;
this.softnessOverDt = this.softness / timestep;
this.effectiveMass += this.softnessOverDt;
this.effectiveMass = FP.One / this.effectiveMass;
this.bias = x * this.biasFactor * (FP.One / timestep);
bool flag4 = !this.body1.isStatic;
if (flag4)
{
this.body1.linearVelocity += this.body1.inverseMass * this.accumulatedImpulse * this.jacobian[0];
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
this.body2.linearVelocity += this.body2.inverseMass * this.accumulatedImpulse * this.jacobian[1];
}
}
}
}
public void Initialize(RigidBody body1, RigidBody body2, ref TSVector point1, ref TSVector point2, ref TSVector n, FP penetration, bool newContact, ContactSettings settings)
{
this.body1 = body1;
this.body2 = body2;
this.normal = n;
this.normal.Normalize();
this.p1 = point1;
this.p2 = point2;
this.newContact = newContact;
TSVector.Subtract(ref this.p1, ref body1.position, out this.relativePos1);
TSVector.Subtract(ref this.p2, ref body2.position, out this.relativePos2);
TSVector.Transform(ref this.relativePos1, ref body1.invOrientation, out this.realRelPos1);
TSVector.Transform(ref this.relativePos2, ref body2.invOrientation, out this.realRelPos2);
this.initialPen = penetration;
this.penetration = penetration;
this.body1IsMassPoint = body1.isParticle;
this.body2IsMassPoint = body2.isParticle;
if (newContact)
{
this.treatBody1AsStatic = body1.isStatic;
this.treatBody2AsStatic = body2.isStatic;
this.accumulatedNormalImpulse = FP.Zero;
this.accumulatedTangentImpulse = FP.Zero;
this.lostSpeculativeBounce = FP.Zero;
switch (settings.MaterialCoefficientMixing)
{
case ContactSettings.MaterialCoefficientMixingType.TakeMaximum:
this.staticFriction = TSMath.Max(body1.material.staticFriction, body2.material.staticFriction);
this.dynamicFriction = TSMath.Max(body1.material.kineticFriction, body2.material.kineticFriction);
this.restitution = TSMath.Max(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.TakeMinimum:
this.staticFriction = TSMath.Min(body1.material.staticFriction, body2.material.staticFriction);
this.dynamicFriction = TSMath.Min(body1.material.kineticFriction, body2.material.kineticFriction);
this.restitution = TSMath.Min(body1.material.restitution, body2.material.restitution);
break;
case ContactSettings.MaterialCoefficientMixingType.UseAverage:
this.staticFriction = (body1.material.staticFriction + body2.material.staticFriction) / (2 * FP.One);
this.dynamicFriction = (body1.material.kineticFriction + body2.material.kineticFriction) / (2 * FP.One);
this.restitution = (body1.material.restitution + body2.material.restitution) / (2 * FP.One);
break;
}
}
this.settings = settings;
}
public override void PrepareForIteration(FP timestep)
{
TSVector.Transform(ref this.localAnchor1, ref this.body1.orientation, out this.r1);
TSVector.Transform(ref this.localAnchor2, ref this.body2.orientation, out this.r2);
TSVector value;
TSVector.Add(ref this.body1.position, ref this.r1, out value);
TSVector value2;
TSVector.Add(ref this.body2.position, ref this.r2, out value2);
TSVector tSVector;
TSVector.Subtract(ref value2, ref value, out tSVector);
FP magnitude = tSVector.magnitude;
TSVector value3 = value2 - value;
bool flag = value3.sqrMagnitude != FP.Zero;
if (flag)
{
value3.Normalize();
}
this.jacobian[0] = -FP.One * value3;
this.jacobian[1] = -FP.One * (this.r1 % value3);
this.jacobian[2] = FP.One * value3;
this.jacobian[3] = this.r2 % value3;
this.effectiveMass = this.body1.inverseMass + this.body2.inverseMass + TSVector.Transform(this.jacobian[1], this.body1.invInertiaWorld) * this.jacobian[1] + TSVector.Transform(this.jacobian[3], this.body2.invInertiaWorld) * this.jacobian[3];
this.softnessOverDt = this.softness / timestep;
this.effectiveMass += this.softnessOverDt;
this.effectiveMass = FP.One / this.effectiveMass;
this.bias = magnitude * this.biasFactor * (FP.One / timestep);
bool flag2 = !this.body1.isStatic;
if (flag2)
{
this.body1.linearVelocity += this.body1.inverseMass * this.accumulatedImpulse * this.jacobian[0];
this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse * this.jacobian[1], this.body1.invInertiaWorld);
}
bool flag3 = !this.body2.isStatic;
if (flag3)
{
this.body2.linearVelocity += this.body2.inverseMass * this.accumulatedImpulse * this.jacobian[2];
this.body2.angularVelocity += TSVector.Transform(this.accumulatedImpulse * this.jacobian[3], this.body2.invInertiaWorld);
}
}
public static int FindNearestTrianglePoint(SoftBody sb, int id, ref TSVector point)
{
SoftBody.Triangle userData = sb.dynamicTree.GetUserData(id);
TSVector position = sb.VertexBodies[userData.indices.I0].position;
TSVector.Subtract(ref position, ref point, out position);
FP sqrMagnitude = position.sqrMagnitude;
position = sb.VertexBodies[userData.indices.I1].position;
TSVector.Subtract(ref position, ref point, out position);
FP sqrMagnitude2 = position.sqrMagnitude;
position = sb.VertexBodies[userData.indices.I2].position;
TSVector.Subtract(ref position, ref point, out position);
FP sqrMagnitude3 = position.sqrMagnitude;
bool flag = sqrMagnitude < sqrMagnitude2;
int result;
if (flag)
{
bool flag2 = sqrMagnitude < sqrMagnitude3;
if (flag2)
{
result = userData.indices.I0;
}
else
{
result = userData.indices.I2;
}
}
else
{
bool flag3 = sqrMagnitude2 < sqrMagnitude3;
if (flag3)
{
result = userData.indices.I1;
}
else
{
result = userData.indices.I2;
}
}
return(result);
}
private int GetCacheEntry(ref TSVector realRelPos1, FP contactBreakThreshold)
{
FP y = contactBreakThreshold * contactBreakThreshold;
int count = this.contactList.Count;
int result = -1;
for (int i = 0; i < count; i++)
{
TSVector tSVector;
TSVector.Subtract(ref this.contactList[i].relativePos1, ref realRelPos1, out tSVector);
FP sqrMagnitude = tSVector.sqrMagnitude;
bool flag = sqrMagnitude < y;
if (flag)
{
y = sqrMagnitude;
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 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 Contact AddContact(TSVector point1, TSVector point2, TSVector normal, FP penetration, ContactSettings contactSettings)
{
TSVector tSVector;
TSVector.Subtract(ref point1, ref this.body1.position, out tSVector);
ContactList obj = this.contactList;
Contact result;
lock (obj)
{
bool flag = this.contactList.Count == 4;
if (flag)
{
int num = this.SortCachedPoints(ref tSVector, penetration);
this.ReplaceContact(ref point1, ref point2, ref normal, penetration, num, contactSettings);
result = null;
}
else
{
int num = this.GetCacheEntry(ref tSVector, contactSettings.breakThreshold);
bool flag2 = num >= 0;
if (flag2)
{
this.ReplaceContact(ref point1, ref point2, ref normal, penetration, num, contactSettings);
result = null;
}
else
{
Contact @new = Contact.Pool.GetNew();
@new.Initialize(this.body1, this.body2, ref point1, ref point2, ref normal, penetration, true, contactSettings);
this.contactList.Add(@new);
result = @new;
}
}
}
return(result);
}
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);
}
internal void InverseTransform(ref TSVector position, ref TSMatrix orientation)
{
TSVector.Subtract(ref this.max, ref position, out this.max);
TSVector.Subtract(ref this.min, ref position, out this.min);
TSVector tSVector;
TSVector.Add(ref this.max, ref this.min, out tSVector);
tSVector.x *= FP.Half;
tSVector.y *= FP.Half;
tSVector.z *= FP.Half;
TSVector tSVector2;
TSVector.Subtract(ref this.max, ref this.min, out tSVector2);
tSVector2.x *= FP.Half;
tSVector2.y *= FP.Half;
tSVector2.z *= FP.Half;
TSVector.TransposedTransform(ref tSVector, ref orientation, out tSVector);
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector.TransposedTransform(ref tSVector2, ref tSMatrix, out tSVector2);
TSVector.Add(ref tSVector, ref tSVector2, out this.max);
TSVector.Subtract(ref tSVector, ref tSVector2, out this.min);
}
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);
}
/// <summary>
/// Subtracts two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>The difference of both vectors.</returns>
#region public static JVector operator -(JVector value1, JVector value2)
public static TSVector operator -(TSVector value1, TSVector value2)
{
TSVector result; TSVector.Subtract(value1, value2, out result);
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);
}
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));
}
