public RigidBodyEntity(Vector3 pos)
{
body = new RigidBody(pos);
debug_compass = new DebugCompass(this.CenterOfMass);
debug_compass.LoadContent(null);
}
public ModelEntity CreateCube(Vector3 position, float mass, float dimension)
{
Model model = Models[CollideType.Box];
float size = dimension;
RigidBody body = new RigidBody(position);
body.LinearDamping = LinearDamping;
body.AngularDamping = AngularDamping;
body.Awake = true;
body.Mass = mass;
body.InertiaTensor = InertiaTensorFactory.Cubiod(mass, size, size, size);
float halfSize = size;
Box prim = new Box(body, Matrix.Identity, new Vector3(halfSize, halfSize, halfSize));
EntityGeometryData geoData = new EntityGeometryData();
geoData.Prim = prim;
geoData.BoundingSphere = new BoundingSphere(Vector3.Zero, halfSize * TrickyMath.Sqrt(2f));
return new ModelEntity(model, body, geoData);
}
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);
}
public static RigidBody CreateSphereBody(float mass, float radius)
{
RigidBody body = new RigidBody(Vector3.Zero);
body.Awake = true;
//simulating hollow sphere of mass and length
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, sphere_coeff
);
body.InertiaTensor = inertiaTensor;
body.LinearDamping = 1f;
body.AngularDamping = 1f;
return body;
}
// Just checks that body isn't moving and has default values
public static void AssertBodyAtRest(RigidBody body, Vector3 expectedPosition = new Vector3())
{
Assert.AreEqual(expectedPosition, body.CenterOfMass, "Center of Mass");
Assert.AreEqual(expectedPosition, body.Position, "Position");
Assert.AreEqual(Vector3.Zero, body.Velocity, "Velocity");
Assert.AreEqual(Vector3.Zero, body.Acceler, "Acceleration");
Assert.AreEqual(Quaternion.Identity, body.Orientation, "Orientation");
Assert.AreEqual(1f, body.LinearDamping, "LinearDamping");
Assert.AreEqual(1f, body.AngularDamping, "AngularDamping");
Assert.AreEqual(Matrix.Identity, body.World, "World Matrix");
}
void IntegrateBody(RigidBody rigidBody, float duration, int numIterations)
{
for (int i = 0; i < numIterations; i++)
{
rigidBody.integrate(duration);
}
}
public void RigidBody_Zero_Case()
{
RigidBody body = new RigidBody(Vector3.Zero);
//starts off awake
Assert.AreEqual(true, body.Awake);
//default starting state
Helpers.AssertBodyAtRest(body);
}
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_Translate()
{
RigidBody body = new RigidBody(Vector3.Zero);
Helpers.AssertBodyAtRest(body);
body.Translate(Vector3.Up);
Helpers.AssertBodyAtRest(body, Vector3.Up);
body.Translate(Vector3.Up);
Helpers.AssertBodyAtRest(body, Vector3.Up*2f);
}
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");
}
public ModelEntity CreateDefaultFromModel(Model model, Vector3 pos)
{
AABBTree tree = AABBFactory.AABBTree(model);
BoundingBox box = tree.Root.BBox;
float radius = BoundingSphere.CreateFromBoundingBox(box).Radius;
float mass = 1f;
RigidBody body = new RigidBody(pos);
body.LinearDamping = LinearDamping;
body.AngularDamping = AngularDamping;
body.Awake = true;
body.Mass = mass;
body.InertiaTensor = InertiaTensorFactory.Sphere(mass, radius);
Sphere prim = new Sphere(body, Matrix.Identity, radius);
EntityGeometryData geoData = new EntityGeometryData();
geoData.BoundingSphere = new BoundingSphere(Vector3.Zero, radius);
geoData.Prim = prim;
return new ModelEntity(model, body, geoData);
}
public ModelEntity CreateSphere(Vector3 position, float mass, float dimension)
{
Model model = Models[CollideType.Sphere];
float radius = dimension;
RigidBody body = new RigidBody(position);
body.LinearDamping = LinearDamping;
body.AngularDamping = AngularDamping;
body.Awake = true;
body.Mass = mass;
body.InertiaTensor = InertiaTensorFactory.Sphere(mass, radius);
Sphere prim = new Sphere(body, Matrix.Identity, radius);
EntityGeometryData geoData = new EntityGeometryData();
geoData.Prim = prim;
geoData.BoundingSphere = new BoundingSphere(Vector3.Zero, prim.Radius);
return new ModelEntity(model, body, geoData);
}
protected void SetupRenderContextAndCamera()
{
ICamera cam = new EyeCamera();
renderContext = new RenderContext(
cam,
this.ScreenManager.Game.GraphicsDevice
);
float radius = 2f;
float mass = 1f;
CamBody = new RigidBody(cam.Position);
CamBody.Label = "Camera Body";
CamBody.Awake = true;
CamBody.LinearDamping = 0.9f;
CamBody.AngularDamping = 0.7f;
CamBody.Mass = mass;
CamBody.InertiaTensor = InertiaTensorFactory.Sphere(mass, radius);
CamControls = new UserControls(PlayerIndex.One, 0.000015f, 0.25f, 0.0003f, 1f);
}
