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");
}
public static int sphereAndHalfSpace(Sphere sphere, Plane plane, CollisionData data)
{
Vector3 pos = sphere.Body.Position;
//d = p * L - l
float ballDist = Vector3.Dot(plane.Normal, pos) - (sphere.Radius - plane.Offset);
if (ballDist > 0) return 0;
Contact contact = new Contact();
contact.Normal = plane.Normal;
contact.Penetration = -ballDist;
contact.Point = pos - (plane.Normal * (ballDist + sphere.Radius));
contact.Bodies[0] = sphere.Body;
contact.Bodies[1] = null;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
public void IntersectionTests_BoxVsPlane()
{
IRigidBody planeBody = new NoBody();
Box box;
Forever.Physics.Collide.Plane plane;
CollisionData data;
Vector3 planeNormal = Vector3.Up;
// Plane is x-z plane for all tests
//////// unit box is sitting at {0,1,0} (surface touching)
box = new Box(new NoBody( Matrix.CreateTranslation(new Vector3(0f, 1f, 0f))), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
int contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(4, contactsFound);
Contact contact1 = data.contacts[0];
Contact contact2 = data.contacts[1];
Contact contact3 = data.contacts[2];
Contact contact4 = data.contacts[3];
Assert.AreEqual(new Vector3(-1f, 0f, -1f), contact1.Point);
Assert.AreEqual(planeNormal, contact1.Normal);
Assert.AreEqual(0, contact1.Penetration);
Assert.AreEqual(new Vector3(-1f, 0f, 1f), contact2.Point);
Assert.AreEqual(planeNormal, contact2.Normal);
Assert.AreEqual(0, contact2.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, -1f), contact3.Point);
Assert.AreEqual(planeNormal, contact3.Normal);
Assert.AreEqual(0, contact3.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, 1f), contact4.Point);
Assert.AreEqual(planeNormal, contact4.Normal);
Assert.AreEqual(0, contact4.Penetration);
//////// unit box is centered at origin
box = new Box(new NoBody(Matrix.CreateTranslation(new Vector3(0f, 0f, 0f))), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(4, contactsFound);
contact1 = data.contacts[0];
contact2 = data.contacts[1];
contact3 = data.contacts[2];
contact4 = data.contacts[3];
Assert.AreEqual(new Vector3(-1f, 0f, -1f), contact1.Point);
Assert.AreEqual(planeNormal, contact1.Normal);
Assert.AreEqual(1f, contact1.Penetration);
Assert.AreEqual(new Vector3(-1f, 0f, 1f), contact2.Point);
Assert.AreEqual(planeNormal, contact2.Normal);
Assert.AreEqual(1f, contact2.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, -1f), contact3.Point);
Assert.AreEqual(planeNormal, contact3.Normal);
Assert.AreEqual(1f, contact3.Penetration);
Assert.AreEqual(new Vector3(1f, 0f, 1f), contact4.Point);
Assert.AreEqual(planeNormal, contact4.Normal);
Assert.AreEqual(1f, contact4.Penetration);
//// unit box centered at y=1 and rotated 45 degrees on each axis
float angle = (float)Math.PI * 0.25f;
Matrix boxMatrix = Matrix.CreateFromYawPitchRoll(angle, angle, angle) * Matrix.CreateTranslation(new Vector3(0f, 1f, 0f));
box = new Box(new NoBody(boxMatrix), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0.7071067f, contact1.Penetration)
);
Assert.True(
TrickyMath.CloseEnoughToBeSame(
new Vector3(-0.2071069f, 0f, -0.2071068f),
contact1.Point));
////unit box centered at y=sqrt(2) and rotated 45 degrees on each axis
boxMatrix = Matrix.CreateFromYawPitchRoll(angle, angle, angle) * Matrix.CreateTranslation(new Vector3(0f, TrickyMath.Sqrt(2), 0f));
box = new Box(new NoBody(boxMatrix), Matrix.Identity, new Vector3(1f, 1f, 1f));
plane = new Forever.Physics.Collide.Plane(planeBody, Vector3.Zero, planeNormal);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndPlane(box, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0.292893142f,contact1.Penetration)
);
Assert.True(
TrickyMath.CloseEnoughToBeSame(
new Vector3(-0.2071069f, 0f, -0.2071068f),
contact1.Point
)
);
Assert.AreEqual(Vector3.UnitY, contact1.Normal);
}
/// <summary>
/// danger! This works for ContactDemo, but there is one hang up. The call to Math.Abs() should not
/// be needed. It isn't in the orange book text. However, we only get negative numbers from that Dot.
/// </summary>
/// <param name="box"></param>
/// <param name="plane"></param>
/// <param name="data"></param>
/// <returns></returns>
public static int boxAndPlane(Box box, Plane plane, CollisionData data)
{
if (data.contactsLeft <= 0) return 0;
int contacts_found = 0;
// p dot L < l
Vector3[] verts = box.WorldVerts();
foreach (Vector3 vert in verts)
{
//float vertexDist = (float)Math.Abs( (double)Vector3.Dot(plane.Normal, vert));
float vertexDist = Vector3.Dot(-plane.Normal, vert);
if (vertexDist >= plane.Offset + data.tolerance)
{
// the contact point is halfway between the vertex and the plane
// we multiply the direction by half the spearation distance
// and add the vertex location
Contact contact = new Contact();
contact.DiscoveryHint = ContactDiscoveryHint.BoxOnPlane_Corner;
contact.Bodies[0] = box.Body;
contact.Bodies[1] = null;
contact.Point = plane.ClosestPoint(vert);
contact.Normal = plane.Normal;
contact.Penetration = vertexDist - plane.Offset;//TrickyMathHelper.Abs(plane.Offset - vertexDist);
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
contacts_found++;
}
}
return contacts_found;
}
public void IntersectionTests_SphereVsPlane()
{
Matrix sphereMatrix;
float sphereRadius = 1f;
Forever.Physics.Collide.Plane plane;
Forever.Physics.Collide.Sphere sphere;
CollisionData data;
int contactsFound;
Contact contact1;
// plane is always the x-z plane for this test method
Vector3 planeNormal = Vector3.Up;
plane = new Forever.Physics.Collide.Plane(new NoBody(), Vector3.Zero, planeNormal);
// unit sphere centered at y=1 (one point of surface touching)
sphereMatrix = Matrix.CreateTranslation(Vector3.UnitY);
sphere = new Sphere( new NoBody(sphereMatrix), Matrix.Identity, sphereRadius);
data = new CollisionData();
contactsFound = IntersectionTests.sphereAndHalfSpace(sphere, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.AreEqual(Vector3.Zero, contact1.Point);
Assert.AreEqual(0f, contact1.Penetration);
Assert.AreEqual(planeNormal, contact1.Normal);
// unit sphere centered at y=0.9 (.1 penetration)
sphereMatrix = Matrix.CreateTranslation(Vector3.UnitY * 0.9f);
sphere = new Sphere(new NoBody(sphereMatrix), Matrix.Identity, sphereRadius);
data = new CollisionData();
contactsFound = IntersectionTests.sphereAndHalfSpace(sphere, plane, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.AreEqual(Vector3.Zero, contact1.Point);
Assert.True(
TrickyMath.AlmostEquals(0.1f, contact1.Penetration)
);
Assert.AreEqual(planeNormal, contact1.Normal);
}