/* Copyright (c) <2003-2011> * * 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 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 &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