/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
* Copyright (c) 2010-2012 Daniel Chappuis                                       *
*********************************************************************************
*                                                                               *
* This software is provided 'as-is', without any express or implied warranty.   *
* In no event will the authors be held liable for any damages arising from the  *
* use of this software.                                                         *
*                                                                               *
* Permission is granted to anyone to use this software for any purpose,         *
* including commercial applications, and to alter it and redistribute it        *
* freely, subject to the following restrictions:                                *
*                                                                               *
* 1. The origin of this software must not be misrepresented; you must not claim *
*    that you wrote the original software. If you use this software in a        *
*    product, an acknowledgment in the product documentation would be           *
*    appreciated but is not required.                                           *
*                                                                               *
* 2. Altered source versions must be plainly marked as such, and must not be    *
*    misrepresented as being the original software.                             *
*                                                                               *
* 3. This notice may not be removed or altered from any source distribution.    *
*                                                                               *
********************************************************************************/

// Libraries
#include "ConstraintSolver.h"
#include "DynamicsWorld.h"
#include "../body/RigidBody.h"

using namespace reactphysics3d;
using namespace std;


// Constructor
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
    :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), Vconstraint(0), Wconstraint(0), V1(0), W1(0) {

}

// Destructor
ConstraintSolver::~ConstraintSolver() {

}

 // Initialize the constraint solver before each solving
void ConstraintSolver::initialize() {
    
    nbConstraints = 0;

    // TOOD : Use better allocation here
    mContactConstraints = new ContactConstraint[world->getNbContactManifolds()];

    mNbContactConstraints = 0;

    // For each contact manifold of the world
    vector<ContactManifold>::iterator it;
    for (it = world->getContactManifoldsBeginIterator(); it != world->getContactManifoldsEndIterator(); ++it) {
        ContactManifold contactManifold = *it;

        ContactConstraint& constraint = mContactConstraints[mNbContactConstraints];

        assert(contactManifold.nbContacts > 0);

        RigidBody* body1 = contactManifold.contacts[0]->getBody1();
        RigidBody* body2 = contactManifold.contacts[0]->getBody2();

        // Fill in the body number maping
        mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
        mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));

        // Add the two bodies of the constraint in the constraintBodies list
        mConstraintBodies.insert(body1);
        mConstraintBodies.insert(body2);

        Vector3 x1 = body1->getTransform().getPosition();
        Vector3 x2 = body2->getTransform().getPosition();

        constraint.indexBody1 = mMapBodyToIndex[body1];
        constraint.indexBody2 = mMapBodyToIndex[body2];
        constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
        constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
        constraint.isBody1Moving = body1->getIsMotionEnabled();
        constraint.isBody2Moving = body2->getIsMotionEnabled();
        constraint.massInverseBody1 = body1->getMassInverse();
        constraint.massInverseBody2 = body2->getMassInverse();
        constraint.nbContacts = contactManifold.nbContacts;

        // For each contact point of the contact manifold
        for (uint c=0; c<contactManifold.nbContacts; c++) {

            ContactPointConstraint& contactPointConstraint = constraint.contacts[c];

            // Get a contact point
            Contact* contact = contactManifold.contacts[c];

            Vector3 p1 = contact->getWorldPointOnBody1();
            Vector3 p2 = contact->getWorldPointOnBody2();

            contactPointConstraint.contact = contact;
            contactPointConstraint.normal = contact->getNormal();
            contactPointConstraint.r1 = p1 - x1;
            contactPointConstraint.r2 = p2 - x2;
            contactPointConstraint.frictionVector1 = contact->getFrictionVector1();
            contactPointConstraint.frictionVector2 = contact->getFrictionVector2();
        }

        mNbContactConstraints++;
    }

    // Compute the number of bodies that are part of some active constraint
    nbBodies = mConstraintBodies.size();

    Vconstraint = new Vector3[nbBodies];
    Wconstraint = new Vector3[nbBodies];
    V1 = new Vector3[nbBodies];
    W1 = new Vector3[nbBodies];

    assert(mMapBodyToIndex.size() == nbBodies);
}

