/// <summary>
/// Transforms a vector by the given matrix.
/// </summary>
/// <param name="position">The vector to transform.</param>
/// <param name="matrix">The transform matrix.</param>
/// <returns>The transformed vector.</returns>
#region public static JVector Transform(JVector position, JMatrix matrix)
public static TSVector Transform(TSVector position, TSMatrix matrix)
{
TSVector result;
TSVector.Transform(ref position, ref matrix, 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);
}
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);
}
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 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 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 Rotate(TSMatrix orientation, TSVector center)
{
for (int i = 0; i < this.points.Count; i++)
{
this.points[i].position = TSVector.Transform(this.points[i].position - center, orientation);
}
}
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 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;
}
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);
}
/**
* @brief Rotates game object based on provided axis, point and angle of rotation.
**/
public void RotateAround(TSVector point, TSVector axis, FP angle)
{
TSVector vector = this.position;
TSVector vector2 = vector - point;
vector2 = TSVector.Transform(vector2, TSMatrix.AngleAxis(angle * FP.Deg2Rad, axis));
vector = point + vector2;
this.position = vector;
Rotate(axis, angle);
}
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);
}
private void UpdateChildRotation()
{
TSMatrix matrix = TSMatrix.CreateFromQuaternion(_rotation);
foreach (TSTransform child in tsChildren)
{
child.localRotation = TSQuaternion.CreateFromMatrix(TSMatrix.Inverse(matrix)) * _rotation;
child.localPosition = TSVector.Transform(child.localPosition, TSMatrix.CreateFromQuaternion(child.localRotation));
child.position = TransformPoint(child.localPosition);
}
}
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 void Transform(ref TSMatrix orientation)
{
TSVector value = FP.Half * (this.max - this.min);
TSVector value2 = FP.Half * (this.max + this.min);
TSVector.Transform(ref value2, ref orientation, out value2);
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector.Transform(ref value, ref tSMatrix, out value);
this.max = value2 + value;
this.min = value2 - value;
}
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 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 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 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);
}
}
public override void GetBoundingBox(ref TSMatrix orientation, out TSBBox box)
{
box.min = this.mInternalBBox.min;
box.max = this.mInternalBBox.max;
TSVector tSVector = FP.Half * (box.max - box.min);
TSVector tSVector2 = FP.Half * (box.max + box.min);
TSVector value;
TSVector.Transform(ref tSVector2, ref orientation, out value);
TSMatrix tSMatrix;
TSMath.Absolute(ref orientation, out tSMatrix);
TSVector value2;
TSVector.Transform(ref tSVector, ref tSMatrix, out value2);
box.max = value + value2;
box.min = value - value2;
}
public override void Iterate()
{
bool flag = this.skipConstraint;
if (!flag)
{
FP x = this.body1.linearVelocity * this.jacobian[0] + this.body1.angularVelocity * this.jacobian[1] + this.body2.linearVelocity * this.jacobian[2] + this.body2.angularVelocity * this.jacobian[3];
FP y = this.accumulatedImpulse * this.softnessOverDt;
FP fP = -this.effectiveMass * (x + this.bias + y);
bool flag2 = this.behavior == PointPointDistance.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 == PointPointDistance.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)
{
this.body1.linearVelocity += this.body1.inverseMass * fP * this.jacobian[0];
this.body1.angularVelocity += TSVector.Transform(fP * this.jacobian[1], this.body1.invInertiaWorld);
}
bool flag5 = !this.body2.isStatic;
if (flag5)
{
this.body2.linearVelocity += this.body2.inverseMass * fP * this.jacobian[2];
this.body2.angularVelocity += TSVector.Transform(fP * this.jacobian[3], this.body2.invInertiaWorld);
}
}
}
public override void Iterate()
{
TSVector value = this.body1.angularVelocity - this.body2.angularVelocity;
TSVector value2 = this.accumulatedImpulse * this.softnessOverDt;
TSVector tSVector = -FP.One * TSVector.Transform(value + this.bias + value2, this.effectiveMass);
this.accumulatedImpulse += tSVector;
bool flag = !this.body1.IsStatic;
if (flag)
{
this.body1.angularVelocity += TSVector.Transform(tSVector, this.body1.invInertiaWorld);
}
bool flag2 = !this.body2.IsStatic;
if (flag2)
{
this.body2.angularVelocity += TSVector.Transform(-FP.One * tSVector, this.body2.invInertiaWorld);
}
}
public override void PrepareForIteration(FP timestep)
{
this.effectiveMass = this.body1.invInertiaWorld + this.body2.invInertiaWorld;
this.softnessOverDt = this.softness / timestep;
this.effectiveMass.M11 = this.effectiveMass.M11 + this.softnessOverDt;
this.effectiveMass.M22 = this.effectiveMass.M22 + this.softnessOverDt;
this.effectiveMass.M33 = this.effectiveMass.M33 + this.softnessOverDt;
TSMatrix.Inverse(ref this.effectiveMass, out this.effectiveMass);
TSMatrix value;
TSMatrix.Multiply(ref this.initialOrientation1, ref this.initialOrientation2, out value);
TSMatrix.Transpose(ref value, out value);
TSMatrix tSMatrix = value * this.body2.invOrientation * this.body1.orientation;
FP fP = tSMatrix.M32 - tSMatrix.M23;
FP fP2 = tSMatrix.M13 - tSMatrix.M31;
FP fP3 = tSMatrix.M21 - tSMatrix.M12;
FP fP4 = TSMath.Sqrt(fP * fP + fP2 * fP2 + fP3 * fP3);
FP x = tSMatrix.M11 + tSMatrix.M22 + tSMatrix.M33;
FP value2 = FP.Atan2(fP4, x - 1);
TSVector value3 = new TSVector(fP, fP2, fP3) * value2;
bool flag = fP4 != FP.Zero;
if (flag)
{
value3 *= FP.One / fP4;
}
this.bias = value3 * this.biasFactor * (-FP.One / timestep);
bool flag2 = !this.body1.IsStatic;
if (flag2)
{
this.body1.angularVelocity += TSVector.Transform(this.accumulatedImpulse, this.body1.invInertiaWorld);
}
bool flag3 = !this.body2.IsStatic;
if (flag3)
{
this.body2.angularVelocity += TSVector.Transform(-FP.One * this.accumulatedImpulse, this.body2.invInertiaWorld);
}
}
public override void Iterate()
{
FP x = this.body1.linearVelocity * this.jacobian[0] + this.body1.angularVelocity * this.jacobian[1] + this.body2.linearVelocity * this.jacobian[2] + this.body2.angularVelocity * this.jacobian[3];
FP y = this.accumulatedImpulse * this.softnessOverDt;
FP fP = -this.effectiveMass * (x + this.bias + y);
this.accumulatedImpulse += fP;
bool flag = !this.body1.isStatic;
if (flag)
{
this.body1.linearVelocity += this.body1.inverseMass * fP * this.jacobian[0];
this.body1.angularVelocity += TSVector.Transform(fP * this.jacobian[1], this.body1.invInertiaWorld);
}
bool flag2 = !this.body2.isStatic;
if (flag2)
{
this.body2.linearVelocity += this.body2.inverseMass * fP * this.jacobian[2];
this.body2.angularVelocity += TSVector.Transform(fP * this.jacobian[3], this.body2.invInertiaWorld);
}
}
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);
}
public override void DebugDraw(IDebugDrawer drawer)
{
drawer.DrawLine(this.body1.position + this.r1, this.body1.position + this.r1 + TSVector.Transform(this.lineNormal, this.body1.orientation) * 100);
}
/**
* @brief Moves game object based on provided translation vector and a relative {@link TSTransform}.
*
* The game object will move based on TSTransform's forward vector.
**/
public void Translate(TSVector translation, TSTransform relativeTo)
{
this.position += TSVector.Transform(translation, TSMatrix.CreateFromQuaternion(relativeTo.rotation));
}
public static bool Pointcast(ISupportMappable support, ref TSMatrix orientation, ref TSVector position, ref TSVector point)
{
TSVector tSVector;
GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref point, out tSVector);
TSVector.Subtract(ref point, ref tSVector, out tSVector);
TSVector tSVector2;
support.SupportCenter(out tSVector2);
TSVector.Transform(ref tSVector2, ref orientation, out tSVector2);
TSVector.Add(ref position, ref tSVector2, out tSVector2);
TSVector.Subtract(ref point, ref tSVector2, out tSVector2);
TSVector p = point;
TSVector tSVector3;
TSVector.Subtract(ref p, ref tSVector, out tSVector3);
FP x = tSVector3.sqrMagnitude;
FP eN = FP.EN4;
int num = 15;
VoronoiSimplexSolver @new = GJKCollide.simplexSolverPool.GetNew();
@new.Reset();
bool result;
while (x > eN && num-- != 0)
{
TSVector q;
GJKCollide.SupportMapTransformed(support, ref orientation, ref position, ref tSVector3, out q);
TSVector w;
TSVector.Subtract(ref p, ref q, out w);
FP x2 = TSVector.Dot(ref tSVector3, ref w);
bool flag = x2 > FP.Zero;
if (flag)
{
FP x3 = TSVector.Dot(ref tSVector3, ref tSVector2);
bool flag2 = x3 >= -(TSMath.Epsilon * TSMath.Epsilon);
if (flag2)
{
GJKCollide.simplexSolverPool.GiveBack(@new);
result = false;
return(result);
}
@new.Reset();
}
bool flag3 = [email protected](w);
if (flag3)
{
@new.AddVertex(w, p, q);
}
bool flag4 = @new.Closest(out tSVector3);
if (flag4)
{
x = tSVector3.sqrMagnitude;
}
else
{
x = FP.Zero;
}
}
GJKCollide.simplexSolverPool.GiveBack(@new);
result = true;
return(result);
}
private void DetectRigidRigid(RigidBody body1, RigidBody body2)
{
bool flag = body1.Shape is Multishape;
bool flag2 = body2.Shape is Multishape;
bool flag3 = this.speculativeContacts || body1.EnableSpeculativeContacts || body2.EnableSpeculativeContacts;
bool flag4 = !flag && !flag2;
if (flag4)
{
TSVector tSVector;
TSVector value;
FP fP;
bool flag5 = XenoCollide.Detect(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out tSVector, out value, out fP);
if (flag5)
{
TSVector tSVector2;
TSVector tSVector3;
this.FindSupportPoints(body1, body2, body1.Shape, body2.Shape, ref tSVector, ref value, out tSVector2, out tSVector3);
this.RaiseCollisionDetected(body1, body2, ref tSVector2, ref tSVector3, ref value, fP);
}
else
{
bool flag6 = flag3;
if (flag6)
{
TSVector value2;
TSVector value3;
bool flag7 = GJKCollide.ClosestPoints(body1.Shape, body2.Shape, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out value2, out value3, out value);
if (flag7)
{
TSVector value4 = value3 - value2;
bool flag8 = value4.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude;
if (flag8)
{
fP = value4 * value;
bool flag9 = fP < FP.Zero;
if (flag9)
{
this.RaiseCollisionDetected(body1, body2, ref value2, ref value3, ref value, fP);
}
}
}
}
}
}
else
{
bool flag10 = flag & flag2;
if (flag10)
{
Multishape multishape = body1.Shape as Multishape;
Multishape multishape2 = body2.Shape as Multishape;
multishape = multishape.RequestWorkingClone();
multishape2 = multishape2.RequestWorkingClone();
TSBBox boundingBox = body2.boundingBox;
boundingBox.InverseTransform(ref body1.position, ref body1.orientation);
int num = multishape.Prepare(ref boundingBox);
boundingBox = body1.boundingBox;
boundingBox.InverseTransform(ref body2.position, ref body2.orientation);
int num2 = multishape2.Prepare(ref boundingBox);
bool flag11 = num == 0 || num2 == 0;
if (flag11)
{
multishape.ReturnWorkingClone();
multishape2.ReturnWorkingClone();
}
else
{
for (int i = 0; i < num; i++)
{
multishape.SetCurrentShape(i);
for (int j = 0; j < num2; j++)
{
multishape2.SetCurrentShape(j);
TSVector tSVector;
TSVector value;
FP fP;
bool flag12 = XenoCollide.Detect(multishape, multishape2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out tSVector, out value, out fP);
if (flag12)
{
TSVector tSVector4;
TSVector tSVector5;
this.FindSupportPoints(body1, body2, multishape, multishape2, ref tSVector, ref value, out tSVector4, out tSVector5);
this.RaiseCollisionDetected(body1, body2, ref tSVector4, ref tSVector5, ref value, fP);
}
else
{
bool flag13 = flag3;
if (flag13)
{
TSVector value5;
TSVector value6;
bool flag14 = GJKCollide.ClosestPoints(multishape, multishape2, ref body1.orientation, ref body2.orientation, ref body1.position, ref body2.position, out value5, out value6, out value);
if (flag14)
{
TSVector value7 = value6 - value5;
bool flag15 = value7.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude;
if (flag15)
{
fP = value7 * value;
bool flag16 = fP < FP.Zero;
if (flag16)
{
this.RaiseCollisionDetected(body1, body2, ref value5, ref value6, ref value, fP);
}
}
}
}
}
}
}
multishape.ReturnWorkingClone();
multishape2.ReturnWorkingClone();
}
}
else
{
bool flag17 = body2.Shape is Multishape;
RigidBody rigidBody;
RigidBody rigidBody2;
if (flag17)
{
rigidBody = body2;
rigidBody2 = body1;
}
else
{
rigidBody2 = body2;
rigidBody = body1;
}
Multishape multishape3 = rigidBody.Shape as Multishape;
multishape3 = multishape3.RequestWorkingClone();
TSBBox boundingBox2 = rigidBody2.boundingBox;
boundingBox2.InverseTransform(ref rigidBody.position, ref rigidBody.orientation);
int num3 = multishape3.Prepare(ref boundingBox2);
bool flag18 = num3 == 0;
if (flag18)
{
multishape3.ReturnWorkingClone();
}
else
{
for (int k = 0; k < num3; k++)
{
multishape3.SetCurrentShape(k);
TSVector tSVector;
TSVector value;
FP fP;
bool flag19 = XenoCollide.Detect(multishape3, rigidBody2.Shape, ref rigidBody.orientation, ref rigidBody2.orientation, ref rigidBody.position, ref rigidBody2.position, out tSVector, out value, out fP);
if (flag19)
{
TSVector tSVector6;
TSVector tSVector7;
this.FindSupportPoints(rigidBody, rigidBody2, multishape3, rigidBody2.Shape, ref tSVector, ref value, out tSVector6, out tSVector7);
bool flag20 = this.useTerrainNormal && multishape3 is TerrainShape;
if (flag20)
{
(multishape3 as TerrainShape).CollisionNormal(out value);
TSVector.Transform(ref value, ref rigidBody.orientation, out value);
}
else
{
bool flag21 = this.useTriangleMeshNormal && multishape3 is TriangleMeshShape;
if (flag21)
{
(multishape3 as TriangleMeshShape).CollisionNormal(out value);
TSVector.Transform(ref value, ref rigidBody.orientation, out value);
}
}
this.RaiseCollisionDetected(rigidBody, rigidBody2, ref tSVector6, ref tSVector7, ref value, fP);
}
else
{
bool flag22 = flag3;
if (flag22)
{
TSVector value8;
TSVector value9;
bool flag23 = GJKCollide.ClosestPoints(multishape3, rigidBody2.Shape, ref rigidBody.orientation, ref rigidBody2.orientation, ref rigidBody.position, ref rigidBody2.position, out value8, out value9, out value);
if (flag23)
{
TSVector value10 = value9 - value8;
bool flag24 = value10.sqrMagnitude < (body1.sweptDirection - body2.sweptDirection).sqrMagnitude;
if (flag24)
{
fP = value10 * value;
bool flag25 = fP < FP.Zero;
if (flag25)
{
this.RaiseCollisionDetected(rigidBody, rigidBody2, ref value8, ref value9, ref value, fP);
}
}
}
}
}
}
multishape3.ReturnWorkingClone();
}
}
}
}
