/// <summary>
/// Obtiene la distancia entre dos puntos.
/// </summary>
/// <param name="p1">Punto 1</param>
/// <param name="p2">Punto 2</param>
/// <returns>Distancia entre los dos puntos</returns>
private float distancePointPoint(Vector3 p1, Vector3 p2)
{
Vector3 slidePlaneNormal = p1 - p2;
slidePlaneNormal.Normalize();
Plane slidePlane = Plane.FromPointNormal(p2, slidePlaneNormal);
return(TgcCollisionUtils.distPointPlane(p1, slidePlane));
}
public static Plane detectarCaraChocada(List <Plane> carasDelObstaculo, Vector3 puntoChoque)
{
Plane caraMasCercana = carasDelObstaculo[0];
float distMinima = FastMath.Abs(TgcCollisionUtils.distPointPlane(puntoChoque, carasDelObstaculo[0]));
foreach (Plane cara in carasDelObstaculo)
{
float unaDistancia = FastMath.Abs(TgcCollisionUtils.distPointPlane(puntoChoque, cara));
if (unaDistancia < distMinima)
{
distMinima = unaDistancia;
caraMasCercana = cara;
}
}
GuiController.Instance.UserVars.setValue("DistMinima", distMinima);
return(caraMasCercana);
}
/// <summary>
/// Detección de colisiones recursiva
/// </summary>
/// <param name="eSphere">Sphere de radio 1 pasada a Elipsoid space</param>
/// <param name="eMovementVector">Movimiento pasado a Elipsoid space</param>
/// <param name="eRadius">Radio de la elipsoide</param>
/// <param name="colliders">Objetos contra los cuales colisionar</param>
/// <param name="recursionDepth">Nivel de recursividad</param>
/// <param name="movementSphere">Esfera real que representa el movimiento abarcado</param>
/// <param name="slidingMinY">Minimo valor de normal Y de colision para hacer sliding</param>
/// <returns>Resultado de colision</returns>
public CollisionResult doCollideWithWorld(TgcBoundingSphere eSphere, Vector3 eMovementVector, Vector3 eRadius,
List <Collider> colliders, int recursionDepth, TgcBoundingSphere movementSphere, float slidingMinY)
{
var result = new CollisionResult();
result.collisionFound = false;
//Limitar recursividad
if (recursionDepth > 5)
{
return(result);
}
//Posicion deseada
var nextSphereCenter = eSphere.Center + eMovementVector;
//Buscar el punto de colision mas cercano de todos los objetos candidatos
Vector3 q;
float t;
Vector3 n;
var minT = float.MaxValue;
foreach (var collider in colliders)
{
//Colisionar Sphere en movimiento contra Collider (cada Collider resuelve la colision)
if (collider.intersectMovingElipsoid(eSphere, eMovementVector, eRadius, movementSphere, out t, out q,
out n))
{
//Quedarse con el menor instante de colision
if (t < minT)
{
minT = t;
result.collisionFound = true;
result.collisionPoint = q;
result.collisionNormal = n;
result.collider = collider;
}
}
}
//Si nunca hubo colisión, avanzar todo lo requerido
if (!result.collisionFound)
{
//Avanzar todo lo pedido
eSphere.moveCenter(eMovementVector);
result.realMovmentVector = eMovementVector;
result.collisionNormal = Vector3.Empty;
result.collisionPoint = Vector3.Empty;
result.collider = null;
return(result);
}
//Solo movernos si ya no estamos muy cerca
if (minT >= EPSILON)
{
//Restar un poco al instante de colision, para movernos hasta casi esa distancia
minT -= EPSILON;
result.realMovmentVector = eMovementVector * minT;
eSphere.moveCenter(result.realMovmentVector);
//Quitarle al punto de colision el EPSILON restado al movimiento, para no afectar al plano de sliding
var v = Vector3.Normalize(result.realMovmentVector);
result.collisionPoint -= v * EPSILON;
}
//Calcular plano de Sliding, como un plano tangete al punto de colision con la esfera, apuntando hacia el centro de la esfera
var slidePlaneOrigin = result.collisionPoint;
var slidePlaneNormal = eSphere.Center - result.collisionPoint;
slidePlaneNormal.Normalize();
var slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal);
//Calcular vector de movimiento para sliding, proyectando el punto de destino original sobre el plano de sliding
var distance = TgcCollisionUtils.distPointPlane(nextSphereCenter, slidePlane);
var newDestinationPoint = nextSphereCenter - distance * slidePlaneNormal;
var slideMovementVector = newDestinationPoint - result.collisionPoint;
//No hacer recursividad si es muy pequeño
slideMovementVector.Scale(SlideFactor);
if (slideMovementVector.Length() < EPSILON)
{
return(result);
}
//Ver si posee la suficiente pendiente en Y para hacer sliding
if (result.collisionNormal.Y <= slidingMinY)
{
//Recursividad para aplicar sliding
doCollideWithWorld(eSphere, slideMovementVector, eRadius, colliders, recursionDepth + 1, movementSphere,
slidingMinY);
}
return(result);
}
/// <summary>
/// Detección de colisiones recursiva
/// </summary>
public void doCollideWithWorld(TgcBoundingSphere characterSphere, Vector3 movementVector,
List <Collider> colliders, int recursionDepth, TgcBoundingSphere movementSphere, bool sliding,
float slidingMinY)
{
//Limitar recursividad
if (recursionDepth > 5)
{
return;
}
//Posicion deseada
var originalSphereCenter = characterSphere.Center;
var nextSphereCenter = originalSphereCenter + movementVector;
//Buscar el punto de colision mas cercano de todos los objetos candidatos
Collision = false;
Vector3 q;
float t;
Vector3 n;
var minT = float.MaxValue;
foreach (var collider in colliders)
{
//Colisionar Sphere en movimiento contra Collider (cada Collider resuelve la colision)
if (collider.intersectMovingSphere(characterSphere, movementVector, movementSphere, out t, out q, out n))
{
//Quedarse con el menor instante de colision
if (t < minT)
{
minT = t;
Collision = true;
LastCollisionPoint = q;
LastCollisionNormal = n;
lastCollider = collider;
}
}
}
//Si nunca hubo colisión, avanzar todo lo requerido
if (!Collision)
{
//Avanzar todo lo pedido
//lastCollisionDistance = movementVector.Length();
characterSphere.moveCenter(movementVector);
return;
}
//Solo movernos si ya no estamos muy cerca
if (minT >= EPSILON)
{
//Restar un poco al instante de colision, para movernos hasta casi esa distancia
minT -= EPSILON;
var realMovementVector = movementVector * minT;
//Mover el BoundingSphere
characterSphere.moveCenter(realMovementVector);
//Quitarle al punto de colision el EPSILON restado al movimiento, para no afectar al plano de sliding
var v = Vector3.Normalize(realMovementVector);
LastCollisionPoint -= v * EPSILON;
}
if (sliding)
{
//Calcular plano de Sliding, como un plano tangete al punto de colision con la esfera, apuntando hacia el centro de la esfera
var slidePlaneOrigin = LastCollisionPoint;
var slidePlaneNormal = characterSphere.Center - LastCollisionPoint;
slidePlaneNormal.Normalize();
var slidePlane = Plane.FromPointNormal(slidePlaneOrigin, slidePlaneNormal);
//Calcular vector de movimiento para sliding, proyectando el punto de destino original sobre el plano de sliding
var distance = TgcCollisionUtils.distPointPlane(nextSphereCenter, slidePlane);
var newDestinationPoint = nextSphereCenter - distance * slidePlaneNormal;
var slideMovementVector = newDestinationPoint - LastCollisionPoint;
//No hacer recursividad si es muy pequeño
slideMovementVector.Scale(SlideFactor);
if (slideMovementVector.Length() < EPSILON)
{
return;
}
if (LastCollisionNormal.Y <= slidingMinY)
{
//Recursividad para aplicar sliding
doCollideWithWorld(characterSphere, slideMovementVector, colliders, recursionDepth + 1,
movementSphere, sliding, slidingMinY);
}
}
}
private TGCPlane SelectPlane(List <TGCPlane> planes, TGCVector3 testPoint)
{
planes.Sort((x, y) => (int)TgcCollisionUtils.distPointPlane(testPoint, y) - (int)TgcCollisionUtils.distPointPlane(testPoint, x));
return(planes[0]);
}