public void RigidBody_ForceOnCenterOfMass()
{
RigidBody body = new RigidBody(Vector3.Zero);
body.Awake = true;
body.Mass = 1f;
Helpers.AssertBodyAtRest(body);
body.addForce(Vector3.UnitZ);
body.integrate(1f);
Assert.AreEqual(Vector3.Zero, body.AngularMomentum);
Assert.AreEqual(Vector3.UnitZ, body.LinearMomentum);
Assert.AreEqual(new Vector3(0f, 0f, 1f), body.Position);
body.addForce(-Vector3.UnitZ * 2f);
body.integrate(1f);
Assert.AreEqual(Vector3.Zero, body.AngularMomentum);
Assert.AreEqual(-Vector3.UnitZ, body.LinearMomentum);
Assert.AreEqual(new Vector3(0f, 0f, 0f), body.Position);
body = new RigidBody(Vector3.Zero);
body.Awake = true;
body.Mass = 1f;
Helpers.AssertBodyAtRest(body);
body.addForce(-Vector3.UnitZ);
body.integrate(10f);
Assert.AreEqual(new Vector3(0f, 0f, -1000f), body.Position);
Assert.AreEqual(Vector3.Zero, body.AngularMomentum);
}
void IntegrateBody(RigidBody rigidBody, float duration, int numIterations)
{
for (int i = 0; i < numIterations; i++)
{
rigidBody.integrate(duration);
}
}
public void RigidBody_WorldMatrix()
{
RigidBody body = new RigidBody(Vector3.Zero);
body.Awake = true;
body.Mass = 1f;
float angle = (float)Math.PI;
Quaternion orientation = Quaternion.CreateFromYawPitchRoll(angle, 0f, 0f);
body.Orientation = orientation;
body.Position = Vector3.Up;
Matrix expectedWorld = new Matrix(
-1f, 0f, .00000008742278f, 0f,
0f, 1f, 0f, 0f,
-.00000008742278f, 0f, -1f, 0f,
0f, 1f, 0f, 1f
);
//integrate once for update
body.integrate(1f);
Assert.AreEqual(expectedWorld, body.World);
int total_iterations = 1000000;
for (int i = 1; i <= total_iterations; i++)
{
body.integrate(1);
Assert.AreEqual(expectedWorld, body.World, "Iteration: " + i);
}
Assert.AreEqual(expectedWorld, body.World);
}
public void RigidBody_Sleeping()
{
RigidBody body = new RigidBody(Vector3.Zero);
body.Mass = 1f; // it's allowed to move if it thinks it shoud...
//starts out awake
Assert.AreEqual(true, body.Awake);
Helpers.AssertBodyAtRest(body);
// Adding a force, wakes it up
body.addForce(Vector3.Up);
Assert.AreEqual(true, body.Awake);
// putting it to sleep before integration should bypass any movement calcs.
body.Awake = false;
body.integrate(1);
Helpers.AssertBodyAtRest(body);
}
public void RigidBody_InfiniteMass()
{
RigidBody body = new RigidBody(Vector3.Zero);
body.InverseMass = 0f;
Assert.AreEqual(float.MaxValue, body.Mass, "Mass");
Assert.False(body.HasFiniteMass, "HasFiniteMass");
Vector3 force = Vector3.Up;
Helpers.AssertBodyAtRest(body, Vector3.Zero);
//add force at center of mass
body.addForce(force);
body.integrate(1);
Helpers.AssertBodyAtRest(body);
//Now add force at point other than center of mass
Vector3 worldCoord = Vector3.Right;
body.addForce(force, worldCoord);
body.integrate(1);
Helpers.AssertBodyAtRest(body);
//Now let's show that infinitely massed objects don't respond to torque application
Vector3 torque = Vector3.Right;
body.addTorque(torque);
body.integrate(1);
Helpers.AssertBodyAtRest(body);
}
public void RigidBody_InertiaTensor()
{
RigidBody body = new RigidBody(Vector3.Zero);
body.Awake = true;
//simulating hollow sphere of mass and length
float radius = 2f;
float mass = 1f;
body.Mass = mass;
float sphere_coeff = ((1f / 12f) * mass * radius * radius);
Matrix inertiaTensor = new Matrix(
sphere_coeff, 0f, 0f, 0f,
0f, sphere_coeff, 0f, 0f,
0f, 0f, sphere_coeff, 0f,
0f, 0f, 0f, 1f
);
body.InertiaTensor = inertiaTensor;
Assert.AreEqual(Matrix.Identity, body.World, "Initial World");
Assert.AreEqual(inertiaTensor, body.InertiaTensor, "Initial Intertia Tensor");
Assert.AreEqual(Matrix.Invert(inertiaTensor), body.InverseInertiaTensor, "Initial InverseInertiaTensor");
//no bleed off
body.LinearDamping = 1f;
//adding a force once should set it moving, never to stop again
body.addForce(Vector3.UnitZ, body.Position + (new Vector3(-radius, 0f, 0f)));
//nothing physically happens until integration time
Assert.AreEqual(Vector3.Zero, body.Position, "Intitial Position");
Assert.AreEqual(Vector3.Zero, body.AngularMomentum, "Intial Rotation");
Assert.AreEqual(Quaternion.Identity, body.Orientation, "Intial Orientation");
body.integrate(1f);
Assert.AreEqual(Vector3.UnitZ * 1f, body.Position, "Computed Position");
Assert.AreEqual(new Vector3(0f, 2f, 0f), body.AngularMomentum, "Computed Rotation");
Quaternion orientation = new Quaternion(0f, 0.707106769f, 0f, 0.707106769f);
Assert.AreEqual(orientation.X, body.Orientation.X);
Assert.AreEqual(orientation.Y, body.Orientation.Y);
Assert.AreEqual(orientation.Z, body.Orientation.Z);
Assert.AreEqual(orientation.W, body.Orientation.W);
Assert.AreEqual(orientation, body.Orientation, "Computed Orientation");
}