public void IntersectionTests_BoxVsBox_Faces()
{
NoBody topBody = new NoBody(Matrix.CreateTranslation(new Vector3(1.5f, 2.0f, 0.25f)));
Box top = new Box(
topBody,
Matrix.Identity,
new Vector3(1f, 1f, 1f));
NoBody bottomBody =
new NoBody(Matrix.CreateTranslation(new Vector3(0f, 0f, 0f)));
Box bottom = new Box(
bottomBody,
Matrix.Identity,
new Vector3(1f, 1f, 1f));
CollisionData data = new CollisionData();
int contactsFound = IntersectionTests.boxAndBox(top, bottom, data);
Assert.AreEqual(1, contactsFound);
Contact contact = data.contacts[0];
Assert.AreEqual(new Vector3(1f, 1f, 1f), contact.Point);
Assert.AreEqual(Vector3.Up, contact.Normal);
Assert.AreEqual(0f, contact.Penetration);
}
public void IntersectionTests_BoxVsBox_NotTouching()
{
Vector3 leftPos = new Vector3(-100f, -100f, -100f);
IRigidBody leftBody = new NoBody(Matrix.CreateTranslation(leftPos));
Box left = new Box(leftBody, Matrix.Identity, new Vector3(1f, 1f, 1f));
Vector3 rightPos = new Vector3(100f, 100f, 100f);
IRigidBody rightBody = new NoBody(Matrix.CreateTranslation(rightPos));
Box right = new Box(rightBody, Matrix.Identity, new Vector3(1f, 1f, 1f));
CollisionData data = new CollisionData();
int contactsFound = IntersectionTests.boxAndBox(left, right, data);
Assert.AreEqual(0, contactsFound);
Assert.AreEqual(0, data.contacts.Count);
}
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");
}
public void IntersectionTests_SphereVsSphere_SurfaceCollision()
{
Vector3 sphereOnePosition = Vector3.Zero;
IRigidBody sphereOneBody = new NoBody(Matrix.CreateTranslation(sphereOnePosition));
float sphereOneRadius = 0.5f;
Matrix sphereOneMatrix = Matrix.Identity;
Sphere sphereOne = new Sphere(sphereOneBody, sphereOneMatrix, sphereOneRadius);
Vector3 sphereTwoPosition = Vector3.UnitX;
IRigidBody sphereTwoBody = new NoBody(Matrix.CreateTranslation(sphereTwoPosition));
float sphereTwoRadius = 0.5f;
Matrix sphereTwoMatrix = Matrix.Identity;
Sphere sphereTwo = new Sphere(sphereTwoBody, sphereTwoMatrix, sphereTwoRadius);
CollisionData data = new CollisionData();
Assert.AreEqual(1, IntersectionTests.sphereAndSphere(sphereOne, sphereTwo, data), "How many contacts?");
Contact contact;
Assert.NotNull(data.contacts, "Contacts Container");
Assert.AreEqual(1, data.contacts.Count, "One Contact Expected");
contact = data.contacts[0];
Assert.AreEqual(0, contact.Friction, "0 friction");
Assert.AreEqual(1f, contact.Restitution, "1 restitution");
Assert.AreEqual(0, contact.Penetration, "0 penetration (surface collision)");
Assert.AreEqual(new Vector3(0.5f, 0f, 0f), contact.Point, "expected contact point");
Assert.AreEqual(-Vector3.UnitX, contact.Normal, "expected contact normal");
//Assymetric
data = new CollisionData();
IntersectionTests.sphereAndSphere(sphereTwo, sphereOne, data);
Assert.NotNull(data.contacts, "Contacts Container");
Assert.AreEqual(1, data.contacts.Count, "One Contact Expected");
contact = data.contacts[0];
Assert.AreEqual(0, contact.Friction, "0 friction");
Assert.AreEqual(1, contact.Restitution, "1 restitution");
Assert.AreEqual(0, contact.Penetration, "0 penetration (surface collision)");
Assert.AreEqual(new Vector3(0.5f, 0f, 0f), contact.Point, "expected contact point");
Assert.AreEqual(Vector3.UnitX, contact.Normal, "expected contact normal");
}
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);
}
public void IntersectionTests_SphereVsSphere_NoIntersection()
{
IRigidBody sphereOneBody = new NoBody(Matrix.CreateTranslation(Vector3.UnitX * -2f));
float sphereOneRadius = 1f;
Matrix sphereOneMatrix = Matrix.Identity;
Sphere sphereOne = new Sphere(sphereOneBody, sphereOneMatrix, sphereOneRadius);
IRigidBody sphereTwoBody = new NoBody(Matrix.CreateTranslation(Vector3.UnitX * 2f));
float sphereTwoRadius = 1f;
Matrix sphereTwoMatrix = Matrix.Identity;
Sphere sphereTwo = new Sphere(sphereTwoBody, sphereTwoMatrix, sphereTwoRadius);
CollisionData data = new CollisionData();
Assert.AreEqual(0, IntersectionTests.sphereAndSphere(sphereOne, sphereTwo, data));
}
public void IntersectionTests_SphereVsSphere_BodyPointIntersection()
{
IRigidBody sphereOneBody = new NoBody();
float sphereOneRadius = 1f;
Matrix sphereOneMatrix = Matrix.Identity;
Sphere sphereOne = new Sphere(sphereOneBody, sphereOneMatrix, sphereOneRadius);
IRigidBody sphereTwoBody = new NoBody();
float sphereTwoRadius = 1f;
Matrix sphereTwoMatrix = Matrix.Identity;
Sphere sphereTwo = new Sphere(sphereTwoBody, sphereTwoMatrix, sphereTwoRadius);
CollisionData data = new CollisionData();
IntersectionTests.sphereAndSphere(sphereOne, sphereTwo, data);
}
public void IntersectionTests_BoxVsPoint()
{
IRigidBody body = new NoBody();
Box box;
Vector3 point;
CollisionData data;
Contact contact;
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
////////////////////////// point@+,+,+
point = new Vector3(0.75f, 1f, 1f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(Vector3.Left, contact.Normal);
Assert.AreEqual(0.25f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.Null(contact.Bodies[1]);
///////////////////////////// point@center when box is translated
Vector3 translatedPos = Vector3.UnitY * 10f;
body = new NoBody(Matrix.CreateTranslation(translatedPos));
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
point = translatedPos;
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(1f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.AreEqual(Vector3.Right, contact.Normal);
Assert.Null(contact.Bodies[1]);
/////////////////////////////////////point@+,+,+ when box is translated
point = translatedPos + Vector3.Right * 0.75f;
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(0.25f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Vector3 normal = Vector3.Left;
Assert.AreEqual(normal, contact.Normal);
Assert.Null(contact.Bodies[1]);
//////////////////////////////////////////point@tip(+,+,+)
body = new NoBody();
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
point = new Vector3(1f, 1f, 1f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(0f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.AreEqual(Vector3.Left, contact.Normal);
Assert.Null(contact.Bodies[1]);
/////////////////////////////////////////point@tip(-,-,-)
point = new Vector3(-1f, -1f, -1f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
contact = data.contacts[0];
Assert.AreEqual(0f, contact.Penetration);
Assert.AreEqual(1, data.contacts.Count);
Assert.AreEqual(point, contact.Point);
Assert.AreEqual(Vector3.Right, contact.Normal);
Assert.Null(contact.Bodies[1]);
/////////////////////////////////////////point@middle of nowhere
point = new Vector3(10000f, 10000f, 10000f);
data = new CollisionData();
data.contactsLeft = 1;
Assert.AreEqual(0, IntersectionTests.boxAndPoint(box, point, data));
/////////////////////////////////////////point@+,0,0 after box is rotated
float rotY = (float)Math.PI / 4f; //45 degrees
point = new Vector3(1.001f, 0f, 0f);
body = new NoBody(Matrix.CreateRotationY(rotY));
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
data = new CollisionData();
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
Assert.AreEqual(1, data.contacts.Count);
contact = data.contacts[0];
Assert.True(
TrickyMath.CloseEnoughToBeSame(contact.Normal, new Vector3(-0.7071068f, 0, 0.7071068f) )
);
/////////////////////////////////////////////////////point@+,0,0 after box is rotated other direction
point = new Vector3(1.001f, 0f, 0f);
body = new NoBody(Matrix.CreateRotationY(-rotY));
box = new Box(body, Matrix.Identity, new Vector3(1f, 1f, 1f));
data = new CollisionData();
Assert.AreEqual(1, IntersectionTests.boxAndPoint(box, point, data));
Assert.AreEqual(1, data.contacts.Count);
contact = data.contacts[0];
Assert.True(
TrickyMath.CloseEnoughToBeSame(contact.Normal, new Vector3(-0.7071068f, 0, -0.7071068f))
);
}
