/// <summary>
/// Colisiona un Elipsoide en movimiento contra un conjunto de triangulos.
/// 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.
/// Pasa cada triangulo a Elipsoid space para hacer el testeo.
/// </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="minT">Menor instante de colision, en Elipsoid space</param>
/// <param name="minQ">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 minT, out Vector3 minQ, out Vector3 n)
{
minQ = Vector3.Empty;
minT = float.MaxValue;
n = Vector3.Empty;
var collisionPlane = Plane.Empty;
//Colision contra cada triangulo del collider, quedarse con el menor
Vector3 q;
float t;
for (var i = 0; i < Triangles.Length; i++)
{
var triangle = Triangles[i];
//Primero hacer un Sphere-Sphere test
if (TgcCollisionUtils.testSphereSphere(movementSphere, triangle.BoundingSphere))
{
//Pasar triangle a Elipsoid Space
var eTriangle = new Triangle(
TgcVectorUtils.div(triangle.A, eRadius),
TgcVectorUtils.div(triangle.B, eRadius),
TgcVectorUtils.div(triangle.C, eRadius),
null
);
//Interseccion Moving Sphere-Triangle
if (intersectMovingSphereTriangle(eSphere, eMovementVector, eTriangle, out t, out q))
{
if (t < minT)
{
minT = t;
minQ = q;
collisionPlane = triangle.Plane;
}
}
}
}
if (minT != float.MaxValue)
{
n = TgcCollisionUtils.getPlaneNormal(collisionPlane);
return(true);
}
return(false);
}
/// <summary>
/// Colisiona un BoundingSphere en movimiento contra todos los triangulos del Collider.
/// 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 del triangulo colisionado</param>
/// <returns>True si hay colision</returns>
public override bool intersectMovingSphere(TgcBoundingSphere sphere, TGCVector3 movementVector,
TgcBoundingSphere movementSphere, out float minT, out TGCVector3 minQ, out TGCVector3 n)
{
minQ = TGCVector3.Empty;
minT = float.MaxValue;
n = TGCVector3.Empty;
var collisionPlane = TGCPlane.Zero;
//Colision contra cada triangulo del collider, quedarse con el menor
TGCVector3 q;
float t;
for (var i = 0; i < Triangles.Length; i++)
{
var triangle = Triangles[i];
//Primero hacer un Sphere-Sphere test
if (TgcCollisionUtils.testSphereSphere(movementSphere, triangle.BoundingSphere))
{
//Interseccion Moving Sphere-Triangle
if (intersectMovingSphereTriangle(sphere, movementVector, triangle, out t, out q))
{
if (t < minT)
{
minT = t;
minQ = q;
collisionPlane = triangle.Plane;
}
}
}
}
if (minT != float.MaxValue)
{
n = TgcCollisionUtils.getPlaneNormal(collisionPlane);
return(true);
}
return(false);
}
/// <summary>
/// Detección de colisiones recursiva
/// </summary>
public void doCollideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector, List <TgcBoundingBox> obstaculos, int recursionDepth)
{
//Limitar recursividad
if (recursionDepth > 5)
{
return;
}
//Ver si la distancia a recorrer es para tener en cuenta
float distanceToTravelSq = movementVector.LengthSq();
if (distanceToTravelSq < EPSILON)
{
return;
}
//Posicion deseada
Vector3 originalSphereCenter = characterSphere.Center;
Vector3 nextSphereCenter = originalSphereCenter + movementVector;
//Buscar el punto de colision mas cercano de todos los objetos candidatos
float minCollisionDistSq = float.MaxValue;
Vector3 realMovementVector = movementVector;
TgcBoundingBox.Face collisionFace = null;
TgcBoundingBox collisionObstacle = null;
Vector3 nearestPolygonIntersectionPoint = Vector3.Empty;
foreach (TgcBoundingBox obstaculoBB in obstaculos)
{
//Obtener los polígonos que conforman las 6 caras del BoundingBox
TgcBoundingBox.Face[] bbFaces = obstaculoBB.computeFaces();
foreach (TgcBoundingBox.Face bbFace in bbFaces)
{
Vector3 pNormal = TgcCollisionUtils.getPlaneNormal(bbFace.Plane);
TgcRay movementRay = new TgcRay(originalSphereCenter, movementVector);
float brutePlaneDist;
Vector3 brutePlaneIntersectionPoint;
if (!TgcCollisionUtils.intersectRayPlane(movementRay, bbFace.Plane, out brutePlaneDist, out brutePlaneIntersectionPoint))
{
continue;
}
float movementRadiusLengthSq = Vector3.Multiply(movementVector, characterSphere.Radius).LengthSq();
if (brutePlaneDist * brutePlaneDist > movementRadiusLengthSq)
{
continue;
}
//Obtener punto de colisión en el plano, según la normal del plano
float pDist;
Vector3 planeIntersectionPoint;
Vector3 sphereIntersectionPoint;
TgcRay planeNormalRay = new TgcRay(originalSphereCenter, -pNormal);
bool embebbed = false;
bool collisionFound = false;
if (TgcCollisionUtils.intersectRayPlane(planeNormalRay, bbFace.Plane, out pDist, out planeIntersectionPoint))
{
//Ver si el plano está embebido en la esfera
if (pDist <= characterSphere.Radius)
{
embebbed = true;
//TODO: REVISAR ESTO, caso embebido a analizar con más detalle
sphereIntersectionPoint = originalSphereCenter - pNormal * characterSphere.Radius;
}
//Esta fuera de la esfera
else
{
//Obtener punto de colisión del contorno de la esfera según la normal del plano
sphereIntersectionPoint = originalSphereCenter - Vector3.Multiply(pNormal, characterSphere.Radius);
//Disparar un rayo desde el contorno de la esfera hacia el plano, con el vector de movimiento
TgcRay sphereMovementRay = new TgcRay(sphereIntersectionPoint, movementVector);
if (!TgcCollisionUtils.intersectRayPlane(sphereMovementRay, bbFace.Plane, out pDist, out planeIntersectionPoint))
{
//no hay colisión
continue;
}
}
//Ver si planeIntersectionPoint pertenece al polígono
Vector3 newMovementVector;
float newMoveDistSq;
Vector3 polygonIntersectionPoint;
if (pointInBounbingBoxFace(planeIntersectionPoint, bbFace))
{
if (embebbed)
{
//TODO: REVISAR ESTO, nunca debería pasar
//throw new Exception("El polígono está dentro de la esfera");
}
polygonIntersectionPoint = planeIntersectionPoint;
collisionFound = true;
}
else
{
//Buscar el punto mas cercano planeIntersectionPoint que tiene el polígono real de esta cara
polygonIntersectionPoint = TgcCollisionUtils.closestPointRectangle3d(planeIntersectionPoint,
bbFace.Extremes[0], bbFace.Extremes[1], bbFace.Extremes[2]);
//Revertir el vector de velocidad desde el nuevo polygonIntersectionPoint para ver donde colisiona la esfera, si es que llega
Vector3 reversePointSeg = polygonIntersectionPoint - movementVector;
if (TgcCollisionUtils.intersectSegmentSphere(polygonIntersectionPoint, reversePointSeg, characterSphere, out pDist, out sphereIntersectionPoint))
{
collisionFound = true;
}
}
if (collisionFound)
{
//Nuevo vector de movimiento acotado
newMovementVector = polygonIntersectionPoint - sphereIntersectionPoint;
newMoveDistSq = newMovementVector.LengthSq();
if (newMoveDistSq <= distanceToTravelSq && newMoveDistSq < minCollisionDistSq)
{
minCollisionDistSq = newMoveDistSq;
realMovementVector = newMovementVector;
nearestPolygonIntersectionPoint = polygonIntersectionPoint;
collisionFace = bbFace;
collisionObstacle = obstaculoBB;
}
}
}
}
}
//Si nunca hubo colisión, avanzar todo lo requerido
if (collisionFace == null)
{
//Avanzar hasta muy cerca
float movementLength = movementVector.Length();
movementVector.Multiply((movementLength - EPSILON) / movementLength);
characterSphere.moveCenter(movementVector);
return;
}
//Solo movernos si ya no estamos muy cerca
if (minCollisionDistSq >= EPSILON)
{
//Mover el BoundingSphere hasta casi la nueva posición real
float movementLength = realMovementVector.Length();
realMovementVector.Multiply((movementLength - EPSILON) / movementLength);
characterSphere.moveCenter(realMovementVector);
}
//Calcular plano de Sliding
Vector3 slidePlaneOrigin = nearestPolygonIntersectionPoint;
Vector3 slidePlaneNormal = characterSphere.Center - nearestPolygonIntersectionPoint;
slidePlaneNormal.Normalize();
Plane slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal);
//Proyectamos el punto original de destino en el plano de sliding
TgcRay slideRay = new TgcRay(nearestPolygonIntersectionPoint + Vector3.Multiply(movementVector, slideFactor), slidePlaneNormal);
float slideT;
Vector3 slideDestinationPoint;
if (TgcCollisionUtils.intersectRayPlane(slideRay, slidePlane, out slideT, out slideDestinationPoint))
{
//Nuevo vector de movimiento
Vector3 slideMovementVector = slideDestinationPoint - nearestPolygonIntersectionPoint;
if (slideMovementVector.LengthSq() < EPSILON)
{
return;
}
//Recursividad para aplicar sliding
doCollideWithWorld(characterSphere, slideMovementVector, obstaculos, recursionDepth + 1);
}
}
/// <summary>
/// Detectar colision entre una esfera que se mueve y un triangulo
/// </summary>
/// <param name="sphere">BoundingSphere</param>
/// <param name="movementVector">Vector de movimiento de la esferfa</param>
/// <param name="triangle">Triangulo</param>
/// <param name="collisionPoint">Menor punto de colision encontrado</param>
/// <returns>True si hay colision</returns>
private bool intersectMovingSphereTriangle(TgcBoundingSphere sphere, Vector3 movementVector, Triangle triangle,
out float minT, out Vector3 collisionPoint)
{
float t;
Vector3 q;
collisionPoint = Vector3.Empty;
minT = float.MaxValue;
//Ver si la esfera en movimiento colisiona con el plano del triangulo
if (!TgcCollisionUtils.intersectMovingSpherePlane(sphere, movementVector, triangle.Plane, out t, out q))
{
return(false);
}
//Ver si la esfera ya esta dentro del Plano, hacer un chequeo Sphere-Triangle
if (t == 0.0f)
{
if (TgcCollisionUtils.testSphereTriangle(sphere, triangle.A, triangle.B, triangle.C, out q))
{
minT = 0.0f;
collisionPoint = q;
return(true);
}
}
else
{
//Si el punto de colision contra el plano pertenece al triangulo, entonces ya encontramos el punto de colision
if (TgcCollisionUtils.testPointInTriangle(q, triangle.A, triangle.B, triangle.C))
{
minT = t;
collisionPoint = q;
return(true);
}
}
//Ver de que lado del plano del triangulo esta la esfera
var distPlane = triangle.Plane.Dot(sphere.Center);
var sphereRad = distPlane >= 0.0f ? sphere.Radius : -sphere.Radius;
var planeNormal = TgcCollisionUtils.getPlaneNormal(triangle.Plane);
//Chequear colision entre la esfera en movimiento y los tres Edge y obtener el menor punto de colision
//Es como un Ray del centro de la esfera contra un edge que se convierte en cilindro sin endcap
var segmentEnd = sphere.Center + movementVector;
if (intersectSegmentCylinderNoEndcap(sphere.Center, segmentEnd, triangle.A, triangle.B, sphere.Radius, out t))
{
minT = TgcCollisionUtils.min(t, minT);
}
if (intersectSegmentCylinderNoEndcap(sphere.Center, segmentEnd, triangle.B, triangle.C, sphere.Radius, out t))
{
minT = TgcCollisionUtils.min(t, minT);
}
if (intersectSegmentCylinderNoEndcap(sphere.Center, segmentEnd, triangle.C, triangle.A, sphere.Radius, out t))
{
minT = TgcCollisionUtils.min(t, minT);
}
//Si hubo colision, retornar la menor encontrada
if (minT != float.MaxValue)
{
collisionPoint = sphere.Center + minT * movementVector - sphereRad * planeNormal;
return(true);
}
//Sino, chequear colision entra la esfera y los tres vertices del triangulo y obtener la menor
//Es como un Ray del centro de la esfera contra un vertice que se convierte en esfera
var vertSphere = new TgcBoundingSphere();
minT = float.MaxValue;
vertSphere.setValues(triangle.A, sphere.Radius);
if (TgcCollisionUtils.intersectSegmentSphere(sphere.Center, segmentEnd, vertSphere, out t, out q))
{
minT = TgcCollisionUtils.min(t, minT);
}
vertSphere.setValues(triangle.B, sphere.Radius);
if (TgcCollisionUtils.intersectSegmentSphere(sphere.Center, segmentEnd, vertSphere, out t, out q))
{
minT = TgcCollisionUtils.min(t, minT);
}
vertSphere.setValues(triangle.C, sphere.Radius);
if (TgcCollisionUtils.intersectSegmentSphere(sphere.Center, segmentEnd, vertSphere, out t, out q))
{
minT = TgcCollisionUtils.min(t, minT);
}
//Si hubo colision, retornar la menor encontrada
if (minT != float.MaxValue)
{
collisionPoint = sphere.Center + minT * movementVector - sphereRad * planeNormal;
return(true);
}
//No hay colision
return(false);
}
