/// <summary> /// Detecta la intersección entre una caja y un triángulo /// </summary> /// <param name="box">Caja</param> /// <param name="tri">Triángulo</param> /// <returns>Devuelve verdadero si hay intersección</returns> public static bool BoxAndTri(CollisionBox box, Triangle tri) { //Transformar la caja a coordenadas locales para poder usar un AABB-Triangle Quaternion orientation = box.Orientation; Quaternion orientationInv; Quaternion.Conjugate(ref orientation, out orientationInv); //Llevar el triángulo a las coordenadas de la caja Matrix minv = Matrix.CreateFromQuaternion(orientationInv); Vector3 localPoint1 = Vector3.TransformNormal(tri.Point1 - box.Position, minv); Vector3 localPoint2 = Vector3.TransformNormal(tri.Point2 - box.Position, minv); Vector3 localPoint3 = Vector3.TransformNormal(tri.Point3 - box.Position, minv); Triangle localTri = new Triangle(localPoint1, localPoint2, localPoint3); BoundingBox localTriBounds = PhysicsMathHelper.GenerateFromTriangle(localTri); Vector3 localTriBoundhalfExtent = localTriBounds.GetHalfSizes(); Vector3 localTriBoundCenter = localTriBounds.GetCenter(); float localTriBoundCenterX = Math.Abs(localTriBoundCenter.X); float localTriBoundCenterY = Math.Abs(localTriBoundCenter.Y); float localTriBoundCenterZ = Math.Abs(localTriBoundCenter.Z); if (localTriBoundhalfExtent.X + box.HalfSize.X <= localTriBoundCenterX || localTriBoundhalfExtent.Y + box.HalfSize.Y <= localTriBoundCenterY || localTriBoundhalfExtent.Z + box.HalfSize.Z <= localTriBoundCenterZ) { //El cuerpo está fuera de la caja return(false); } if (localTriBoundhalfExtent.X + localTriBoundCenterX <= box.HalfSize.X && localTriBoundhalfExtent.Y + localTriBoundCenterY <= box.HalfSize.Y && localTriBoundhalfExtent.Z + localTriBoundCenterZ <= box.HalfSize.Z) { //El cuerpo está dentro de la caja return(true); } Vector3 point1 = localTri.Point1; Vector3 point2 = localTri.Point2; Vector3 point3 = localTri.Point3; //Obtener eje 1, entre el punto 1 y el 2 del triángulo Vector3 edge1; Vector3.Subtract(ref point2, ref point1, out edge1); //Obtener eje 2, entre el punto 1 y el 3 del triángulo Vector3 edge2; Vector3.Subtract(ref point3, ref point1, out edge2); //Obtener el eje perpendicular entre los dos ejes Vector3 crossEdge; Vector3.Cross(ref edge1, ref edge2, out crossEdge); //Obtener la distancia del triángulo al eje float triangleDist = Vector3.Dot(localTri.Point1, crossEdge); if (Math.Abs(crossEdge.X * box.HalfSize.X) + Math.Abs(crossEdge.Y * box.HalfSize.Y) + Math.Abs(crossEdge.Z * box.HalfSize.Z) <= Math.Abs(triangleDist)) { return(false); } // No hay resultados en los tests con el AABB del triángulo, hay que probar los 9 casos, 3 por eje // Al usar la transformación local de la caja, cada plano calculado es paralelo a cada eje de la caja // Como son paralelos, el producto es siempre 0 y se puede omitir //Obtener eje 3, entre el punto 2 y el 3 del triángulo Vector3 edge3; Vector3.Subtract(ref point2, ref point3, out edge3); if (OverlapOnAxis(box, localTri, edge1, 0)) { return(false); } if (OverlapOnAxis(box, localTri, edge2, 0)) { return(false); } if (OverlapOnAxis(box, localTri, edge3, 0)) { return(false); } if (OverlapOnAxis(box, localTri, edge1, 1)) { return(false); } if (OverlapOnAxis(box, localTri, edge2, 1)) { return(false); } if (OverlapOnAxis(box, localTri, edge3, 1)) { return(false); } if (OverlapOnAxis(box, localTri, edge1, 2)) { return(false); } if (OverlapOnAxis(box, localTri, edge2, 2)) { return(false); } if (OverlapOnAxis(box, localTri, edge3, 2)) { return(false); } #if DEBUG if (m_DEBUGUSE) { m_DEBUGTRI[m_DEBUGTRICOUNT++] = tri; m_DEBUGAABB[m_DEBUGAABBCOUNT++] = PhysicsMathHelper.GenerateFromTriangle(tri); } #endif return(true); }
/// <summary> /// Integra el cuerpo en el tiempo /// </summary> /// <param name="duration">Tiempo</param> public void Integrate(float duration) { if (this.m_IsAwake) { // Obtener los coheficientes de rebote para este intervalo de tiempo float linearDampingOnTime = PhysicsMathHelper.Pow(this.m_LinearDamping, duration); float angularDampingOnTime = PhysicsMathHelper.Pow(this.m_AngularDamping, duration); // Calcular la aceleración lineal desde las fuerzas this.m_CurrentAcceleration = this.m_ConstantAcceleration; if (this.m_ForceAccum != Vector3.Zero && this.m_InverseMass != 0f) { this.m_CurrentAcceleration += Vector3.Multiply(this.m_ForceAccum, this.m_InverseMass); } // Calcular la aceleración angular desde los pares de torsión Vector3 angularAcceleration = Matrix3.Transform(this.m_InverseInertiaTensorWorld, this.m_TorqueAccum); // Ajustar velocidades // Actualizar la velocidad lineal usando aceleración lineal this.m_LinearVelocity += Vector3.Multiply(this.m_CurrentAcceleration, duration); // Actualizar la velocidad angular usando aceleración angular this.m_AngularVelocity += Vector3.Multiply(angularAcceleration, duration); // Aplicar los coheficientes de rebote this.m_LinearVelocity *= linearDampingOnTime; this.m_AngularVelocity *= angularDampingOnTime; // Ajustar posiciones // Actualizar posición lineal if (this.m_LinearVelocity != Vector3.Zero) { this.m_Position += Vector3.Multiply(this.m_LinearVelocity, duration); } // Actualizar orientación (posición angular) if (this.m_AngularVelocity != Vector3.Zero) { this.m_Orientation += new Quaternion(this.m_AngularVelocity * duration, 0f) * this.m_Orientation; } // Aplicar los coheficientes de rebote this.m_LinearVelocity *= linearDampingOnTime; this.m_AngularVelocity *= angularDampingOnTime; // Normalizar la orientación y actualizar las matrices con la nueva posición y orientación this.CalculateDerivedData(); // Limpiar los acumuladores de fuerzas this.ClearAccumulators(); // Actualizar el acumulador de energía cinética if (this.m_CanSleep) { // Energía cinética actual float currentMotion = Vector3.Dot(this.m_LinearVelocity, this.m_LinearVelocity) + Vector3.Dot(this.m_AngularVelocity, this.m_AngularVelocity); float bias = PhysicsMathHelper.Pow(0.5f, duration); this.m_Motion = bias * this.m_Motion + (1f - bias) * currentMotion; if (this.m_Motion < Constants.SleepEpsilon) { // Si no hay energía cinética suficiente se pone el cuerpo a dormir this.IsAwake = false; } else if (this.m_Motion > 10f * Constants.SleepEpsilon) { // Acumular la energía cinética this.m_Motion = 10f * Constants.SleepEpsilon; } } } }