public void SupportCenter(out TSVector center)
{
center = this.owner.points[this.indices.I0].position;
TSVector.Add(ref center, ref this.owner.points[this.indices.I1].position, out center);
TSVector.Add(ref center, ref this.owner.points[this.indices.I2].position, out center);
TSVector.Multiply(ref center, FP.One / (3 * FP.One), out center);
}
/// <summary>
/// Adds two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>The sum of both vectors.</returns>
#region public static void Add(JVector value1, JVector value2)
public static TSVector Add(TSVector value1, TSVector value2)
{
TSVector result;
TSVector.Add(ref value1, ref value2, out result);
return(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 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 virtual void Update()
{
bool flag = this.isParticle;
if (flag)
{
this.inertia = TSMatrix.Zero;
this.invInertia = (this.invInertiaWorld = TSMatrix.Zero);
this.invOrientation = (this.orientation = TSMatrix.Identity);
this.boundingBox = this.shape.boundingBox;
TSVector.Add(ref this.boundingBox.min, ref this.position, out this.boundingBox.min);
TSVector.Add(ref this.boundingBox.max, ref this.position, out this.boundingBox.max);
this.angularVelocity.MakeZero();
}
else
{
TSMatrix.Transpose(ref this.orientation, out this.invOrientation);
this.Shape.GetBoundingBox(ref this.orientation, out this.boundingBox);
TSVector.Add(ref this.boundingBox.min, ref this.position, out this.boundingBox.min);
TSVector.Add(ref this.boundingBox.max, ref this.position, out this.boundingBox.max);
bool flag2 = !this.isStatic;
if (flag2)
{
TSMatrix.Multiply(ref this.invOrientation, ref this.invInertia, out this.invInertiaWorld);
TSMatrix.Multiply(ref this.invInertiaWorld, ref this.orientation, out this.invInertiaWorld);
}
}
}
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 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);
}
private void SupportMapping(RigidBody body, Shape workingShape, ref TSVector direction, out TSVector result)
{
TSVector.Transform(ref direction, ref body.invOrientation, out result);
workingShape.SupportMapping(ref result, out result);
TSVector.Transform(ref result, ref body.orientation, out result);
TSVector.Add(ref result, ref body.position, out result);
}
private void IntegrateForces()
{
int i = 0;
int count = this.rigidBodies.Count;
while (i < count)
{
RigidBody rigidBody = this.rigidBodies[i];
bool flag = !rigidBody.isStatic && rigidBody.IsActive;
if (flag)
{
TSVector tSVector;
TSVector.Multiply(ref rigidBody.force, rigidBody.inverseMass * this.timestep, out tSVector);
TSVector.Add(ref tSVector, ref rigidBody.linearVelocity, out rigidBody.linearVelocity);
bool flag2 = !rigidBody.isParticle;
if (flag2)
{
TSVector.Multiply(ref rigidBody.torque, this.timestep, out tSVector);
TSVector.Transform(ref tSVector, ref rigidBody.invInertiaWorld, out tSVector);
TSVector.Add(ref tSVector, ref rigidBody.angularVelocity, out rigidBody.angularVelocity);
}
bool affectedByGravity = rigidBody.affectedByGravity;
if (affectedByGravity)
{
TSVector.Multiply(ref this.gravity, this.timestep, out tSVector);
TSVector.Add(ref rigidBody.linearVelocity, ref tSVector, out rigidBody.linearVelocity);
}
}
rigidBody.force.MakeZero();
rigidBody.torque.MakeZero();
i++;
}
}
public override void SupportMapping(ref TSVector direction, out TSVector result)
{
TSVector.Transform(ref direction, ref this.shapes[this.currentShape].invOrientation, out result);
this.shapes[this.currentShape].Shape.SupportMapping(ref direction, out result);
TSVector.Transform(ref result, ref this.shapes[this.currentShape].orientation, out result);
TSVector.Add(ref result, ref this.shapes[this.currentShape].position, out 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);
}
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 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 void ApplyImpulse(TSVector impulse)
{
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);
}
private void IntegrateCallback(object obj)
{
RigidBody rigidBody = obj as RigidBody;
TSVector tSVector;
TSVector.Multiply(ref rigidBody.linearVelocity, this.timestep, out tSVector);
TSVector.Add(ref tSVector, ref rigidBody.position, out rigidBody.position);
bool flag = !rigidBody.isParticle;
if (flag)
{
FP magnitude = rigidBody.angularVelocity.magnitude;
bool flag2 = magnitude < FP.EN3;
TSVector tSVector2;
if (flag2)
{
TSVector.Multiply(ref rigidBody.angularVelocity, FP.Half * this.timestep - this.timestep * this.timestep * this.timestep * (2082 * FP.EN6) * magnitude * magnitude, out tSVector2);
}
else
{
TSVector.Multiply(ref rigidBody.angularVelocity, FP.Sin(FP.Half * magnitude * this.timestep) / magnitude, out tSVector2);
}
TSQuaternion tSQuaternion = new TSQuaternion(tSVector2.x, tSVector2.y, tSVector2.z, FP.Cos(magnitude * this.timestep * FP.Half));
TSQuaternion tSQuaternion2;
TSQuaternion.CreateFromMatrix(ref rigidBody.orientation, out tSQuaternion2);
TSQuaternion.Multiply(ref tSQuaternion, ref tSQuaternion2, out tSQuaternion);
tSQuaternion.Normalize();
TSMatrix.CreateFromQuaternion(ref tSQuaternion, out rigidBody.orientation);
}
bool flag3 = (rigidBody.Damping & RigidBody.DampingType.Linear) > RigidBody.DampingType.None;
if (flag3)
{
TSVector.Multiply(ref rigidBody.linearVelocity, this.currentLinearDampFactor, out rigidBody.linearVelocity);
}
bool flag4 = (rigidBody.Damping & RigidBody.DampingType.Angular) > RigidBody.DampingType.None;
if (flag4)
{
TSVector.Multiply(ref rigidBody.angularVelocity, this.currentAngularDampFactor, out rigidBody.angularVelocity);
}
rigidBody.Update();
bool flag5 = this.CollisionSystem.EnableSpeculativeContacts || rigidBody.EnableSpeculativeContacts;
if (flag5)
{
rigidBody.SweptExpandBoundingBox(this.timestep);
}
}
public void DebugDraw(IDebugDrawer drawer)
{
for (int i = 0; i < this.hullPoints.Count; i += 3)
{
TSVector pos = this.hullPoints[i];
TSVector pos2 = this.hullPoints[i + 1];
TSVector pos3 = this.hullPoints[i + 2];
TSVector.Transform(ref pos, ref this.orientation, out pos);
TSVector.Add(ref pos, ref this.position, out pos);
TSVector.Transform(ref pos2, ref this.orientation, out pos2);
TSVector.Add(ref pos2, ref this.position, out pos2);
TSVector.Transform(ref pos3, ref this.orientation, out pos3);
TSVector.Add(ref pos3, ref this.position, out pos3);
drawer.DrawTriangle(pos, pos2, pos3);
}
}
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 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 MakeHull(ref List <TSVector> triangleList, int generationThreshold)
{
List <TSVector> list = new List <TSVector>();
for (int i = 0; i < this.shapes.Length; i++)
{
this.shapes[i].Shape.MakeHull(ref list, 4);
for (int j = 0; j < list.Count; j++)
{
TSVector item = list[j];
TSVector.Transform(ref item, ref this.shapes[i].orientation, out item);
TSVector.Add(ref item, ref this.shapes[i].position, out item);
triangleList.Add(item);
}
list.Clear();
}
}
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);
}
}
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();
}
}
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 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);
}
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 void AddTorque(TSVector torque)
{
TSVector.Add(ref torque, ref this.torque, out this.torque);
}
public void AddForce(TSVector force)
{
TSVector.Add(ref force, ref this.force, out this.force);
}
/// <summary>
/// Adds two vectors.
/// </summary>
/// <param name="value1">The first vector.</param>
/// <param name="value2">The second vector.</param>
/// <returns>The sum of both vectors.</returns>
#region public static JVector operator +(JVector value1, JVector value2)
public static TSVector operator +(TSVector value1, TSVector value2)
{
TSVector result; TSVector.Add(value1, value2, out result);
return(result);
}
