dgWorldDynamicUpdate.h

/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
*
* 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.
*/

#if !defined(AFX_DYNAMIC_UPDATE_H__EC18C699_D48D_448F_A510_A865B2CC0789__INCLUDED_)
#define AFX_DYNAMIC_UPDATE_H__EC18C699_D48D_448F_A510_A865B2CC0789__INCLUDED_


#include "dgPhysicsStdafx.h"


//#define DG_PSD_DAMP_TOL           dgFloat32 (1.0e-2f)
#define DG_PSD_DAMP_TOL             dgFloat32 (1.0e-3f)
//#define DG_PSD_DAMP_TOL           dgFloat32 (1.0e-4f)


#define LINEAR_SOLVER_SUB_STEPS         4


class dgIsland;
class dgJointInfo;
class dgBodyInfo;
class dgJacobianMemory;
class dgWorldDynamicUpdate;


class dgIslandCallbackStruct {
public:
    dgWorld *m_world;
    dgInt32 m_count;
    dgInt32 m_strideInByte;
    void *m_bodyArray;
};



template<class T>
class dgQueue {
public:
    dgQueue(T *pool, dgInt32 size)
        : m_pool(pool) {

        m_mod = size;
        m_lastIndex = 0;
        m_firstIndex = 0;
    }

    void Insert(T info) {
        m_pool[m_firstIndex] = info;
        m_firstIndex ++;
        if (m_firstIndex >= m_mod) {
            m_firstIndex = 0;
        }
        NEWTON_ASSERT(m_firstIndex != m_lastIndex);
    }

    T Remove() {
        NEWTON_ASSERT(m_firstIndex != m_lastIndex);

        T element = m_pool[m_lastIndex];
        m_lastIndex ++;
        if (m_lastIndex >= m_mod) {
            m_lastIndex = 0;
        }

        return element;
    }

    void Reset() {
        m_lastIndex = m_firstIndex;
    }

    bool IsEmpty() const {
        return (m_firstIndex == m_lastIndex);
    }

    dgInt32 m_mod;
    dgInt32 m_firstIndex;
    dgInt32 m_lastIndex;
    T *m_pool;
};


class dgSolverWorlkerThreads: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_threads;
    dgInt32 m_useSimd;
    dgInt32 m_solverMode;
    dgFloat32 m_timestep;
    dgWorld *m_world;
    dgJacobianMemory *m_system;
    dgWorldDynamicUpdate *m_dynamics;
};



class dgParallelSolverClear: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgWorld *m_world;
    const dgBodyInfo *m_bodyArray;
    dgJacobian *m_internalVeloc;
    dgJacobian *m_internalForces;
};

class dgParallelSolverInitInternalForces: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgInt32 *m_locks;
    dgWorld *m_world;
    dgFloat32 *m_force;
    const dgJacobianPair *m_Jt;
    dgJacobian *m_internalForces;
    const dgJointInfo *m_constraintArray;
};

class dgParallelSolverInitFeedbackUpdate: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_hasJointFeeback;
    dgInt32 *m_locks;
    dgWorld *m_world;
    dgFloat32 m_timestep;
    dgFloat32 *m_force;
    dgFloat32 **m_jointForceFeeback;
    const dgJointInfo *m_constraintArray;
};


class dgParallelSolverBodyInertia: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgWorld *m_world;
    dgBodyInfo *m_bodyArray;
};

class dgParallelSolverBuildJacobianRows: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgInt32 m_bodyCount;
    dgWorld *m_world;
    dgFloat32 *m_force;
    dgFloat32 *m_diagDamp;
    dgFloat32 *m_deltaAccel;
    dgFloat32 *m_invDJMinvJt;
    dgFloat32 *m_coordenateAccel;
    dgFloat32 **m_jointFeebackForce;
    dgJacobianPair *m_Jt;
    dgJacobianPair *m_JMinv;
    dgBodyInfo *m_bodyArray;
    dgJointInfo *m_constraintArray;
};

class dgParallelSolverBuildJacobianMatrix: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    bool m_bitMode;
    dgInt32 m_count;
    dgInt32 m_jointSolved;
    dgInt32 *m_rowsCount;
    dgFloat32 m_timestep;
    dgFloat32 m_invTimestep;
    dgWorld *m_world;
    dgJacobianMemory *m_solverMemory;
    dgJointInfo *m_constraintArray;
};

