/// <summary>
/// Detectar colision entre un Segmento de recta y una Capsula.
/// </summary>
/// <param name="seg">Segmento</param>
/// <param name="capsule">Capsula</param>
/// <param name="t">Menor instante de colision</param>
/// <returns>True si hay colision</returns>
private bool intersectSegmentCapsule(Segment seg, Capsule capsule, out float t)
{
TgcRay.RayStruct ray = new TgcRay.RayStruct();
ray.origin = seg.a;
ray.direction = seg.dir;
if (intersectRayCapsule(ray, capsule, out t))
{
if (t >= 0.0f && t <= seg.length)
{
t /= seg.length;
return true;
}
}
return false;
}
/// <summary>
/// Colisiona un Elipsoide en movimiento contra el BoundingBox.
/// Si hay colision devuelve el instante t de colision, el punto q de colision y el vector normal n de la superficie contra la que
/// se colisiona.
/// Todo se devuelve en Elipsoid space.
/// El BoundingBox se pasa a Elipsoid space para comparar.
/// </summary>
/// <param name="eSphere">BoundingSphere de radio 1 en Elipsoid space</param>
/// <param name="eMovementVector">movimiento en Elipsoid space</param>
/// <param name="eRadius">radio del Elipsoide</param>
/// <param name="movementSphere">BoundingSphere que abarca el sphere en su punto de origen mas el sphere en su punto final deseado</param>
/// <param name="t">Menor instante de colision, en Elipsoid space</param>
/// <param name="q">Punto mas cercano de colision, en Elipsoid space</param>
/// <param name="n">Vector normal de la superficie contra la que se colisiona</param>
/// <returns>True si hay colision</returns>
public override bool intersectMovingElipsoid(TgcBoundingSphere eSphere, Vector3 eMovementVector, Vector3 eRadius, TgcBoundingSphere movementSphere, out float t, out Vector3 q, out Vector3 n)
{
//Pasar AABB a Elipsoid Space
eAABB.setExtremes(
TgcVectorUtils.div(aabb.PMin, eRadius),
TgcVectorUtils.div(aabb.PMax, eRadius)
);
t = -1f;
q = Vector3.Empty;
n = Vector3.Empty;
// Compute the AABB resulting from expanding b by sphere radius r
TgcBoundingBox.AABBStruct e = eAABB.toStruct();
e.min.X -= eSphere.Radius; e.min.Y -= eSphere.Radius; e.min.Z -= eSphere.Radius;
e.max.X += eSphere.Radius; e.max.Y += eSphere.Radius; e.max.Z += eSphere.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 = eSphere.Center;
ray.direction = eMovementVector;
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 < eAABB.PMin.X)
{
sign[0] = -1;
i++;
}
if (p.X > eAABB.PMax.X)
{
sign[0] = 1;
i++;
}
if (p.Y < eAABB.PMin.Y)
{
sign[1] = -1;
i++;
}
if (p.Y > eAABB.PMax.Y)
{
sign[1] = 1;
i++;
}
if (p.Z < eAABB.PMin.Z)
{
sign[2] = -1;
i++;
}
if (p.Z > eAABB.PMax.Z)
{
sign[2] = 1;
i++;
}
//Face
if (i == 1)
{
n = new Vector3(sign[0], sign[1], sign[2]);
q = eSphere.Center + t * eMovementVector - eSphere.Radius * n;
return true;
}
// Define line segment [c, c+d] specified by the sphere movement
Segment seg = new Segment(eSphere.Center, eSphere.Center + eMovementVector);
//Box extent and center
Vector3 extent = eAABB.calculateAxisRadius();
Vector3 center = eAABB.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, eSphere.Radius), out t))
{
n = new Vector3(sign[0], sign[1], sign[2]);
n.Normalize();
q = eSphere.Center + t * eMovementVector - eSphere.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, eSphere.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, eSphere.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, eSphere.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 = eSphere.Center + t * eMovementVector - eSphere.Radius * n;
return true; // Intersection at time t == tmin
}
return false;
}
/// <summary>
/// Interseccion Ray-OBB.
/// Devuelve true y el punto q de colision si hay interseccion.
/// </summary>
public static bool intersectRayObb(TgcRay ray, TgcObb obb, out Vector3 q)
{
//Transformar Ray a OBB-space
Vector3 a = ray.Origin;
Vector3 b = ray.Origin + ray.Direction;
a = obb.toObbSpace(a);
b = obb.toObbSpace(b);
TgcRay.RayStruct ray2 = new TgcRay.RayStruct();
ray2.origin = a;
ray2.direction = Vector3.Normalize(b - a);
//Crear AABB que representa al OBB
Vector3 min = -obb.Extents;
Vector3 max = obb.Extents;
TgcBoundingBox.AABBStruct aabb = new TgcBoundingBox.AABBStruct();
aabb.min = min;
aabb.max = max;
//Hacer interseccion Ray-AABB
if (TgcCollisionUtils.intersectRayAABB(ray2, aabb, out q))
{
//Pasar q a World-Space
q = obb.toWorldSpace(q);
return true;
}
return false;
}
