lolengine
/
lol
kopie van https://github.com/lolengine/lol
1
0
Vork 0
Code Kwesties 0 Publicaties 0 Wiki Activiteit
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2606 Commits
5 Branches
56 MiB
 
 
 
Tree: 5d0eec893e
Branches Labels
${ item.name }
Maak branch ${ searchTerm }
van '5d0eec893e'
${ noResults }
lol/src/bullet/BulletMultiThreaded/btParallelConstraintSolver.cpp

1392 regels
45 KiB
Ruw Blame Geschiedenis 

  1. /*

  2.    Copyright (C) 2010 Sony Computer Entertainment Inc.

  3.    All rights reserved.

  4. 

  5. This software is provided 'as-is', without any express or implied warranty.

  6. In no event will the authors be held liable for any damages arising from the use of this software.

  7. Permission is granted to anyone to use this software for any purpose, 

  8. including commercial applications, and to alter it and redistribute it freely, 

  9. subject to the following restrictions:

  10. 

  11. 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.

  12. 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

  13. 3. This notice may not be removed or altered from any source distribution.

  14. 

  15. */

  16. 

  17. 

  18. #include "btParallelConstraintSolver.h"

  19. #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"

  20. #include "BulletCollision/BroadphaseCollision/btDispatcher.h"

  21. #include "LinearMath/btPoolAllocator.h"

  22. 

  23. #include "BulletMultiThreaded/vectormath2bullet.h"

  24. 

  25. #include "LinearMath/btQuickprof.h"

  26. #include "BulletMultiThreaded/btThreadSupportInterface.h"

  27. #ifdef PFX_USE_FREE_VECTORMATH

  28. #include "vecmath/vmInclude.h"

  29. #else

  30. #include "vectormath/vmInclude.h"

  31. #endif //PFX_USE_FREE_VECTORMATH

  32. 

  33. #include "HeapManager.h"

  34. 

  35. #include "PlatformDefinitions.h"

  36. 

  37. //#include "PfxSimdUtils.h"

  38. #include "LinearMath/btScalar.h"

  39. 

  40. #include "TrbStateVec.h"

  41. 

  42. 

  43. 

  44. /////////////////

  45. 

  46. 

  47. #define TMP_BUFF_BYTES (15*1024*1024)

  48. unsigned char ATTRIBUTE_ALIGNED128(tmp_buff[TMP_BUFF_BYTES]);

  49. 

  50. 

  51. 

  52. // Project Gauss Seidel or the equivalent Sequential Impulse

  53.  inline void resolveSingleConstraintRowGeneric(PfxSolverBody& body1,PfxSolverBody& body2,const btSolverConstraint& c)

  54. {

  55. 

  56. 	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;

  57. 	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(getBtVector3(body1.mDeltaLinearVelocity)) 	+ c.m_relpos1CrossNormal.dot(getBtVector3(body1.mDeltaAngularVelocity));

  58. 	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(getBtVector3(body2.mDeltaLinearVelocity)) + c.m_relpos2CrossNormal.dot(getBtVector3(body2.mDeltaAngularVelocity));

  59. 

  60. //	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;

  61. 	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;

  62. 	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;

  63. 

  64. 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;

  65. 	if (sum < c.m_lowerLimit)

  66. 	{

  67. 		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;

  68. 		c.m_appliedImpulse = c.m_lowerLimit;

  69. 	}

  70. 	else if (sum > c.m_upperLimit) 

  71. 	{

  72. 		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;

  73. 		c.m_appliedImpulse = c.m_upperLimit;

  74. 	}

  75. 	else

  76. 	{

  77. 		c.m_appliedImpulse = sum;

  78. 	}

  79. 	

  80. 

  81. 	if (body1.mMassInv)

  82. 	{

  83. 		btVector3 linearComponent = c.m_contactNormal*body1.mMassInv;

  84. 		body1.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);

  85. 		btVector3 tmp=c.m_angularComponentA*(btVector3(deltaImpulse,deltaImpulse,deltaImpulse));

  86. 		body1.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());

  87. 	}

  88. 

  89. 	if (body2.mMassInv)

  90. 	{

  91. 		btVector3 linearComponent = -c.m_contactNormal*body2.mMassInv;

  92. 		body2.mDeltaLinearVelocity += vmVector3(linearComponent.getX()*deltaImpulse,linearComponent.getY()*deltaImpulse,linearComponent.getZ()*deltaImpulse);

  93. 		btVector3 tmp = c.m_angularComponentB*((btVector3(deltaImpulse,deltaImpulse,deltaImpulse)));//*m_angularFactor);

  94. 		body2.mDeltaAngularVelocity += vmVector3(tmp.getX(),tmp.getY(),tmp.getZ());

  95. 	}

  96. 

  97. 	//body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);

  98. 	//body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);

  99. 

  100. }

  101. 

  102.  

  103. static SIMD_FORCE_INLINE

  104. void pfxSolveLinearConstraintRow(btConstraintRow &constraint,

  105. 	vmVector3 &deltaLinearVelocityA,vmVector3 &deltaAngularVelocityA,

  106. 	float massInvA,const vmMatrix3 &inertiaInvA,const vmVector3 &rA,

  107. 	vmVector3 &deltaLinearVelocityB,vmVector3 &deltaAngularVelocityB,

  108. 	float massInvB,const vmMatrix3 &inertiaInvB,const vmVector3 &rB)

  109. {

  110. 	const vmVector3 normal(btReadVector3(constraint.m_normal));

  111. 	btScalar deltaImpulse = constraint.m_rhs;

  112. 	vmVector3 dVA = deltaLinearVelocityA + cross(deltaAngularVelocityA,rA);

  113. 	vmVector3 dVB = deltaLinearVelocityB + cross(deltaAngularVelocityB,rB);

  114. 	deltaImpulse -= constraint.m_jacDiagInv * dot(normal,dVA-dVB);

  115. 	btScalar oldImpulse = constraint.m_accumImpulse;

  116. 	constraint.m_accumImpulse = btClamped(oldImpulse + deltaImpulse,constraint.m_lowerLimit,constraint.m_upperLimit);

  117. 	deltaImpulse = constraint.m_accumImpulse - oldImpulse;

  118. 	deltaLinearVelocityA += deltaImpulse * massInvA * normal;

  119. 	deltaAngularVelocityA += deltaImpulse * inertiaInvA * cross(rA,normal);

  120. 	deltaLinearVelocityB -= deltaImpulse * massInvB * normal;

  121. 	deltaAngularVelocityB -= deltaImpulse * inertiaInvB * cross(rB,normal);

  122. 

  123. }

  124.  

  125. void btSolveContactConstraint(

  126. 	btConstraintRow &constraintResponse,

  127. 	btConstraintRow &constraintFriction1,

  128. 	btConstraintRow &constraintFriction2,

  129. 	const vmVector3 &contactPointA,

  130. 	const vmVector3 &contactPointB,

  131. 	PfxSolverBody &solverBodyA,

  132. 	PfxSolverBody &solverBodyB,

  133. 	float friction

  134. 	)

  135. {

  136. 	vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);

  137. 	vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);

  138. 

  139. 	pfxSolveLinearConstraintRow(constraintResponse,

  140. 		solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,

  141. 		solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);

  142. 

  143. 	float mf = friction*fabsf(constraintResponse.m_accumImpulse);

  144. 	constraintFriction1.m_lowerLimit = -mf;

  145. 	constraintFriction1.m_upperLimit =  mf;

  146. 	constraintFriction2.m_lowerLimit = -mf;

  147. 	constraintFriction2.m_upperLimit =  mf;

  148. 

  149. 	pfxSolveLinearConstraintRow(constraintFriction1,

  150. 		solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,

  151. 		solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);

  152. 

  153. 	pfxSolveLinearConstraintRow(constraintFriction2,

  154. 		solverBodyA.mDeltaLinearVelocity,solverBodyA.mDeltaAngularVelocity,solverBodyA.mMassInv,solverBodyA.mInertiaInv,rA,

  155. 		solverBodyB.mDeltaLinearVelocity,solverBodyB.mDeltaAngularVelocity,solverBodyB.mMassInv,solverBodyB.mInertiaInv,rB);

  156. }

  157. 

  158. 

  159. void CustomSolveConstraintsTaskParallel(

  160. 	const PfxParallelGroup *contactParallelGroup,const PfxParallelBatch *contactParallelBatches,

  161. 	PfxConstraintPair *contactPairs,uint32_t numContactPairs,

  162. 	btPersistentManifold* offsetContactManifolds,

  163. 	const PfxParallelGroup *jointParallelGroup,const PfxParallelBatch *jointParallelBatches,

  164. 	PfxConstraintPair *jointPairs,uint32_t numJointPairs,

  165. 	btSolverConstraint* offsetSolverConstraints,

  166. 	TrbState *offsetRigStates,

  167. 	PfxSolverBody *offsetSolverBodies,

  168. 	uint32_t numRigidBodies,

  169. 	int iteration,unsigned int taskId,unsigned int numTasks,btBarrier *barrier)

  170. {

  171. 

  172. 	PfxSolverBody staticBody;

  173. 	staticBody.mMassInv = 0.f;

  174. 	staticBody.mDeltaAngularVelocity=vmVector3(0,0,0);

  175. 	staticBody.mDeltaLinearVelocity =vmVector3(0,0,0);

  176. 

  177. 

  178. 	for(int k=0;k<iteration+1;k++) {

  179. 		// Joint

  180. 		for(uint32_t phaseId=0;phaseId<jointParallelGroup->numPhases;phaseId++) {

  181. 			for(uint32_t batchId=0;batchId<jointParallelGroup->numBatches[phaseId];batchId++) {

  182. 				uint32_t numPairs = jointParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];

  183. 				if(batchId%numTasks == taskId && numPairs > 0) {

  184. 					const PfxParallelBatch &batch = jointParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];

  185. 					for(uint32_t i=0;i<numPairs;i++) {

  186. 						PfxConstraintPair &pair = jointPairs[batch.pairIndices[i]];

  187. 						uint16_t iA = pfxGetRigidBodyIdA(pair);

  188. 						uint16_t iB = pfxGetRigidBodyIdB(pair);

  189. 

  190. 						

  191. 						PfxSolverBody &solverBodyA = iA != 65535 ? offsetSolverBodies[iA] : staticBody;

  192. 						PfxSolverBody &solverBodyB = iB != 65535 ? offsetSolverBodies[iB] : staticBody;

  193. 

  194. 						if(k==0) {

  195. 							

  196. 						}

  197. 						else {

  198. 							btSolverConstraint* constraintRow = &offsetSolverConstraints[pfxGetContactId1(pair)];

  199. 							int numRows = pfxGetNumConstraints(pair);

  200. 							int i;

  201. 							for (i=0;i<numRows;i++)

  202. 							{

  203. 								resolveSingleConstraintRowGeneric(solverBodyA,solverBodyB,constraintRow[i]);

  204. 							}

  205. 							

  206. 						}

  207. 					}

  208. 				}

  209. 			}

  210. 

  211. 			barrier->sync();

  212. 		}

  213. 

  214. 		// Contact

  215. 		for(uint32_t phaseId=0;phaseId<contactParallelGroup->numPhases;phaseId++) {

  216. 			for(uint32_t batchId=0;batchId<contactParallelGroup->numBatches[phaseId];batchId++) {

  217. 				uint32_t numPairs = contactParallelGroup->numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];

  218. 				if(batchId%numTasks == taskId && numPairs > 0) {

  219. 					const PfxParallelBatch &batch = contactParallelBatches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];

  220. 					for(uint32_t i=0;i<numPairs;i++) {

  221. 						PfxConstraintPair &pair = contactPairs[batch.pairIndices[i]];

  222. 						uint16_t iA = pfxGetRigidBodyIdA(pair);

  223. 						uint16_t iB = pfxGetRigidBodyIdB(pair);

  224. 

  225. 						btPersistentManifold& contact = offsetContactManifolds[pfxGetConstraintId1(pair)];

  226. 

  227. 

  228. 						PfxSolverBody &solverBodyA = offsetSolverBodies[iA];

  229. 						PfxSolverBody &solverBodyB = offsetSolverBodies[iB];

  230. 						

  231. 						for(int j=0;j<contact.getNumContacts();j++) {

  232. 							btManifoldPoint& cp = contact.getContactPoint(j);

  233. 							

  234. 							if(k==0) {

  235. 								vmVector3 rA = rotate(solverBodyA.mOrientation,btReadVector3(cp.m_localPointA));

  236. 								vmVector3 rB = rotate(solverBodyB.mOrientation,btReadVector3(cp.m_localPointB));

  237. 								

  238. 								for(int k=0;k<3;k++) {

  239. 									vmVector3 normal = btReadVector3(cp.mConstraintRow[k].m_normal);

  240. 									float deltaImpulse = cp.mConstraintRow[k].m_accumImpulse;

  241. 									solverBodyA.mDeltaLinearVelocity += deltaImpulse * solverBodyA.mMassInv * normal;

  242. 									solverBodyA.mDeltaAngularVelocity += deltaImpulse * solverBodyA.mInertiaInv * cross(rA,normal);

  243. 									solverBodyB.mDeltaLinearVelocity -= deltaImpulse * solverBodyB.mMassInv * normal;

  244. 									solverBodyB.mDeltaAngularVelocity -= deltaImpulse * solverBodyB.mInertiaInv * cross(rB,normal);

  245. 								}

  246. 							}

  247. 							else {

  248. 								btSolveContactConstraint(

  249. 									cp.mConstraintRow[0],

  250. 									cp.mConstraintRow[1],

  251. 									cp.mConstraintRow[2],

  252. 									btReadVector3(cp.m_localPointA),

  253. 									btReadVector3(cp.m_localPointB),

  254. 									solverBodyA,

  255. 									solverBodyB,

  256. 									cp.m_combinedFriction

  257. 									);

  258. 							}

  259. 						}

  260. 					}

  261. 				}

  262. 			}

  263. 

  264. 			if (barrier)

  265. 				barrier->sync();

  266. 		}

  267. 	}

  268. }

  269. 

  270. void CustomPostSolverTask(

  271. 	TrbState *states,

  272. 	PfxSolverBody *solverBodies,

  273. 	uint32_t numRigidBodies)

  274. {

  275. 	for(uint32_t i=0;i<numRigidBodies;i++) {

  276. 		TrbState &state = states[i];

  277. 		PfxSolverBody &solverBody = solverBodies[i];

  278. 		state.setLinearVelocity(state.getLinearVelocity()+solverBody.mDeltaLinearVelocity);

  279. 		state.setAngularVelocity(state.getAngularVelocity()+solverBody.mDeltaAngularVelocity);

  280. 	}

  281. }

  282. 

  283. void*	SolverlsMemoryFunc()

  284. {

  285. 	//don't create local store memory, just return 0

  286. 	return 0;

  287. }

  288. 

  289. 

  290. static SIMD_FORCE_INLINE

  291. void pfxGetPlaneSpace(const vmVector3& n, vmVector3& p, vmVector3& q)

  292. {

  293. 	if(fabsf(n[2]) > 0.707f) {

  294. 		// choose p in y-z plane

  295. 		float a = n[1]*n[1] + n[2]*n[2];

  296. 		float k = 1.0f/sqrtf(a);

  297. 		p[0] = 0;

  298. 		p[1] = -n[2]*k;

  299. 		p[2] = n[1]*k;

  300. 		// set q = n x p

  301. 		q[0] = a*k;

  302. 		q[1] = -n[0]*p[2];

  303. 		q[2] = n[0]*p[1];

  304. 	}

  305. 	else {

  306. 		// choose p in x-y plane

  307. 		float a = n[0]*n[0] + n[1]*n[1];

  308. 		float k = 1.0f/sqrtf(a);

  309. 		p[0] = -n[1]*k;

  310. 		p[1] = n[0]*k;

  311. 		p[2] = 0;

  312. 		// set q = n x p

  313. 		q[0] = -n[2]*p[1];

  314. 		q[1] = n[2]*p[0];

  315. 		q[2] = a*k;

  316. 	}

  317. }

  318. 

  319. 

  320. 

  321. #define PFX_CONTACT_SLOP 0.001f

  322. 

  323. void btSetupContactConstraint(

  324. 	btConstraintRow &constraintResponse,

  325. 	btConstraintRow &constraintFriction1,

  326. 	btConstraintRow &constraintFriction2,

  327. 	float penetrationDepth,

  328. 	float restitution,

  329. 	float friction,

  330. 	const vmVector3 &contactNormal,

  331. 	const vmVector3 &contactPointA,

  332. 	const vmVector3 &contactPointB,

  333. 	const TrbState &stateA,

  334. 	const TrbState &stateB,

  335. 	PfxSolverBody &solverBodyA,

  336. 	PfxSolverBody &solverBodyB,

  337. 	float separateBias,

  338. 	float timeStep

  339. 	)

  340. {

  341. 	vmVector3 rA = rotate(solverBodyA.mOrientation,contactPointA);

  342. 	vmVector3 rB = rotate(solverBodyB.mOrientation,contactPointB);

  343. 

  344. 	vmMatrix3 K = vmMatrix3::scale(vmVector3(solverBodyA.mMassInv + solverBodyB.mMassInv)) - 

  345. 			crossMatrix(rA) * solverBodyA.mInertiaInv * crossMatrix(rA) - 

  346. 			crossMatrix(rB) * solverBodyB.mInertiaInv * crossMatrix(rB);

  347. 

  348. 	vmVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);

  349. 	vmVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);

  350. 	vmVector3 vAB = vA-vB;

  351. 

  352. 	vmVector3 tangent1,tangent2;

  353. 	btPlaneSpace1(contactNormal,tangent1,tangent2);

  354. 

  355. //	constraintResponse.m_accumImpulse = 0.f;

  356. //	constraintFriction1.m_accumImpulse = 0.f;

  357. //	constraintFriction2.m_accumImpulse = 0.f;

  358. 

  359. 	// Contact Constraint

  360. 	{

  361. 		vmVector3 normal = contactNormal;

  362. 

  363. 		float denom = dot(K*normal,normal);

  364. 

  365. 		constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error

  366. 		constraintResponse.m_rhs -= (separateBias * btMin(0.0f,penetrationDepth+PFX_CONTACT_SLOP)) / timeStep; // position error

  367. 		constraintResponse.m_rhs /= denom;

  368. 		constraintResponse.m_jacDiagInv = 1.0f/denom;

  369. 		constraintResponse.m_lowerLimit = 0.0f;

  370. 		constraintResponse.m_upperLimit = SIMD_INFINITY;

  371. 		btStoreVector3(normal,constraintResponse.m_normal);

  372. 	}

  373. 

  374. 	// Friction Constraint 1

  375. 	{

  376. 		vmVector3 normal = tangent1;

  377. 

  378. 		float denom = dot(K*normal,normal);

  379. 

  380. 		constraintFriction1.m_jacDiagInv = 1.0f/denom;

  381. 		constraintFriction1.m_rhs = -dot(vAB,normal);

  382. 		constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;

  383. 		constraintFriction1.m_lowerLimit = 0.0f;

  384. 		constraintFriction1.m_upperLimit = SIMD_INFINITY;

  385. 		btStoreVector3(normal,constraintFriction1.m_normal);

  386. 	}

  387. 	

  388. 	// Friction Constraint 2

  389. 	{

  390. 		vmVector3 normal = tangent2;

  391. 

  392. 		float denom = dot(K*normal,normal);

  393. 

  394. 		constraintFriction2.m_jacDiagInv = 1.0f/denom;

  395. 		constraintFriction2.m_rhs = -dot(vAB,normal);

  396. 		constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;

  397. 		constraintFriction2.m_lowerLimit = 0.0f;

  398. 		constraintFriction2.m_upperLimit = SIMD_INFINITY;

  399. 		btStoreVector3(normal,constraintFriction2.m_normal);

  400. 	}

  401. }

  402. 

  403. 

  404. void CustomSetupContactConstraintsTask(

  405. 	PfxConstraintPair *contactPairs,uint32_t numContactPairs,

  406. 	btPersistentManifold*	offsetContactManifolds,

  407. 	TrbState *offsetRigStates,

  408. 	PfxSolverBody *offsetSolverBodies,

  409. 	uint32_t numRigidBodies,

  410. 	float separateBias,

  411. 	float timeStep)

  412. {

  413. 	for(uint32_t i=0;i<numContactPairs;i++) {

  414. 		PfxConstraintPair &pair = contactPairs[i];

  415. 		if(!pfxGetActive(pair) || pfxGetNumConstraints(pair) == 0 ||

  416. 			((pfxGetMotionMaskA(pair)&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pair)&PFX_MOTION_MASK_STATIC)) ) {

  417. 			continue;

  418. 		}

  419. 

  420. 		uint16_t iA = pfxGetRigidBodyIdA(pair);

  421. 		uint16_t iB = pfxGetRigidBodyIdB(pair);

  422. 

  423. 		int id = pfxGetConstraintId1(pair);

  424. 		btPersistentManifold& contact = offsetContactManifolds[id];

  425. 

  426. 

  427. 		TrbState &stateA = offsetRigStates[iA];

  428. //		PfxRigBody &bodyA = offsetRigBodies[iA];

  429. 		PfxSolverBody &solverBodyA = offsetSolverBodies[iA];

  430. 

  431. 		TrbState &stateB = offsetRigStates[iB];

  432. //		PfxRigBody &bodyB = offsetRigBodies[iB];

  433. 		PfxSolverBody &solverBodyB = offsetSolverBodies[iB];

  434. 		

  435. 		float restitution = 0.5f * (solverBodyA.restitution + solverBodyB.restitution);

  436. 		//if(contact.getDuration() > 1) restitution = 0.0f;

  437. 		

  438. 		float friction = sqrtf(solverBodyA.friction * solverBodyB.friction);

  439. 

  440. 		for(int j=0;j<contact.getNumContacts();j++) {

  441. 			btManifoldPoint& cp = contact.getContactPoint(j);

  442. 			

  443. 			btSetupContactConstraint(

  444. 				cp.mConstraintRow[0],

  445. 				cp.mConstraintRow[1],

  446. 				cp.mConstraintRow[2],

  447. 				cp.getDistance(),

  448. 				restitution,

  449. 				friction,

  450. 				btReadVector3(cp.m_normalWorldOnB),//.mConstraintRow[0].m_normal),

  451. 				btReadVector3(cp.m_localPointA),

  452. 				btReadVector3(cp.m_localPointB),

  453. 				stateA,

  454. 				stateB,

  455. 				solverBodyA,

  456. 				solverBodyB,

  457. 				separateBias,

  458. 				timeStep

  459. 				);

  460. 		}

  461. 

  462. 		//contact.setCompositeFriction(friction);

  463. 	}

  464. }

  465. 

  466. void	SolverThreadFunc(void* userPtr,void* lsMemory)

  467. {

  468. 	btConstraintSolverIO* io = (btConstraintSolverIO*)(userPtr);//arg->io);

  469. 	btCriticalSection* criticalsection = io->setupContactConstraints.criticalSection;

  470. 	

  471. 

  472. 	//CustomCriticalSection *criticalsection = &io->m_cs;

  473. 	switch(io->cmd) {

  474. 

  475. 		case PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS:

  476. 		CustomSolveConstraintsTaskParallel(

  477. 			io->solveConstraints.contactParallelGroup,

  478. 			io->solveConstraints.contactParallelBatches,

  479. 			io->solveConstraints.contactPairs,

  480. 			io->solveConstraints.numContactPairs,

  481. 			io->solveConstraints.offsetContactManifolds,

  482. 

  483. 			io->solveConstraints.jointParallelGroup,

  484. 			io->solveConstraints.jointParallelBatches,

  485. 			io->solveConstraints.jointPairs,

  486. 			io->solveConstraints.numJointPairs,

  487. 			io->solveConstraints.offsetSolverConstraints,

  488. 			io->solveConstraints.offsetRigStates1,

  489. 			io->solveConstraints.offsetSolverBodies,

  490. 			io->solveConstraints.numRigidBodies,

  491. 			io->solveConstraints.iteration,

  492. 

  493. 			io->solveConstraints.taskId,

  494. 			io->maxTasks1,

  495. 			io->solveConstraints.barrier

  496. 			);

  497. 		break;

  498. 

  499. 		case PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER:

  500. 			CustomPostSolverTask(	io->postSolver.states,io->postSolver.solverBodies,	io->postSolver.numRigidBodies);

  501. 			break;

  502. 

  503. 

  504. 		case PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS:

  505. 		{

  506. 			bool empty = false;

  507. 			while(!empty) {

  508. 				int start,batch;

  509. 				

  510. 				criticalsection->lock();

  511. 

  512. 				start = (int)criticalsection->getSharedParam(0);

  513. 				batch = (int)criticalsection->getSharedParam(1);

  514. 

  515. 				//PFX_PRINTF("taskId %d start %d num %d\n",arg->taskId,start,batch);

  516. 

  517. 				// 次のバッファをセット

  518. 				int nextStart = start + batch;

  519. 				int rest = btMax((int)io->setupContactConstraints.numContactPairs1 - nextStart,0);

  520. 				int nextBatch = (rest > batch)?batch:rest;

  521. 

  522. 				criticalsection->setSharedParam(0,nextStart);

  523.                 criticalsection->setSharedParam(1,nextBatch);

  524. 

  525. 				criticalsection->unlock();

  526. 				

  527. 				if(batch > 0) {

  528. 					CustomSetupContactConstraintsTask(

  529. 						io->setupContactConstraints.offsetContactPairs+start,batch,

  530. 						io->setupContactConstraints.offsetContactManifolds,

  531. 						io->setupContactConstraints.offsetRigStates,

  532. //						io->setupContactConstraints.offsetRigBodies,

  533. 						io->setupContactConstraints.offsetSolverBodies,

  534. 						io->setupContactConstraints.numRigidBodies,

  535. 						io->setupContactConstraints.separateBias,

  536. 						io->setupContactConstraints.timeStep);

  537. 				}

  538. 				else {

  539. 					empty = true;

  540. 				}

  541. 			}

  542. 		}

  543. 		break;

  544. 

  545. 		default:

  546. 			{

  547. 				btAssert(0);

  548. 			}

  549. 	}

  550. 

  551. }

  552. 

  553. 

  554. void CustomSetupContactConstraintsNew(

  555. 	PfxConstraintPair *contactPairs1,uint32_t numContactPairs,

  556. 	btPersistentManifold *offsetContactManifolds,

  557. 	TrbState *offsetRigStates,

  558. 	PfxSolverBody *offsetSolverBodies,

  559. 	uint32_t numRigidBodies,

  560. 	float separationBias,

  561. 	float timeStep,

  562. 	class btThreadSupportInterface* threadSupport,

  563. 	btCriticalSection* criticalSection,

  564. 	btConstraintSolverIO *io 

  565. 	)

  566. {

  567. 	int maxTasks = threadSupport->getNumTasks();

  568. 

  569. 	int div = (int)maxTasks * 4;

  570. 	int batch = ((int)numContactPairs + div - 1) / div;

  571. #ifdef __PPU__

  572. 		BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;

  573. #endif

  574. 	if (criticalSection)

  575. 	{

  576. 		criticalSection->setSharedParam(0,0);

  577. 		criticalSection->setSharedParam(1,btMin(batch,64)); // batched number

  578. 	} else

  579. 	{

  580. #ifdef __PPU__

  581. 		spursThread->setSharedParam(0,0);

  582. 		spursThread->setSharedParam(1,btMin(batch,64)); // batched number

  583. #endif //__PPU__

  584. 	}

  585. 

  586. 	for(int t=0;t<maxTasks;t++) {

  587. 		io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_SETUP_CONTACT_CONSTRAINTS;

  588. 		io[t].setupContactConstraints.offsetContactPairs = contactPairs1;

  589. 		io[t].setupContactConstraints.numContactPairs1 = numContactPairs;

  590. 		io[t].setupContactConstraints.offsetRigStates = offsetRigStates;

  591. 		io[t].setupContactConstraints.offsetContactManifolds = offsetContactManifolds;		

  592. 		io[t].setupContactConstraints.offsetSolverBodies = offsetSolverBodies;

  593. 		io[t].setupContactConstraints.numRigidBodies = numRigidBodies;

  594. 		io[t].setupContactConstraints.separateBias = separationBias;

  595. 		io[t].setupContactConstraints.timeStep = timeStep;

  596. 		io[t].setupContactConstraints.criticalSection = criticalSection;

  597. 		io[t].maxTasks1 = maxTasks;

  598. #ifdef __PPU__

  599. 		io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();

  600. 		io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();

  601. #endif

  602. 	

  603. 

  604. //#define SEQUENTIAL_SETUP

  605. #ifdef SEQUENTIAL_SETUP

  606. 		CustomSetupContactConstraintsTask(contactPairs1,numContactPairs,offsetContactManifolds,offsetRigStates,offsetSolverBodies,numRigidBodies,separationBias,timeStep);

  607. #else

  608. 		threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);

  609. #endif

  610. 

  611. 	}

  612. #ifndef SEQUENTIAL_SETUP

  613. 	unsigned int arg0,arg1;

  614. 	for(int t=0;t<maxTasks;t++) {

  615. 		arg0 = t;

  616. 		threadSupport->waitForResponse(&arg0,&arg1);

  617. 	}

  618. #endif //SEQUENTIAL_SETUP

  619. 

  620. }

  621. 

  622. 

  623. void CustomSplitConstraints(

  624. 	PfxConstraintPair *pairs,uint32_t numPairs,

  625. 	PfxParallelGroup &group,PfxParallelBatch *batches,

  626. 	uint32_t numTasks,

  627. 	uint32_t numRigidBodies,

  628. 	void *poolBuff,

  629. 	uint32_t poolBytes

  630. 	)

  631. {

  632. 	HeapManager pool((unsigned char*)poolBuff,poolBytes);

  633. 

  634. 	// ステートチェック用ビットフラグテーブル

  635. 	int bufSize = sizeof(uint8_t)*numRigidBodies;

  636. 	bufSize = ((bufSize+127)>>7)<<7; // 128 bytes alignment

  637. 	uint8_t *bodyTable = (uint8_t*)pool.allocate(bufSize,HeapManager::ALIGN128);

  638. 

  639. 	// ペアチェック用ビットフラグテーブル

  640. 	uint32_t *pairTable;

  641. 	size_t allocSize = sizeof(uint32_t)*((numPairs+31)/32);

  642. 	pairTable = (uint32_t*)pool.allocate(allocSize);

  643. 	memset(pairTable,0,allocSize);

  644. 

  645. 	// 目標とする分割数

  646. 	uint32_t targetCount = btMax(uint32_t(PFX_MIN_SOLVER_PAIRS),btMin(numPairs / (numTasks*2),uint32_t(PFX_MAX_SOLVER_PAIRS)));

  647. 	uint32_t startIndex = 0;

  648. 	

  649. 	uint32_t phaseId;

  650. 	uint32_t batchId;

  651. 	uint32_t totalCount=0;

  652. 	

  653. 	uint32_t maxBatches = btMin(numTasks,uint32_t(PFX_MAX_SOLVER_BATCHES));

  654. 	

  655. 	for(phaseId=0;phaseId<PFX_MAX_SOLVER_PHASES&&totalCount<numPairs;phaseId++) {

  656. 		bool startIndexCheck = true;

  657. 		

  658. 		group.numBatches[phaseId] = 0;

  659. 		

  660. 		uint32_t i = startIndex;

  661. 		

  662.         // チェック用ビットフラグテーブルをクリア

  663. 		memset(bodyTable,0xff,bufSize);

  664. 		

  665. 		for(batchId=0;i<numPairs&&totalCount<numPairs&&batchId<maxBatches;batchId++) {

  666. 			uint32_t pairCount=0;

  667. 			

  668. 			PfxParallelBatch &batch = batches[phaseId*PFX_MAX_SOLVER_BATCHES+batchId];

  669. 			uint32_t pairId = 0;

  670. 			

  671. 			for(;i<numPairs&&pairCount<targetCount;i++) {

  672. 				uint32_t idxP = i>>5;

  673. 				uint32_t maskP = 1L << (i & 31);

  674. 				

  675. 				//pair is already assigned to a phase/batch

  676. 				if(pairTable[idxP] & maskP) {

  677. 					continue;

  678. 				}

  679. 				

  680. 				uint32_t idxA = pfxGetRigidBodyIdA(pairs[i]);

  681. 				uint32_t idxB = pfxGetRigidBodyIdB(pairs[i]);

  682. 

  683. 				// 両方ともアクティブでない、または衝突点が０のペアは登録対象からはずす

  684. 				if(!pfxGetActive(pairs[i]) || pfxGetNumConstraints(pairs[i]) == 0 ||

  685. 					((pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_STATIC) && (pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_STATIC)) ) {

  686. 					if(startIndexCheck) 

  687. 						startIndex++;

  688. 					//assign pair -> skip it because it has no constraints

  689. 					pairTable[idxP] |= maskP;

  690. 					totalCount++;

  691. 					continue;

  692. 				}

  693. 				

  694. 				// 依存性のチェック

  695. 				if( (bodyTable[idxA] != batchId && bodyTable[idxA] != 0xff) || 

  696. 					(bodyTable[idxB] != batchId && bodyTable[idxB] != 0xff) ) {

  697. 					startIndexCheck = false;

  698. 					//bodies of the pair are already assigned to another batch within this phase

  699. 					continue;

  700. 				}

  701. 				

  702. 				// 依存性判定テーブルに登録

  703. 				if(pfxGetMotionMaskA(pairs[i])&PFX_MOTION_MASK_DYNAMIC) 

  704. 						bodyTable[idxA] = batchId;

  705. 				if(pfxGetMotionMaskB(pairs[i])&PFX_MOTION_MASK_DYNAMIC) 

  706. 						bodyTable[idxB] = batchId;

  707. 				

  708. 				if(startIndexCheck) 

  709. 					startIndex++;

  710. 				

  711. 				pairTable[idxP] |= maskP;

  712. 				//add the pair 'i' to the current batch

  713. 				batch.pairIndices[pairId++] = i;

  714. 				pairCount++;

  715. 			}

  716. 

  717. 			group.numPairs[phaseId*PFX_MAX_SOLVER_BATCHES+batchId] = (uint16_t)pairId;

  718. 			totalCount += pairCount;

  719. 		}

  720. 

  721. 		group.numBatches[phaseId] = batchId;

  722. 	}

  723. 

  724. 	group.numPhases = phaseId;

  725. 

  726. 	pool.clear();

  727. }

  728. 

  729. 

  730. 

  731. void CustomSolveConstraintsParallel(

  732. 	PfxConstraintPair *contactPairs,uint32_t numContactPairs,

  733. 	

  734. 	PfxConstraintPair *jointPairs,uint32_t numJointPairs,

  735. 	btPersistentManifold* offsetContactManifolds,

  736. 	btSolverConstraint* offsetSolverConstraints,

  737. 	TrbState *offsetRigStates,

  738. 	PfxSolverBody *offsetSolverBodies,

  739. 	uint32_t numRigidBodies,

  740. 	struct btConstraintSolverIO* io,

  741. 	class btThreadSupportInterface* threadSupport,

  742. 	int iteration,

  743. 	void* poolBuf,

  744. 	int poolBytes,

  745. 	class btBarrier* barrier)

  746. 	{

  747. 

  748. 	int maxTasks = threadSupport->getNumTasks();

  749. //	config.taskManager->setTaskEntry(PFX_SOLVER_ENTRY);

  750. 

  751. 	HeapManager pool((unsigned char*)poolBuf,poolBytes);

  752. 

  753. 	{

  754. 		PfxParallelGroup *cgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));

  755. 		PfxParallelBatch *cbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);

  756. 		PfxParallelGroup *jgroup = (PfxParallelGroup*)pool.allocate(sizeof(PfxParallelGroup));

  757. 		PfxParallelBatch *jbatches = (PfxParallelBatch*)pool.allocate(sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES),128);

  758. 		

  759. 		uint32_t tmpBytes = poolBytes - 2 * (sizeof(PfxParallelGroup) + sizeof(PfxParallelBatch)*(PFX_MAX_SOLVER_PHASES*PFX_MAX_SOLVER_BATCHES) + 128);

  760. 		void *tmpBuff = pool.allocate(tmpBytes);

  761. 		

  762. 		{

  763. 			BT_PROFILE("CustomSplitConstraints");

  764. 			CustomSplitConstraints(contactPairs,numContactPairs,*cgroup,cbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);

  765. 			CustomSplitConstraints(jointPairs,numJointPairs,*jgroup,jbatches,maxTasks,numRigidBodies,tmpBuff,tmpBytes);

  766. 		}

  767. 

  768. 		{

  769. 			BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS");

  770. //#define SOLVE_SEQUENTIAL

  771. #ifdef SOLVE_SEQUENTIAL

  772. 		CustomSolveConstraintsTask(

  773. 			io->solveConstraints.contactParallelGroup,

  774. 			io->solveConstraints.contactParallelBatches,

  775. 			io->solveConstraints.contactPairs,

  776. 			io->solveConstraints.numContactPairs,

  777. 			io->solveConstraints.offsetContactManifolds,

  778. 

  779. 			io->solveConstraints.jointParallelGroup,

  780. 			io->solveConstraints.jointParallelBatches,

  781. 			io->solveConstraints.jointPairs,

  782. 			io->solveConstraints.numJointPairs,

  783. 			io->solveConstraints.offsetJoints,

  784. 

  785. 			io->solveConstraints.offsetRigStates,

  786. 			io->solveConstraints.offsetSolverBodies,

  787. 			io->solveConstraints.numRigidBodies,

  788. 			io->solveConstraints.iteration,0,1,0);//arg->taskId,1,0);//,arg->maxTasks,arg->barrier);

  789. #else

  790. 		for(int t=0;t<maxTasks;t++) {

  791. 			io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_SOLVE_CONSTRAINTS;

  792. 			io[t].solveConstraints.contactParallelGroup = cgroup;

  793. 			io[t].solveConstraints.contactParallelBatches = cbatches;

  794. 			io[t].solveConstraints.contactPairs = contactPairs;

  795. 			io[t].solveConstraints.numContactPairs = numContactPairs;

  796. 			io[t].solveConstraints.offsetContactManifolds = offsetContactManifolds;

  797. 			io[t].solveConstraints.jointParallelGroup = jgroup;

  798. 			io[t].solveConstraints.jointParallelBatches = jbatches;

  799. 			io[t].solveConstraints.jointPairs = jointPairs;

  800. 			io[t].solveConstraints.numJointPairs = numJointPairs;

  801. 			io[t].solveConstraints.offsetSolverConstraints = offsetSolverConstraints;

  802. 			io[t].solveConstraints.offsetRigStates1 = offsetRigStates;

  803. 			io[t].solveConstraints.offsetSolverBodies = offsetSolverBodies;

  804. 			io[t].solveConstraints.numRigidBodies = numRigidBodies;

  805. 			io[t].solveConstraints.iteration = iteration;

  806. 			io[t].solveConstraints.taskId = t;

  807. 			io[t].solveConstraints.barrier = barrier;

  808. 

  809. 		io[t].maxTasks1 = maxTasks;

  810. #ifdef __PPU__

  811. 		BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;

  812. 		io[t].barrierAddr2 = (unsigned int) spursThread->getBarrierAddress();

  813. 		io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();

  814. #endif

  815. 

  816. 			threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);

  817. 		}

  818. 

  819. 		unsigned int arg0,arg1;

  820. 		for(int t=0;t<maxTasks;t++) {

  821. 			arg0 = t;

  822. 			threadSupport->waitForResponse(&arg0,&arg1);

  823. 		}

  824. #endif

  825. 		}

  826. 		pool.clear();

  827. 	}

  828. 

  829. 	{

  830. 			BT_PROFILE("PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER");

  831. 		int batch = ((int)numRigidBodies + maxTasks - 1) / maxTasks;

  832. 		int rest = (int)numRigidBodies;

  833. 		int start = 0;

  834. 

  835. 		for(int t=0;t<maxTasks;t++) {

  836. 			int num = (rest - batch ) > 0 ? batch : rest;

  837. 			io[t].cmd = PFX_CONSTRAINT_SOLVER_CMD_POST_SOLVER;

  838. 			io[t].postSolver.states = offsetRigStates + start;

  839. 			io[t].postSolver.solverBodies = offsetSolverBodies + start;

  840. 			io[t].postSolver.numRigidBodies = (uint32_t)num;

  841. 		io[t].maxTasks1 = maxTasks;

  842. #ifdef __PPU__

  843. 		BulletPE2ConstraintSolverSpursSupport* spursThread = (BulletPE2ConstraintSolverSpursSupport*) threadSupport;

  844. 		io[t].barrierAddr2 = (unsigned int)spursThread->getBarrierAddress();

  845. 		io[t].criticalsectionAddr2 = (unsigned int)spursThread->getCriticalSectionAddress();

  846. #endif

  847. 

  848. #ifdef SOLVE_SEQUENTIAL

  849. 			CustomPostSolverTask(	io[t].postSolver.states,io[t].postSolver.solverBodies,	io[t].postSolver.numRigidBodies);

  850. #else

  851. 			threadSupport->sendRequest(1,(ppu_address_t)&io[t],t);

  852. #endif

  853. 			rest -= num;

  854. 			start += num;

  855. 		}

  856. 

  857. 		unsigned int arg0,arg1;

  858. 		for(int t=0;t<maxTasks;t++) {

  859. #ifndef SOLVE_SEQUENTIAL

  860. 			arg0 = t;

  861. 			threadSupport->waitForResponse(&arg0,&arg1);

  862. #endif

  863. 		}

  864. 	}

  865. 

  866. }

  867. 

  868. 

  869. 

  870. void BPE_customConstraintSolverSequentialNew(unsigned int new_num, PfxBroadphasePair *new_pairs1 ,

  871. 									btPersistentManifold* offsetContactManifolds,

  872. 									  TrbState* states,int numRigidBodies, 

  873. 									  struct PfxSolverBody* solverBodies, 

  874. 									  PfxConstraintPair* jointPairs, unsigned int numJoints,

  875. 									  btSolverConstraint* offsetSolverConstraints,

  876. 									  float separateBias,

  877. 									  float timeStep,

  878. 									  int iteration,

  879. 									  btThreadSupportInterface* solverThreadSupport,

  880. 									  btCriticalSection* criticalSection,

  881. 									  struct btConstraintSolverIO* solverIO,

  882. 									  btBarrier* barrier

  883. 									  )

  884. {

  885. 

  886. 	{

  887. 		BT_PROFILE("pfxSetupConstraints");

  888. 

  889. 		for(uint32_t i=0;i<numJoints;i++) {

  890. 			// 情報の更新

  891. 			PfxConstraintPair &pair = jointPairs[i];

  892. 			int idA = pfxGetRigidBodyIdA(pair);

  893. 

  894. 			if (idA != 65535)

  895. 			{

  896. 				pfxSetMotionMaskA(pair,states[pfxGetRigidBodyIdA(pair)].getMotionMask());

  897. 			}

  898. 			else

  899. 			{

  900. 				pfxSetMotionMaskA(pair,PFX_MOTION_MASK_STATIC);

  901. 			}

  902. 			int idB = pfxGetRigidBodyIdB(pair);

  903. 			if (idB!= 65535)

  904. 			{

  905. 				pfxSetMotionMaskB(pair,states[pfxGetRigidBodyIdB(pair)].getMotionMask());

  906. 			} else

  907. 			{

  908. 				pfxSetMotionMaskB(pair,PFX_MOTION_MASK_STATIC);

  909. 			}

  910. 		}

  911. 

  912. //		CustomSetupJointConstraintsSeq(			jointPairs,numJoints,joints,			states,			solverBodies,			numRigidBodies,			timeStep);

  913. 

  914. #ifdef SEQUENTIAL_SETUP

  915. 		CustomSetupContactConstraintsSeqNew(

  916. 			(PfxConstraintPair*)new_pairs1,new_num,contacts,

  917. 			states,

  918. 			solverBodies,

  919. 			numRigidBodies,

  920. 			separateBias,

  921. 			timeStep);

  922. #else

  923. 		CustomSetupContactConstraintsNew(

  924. 			(PfxConstraintPair*)new_pairs1,new_num,

  925. 			offsetContactManifolds,

  926. 			states,

  927. 			solverBodies,

  928. 			numRigidBodies,

  929. 			separateBias,

  930. 			timeStep,

  931. 			solverThreadSupport,

  932. 			criticalSection,solverIO

  933. 			);

  934. 

  935. #endif //SEQUENTIAL_SETUP

  936. 

  937. 	}

  938. 	{

  939. 		BT_PROFILE("pfxSolveConstraints");

  940. 

  941. //#define SEQUENTIAL

  942. #ifdef SEQUENTIAL

  943. 		CustomSolveConstraintsSeq(

  944. 			(PfxConstraintPair*)new_pairs1,new_num,contacts,

  945. 			jointPairs,numJoints,

  946. 			states,

  947. 			solverBodies,

  948. 			numRigidBodies,

  949. 			separateBias,

  950. 			timeStep,

  951. 			iteration);

  952. #else //SEQUENTIAL

  953. 		CustomSolveConstraintsParallel(

  954. 			(PfxConstraintPair*)new_pairs1,new_num,

  955. 			jointPairs,numJoints,

  956. 			offsetContactManifolds,

  957. 			offsetSolverConstraints,

  958. 			states,

  959. 			solverBodies,

  960. 			numRigidBodies,

  961. 			solverIO, solverThreadSupport,

  962. 			iteration,

  963. 			tmp_buff,

  964. 			TMP_BUFF_BYTES,

  965. 			barrier

  966. 			);

  967. 

  968. #endif //SEQUENTIAL

  969. 	}

  970. 

  971. }

  972. 

  973. 

  974. struct	btParallelSolverMemoryCache

  975. {

  976. 	btAlignedObjectArray<TrbState>	m_mystates;

  977. 	btAlignedObjectArray<PfxSolverBody>  m_mysolverbodies;

  978. 	btAlignedObjectArray<PfxBroadphasePair> m_mypairs;

  979. 	btAlignedObjectArray<PfxConstraintPair> m_jointPairs;

  980. 	

  981. };

  982. 

  983. 

  984. btConstraintSolverIO* createSolverIO(int numThreads)

  985. {

  986. 	return new btConstraintSolverIO[numThreads];

  987. }

  988. 

  989. btParallelConstraintSolver::btParallelConstraintSolver(btThreadSupportInterface* solverThreadSupport)

  990. {

  991. 	

  992. 	m_solverThreadSupport = solverThreadSupport;//createSolverThreadSupport(maxNumThreads);

  993. 	m_solverIO = createSolverIO(m_solverThreadSupport->getNumTasks());

  994. 

  995. 	m_barrier = m_solverThreadSupport->createBarrier();

  996. 	m_criticalSection = m_solverThreadSupport->createCriticalSection();

  997. 

  998. 	m_memoryCache = new btParallelSolverMemoryCache();

  999. }

  1000. 	

  1001. btParallelConstraintSolver::~btParallelConstraintSolver()

  1002. {

  1003. 	delete m_memoryCache;

  1004. 	delete m_solverIO;

  1005. }

  1006. 

  1007. 

  1008. 

  1009. btScalar btParallelConstraintSolver::solveGroup(btCollisionObject** bodies1,int numRigidBodies,btPersistentManifold** manifoldPtr,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher)

  1010. {

  1011. 	

  1012. /*	int sz = sizeof(PfxSolverBody);

  1013. 	int sz2 = sizeof(vmVector3);

  1014. 	int sz3 = sizeof(vmMatrix3);

  1015. 	int sz4 = sizeof(vmQuat);

  1016. 	int sz5 = sizeof(btConstraintRow);

  1017. 	int sz6 = sizeof(btSolverConstraint);

  1018. 	int sz7 = sizeof(TrbState);

  1019. */

  1020. 

  1021. 	btPersistentManifold* offsetContactManifolds= (btPersistentManifold*) dispatcher->getInternalManifoldPool()->getPoolAddress();

  1022. 

  1023. 		

  1024. 	m_memoryCache->m_mysolverbodies.resize(numRigidBodies);

  1025. 	m_memoryCache->m_mystates.resize(numRigidBodies);

  1026. 

  1027. 	{

  1028. 			BT_PROFILE("create states and solver bodies");

  1029. 	for (int i=0;i<numRigidBodies;i++)

  1030. 	{

  1031. 		btCollisionObject* obj = bodies1[i];

  1032. 		obj->setCompanionId(i);

  1033. 

  1034. 		PfxSolverBody& solverBody = m_memoryCache->m_mysolverbodies[i];

  1035. 		btRigidBody* rb = btRigidBody::upcast(obj);

  1036. 		TrbState& state = m_memoryCache->m_mystates[i];

  1037. 	

  1038. 		state.reset();

  1039. 		const btQuaternion& orgOri = obj->getWorldTransform().getRotation();

  1040. 		vmQuat orn(orgOri.getX(),orgOri.getY(),orgOri.getZ(),orgOri.getW());

  1041. 		state.setPosition(getVmVector3(obj->getWorldTransform().getOrigin()));

  1042. 		state.setOrientation(orn);

  1043. 		state.setPosition(state.getPosition());

  1044. 		state.setRigidBodyId(i);

  1045. 		state.setAngularDamping(0);

  1046. 		state.setLinearDamping(0);

  1047. 		

  1048. 		

  1049. 		solverBody.mOrientation = state.getOrientation();

  1050. 		solverBody.mDeltaLinearVelocity = vmVector3(0.0f);

  1051. 		solverBody.mDeltaAngularVelocity = vmVector3(0.0f);

  1052. 		solverBody.friction = obj->getFriction();

  1053. 		solverBody.restitution = obj->getRestitution();

  1054. 		

  1055. 		state.resetSleepCount();

  1056. 		

  1057. 		//if(state.getMotionMask()&PFX_MOTION_MASK_DYNAMIC) {

  1058. 		if (rb && (rb->getInvMass()>0.f))

  1059. 		{

  1060. 			state.setAngularVelocity(vmVector3(rb->getAngularVelocity().getX(),rb->getAngularVelocity().getY(),rb->getAngularVelocity().getZ()));

  1061. 			state.setLinearVelocity(vmVector3(rb->getLinearVelocity().getX(),rb->getLinearVelocity().getY(),rb->getLinearVelocity().getZ()));

  1062. 	

  1063. 			state.setMotionType(PfxMotionTypeActive);

  1064. 			vmMatrix3 ori(solverBody.mOrientation);

  1065. 			vmMatrix3 localInvInertia = vmMatrix3::identity();

  1066. 			localInvInertia.setCol(0,vmVector3(rb->getInvInertiaDiagLocal().getX(),0,0));

  1067. 			localInvInertia.setCol(1,vmVector3(0, rb->getInvInertiaDiagLocal().getY(),0));

  1068. 			localInvInertia.setCol(2,vmVector3(0,0, rb->getInvInertiaDiagLocal().getZ()));

  1069. 

  1070. 			solverBody.mMassInv = rb->getInvMass();

  1071. 			solverBody.mInertiaInv = ori * localInvInertia * transpose(ori);

  1072. 		} else

  1073. 		{

  1074. 			state.setAngularVelocity(vmVector3(0));

  1075. 			state.setLinearVelocity(vmVector3(0));

  1076. 		

  1077. 			state.setMotionType(PfxMotionTypeFixed);

  1078. 			m_memoryCache->m_mysolverbodies[i].mMassInv = 0.f;

  1079. 			m_memoryCache->m_mysolverbodies[i].mInertiaInv = vmMatrix3(0.0f);

  1080. 		}

  1081. 

  1082. 	}

  1083. 	}

  1084. 

  1085. 

  1086. 

  1087. 	int totalPoints = 0;

  1088. #ifndef USE_C_ARRAYS

  1089. 	m_memoryCache->m_mypairs.resize(numManifolds);

  1090. 	m_memoryCache->m_jointPairs.resize(numConstraints);

  1091. #endif//USE_C_ARRAYS

  1092. 

  1093. 	int actualNumManifolds= 0;

  1094. 	{

  1095. 		BT_PROFILE("convert manifolds");

  1096. 		for (int i1=0;i1<numManifolds;i1++)

  1097. 		{

  1098. 			if (manifoldPtr[i1]->getNumContacts()>0)

  1099. 			{

  1100. 				btPersistentManifold* m = manifoldPtr[i1];

  1101. 				btCollisionObject* obA = (btCollisionObject*)m->getBody0();

  1102. 				btCollisionObject* obB = (btCollisionObject*)m->getBody1();

  1103. 				bool obAisActive = !obA->isStaticOrKinematicObject() && obA->isActive();

  1104. 				bool obBisActive = !obB->isStaticOrKinematicObject() && obB->isActive();

  1105. 

  1106. 				if (!obAisActive && !obBisActive)

  1107. 					continue;

  1108. 

  1109. 

  1110. 				//int contactId = i1;//actualNumManifolds;

  1111. 				

  1112. 				PfxBroadphasePair& pair = m_memoryCache->m_mypairs[actualNumManifolds];

  1113. 				//init those

  1114. 				float compFric = obA->getFriction()*obB->getFriction();//@todo

  1115. 				int idA = obA->getCompanionId();

  1116. 				int idB = obB->getCompanionId();

  1117. 				

  1118. 				m->m_companionIdA = idA;

  1119. 				m->m_companionIdB = idB;

  1120. 				

  1121. 				

  1122. 			//	if ((mysolverbodies[idA].mMassInv!=0)&&(mysolverbodies[idB].mMassInv!=0))

  1123. 			//		continue;

  1124. 				int numPosPoints=0;

  1125. 				for (int p=0;p<m->getNumContacts();p++)

  1126. 				{

  1127. 					//btManifoldPoint& pt = m->getContactPoint(p);

  1128. 					//float dist = pt.getDistance();

  1129. 					//if (dist<0.001)

  1130. 						numPosPoints++;

  1131. 				}

  1132. 

  1133. 				

  1134. 				numPosPoints = numPosPoints;

  1135. 				totalPoints+=numPosPoints;

  1136. 				pfxSetRigidBodyIdA(pair,idA);

  1137. 				pfxSetRigidBodyIdB(pair,idB);

  1138. 				pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());

  1139. 				pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());

  1140. 				pfxSetActive(pair,numPosPoints>0);

  1141. 				

  1142. 				pfxSetBroadphaseFlag(pair,0);

  1143. 				int contactId = m-offsetContactManifolds;

  1144. 				//likely the contact pool is not contiguous, make sure to allocate large enough contact pool

  1145. 				btAssert(contactId>=0);

  1146. 				btAssert(contactId<dispatcher->getInternalManifoldPool()->getMaxCount());

  1147. 				

  1148. 				pfxSetContactId(pair,contactId);

  1149. 				pfxSetNumConstraints(pair,numPosPoints);//manifoldPtr[i]->getNumContacts());

  1150. 				actualNumManifolds++;

  1151. 			}

  1152. 

  1153. 		}

  1154. 	}

  1155. 

  1156. 	PfxConstraintPair* jointPairs=0;

  1157. 	jointPairs = numConstraints? &m_memoryCache->m_jointPairs[0]:0;

  1158. 	int actualNumJoints=0;

  1159. 

  1160. 

  1161. 	btSolverConstraint* offsetSolverConstraints = 0;

  1162. 

  1163. 	//if (1)

  1164. 	{

  1165. 		

  1166. 		{

  1167. 			BT_PROFILE("convert constraints");

  1168. 

  1169. 			int totalNumRows = 0;

  1170. 			int i;

  1171. 			

  1172. 			m_tmpConstraintSizesPool.resize(numConstraints);

  1173. 			//calculate the total number of contraint rows

  1174. 			for (i=0;i<numConstraints;i++)

  1175. 			{

  1176. 				btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];

  1177. 				constraints[i]->getInfo1(&info1);

  1178. 				totalNumRows += info1.m_numConstraintRows;

  1179. 			}

  1180. 			m_tmpSolverNonContactConstraintPool.resize(totalNumRows);

  1181. 			offsetSolverConstraints =totalNumRows? &m_tmpSolverNonContactConstraintPool[0]:0;

  1182. 

  1183. 			

  1184. 			///setup the btSolverConstraints

  1185. 			int currentRow = 0;

  1186. 

  1187. 			for (i=0;i<numConstraints;i++)

  1188. 			{

  1189. 				const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];

  1190. 				

  1191. 				if (info1.m_numConstraintRows)

  1192. 				{

  1193. 					btAssert(currentRow<totalNumRows);

  1194. 					btTypedConstraint* constraint = constraints[i];

  1195. 					btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];

  1196. 

  1197. 					btRigidBody& rbA = constraint->getRigidBodyA();

  1198. 					btRigidBody& rbB = constraint->getRigidBodyB();

  1199. 

  1200. 					

  1201. 					int j;

  1202. 					for ( j=0;j<info1.m_numConstraintRows;j++)

  1203. 					{

  1204. 						memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));

  1205. 						currentConstraintRow[j].m_lowerLimit = -FLT_MAX;

  1206. 						currentConstraintRow[j].m_upperLimit = FLT_MAX;

  1207. 						currentConstraintRow[j].m_appliedImpulse = 0.f;

  1208. 						currentConstraintRow[j].m_appliedPushImpulse = 0.f;

  1209. 						currentConstraintRow[j].m_solverBodyA = &rbA;

  1210. 						currentConstraintRow[j].m_solverBodyB = &rbB;

  1211. 					}

  1212. 

  1213. 					rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);

  1214. 					rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);

  1215. 					rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);

  1216. 					rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);

  1217. 

  1218. 

  1219. 

  1220. 					btTypedConstraint::btConstraintInfo2 info2;

  1221. 					info2.fps = 1.f/infoGlobal.m_timeStep;

  1222. 					info2.erp = infoGlobal.m_erp;

  1223. 					info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;

  1224. 					info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;

  1225. 					info2.m_J2linearAxis = 0;

  1226. 					info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;

  1227. 					info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this

  1228. 					///the size of btSolverConstraint needs be a multiple of btScalar

  1229. 					btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));

  1230. 					info2.m_constraintError = &currentConstraintRow->m_rhs;

  1231. 					currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;

  1232. 					info2.cfm = &currentConstraintRow->m_cfm;

  1233. 					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;

  1234. 					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;

  1235. 					info2.m_numIterations = infoGlobal.m_numIterations;

  1236. 					constraints[i]->getInfo2(&info2);

  1237. 

  1238. 					

  1239. 					int idA = constraint->getRigidBodyA().getCompanionId();

  1240. 					int idB = constraint->getRigidBodyB().getCompanionId();

  1241. 				

  1242. 

  1243. 					///finalize the constraint setup

  1244. 					for ( j=0;j<info1.m_numConstraintRows;j++)

  1245. 					{

  1246. 						btSolverConstraint& solverConstraint = currentConstraintRow[j];

  1247. 						solverConstraint.m_originalContactPoint = constraint;

  1248. 

  1249. 						solverConstraint.m_companionIdA = idA;

  1250. 						solverConstraint.m_companionIdB = idB;

  1251. 

  1252. 						{

  1253. 							const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;

  1254. 							solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();

  1255. 						}

  1256. 						{

  1257. 							const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;

  1258. 							solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();

  1259. 						}

  1260. 

  1261. 						{

  1262. 							btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();

  1263. 							btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;

  1264. 							btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?

  1265. 							btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;

  1266. 

  1267. 							btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);

  1268. 							sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);

  1269. 							sum += iMJlB.dot(solverConstraint.m_contactNormal);

  1270. 							sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);

  1271. 

  1272. 							solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;

  1273. 						}

  1274. 

  1275. 

  1276. 						///fix rhs

  1277. 						///todo: add force/torque accelerators

  1278. 						{

  1279. 							btScalar rel_vel;

  1280. 							btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());

  1281. 							btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());

  1282. 

  1283. 							rel_vel = vel1Dotn+vel2Dotn;

  1284. 

  1285. 							btScalar restitution = 0.f;

  1286. 							btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2

  1287. 							btScalar	velocityError = restitution - rel_vel;// * damping;

  1288. 							btScalar	penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;

  1289. 							btScalar	velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;

  1290. 							solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;

  1291. 							solverConstraint.m_appliedImpulse = 0.f;

  1292. 

  1293. 						}

  1294. 					}

  1295. 

  1296. 					PfxConstraintPair& pair = jointPairs[actualNumJoints];

  1297. 					

  1298. 					int numConstraintRows= info1.m_numConstraintRows;

  1299. 					pfxSetNumConstraints(pair,numConstraintRows);

  1300. 					

  1301. 

  1302. 

  1303. 					pfxSetRigidBodyIdA(pair,idA);

  1304. 					pfxSetRigidBodyIdB(pair,idB);

  1305. 					//is this needed?

  1306. 					if (idA>=0)

  1307. 						pfxSetMotionMaskA(pair,m_memoryCache->m_mystates[idA].getMotionMask());

  1308. 					if (idB>=0)

  1309. 						pfxSetMotionMaskB(pair,m_memoryCache->m_mystates[idB].getMotionMask());

  1310. 

  1311. 					pfxSetActive(pair,true);

  1312. 					int id = currentConstraintRow-offsetSolverConstraints;

  1313. 					pfxSetContactId(pair,id);

  1314. 					actualNumJoints++;

  1315. 

  1316. 

  1317. 				}

  1318. 				currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;

  1319. 			}

  1320. 		}

  1321. 	}

  1322. 

  1323. 

  1324. 	

  1325. 	float separateBias=0.1;//info.m_erp;//or m_erp2?

  1326. 	float timeStep=infoGlobal.m_timeStep;

  1327. 	int iteration=infoGlobal.m_numIterations;

  1328. 

  1329. 	//create a pair for each constraints, copy over info etc

  1330. 	

  1331. 	

  1332. 

  1333. 

  1334. 	

  1335. 	{

  1336. 		BT_PROFILE("compute num contacts");

  1337. 		int totalContacts =0;

  1338. 

  1339. 		for (int i=0;i<actualNumManifolds;i++)

  1340. 		{

  1341. 			PfxConstraintPair* pair = &m_memoryCache->m_mypairs[i];

  1342. 			totalContacts += pfxGetNumConstraints(*pair);

  1343. 		}

  1344. 		//printf("numManifolds = %d\n",numManifolds);

  1345. 		//printf("totalContacts=%d\n",totalContacts);

  1346. 	}

  1347. 	

  1348. 

  1349. 

  1350. //	printf("actualNumManifolds=%d\n",actualNumManifolds);

  1351. 	{

  1352. 		BT_PROFILE("BPE_customConstraintSolverSequentialNew");

  1353. 		if (numRigidBodies>0 && (actualNumManifolds+actualNumJoints)>0)

  1354. 		{

  1355. //			PFX_PRINTF("num points = %d\n",totalPoints);

  1356. //			PFX_PRINTF("num points PFX = %d\n",total);

  1357. 			

  1358. 			

  1359. 			 

  1360. 			BPE_customConstraintSolverSequentialNew(

  1361. 				actualNumManifolds,

  1362. 				&m_memoryCache->m_mypairs[0],

  1363. 				offsetContactManifolds,

  1364. 				&m_memoryCache->m_mystates[0],numRigidBodies,

  1365. 				&m_memoryCache->m_mysolverbodies[0],

  1366. 				jointPairs,actualNumJoints,

  1367. 				offsetSolverConstraints,

  1368. 				separateBias,timeStep,iteration,

  1369. 				m_solverThreadSupport,m_criticalSection,m_solverIO,m_barrier);

  1370. 		}

  1371. 	}

  1372. 

  1373. 	//copy results back to bodies

  1374. 	{

  1375. 		BT_PROFILE("copy back");

  1376. 		for (int i=0;i<numRigidBodies;i++)

  1377. 		{

  1378. 			btCollisionObject* obj = bodies1[i];

  1379. 			btRigidBody* rb = btRigidBody::upcast(obj);

  1380. 			TrbState& state = m_memoryCache->m_mystates[i];

  1381. 			if (rb && (rb->getInvMass()>0.f))

  1382. 			{

  1383. 				rb->setLinearVelocity(btVector3(state.getLinearVelocity().getX(),state.getLinearVelocity().getY(),state.getLinearVelocity().getZ()));

  1384. 				rb->setAngularVelocity(btVector3(state.getAngularVelocity().getX(),state.getAngularVelocity().getY(),state.getAngularVelocity().getZ()));

  1385. 			}

  1386. 		}

  1387. 	}

  1388. 

  1389. 

  1390. 	return 0.f;

  1391. }