liangchengyou / wow_english

/*
   Copyright (C) 2010 Sony Computer Entertainment Inc.
   All rights reserved.

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.

*/

#ifndef __BT_PARALLEL_CONSTRAINT_SOLVER_H
#define __BT_PARALLEL_CONSTRAINT_SOLVER_H

#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"




#include "LinearMath/btScalar.h"
#include "PlatformDefinitions.h"


#define PFX_MAX_SOLVER_PHASES 64
#define PFX_MAX_SOLVER_BATCHES 16
#define PFX_MAX_SOLVER_PAIRS  128
#define PFX_MIN_SOLVER_PAIRS  16

#ifdef __CELLOS_LV2__
ATTRIBUTE_ALIGNED128(struct) PfxParallelBatch {
#else
ATTRIBUTE_ALIGNED16(struct) PfxParallelBatch {
#endif
	uint16_t pairIndices[PFX_MAX_SOLVER_PAIRS];
};

#ifdef __CELLOS_LV2__
ATTRIBUTE_ALIGNED128(struct) PfxParallelGroup {
#else
ATTRIBUTE_ALIGNED16(struct) PfxParallelGroup {
#endif
	uint16_t numPhases;
	uint16_t numBatches[PFX_MAX_SOLVER_PHASES];
	uint16_t numPairs[PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES];
};



ATTRIBUTE_ALIGNED16(struct) PfxSortData16 {
	union {
		uint8_t   i8data[16];
		uint16_t  i16data[8];
		uint32_t  i32data[4];
#ifdef __SPU__
		vec_uint4 vdata;
#endif
	};

#ifdef __SPU__
	void set8(int elem,uint8_t data)   {vdata=(vec_uint4)spu_insert(data,(vec_uchar16)vdata,elem);}
	void set16(int elem,uint16_t data) {vdata=(vec_uint4)spu_insert(data,(vec_ushort8)vdata,elem);}
	void set32(int elem,uint32_t data) {vdata=(vec_uint4)spu_insert(data,(vec_uint4)vdata,elem);}
	uint8_t get8(int elem)   const {return spu_extract((vec_uchar16)vdata,elem);}
	uint16_t get16(int elem) const {return spu_extract((vec_ushort8)vdata,elem);}
	uint32_t get32(int elem) const {return spu_extract((vec_uint4)vdata,elem);}
#else
	void set8(int elem,uint8_t data)   {i8data[elem] = data;}
	void set16(int elem,uint16_t data) {i16data[elem] = data;}
	void set32(int elem,uint32_t data) {i32data[elem] = data;}
	uint8_t get8(int elem)   const {return i8data[elem];}
	uint16_t get16(int elem) const {return i16data[elem];}
	uint32_t get32(int elem) const {return i32data[elem];}
#endif
};

typedef PfxSortData16 PfxConstraintPair;


//J	PfxBroadphasePair‚Æ‹¤’Ê

SIMD_FORCE_INLINE void pfxSetConstraintId(PfxConstraintPair &pair,uint32_t i)	{pair.set32(2,i);}
SIMD_FORCE_INLINE void pfxSetNumConstraints(PfxConstraintPair &pair,uint8_t n)	{pair.set8(7,n);}

SIMD_FORCE_INLINE uint32_t pfxGetConstraintId1(const PfxConstraintPair &pair)	{return pair.get32(2);}
SIMD_FORCE_INLINE uint8_t  pfxGetNumConstraints(const PfxConstraintPair &pair)	{return pair.get8(7);}

typedef PfxSortData16 PfxBroadphasePair;

SIMD_FORCE_INLINE void pfxSetRigidBodyIdA(PfxBroadphasePair &pair,uint16_t i)	{pair.set16(0,i);}
SIMD_FORCE_INLINE void pfxSetRigidBodyIdB(PfxBroadphasePair &pair,uint16_t i)	{pair.set16(1,i);}
SIMD_FORCE_INLINE void pfxSetMotionMaskA(PfxBroadphasePair &pair,uint8_t i)		{pair.set8(4,i);}
SIMD_FORCE_INLINE void pfxSetMotionMaskB(PfxBroadphasePair &pair,uint8_t i)		{pair.set8(5,i);}
SIMD_FORCE_INLINE void pfxSetBroadphaseFlag(PfxBroadphasePair &pair,uint8_t f)	{pair.set8(6,(pair.get8(6)&0xf0)|(f&0x0f));}
SIMD_FORCE_INLINE void pfxSetActive(PfxBroadphasePair &pair,bool b)			{pair.set8(6,(pair.get8(6)&0x0f)|((b?1:0)<<4));}
SIMD_FORCE_INLINE void pfxSetContactId(PfxBroadphasePair &pair,uint32_t i)		{pair.set32(2,i);}

SIMD_FORCE_INLINE uint16_t pfxGetRigidBodyIdA(const PfxBroadphasePair &pair)	{return pair.get16(0);}
SIMD_FORCE_INLINE uint16_t pfxGetRigidBodyIdB(const PfxBroadphasePair &pair)	{return pair.get16(1);}
SIMD_FORCE_INLINE uint8_t  pfxGetMotionMaskA(const PfxBroadphasePair &pair)		{return pair.get8(4);}
SIMD_FORCE_INLINE uint8_t  pfxGetMotionMaskB(const PfxBroadphasePair &pair)		{return pair.get8(5);}
SIMD_FORCE_INLINE uint8_t  pfxGetBroadphaseFlag(const PfxBroadphasePair &pair)	{return pair.get8(6)&0x0f;}
SIMD_FORCE_INLINE bool     pfxGetActive(const PfxBroadphasePair &pair)			{return (pair.get8(6)>>4)!=0;}
SIMD_FORCE_INLINE uint32_t pfxGetContactId1(const PfxBroadphasePair &pair)		{return pair.get32(2);}



#if defined(__PPU__) || defined (__SPU__)
ATTRIBUTE_ALIGNED128(struct) PfxSolverBody {
#else
ATTRIBUTE_ALIGNED16(struct) PfxSolverBody {
#endif
	vmVector3 mDeltaLinearVelocity;
	vmVector3 mDeltaAngularVelocity;
	vmMatrix3 mInertiaInv;
	vmQuat    mOrientation;
	float   mMassInv;
	float   friction;
	float   restitution;
	float   unused;
	float   unused2;
	float   unused3;
	float   unused4;
	float   unused5;
};


#ifdef __PPU__
#include "SpuDispatch/BulletPE2ConstraintSolverSpursSupport.h"
#endif

static SIMD_FORCE_INLINE vmVector3 btReadVector3(const double* p)
{
	float tmp[3] = {float(p[0]),float(p[1]),float(p[2])};
	vmVector3 v;
	loadXYZ(v, tmp);
	return v;
}

static SIMD_FORCE_INLINE vmQuat btReadQuat(const double* p)
{
	float tmp[4] = {float(p[0]),float(p[1]),float(p[2]),float(p[4])};
	vmQuat vq;
	loadXYZW(vq, tmp);
	return vq;
}

static SIMD_FORCE_INLINE void btStoreVector3(const vmVector3 &src, double* p)
{
	float tmp[3];
	vmVector3 v = src;
	storeXYZ(v, tmp);
	p[0] = tmp[0];
	p[1] = tmp[1];
	p[2] = tmp[2];
}


static SIMD_FORCE_INLINE vmVector3 btReadVector3(const float* p)
{
	vmVector3 v;
	loadXYZ(v, p);
	return v;
}

static SIMD_FORCE_INLINE vmQuat btReadQuat(const float* p)
{
	vmQuat vq;
	loadXYZW(vq, p);
	return vq;
}

static SIMD_FORCE_INLINE void btStoreVector3(const vmVector3 &src, float* p)
{
	vmVector3 v = src;
	storeXYZ(v, p);
}




class btPersistentManifold;

enum {
	PFX_CONSTRAINT_SOLVER_CMD_SETUP_SOLVER_BODIES,
	PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS,
	PFX_CONSTRAINT_SOLVER_CMD_WRITEBACK_APPLIED_IMPULSES_CONTACT_CONSTRAINTS,
	PFX_CONSTRAINT_SOLVER_CMD_SETUP_JOINT_CONSTRAINTS,
	PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS,
	PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER
};


struct PfxSetupContactConstraintsIO {
	PfxConstraintPair *offsetContactPairs;
	uint32_t numContactPairs1;
	btPersistentManifold*	offsetContactManifolds;
	btConstraintRow* offsetContactConstraintRows;
	class TrbState *offsetRigStates;
	struct PfxSolverBody *offsetSolverBodies;
	uint32_t numRigidBodies;
	float separateBias;
	float timeStep;
	class btCriticalSection* criticalSection;
};



struct PfxSolveConstraintsIO {
	PfxParallelGroup *contactParallelGroup;
	PfxParallelBatch *contactParallelBatches;
	PfxConstraintPair *contactPairs;
	uint32_t numContactPairs;
	btPersistentManifold *offsetContactManifolds;
	btConstraintRow*	offsetContactConstraintRows;
	PfxParallelGroup *jointParallelGroup;
	PfxParallelBatch *jointParallelBatches;
	PfxConstraintPair *jointPairs;
	uint32_t numJointPairs;
	struct btSolverConstraint* offsetSolverConstraints;
	TrbState *offsetRigStates1;
	PfxSolverBody *offsetSolverBodies;
	uint32_t numRigidBodies;
	uint32_t iteration;

	uint32_t	taskId;
	
	class btBarrier* barrier;

};

struct PfxPostSolverIO {
	TrbState *states;
	PfxSolverBody *solverBodies;
	uint32_t numRigidBodies;
};

ATTRIBUTE_ALIGNED16(struct) btConstraintSolverIO {
	uint8_t cmd;
	union {
		PfxSetupContactConstraintsIO setupContactConstraints;
		PfxSolveConstraintsIO solveConstraints;
		PfxPostSolverIO postSolver;
	};
	
	//SPU only
	uint32_t barrierAddr2;
	uint32_t criticalsectionAddr2;
	uint32_t maxTasks1;

#ifdef WINRT
    void* operator new(size_t i)
    {
        return _aligned_malloc(i, 16);
    }

    void operator delete(void* p)
    {
        _aligned_free(p);
    }

    void* operator new[](size_t i)
    {
        return _aligned_malloc(i, 16);
    }

    void operator delete[](void* p)
    {
        _aligned_free(p);
    }
#endif
};




void	SolverThreadFunc(void* userPtr,void* lsMemory);
void*	SolverlsMemoryFunc();
///The btParallelConstraintSolver performs computations on constraint rows in parallel
///Using the cross-platform threading it supports Windows, Linux, Mac OSX and PlayStation 3 Cell SPUs
class btParallelConstraintSolver : public btSequentialImpulseConstraintSolver
{
	
protected:
	struct btParallelSolverMemoryCache*	m_memoryCache;

	class btThreadSupportInterface*	m_solverThreadSupport;

	struct btConstraintSolverIO* m_solverIO;
	class btBarrier*			m_barrier;
	class btCriticalSection*	m_criticalSection;


public:

	btParallelConstraintSolver(class btThreadSupportInterface* solverThreadSupport);
	
	virtual ~btParallelConstraintSolver();

	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);

};



#endif //__BT_PARALLEL_CONSTRAINT_SOLVER_H