// Initialize bodies velocities
void ConstraintSolver::initializeBodies() {

    // For each current body that is implied in some constraint
    RigidBody* rigidBody;
    RigidBody* body;
    uint b=0;
    for (set<RigidBody*>::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it, b++) {
        rigidBody = *it;
        uint bodyNumber = mMapBodyToIndex[rigidBody];

        // TODO : Use polymorphism and remove this downcasting
        assert(rigidBody);

        // Compute the vector V1 with initial velocities values
        int bodyIndexArray = 6 * bodyNumber;
        V1[bodyNumber] = rigidBody->getLinearVelocity();
        W1[bodyNumber] = rigidBody->getAngularVelocity();

        // Compute the vector Vconstraint with final constraint velocities
        Vconstraint[bodyNumber] = Vector3(0, 0, 0);
        Wconstraint[bodyNumber] = Vector3(0, 0, 0);

        // Initialize the mass and inertia tensor matrices
        Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
        Minv_sp_mass_diag[bodyNumber] = 0.0;

        // If the motion of the rigid body is enabled
        if (rigidBody->getIsMotionEnabled()) {
            Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
            Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
        }
    }
}

// Fill in all the matrices needed to solve the LCP problem
// Notice that all the active constraints should have been evaluated first
void ConstraintSolver::initializeContactConstraints(decimal dt) {
    decimal oneOverDT = 1.0 / dt;
    
    // For each contact constraint
    for (uint c=0; c<mNbContactConstraints; c++) {

        ContactConstraint& constraint = mContactConstraints[c];

        uint indexBody1 = constraint.indexBody1;
        uint indexBody2 = constraint.indexBody2;

        Matrix3x3& I1 = constraint.inverseInertiaTensorBody1;
        Matrix3x3& I2 = constraint.inverseInertiaTensorBody2;

        // For each contact point constraint
        for (uint i=0; i<constraint.nbContacts; i++) {

            ContactPointConstraint& contact = constraint.contacts[i];
            Contact* realContact = contact.contact;

            contact.r1CrossN = contact.r1.cross(contact.normal);
            contact.r1CrossT1 = contact.r1.cross(contact.frictionVector1);
            contact.r1CrossT2 = contact.r1.cross(contact.frictionVector2);
            contact.r2CrossT1 = contact.r2.cross(contact.frictionVector1);
            contact.r2CrossT2 = contact.r2.cross(contact.frictionVector2);

            contact.inversePenetrationMass = 0.0;
            if (constraint.isBody1Moving) {
                contact.inversePenetrationMass += constraint.massInverseBody1 +
                        ((I1 * contact.r1CrossN).cross(contact.r1)).dot(contact.normal);
            }
            if (constraint.isBody2Moving) {
                contact.inversePenetrationMass += constraint.massInverseBody2 +
                        ((I2 * contact.r2CrossN).cross(contact.r2)).dot(contact.normal);
            }

            // Compute the inverse mass matrix K for the friction constraints
            contact.inverseFriction1Mass = 0.0;
            contact.inverseFriction2Mass = 0.0;
            if (constraint.isBody1Moving) {
                contact.inverseFriction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1);
                contact.inverseFriction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2);
            }
            if (constraint.isBody2Moving) {
                contact.inverseFriction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1);
                contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2);
            }

            // Fill in the J_sp matrix
            realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration);
            realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1);
            realContact->computeJacobianFriction2(contact.J_spBody1Friction2, contact.J_spBody2Friction2);

            // Fill in the body mapping matrix
            //for(int i=0; i<realContact->getNbConstraints(); i++) {
            //    bodyMapping[noConstraint+i][0] = constraint->getBody1();
            //    bodyMapping[noConstraint+i][1] = constraint->getBody2();
            //}

            // Fill in the limit vectors for the constraint
            realContact->computeLowerBoundPenetration(contact.lowerBoundPenetration);
            realContact->computeLowerBoundFriction1(contact.lowerBoundFriction1);
            realContact->computeLowerBoundFriction2(contact.lowerBoundFriction2);
            realContact->computeUpperBoundPenetration(contact.upperBoundPenetration);
            realContact->computeUpperBoundFriction1(contact.upperBoundFriction1);
            realContact->computeUpperBoundFriction2(contact.upperBoundFriction2);

            // Fill in the error vector
            realContact->computeErrorPenetration(contact.errorPenetration);
            contact.errorFriction1 = 0.0;
            contact.errorFriction2 = 0.0;

            // Get the cached lambda values of the constraint
            contact.penetrationImpulse = realContact->getCachedLambda(0);
            contact.friction1Impulse = realContact->getCachedLambda(1);
            contact.friction2Impulse = realContact->getCachedLambda(2);
            //for (int i=0; i<constraint->getNbConstraints(); i++) {
            //    lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
           // }

            contact.errorPenetration = 0.0;
            decimal slop = 0.005;
            if (realContact->getPenetrationDepth() > slop) {
                 contact.errorPenetration += 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0);
            }
            contact.errorFriction1 = 0.0;
            contact.errorFriction2 = 0.0;

            // ---------- Penetration ---------- //

            // b = errorValues * oneOverDT;
            contact.b_Penetration = contact.errorPenetration * oneOverDT;

             // ---------- Friction 1 ---------- //

            contact.b_Friction1 = contact.errorFriction1 * oneOverDT;

             // ---------- Friction 2 ---------- //

            contact.b_Friction2 = contact.errorFriction2 * oneOverDT;
        }
    }


}

