Initial commit

engines/hpl1/engine/libraries/newton/physics/dgWorldDynamicUpdate.h

@@ -0,0 +1,409 @@
+/* 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