/// <summary>
/// Detecta la colisión entre una caja y una colección de triángulos
/// </summary>
/// <param name="box">Caja</param>
/// <param name="triangleSoup">Colección de triángulos</param>
/// <param name="data">Datos de colisión a llenar</param>
/// <returns>Devuelve verdadero si existe colisión, falso en el resto de los casos</returns>
public static bool BoxAndTriangleSoup(CollisionBox box, CollisionTriangleSoup triangleSoup, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
bool intersection = false;
int contacts = 0;
foreach (Triangle triangle in triangleSoup.Triangles)
{
// Comprobar la intersección
if (IntersectionTests.BoxAndTri(box, triangle))
{
// Hay intersección, ahora hay que encontrar los puntos de intersección.
// Podemos hacerlo únicamente chequeando los vértices.
// Si la caja está descansando sobre el plano o un eje, se reportarán cuatro o dos puntos de contacto.
for (int i = 0; i < 8; i++)
{
// Calcular la positición de cada vértice
Vector3 vertexPos = box.GetCorner(i);
Plane plane = triangle.Plane;
// Calcular la distancia al plano
float distanceToPlane = Vector3.Dot(vertexPos, plane.Normal) + plane.D;
// Si la distancia es negativa está tras el plano. Si es 0, está en el plano
if (distanceToPlane <= 0f)
{
// Intersección entre línea y triángulo
Vector3 direction = vertexPos - box.Position;
if (IntersectionTests.TriAndRay(triangle, new Ray(box.Position, direction)))
{
intersection = true;
contacts++;
// Crear la información del contacto.
// El punto de contacto está a medio camino entre el vértice y el plano.
// Se obtiene multiplicando la dirección por la mitad de la distancia de separación, y añadiendo la posición del vértice.
Contact contact = data.CurrentContact;
contact.ContactPoint = vertexPos;
contact.ContactNormal = plane.Normal;
contact.Penetration = -distanceToPlane;
// Establecer los datos del contacto
RigidBody one = box.Body;
RigidBody two = null;
contact.SetBodyData(ref one, ref two, data.Friction, data.Restitution);
// Añadir contacto
data.AddContact();
if (data.ContactsLeft <= 0)
{
return(true);
}
if (contacts > 4)
{
return(true);
}
}
}
}
}
}
return(intersection);
}
/// <summary>
/// Detecta la colisión entre una esfera y un plano
/// </summary>
/// <param name="sphere">Esfera</param>
/// <param name="plane">Plano</param>
/// <param name="data">Rellena los datos de colisión</param>
/// <returns>Devuelve verdadero si hay colisión o falso en el resto de los casos</returns>
public static bool SphereAndTruePlane(CollisionSphere sphere, CollisionPlane plane, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Almacenar la posición de la esfera
Vector3 position = sphere.Position;
// Encontrar la distancia al plano
float centreDistance = Vector3.Dot(plane.Normal, position) - plane.D;
if (centreDistance * centreDistance > sphere.Radius * sphere.Radius)
{
return(false);
}
// Chequear la cara del plano en la que estamos para calcular normal y penetración
Vector3 normal = plane.Normal;
float penetration = -centreDistance;
if (centreDistance < 0)
{
normal *= -1;
penetration = -penetration;
}
penetration += sphere.Radius;
// Crear el contacto. Tiene una normal en la dirección del plano.
Contact contact = data.CurrentContact;
contact.ContactNormal = normal;
contact.Penetration = penetration;
contact.ContactPoint = position - plane.Normal * centreDistance;
RigidBody two = null;
contact.SetBodyData(ref sphere.Body, ref two, data.Friction, data.Restitution);
data.AddContact();
return(true);
}
/// <summary>
/// Detecta la colisión entre cajas
/// </summary>
/// <param name="one">Caja uno</param>
/// <param name="two">Caja dos</param>
/// <param name="data">Datos de colisión</param>
/// <returns>Devuelve verdadero si hay colisión, o falso en el resto de los casos</returns>
public static bool BoxAndBox(CollisionBox one, CollisionBox two, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Encontrar el vector entre los dos centros
Vector3 toCentre = two.Position - one.Position;
// Se asume que no hay contacto
float pen = float.MaxValue;
uint best = uint.MaxValue;
// Chequear cada eje, almacenando penetración y el mejor eje
if (!TryAxis(one, two, one.XAxis, toCentre, 0, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, one.YAxis, toCentre, 1, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, one.ZAxis, toCentre, 2, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, two.XAxis, toCentre, 3, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, two.YAxis, toCentre, 4, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, two.ZAxis, toCentre, 5, ref pen, ref best))
{
return(false);
}
// Almacenar el mejor eje hasta ahora, en el caso de estar en una colisión de ejes paralelos más adelante.
uint bestSingleAxis = best;
if (!TryAxis(one, two, Vector3.Cross(one.XAxis, two.XAxis), toCentre, 6, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.XAxis, two.YAxis), toCentre, 7, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.XAxis, two.ZAxis), toCentre, 8, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.YAxis, two.XAxis), toCentre, 9, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.YAxis, two.YAxis), toCentre, 10, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.YAxis, two.ZAxis), toCentre, 11, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.ZAxis, two.XAxis), toCentre, 12, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.ZAxis, two.YAxis), toCentre, 13, ref pen, ref best))
{
return(false);
}
if (!TryAxis(one, two, Vector3.Cross(one.ZAxis, two.ZAxis), toCentre, 14, ref pen, ref best))
{
return(false);
}
// Asegurarse de que tenemos un resultado.
if (best != uint.MaxValue)
{
// Tenemos colisión, y tenemos el eje de colisión con menor penetración
if (best < 3)
{
// Hay un vértice la caja dos en una cara de la caja uno.
FillPointFaceBoxBox(one, two, toCentre, best, pen, ref data);
data.AddContact();
return(true);
}
else if (best < 6)
{
// Hay un vértice de la caja uno en una cara de la caja dos.
FillPointFaceBoxBox(two, one, toCentre * -1.0f, best - 3, pen, ref data);
data.AddContact();
return(true);
}
else
{
// Contacto canto a canto. Obtener el eje común.
best -= 6;
uint oneAxisIndex = best / 3;
uint twoAxisIndex = best % 3;
Vector3 oneAxis = one.GetAxis((TransformAxis)oneAxisIndex);
Vector3 twoAxis = two.GetAxis((TransformAxis)twoAxisIndex);
Vector3 axis = Vector3.Cross(oneAxis, twoAxis);
axis.Normalize();
// El eje debería apuntar desde la caja uno a la dos.
if (Vector3.Dot(axis, toCentre) > 0f)
{
axis = axis * -1.0f;
}
// Tenemos los ejes, pero no los cantos.
// Cada eje tiene 4 cantos paralelos a él, tenemos que encontrar los 4 de cada caja.
// Buscaremos el punto en el centro del canto. Sabemos que su componente en el eje de colisión es 0 y
// determinamos cual de los extremos en cada uno de los otros ejes es el más cercano.
Vector3 vOne = one.HalfSize;
Vector3 vTwo = two.HalfSize;
float[] ptOnOneEdge = new float[] { vOne.X, vOne.Y, vOne.Z };
float[] ptOnTwoEdge = new float[] { vTwo.X, vTwo.Y, vTwo.Z };
for (uint i = 0; i < 3; i++)
{
if (i == oneAxisIndex)
{
ptOnOneEdge[i] = 0;
}
else if (Vector3.Dot(one.GetAxis((TransformAxis)i), axis) > 0f)
{
ptOnOneEdge[i] = -ptOnOneEdge[i];
}
if (i == twoAxisIndex)
{
ptOnTwoEdge[i] = 0;
}
else if (Vector3.Dot(two.GetAxis((TransformAxis)i), axis) < 0f)
{
ptOnTwoEdge[i] = -ptOnTwoEdge[i];
}
}
vOne.X = ptOnOneEdge[0];
vOne.Y = ptOnOneEdge[1];
vOne.Z = ptOnOneEdge[2];
vTwo.X = ptOnTwoEdge[0];
vTwo.Y = ptOnTwoEdge[1];
vTwo.Z = ptOnTwoEdge[2];
// Pasar a coordenadas del mundo
vOne = Vector3.Transform(vOne, one.Transform);
vTwo = Vector3.Transform(vTwo, two.Transform);
// Tenemos un punto y una dirección para los cantos que colisionan.
// Necesitamos encontrar el punto de mayor cercanía de los dos segmentos.
float[] vOneAxis = new float[] { one.HalfSize.X, one.HalfSize.Y, one.HalfSize.Z };
float[] vTwoAxis = new float[] { two.HalfSize.X, two.HalfSize.Y, two.HalfSize.Z };
Vector3 vertex = ContactPoint(
vOne, oneAxis, vOneAxis[oneAxisIndex],
vTwo, twoAxis, vTwoAxis[twoAxisIndex],
bestSingleAxis > 2);
// Llenar el contacto.
Contact contact = data.CurrentContact;
contact.Penetration = pen;
contact.ContactNormal = axis;
contact.ContactPoint = vertex;
contact.SetBodyData(ref one.Body, ref two.Body, data.Friction, data.Restitution);
data.AddContact();
return(true);
}
}
return(false);
}
/// <summary>
/// Detecta la colisión entre una caja y una esfera
/// </summary>
/// <param name="box">Caja</param>
/// <param name="sphere">Esfera</param>
/// <param name="data">Datos de colisión a llenar</param>
/// <returns>Devuelve verdadero si existe colisión, falso en el resto de los casos</returns>
public static bool BoxAndSphere(CollisionBox box, CollisionSphere sphere, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Transformar el cetro de la esfera
Vector3 centre = sphere.Position;
Vector3 relCentre = Vector3.Transform(centre, Matrix.Invert(box.Transform));
// Comprobar si se puede excluir el contacto
if (Math.Abs(relCentre.X) - sphere.Radius > box.HalfSize.X ||
Math.Abs(relCentre.Y) - sphere.Radius > box.HalfSize.Y ||
Math.Abs(relCentre.Z) - sphere.Radius > box.HalfSize.Z)
{
return(false);
}
Vector3 closestPt = Vector3.Zero;
float dist = relCentre.X;
if (dist > box.HalfSize.X)
{
dist = box.HalfSize.X;
}
if (dist < -box.HalfSize.X)
{
dist = -box.HalfSize.X;
}
closestPt.X = dist;
dist = relCentre.Y;
if (dist > box.HalfSize.Y)
{
dist = box.HalfSize.Y;
}
if (dist < -box.HalfSize.Y)
{
dist = -box.HalfSize.Y;
}
closestPt.Y = dist;
dist = relCentre.Z;
if (dist > box.HalfSize.Z)
{
dist = box.HalfSize.Z;
}
if (dist < -box.HalfSize.Z)
{
dist = -box.HalfSize.Z;
}
closestPt.Z = dist;
// Comprobar si estamos en contacto.
dist = (closestPt - relCentre).LengthSquared();
if (dist > sphere.Radius * sphere.Radius)
{
return(false);
}
Vector3 closestPtWorld = Vector3.Transform(closestPt, box.Transform);
//HACKBYME: Añadimos la velocidad de la esfera para calcular la normal
Vector3 relativeVelocity = sphere.Body.Velocity + box.Body.Velocity;
Vector3 normal = Vector3.Normalize(closestPtWorld - centre + relativeVelocity);
Contact contact = data.CurrentContact;
contact.ContactNormal = normal;
contact.ContactPoint = closestPtWorld;
contact.Penetration = sphere.Radius - (float)Math.Sqrt(dist);
contact.SetBodyData(ref box.Body, ref sphere.Body, data.Friction, data.Restitution);
data.AddContact();
return(true);
}
/// <summary>
/// Detecta la colisión entre una esfera y una lista de triángulos
/// </summary>
/// <param name="sphere">Esfera</param>
/// <param name="triangleSoup">Lista de triángulos</param>
/// <param name="data">Datos de la colisión</param>
/// <returns>Devuelve verdadero si hay colisión, o falso en el resto de los casos</returns>
public static bool SphereAndTriangleSoup(CollisionSphere sphere, CollisionTriangleSoup triangleSoup, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
foreach (Triangle triangle in triangleSoup.Triangles)
{
// Comprobar la intersección
if (IntersectionTests.SphereAndTri(sphere, triangle, true))
{
// Informar la colisión
if (CollisionDetector.SphereAndHalfSpace(sphere, new CollisionPlane(triangle.Plane), ref data))
{
return(true);
}
}
}
return(false);
}
/// <summary>
/// Detecta la colisión entre una esfera y un plano
/// </summary>
/// <param name="sphere">Esfera</param>
/// <param name="plane">Plano</param>
/// <param name="data">Rellena los datos de colisión</param>
/// <returns>Devuelve verdadero si hay colisión o falso en el resto de los casos</returns>
public static bool SphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Almacenar la posición de la esfera
Vector3 position = sphere.Position;
// Obtener la distancia al plano.
float ballDistance = Vector3.Dot(plane.Normal, position) - sphere.Radius - plane.D;
if (ballDistance >= 0)
{
return(false);
}
// Crear el contacto. Tiene una normal en la dirección del plano.
Contact contact = data.CurrentContact;
contact.ContactNormal = plane.Normal;
contact.Penetration = -ballDistance;
contact.ContactPoint = position - plane.Normal * (ballDistance + sphere.Radius);
// No hay cuerpo para el plano. Se considera escenario.
RigidBody two = null;
contact.SetBodyData(ref sphere.Body, ref two, data.Friction, data.Restitution);
// Añadir el contacto
data.AddContact();
return(true);
}
public void ResolveCollision(CollisionData collision)
{
// Projectiles shouldn't collide with eachother
if (collision.BodyA is Projectile && collision.BodyB is Projectile)
{
return;
}
// Characters shouldn't collide with eachother
if (collision.BodyA is Character && collision.BodyB is Character && collision.IgnoreCollision)
{
return;
}
// Check what has collided with the target
if (collision.BodyB.IsTarget || (collision.BodyA.IsTarget && collision.BodyB.RigidBody.IsDynamic))
{
if (collision.BodyB is Character)
{
Character c = (Character)collision.BodyB;
// If not a player, then must be an AI hitting the target, so delete the AI and lower the portal health
if (!c.IsPlayer)
{
collision.BodyB.MarkedForDeletion = true;
World.PortalHealth -= 10;
}
}
else if (collision.BodyA is Character)
{
Character c = (Character)collision.BodyA;
// If not a player, then must be an AI hitting the target, so delete the AI and lower the portal health
if (!c.IsPlayer)
{
collision.BodyA.MarkedForDeletion = true;
World.PortalHealth -= 10;
}
}
return;
}
collision.ContactNormal = Vector2.Normalize(collision.ContactNormal);
RigidBody rb_A = collision.BodyA.RigidBody;
RigidBody rb_B = collision.BodyB.RigidBody;
// Determine if the collision involves a projectile
if (collision.BodyA is Projectile)
{
if (collision.BodyB is Character)
{
Character c = (Character)collision.BodyB;
// Check if the target hit is not the player
if (!c.IsPlayer)
{
World.Player.BallAmmo++;
if (World.Player.BallAmmo > World.Player.MaximumBallAmmo)
{
World.Player.BallAmmo = World.Player.MaximumBallAmmo;
}
}
else
{
return;
}
if (!World.ProjectileCharacterCollision)
{
if (!c.IsPlayer)
{
collision.BodyB.MarkedForDeletion = true;
}
return;
}
}
collision.Elasticity = collision.BodyA.Elasticity;
Projectile projectile = (Projectile)collision.BodyA;
projectile.NumOfBounces++;
}
else if (collision.BodyB is Projectile)
{
if (collision.BodyA is Character)
{
Character c = (Character)collision.BodyA;
// Check if the target hit is not the player
if (!c.IsPlayer)
{
World.Player.BallAmmo++;
if (World.Player.BallAmmo > World.Player.MaximumBallAmmo)
{
World.Player.BallAmmo = World.Player.MaximumBallAmmo;
}
}
else
{
return;
}
if (!World.ProjectileCharacterCollision)
{
if (!c.IsPlayer)
{
collision.BodyA.MarkedForDeletion = true;
}
return;
}
}
collision.Elasticity = collision.BodyB.Elasticity;
Projectile projectile = (Projectile)collision.BodyB;
projectile.NumOfBounces++;
}
else if (collision.BodyA.IsTarget)
{
collision.Elasticity = collision.BodyA.Elasticity;
}
else
{
collision.Elasticity = collision.BodyB.Elasticity;
}
// If body A is sitting on top of body B
if (collision.BodyA is Character && collision.ContactNormal.Y == -1)
{
if (!collision.BodyB.BodiesOnTop.Contains(collision.BodyA))
{
collision.BodyB.BodiesOnTop.Add(collision.BodyA);
}
}
#region Displacement Method
Vector2 displacement = new Vector2();
Vector2 queryPoint = new Vector2();
queryPoint = rb_A.Position;
float d = -(Vector2.Dot(collision.ContactNormal, collision.ContactPoint));
float penetrationDepth = Vector2.Dot(queryPoint, collision.ContactNormal) + d;
displacement = (collision.ContactNormal * penetrationDepth) * -1;
if (rb_A.IsDynamic)
{
rb_A.Position -= displacement;
}
if (rb_B.IsDynamic)
{
rb_B.Position += displacement;
}
#endregion
#region Impulse Method
Vector2 relativeVelocity = rb_A.LinearVelocity - rb_B.LinearVelocity;
Vector2 bodyAInitialVelocity = new Vector2(rb_A.LinearVelocity.X, rb_A.LinearVelocity.Y);
Vector2 bodyBInitialVelocity = new Vector2(rb_B.LinearVelocity.X, rb_B.LinearVelocity.Y);
// 1 for perfectly elastic, 0 for perfectly inelastic
float impulse;
if (!rb_A.IsDynamic)
{
impulse = ((-(1 + collision.Elasticity)) * (Vector2.Dot(relativeVelocity, collision.ContactNormal))) / ((1 / rb_B.Mass));
if (!(collision.BodyA is MovingPlatform))
{
rb_A.LinearVelocity = new Vector2(0, 0);
}
rb_B.LinearVelocity = bodyBInitialVelocity - collision.ContactNormal * (impulse * (1 / rb_B.Mass));
}
else if (!rb_B.IsDynamic)
{
impulse = ((-(1 + collision.Elasticity)) * (Vector2.Dot(relativeVelocity, collision.ContactNormal))) / ((1 / rb_A.Mass));
if (!(collision.BodyB is MovingPlatform))
{
rb_B.LinearVelocity = new Vector2(0, 0);
}
rb_A.LinearVelocity = bodyAInitialVelocity + collision.ContactNormal * (impulse * (1 / rb_A.Mass));
}
else
{
impulse = ((-(1 + collision.Elasticity)) * (Vector2.Dot(relativeVelocity, collision.ContactNormal))) / ((1 / rb_A.Mass) + (1 / rb_B.Mass));
rb_A.LinearVelocity = bodyAInitialVelocity + collision.ContactNormal * (impulse * (1 / rb_A.Mass));
rb_B.LinearVelocity = bodyBInitialVelocity - collision.ContactNormal * (impulse * (1 / rb_B.Mass));
}
#endregion
}
/// <summary>
/// Detecta la colisión entre dos objetos
/// </summary>
/// <param name="obj1">Objeto primero</param>
/// <param name="obj2">Objeto segundo</param>
/// <param name="data">Información de colisión</param>
/// <returns>Devuelve verdadero si ha habido colisión</returns>
public static bool BetweenObjects(ref CollisionPrimitive primitive1, ref CollisionPrimitive primitive2, ref CollisionData data)
{
if (primitive1 != null && primitive2 != null)
{
if (primitive1 is CollisionBox)
{
if (primitive2 is CollisionBox)
{
return(BoxAndBox((CollisionBox)primitive1, (CollisionBox)primitive2, ref data));
}
else if (primitive2 is CollisionSphere)
{
return(BoxAndSphere((CollisionBox)primitive1, (CollisionSphere)primitive2, ref data));
}
else if (primitive2 is CollisionPlane)
{
return(BoxAndHalfSpace((CollisionBox)primitive1, (CollisionPlane)primitive2, ref data));
}
else if (primitive2 is CollisionTriangleSoup)
{
return(BoxAndTriangleSoup((CollisionBox)primitive1, (CollisionTriangleSoup)primitive2, ref data));
}
}
else if (primitive1 is CollisionSphere)
{
if (primitive2 is CollisionBox)
{
return(BoxAndSphere((CollisionBox)primitive2, (CollisionSphere)primitive1, ref data));
}
else if (primitive2 is CollisionSphere)
{
return(SphereAndSphere((CollisionSphere)primitive1, (CollisionSphere)primitive2, ref data));
}
else if (primitive2 is CollisionPlane)
{
return(SphereAndHalfSpace((CollisionSphere)primitive1, (CollisionPlane)primitive2, ref data));
}
else if (primitive2 is CollisionTriangleSoup)
{
return(SphereAndTriangleSoup((CollisionSphere)primitive1, (CollisionTriangleSoup)primitive2, ref data));
}
}
throw new Exception("Tipo de colisión no controlada");
}
return(false);
}
/// <summary>
/// Detecta la colisión entre una caja y una lista de triángulos
/// </summary>
/// <param name="box">Caja</param>
/// <param name="triangleList">Lista de triángulos</param>
/// <param name="data">Datos de colisión a llenar</param>
/// <returns>Devuelve verdadero si existe colisión, falso en el resto de los casos</returns>
private static bool BoxAndTriangleList(CollisionBox box, Triangle[] triangleList, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
bool intersection = false;
if (data.ContactsLeft > 0)
{
int firstContact = data.ContactCount;
foreach (Triangle triangle in triangleList)
{
// Comprobar la intersección con el triángulo
if (IntersectionTests.BoxAndTri(box, triangle))
{
if (data.ContactsLeft > 0)
{
if (CollisionDetector.BoxAndTriangle(box, triangle, ref data))
{
intersection = true;
}
}
else
{
break;
}
}
}
//int contactCount = data.ContactCount - firstContact;
//if (intersection && contactCount > 1)
//{
// //Agrupar los contactos
// Vector3 contactPoint = Vector3.Zero;
// Vector3 contactNormal = Vector3.Zero;
// float penetration = 0f;
// for (int i = firstContact; i < data.ContactCount; i++)
// {
// contactPoint += data.ContactArray[i].ContactPoint;
// contactNormal += data.ContactArray[i].ContactNormal;
// penetration += data.ContactArray[i].Penetration;
// }
// contactPoint /= contactCount;
// contactNormal /= contactCount;
// penetration /= contactCount;
// Contact newContact = new Contact();
// data.ContactArray[firstContact].ContactPoint = contactPoint;
// data.ContactArray[firstContact].ContactNormal = Vector3.Normalize(contactNormal);
// data.ContactArray[firstContact].Penetration = penetration;
// data.SetContactIndex(firstContact + 1);
//}
}
return(intersection);
}
/// <summary>
/// Detecta la colisión entre una esfera y un plano
/// </summary>
/// <param name="sphere">Esfera</param>
/// <param name="plane">Plano</param>
/// <param name="data">Rellena los datos de colisión</param>
/// <returns>Devuelve verdadero si hay colisión o falso en el resto de los casos</returns>
private static bool SphereAndHalfSpace(CollisionSphere sphere, CollisionPlane plane, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Distancia del centro al plano
float centerToPlane = Math.Abs(Vector3.Dot(plane.Normal, sphere.Position) + plane.D);
// Obtener la penetración de la esfera en el plano.
float penetration = centerToPlane - sphere.Radius;
if (penetration >= 0)
{
return(false);
}
// Crear el contacto. Tiene una normal en la dirección del plano.
Contact contact = data.CurrentContact;
contact.ContactNormal = plane.Normal;
contact.Penetration = -penetration;
contact.ContactPoint = sphere.Position - plane.Normal * centerToPlane;
// No hay cuerpo para el plano. Se considera escenario.
RigidBody one = sphere;
RigidBody two = null;
contact.SetBodyData(ref one, ref two, data.Friction, data.Restitution);
// Añadir el contacto
data.AddContact();
return(true);
}
/// <summary>
/// Detecta la colisión entre una caja y una colección de triángulos
/// </summary>
/// <param name="box">Caja</param>
/// <param name="triangleSoup">Colección de triángulos</param>
/// <param name="data">Datos de colisión a llenar</param>
/// <returns>Devuelve verdadero si existe colisión, falso en el resto de los casos</returns>
private static bool BoxAndTriangleSoup(CollisionBox box, CollisionTriangleSoup triangleSoup, ref CollisionData data)
{
//CollisionPlane plane = new CollisionPlane(triangleSoup.Triangles[0].Plane, triangleSoup.Mass);
//return BoxAndHalfSpace(box, plane, ref data);
return(BoxAndTriangleList(box, triangleSoup.Triangles, ref data));
}
/// <summary>
/// Detecta la colisión entre una esfera y una lista de triángulos
/// </summary>
/// <param name="sphere">Esfera</param>
/// <param name="triangleList">Lista de triángulos</param>
/// <param name="data">Datos de la colisión</param>
/// <returns>Devuelve verdadero si hay colisión, o falso en el resto de los casos</returns>
private static bool SphereAndTriangleList(CollisionSphere sphere, Triangle[] triangleList, ref CollisionData data)
{
bool contact = false;
if (data.ContactsLeft > 0)
{
if (triangleList != null && triangleList.Length > 0)
{
foreach (Triangle triangle in triangleList)
{
if (data.ContactsLeft > 0)
{
// Comprobar la intersección
if (CollisionDetector.SphereAndTriangle(sphere, triangle, ref data))
{
contact = true;
}
}
else
{
break;
}
}
}
}
return(contact);
}
/// <summary>
/// Detecta la colisión entre una esfera y una lista de triángulos
/// </summary>
/// <param name="sphere">Esfera</param>
/// <param name="triangleSoup">Lista de triángulos</param>
/// <param name="data">Datos de la colisión</param>
/// <returns>Devuelve verdadero si hay colisión, o falso en el resto de los casos</returns>
private static bool SphereAndTriangleSoup(CollisionSphere sphere, CollisionTriangleSoup triangleSoup, ref CollisionData data)
{
return(SphereAndTriangleList(sphere, triangleSoup.Triangles, ref data));
}
/// <summary>
/// Llena la información de colisión entre dos cajas, una vez se conoce que hay contacto del tipo vértice - cara
/// </summary>
/// <param name="one">Caja uno</param>
/// <param name="two">Caja dos</param>
/// <param name="toCentre">Distancia entre centros</param>
/// <param name="data">Información de colisión</param>
/// <param name="best">Eje de penetración menor</param>
/// <param name="pen">Pentración menor</param>
private static void FillPointFaceBoxBox(CollisionBox one, CollisionBox two, Vector3 toCentre, uint best, float pen, ref CollisionData data)
{
// Sabemos cual es el eje de la colisión, pero tenemos que conocer con qué cara tenemos que trabjar
Vector3 normal = one.GetAxis((TransformAxis)best);
if (Vector3.Dot(one.GetAxis((TransformAxis)best), toCentre) > 0f)
{
normal = normal * -1.0f;
}
// Obtenemos el vértice
Vector3 vertex = two.HalfSize;
if (Vector3.Dot(two.XAxis, normal) < 0f)
{
vertex.X = -vertex.X;
}
if (Vector3.Dot(two.YAxis, normal) < 0f)
{
vertex.Y = -vertex.Y;
}
if (Vector3.Dot(two.ZAxis, normal) < 0f)
{
vertex.Z = -vertex.Z;
}
Contact contact = data.CurrentContact;
contact.ContactNormal = normal;
contact.Penetration = pen;
contact.ContactPoint = Vector3.Transform(vertex, two.Transform);
contact.SetBodyData(ref one.Body, ref two.Body, data.Friction, data.Restitution);
}
/// <summary> /// Genera los contactos entre los cuerpos incluídos en el generador de contactos /// </summary> /// <param name="contactData">Información de contactos</param> /// <param name="limit">Límite de contactos a generar</param> /// <returns>Devuelve el número de contactos generados</returns> public abstract int AddContact(ref CollisionData contactData, int limit);
/// <summary>
/// Añade los contactos necesarios para mantener unidos mediante la barra a los cuerpos
/// </summary>
/// <param name="contactData">Datos de colisión</param>
/// <param name="limit">Límite de contactos a añadir</param>
/// <returns>Devuelve el número de contacos añadidos</returns>
/// <remarks>Sólo añade un contacto o ninguno</remarks>
public override int AddContact(ref CollisionData contactData, int limit)
{
if (contactData.HasFreeContacts())
{
CollisionPrimitive objectOne = null;
if (this.m_BodyOne != null)
{
objectOne = this.m_BodyOne.Primitive;
}
CollisionPrimitive objectTwo = null;
if (this.m_BodyTwo != null)
{
objectTwo = this.m_BodyTwo.Primitive;
}
// Encontrar la longitud actual
Vector3 positionOne = this.m_RelativePointOne;
Vector3 positionOneWorld = this.m_RelativePointOne;
if (objectOne != null)
{
positionOne = objectOne.Position;
positionOneWorld = objectOne.GetPointInWorldSpace(this.m_RelativePointOne);
}
Vector3 positionTwo = this.m_RelativePointTwo;
Vector3 positionTwoWorld = this.m_RelativePointTwo;
if (objectTwo != null)
{
positionTwo = objectTwo.Position;
positionTwoWorld = objectTwo.GetPointInWorldSpace(this.m_RelativePointTwo);
}
float currentLen = Vector3.Distance(positionOneWorld, positionTwoWorld);
// Comprobar si estamos en extensión correcta
if (currentLen != m_Length)
{
// Rellenar el contacto
Contact contact = contactData.CurrentContact;
contact.Bodies[0] = objectOne;
contact.Bodies[1] = objectTwo;
contact.ContactPoint = (positionOneWorld + positionTwoWorld) * 0.5f;
// Calcular la normal
Vector3 normal = Vector3.Normalize(positionTwo - positionOne);
// La normal de contacto depende de si hay que extender o contraer para conservar la longitud
if (currentLen > m_Length)
{
contact.ContactNormal = normal;
contact.Penetration = currentLen - m_Length;
}
else
{
contact.ContactNormal = Vector3.Negate(normal);
contact.Penetration = this.m_Length - currentLen;
}
// Siempre restitución 0
contact.Restitution = 0f;
contactData.AddContact();
return(1);
}
}
return(0);
}
/// <summary>
/// Detecta la colisión entre dos esferas
/// </summary>
/// <param name="one">Esfera uno</param>
/// <param name="two">Esfera dos</param>
/// <param name="data">Datos de la colisión</param>
/// <returns>Devuelve verdadero si hay colisión, o falso en el resto de los casos</returns>
private static bool SphereAndSphere(CollisionSphere one, CollisionSphere two, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Almacenar las posiciones de las esferas
Vector3 positionOne = one.Position;
Vector3 positionTwo = two.Position;
// Encontrar el vector entre los objetos
Vector3 midline = positionOne - positionTwo;
float size = midline.Length();
if (size <= 0.0f || size >= one.Radius + two.Radius)
{
return(false);
}
// Obtener la normal de forma manual
Vector3 normal = midline * (1.0f / size);
Contact contact = data.CurrentContact;
contact.ContactNormal = normal;
contact.ContactPoint = positionOne + midline * 0.5f;
contact.Penetration = (one.Radius + two.Radius - size);
RigidBody rbOne = one;
RigidBody rbTwo = two;
contact.SetBodyData(ref rbOne, ref rbTwo, data.Friction, data.Restitution);
data.AddContact();
return(true);
}