/// <summary>
/// Interseccion entre un Ray y un AABB
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="aabb">AABB</param>
/// <param name="tmin">Instante minimo de colision</param>
/// <param name="q">Punto minimo de colision</param>
/// <returns>True si hay colision</returns>
private bool intersectRayAABB(TgcRay.RayStruct ray, TgcBoundingBox.AABBStruct aabb, out float tmin,
out Vector3 q)
{
var aabbMin = TgcCollisionUtils.toArray(aabb.min);
var aabbMax = TgcCollisionUtils.toArray(aabb.max);
var p = TgcCollisionUtils.toArray(ray.origin);
var d = TgcCollisionUtils.toArray(ray.direction);
tmin = 0.0f; // set to -FLT_MAX to get first hit on line
var tmax = float.MaxValue; // set to max distance ray can travel (for segment)
q = Vector3.Empty;
// For all three slabs
for (var i = 0; i < 3; i++)
{
if (FastMath.Abs(d[i]) < float.Epsilon)
{
// Ray is parallel to slab. No hit if origin not within slab
if (p[i] < aabbMin[i] || p[i] > aabbMax[i])
{
return(false);
}
}
else
{
// Compute intersection t value of ray with near and far plane of slab
var ood = 1.0f / d[i];
var t1 = (aabbMin[i] - p[i]) * ood;
var t2 = (aabbMax[i] - p[i]) * ood;
// Make t1 be intersection with near plane, t2 with far plane
if (t1 > t2)
{
TgcCollisionUtils.swap(ref t1, ref t2);
}
// Compute the intersection of slab intersection intervals
tmin = TgcCollisionUtils.max(tmin, t1);
tmax = TgcCollisionUtils.min(tmax, t2);
// Exit with no collision as soon as slab intersection becomes empty
if (tmin > tmax)
{
return(false);
}
}
}
// Ray intersects all 3 slabs. Return point (q) and intersection t value (tmin)
q = ray.origin + ray.direction * tmin;
return(true);
}
/// <summary>
/// Colisiona un BoundingSphere en movimiento contra el BoundingBox.
/// Si hay colision devuelve el instante t de colision mas proximo y el punto q de colision mas cercano
/// </summary>
/// <param name="sphere">BoundingSphere</param>
/// <param name="movementVector">movimiento del BoundingSphere</param>
/// <param name="t">Menor instante de colision</param>
/// <param name="q">Punto mas cercano de colision</param>
/// <param name="n">Normal de la cara colisionada</param>
/// <returns>True si hay colision</returns>
public override bool intersectMovingSphere(TgcBoundingSphere sphere, Vector3 movementVector, TgcBoundingSphere movementSphere, out float t, out Vector3 q, out Vector3 n)
{
t = -1f;
q = Vector3.Empty;
n = Vector3.Empty;
// Compute the AABB resulting from expanding b by sphere radius r
TgcBoundingBox.AABBStruct e = aabb.toStruct();
e.min.X -= sphere.Radius; e.min.Y -= sphere.Radius; e.min.Z -= sphere.Radius;
e.max.X += sphere.Radius; e.max.Y += sphere.Radius; e.max.Z += sphere.Radius;
// Intersect ray against expanded AABB e. Exit with no intersection if ray
// misses e, else get intersection point p and time t as result
Vector3 p;
TgcRay.RayStruct ray = new TgcRay.RayStruct();
ray.origin = sphere.Center;
ray.direction = movementVector;
if (!intersectRayAABB(ray, e, out t, out p) || t > 1.0f)
{
return(false);
}
// Compute which min and max faces of b the intersection point p lies
// outside of. Note, u and v cannot have the same bits set and
// they must have at least one bit set among them
int i = 0;
int[] sign = new int[3];
if (p.X < aabb.PMin.X)
{
sign[0] = -1;
i++;
}
if (p.X > aabb.PMax.X)
{
sign[0] = 1;
i++;
}
if (p.Y < aabb.PMin.Y)
{
sign[1] = -1;
i++;
}
if (p.Y > aabb.PMax.Y)
{
sign[1] = 1;
i++;
}
if (p.Z < aabb.PMin.Z)
{
sign[2] = -1;
i++;
}
if (p.Z > aabb.PMax.Z)
{
sign[2] = 1;
i++;
}
//Face
if (i == 1)
{
n = new Vector3(sign[0], sign[1], sign[2]);
q = sphere.Center + t * movementVector - sphere.Radius * n;
return(true);
}
// Define line segment [c, c+d] specified by the sphere movement
Segment seg = new Segment(sphere.Center, sphere.Center + movementVector);
//Box extent and center
Vector3 extent = aabb.calculateAxisRadius();
Vector3 center = aabb.PMin + extent;
//Edge
if (i == 2)
{
//Generar los dos puntos extremos del Edge
float[] extentDir = new float[] { sign[0], sign[1], sign[2] };
int zeroIndex = sign[0] == 0 ? 0 : (sign[1] == 0 ? 1 : 2);
extentDir[zeroIndex] = 1;
Vector3 capsuleA = center + new Vector3(extent.X * extentDir[0], extent.Y * extentDir[1], extent.Z * extentDir[2]);
extentDir[zeroIndex] = -1;
Vector3 capsuleB = center + new Vector3(extent.X * extentDir[0], extent.Y * extentDir[1], extent.Z * extentDir[2]);
//Colision contra el Edge hecho Capsula
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
n = new Vector3(sign[0], sign[1], sign[2]);
n.Normalize();
q = sphere.Center + t * movementVector - sphere.Radius * n;
return(true);
}
}
//Vertex
if (i == 3)
{
float tmin = float.MaxValue;
Vector3 capsuleA = center + new Vector3(extent.X * sign[0], extent.Y * sign[1], extent.Z * sign[2]);
Vector3 capsuleB;
capsuleB = center + new Vector3(extent.X * -sign[0], extent.Y * sign[1], extent.Z * sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
capsuleB = center + new Vector3(extent.X * sign[0], extent.Y * -sign[1], extent.Z * sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
capsuleB = center + new Vector3(extent.X * sign[0], extent.Y * sign[1], extent.Z * -sign[2]);
if (intersectSegmentCapsule(seg, new Capsule(capsuleA, capsuleB, sphere.Radius), out t))
{
tmin = TgcCollisionUtils.min(t, tmin);
}
if (tmin == float.MaxValue)
{
return(false); // No intersection
}
t = tmin;
n = new Vector3(sign[0], sign[1], sign[2]);
n.Normalize();
q = sphere.Center + t * movementVector - sphere.Radius * n;
return(true); // Intersection at time t == tmin
}
return(false);
}