//Point-Of-View Ray Casting collision detection for Boxes
public List<Contact> ProjectPOVRay(Box box, float RayLength)
{
List<Contact> Contacts = new List<Contact>();
Vector3 sideOffset = Vector3.Normalize(Vector3.Cross(this.Velocity, new Vector3(0, 1, 0)));
Vector3 side1 = this.Position - sideOffset;
Vector3 side2 = this.Position + sideOffset;
Vector3[] vertices = box.GetVertices();
Vector3[] normals = box.GetNormals();
float[] Distances = new float[6];
for (int i = 0; i < normals.Length; i++)
{
Distances[i] = Vector3.Dot(((box.Position + (normals[i] * box.Size / 2) - side1)), normals[i]) / Vector3.Dot(this.velocity, normals[i]);
}
//The number of faces whose normals are not pointing away from the sphere.
int PositiveCount = 0;
List<int> indices = new List<int>();
//Find shortest distance to Box Face
int shortestIndex = -1;
//Obtain list of faces whose normals are not pointing away from he sphere and the distance between the sphere and the closest box face
for (int i = 0; i < Distances.Length; i++)
{
if (Distances[i] > 0)
{
if (Distances[i] <= RayLength)
{
indices.Add(i);
PositiveCount++;
if (shortestIndex == -1)
{
shortestIndex = i;
}
else
{
if (Distances[shortestIndex] > Distances[i])
{
shortestIndex = i;
}
}
}
else
{
Distances[i] = -1;
}
}
else
{
Distances[i] = -1;
}
}
//Test For Left Ray
//Don't collide if many positives are return, false positive
if (PositiveCount > 0)
{
foreach (int index in indices)
{
//Check for validity with point intersecting the box
Vector3 RayPoint = side1 + velocity / velocity.Length() * Distances[index];
Vector3 min = new Vector3(vertices[0].X, vertices[0].Y, vertices[0].Z);
Vector3 max = new Vector3(vertices[0].X, vertices[0].Y, vertices[0].Z);
for (int i = 0; i < vertices.Length; i++)
{
if (min.X > vertices[i].X)
min.X = vertices[i].X;
if (min.Y > vertices[i].Y)
min.Y = vertices[i].Y;
if (min.Z > vertices[i].Z)
min.Z = vertices[i].Z;
if (max.X < vertices[i].X)
max.X = vertices[i].X;
if (max.Y < vertices[i].Y)
max.Y = vertices[i].Y;
if (max.Z < vertices[i].Z)
max.Z = vertices[i].Z;
}
double dSquared = 0;
if (RayPoint.X < min.X)
{
dSquared += (float)Math.Pow((RayPoint.X - min.X), 2);
}
else if (RayPoint.X > max.X)
{
dSquared += (float)Math.Pow((RayPoint.X - max.X), 2);
}
if (RayPoint.Y < min.Y)
{
dSquared += (float)Math.Pow((RayPoint.Y - min.Y), 2);
}
else if (RayPoint.Y > max.Y)
{
dSquared += (float)Math.Pow((RayPoint.Y - max.Y), 2);
}
if (RayPoint.Z < min.Z)
{
dSquared += (float)Math.Pow((RayPoint.Z - min.Z), 2);
}
else if (RayPoint.Z > max.Z)
{
dSquared += (float)Math.Pow((RayPoint.Z - max.Z), 2);
}
if (dSquared <= 0.01)
{
Contact contact = new Contact();
contact.ContactType = CollisionType.POVRay;
contact.RayCollided = RayCollisionResult.Left;
contact.ContactNormal = -velocity / Velocity.Length();
contact.ContactPoint = RayPoint;
contact.DeepestPoint = side1 + velocity / Velocity.Length() * RayLength;
contact.PenetrationDepth = (contact.DeepestPoint - contact.ContactPoint).Length();
//Compute contact angle
if (PositiveCount == 1)
{
float scalar = Vector3.Dot(normals[shortestIndex], velocity / velocity.Length());
contact.ContactAngle = (float)Math.Acos(scalar);
}
else
{
float scalar = Vector3.Dot(normals[shortestIndex], velocity / velocity.Length());
contact.ContactAngle = (float)Math.Acos(scalar);
}
//Angle should never be higher than 90 degrees.
if (contact.ContactAngle > Math.PI / 2)
{
contact.ContactAngle = (float)Math.PI - contact.ContactAngle;
}
Contacts.Add(contact);
break;
}
}
}
Distances = new float[6];
for (int i = 0; i < normals.Length; i++)
{
Distances[i] = Vector3.Dot(((box.Position + (normals[i] * box.Size / 2) - side2)), normals[i]) / Vector3.Dot(this.velocity, normals[i]);
}
//The number of faces whose normals are not pointing away from the sphere.
PositiveCount = 0;
indices = new List<int>();
//Obtain list of faces whose normals are not pointing away from he sphere and the distance between the sphere and the closest box face
shortestIndex = 0;
for (int i = 0; i < Distances.Length; i++)
{
if (Distances[i] > 0)
{
if (Distances[i] <= RayLength)
{
indices.Add(i);
PositiveCount++;
if (shortestIndex == -1)
{
shortestIndex = i;
}
else
{
if (Distances[shortestIndex] > Distances[i])
{
shortestIndex = i;
}
}
}
else
{
Distances[i] = -1;
}
}
else
{
Distances[i] = -1;
}
}
//Test For Right Ray
//Don't collide if many positives are return, false positive
if (PositiveCount > 0)
{
foreach (int index in indices)
{
//Check for validity with point intersecting the box
Vector3 RayPoint = side2 + velocity / velocity.Length() * Distances[index];
Vector3 min = new Vector3(vertices[0].X, vertices[0].Y, vertices[0].Z);
Vector3 max = new Vector3(vertices[0].X, vertices[0].Y, vertices[0].Z);
for (int i = 0; i < vertices.Length; i++)
{
if (min.X > vertices[i].X)
min.X = vertices[i].X;
if (min.Y > vertices[i].Y)
min.Y = vertices[i].Y;
if (min.Z > vertices[i].Z)
min.Z = vertices[i].Z;
if (max.X < vertices[i].X)
max.X = vertices[i].X;
if (max.Y < vertices[i].Y)
max.Y = vertices[i].Y;
if (max.Z < vertices[i].Z)
max.Z = vertices[i].Z;
}
double dSquared = 0;
if (RayPoint.X < min.X)
{
dSquared += (float)Math.Pow((RayPoint.X - min.X), 2);
}
else if (RayPoint.X > max.X)
{
dSquared += (float)Math.Pow((RayPoint.X - max.X), 2);
}
if (RayPoint.Y < min.Y)
{
dSquared += (float)Math.Pow((RayPoint.Y - min.Y), 2);
}
else if (RayPoint.Y > max.Y)
{
dSquared += (float)Math.Pow((RayPoint.Y - max.Y), 2);
}
if (RayPoint.Z < min.Z)
{
dSquared += (float)Math.Pow((RayPoint.Z - min.Z), 2);
}
else if (RayPoint.Z > max.Z)
{
dSquared += (float)Math.Pow((RayPoint.Z - max.Z), 2);
}
if (dSquared <= 0.01)
{
Contact contact = new Contact();
contact.ContactType = CollisionType.POVRay;
contact.RayCollided = RayCollisionResult.Right;
contact.ContactNormal = -velocity / Velocity.Length();
contact.ContactPoint = RayPoint;
contact.DeepestPoint = side2 + velocity / Velocity.Length() * RayLength;
contact.PenetrationDepth = (contact.DeepestPoint - contact.ContactPoint).Length();
//Compute contact angle
if (PositiveCount == 1)
{
float scalar = Vector3.Dot(normals[shortestIndex], velocity / velocity.Length());
contact.ContactAngle = (float)Math.Acos(scalar);
}
else
{
float scalar = Vector3.Dot(normals[shortestIndex], velocity / velocity.Length());
contact.ContactAngle = (float)Math.Acos(scalar);
}
//Angle should never be higher than 90 degrees.
if (contact.ContactAngle > Math.PI / 2)
{
contact.ContactAngle = (float)Math.PI - contact.ContactAngle;
}
Contacts.Add(contact);
break;
}
}
}
//If contacts exist, collisions were found else return nothing
if (Contacts.Count > 0)
{
return Contacts;
}
else
{
return null;
}
}
//Box --> Plane Collision Detection and Contact Info generation
//Returns null if no collision, a contact List object if a collision occurs
protected List<Contact> Collides(Box box, Plane plane)
{
List<Contact> contacts = new List<Contact>();
Vector3[] Vertices = box.GetVertices();
foreach (Vector3 vertex in Vertices)
{
float Distance = Vector3.Dot(plane.Normal, vertex - plane.Position) / plane.Normal.Length();
if (Distance <= 0)
{
Contact contact = new Contact();
contact.ContactType = CollisionType.VertexFace;
contact.ContactNormal = plane.Normal;
contact.PenetrationDepth = -Distance;
contact.ContactPoint = vertex;
contact.DeepestPoint = vertex;
contacts.Add(contact);
}
}
return contacts;
}
//Sphere --> Box Collision Detection and Contact Info generation
//Returns null if no collision, a contact object if a collision occurs
protected Contact Collides(Sphere sphere, Box box)
{
Contact contact = new Contact();
Vector3[] vertices = box.GetVertices();
//Implementation based on pages 644-645 of Geometric Tools for Computer Graphics [Philip J. Schneider & David H. Eberly, Morgan Kaufmann]
//Make sure the sphere is touching the box before continuing
//Distance from sphere to each of the box vertices
float[] VertexDistances = new float[8];
//Compute distance
for (int i = 0; i < vertices.Length; i++)
{
VertexDistances[i] = (vertices[i] - sphere.Position).Length();
}
//Obtain Minimum and Maximum values for X,Y,Z
Vector3 min = new Vector3(vertices[0].X, vertices[0].Y, vertices[0].Z);
Vector3 max = new Vector3(vertices[0].X, vertices[0].Y, vertices[0].Z);
for (int i = 0; i < vertices.Length; i++)
{
if (min.X > vertices[i].X)
min.X = vertices[i].X;
if (min.Y > vertices[i].Y)
min.Y = vertices[i].Y;
if (min.Z > vertices[i].Z)
min.Z = vertices[i].Z;
if (max.X < vertices[i].X)
max.X = vertices[i].X;
if (max.Y < vertices[i].Y)
max.Y = vertices[i].Y;
if (max.Z < vertices[i].Z)
max.Z = vertices[i].Z;
}
//Test for collision
double dSquared = 0;
if (sphere.Position.X < min.X)
{
dSquared += (float)Math.Pow((sphere.Position.X - min.X), 2);
}
else if (sphere.Position.X > max.X)
{
dSquared += (float)Math.Pow((sphere.Position.X - max.X), 2);
}
if (sphere.Position.Y < min.Y)
{
dSquared += (float)Math.Pow((sphere.Position.Y - min.Y), 2);
}
else if (sphere.Position.Y > max.Y)
{
dSquared += (float)Math.Pow((sphere.Position.Y - max.Y), 2);
}
if (sphere.Position.Z < min.Z)
{
dSquared += (float)Math.Pow((sphere.Position.Z - min.Z), 2);
}
else if (sphere.Position.Z > max.Z)
{
float test = sphere.Position.Z - max.Z;
dSquared += (float)Math.Pow((sphere.Position.Z - max.Z), 2);
}
if (dSquared <= Math.Pow(sphere.Radius, 2))
{
#region Vertex-Face
foreach (Vector3 vertex in vertices)
{
float Distance = (sphere.Position - vertex).Length();
if (Distance <= sphere.Radius)
{
Vector3 midline = vertex - sphere.Position;
contact.ContactType = CollisionType.VertexFace;
contact.ContactNormal = -midline / midline.Length();
contact.ContactPoint = vertex;// sphere.position + (midline * sphere.Radius) / midline.Length();
contact.DeepestPoint = sphere.Position + (midline * sphere.Radius) / midline.Length();
contact.PenetrationDepth = ((vertex - contact.DeepestPoint)).Length();
return contact;
}
}
#endregion
#region Face-Face
Vector3[] normals = box.GetNormals();
float[] Distances = new float[6];
for (int i = 0; i < normals.Length; i++)
{
Distances[i] = Vector3.Dot(normals[i], sphere.Position - (box.Position + (normals[i] * box.Size / 2))) / normals[i].Length();
}
int index = 0;
//The number of faces whose normals are not pointing away from the sphere.
int PositiveCount = 0;
for (int i = 0; i < Distances.Length; i++)
{
if (Distances[i] > 0)
{
if (Distances[i] <= sphere.Radius)
{
PositiveCount++;
}
if (Distances[index] > Distances[i] || Distances[index] < 0)
{
index = i;
}
}
}
if (Distances[index] <= sphere.Radius && PositiveCount == 1)
{
contact = new Contact();
contact.ContactType = CollisionType.FaceFace;
contact.ContactNormal = normals[index];
contact.PenetrationDepth = sphere.Radius - Distances[index]; //+
contact.ContactPoint = sphere.Position - contact.ContactNormal * VertexDistances[index];
contact.DeepestPoint = sphere.Position - contact.ContactNormal * (sphere.Radius);
return contact;
}
#endregion
#region Edge-Face
float[] PointDistances = new float[12];
Vector3[,] VertexPair = new Vector3[12, 2];
//Front Vertices
Vector3 x0 = sphere.Position;
Vector3 x1 = VertexPair[0, 0] = vertices[0];
Vector3 x2 = VertexPair[0, 1] = vertices[1];
PointDistances[0] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[1, 0] = vertices[1];
x2 = VertexPair[1, 1] = vertices[3];
PointDistances[1] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[2, 0] = vertices[3];
x2 = VertexPair[2, 1] = vertices[2];
PointDistances[2] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[3, 0] = vertices[2];
x2 = VertexPair[3, 1] = vertices[0];
PointDistances[3] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
//Back Vertices
x1 = VertexPair[4, 0] = vertices[4];
x2 = VertexPair[4, 1] = vertices[5];
PointDistances[4] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[5, 0] = vertices[5];
x2 = VertexPair[5, 1] = vertices[7];
PointDistances[5] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[6, 0] = vertices[7];
x2 = VertexPair[6, 1] = vertices[6];
PointDistances[6] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[7, 0] = vertices[6];
x2 = VertexPair[7, 1] = vertices[4];
PointDistances[7] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
//Side Vertices
x1 = VertexPair[8, 0] = vertices[0];
x2 = VertexPair[8, 1] = vertices[4];
PointDistances[8] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[9, 0] = vertices[1];
x2 = VertexPair[9, 1] = vertices[5];
PointDistances[9] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[10, 0] = vertices[2];
x2 = VertexPair[10, 1] = vertices[6];
PointDistances[10] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
x1 = VertexPair[11, 0] = vertices[3];
x2 = VertexPair[11, 1] = vertices[7];
PointDistances[11] = Vector3.Cross((x0 - x1), (x0 - x2)).Length() / (x2 - x1).Length();
index = 0;
for (int i = 0; i < PointDistances.Length; i++)
{
if (PointDistances[index] > PointDistances[i])
{
index = i;
}
}
if (PointDistances[index] < sphere.Radius)
{
x1 = VertexPair[index, 0];
x2 = VertexPair[index, 1];
//Required to compute point on the line
float t = -Vector3.Dot((x1 - x0), (x2 - x1)) / (x2 - x1).LengthSquared();
Vector3 LinePoint = new Vector3(x1.X + (x2.X - x1.X) * t, x1.Y + (x2.Y - x1.Y) * t, x1.Z + (x2.Z - x1.Z) * t);
contact.ContactType = CollisionType.EdgeFace;
contact.ContactNormal = Vector3.Normalize(x0 - LinePoint);
contact.ContactPoint = LinePoint;
contact.DeepestPoint = x0 - contact.ContactNormal / contact.ContactNormal.Length() * sphere.Radius;
contact.PenetrationDepth = (contact.DeepestPoint - contact.ContactPoint).Length();
return contact;
}
#endregion
}
//No Contact found...
return null;
}
//Sphere --> Plane Collision Detection and Contact Info generation
//Returns null if no collision, a contact object if a collision occurs
protected Contact Collides(Sphere sphere, Plane plane)
{
//Remove the absolute value to allow full penetration of the sphere
//float Distance = Vector3.Dot(plane.Normal, sphere.position - plane.position) / plane.Normal.Length();
float Distance = Math.Abs(Vector3.Dot(plane.Normal, sphere.Position - plane.Position)) / plane.Normal.Length();
if (Distance <= sphere.Radius)
{
Contact contact = new Contact();
contact.ContactType = CollisionType.FaceFace;
contact.ContactNormal = plane.Normal;
contact.PenetrationDepth = sphere.Radius - Distance;
contact.ContactPoint = sphere.Position - contact.ContactNormal * Distance;
contact.DeepestPoint = sphere.Position - contact.ContactNormal * (Distance + contact.PenetrationDepth);
return contact;
}
//No Collision
return null;
}
//Sphere --> Sphere Collision Detection and Contact Info generation
//Returns null if no collision, a contact object if a collision occurs
protected Contact Collides(Sphere sphere1, Sphere sphere2)
{
if ((sphere1.Position - sphere2.Position).Length() <= (sphere1.Radius + sphere2.Radius))
{
Contact contact = new Contact();
contact.ContactType = CollisionType.FaceFace;
contact.ContactNormal = (sphere1.Position - sphere2.Position) / (sphere1.Position - sphere2.Position).Length();
Vector3 Midline = (sphere1.Position - sphere2.Position);
contact.ContactPoint = sphere2.Position + (Midline / Midline.Length() * sphere2.Radius);
contact.DeepestPoint = (sphere1.Position - (Midline / Midline.Length() * sphere1.Radius));
contact.PenetrationDepth = sphere1.Radius + sphere2.Radius - Midline.Length();
return contact;
}
//No Collision
return null;
}
//Returns vertices to display for a single contact from the contact point to the deepest penetration point
public static VertexPositionColor[] GetPenetrationVector(Contact contact)
{
VertexPositionColor[] vertices = new VertexPositionColor[2];
vertices[0] = new VertexPositionColor(contact.DeepestPoint, Color.Yellow);
vertices[1] = new VertexPositionColor(contact.DeepestPoint + (contact.ContactNormal * contact.PenetrationDepth), Color.Yellow);
return vertices;
}