class dgParallelSolverJointAcceleration: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgInt32 m_jointStart;
    dgWorld *m_world;
    dgFloat32 m_timeStep;
    dgFloat32 m_invTimeStep;
    dgFloat32 m_firstPassCoef;
    const dgJacobianPair *m_Jt;
    dgFloat32 *m_penetration;
    const dgFloat32 *m_restitution;
    const dgFloat32 *m_externAccel;
    dgFloat32 *m_coordenateAccel;
    const dgInt32 *m_accelIsMortor;
    const dgInt32 *m_normalForceIndex;
    const dgFloat32 *m_penetrationStiffness;
    const dgJointInfo *m_constraintArray;
};


class dgParallelSolverUpdateVeloc: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgWorld *m_world;
    dgFloat32 m_timeStep;
    dgJacobian *m_internalVeloc;
    const dgJacobian *m_internalForces;
    const dgBodyInfo *m_bodyArray;
};

class dgParallelSolverUpdateForce: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgWorld *m_world;

    dgFloat32 m_invStep;
    dgFloat32 m_invTimeStep;
    dgFloat32 m_maxAccNorm2;
    const dgJacobian *m_internalVeloc;
    const dgBodyInfo *m_bodyArray;
};




class dgParallelSolverCalculateForces: public dgWorkerThread {
public:
    virtual void ThreadExecute();

    dgInt32 m_count;
    dgInt32 m_useSimd;
    dgInt32 m_threads;
    dgWorld *m_world;
    dgInt32 *m_locks;
    dgJacobian *m_internalForces;
    dgFloat32 m_accNorm;
    dgFloat32 *m_force;
    const dgFloat32 *m_diagDamp;
    const dgInt32 *m_normalForceIndex;
    const dgFloat32 *m_invDJMinvJt;
    const dgFloat32 *m_coordenateAccel;
    const dgFloat32 *m_lowerFrictionCoef;
    const dgFloat32 *m_upperFrictionCoef;
    const dgJacobianPair *m_Jt;
    const dgJacobianPair *m_JMinv;
    const dgJointInfo *m_constraintArray;
};



class dgJacobianMemory {
public:
    dgInt32 m_rowCount;
    dgInt32 m_bodyCount;
    dgInt32 m_jointCount;
    dgInt32 m_maxBodiesCount;
    dgInt32 m_maxJacobiansCount;
    dgInt32 m_threadIndex;
    dgFloat32 m_timeStep;
    dgFloat32 m_invTimeStep;
    dgWorld *m_world;
    dgInt32 *m_treadLocks;
    dgJacobian *m_internalVeloc;
    dgJacobian *m_internalForces;
    dgJointInfo *m_constraintArray;
    dgJacobianPair *m_Jt;
    dgJacobianPair *m_JMinv;
    dgFloat32 *m_force;
    dgFloat32 *m_accel;
    dgFloat32 *m_deltaAccel;
    dgFloat32 *m_deltaForce;
    dgFloat32 *m_diagDamp;
    dgFloat32 *m_invDJMinvJt;
    dgFloat32 *m_restitution;
    dgFloat32 *m_penetration;
    dgFloat32 *m_coordenateAccel;
    dgFloat32 *m_penetrationStiffness;
    dgFloat32 *m_lowerBoundFrictionCoefficent;
    dgFloat32 *m_upperBoundFrictionCoefficent;
    dgFloat32 **m_jointFeebackForce;
    dgBodyInfo *m_bodyArray;
    dgInt32 *m_normalForceIndex;
    dgInt32 *m_accelIsMotor;

    void SwapRows(dgInt32 i, dgInt32 j) const;
    void SwapRowsSimd(dgInt32 i, dgInt32 j) const;

    void CalculateReactionsForces(dgInt32 solverMode, dgFloat32 maxAccNorm) const;
    void CalculateReactionsForcesSimd(dgInt32 solverMode, dgFloat32 maxAccNorm) const;

    void ApplyExternalForcesAndAcceleration(dgFloat32 maxAccNorm) const;
    void ApplyExternalForcesAndAccelerationSimd(dgFloat32 maxAccNorm) const;

    void CalculateSimpleBodyReactionsForces(dgFloat32 maxAccNorm) const;
    void CalculateSimpleBodyReactionsForcesSimd(dgFloat32 maxAccNorm) const;

    dgFloat32 CalculateJointForces(dgInt32 joint, dgFloat32 *forceStep, dgFloat32 maxAccNorm) const;
    dgFloat32 CalculateJointForcesSimd(dgInt32 joint, dgFloat32 *forceStep, dgFloat32 maxAccNorm) const;

    void CalculateForcesSimulationMode(dgFloat32 maxAccNorm) const;
    void CalculateForcesSimulationModeSimd(dgFloat32 maxAccNorm) const;

