public void Contact_XAxis()
{
NoBody bodyZero = new NoBody(Vector3.Right);
bodyZero.Velocity = Vector3.Left;
bodyZero.Mass = 1f;
bodyZero.calculateDerivedData();
NoBody bodyOne = new NoBody(Vector3.Left * .9f);
bodyOne.Velocity = Vector3.Right;
bodyOne.Mass = 1f;
bodyOne.calculateDerivedData();
Contact contact = new Contact();
contact.Bodies[0] = bodyZero;
contact.Bodies[1] = bodyOne;
contact.Point = Vector3.Zero;
contact.Penetration = 0.1f;
contact.Restitution = 1f;
contact.Friction = 0f;
contact.Normal = Vector3.Right;
contact.ReCalc(1f);
Assert.AreEqual(Vector3.Left, contact.CalcLocalVelocity(0, 1f));
Assert.AreEqual(Vector3.Right, contact.CalcLocalVelocity(1, 1f));
Assert.AreEqual(Vector3.Left, contact.RelativeContactPositions[0]);
Assert.AreEqual(Vector3.Right * 0.9f, contact.RelativeContactPositions[1]);
Assert.AreEqual(new Vector3(-2f, 0f, 0f), contact.ContactVelocity);
// Position Change
Vector3[] linearChange = new Vector3[2];
Vector3[] angularChange = new Vector3[2];
contact.ApplyPositionChange(
ref linearChange, ref angularChange, contact.Penetration);
Assert.AreEqual(Vector3.Zero, angularChange[0], "Zero angular change for object 0");
Assert.AreEqual(Vector3.Zero, angularChange[1], "Zero angular change for object 1");
Assert.AreEqual(new Vector3(0.05f, 0f, 0f), linearChange[0],
"Body 0 is pushed right by half penetration");
Assert.AreEqual(new Vector3(-0.05f, 0f, 0f), linearChange[1],
"Body 1 is pushed left by half penetration");
//Velocity Change
Vector3[] velocityChange = new Vector3[2];
Vector3[] rotationChange = new Vector3[2];
contact.ApplyVelocityChange(ref velocityChange, ref rotationChange);
Assert.AreEqual(
Vector3.Zero,
rotationChange[0],
"Zero rotation change for object 0");
Assert.AreEqual(
Vector3.Zero,
rotationChange[1],
"Zero rotation change for object 1");
Assert.AreEqual(
Vector3.Right * 2f,
velocityChange[0],
"Counter velocity applied to body 0");
Assert.AreEqual(
Vector3.Left * 2f,
velocityChange[1],
"Counter velocity applied to body 1");
Assert.AreEqual(
Vector3.Right,
bodyZero.Velocity);
Assert.AreEqual(Vector3.Zero, bodyZero.Rotation);
Assert.AreEqual(
Vector3.Left,
bodyOne.Velocity);
Assert.AreEqual(Vector3.Zero, bodyOne.Rotation);
}
public void Contact_BoxSpinningOnYAxisFallsOnToXZPlane()
{
// X-Z plane
NoBody planeBody = new NoBody(Vector3.Zero);
planeBody.InverseMass = 0f; //infinite mass
Forever.Physics.Collide.Plane plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, Vector3.Up);
planeBody.calculateDerivedData();
// Spinning box colliding with the plane
NoBody boxBody = new NoBody(Vector3.Up * 0.95f);
boxBody.Mass = 1f;
boxBody.Rotation = new Vector3(0f, 0.01f, 0f);
boxBody.Velocity = new Vector3(0f, -1f, 0f);
boxBody.calculateDerivedData();
Box box = new Box(boxBody, Matrix.Identity, new Vector3(1f, 1f, 1f));
Contact contact = new Contact();
contact.Bodies[0] = planeBody;
contact.Bodies[1] = boxBody;
contact.Point = Vector3.Zero;
contact.Penetration = 0.05f;
contact.Restitution = 1f;
contact.Friction = 0f;
contact.Normal = Vector3.Up;
contact.ReCalc(1f);
Assert.AreEqual(Vector3.Zero, contact.CalcLocalVelocity(0, 1f));
Assert.AreEqual(Vector3.Left, contact.CalcLocalVelocity(1, 1f));
Assert.AreEqual(Vector3.Zero, contact.RelativeContactPositions[0]);
Assert.AreEqual(Vector3.Down * 0.95f, contact.RelativeContactPositions[1]);
Assert.AreEqual(Vector3.Right, contact.ContactVelocity);
// Position Change
Vector3[] linearChange = new Vector3[2];
Vector3[] angularChange = new Vector3[2];
contact.ApplyPositionChange(
ref linearChange, ref angularChange, contact.Penetration);
Assert.AreEqual(Vector3.Zero, angularChange[0], "Zero angular change for object 0");
Assert.AreEqual(Vector3.Zero, angularChange[1], "Zero angular change for object 1");
Assert.AreEqual(new Vector3(0.0f, 0f, 0f), linearChange[0],
"Body 0 is not pushed at all because it has infinite mass");
Assert.AreEqual(new Vector3(0f, -0.05f, 0f), linearChange[1],
"Body 1 is pushed up (forward in contact direction) by half penetration");
//Velocity Change
Vector3[] velocityChange = new Vector3[2];
Vector3[] rotationChange = new Vector3[2];
contact.ApplyVelocityChange(ref velocityChange, ref rotationChange);
Assert.AreEqual(
Vector3.Zero,
rotationChange[0],
"Zero rotation change for object 0");
Assert.AreEqual(
Vector3.Zero,
rotationChange[1],
"Zero rotation change for object 1");
Assert.AreEqual(
Vector3.Zero,
velocityChange[0],
"Zero velocity applied to body 0");
Assert.AreEqual(
Vector3.Up * 2f,
velocityChange[1],
"Counter velocity applied to body 1");
}
