410 lines
11 KiB
C++
410 lines
11 KiB
C++
/* 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
|