/// <summary>
/// Genera los contactos requeridos para restaurar la unión si ha sido violada
/// </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>
/// <remarks>Tan solo generará un contacto o ninguno</remarks>
public override int AddContact(ref CollisionData contactData, int limit)
{
if (contactData.HasMoreContacts())
{
// Calcular las posiciones de los puntos de conexión en coordenadas del mundo
Vector3 positionOneWorld = this.m_BodyOne.GetPointInWorldSpace(this.m_PositionOne);
Vector3 positionTwoWorld = this.m_BodyTwo.GetPointInWorldSpace(this.m_PositionTwo);
// Calcular la longitud de la unión
float length = Vector3.Distance(positionTwoWorld, positionOneWorld);
// Check if it is violated
if (Math.Abs(length) > m_Error)
{
Contact contact = contactData.CurrentContact;
contact.Bodies[0] = this.m_BodyOne;
contact.Bodies[1] = this.m_BodyTwo;
contact.ContactNormal = Vector3.Normalize(positionTwoWorld - positionOneWorld);
contact.ContactPoint = (positionOneWorld + positionTwoWorld) * 0.5f;
contact.Penetration = length - m_Error;
contact.Friction = 1.0f;
contact.Restitution = 0;
contactData.AddContact();
return(1);
}
}
return(0);
}
/// <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>
private 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);
}
// Obtener el punto de la caja más cercano al centro de la esfera
Vector3 closestPoint = CollisionBox.ClosestPointInBox(box, sphere.Position);
// Obtener la distancia entre los puntos
float distance = Vector3.Distance(sphere.Position, closestPoint);
if (distance <= sphere.Radius)
{
Vector3 normal = Vector3.Normalize(box.Position - closestPoint);
Contact contact = data.CurrentContact;
contact.ContactNormal = normal;
contact.ContactPoint = closestPoint;
contact.Penetration = sphere.Radius - distance;
RigidBody one = box;
RigidBody two = sphere;
contact.SetBodyData(ref one, ref two, data.Friction, data.Restitution);
data.AddContact();
return(true);
}
return(false);
}
/// <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>
public 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);
contact.SetBodyData(ref one.Body, ref two.Body, data.Friction, data.Restitution);
data.AddContact();
return(true);
}
/// <summary>
/// Detecta la colisión entre una esfera y un triángulo
/// </summary>
/// <param name="sphere">Esfera</param>
/// <param name="tri">Triángulo</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 SphereAndTriangle(CollisionSphere sphere, Triangle tri, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Obtener el punto del triángulo más cercano al centro de la esfera
Vector3 closestPoint = Triangle.ClosestPointInTriangle(tri, sphere.Position);
// Obtener la distancia del punto obtenido al centro de la esfera
float distance = Vector3.Distance(closestPoint, sphere.Position);
if (distance <= sphere.Radius)
{
// Crear el contacto. Tiene una normal en la dirección del plano.
Contact contact = data.CurrentContact;
contact.ContactNormal = tri.Normal;
contact.Penetration = sphere.Radius - distance;
contact.ContactPoint = closestPoint;
RigidBody one = sphere;
RigidBody two = null;
contact.SetBodyData(ref one, ref two, data.Friction, data.Restitution);
data.AddContact();
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// Detecta la colisión entre caja y plano
/// </summary>
/// <param name="box">Caja</param>
/// <param name="plane">Plano</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 BoxAndHalfSpace(CollisionBox box, CollisionPlane plane, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Comprobar la intersección
if (!IntersectionTests.BoxAndHalfSpace(box, plane))
{
return(false);
}
// 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.
uint contactsUsed = 0;
for (int i = 0; i < 8; i++)
{
// Calcular la positición de cada vértice
Vector3 vertexPos = box.GetCorner(i);
// Calcular la distancia al plano
float vertexDistance = Vector3.Dot(vertexPos, plane.Normal);
// Comparar las distancias
if (vertexDistance <= plane.D)
{
// 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 = plane.D - vertexDistance;
// 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();
contactsUsed++;
if (contactsUsed == data.ContactsLeft)
{
return(true);
}
}
}
return(true);
}
/// <summary>
/// Detecta la colisión entre una caja y un punto
/// </summary>
/// <param name="box">Caja</param>
/// <param name="point">Punto</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>
private static bool BoxAndPoint(CollisionBox box, Vector3 point, ref CollisionData data)
{
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// Transformar el punto
Vector3 relPt = Vector3.Transform(point, Matrix.Invert(box.Transform));
// Chequear cada eje buscando el eje en el que la penetración es menos profunda.
float min_depth = box.HalfSize.X - Math.Abs(relPt.X);
if (min_depth < 0)
{
return(false);
}
Vector3 normal = box.XAxis * ((relPt.X < 0) ? -1 : 1);
float depth = box.HalfSize.Y - Math.Abs(relPt.Y);
if (depth < 0)
{
return(false);
}
else if (depth < min_depth)
{
min_depth = depth;
normal = box.YAxis * ((relPt.Y < 0) ? -1 : 1);
}
depth = box.HalfSize.Z - Math.Abs(relPt.Z);
if (depth < 0)
{
return(false);
}
else if (depth < min_depth)
{
min_depth = depth;
normal = box.ZAxis * ((relPt.Z < 0) ? -1 : 1);
}
Contact contact = data.CurrentContact;
contact.ContactNormal = normal;
contact.ContactPoint = point;
contact.Penetration = min_depth;
RigidBody one = box;
RigidBody two = null;
contact.SetBodyData(ref one, ref two, data.Friction, data.Restitution);
data.AddContact();
return(true);
}
/// <summary>
/// Detecta la colisión entre caja y triángulo
/// </summary>
/// <param name="box">Caja</param>
/// <param name="tri">Triángulo</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 BoxAndTriangle(CollisionBox box, Triangle tri, ref CollisionData data)
{
bool intersectionExists = false;
if (data.ContactsLeft <= 0)
{
// Si no hay más contactos disponibles se sale de la función.
return(false);
}
// 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);
// Calcular la distancia al plano
float distanceToPlane = tri.Plane.Distance(vertexPos);
if (distanceToPlane <= 0f)
{
// Si la distancia es negativa está tras el plano. Si es 0, está en el plano
// Intersección entre línea y triángulo
Vector3 direction = Vector3.Normalize(box.Position - vertexPos);
Ray r = new Ray(vertexPos, direction);
if (IntersectionTests.TriAndRay(tri, r))
{
intersectionExists = true;
// 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 = tri.Plane.Normal;
contact.Penetration = -distanceToPlane;
// Establecer los datos del contacto
RigidBody one = box;
RigidBody two = null;
contact.SetBodyData(ref one, ref two, data.Friction, data.Restitution);
// Añadir contacto
data.AddContact();
if (data.ContactsLeft <= 0)
{
return(true);
}
}
}
}
return(intersectionExists);
}
/// <summary>
/// Genera los contactos requeridos para restaurar la unión si ha sido violada
/// </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>
/// <remarks>Tan solo generará 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;
}
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);
if (Math.Abs(currentLen) > this.m_Length)
{
Contact contact = contactData.CurrentContact;
contact.Bodies[0] = objectOne;
contact.Bodies[1] = objectTwo;
contact.ContactNormal = Vector3.Normalize(positionTwoWorld - positionOneWorld);
contact.ContactPoint = (positionOneWorld + positionTwoWorld) * 0.5f;
contact.Penetration = currentLen - this.m_Length;
contact.Friction = 1.0f;
contact.Restitution = 0;
contactData.AddContact();
return(1);
}
}
return(0);
}
/// <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.HasMoreContacts())
{
// Encontrar la longitud actual
Vector3 positionOneWorld = m_BodyOne.GetPointInWorldSpace(m_PositionOne);
Vector3 positionTwoWorld = m_BodyTwo.GetPointInWorldSpace(m_PositionTwo);
float currentLen = Vector3.Distance(positionOneWorld, positionTwoWorld);
// Comprobar si estamos en extensión correcta
if (currentLen == m_Length)
{
return(0);
}
// Rellenar el contacto
Contact contact = contactData.CurrentContact;
contact.Bodies[0] = m_BodyOne;
contact.Bodies[1] = m_BodyTwo;
contact.ContactPoint = (positionOneWorld + positionTwoWorld) * 0.5f;
// Calcular la normal
Vector3 normal = Vector3.Normalize(m_BodyTwo.Position - m_BodyOne.Position);
// 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 = m_Length - currentLen;
}
// Siempre restitución 0
contact.Restitution = 0f;
contactData.AddContact();
return(1);
}
return(0);
}
/// <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 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 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);
}
/// <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 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 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);
}