public void addForces()
{
// TO BE COMPLETED: ADD SOFT CONSTRAINT FORCES TO THE RIGID BODIES
// RESTRICCIONES DEBILES:
// por sólidos articulados (3 Restricciones por ser en 3D):
// C = Ra*ra + Xa - Rb*rb - Xb = 0
// Los rigidBodies pueden ser null si la constraint no está asociada a uno
// Por lo que de forma genérica deberían ser cero y así, si son null,
// no se tienen en cuenta.
// referencia en local de la posición del rigidbody
Vector3 ra = m_pA;
Vector3 rb = m_pB;
// Posición del rigidbody
Vector3 Xa = Vector3.zero;
Vector3 Xb = Vector3.zero;
//Rotaciones de los rigidbody
Quaternion Ra = Quaternion.identity;
Quaternion Rb = Quaternion.identity;
// Si tenemos el cuerpo 1 asociado - Cogemos su posición y rotación para calcular C
if (BodyA != null)
{
Xa = BodyA.Position;
Ra = BodyA.Rotation;
}
// Si tenemos el cuerpo 2 asociado - Cogemos su posición y rotación para calcular C
if (BodyB != null)
{
Xb = BodyB.Position;
Rb = BodyB.Rotation;
}
// Sin cuerpos, Xb/Xa y Rb/Ra son 0
Vector3 C = (Ra * ra + Xa - Rb * rb - Xb);
// Restricciones débiles:
// Fuerza = -Gradiente de la energía
if (BodyA != null)
{
// Grad de ||Xa-xb|| -> 2 * (xa-xb / | xa-xb |)
float XaXb = (Xa - Xb).magnitude;
// Gradiente
float C_Grad_Xa = 2f * (XaXb / Mathf.Abs(XaXb));
// Fa = - k * C Grad Xa * C
Vector3 Fa = -m_manager.K * C_Grad_Xa * C;
BodyA.AddToForce(Fa);
BodyA.AddToTorque(Vector3.Cross(BodyA.Rotation * m_pA, Fa));
}
if (BodyB != null)
{
// Grad de ||Xb-xa|| -> 2 * (xb-xa / | xb-xa |)
float XbXa = (Xb - Xa).magnitude;
// Gradiente
float C_Grad_Xb = 2f * (-XbXa / Mathf.Abs(XbXa));
// Fb = - k * C Grad Xb * C
Vector3 Fb = -m_manager.K * C_Grad_Xb * C;
BodyB.AddToForce(Fb);
BodyB.AddToTorque(Vector3.Cross(BodyB.Rotation * m_pB, Fb));
}
}