/// <summary>
/// Indica si un Ray colisiona con un BoundingSphere.
/// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el Ray (t).
/// La dirección del Ray debe estar normalizada.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="sphere">BoundingSphere</param>
/// <param name="t">Distancia de colision del Ray</param>
/// <param name="q">Punto de colision</param>
/// <returns>True si hay colision</returns>
public static bool intersectRaySphere(TgcRay ray, TgcBoundingSphere sphere, out float t, out Vector3 q)
{
t = -1;
q = Vector3.Empty;
Vector3 m = ray.Origin - sphere.Center;
float b = Vector3.Dot(m, ray.Direction);
float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius;
// Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0)
if (c > 0.0f && b > 0.0f)
{
return(false);
}
float discr = b * b - c;
// A negative discriminant corresponds to ray missing sphere
if (discr < 0.0f)
{
return(false);
}
// Ray now found to intersect sphere, compute smallest t value of intersection
t = -b - FastMath.Sqrt(discr);
// If t is negative, ray started inside sphere so clamp t to zero
if (t < 0.0f)
{
t = 0.0f;
}
q = ray.Origin + t * ray.Direction;
return(true);
}
//
/// <summary>
/// Indica si un BoundingSphere colisiona con un Ray (sin indicar su punto de colision)
/// La dirección del Ray debe estar normalizada.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="sphere">BoundingSphere</param>
/// <returns>True si hay colision</returns>
public static bool testRaySphere(TgcRay ray, TgcBoundingSphere sphere)
{
Vector3 m = ray.Origin - sphere.Center;
float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius;
// If there is definitely at least one real root, there must be an intersection
if (c <= 0.0f)
{
return(true);
}
float b = Vector3.Dot(m, ray.Direction);
// Early exit if ray origin outside sphere and ray pointing away from sphere
if (b > 0.0f)
{
return(false);
}
float disc = b * b - c;
// A negative discriminant corresponds to ray missing sphere
if (disc < 0.0f)
{
return(false);
}
// Now ray must hit sphere
return(true);
}
public override bool intersectRay(TgcRay ray, Matrix transform, out Vector3 q)
{
Vector3[] v = new Vector3[vertices.Count];
for (int i = 0; i < v.Length; i++)
{
v[i] = Vector3.TransformCoordinate(vertices[i].position, transform);
}
float t;
return TgcCollisionUtils.intersectRayConvexPolygon(ray, v, out t, out q);
}
public override bool intersectRay(TgcRay ray, Matrix transform, out Vector3 q)
{
//Actualizar OBB con posiciones de la arista para utilizar en colision
EditablePolyUtils.updateObbFromSegment(COLLISION_OBB,
Vector3.TransformCoordinate(a.position, transform),
Vector3.TransformCoordinate(b.position, transform),
0.4f);
//ray-obb
return TgcCollisionUtils.intersectRayObb(ray, COLLISION_OBB, out q);
}
/// <summary>
/// Indica si un Ray colisiona con un Plano.
/// Tanto la normal del plano como la dirección del Ray se asumen normalizados.
/// </summary>
/// <param name="ray">Ray a testear</param>
/// <param name="plane">Plano a testear</param>
/// <param name="t">Instante de colisión</param>
/// <param name="q">Punto de colisión con el plano</param>
/// <returns>True si hubo colisión</returns>
public static bool intersectRayPlane(TgcRay ray, Plane plane, out float t, out Vector3 q)
{
Vector3 planeNormal = TgcCollisionUtils.getPlaneNormal(plane);
float numer = plane.Dot(ray.Origin);
float denom = Vector3.Dot(planeNormal, ray.Direction);
t = -numer / denom;
if (t > 0.0f)
{
q = ray.Origin + ray.Direction * t;
return(true);
}
q = Vector3.Empty;
return(false);
}
/// <summary>
/// Indica si un segmento de recta colisiona con un BoundingSphere.
/// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el t.
/// La dirección del Ray debe estar normalizada.
/// </summary>
/// <param name="p0">Punto inicial del segmento</param>
/// <param name="p1">Punto final del segmento</param>
/// <param name="s">BoundingSphere</param>
/// <param name="t">Distancia de colision del segmento</param>
/// <param name="q">Punto de colision</param>
/// <returns>True si hay colision</returns>
public static bool intersectSegmentSphere(Vector3 p0, Vector3 p1, TgcBoundingSphere sphere, out float t, out Vector3 q)
{
Vector3 segmentDir = p1 - p0;
TgcRay ray = new TgcRay(p0, segmentDir);
if (TgcCollisionUtils.intersectRaySphere(ray, sphere, out t, out q))
{
float segmentLengthSq = segmentDir.LengthSq();
Vector3 collisionDiff = q - p0;
float collisionLengthSq = collisionDiff.LengthSq();
if (collisionLengthSq <= segmentLengthSq)
{
return(true);
}
}
return(false);
}
/// <summary>
/// Indica si un Ray colisiona con un AABB.
/// Si hay intersección devuelve True, q contiene
/// el punto de intesección.
/// Basado en el código de: http://www.codercorner.com/RayAABB.cpp
/// La dirección del Ray puede estar sin normalizar.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="a">AABB</param>
/// <param name="q">Punto de intersección</param>
/// <returns>True si hay colisión</returns>
public static bool intersectRayAABB(TgcRay ray, TgcBoundingBox aabb, out Vector3 q)
{
float t;
return TgcCollisionUtils.intersectRayAABB(ray.toStruct(), aabb.toStruct(), out q);
}
/// <summary>
/// Detecta colision entre un segmento pq y un triangulo abc.
/// Devuelve true si hay colision y carga las coordenadas barycentricas (u,v,w) de la colision, el
/// instante t de colision y el punto c de colision.
/// Basado en paper Tomas Moller: http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="a">Vertice 1 del triangulo</param>
/// <param name="b">Vertice 2 del triangulo</param>
/// <param name="c">Vertice 3 del triangulo</param>
/// <param name="t">Instante de colision</param>
/// <param name="q">Punto de colision</param>
/// <returns>True si hay colision</returns>
public static bool intersectRayTriangle(TgcRay ray, Vector3 v1, Vector3 v2, Vector3 v3, out float t, out Vector3 q)
{
q = Vector3.Empty;
t = -1;
Vector3 e1, e2; //Edge1, Edge2
Vector3 P, Q, T;
float det, inv_det, u, v;
//Find vectors for two edges sharing V1
e1 = v2 - v1;
e2 = v3 - v1;
//Begin calculating determinant - also used to calculate u parameter
P = Vector3.Cross(ray.Direction, e2);
//if determinant is near zero, ray lies in plane of triangle
det = Vector3.Dot(e1, P);
//NOT CULLING
if(det > -float.Epsilon && det < float.Epsilon) return false;
inv_det = 1.0f / det;
//calculate distance from V1 to ray origin
T = ray.Origin - v1;
//Calculate u parameter and test bound
u = Vector3.Dot(T, P) * inv_det;
//The intersection lies outside of the triangle
if(u < 0.0f || u > 1.0f) return false;
//Prepare to test v parameter
Q = Vector3.Cross(T, e1);
//Calculate V parameter and test bound
v = Vector3.Dot(ray.Direction, Q) * inv_det;
//The intersection lies outside of the triangle
if(v < 0.0f || u + v > 1.0f) return false;
t = Vector3.Dot(e2, Q) * inv_det;
if(t > float.Epsilon) { //ray intersection
q = ray.Origin + t * ray.Direction;
return true;
}
// No hit, no win
return false;
}
/// <summary>
/// Indica si un Ray colisiona con un AABB.
/// Si hay intersección devuelve True, q contiene
/// el punto de intesección.
/// Basado en el código de: http://www.codercorner.com/RayAABB.cpp
/// La dirección del Ray puede estar sin normalizar.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="a">AABB</param>
/// <param name="q">Punto de intersección</param>
/// <returns>True si hay colisión</returns>
public static bool intersectRayAABB(TgcRay ray, TgcBoundingBox aabb, out Vector3 q)
{
q = Vector3.Empty;
bool inside = true;
float[] aabbMin = toArray(aabb.PMin);
float[] aabbMax = toArray(aabb.PMax);
float[] rayOrigin = toArray(ray.Origin);
float[] rayDir = toArray(ray.Direction);
float[] max_t = new float[3] {
-1.0f, -1.0f, -1.0f
};
float[] coord = new float[3];
for (uint i = 0; i < 3; ++i)
{
if (rayOrigin[i] < aabbMin[i])
{
inside = false;
coord[i] = aabbMin[i];
if (rayDir[i] != 0.0f)
{
max_t[i] = (aabbMin[i] - rayOrigin[i]) / rayDir[i];
}
}
else if (rayOrigin[i] > aabbMax[i])
{
inside = false;
coord[i] = aabbMax[i];
if (rayDir[i] != 0.0f)
{
max_t[i] = (aabbMax[i] - rayOrigin[i]) / rayDir[i];
}
}
}
// If the Ray's start position is inside the Box, we can return true straight away.
if (inside)
{
q = toVector3(rayOrigin);
return(true);
}
uint plane = 0;
if (max_t[1] > max_t[plane])
{
plane = 1;
}
if (max_t[2] > max_t[plane])
{
plane = 2;
}
if (max_t[plane] < 0.0f)
{
return(false);
}
for (uint i = 0; i < 3; ++i)
{
if (plane != i)
{
coord[i] = rayOrigin[i] + max_t[plane] * rayDir[i];
if (coord[i] < aabbMin[i] - float.Epsilon || coord[i] > aabbMax[i] + float.Epsilon)
{
return(false);
}
}
}
q = toVector3(coord);
return(true);
}
public override bool intersectRay(TgcRay ray, Matrix transform, out Vector3 q)
{
COLLISION_SPHERE.setCenter(Vector3.TransformCoordinate(position, transform));
float t;
return TgcCollisionUtils.intersectRaySphere(ray, COLLISION_SPHERE, out t, out q);
}
public override Boolean Intercepts(TgcRay ray)
{
return TgcCollisionUtils.testRaySphere(ray, Sphere);
}
public override Boolean Intercepts(TgcRay ray)
{
var v = new Vector3();
return TgcCollisionUtils.intersectRayObb(ray, Obb, out v);
}
/// <summary>
/// Retorna true si hubo interseccion con el terreno y setea el collisionPoint.
/// </summary>
/// <param name="ray"></param>
/// <param name="collisionPoint"></param>
/// <returns></returns>
public bool intersectRay(TgcRay ray, out Vector3 collisionPoint)
{
collisionPoint = Vector3.Empty;
Matrix scaleInv = Matrix.Scaling(new Vector3(1/ScaleXZ, 1/ScaleY, 1/ScaleXZ));
Vector3 a = Vector3.TransformCoordinate(ray.Origin, scaleInv) - traslation;
Vector3 r = Vector3.TransformCoordinate(ray.Direction, scaleInv);
if (a.Y < minIntensity)
return false;
Vector3 q;
//Me fijo si intersecta con el BB del terreno.
if (!TgcCollisionUtils.intersectRayAABB(new TgcRay(a, r).toStruct(), aabb.toStruct(), out q))
return false;
float minT=0;
//Obtengo el T de la interseccion.
if (q != a)
{
if (r.X != 0) minT = (q.X - a.X) / r.X;
else if (r.Y != 0) minT = (q.Y - a.Y) / r.Y;
else if (r.Z != 0) minT = (q.Z - a.Z) / r.Z;
}
//Me desplazo por el rayo hasta que su altura sea menor a la del terreno en ese punto
//o me salga del AABB.
float t=0;
float step = 1;
for (t = minT; ; t += step)
{
collisionPoint = a + t * r;
float y;
if(!interpoledIntensity(collisionPoint.X, collisionPoint.Z, out y))
return false;
if (collisionPoint.Y <= y + float.Epsilon)
{
collisionPoint.Y = y;
collisionPoint = Vector3.TransformCoordinate(collisionPoint + traslation, Matrix.Scaling(ScaleXZ, ScaleY, ScaleXZ));
return true;
}
}
}
/// <summary>
/// Selecciona el eje actual del gizmo haciendo picking
/// </summary>
public void selectAxisByPicking(TgcRay ray)
{
this.selectedAxis = doPickAxis(ray);
}
/// <summary>
/// Hacer picking contra todos los ejes y devolver el eje seleccionado (si hay colision).
/// Tambien se evaluan los ejes compuestos (XY, XZ, YZ)
/// </summary>
public Axis doPickAxis(TgcRay ray)
{
Vector3 collP;
if (TgcCollisionUtils.intersectRayAABB(ray, boxX.BoundingBox, out collP))
{
return Axis.X;
}
else if (TgcCollisionUtils.intersectRayAABB(ray, boxY.BoundingBox, out collP))
{
return Axis.Y;
}
else if (TgcCollisionUtils.intersectRayAABB(ray, boxZ.BoundingBox, out collP))
{
return Axis.Z;
}
else if (TgcCollisionUtils.intersectRayAABB(ray, boxXZ.BoundingBox, out collP))
{
return Axis.XZ;
}
else if (TgcCollisionUtils.intersectRayAABB(ray, boxXY.BoundingBox, out collP))
{
return Axis.XY;
}
else if (TgcCollisionUtils.intersectRayAABB(ray, boxYZ.BoundingBox, out collP))
{
return Axis.YZ;
}
else
{
return Axis.None;
}
}
public Vector3 intersectRayTerrain(TgcRay ray)
{
int iteraciones = heightmapResolution/cantidadFilasColumnas * (int)currentScaleXZ;
Vector3 dir = ray.Direction;
dir.Normalize();
Vector3 origin = ray.Origin;
Vector3 pos = origin;
float y = 0;
for (int i = 0; i < iteraciones; i++)
{
interpoledHeight(pos.X, pos.Z, out y);
if (FastMath.Abs(pos.Y - y) < 1f)
{
return pos;
}
pos += dir;
}
return pos;
}
public void fireSniper()
{
//Disparamos el arma, nos fijamos si colisiona con un enemigo, y si hay obstaculos en el medio
Vector3 dir = CustomFpsCamera.Instance.LookAt - CustomFpsCamera.Instance.Position;
TgcRay ray = new TgcRay(CustomFpsCamera.Instance.Position, dir);
Vector3 newPosition = new Vector3(0, 0, 0);
List<Vector3> posicionObstaculos = new List<Vector3>();
bool vegetacionFrenoDisparo = false;
foreach (TgcMesh obstaculo in vegetation)
{
if (TgcCollisionUtils.intersectRayAABB(ray, obstaculo.BoundingBox, out newPosition))
posicionObstaculos.Add(newPosition);
}
int killHeadTracker = 0;
bool hit = false;
for (int i = enemies.Count - 1; i >= 0; i--)
{
if (TgcCollisionUtils.intersectRayAABB(ray, enemies[i].HEADSHOT_BOUNDINGBOX, out newPosition))
{
foreach(Vector3 posicion in posicionObstaculos){
if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin))
{
vegetacionFrenoDisparo = true;
break;
}
}
if (!vegetacionFrenoDisparo)
{
hit = true;
score += 1;
killHeadTracker++;
HUDManager.Instance.headShot();
enemies[i].health = 0;
enemies[i].sangrar(-dir, newPosition.Y - enemies[i].getPosicionActual().Y);
//eliminarEnemigo(enemies[i]);
enemies[i].morirse();
sumarScore(enemies[i]);
}
vegetacionFrenoDisparo = false;
}
if (!hit && TgcCollisionUtils.intersectRayAABB(ray, enemies[i].LEGS_BOUNDINGBOX, out newPosition))
{
foreach (Vector3 posicion in posicionObstaculos)
{
if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin))
{
vegetacionFrenoDisparo = true;
break;
}
}
if (!vegetacionFrenoDisparo)
{
enemies[i].health -= 25;
enemies[i].sangrar(-dir, newPosition.Y - enemies[i].getPosicionActual().Y);
hit = true;
if (enemies[i].health <= 0)
{
// eliminarEnemigo(enemies[i]);
enemies[i].morirse();
sumarScore(enemies[i]);
}
}
vegetacionFrenoDisparo = false;
}
if (!hit && TgcCollisionUtils.intersectRayAABB(ray, enemies[i].CHEST_BOUNDINGBOX, out newPosition))
{
foreach(Vector3 posicion in posicionObstaculos){
if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin))
{
vegetacionFrenoDisparo = true;
break;
}
}
if (!vegetacionFrenoDisparo)
{
enemies[i].health -= 50;
enemies[i].sangrar(-dir, newPosition.Y - enemies[i].getPosicionActual().Y);
if (enemies[i].health <= 0)
{
//eliminarEnemigo(enemies[i]);
enemies[i].morirse();
sumarScore(enemies[i]);
}
}
vegetacionFrenoDisparo = false;
}
hit = false;
}
//////////////////////////disparo a barriles////////////////////////////////////////
for (int i = barriles.Count - 1; i >= 0; i--)
{
if (TgcCollisionUtils.intersectRayAABB(ray, barriles[i].mesh.BoundingBox, out newPosition))
{
foreach (Vector3 posicion in posicionObstaculos)
{
if (Vector3.Length(posicion - ray.Origin) < Vector3.Length(newPosition - ray.Origin))
{
vegetacionFrenoDisparo = true;
break;
}
}
if (!vegetacionFrenoDisparo)
{
// playSound(explosionSoundDir); TODO
barriles[i].explotar();
}
vegetacionFrenoDisparo = false;
}
}
/////////////////////////////////////////////////////////////
if (killHeadTracker > 1)
{
HUDManager.Instance.headHunter();//Constante que reproduce el efecto de headhunter.
score += killHeadTracker;
HUDManager.Instance.refreshScore();
}
}
public Vector3 fireLauncher()
{
//Disparamos el arma, nos fijamos si colisiona con un enemigo, y si hay obstaculos en el medio
TgcRay ray = new TgcRay(CustomFpsCamera.Instance.Position, CustomFpsCamera.Instance.LookAt - CustomFpsCamera.Instance.Position);
Vector3 newPosition = new Vector3(0, 0, 0);
List<Vector3> posicionObstaculos = new List<Vector3>();
foreach (TgcMesh obstaculo in vegetation)
{
if (TgcCollisionUtils.intersectRayAABB(ray, obstaculo.BoundingBox, out newPosition))
posicionObstaculos.Add(newPosition);
}
for (int i = enemies.Count - 1; i >= 0; i--)
{
if (TgcCollisionUtils.intersectRayAABB(ray, enemies[i].mesh.BoundingBox, out newPosition))
posicionObstaculos.Add(newPosition);
}
//////////////////////////disparo a barriles////////////////////////////////////////
for (int i = barriles.Count - 1; i >= 0; i--)
{
if (TgcCollisionUtils.intersectRayAABB(ray, barriles[i].mesh.BoundingBox, out newPosition))
posicionObstaculos.Add(newPosition);
}
posicionObstaculos.Add(intersectRayTerrain(ray));
posicionObstaculos.Sort(delegate(Vector3 x, Vector3 y)
{
return distanciaACamara(x).CompareTo(distanciaACamara(y));
});
Vector3 min = posicionObstaculos[0];
return min;
}
/// <summary>
/// Indica si un Ray colisiona con un AABB.
/// Si hay intersección devuelve True, q contiene
/// el punto de intesección.
/// Basado en el código de: http://www.codercorner.com/RayAABB.cpp
/// La dirección del Ray puede estar sin normalizar.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="a">AABB</param>
/// <param name="q">Punto de intersección</param>
/// <returns>True si hay colisión</returns>
public static bool intersectRayAABB(TgcRay.RayStruct ray, TgcBoundingBox.AABBStruct aabb, out Vector3 q)
{
q = Vector3.Empty;
bool inside = true;
float[] aabbMin = toArray(aabb.min);
float[]aabbMax = toArray(aabb.max);
float[]rayOrigin = toArray(ray.origin);
float[]rayDir = toArray(ray.direction);
float[] max_t = new float[3]{-1.0f, -1.0f, -1.0f};
float[] coord = new float[3];
for (uint i = 0; i < 3; ++i)
{
if (rayOrigin[i] < aabbMin[i])
{
inside = false;
coord[i] = aabbMin[i];
if (rayDir[i] != 0.0f)
{
max_t[i] = (aabbMin[i] - rayOrigin[i]) / rayDir[i];
}
}
else if (rayOrigin[i] > aabbMax[i])
{
inside = false;
coord[i] = aabbMax[i];
if (rayDir[i] != 0.0f)
{
max_t[i] = (aabbMax[i] - rayOrigin[i]) / rayDir[i];
}
}
}
// If the Ray's start position is inside the Box, we can return true straight away.
if (inside)
{
q = toVector3(rayOrigin);
return true;
}
uint plane = 0;
if (max_t[1] > max_t[plane])
{
plane = 1;
}
if (max_t[2] > max_t[plane])
{
plane = 2;
}
if (max_t[plane] < 0.0f)
{
return false;
}
for (uint i = 0; i < 3; ++i)
{
if (plane != i)
{
coord[i] = rayOrigin[i] + max_t[plane] * rayDir[i];
if (coord[i] < aabbMin[i] - float.Epsilon || coord[i] > aabbMax[i] + float.Epsilon)
{
return false;
}
}
}
q = toVector3(coord);
return true;
}
/// <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>
/// Indica si un Ray colisiona con un Plano.
/// Tanto la normal del plano como la dirección del Ray se asumen normalizados.
/// </summary>
/// <param name="ray">Ray a testear</param>
/// <param name="plane">Plano a testear</param>
/// <param name="t">Instante de colisión</param>
/// <param name="q">Punto de colisión con el plano</param>
/// <returns>True si hubo colisión</returns>
public static bool intersectRayPlane(TgcRay ray, Plane plane, out float t, out Vector3 q)
{
Vector3 planeNormal = TgcCollisionUtils.getPlaneNormal(plane);
float numer = plane.Dot(ray.Origin);
float denom = Vector3.Dot(planeNormal, ray.Direction);
t = -numer / denom;
if(t > 0.0f)
{
q = ray.Origin + ray.Direction * t;
return true;
}
q = Vector3.Empty;
return false;
}
/// <summary>
/// Picking con los planos XZ e YZ ubicados en el centro del objeto
/// </summary>
public Vector3 getPickingZ(TgcRay ray, Vector3 objCenter)
{
//Mover ambos planos hacia el centro del objeto
pickingXZAabb.setExtremes(
new Vector3(pickingXZAabb.PMin.X, objCenter.Y - SMALL_VAL, pickingXZAabb.PMin.Z),
new Vector3(pickingXZAabb.PMax.X, objCenter.Y, pickingXZAabb.PMax.Z));
pickingYZAabb.setExtremes(
new Vector3(objCenter.X - SMALL_VAL, pickingYZAabb.PMin.Y, pickingYZAabb.PMin.Z),
new Vector3(objCenter.X, pickingYZAabb.PMax.Y, pickingYZAabb.PMax.Z));
Vector3 q1, q2;
bool r1, r2;
r1 = TgcCollisionUtils.intersectRayAABB(ray, pickingXZAabb, out q1);
r2 = TgcCollisionUtils.intersectRayAABB(ray, pickingYZAabb, out q2);
if (r1 && r2)
{
Vector2 objPos = new Vector2(objCenter.X, objCenter.Y);
float diff1 = Vector2.Length(new Vector2(q1.X, q1.Y) - objPos);
float diff2 = Vector2.Length(new Vector2(q2.X, q2.Y) - objPos);
return diff1 < diff2 ? q1 : q2;
}
else if (r1)
return clampPickingResult(q1);
else if (r2)
return clampPickingResult(q2);
return objCenter;
}
/// <summary>
/// Retorna true si hubo interseccion con el plano del terreno y setea el collisionPoint con la altura en ese punto.
/// </summary>
/// <param name="ray"></param>
/// <param name="collisionPoint"></param>
/// <returns></returns>
public bool intersectRayPlane(TgcRay ray, out Vector3 collisionPoint)
{
collisionPoint = Vector3.Empty;
float minHeight = (minIntensity + traslation.Y) * ScaleY;
float t;
//Me fijo si intersecta con el BB del terreno.
if (!TgcCollisionUtils.intersectRayPlane(ray, new Plane(0, 1, 0, -minHeight), out t, out collisionPoint)) return false;
return interpoledHeight(collisionPoint.X, collisionPoint.Z, out collisionPoint.Y);
}
/// <summary>
/// Detectar colision entre un Ray y una Capsula
/// Basado en: http://www.geometrictools.com/LibMathematics/Intersection/Wm5IntrLine3Capsule3.cpp
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="capsule">Capsula</param>
/// <param name="t">Menor instante de colision</param>
/// <returns>True si hay colision</returns>
private bool intersectRayCapsule(TgcRay.RayStruct ray, Capsule capsule, out float t)
{
t = -1;
Vector3 origin = ray.origin;
Vector3 dir = ray.direction;
// Create a coordinate system for the capsule. In this system, the
// capsule segment center C is the origin and the capsule axis direction
// W is the z-axis. U and V are the other coordinate axis directions.
// If P = x*U+y*V+z*W, the cylinder containing the capsule wall is
// x^2 + y^2 = r^2, where r is the capsule radius. The finite cylinder
// that makes up the capsule minus its hemispherical end caps has z-values
// |z| <= e, where e is the extent of the capsule segment. The top
// hemisphere cap is x^2+y^2+(z-e)^2 = r^2 for z >= e, and the bottom
// hemisphere cap is x^2+y^2+(z+e)^2 = r^2 for z <= -e.
Vector3 U = capsule.segment.dir;
Vector3 V = U;
Vector3 W = U;
generateComplementBasis(ref U, ref V, W);
float rSqr = capsule.radius * capsule.radius;
float extent = capsule.segment.extent;
// Convert incoming line origin to capsule coordinates.
Vector3 diff = origin - capsule.segment.center;
Vector3 P = new Vector3(Vector3.Dot(U, diff), Vector3.Dot(V, diff), Vector3.Dot(W, diff));
// Get the z-value, in capsule coordinates, of the incoming line's unit-length direction.
float dz = Vector3.Dot(W, dir);
if (FastMath.Abs(dz) >= 1f - float.Epsilon)
{
// The line is parallel to the capsule axis. Determine whether the line intersects the capsule hemispheres.
float radialSqrDist = rSqr - P.X * P.X - P.Y * P.Y;
if (radialSqrDist < 0f)
{
// Line outside the cylinder of the capsule, no intersection.
return false;
}
// line intersects the hemispherical caps
float zOffset = FastMath.Sqrt(radialSqrDist) + extent;
if (dz > 0f)
{
t = -P.Z - zOffset;
}
else
{
t = P.Z - zOffset;
}
return true;
}
// Convert incoming line unit-length direction to capsule coordinates.
Vector3 D = new Vector3(Vector3.Dot(U, dir), Vector3.Dot(V, dir), dz);
// Test intersection of line P+t*D with infinite cylinder x^2+y^2 = r^2.
// This reduces to computing the roots of a quadratic equation. If
// P = (px,py,pz) and D = (dx,dy,dz), then the quadratic equation is
// (dx^2+dy^2)*t^2 + 2*(px*dx+py*dy)*t + (px^2+py^2-r^2) = 0
float a0 = P.X * P.X + P.Y * P.Y - rSqr;
float a1 = P.X * D.X + P.Y * D.Y;
float a2 = D.X * D.X + D.Y * D.Y;
float discr = a1 * a1 - a0 * a2;
if (discr < 0f)
{
// Line does not intersect infinite cylinder.
return false;
}
float root, inv, tValue, zValue;
int quantity = 0;
if (discr > float.Epsilon)
{
// Line intersects infinite cylinder in two places.
root = FastMath.Sqrt(discr);
inv = (1f)/a2;
tValue = (-a1 - root)* inv;
zValue = P.Z + tValue * D.Z;
if (FastMath.Abs(zValue) <= extent)
{
quantity++;
t = tValue;
}
tValue = (-a1 + root) * inv;
zValue = P.Z + tValue * D.Z;
if (FastMath.Abs(zValue) <= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
}
if (quantity == 2)
{
// Line intersects capsule wall in two places.
return true;
}
}
else
{
// Line is tangent to infinite cylinder.
tValue = -a1 / a2;
zValue = P.Z + tValue * D.Z;
if (FastMath.Abs(zValue) <= extent)
{
t = tValue;
return true;
}
}
// Test intersection with bottom hemisphere. The quadratic equation is
// t^2 + 2*(px*dx+py*dy+(pz+e)*dz)*t + (px^2+py^2+(pz+e)^2-r^2) = 0
// Use the fact that currently a1 = px*dx+py*dy and a0 = px^2+py^2-r^2.
// The leading coefficient is a2 = 1, so no need to include in the
// construction.
float PZpE = P.Z + extent;
a1 += PZpE * D.Z;
a0 += PZpE * PZpE;
discr = a1 * a1 - a0;
if (discr > float.Epsilon)
{
root = FastMath.Sqrt(discr);
tValue = -a1 - root;
zValue = P.Z + tValue * D.Z;
if (zValue <= -extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return true;
}
}
tValue = -a1 + root;
zValue = P.Z + tValue * D.Z;
if (zValue <= -extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return true;
}
}
}
else if (FastMath.Abs(discr) <= float.Epsilon)
{
tValue = -a1;
zValue = P.Z + tValue * D.Z;
if (zValue <= -extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return true;
}
}
}
// Test intersection with top hemisphere. The quadratic equation is
// t^2 + 2*(px*dx+py*dy+(pz-e)*dz)*t + (px^2+py^2+(pz-e)^2-r^2) = 0
// Use the fact that currently a1 = px*dx+py*dy+(pz+e)*dz and
// a0 = px^2+py^2+(pz+e)^2-r^2. The leading coefficient is a2 = 1, so
// no need to include in the construction.
a1 -= 2f * extent * D.Z;
a0 -= 4 * extent * P.Z;
discr = a1*a1 - a0;
if (discr > float.Epsilon)
{
root = FastMath.Sqrt(discr);
tValue = -a1 - root;
zValue = P.Z + tValue*D.Z;
if (zValue >= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return true;
}
}
tValue = -a1 + root;
zValue = P.Z + tValue * D.Z;
if (zValue >= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return true;
}
}
}
else if (FastMath.Abs(discr) <= float.Epsilon)
{
tValue = -a1;
zValue = P.Z + tValue * D.Z;
if (zValue >= extent)
{
quantity++;
t = TgcCollisionUtils.min(t, tValue);
if (quantity == 2)
{
return true;
}
}
}
return quantity > 0;
}
/// <summary>
/// Picking con plano YZ ubicado en el centro del objeto
/// </summary>
public Vector3 getPickingYZ(TgcRay ray, Vector3 objCenter)
{
//Mover aabb en Y al centro del mesh
pickingYZAabb.setExtremes(
new Vector3(objCenter.X - SMALL_VAL, pickingYZAabb.PMin.Y, pickingYZAabb.PMin.Z),
new Vector3(objCenter.X, pickingYZAabb.PMax.Y, pickingYZAabb.PMax.Z));
Vector3 q;
bool r = TgcCollisionUtils.intersectRayAABB(ray, pickingYZAabb, out q);
if (r)
return clampPickingResult(q);
return objCenter;
}
/// <summary>
/// Indica si un rayo colisiona con un cilindro.
/// </summary>
/// <param name="ray">Rayo</param>
/// <param name="cylinder">Cilindro alineado</param>
/// <returns>True si el rayo colisiona con el cilindro</returns>
public static bool testRayCylinder(TgcRay ray, TgcFixedYBoundingCylinder cylinder)
{
Matrix transformation = cylinder.AntiTransformationMatrix;
Vector3 origin = Vector3.TransformCoordinate(ray.Origin, transformation);
Vector3 direction = Vector3.TransformNormal(ray.Direction, transformation);
return TgcCollisionUtils.testRayCylinder(origin, direction);
}
/// <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)
{
float[] aabbMin = TgcCollisionUtils.toArray(aabb.min);
float[] aabbMax = TgcCollisionUtils.toArray(aabb.max);
float[] p = TgcCollisionUtils.toArray(ray.origin);
float[] d = TgcCollisionUtils.toArray(ray.direction);
tmin = 0.0f; // set to -FLT_MAX to get first hit on line
float tmax = float.MaxValue; // set to max distance ray can travel (for segment)
q = Vector3.Empty;
// For all three slabs
for (int 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
float ood = 1.0f / d[i];
float t1 = (aabbMin[i] - p[i]) * ood;
float 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;
}
private bool canSeeWithObstacles(Vector3 targetPoint, List<ILevelObject> obstacles)
{
Vector3 pt = targetPoint-this.Position;
TgcRay ray = new TgcRay(this.Position, pt);
foreach (ILevelObject o in obstacles)
{
if (objectInsideRadius(pt.Length(), o))
{
if (o.collidesWith(ray)) return false;
}
}
return true;
}
public TgcPickingRay()
{
ray = new TgcRay();
}
/// <summary>
/// Indica si un Ray colisiona con un BoundingSphere.
/// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el Ray (t).
/// La dirección del Ray debe estar normalizada.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="sphere">BoundingSphere</param>
/// <param name="t">Distancia de colision del Ray</param>
/// <param name="q">Punto de colision</param>
/// <returns>True si hay colision</returns>
public static bool intersectRaySphere(TgcRay ray, TgcBoundingSphere sphere, out float t, out Vector3 q)
{
t = -1;
q = Vector3.Empty;
Vector3 m = ray.Origin - sphere.Center;
float b = Vector3.Dot(m, ray.Direction);
float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius;
// Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0)
if (c > 0.0f && b > 0.0f) return false;
float discr = b*b - c;
// A negative discriminant corresponds to ray missing sphere
if (discr < 0.0f) return false;
// Ray now found to intersect sphere, compute smallest t value of intersection
t = -b - FastMath.Sqrt(discr);
// If t is negative, ray started inside sphere so clamp t to zero
if (t < 0.0f) t = 0.0f;
q = ray.Origin + t * ray.Direction;
return true;
}
/// <summary>
/// Detecta colision entre un rayo y un poligono convexo formado por un conjunto de vertices.
/// </summary>
/// <param name="ray">Rayo</param>
/// <param name="polyVertices">Conjunto de vertices del poligono</param>
/// <param name="t">Instante de tiempo de colision</param>
/// <param name="q">Punto de colision</param>
/// <returns>True si hay colision</returns>
public static bool intersectRayConvexPolygon(TgcRay ray, Vector3[] polyVertices, out float t, out Vector3 q)
{
t = -1;
q = Vector3.Empty;
Vector3 v0 = polyVertices[0];
Vector3 v1 = polyVertices[1];
for (int i = 2; i < polyVertices.Length; i++)
{
Vector3 v2 = polyVertices[i];
if (intersectRayTriangle(ray, v0, v1, v2, out t, out q))
{
return true;
}
v1 = v2;
}
return false;
}
public Boolean Intercepts(TgcRay ray)
{
return _Colliders.Any(c => c.Intercepts(ray));
}
/// <summary>
/// Indica si un segmento de recta colisiona con un BoundingSphere.
/// Si el resultado es True se carga el punto de colision (q) y la distancia de colision en el t.
/// La dirección del Ray debe estar normalizada.
/// </summary>
/// <param name="p0">Punto inicial del segmento</param>
/// <param name="p1">Punto final del segmento</param>
/// <param name="s">BoundingSphere</param>
/// <param name="t">Distancia de colision del segmento</param>
/// <param name="q">Punto de colision</param>
/// <returns>True si hay colision</returns>
public static bool intersectSegmentSphere(Vector3 p0, Vector3 p1, TgcBoundingSphere sphere, out float t, out Vector3 q)
{
Vector3 segmentDir = p1 - p0;
TgcRay ray = new TgcRay(p0, segmentDir);
if (TgcCollisionUtils.intersectRaySphere(ray, sphere, out t, out q))
{
float segmentLengthSq = segmentDir.LengthSq();
Vector3 collisionDiff = q - p0;
float collisionLengthSq = collisionDiff.LengthSq();
if (collisionLengthSq <= segmentLengthSq)
{
return true;
}
}
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;
}
public PickingRay()
{
ray = new TgcRay();
}
//
/// <summary>
/// Indica si un BoundingSphere colisiona con un Ray (sin indicar su punto de colision)
/// La dirección del Ray debe estar normalizada.
/// </summary>
/// <param name="ray">Ray</param>
/// <param name="sphere">BoundingSphere</param>
/// <returns>True si hay colision</returns>
public static bool testRaySphere(TgcRay ray, TgcBoundingSphere sphere)
{
Vector3 m = ray.Origin - sphere.Center;
float c = Vector3.Dot(m, m) - sphere.Radius * sphere.Radius;
// If there is definitely at least one real root, there must be an intersection
if (c <= 0.0f) return true;
float b = Vector3.Dot(m, ray.Direction);
// Early exit if ray origin outside sphere and ray pointing away from sphere
if (b > 0.0f) return false;
float disc = b * b - c;
// A negative discriminant corresponds to ray missing sphere
if (disc < 0.0f) return false;
// Now ray must hit sphere
return true;
}
