Ejemplo n.º 1
0
    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));
        }
    }