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);
}
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 static int sphereAndSphere(Sphere one, Sphere two, CollisionData data)
{
//make sure we have contacts
if (data.contactsLeft <= 0) return 0;
// Cache the sphere positions
Vector3 positionOne = one.Body.Position;
Vector3 positionTwo = two.Body.Position;
if (positionOne == positionTwo)
{
throw new ArgumentException("Two bodies with the same position are unsupported....TODO");
}
// find the vector between the objects
Vector3 midline = positionOne - positionTwo;
float size = midline.Length();
//see if it is large enough
if (size <= 0.0f || size > one.Radius + two.Radius)
{
return 0;
}
// we manually create the normal, because we have the size at hand;
Vector3 normal = midline * (1f / size);
Contact contact = new Contact();
contact.Normal = normal;
contact.Point = positionTwo + midline * 0.5f;
contact.Penetration = (one.Radius + two.Radius - size);
//write the appropriate data
contact.Bodies[0] = one.Body;
contact.Bodies[1] = two.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 1;
}
public static void Render(Sphere sphere, RenderContext context, Color color)
{
BoundingSphere bs = new BoundingSphere(sphere.Body.Position, sphere.Radius);
BoundingSphereRenderer.Render(bs, context, color);
}
public static int boxAndSphere(Box box, Sphere sph, CollisionData data)
{
Vector3 center = sph.Body.Position;
Vector3 relCenter = Vector3.Transform(center, Matrix.Invert(box.Transform));
double x = relCenter.X;
double y = relCenter.Y;
double z = relCenter.Z;
if (Math.Abs(x) - sph.Radius > box.HalfSizes.X
|| Math.Abs(y) - sph.Radius > box.HalfSizes.Y
|| Math.Abs(z) - sph.Radius > box.HalfSizes.Z)
{
return 0;
}
Vector3 closest = Vector3.Zero;
float dist;
dist = relCenter.X;
if (dist > box.HalfSizes.X) dist = box.HalfSizes.X;
if (dist < -box.HalfSizes.X) dist = -box.HalfSizes.X;
closest.X = dist;
dist = relCenter.Y;
if (dist > box.HalfSizes.Y) dist = box.HalfSizes.Y;
if (dist < -box.HalfSizes.Y) dist = -box.HalfSizes.Y;
closest.Y = dist;
dist = relCenter.Z;
if (dist > box.HalfSizes.Z) dist = box.HalfSizes.Z;
if (dist < -box.HalfSizes.Z) dist = -box.HalfSizes.Z;
closest.Z = dist;
dist = (closest - relCenter).Length();
if (dist > sph.Radius && !TrickyMath.AlmostEquals(0f, dist - sph.Radius)) return 0;
Vector3 closestWorld = Vector3.Transform(closest, box.Transform);
Contact contact = new Contact();
Vector3 normal = closestWorld - center;// (center - closestWorld);
normal.Normalize();
contact.Normal = normal;
Debug.Sanity(contact.Normal);
contact.Point = closestWorld;
contact.Penetration = sph.Radius - dist;// (float)Math.Sqrt((float)dist);
contact.Bodies[0] = box.Body;
contact.Bodies[1] = sph.Body;
contact.Restitution = data.restitution;
contact.Friction = data.friction;
data.contacts.Add(contact);
return 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_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_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_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_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);
}
public void IntersectionTests_BoxVsSphere_NoIntersection()
{
float sphereRadius = 1f;
CollisionData data;
int contactsFound;
Contact contact1;
Forever.Physics.Collide.Sphere sphere;
Forever.Physics.Collide.Box box;
// no intersection
sphere = new Sphere(
new NoBody( Matrix.CreateTranslation(new Vector3(-10000f, -10000f, -10000f))),
Matrix.Identity,
sphereRadius
);
box = new Box(
new NoBody(Matrix.CreateTranslation(new Vector3(10000f, 10000f, 10000f))),
Matrix.Identity,
new Vector3(1f, 1f, 1f)
);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);
Assert.AreEqual(0, contactsFound);
}
public void IntersectionTests_BoxVsSphere_BoxFace()
{
//box face intersection
float sphereRadius = 1f;
CollisionData data;
int contactsFound;
Contact contact1;
Forever.Physics.Collide.Sphere sphere;
Forever.Physics.Collide.Box box;
sphere = new Sphere(
new NoBody(Matrix.CreateTranslation(Vector3.Up)),
Matrix.Identity,
sphereRadius);
box = new Box(
new NoBody(Matrix.CreateTranslation(Vector3.Down * 0.95f)),
Matrix.Identity,
new Vector3(1f, 1f, 1f));
data = new CollisionData();
contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0.05f, contact1.Penetration)
);
Assert.True(
TrickyMath.CloseEnoughToBeSame(new Vector3(0f, 0.05f, 0f), contact1.Point)
);
Assert.AreEqual(Vector3.Down, contact1.Normal);
}
public void IntersectionTests_BoxVsSphere_BoxEdge_ZeroPenetration()
{
//box edge intersection
float sphereRadius = 1f;
CollisionData data;
int contactsFound;
Contact contact1;
Forever.Physics.Collide.Sphere sphere;
Forever.Physics.Collide.Box box;
box = new Box(
new NoBody(), //box centered at origin
Matrix.Identity,
new Vector3(1f, 1f, 1f)
);
Vector3 spherePos = new Vector3(0f, 1f, 1f);
spherePos.Normalize();
spherePos = new Vector3(0f, 1f, 1f) + spherePos;
sphere = new Sphere(
new NoBody(
Matrix.CreateTranslation(spherePos)
),
Matrix.Identity,
sphereRadius);
data = new CollisionData();
contactsFound = IntersectionTests.boxAndSphere(box, sphere, data);
Assert.AreEqual(1, contactsFound);
contact1 = data.contacts[0];
Assert.True(
TrickyMath.AlmostEquals(0f, contact1.Penetration)
);
Assert.AreEqual(new Vector3(0f, 1f, 1f), contact1.Point);
Assert.True(
TrickyMath.CloseEnoughToBeSame(
new Vector3(0f, -0.7071068f, -0.7071068f),
contact1.Normal)
);
}