public static TSMatrix Inverse(TSMatrix matrix)
{
TSMatrix result;
TSMatrix.Inverse(ref matrix, out result);
return(result);
}
public void SetMassProperties(TSMatrix inertia, FP mass, bool setAsInverseValues)
{
if (setAsInverseValues)
{
bool flag = !this.isParticle;
if (flag)
{
this.invInertia = inertia;
TSMatrix.Inverse(ref inertia, out this.inertia);
}
this.inverseMass = mass;
}
else
{
bool flag2 = !this.isParticle;
if (flag2)
{
this.inertia = inertia;
TSMatrix.Inverse(ref inertia, out this.invInertia);
}
this.inverseMass = FP.One / mass;
}
this.useShapeMassProperties = false;
this.Update();
}
public void SetMassProperties()
{
this.inertia = this.Shape.inertia;
TSMatrix.Inverse(ref this.inertia, out this.invInertia);
this.inverseMass = FP.One / this.Shape.mass;
this.useShapeMassProperties = true;
}
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)
{
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);
}
}
private void UpdatePlayMode()
{
if (tsParent != null)
{
_localPosition = tsParent.InverseTransformPoint(position);
TSMatrix matrix = TSMatrix.CreateFromQuaternion(tsParent.rotation);
_localRotation = TSQuaternion.CreateFromMatrix(TSMatrix.Inverse(matrix)) * rotation;
}
else
{
_localPosition = position;
_localRotation = rotation;
}
if (rb != null)
{
transform.position = position.ToVector();
transform.rotation = rotation.ToQuaternion();
transform.localScale = localScale.ToVector();
/*if (rb.interpolation == TSRigidBody.InterpolateMode.Interpolate) {
* transform.position = Vector3.Lerp(transform.position, position.ToVector(), Time.deltaTime * DELTA_TIME_FACTOR);
* transform.rotation = Quaternion.Lerp(transform.rotation, rotation.ToQuaternion(), Time.deltaTime * DELTA_TIME_FACTOR);
* transform.localScale = Vector3.Lerp(transform.localScale, localScale.ToVector(), Time.deltaTime * DELTA_TIME_FACTOR);
* return;
* } else if (rb.interpolation == TSRigidBody.InterpolateMode.Extrapolate) {
* transform.position = (position + rb.tsCollider.Body.TSLinearVelocity * Time.deltaTime * DELTA_TIME_FACTOR).ToVector();
* transform.rotation = Quaternion.Lerp(transform.rotation, rotation.ToQuaternion(), Time.deltaTime * DELTA_TIME_FACTOR);
* transform.localScale = Vector3.Lerp(transform.localScale, localScale.ToVector(), Time.deltaTime * DELTA_TIME_FACTOR);
* return;
* }*/
}
transform.position = position.ToVector();
transform.rotation = rotation.ToQuaternion();
transform.localScale = localScale.ToVector();
_scale = transform.lossyScale.ToTSVector();
}