    void CalculateForcesGameMode(dgInt32 itertions, dgFloat32 maxAccNorm) const;
    void CalculateForcesGameModeSimd(dgInt32 itertions, dgFloat32 maxAccNorm) const;

    void CalculateForcesGameModeParallel(dgInt32 itertions, dgFloat32 maxAccNorm, dgInt32 archModel) const;
    void CalculateReactionsForcesParallel(dgInt32 solverMode, dgFloat32 maxAccNorm, dgInt32 archModel) const;
};


class dgWorldDynamicUpdate {

    dgWorldDynamicUpdate();
    void UpdateDynamics(dgWorld *const world, dgInt32 archMode, dgFloat32 timestep);

private:
    // single core functions
    void BuildIsland(dgQueue<dgBody *> &queue, dgInt32 jountCount, dgInt32 hasUnilateralJoints, dgInt32 isContinueCollisionIsland);
//  dgBody* SpanningTree (dgBody* body, dgBody** const queuePool, dgInt32 queueSize, dgInt32 solveMode);
    void SpanningTree(dgBody *const body);
    void ReallocBodyMemory(dgInt32 count);
    void ReallocJointsMemory(dgInt32 count);
    void ReallocIslandMemory(dgInt32 count);


    // multi-cores functions
    void BuildJacobianMatrix(const dgIsland &island, dgInt32 threadIndex, dgFloat32 timestep);
    void BuildJacobianMatrixSimd(const dgIsland &island, dgInt32 threadIndex, dgFloat32 timestep);
    dgInt32 GetJacobialDerivatives(const dgIsland &island, dgInt32 threadIndex, bool bitMode, dgInt32 rowCount, dgFloat32 timestep);


    void BuildJacobianMatrixParallel(const dgIsland &island, dgFloat32 timestep, dgInt32 archModel);
    dgInt32 GetJacobialDerivativesParallel(const dgIsland &island, bool bitMode, dgInt32 rowCount, dgFloat32 timestep);

    void ReallocIntenalForcesMemory(dgInt32 count, dgInt32 threadIndex);
    void ReallocJacobiansMemory(dgInt32 count, dgInt32 threadIndex);

//  void SortIslands ();
//  dgInt32 CompareIslands (const dgIsland& A, const dgIsland& B) const;
    void IntegrateArray(const dgBodyInfo *body, dgInt32 count, dgFloat32 accelTolerance, dgFloat32 timestep, dgInt32 threadIndex, bool update) const;

    dgInt32 m_bodies;
    dgInt32 m_joints;
    dgInt32 m_islands;
    dgInt32 m_markLru;
    dgInt32 m_maxJointCount;
    dgInt32 m_maxBodiesCount;
    dgInt32 m_maxIslandCount;

    dgIsland *m_islandArray;
    dgBodyInfo *m_bodyArray;
    dgJointInfo *m_constraintArray;


    dgJacobianMemory m_solverMemory [DG_MAXIMUN_THREADS];
    dgSolverWorlkerThreads m_workerThreads[DG_MAXIMUN_THREADS];
    dgParallelSolverClear m_clearAccumulators[DG_MAXIMUN_THREADS];
    dgParallelSolverBodyInertia m_parallelBodyInertiaMatrix[DG_MAXIMUN_THREADS];
    dgParallelSolverUpdateVeloc m_parallelSolverUpdateVeloc[DG_MAXIMUN_THREADS];
    dgParallelSolverUpdateForce m_parallelSolverUpdateForce[DG_MAXIMUN_THREADS];
    dgParallelSolverCalculateForces m_parallelSolverCalculateForces[DG_MAXIMUN_THREADS];
    dgParallelSolverInitInternalForces m_parallelInitIntenalForces[DG_MAXIMUN_THREADS];
    dgParallelSolverBuildJacobianRows m_parallelSolverBuildJacobianRows[DG_MAXIMUN_THREADS];
    dgParallelSolverJointAcceleration m_parallelSolverJointAcceleration[DG_MAXIMUN_THREADS];
    dgParallelSolverInitFeedbackUpdate m_parallelSolverInitFeedbackUpdate[DG_MAXIMUN_THREADS];
    dgParallelSolverBuildJacobianMatrix m_parallelSolverBuildJacobianMatrix[DG_MAXIMUN_THREADS];


    dgBody *m_sentinelBody;
    dgWorld *m_world;
    friend class dgWorld;
    friend class dgJacobianMemory;
    friend class dgSolverWorlkerThreads;
};


#define DG_CHECK_ACTIVE(body) NEWTON_ASSERT (((body->m_invMass.m_w == dgFloat32 (0.0f)) && !body->m_active) || (body->m_invMass.m_w > dgFloat32 (0.0f)))
#endif