public static bool CollisionTest(Ray ray, float[] heightmap, int columns, int rows, float height, out float dist)
{
const float TOLERANCE = 1.0e-8f;
// Find the entry point of the ray into the heightmap's AABB
AABB heightmapAABB = new AABB(Vector3.Zero, new Vector3((float)columns, (float)rows, height));
float exitDist;
if (!RayAABB.CollisionTestSmits(heightmapAABB, ray, out dist, out exitDist))
return false;
Vector3 direction = new Vector3(ray.I, ray.J, ray.K);
Vector3 entryPoint = ray.GetPoint(dist);
Vector3 exitPoint = ray.GetPoint(exitDist);
Vector3 delta = exitPoint - entryPoint;
float incX = (Math.Abs(delta.X) < TOLERANCE) ? 1.0f / TOLERANCE : 1.0f / Math.Abs(delta.X);
float incY = (Math.Abs(delta.Y) < TOLERANCE) ? 1.0f / TOLERANCE : 1.0f / Math.Abs(delta.Y);
// Heightmap coordinates
int x = (int)entryPoint.X;
int y = (int)entryPoint.Y;
int dx = (ray.I < 0.0f) ? -1 : (ray.I > 0.0f) ? 1 : 0;
int dy = (ray.J < 0.0f) ? -1 : (ray.J > 0.0f) ? 1 : 0;
float accumX = (delta.X < 0.0f) ? (entryPoint.X - (float)x) * incX : ((float)(x + 1) - entryPoint.X) * incX;
float accumY = (delta.Y < 0.0f) ? (entryPoint.Y - (float)y) * incY : ((float)(y + 1) - entryPoint.Y) * incY;
float t = 0.0f;
// Digital differential analyzer (DDA) loop over the heightmap
while (t <= 1.0f)
{
// TODO: We could further optimize this by testing if the current
// z value passes below HighestAlt(heightmap, columns, rows, x, y)
if (Intersects(entryPoint, direction, heightmap, columns, rows, x, y, out dist))
return true;
if (accumX < accumY)
{
t = accumX;
accumX += incX;
x += dx;
}
else
{
t = accumY;
accumY += incY;
y += dy;
}
}
return false;
}
public static bool CollisionTest(AABB box, Vector3 position, float radius)
{
// The center of the sphere relative to the center of the AABB
Vector3 sphereCenterRelBox = position - box.Center;
// The point on the surface of the box closest to the center of the sphere
Vector3 boxPoint;
float halfXLength = box.XLength * 0.5f;
float halfYLength = box.YLength * 0.5f;
float halfZLength = box.ZLength * 0.5f;
// X
if (sphereCenterRelBox.X < -halfXLength)
boxPoint.X = -halfXLength;
else if (sphereCenterRelBox.X > halfXLength)
boxPoint.X = halfXLength;
else
boxPoint.X = sphereCenterRelBox.X;
// Y
if (sphereCenterRelBox.Y < -halfYLength)
boxPoint.Y = -halfYLength;
else if (sphereCenterRelBox.Y > halfYLength)
boxPoint.Y = halfYLength;
else
boxPoint.Y = sphereCenterRelBox.Y;
// Z
if (sphereCenterRelBox.Z < -halfZLength)
boxPoint.Z = -halfZLength;
else if (sphereCenterRelBox.Z > halfZLength)
boxPoint.Z = halfZLength;
else
boxPoint.Z = sphereCenterRelBox.Z;
// Get the distance from the closest point on the box to the center of the
// sphere and test if it is less than the sphere radius
return Vector3.DistanceSquared(sphereCenterRelBox, boxPoint) < (radius * radius);
}
/// <summary>
/// A very fast Ray-AABB collision test that only returns true/false
/// </summary>
public static bool CollisionTestPluecker(AABB b, Ray r)
{
if (Contains(b, new Vector3(r.X, r.Y, r.Z)))
return true;
// This source code accompanies the Journal of Graphics Tools paper:
// "Fast Ray-Axis Aligned Bounding Box Overlap Tests With Pluecker Coordinates" by
// Jeffrey Mahovsky and Brian Wyvill
// Department of Computer Science, University of Calgary
// This source code is public domain, but please mention us if you use it.
switch (r.Type)
{
case Ray.RayType.MMM:
// side(R,HD) < 0 or side(R,FB) > 0 or side(R,EF) > 0 or side(R,DC) < 0 or side(R,CB) < 0 or side(R,HE) > 0 to miss
if ((r.X < b.Min.X) || (r.Y < b.Min.Y) || (r.Z < b.Min.Z) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Max.X < 0f) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Min.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Min.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Max.X < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Min.Z < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Max.Z > 0f))
return false;
return true;
case Ray.RayType.MMP:
// side(R,HD) < 0 or side(R,FB) > 0 or side(R,HG) > 0 or side(R,AB) < 0 or side(R,DA) < 0 or side(R,GF) > 0 to miss
if ((r.X < b.Min.X) || (r.Y < b.Min.Y) || (r.Z > b.Max.Z) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Max.X < 0f) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Min.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Max.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Min.X < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Min.Z < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Max.Z > 0f))
return false;
return true;
case Ray.RayType.MPM:
// side(R,EA) < 0 or side(R,GC) > 0 or side(R,EF) > 0 or side(R,DC) < 0 or side(R,GF) < 0 or side(R,DA) > 0 to miss
if ((r.X < b.Min.X) || (r.Y > b.Max.Y) || (r.Z < b.Min.Z) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Min.X < 0f) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Max.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Min.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Max.X < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Max.Z < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Min.Z > 0f))
return false;
return true;
case Ray.RayType.MPP:
// side(R,EA) < 0 or side(R,GC) > 0 or side(R,HG) > 0 or side(R,AB) < 0 or side(R,HE) < 0 or side(R,CB) > 0 to miss
if ((r.X < b.Min.X) || (r.Y > b.Max.Y) || (r.Z > b.Max.Z) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Min.X < 0f) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Max.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Max.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Min.X < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Max.Z < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Min.Z > 0f))
return false;
return true;
case Ray.RayType.PMM:
// side(R,GC) < 0 or side(R,EA) > 0 or side(R,AB) > 0 or side(R,HG) < 0 or side(R,CB) < 0 or side(R,HE) > 0 to miss
if ((r.X > b.Max.X) || (r.Y < b.Min.Y) || (r.Z < b.Min.Z) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Max.X < 0f) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Min.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Min.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Max.X < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Min.Z < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Max.Z > 0f))
return false;
return true;
case Ray.RayType.PMP:
// side(R,GC) < 0 or side(R,EA) > 0 or side(R,DC) > 0 or side(R,EF) < 0 or side(R,DA) < 0 or side(R,GF) > 0 to miss
if ((r.X > b.Max.X) || (r.Y < b.Min.Y) || (r.Z > b.Max.Z) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Max.X < 0f) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Min.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Max.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Min.X < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Min.Z < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Max.Z > 0f))
return false;
return true;
case Ray.RayType.PPM:
// side(R,FB) < 0 or side(R,HD) > 0 or side(R,AB) > 0 or side(R,HG) < 0 or side(R,GF) < 0 or side(R,DA) > 0 to miss
if ((r.X > b.Max.X) || (r.Y > b.Max.Y) || (r.Z < b.Min.Z) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Min.X < 0f) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Max.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Min.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Max.X < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Max.Z < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Min.Z > 0f))
return false;
return true;
case Ray.RayType.PPP:
// side(R,FB) < 0 or side(R,HD) > 0 or side(R,DC) > 0 or side(R,EF) < 0 or side(R,HE) < 0 or side(R,CB) > 0 to miss
if ((r.X > b.Max.X) || (r.Y > b.Max.Y) || (r.Z > b.Max.Z) ||
(r.R0 + r.I * b.Max.Y - r.J * b.Min.X < 0f) ||
(r.R0 + r.I * b.Min.Y - r.J * b.Max.X > 0f) ||
(r.R1 + r.I * b.Min.Z - r.K * b.Max.X > 0f) ||
(r.R1 + r.I * b.Max.Z - r.K * b.Min.X < 0f) ||
(r.R3 - r.K * b.Min.Y + r.J * b.Max.Z < 0f) ||
(r.R3 - r.K * b.Max.Y + r.J * b.Min.Z > 0f))
return false;
return true;
}
return false;
}
private static bool Contains(AABB a, Vector3 b)
{
return
b.X >= a.Min.X &&
b.Y >= a.Min.Y &&
b.Z >= a.Min.Z &&
b.X <= a.Max.X &&
b.Y <= a.Max.Y &&
b.Z <= a.Max.Z;
}
/// <summary>
/// A reasonably fast Ray-AABB collision test that returns true/false
/// and the collision point along the ray
/// </summary>
public static bool CollisionTestSmits(AABB b, Ray r, out float tNear, out float tFar)
{
// This source code accompanies the Journal of Graphics Tools paper:
// "Fast Ray-Axis Aligned Bounding Box Overlap Tests With Pluecker Coordinates" by
// Jeffrey Mahovsky and Brian Wyvill
// Department of Computer Science, University of Calgary
// This source code is public domain, but please mention us if you use it.
tNear = -1.0e6f;
tFar = 1.0e6f;
switch (r.Type)
{
case Ray.RayType.MMM:
{
// multiply by the inverse instead of dividing
float t1 = (b.Max.X - r.X) * r.II;
float t2 = (b.Min.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Y - r.Y) * r.IJ;
float t2 = (b.Min.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Z - r.Z) * r.IK;
float t2 = (b.Min.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
case Ray.RayType.MMP:
{
float t1 = (b.Max.X - r.X) * r.II;
float t2 = (b.Min.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Y - r.Y) * r.IJ;
float t2 = (b.Min.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Z - r.Z) * r.IK;
float t2 = (b.Max.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
case Ray.RayType.MPM:
{
float t1 = (b.Max.X - r.X) * r.II;
float t2 = (b.Min.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Y - r.Y) * r.IJ;
float t2 = (b.Max.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Z - r.Z) * r.IK;
float t2 = (b.Min.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
case Ray.RayType.MPP:
{
float t1 = (b.Max.X - r.X) * r.II;
float t2 = (b.Min.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Y - r.Y) * r.IJ;
float t2 = (b.Max.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Z - r.Z) * r.IK;
float t2 = (b.Max.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
case Ray.RayType.PMM:
{
float t1 = (b.Min.X - r.X) * r.II;
float t2 = (b.Max.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Y - r.Y) * r.IJ;
float t2 = (b.Min.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Z - r.Z) * r.IK;
float t2 = (b.Min.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
case Ray.RayType.PMP:
{
float t1 = (b.Min.X - r.X) * r.II;
float t2 = (b.Max.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Y - r.Y) * r.IJ;
float t2 = (b.Min.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Z - r.Z) * r.IK;
float t2 = (b.Max.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
case Ray.RayType.PPM:
{
float t1 = (b.Min.X - r.X) * r.II;
float t2 = (b.Max.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Y - r.Y) * r.IJ;
float t2 = (b.Max.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Max.Z - r.Z) * r.IK;
float t2 = (b.Min.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
case Ray.RayType.PPP:
{
float t1 = (b.Min.X - r.X) * r.II;
float t2 = (b.Max.X - r.X) * r.II;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Y - r.Y) * r.IJ;
float t2 = (b.Max.Y - r.Y) * r.IJ;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
{
float t1 = (b.Min.Z - r.Z) * r.IK;
float t2 = (b.Max.Z - r.Z) * r.IK;
if (t1 > tNear)
tNear = t1;
if (t2 < tFar)
tFar = t2;
if (tNear > tFar)
return false;
if (tFar < 0f)
return false;
}
tNear = Math.Max(tNear, 0.0f);
return true;
}
return false;
}