public static void Multiply(ref Transform lhs, ref Transform rhs, out Transform result)
{
result = new Transform();
Matrix.Multiply(ref rhs.Orientation, ref lhs.Orientation, out result.Orientation);
//result.Orientation = rhs.Orientation * lhs.Orientation;
Vector3.Transform(ref rhs.Position, ref lhs.Orientation, out result.Position);
Vector3.Add(ref lhs.Position, ref result.Position, out result.Position);
//result.Position = lhs.Position + Vector3.Transform(rhs.Position, lhs.Orientation);
}
public static Transform Multiply(Transform lhs, Transform rhs)
{
Transform result = new Transform();
Matrix.Multiply(ref rhs.Orientation, ref lhs.Orientation, out result.Orientation);
//result.Orientation = rhs.Orientation * lhs.Orientation;
Vector3.Transform(ref rhs.Position, ref lhs.Orientation, out result.Position);
Vector3.Add(ref lhs.Position, ref result.Position, out result.Position);
//result.Position = lhs.Position + Vector3.Transform(rhs.Position, lhs.Orientation);
return result;
}
/// <summary>
/// Copies the current position etc to old - normally called only
/// by tPhysicsSystem.
/// </summary>
public void CopyCurrentStateToOld()
{
oldTransform = transform;
oldTransformRate = transformRate;
}
/// <summary>
/// Copy our current state (position, velocity etc) into the stored state
/// </summary>
public void StoreState()
{
storedTransform = transform;
storedTransformRate = transformRate;
}
/// <summary>
/// Restore from the stored state into our current state.
/// </summary>
public void RestoreState()
{
transform = storedTransform;
transformRate = storedTransformRate;
#region REFERENCE: invOrientation = Matrix4.Transpose(transform.Orientation);
Matrix4.Transpose(ref transform.Orientation, out invOrientation);
#endregion
//worldInvInertia = transform.Orientation * bodyInvInertia * invOrientation;
#region REFERENCE: worldInvInertia = invOrientation *bodyInvInertia* transform.Orientation;
Matrix4Extensions.Multiply(ref invOrientation, ref bodyInvInertia, out worldInvInertia);
Matrix4Extensions.Multiply(ref worldInvInertia, ref transform.Orientation, out worldInvInertia);
#endregion
//worldInertia = transform.Orientation * bodyInertia * invOrientation;
#region REFERENCE: worldInertia = invOrientation *bodyInertia * transform.Orientation;
Matrix4Extensions.Multiply(ref invOrientation, ref bodyInertia, out worldInertia);
Matrix4Extensions.Multiply(ref worldInertia, ref transform.Orientation, out worldInertia);
#endregion
}
/// <summary>
/// Constructor
/// </summary>
public Body()
{
this.ID = idCounter++;
bodiesToBeActivatedOnMovement = new List<Body>();
bodyEnabled = false;
collSkin = null;
this.Mass = 1.0f;
this.BodyInertia = Matrix4.Identity;
transform = new Transform();
transform.Position = Vector3.Zero;
transform.Orientation = Matrix4.Identity;
immovable = false;
origImmovable = false;
doShockProcessing = true;
force = Vector3.Zero;
torque = Vector3.Zero;
velChanged = true;
activity = Activity.Active;
inactiveTime = 0.0f;
deactivationTime = 1.0f;
SetActivityThreshold(0.5f, 20.0f);
allowFreezing = true;
lastPositionForDeactivation = transform.Position;
lastOrientationForDeactivation = transform.Orientation;
CopyCurrentStateToOld();
}
/// <summary>
/// SetTransform
/// </summary>
/// <param name="transformOld"></param>
/// <param name="transformNew"></param>
public void SetTransform(ref Transform transformOld, ref Transform transformNew)
{
this.transformOld = transformOld;
this.transformNew = transformNew;
for (int prim = primitivesNewWorld.Count; prim-- != 0; )
{
primitivesOldWorld[prim].Transform = transformOld * primitivesLocal[prim].Transform;
primitivesNewWorld[prim].Transform = transformNew * primitivesLocal[prim].Transform;
}
UpdateWorldBoundingBox();
if (collSystem != null)
collSystem.CollisionSkinMoved(this);
}
/// <summary>
/// Gets called during the collision detection
/// </summary>
/// <param name="transform"></param>
public void SetOldTransform(ref Transform transform)
{
transformOld = transform;
Transform t;
for (int prim = primitivesNewWorld.Count; prim-- != 0; )
{
t = primitivesLocal[prim].Transform;
primitivesOldWorld[prim].Transform = transform * t;
}
UpdateWorldBoundingBox();
if (collSystem != null)
collSystem.CollisionSkinMoved(this);
}
/// <summary>
/// Applies a transform to the local primitives (e.g. to shift
/// everything after calculating CoM etc)
/// </summary>
/// <param name="transform"></param>
public void ApplyLocalTransform(Transform transform)
{
Transform t;
for (int prim = primitivesNewWorld.Count; prim-- != 0; )
{
t = primitivesLocal[prim].Transform;
primitivesLocal[prim].Transform = transform * t;
}
SetTransform(ref transformOld, ref transformNew);
}
public void UpdateCollisionShapeTransforms(ref Vector3 position, ref Matrix orientation)
{
var transform = new Transform(position, orientation);
ApplyLocalTransform(transform);
}