// Compute the matrix B_sp
void ConstraintSolver::computeMatrixB_sp() {
	uint indexConstraintArray, indexBody1, indexBody2;
	
    // For each constraint
    for (uint m = 0; m<mNbContactConstraints; m++) {

        ContactConstraint& constraint = mContactConstraints[m];

        for (uint c=0; c<constraint.nbContacts; c++) {

            ContactPointConstraint& contact = constraint.contacts[c];

            // ---------- Penetration ---------- //

            indexBody1 = constraint.indexBody1;
            indexBody2 = constraint.indexBody2;
            contact.B_spBody1Penetration[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Penetration[0];
            contact.B_spBody1Penetration[1] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Penetration[1];
            contact.B_spBody1Penetration[2] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Penetration[2];
            contact.B_spBody2Penetration[0] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Penetration[0];
            contact.B_spBody2Penetration[1] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Penetration[1];
            contact.B_spBody2Penetration[2] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Penetration[2];

            for (uint i=0; i<3; i++) {
                contact.B_spBody1Penetration[3 + i] = 0.0;
                contact.B_spBody2Penetration[3 + i] = 0.0;
                for (uint j=0; j<3; j++) {
                    contact.B_spBody1Penetration[3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * contact.J_spBody1Penetration[3 + j];
                    contact.B_spBody2Penetration[3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * contact.J_spBody2Penetration[3 + j];
                }
            }

            // ---------- Friction 1 ---------- //

            Matrix3x3& I1 = constraint.inverseInertiaTensorBody1;
            Matrix3x3& I2 = constraint.inverseInertiaTensorBody2;

            Vector3 JspLinBody1 = Vector3(0, 0, 0);
            Vector3 JspAngBody1 = Vector3(0, 0, 0);
            Vector3 JspLinBody2 = Vector3(0, 0, 0);
            Vector3 JspAngBody2 = Vector3(0, 0, 0);

            if (constraint.isBody1Moving) {
                JspLinBody1 += -constraint.massInverseBody1 * contact.frictionVector1;
                JspAngBody1 += I1 * (-contact.r1CrossT1);
            }
            if (constraint.isBody2Moving) {
                JspLinBody2 += constraint.massInverseBody2 * contact.frictionVector1;
                JspAngBody2 += I1 * (contact.r2CrossT1);
            }

            /*
            std::cout << "------ New Version ----" << std::endl;
            std::cout << "JspLinBody1 : " << JspLinBody1.getX() << ", " << JspLinBody1.getY() << ", " << JspLinBody1.getZ() << std::endl;
            std::cout << "JspAngBody1 : " << JspAngBody1.getX() << ", " << JspAngBody1.getY() << ", " << JspAngBody1.getZ() << std::endl;
            std::cout << "JspLinBody2 : " << JspLinBody2.getX() << ", " << JspLinBody2.getY() << ", " << JspLinBody2.getZ() << std::endl;
            std::cout << "JspAngBody2 : " << JspAngBody2.getX() << ", " << JspAngBody2.getY() << ", " << JspAngBody2.getZ() << std::endl;

            std::cout << ": friction vector 1 : " << contact.frictionVector1.getX() << ", " << contact.frictionVector1.getY() << ", " << contact.frictionVector1.getZ() << std::endl;
            std::cout << ": r1 x t1 : " << contact.r1CrossT1.getX() << ", " << contact.r1CrossT1.getY() << ", " << contact.r1CrossT1.getZ() << std::endl;
            std::cout << ": r2 x t1 : " << contact.r2CrossT1.getX() << ", " << contact.r2CrossT1.getY() << ", " << contact.r2CrossT1.getZ() << std::endl;
            std::cout << ": inverse mass body 1 = " << constraint.massInverseBody1 << std::endl;
            std::cout << ": inverse mass body 2 = " << constraint.massInverseBody2 << std::endl;
            */

            contact.B_spBody1Friction1[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[0];
            contact.B_spBody1Friction1[1] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[1];
            contact.B_spBody1Friction1[2] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[2];
            contact.B_spBody2Friction1[0] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction1[0];
            contact.B_spBody2Friction1[1] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction1[1];
            contact.B_spBody2Friction1[2] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction1[2];

            for (uint i=0; i<3; i++) {
                contact.B_spBody1Friction1[3 + i] = 0.0;
                contact.B_spBody2Friction1[3 + i] = 0.0;
                for (uint j=0; j<3; j++) {
                    contact.B_spBody1Friction1[3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * contact.J_spBody1Friction1[3 + j];
                    contact.B_spBody2Friction1[3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * contact.J_spBody2Friction1[3 + j];
                }
            }

            /*
            std::cout << "------ Old Version ----" << std::endl;
            std::cout << "JspLinBody1 : " << contact.B_spBody1Friction1[0] << ", " << contact.B_spBody1Friction1[1] << ", " << contact.B_spBody1Friction1[2] << std::endl;
            std::cout << "JspAngBody1 : " << contact.B_spBody1Friction1[3] << ", " << contact.B_spBody1Friction1[4] << ", " << contact.B_spBody1Friction1[5] << std::endl;
            std::cout << "JspLinBody2 : " << contact.B_spBody2Friction1[0] << ", " << contact.B_spBody2Friction1[1] << ", " << contact.B_spBody2Friction1[2] << std::endl;
            std::cout << "JspAngBody2 : " << contact.B_spBody2Friction1[3] << ", " << contact.B_spBody2Friction1[4] << ", " << contact.B_spBody2Friction1[5] << std::endl;
            */

            // ---------- Friction 2 ---------- //

            contact.B_spBody1Friction2[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction2[0];
            contact.B_spBody1Friction2[1] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction2[1];
            contact.B_spBody1Friction2[2] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction2[2];
            contact.B_spBody2Friction2[0] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction2[0];
            contact.B_spBody2Friction2[1] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction2[1];
            contact.B_spBody2Friction2[2] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction2[2];

            for (uint i=0; i<3; i++) {
                contact.B_spBody1Friction2[3 + i] = 0.0;
                contact.B_spBody2Friction2[3 + i] = 0.0;
                for (uint j=0; j<3; j++) {
                    contact.B_spBody1Friction2[3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * contact.J_spBody1Friction2[3 + j];
                    contact.B_spBody2Friction2[3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * contact.J_spBody2Friction2[3 + j];
                }
            }
        }
    }
}

// Compute the vector V_constraint (which corresponds to the constraint part of
// the final V2 vector) according to the formula
// V_constraint = dt * (M^-1 * J^T * lambda)
// Note that we use the vector V to store both V1 and V_constraint.
// Note that M^-1 * J^T = B.
// This method is called after that the LCP solver has computed lambda
void ConstraintSolver::computeVectorVconstraint(decimal dt) {
    uint indexBody1Array, indexBody2Array;
	uint j;
	
    // Compute dt * (M^-1 * J^T * lambda
    for (uint c=0; c<mNbContactConstraints; c++) {

        ContactConstraint& constraint = mContactConstraints[c];

        for (uint i=0; i<constraint.nbContacts; i++) {

            ContactPointConstraint& contact = constraint.contacts[i];

            // ---------- Penetration ---------- //

            indexBody1Array = constraint.indexBody1;
            indexBody2Array = constraint.indexBody2;
            for (j=0; j<3; j++) {
                Vconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse * dt;
                Wconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse * dt;

                Vconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse * dt;
                Wconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse * dt;
            }

            // ---------- Friction 1 ---------- //

            for (j=0; j<3; j++) {
                Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse * dt;
                Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse * dt;

                Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse * dt;
                Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse * dt;
            }

            // ---------- Friction 2 ---------- //

            for (j=0; j<3; j++) {
                Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse * dt;
                Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse * dt;

                Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse * dt;
                Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse * dt;
            }
        }
    }
}

// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void ConstraintSolver::solveLCP(decimal dt) {

   // for (uint i=0; i<nbConstraints; i++) {
   //         lambda[i] = lambdaInit[i];
   // }

    uint indexBody1Array, indexBody2Array;
    decimal deltaLambda;
    decimal lambdaTemp;
    uint iter;

    // Compute the vector a
    computeVectorA(dt);

    // For each iteration
    for(iter=0; iter<mNbIterations; iter++) {

        // For each constraint
        for (uint c=0; c<mNbContactConstraints; c++) {

            ContactConstraint& constraint = mContactConstraints[c];

            for (uint i=0; i<constraint.nbContacts; i++) {

                ContactPointConstraint& contact = constraint.contacts[i];

                indexBody1Array = constraint.indexBody1;
                indexBody2Array = constraint.indexBody2;

                // --------- Penetration --------- //

                deltaLambda = contact.b_Penetration;
                for (uint j=0; j<3; j++) {
                    deltaLambda -= (contact.J_spBody1Penetration[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Penetration[j] * aLinear[indexBody2Array][j]);
                    deltaLambda -= (contact.J_spBody1Penetration[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * aAngular[indexBody2Array][j]);
                }
                deltaLambda /= contact.inversePenetrationMass;
                lambdaTemp = contact.penetrationImpulse;
                contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
                deltaLambda = contact.penetrationImpulse - lambdaTemp;
                for (uint j=0; j<3; j++) {
                    aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda;
                    aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda;

                    aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda;
                    aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda;
                }

                // --------- Friction 1 --------- //

                deltaLambda = contact.b_Friction1;
                for (uint j=0; j<3; j++) {
                    deltaLambda -= (contact.J_spBody1Friction1[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction1[j] * aLinear[indexBody2Array][j]);
                    deltaLambda -= (contact.J_spBody1Friction1[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * aAngular[indexBody2Array][j]);
                }
                deltaLambda /= contact.inverseFriction1Mass;
                lambdaTemp = contact.friction1Impulse;
                contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1));
                deltaLambda = contact.friction1Impulse - lambdaTemp;
                for (uint j=0; j<3; j++) {
                    aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda;
                    aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda;

                    aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda;
                    aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda;
                }

                // --------- Friction 2 --------- //

                deltaLambda = contact.b_Friction2;
                for (uint j=0; j<3; j++) {
                    deltaLambda -= (contact.J_spBody1Friction2[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction2[j] * aLinear[indexBody2Array][j]);
                    deltaLambda -= (contact.J_spBody1Friction2[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * aAngular[indexBody2Array][j]);
                }
                deltaLambda /= contact.inverseFriction2Mass;
                lambdaTemp = contact.friction2Impulse;
                contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2));
                deltaLambda = contact.friction2Impulse - lambdaTemp;
                for (uint j=0; j<3; j++) {
                    aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda;
                    aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda;

                    aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda;
                    aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda;
                }
            }
        }
    }
}

// Compute the vector a used in the solve() method
// Note that a = B * lambda
void ConstraintSolver::computeVectorA(decimal dt) {
    uint i;
    uint indexBody1Array, indexBody2Array;
    decimal oneOverDt = 1.0 / dt;
    
    // Init the vector a with zero values
    for (set<RigidBody*>::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
        RigidBody* rigidBody = *it;
        uint bodyNumber = mMapBodyToIndex[rigidBody];

        aLinear[bodyNumber] = oneOverDt * V1[bodyNumber] + rigidBody->getMassInverse() * rigidBody->getExternalForce();
        aAngular[bodyNumber] = oneOverDt * W1[bodyNumber] + rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
    }

    // For each constraint
    for (uint c=0; c<mNbContactConstraints; c++) {

        ContactConstraint& constraint = mContactConstraints[c];

        for (uint i=0; i<constraint.nbContacts; i++) {

            ContactPointConstraint& contact = constraint.contacts[i];

            indexBody1Array = constraint.indexBody1;
            indexBody2Array = constraint.indexBody2;

            // --------- Penetration --------- //

            for (uint j=0; j<3; j++) {
                aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
                aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;

                aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse;
                aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse;
            }

            // --------- Friction 1 --------- //

            for (uint j=0; j<3; j++) {
                aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
                aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;

                aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse;
                aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse;
            }

            // --------- Friction 2 --------- //

            for (uint j=0; j<3; j++) {
                aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
                aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;

                aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse;
                aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse;
            }
        }
    }
}

// Cache the lambda values in order to reuse them in the next step
// to initialize the lambda vector
void ConstraintSolver::cacheLambda() {
    
    // For each constraint
    for (uint c=0; c<mNbContactConstraints; c++) {

        ContactConstraint& constraint = mContactConstraints[c];

        for (uint i=0; i<constraint.nbContacts; i++) {

            ContactPointConstraint& contact = constraint.contacts[i];

            contact.contact->setCachedLambda(0, contact.penetrationImpulse);
            contact.contact->setCachedLambda(1, contact.friction1Impulse);
            contact.contact->setCachedLambda(2, contact.friction2Impulse);
        }
    }
}