public static TSQuaternion LerpUnclamped(TSQuaternion a, TSQuaternion b, FP t)
{
TSQuaternion result = TSQuaternion.Multiply(a, 1 - t) + TSQuaternion.Multiply(b, t);
result.Normalize();
return(result);
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector tSVector = TSVector.Cross(fromVector, toVector);
TSQuaternion result = new TSQuaternion(tSVector.x, tSVector.y, tSVector.z, TSVector.Dot(fromVector, toVector));
result.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
result.Normalize();
return(result);
}
public static TSQuaternion FromToRotation(TSVector fromVector, TSVector toVector)
{
TSVector w = TSVector.Cross(fromVector, toVector);
TSQuaternion q = new TSQuaternion(w.x, w.y, w.z, TSVector.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
internal void PostStep()
{
bool flag = this._freezeConstraints > TSRigidBodyConstraints.None;
if (flag)
{
bool flag2 = (this._freezeConstraints & TSRigidBodyConstraints.FreezePositionX) == TSRigidBodyConstraints.FreezePositionX;
bool flag3 = (this._freezeConstraints & TSRigidBodyConstraints.FreezePositionY) == TSRigidBodyConstraints.FreezePositionY;
bool flag4 = (this._freezeConstraints & TSRigidBodyConstraints.FreezePositionZ) == TSRigidBodyConstraints.FreezePositionZ;
bool flag5 = flag2;
if (flag5)
{
this.position.x = this._freezePosition.x;
}
bool flag6 = flag3;
if (flag6)
{
this.position.y = this._freezePosition.y;
}
bool flag7 = flag4;
if (flag7)
{
this.position.z = this._freezePosition.z;
}
bool flag8 = (this._freezeConstraints & TSRigidBodyConstraints.FreezeRotationX) == TSRigidBodyConstraints.FreezeRotationX;
bool flag9 = (this._freezeConstraints & TSRigidBodyConstraints.FreezeRotationY) == TSRigidBodyConstraints.FreezeRotationY;
bool flag10 = (this._freezeConstraints & TSRigidBodyConstraints.FreezeRotationZ) == TSRigidBodyConstraints.FreezeRotationZ;
bool flag11 = flag8 | flag9 | flag10;
if (flag11)
{
TSQuaternion quaternion = TSQuaternion.CreateFromMatrix(this.Orientation);
bool flag12 = flag8;
if (flag12)
{
quaternion.x = this._freezeRotationQuat.x;
}
bool flag13 = flag9;
if (flag13)
{
quaternion.y = this._freezeRotationQuat.y;
}
bool flag14 = flag10;
if (flag14)
{
quaternion.z = this._freezeRotationQuat.z;
}
quaternion.Normalize();
this.Orientation = TSMatrix.CreateFromQuaternion(quaternion);
}
}
}
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);
}
}
/**
* @brief Rotates game object based on provided axis angles and relative space.
*
* If relative space is SELF then the game object will rotate based on its forward vector.
**/
public void Rotate(TSVector eulerAngles, Space relativeTo)
{
TSQuaternion result = TSQuaternion.identity;
if (relativeTo == Space.Self)
{
result = this.rotation * TSQuaternion.Euler(eulerAngles);
}
else
{
result = TSQuaternion.Euler(eulerAngles) * this.rotation;
}
result.Normalize();
this.rotation = result;
}
/**
* @brief Rotates game object based on provided axis, angle of rotation and relative space.
*
* If relative space is SELF then the game object will rotate based on its forward vector.
**/
public void Rotate(TSVector axis, FP angle, Space relativeTo)
{
TSQuaternion result = TSQuaternion.identity;
if (relativeTo == Space.Self)
{
result = this.rotation * TSQuaternion.AngleAxis(angle, axis);
}
else
{
result = TSQuaternion.AngleAxis(angle, axis) * this.rotation;
}
result.Normalize();
this.rotation = result;
}
public override void PostStep()
{
TSVector position = base.Body1.Position;
position.z = 0;
base.Body1.Position = position;
TSQuaternion quaternion = TSQuaternion.CreateFromMatrix(base.Body1.Orientation);
quaternion.Normalize();
quaternion.x = 0;
quaternion.y = 0;
bool flag = this.freezeZAxis;
if (flag)
{
quaternion.z = 0;
}
base.Body1.Orientation = TSMatrix.CreateFromQuaternion(quaternion);
bool isStatic = base.Body1.isStatic;
if (!isStatic)
{
TSVector linearVelocity = base.Body1.LinearVelocity;
linearVelocity.z = 0;
base.Body1.LinearVelocity = linearVelocity;
TSVector angularVelocity = base.Body1.AngularVelocity;
angularVelocity.x = 0;
angularVelocity.y = 0;
bool flag2 = this.freezeZAxis;
if (flag2)
{
angularVelocity.z = 0;
}
base.Body1.AngularVelocity = angularVelocity;
}
}