lolengine
/
lol
镜像来自 https://github.com/lolengine/lol
1
0
複製 0
程式碼 問題管理 0 版本發佈 0 Wiki Activity
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.
2677 Commit
5 分支
161 MiB
 
 
 
目錄樹: 4e6e359d2f
分支列表 標籤列表
${ item.name }
Create branch ${ searchTerm }
from '4e6e359d2f'
${ noResults }
lol/src/bullet/Bullet3OpenCL/RigidBody/b3Solver.cpp

1215 line
36 KiB
原始文件 Blame 文件歷史 

  1. /*

  2. Copyright (c) 2012 Advanced Micro Devices, Inc.  

  3. 

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

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

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

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

  8. subject to the following restrictions:

  9. 

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

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

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

  13. */

  14. //Originally written by Takahiro Harada

  15. 

  16. 

  17. #include "b3Solver.h"

  18. 

  19. ///useNewBatchingKernel  is a rewritten kernel using just a single thread of the warp, for experiments

  20. bool useNewBatchingKernel = true;

  21. bool gConvertConstraintOnCpu = false;

  22. 

  23. #define B3_SOLVER_SETUP_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl"

  24. #define B3_SOLVER_SETUP2_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl"

  25. #define B3_SOLVER_CONTACT_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl"

  26. #define B3_SOLVER_FRICTION_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl"

  27. #define B3_BATCHING_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl"

  28. #define B3_BATCHING_NEW_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl"

  29. 

  30. #include "Bullet3Dynamics/shared/b3ConvertConstraint4.h"

  31. 

  32. #include "kernels/solverSetup.h"

  33. #include "kernels/solverSetup2.h"

  34. 

  35. #include "kernels/solveContact.h"

  36. #include "kernels/solveFriction.h"

  37. 

  38. #include "kernels/batchingKernels.h"

  39. #include "kernels/batchingKernelsNew.h"

  40. 

  41. 

  42. #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"

  43. #include "Bullet3Common/b3Vector3.h"

  44. 

  45. struct SolverDebugInfo

  46. {

  47. 	int m_valInt0;

  48. 	int m_valInt1;

  49. 	int m_valInt2;

  50. 	int m_valInt3;

  51. 	

  52. 	int m_valInt4;

  53. 	int m_valInt5;

  54. 	int m_valInt6;

  55. 	int m_valInt7;

  56. 

  57. 	int m_valInt8;

  58. 	int m_valInt9;

  59. 	int m_valInt10;

  60. 	int m_valInt11;

  61. 

  62. 	int	m_valInt12;

  63. 	int	m_valInt13;

  64. 	int	m_valInt14;

  65. 	int	m_valInt15;

  66. 

  67. 

  68. 	float m_val0;

  69. 	float m_val1;

  70. 	float m_val2;

  71. 	float m_val3;

  72. };

  73. 

  74. 

  75. 

  76. 

  77. class SolverDeviceInl

  78. {

  79. public:

  80. 	struct ParallelSolveData

  81. 	{

  82. 		b3OpenCLArray<unsigned int>* m_numConstraints;

  83. 		b3OpenCLArray<unsigned int>* m_offsets;

  84. 	};

  85. };

  86. 

  87. 

  88. 

  89. b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity)

  90. 			:m_context(ctx),

  91. 			m_device(device),

  92. 			m_queue(queue),

  93. 			m_batchSizes(ctx,queue),

  94. 			m_nIterations(4)

  95. {

  96. 	m_sort32 = new b3RadixSort32CL(ctx,device,queue);

  97. 	m_scan = new b3PrefixScanCL(ctx,device,queue,B3_SOLVER_N_CELLS);

  98. 	m_search = new b3BoundSearchCL(ctx,device,queue,B3_SOLVER_N_CELLS);

  99. 

  100. 	const int sortSize = B3NEXTMULTIPLEOF( pairCapacity, 512 );

  101. 

  102. 	m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx,queue,sortSize);

  103. 	m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(ctx,queue);

  104. 

  105. 	m_numConstraints = new b3OpenCLArray<unsigned int>(ctx,queue,B3_SOLVER_N_CELLS );

  106. 	m_numConstraints->resize(B3_SOLVER_N_CELLS);

  107. 

  108. 	m_offsets = new b3OpenCLArray<unsigned int>( ctx,queue,B3_SOLVER_N_CELLS);

  109. 	m_offsets->resize(B3_SOLVER_N_CELLS);

  110. 	const char* additionalMacros = "";

  111. 	const char* srcFileNameForCaching="";

  112. 

  113. 

  114. 

  115. 	cl_int pErrNum;

  116. 	const char* batchKernelSource = batchingKernelsCL;

  117. 	const char* batchKernelNewSource = batchingKernelsNewCL;

  118. 	

  119. 	const char* solverSetupSource = solverSetupCL;

  120. 	const char* solverSetup2Source = solverSetup2CL;

  121. 	const char* solveContactSource = solveContactCL;

  122. 	const char* solveFrictionSource = solveFrictionCL;

  123. 	

  124. 	

  125. 	

  126. 	{

  127. 		

  128. 		cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH);

  129. 		b3Assert(solveContactProg);

  130. 		

  131. 		cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH);

  132. 		b3Assert(solveFrictionProg);

  133. 

  134. 		cl_program solverSetup2Prog= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetup2Source, &pErrNum,additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH);

  135. 		b3Assert(solverSetup2Prog);

  136. 

  137. 		

  138. 		cl_program solverSetupProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetupSource, &pErrNum,additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH);

  139. 		b3Assert(solverSetupProg);

  140. 		

  141. 		

  142. 		m_solveFrictionKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg,additionalMacros );

  143. 		b3Assert(m_solveFrictionKernel);

  144. 

  145. 		m_solveContactKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg,additionalMacros );

  146. 		b3Assert(m_solveContactKernel);

  147. 		

  148. 		m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg,additionalMacros );

  149. 		b3Assert(m_contactToConstraintKernel);

  150. 			

  151. 		m_setSortDataKernel =  b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog,additionalMacros );

  152. 		b3Assert(m_setSortDataKernel);

  153. 				

  154. 		m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog,additionalMacros );

  155. 		b3Assert(m_reorderContactKernel);

  156. 		

  157. 

  158. 		m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog,additionalMacros );

  159. 		b3Assert(m_copyConstraintKernel);

  160. 		

  161. 	}

  162. 

  163. 	{

  164. 		cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelSource, &pErrNum,additionalMacros, B3_BATCHING_PATH);

  165. 		//cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, B3_BATCHING_PATH,true);

  166. 		b3Assert(batchingProg);

  167. 		

  168. 		m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg,additionalMacros );

  169. 		b3Assert(m_batchingKernel);

  170. 	}

  171. 	{

  172. 		cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelNewSource, &pErrNum,additionalMacros, B3_BATCHING_NEW_PATH);

  173. 		b3Assert(batchingNewProg);

  174. 

  175. 		m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros );

  176. 		//m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesBruteForce", &pErrNum, batchingNewProg,additionalMacros );

  177. 		b3Assert(m_batchingKernelNew);

  178. 	}

  179. }

  180. 		

  181. b3Solver::~b3Solver()

  182. {

  183. 	delete m_offsets;

  184. 	delete m_numConstraints;

  185. 	delete m_sortDataBuffer;

  186. 	delete m_contactBuffer2;

  187. 

  188. 	delete m_sort32;

  189. 	delete m_scan;

  190. 	delete m_search;

  191. 

  192. 

  193. 	clReleaseKernel(m_batchingKernel);

  194. 	clReleaseKernel(m_batchingKernelNew);

  195. 	

  196. 	clReleaseKernel( m_solveContactKernel);

  197. 	clReleaseKernel( m_solveFrictionKernel);

  198. 

  199. 	clReleaseKernel( m_contactToConstraintKernel);

  200. 	clReleaseKernel( m_setSortDataKernel);

  201. 	clReleaseKernel( m_reorderContactKernel);

  202. 	clReleaseKernel( m_copyConstraintKernel);

  203. 			

  204. }

  205. 

  206. 

  207.  

  208. 

  209. template<bool JACOBI>

  210. static

  211. __inline

  212. void solveContact(b3GpuConstraint4& cs, 

  213. 	const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,

  214. 	const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 

  215. 	float maxRambdaDt[4], float minRambdaDt[4])

  216. {

  217. 

  218. 	b3Vector3 dLinVelA; dLinVelA.setZero();

  219. 	b3Vector3 dAngVelA; dAngVelA.setZero();

  220. 	b3Vector3 dLinVelB; dLinVelB.setZero();

  221. 	b3Vector3 dAngVelB; dAngVelB.setZero();

  222. 

  223. 	for(int ic=0; ic<4; ic++)

  224. 	{

  225. 		//	dont necessary because this makes change to 0

  226. 		if( cs.m_jacCoeffInv[ic] == 0.f ) continue;

  227. 

  228. 		{

  229. 			b3Vector3 angular0, angular1, linear;

  230. 			b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;

  231. 			b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;

  232. 			setLinearAndAngular( (const b3Vector3 &)cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, &linear, &angular0, &angular1 );

  233. 

  234. 			float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,

  235. 				linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];

  236. 			rambdaDt *= cs.m_jacCoeffInv[ic];

  237. 

  238. 			{

  239. 				float prevSum = cs.m_appliedRambdaDt[ic];

  240. 				float updated = prevSum;

  241. 				updated += rambdaDt;

  242. 				updated = b3Max( updated, minRambdaDt[ic] );

  243. 				updated = b3Min( updated, maxRambdaDt[ic] );

  244. 				rambdaDt = updated - prevSum;

  245. 				cs.m_appliedRambdaDt[ic] = updated;

  246. 			}

  247. 

  248. 			b3Vector3 linImp0 = invMassA*linear*rambdaDt;

  249. 			b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;

  250. 			b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;

  251. 			b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;

  252. #ifdef _WIN32

  253.             b3Assert(_finite(linImp0.getX()));

  254. 			b3Assert(_finite(linImp1.getX()));

  255. #endif

  256. 			if( JACOBI )

  257. 			{

  258. 				dLinVelA += linImp0;

  259. 				dAngVelA += angImp0;

  260. 				dLinVelB += linImp1;

  261. 				dAngVelB += angImp1;

  262. 			}

  263. 			else

  264. 			{

  265. 				linVelA += linImp0;

  266. 				angVelA += angImp0;

  267. 				linVelB += linImp1;

  268. 				angVelB += angImp1;

  269. 			}

  270. 		}

  271. 	}

  272. 

  273. 	if( JACOBI )

  274. 	{

  275. 		linVelA += dLinVelA;

  276. 		angVelA += dAngVelA;

  277. 		linVelB += dLinVelB;

  278. 		angVelB += dAngVelB;

  279. 	}

  280. 

  281. }

  282. 

  283. 

  284. 

  285. 

  286. 

  287. 	static

  288. 	__inline

  289. 	void solveFriction(b3GpuConstraint4& cs, 

  290. 		const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,

  291. 		const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 

  292. 		float maxRambdaDt[4], float minRambdaDt[4])

  293. 	{

  294. 

  295. 		if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;

  296. 		const b3Vector3& center = (const b3Vector3&)cs.m_center;

  297. 

  298. 		b3Vector3 n = -(const b3Vector3&)cs.m_linear;

  299. 

  300. 		b3Vector3 tangent[2];

  301. #if 1		

  302. 		b3PlaneSpace1 (n, tangent[0],tangent[1]);

  303. #else

  304. 		b3Vector3 r = cs.m_worldPos[0]-center;

  305. 		tangent[0] = cross3( n, r );

  306. 		tangent[1] = cross3( tangent[0], n );

  307. 		tangent[0] = normalize3( tangent[0] );

  308. 		tangent[1] = normalize3( tangent[1] );

  309. #endif

  310. 

  311. 		b3Vector3 angular0, angular1, linear;

  312. 		b3Vector3 r0 = center - posA;

  313. 		b3Vector3 r1 = center - posB;

  314. 		for(int i=0; i<2; i++)

  315. 		{

  316. 			setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );

  317. 			float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,

  318. 				linVelA, angVelA, linVelB, angVelB );

  319. 			rambdaDt *= cs.m_fJacCoeffInv[i];

  320. 

  321. 				{

  322. 					float prevSum = cs.m_fAppliedRambdaDt[i];

  323. 					float updated = prevSum;

  324. 					updated += rambdaDt;

  325. 					updated = b3Max( updated, minRambdaDt[i] );

  326. 					updated = b3Min( updated, maxRambdaDt[i] );

  327. 					rambdaDt = updated - prevSum;

  328. 					cs.m_fAppliedRambdaDt[i] = updated;

  329. 				}

  330. 

  331. 			b3Vector3 linImp0 = invMassA*linear*rambdaDt;

  332. 			b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;

  333. 			b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;

  334. 			b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;

  335. #ifdef _WIN32

  336. 			b3Assert(_finite(linImp0.getX()));

  337. 			b3Assert(_finite(linImp1.getX()));

  338. #endif

  339. 			linVelA += linImp0;

  340. 			angVelA += angImp0;

  341. 			linVelB += linImp1;

  342. 			angVelB += angImp1;

  343. 		}

  344. 

  345. 		{	//	angular damping for point constraint

  346. 			b3Vector3 ab = ( posB - posA ).normalized();

  347. 			b3Vector3 ac = ( center - posA ).normalized();

  348. 			if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))

  349. 			{

  350. 				float angNA = b3Dot( n, angVelA );

  351. 				float angNB = b3Dot( n, angVelB );

  352. 

  353. 				angVelA -= (angNA*0.1f)*n;

  354. 				angVelB -= (angNB*0.1f)*n;

  355. 			}

  356. 		}

  357. 

  358. 	}

  359. 

  360. 

  361. struct SolveTask// : public ThreadPool::Task

  362. {

  363. 	SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,  b3AlignedObjectArray<b3InertiaData>& shapes, b3AlignedObjectArray<b3GpuConstraint4>& constraints,

  364. 		int start, int nConstraints,int maxNumBatches,b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx, b3AlignedObjectArray<int>* batchSizes, int cellIndex)

  365. 		: m_bodies( bodies ), m_shapes( shapes ),

  366. 		m_constraints( constraints ),

  367. 		m_batchSizes(batchSizes),

  368. 		m_cellIndex(cellIndex),

  369. 		m_curWgidx(curWgidx),

  370. 		m_start( start ),

  371. 		m_nConstraints( nConstraints ),

  372. 		m_solveFriction( true ),

  373. 		m_maxNumBatches(maxNumBatches)

  374. 	{}

  375. 

  376. 	unsigned short int getType(){ return 0; }

  377. 

  378. 	void run(int tIdx)

  379. 	{

  380. 		int offset = 0;

  381. 		for (int ii=0;ii<B3_MAX_NUM_BATCHES;ii++)

  382. 		{

  383. 			int numInBatch = m_batchSizes->at(m_cellIndex*B3_MAX_NUM_BATCHES+ii);

  384. 			if (!numInBatch)

  385. 				break;

  386. 

  387. 			for (int jj=0;jj<numInBatch;jj++)

  388. 			{

  389. 				int i = m_start + offset+jj;

  390. 				int batchId = m_constraints[i].m_batchIdx;

  391. 				b3Assert(batchId==ii);

  392. 				float frictionCoeff = m_constraints[i].getFrictionCoeff();

  393. 				int aIdx = (int)m_constraints[i].m_bodyA;

  394. 				int bIdx = (int)m_constraints[i].m_bodyB;

  395. 				int localBatch = m_constraints[i].m_batchIdx;

  396. 				b3RigidBodyData& bodyA = m_bodies[aIdx];

  397. 				b3RigidBodyData& bodyB = m_bodies[bIdx];

  398. 

  399. 				if( !m_solveFriction )

  400. 				{

  401. 					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};

  402. 					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

  403. 

  404. 					solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 

  405. 							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,

  406. 						maxRambdaDt, minRambdaDt );

  407. 				}

  408. 				else

  409. 				{

  410. 					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};

  411. 					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

  412. 					float sum = 0;

  413. 					for(int j=0; j<4; j++)

  414. 					{

  415. 						sum +=m_constraints[i].m_appliedRambdaDt[j];

  416. 					}

  417. 					frictionCoeff = 0.7f;

  418. 					for(int j=0; j<4; j++)

  419. 					{

  420. 						maxRambdaDt[j] = frictionCoeff*sum;

  421. 						minRambdaDt[j] = -maxRambdaDt[j];

  422. 					}

  423. 					solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 

  424. 						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,

  425. 						maxRambdaDt, minRambdaDt );

  426. 			

  427. 				}

  428. 			}

  429. 			offset+=numInBatch;

  430. 

  431. 

  432. 		}

  433. /*		for (int bb=0;bb<m_maxNumBatches;bb++)

  434. 		{

  435. 			//for(int ic=m_nConstraints-1; ic>=0; ic--)

  436. 			for(int ic=0; ic<m_nConstraints; ic++)

  437. 			{

  438. 				

  439. 				int i = m_start + ic;

  440. 				if (m_constraints[i].m_batchIdx != bb)

  441. 					continue;

  442. 

  443. 				float frictionCoeff = m_constraints[i].getFrictionCoeff();

  444. 				int aIdx = (int)m_constraints[i].m_bodyA;

  445. 				int bIdx = (int)m_constraints[i].m_bodyB;

  446. 				int localBatch = m_constraints[i].m_batchIdx;

  447. 				b3RigidBodyData& bodyA = m_bodies[aIdx];

  448. 				b3RigidBodyData& bodyB = m_bodies[bIdx];

  449. 

  450. 				if( !m_solveFriction )

  451. 				{

  452. 					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};

  453. 					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

  454. 

  455. 					solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 

  456. 							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,

  457. 						maxRambdaDt, minRambdaDt );

  458. 				}

  459. 				else

  460. 				{

  461. 					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};

  462. 					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};

  463. 					float sum = 0;

  464. 					for(int j=0; j<4; j++)

  465. 					{

  466. 						sum +=m_constraints[i].m_appliedRambdaDt[j];

  467. 					}

  468. 					frictionCoeff = 0.7f;

  469. 					for(int j=0; j<4; j++)

  470. 					{

  471. 						maxRambdaDt[j] = frictionCoeff*sum;

  472. 						minRambdaDt[j] = -maxRambdaDt[j];

  473. 					}

  474. 					solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 

  475. 						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,

  476. 						maxRambdaDt, minRambdaDt );

  477. 			

  478. 				}

  479. 			}

  480. 		}

  481. 		*/

  482. 

  483. 

  484. 		

  485. 	}

  486. 

  487. 	b3AlignedObjectArray<b3RigidBodyData>& m_bodies;

  488. 	b3AlignedObjectArray<b3InertiaData>& m_shapes;

  489. 	b3AlignedObjectArray<b3GpuConstraint4>& m_constraints;

  490. 	b3AlignedObjectArray<int>* m_batchSizes;

  491. 	int m_cellIndex;

  492. 	int m_curWgidx;

  493. 	int m_start;

  494. 	int m_nConstraints;

  495. 	bool m_solveFriction;

  496. 	int m_maxNumBatches;

  497. };

  498. 

  499. 

  500. void b3Solver::solveContactConstraintHost(  b3OpenCLArray<b3RigidBodyData>* bodyBuf, b3OpenCLArray<b3InertiaData>* shapeBuf, 

  501. 			b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches,b3AlignedObjectArray<int>* batchSizes)

  502. {

  503. 

  504. #if 0

  505. 	{	

  506. 		int nSplitX = B3_SOLVER_N_SPLIT_X;

  507. 		int nSplitY = B3_SOLVER_N_SPLIT_Y;

  508. 		int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES;

  509. 		for (int z=0;z<4;z++)

  510. 		{

  511. 			for (int y=0;y<4;y++)

  512. 			{

  513. 				for (int x=0;x<4;x++)

  514. 				{

  515. 					int newIndex = (x+y*nSplitX+z*nSplitX*nSplitY);

  516. 				//	printf("newIndex=%d\n",newIndex);

  517. 

  518. 					int zIdx = newIndex/(nSplitX*nSplitY);

  519. 					int remain = newIndex%(nSplitX*nSplitY);

  520. 					int yIdx = remain/nSplitX;

  521. 					int xIdx = remain%nSplitX;

  522. 				//	printf("newIndex=%d\n",newIndex);

  523. 				}

  524. 			}

  525. 		}

  526. 

  527. 		//for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--)

  528. 		for (int cellBatch=0;cellBatch<B3_SOLVER_N_BATCHES;cellBatch++)

  529. 		{

  530. 			for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++)

  531. 			{

  532. 				int zIdx = (wgIdx/((nSplitX*nSplitY)/4))*2+((cellBatch&4)>>2);

  533. 				int remain= (wgIdx%((nSplitX*nSplitY)/4));

  534. 				int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1);

  535. 				int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1);

  536. 				

  537. 				/*int zIdx = newIndex/(nSplitX*nSplitY);

  538. 				int remain = newIndex%(nSplitX*nSplitY);

  539. 				int yIdx = remain/nSplitX;

  540. 				int xIdx = remain%nSplitX;

  541. 				*/

  542. 				int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY);

  543. 			//	printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch);

  544. 			}

  545. 		}

  546. 	}

  547. #endif

  548. 

  549. 	b3AlignedObjectArray<b3RigidBodyData> bodyNative;

  550. 	bodyBuf->copyToHost(bodyNative);

  551. 	b3AlignedObjectArray<b3InertiaData> shapeNative;

  552. 	shapeBuf->copyToHost(shapeNative);

  553. 	b3AlignedObjectArray<b3GpuConstraint4> constraintNative;

  554. 	constraint->copyToHost(constraintNative);

  555. 

  556. 	b3AlignedObjectArray<unsigned int> numConstraintsHost;

  557. 	m_numConstraints->copyToHost(numConstraintsHost);

  558. 

  559. 	//printf("------------------------\n");

  560. 	b3AlignedObjectArray<unsigned int> offsetsHost;

  561. 	m_offsets->copyToHost(offsetsHost);

  562. 	static int frame=0;

  563. 	bool useBatches=true;

  564. 	if (useBatches)

  565. 	{

  566. 		for(int iter=0; iter<m_nIterations; iter++)

  567. 		{

  568. 			for (int cellBatch=0;cellBatch<B3_SOLVER_N_BATCHES;cellBatch++)

  569. 			{

  570. 				

  571. 				int nSplitX = B3_SOLVER_N_SPLIT_X;

  572. 				int nSplitY = B3_SOLVER_N_SPLIT_Y;

  573. 				int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES;

  574. 				//printf("cell Batch %d\n",cellBatch);

  575. 				b3AlignedObjectArray<int> usedBodies[B3_SOLVER_N_CELLS];

  576. 				for (int i=0;i<B3_SOLVER_N_CELLS;i++)

  577. 				{

  578. 					usedBodies[i].resize(0);

  579. 				}

  580. 

  581. 				

  582. 

  583. 

  584. 				//for (int wgIdx=numWorkgroups-1;wgIdx>=0;wgIdx--)

  585. 				for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++)

  586. 				{

  587. 					int zIdx = (wgIdx/((nSplitX*nSplitY)/4))*2+((cellBatch&4)>>2);

  588. 					int remain= (wgIdx%((nSplitX*nSplitY)/4));

  589. 					int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1);

  590. 					int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1);

  591. 					int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY);

  592. 					

  593. 	

  594. 					if( numConstraintsHost[cellIdx] == 0 ) 

  595. 						continue;

  596. 

  597. 					//printf("wgIdx %d: xIdx=%d, yIdx=%d, zIdx=%d, cellIdx=%d, cell Batch %d\n",wgIdx,xIdx,yIdx,zIdx,cellIdx,cellBatch);

  598. 					//printf("cell %d has %d constraints\n", cellIdx,numConstraintsHost[cellIdx]);

  599. 					if (zIdx)

  600. 					{

  601. 					//printf("?\n");

  602. 					}

  603. 

  604. 					if (iter==0)

  605. 					{

  606. 						//printf("frame=%d, Cell xIdx=%x, yIdx=%d ",frame, xIdx,yIdx);

  607. 						//printf("cellBatch=%d, wgIdx=%d, #constraints in cell=%d\n",cellBatch,wgIdx,numConstraintsHost[cellIdx]);

  608. 					}

  609. 					const int start = offsetsHost[cellIdx];

  610. 					int numConstraintsInCell = numConstraintsHost[cellIdx];

  611. 					const int end = start + numConstraintsInCell;

  612. 

  613. 					SolveTask task( bodyNative, shapeNative, constraintNative, start, numConstraintsInCell ,maxNumBatches,usedBodies,wgIdx,batchSizes,cellIdx);

  614. 					task.m_solveFriction = false;

  615. 					task.run(0);

  616. 				

  617. 				}

  618. 			}

  619. 		}

  620. 

  621. 		for(int iter=0; iter<m_nIterations; iter++)

  622. 		{

  623. 			for (int cellBatch=0;cellBatch<B3_SOLVER_N_BATCHES;cellBatch++)

  624. 			{

  625. 				int nSplitX = B3_SOLVER_N_SPLIT_X;

  626. 				int nSplitY = B3_SOLVER_N_SPLIT_Y;

  627. 				

  628. 

  629. 				int numWorkgroups = B3_SOLVER_N_CELLS/B3_SOLVER_N_BATCHES;

  630. 

  631. 				for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++)

  632. 				{

  633. 					int zIdx = (wgIdx/((nSplitX*nSplitY)/4))*2+((cellBatch&4)>>2);

  634. 					int remain= (wgIdx%((nSplitX*nSplitY)/4));

  635. 					int yIdx = (remain/(nSplitX/2))*2 + ((cellBatch&2)>>1);

  636. 					int xIdx = (remain%(nSplitX/2))*2 + (cellBatch&1);

  637. 					

  638. 					int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY);

  639. 	

  640. 					if( numConstraintsHost[cellIdx] == 0 ) 

  641. 						continue;

  642. 	

  643. 					//printf("yIdx=%d\n",yIdx);

  644. 					

  645. 					const int start = offsetsHost[cellIdx];

  646. 					int numConstraintsInCell = numConstraintsHost[cellIdx];

  647. 					const int end = start + numConstraintsInCell;

  648. 

  649. 					SolveTask task( bodyNative, shapeNative, constraintNative, start, numConstraintsInCell,maxNumBatches, 0,0,batchSizes,cellIdx);

  650. 					task.m_solveFriction = true;

  651. 					task.run(0);

  652. 					

  653. 				}

  654. 			}

  655. 		}

  656. 

  657. 

  658. 	} else

  659. 	{

  660. 		for(int iter=0; iter<m_nIterations; iter++)

  661. 		{

  662. 			SolveTask task( bodyNative, shapeNative, constraintNative, 0, n ,maxNumBatches,0,0,0,0);

  663. 			task.m_solveFriction = false;

  664. 			task.run(0);

  665. 		}

  666. 

  667. 		for(int iter=0; iter<m_nIterations; iter++)

  668. 		{

  669. 			SolveTask task( bodyNative, shapeNative, constraintNative, 0, n ,maxNumBatches,0,0,0,0);

  670. 			task.m_solveFriction = true;

  671. 			task.run(0);

  672. 		}

  673. 	}

  674. 

  675. 	bodyBuf->copyFromHost(bodyNative);

  676. 	shapeBuf->copyFromHost(shapeNative);

  677. 	constraint->copyFromHost(constraintNative);

  678. 	frame++;

  679. 	

  680. }

  681. 

  682. void checkConstraintBatch(const b3OpenCLArray<b3RigidBodyData>* bodyBuf,

  683. 					const b3OpenCLArray<b3InertiaData>* shapeBuf,

  684. 					b3OpenCLArray<b3GpuConstraint4>* constraint, 

  685. 					b3OpenCLArray<unsigned int>* m_numConstraints,

  686. 					b3OpenCLArray<unsigned int>* m_offsets,

  687. 					int batchId

  688. 					)

  689. {

  690. //						b3BufferInfoCL( m_numConstraints->getBufferCL() ), 

  691. //						b3BufferInfoCL( m_offsets->getBufferCL() ) 

  692. 	

  693. 	int cellBatch = batchId;

  694. 	const int nn = B3_SOLVER_N_CELLS;

  695. 	int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES;

  696. 

  697. 	b3AlignedObjectArray<unsigned int> gN;

  698. 	m_numConstraints->copyToHost(gN);

  699. 	b3AlignedObjectArray<unsigned int> gOffsets;

  700. 	m_offsets->copyToHost(gOffsets);

  701. 	int nSplitX = B3_SOLVER_N_SPLIT_X;

  702. 	int nSplitY = B3_SOLVER_N_SPLIT_Y;

  703. 	

  704. 	int bIdx = batchId;

  705. 

  706. 	b3AlignedObjectArray<b3GpuConstraint4> cpuConstraints;

  707. 	constraint->copyToHost(cpuConstraints);

  708. 

  709. 	printf("batch = %d\n", batchId);

  710. 

  711. 	int numWorkgroups = nn/B3_SOLVER_N_BATCHES;

  712. 	b3AlignedObjectArray<int> usedBodies;

  713. 

  714. 

  715. 	for (int wgIdx=0;wgIdx<numWorkgroups;wgIdx++)

  716. 	{

  717. 		printf("wgIdx = %d           ", wgIdx);

  718. 

  719. 		int zIdx = (wgIdx/((nSplitX*nSplitY))/2)*2+((cellBatch&4)>>2);					

  720. 		int remain = wgIdx%((nSplitX*nSplitY));

  721. 		int yIdx = (remain%(nSplitX/2))*2 + ((cellBatch&2)>>1);

  722. 		int xIdx = (remain/(nSplitX/2))*2 + (cellBatch&1);

  723. 

  724. 		

  725. 		int cellIdx = xIdx+yIdx*nSplitX+zIdx*(nSplitX*nSplitY);

  726. 		printf("cellIdx=%d\n",cellIdx);

  727. 		if( gN[cellIdx] == 0 ) 

  728. 			continue;

  729. 

  730. 		const int start = gOffsets[cellIdx];

  731. 		const int end = start + gN[cellIdx];

  732. 

  733. 		for (int c=start;c<end;c++)

  734. 		{

  735. 			b3GpuConstraint4& constraint = cpuConstraints[c];

  736. 			//printf("constraint (%d,%d)\n", constraint.m_bodyA,constraint.m_bodyB);

  737. 			if (usedBodies.findLinearSearch(constraint.m_bodyA)< usedBodies.size())

  738. 			{

  739. 				printf("error?\n");

  740. 			}

  741. 			if (usedBodies.findLinearSearch(constraint.m_bodyB)< usedBodies.size())

  742. 			{

  743. 				printf("error?\n");

  744. 			}

  745. 		}

  746. 

  747. 		for (int c=start;c<end;c++)

  748. 		{

  749. 			b3GpuConstraint4& constraint = cpuConstraints[c];

  750. 			usedBodies.push_back(constraint.m_bodyA);

  751. 			usedBodies.push_back(constraint.m_bodyB);

  752. 		}

  753. 

  754. 	}

  755. }

  756. 

  757. static bool verify=false;

  758. 

  759. void b3Solver::solveContactConstraint(  const b3OpenCLArray<b3RigidBodyData>* bodyBuf, const b3OpenCLArray<b3InertiaData>* shapeBuf, 

  760. 			b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches)

  761. {

  762. 	

  763. 	

  764. 	b3Int4 cdata = b3MakeInt4( n, 0, 0, 0 );

  765. 	{

  766. 		

  767. 		const int nn = B3_SOLVER_N_CELLS;

  768. 

  769. 		cdata.x = 0;

  770. 		cdata.y = maxNumBatches;//250;

  771. 

  772. 

  773. 		int numWorkItems = 64*nn/B3_SOLVER_N_BATCHES;

  774. #ifdef DEBUG_ME

  775. 		SolverDebugInfo* debugInfo = new  SolverDebugInfo[numWorkItems];

  776. 		adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems);

  777. #endif

  778. 

  779. 

  780. 

  781. 		{

  782. 

  783. 			B3_PROFILE("m_batchSolveKernel iterations");

  784. 			for(int iter=0; iter<m_nIterations; iter++)

  785. 			{

  786. 				for(int ib=0; ib<B3_SOLVER_N_BATCHES; ib++)

  787. 				{

  788. 					

  789. 					if (verify)

  790. 					{

  791. 						checkConstraintBatch(bodyBuf,shapeBuf,constraint,m_numConstraints,m_offsets,ib);

  792. 					}

  793. 

  794. #ifdef DEBUG_ME

  795. 					memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems);

  796. 					gpuDebugInfo.write(debugInfo,numWorkItems);

  797. #endif

  798. 

  799. 

  800. 					cdata.z = ib;

  801. 					

  802. 

  803. 				b3LauncherCL launcher( m_queue, m_solveContactKernel ,"m_solveContactKernel");

  804. #if 1

  805.                     

  806. 					b3BufferInfoCL bInfo[] = { 

  807. 

  808. 						b3BufferInfoCL( bodyBuf->getBufferCL() ), 

  809. 						b3BufferInfoCL( shapeBuf->getBufferCL() ), 

  810. 						b3BufferInfoCL( constraint->getBufferCL() ),

  811. 						b3BufferInfoCL( m_numConstraints->getBufferCL() ), 

  812. 						b3BufferInfoCL( m_offsets->getBufferCL() ) 

  813. #ifdef DEBUG_ME

  814. 						,	b3BufferInfoCL(&gpuDebugInfo)

  815. #endif

  816. 						};

  817. 

  818. 					

  819. 

  820.                     launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );

  821. 					//launcher.setConst(  cdata.x );

  822.                     launcher.setConst(  cdata.y );

  823.                     launcher.setConst(  cdata.z );

  824.                     b3Int4 nSplit;

  825. 					nSplit.x = B3_SOLVER_N_SPLIT_X;

  826. 					nSplit.y = B3_SOLVER_N_SPLIT_Y;

  827. 					nSplit.z = B3_SOLVER_N_SPLIT_Z;

  828. 

  829.                     launcher.setConst(  nSplit );

  830.                     launcher.launch1D( numWorkItems, 64 );

  831. 

  832.                     

  833. #else

  834.                     const char* fileName = "m_batchSolveKernel.bin";

  835.                     FILE* f = fopen(fileName,"rb");

  836.                     if (f)

  837.                     {

  838.                         int sizeInBytes=0;

  839.                         if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET))

  840.                         {

  841.                             printf("error, cannot get file size\n");

  842.                             exit(0);

  843.                         }

  844.                         

  845.                         unsigned char* buf = (unsigned char*) malloc(sizeInBytes);

  846.                         fread(buf,sizeInBytes,1,f);

  847.                         int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context);

  848.                         int num = *(int*)&buf[serializedBytes];

  849.                         

  850.                         launcher.launch1D( num);

  851. 

  852.                         //this clFinish is for testing on errors

  853.                         clFinish(m_queue);

  854.                     }

  855. 

  856. #endif

  857. 					

  858. 

  859. #ifdef DEBUG_ME

  860. 					clFinish(m_queue);

  861. 					gpuDebugInfo.read(debugInfo,numWorkItems);

  862. 					clFinish(m_queue);

  863. 					for (int i=0;i<numWorkItems;i++)

  864. 					{

  865. 						if (debugInfo[i].m_valInt2>0)

  866. 						{

  867. 							printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2);

  868. 						}

  869. 

  870. 						if (debugInfo[i].m_valInt3>0)

  871. 						{

  872. 							printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3);

  873. 						}

  874. 					}

  875. #endif //DEBUG_ME

  876. 

  877. 

  878. 				}

  879. 			}

  880. 		

  881. 			clFinish(m_queue);

  882. 

  883. 

  884. 		}

  885. 

  886. 		cdata.x = 1;

  887. 		bool applyFriction=true;

  888. 		if (applyFriction)

  889.     	{

  890. 			B3_PROFILE("m_batchSolveKernel iterations2");

  891. 			for(int iter=0; iter<m_nIterations; iter++)

  892. 			{

  893. 				for(int ib=0; ib<B3_SOLVER_N_BATCHES; ib++)

  894. 				{

  895. 					cdata.z = ib;

  896. 					

  897. 

  898. 					b3BufferInfoCL bInfo[] = { 

  899. 						b3BufferInfoCL( bodyBuf->getBufferCL() ), 

  900. 						b3BufferInfoCL( shapeBuf->getBufferCL() ), 

  901. 						b3BufferInfoCL( constraint->getBufferCL() ),

  902. 						b3BufferInfoCL( m_numConstraints->getBufferCL() ), 

  903. 						b3BufferInfoCL( m_offsets->getBufferCL() )

  904. #ifdef DEBUG_ME

  905. 						,b3BufferInfoCL(&gpuDebugInfo)

  906. #endif //DEBUG_ME

  907. 					};

  908. 					b3LauncherCL launcher( m_queue, m_solveFrictionKernel,"m_solveFrictionKernel" );

  909. 					launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );

  910. 					//launcher.setConst(  cdata.x );

  911.                     launcher.setConst(  cdata.y );

  912.                     launcher.setConst(  cdata.z );

  913.                     b3Int4 nSplit;

  914. 					nSplit.x = B3_SOLVER_N_SPLIT_X;

  915. 					nSplit.y = B3_SOLVER_N_SPLIT_Y;

  916. 					nSplit.z = B3_SOLVER_N_SPLIT_Z;

  917. 

  918.                     launcher.setConst(  nSplit );

  919.                     

  920. 					launcher.launch1D( 64*nn/B3_SOLVER_N_BATCHES, 64 );

  921. 				}

  922. 			}

  923. 			clFinish(m_queue);

  924. 			

  925. 		}

  926. #ifdef DEBUG_ME

  927. 		delete[] debugInfo;

  928. #endif //DEBUG_ME

  929. 	}

  930. 

  931. 	

  932. }

  933. 

  934. void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyData>* bodyBuf, 

  935. 	const b3OpenCLArray<b3InertiaData>* shapeBuf, 

  936. 	b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, 

  937. 	int nContacts, const ConstraintCfg& cfg )

  938. {

  939. 	b3OpenCLArray<b3GpuConstraint4>* constraintNative =0;

  940. 	contactCOut->resize(nContacts);

  941. 	struct CB

  942. 	{

  943. 		int m_nContacts;

  944. 		float m_dt;

  945. 		float m_positionDrift;

  946. 		float m_positionConstraintCoeff;

  947. 	};

  948. 

  949. 	{

  950. 

  951. 		CB cdata;

  952. 		cdata.m_nContacts = nContacts;

  953. 		cdata.m_dt = cfg.m_dt;

  954. 		cdata.m_positionDrift = cfg.m_positionDrift;

  955. 		cdata.m_positionConstraintCoeff = cfg.m_positionConstraintCoeff;

  956. 

  957. 		

  958. 		if (gConvertConstraintOnCpu)

  959. 		{

  960. 			b3AlignedObjectArray<b3RigidBodyData> gBodies;

  961. 		bodyBuf->copyToHost(gBodies);

  962. 

  963. 		b3AlignedObjectArray<b3Contact4> gContact;

  964. 		contactsIn->copyToHost(gContact);

  965. 

  966. 		b3AlignedObjectArray<b3InertiaData> gShapes;

  967. 		shapeBuf->copyToHost(gShapes);

  968. 		

  969. 		b3AlignedObjectArray<b3GpuConstraint4> gConstraintOut;

  970. 		gConstraintOut.resize(nContacts);

  971. 		

  972. 			B3_PROFILE("cpu contactToConstraintKernel");

  973. 			for (int gIdx=0;gIdx<nContacts;gIdx++)

  974. 			{

  975. 				int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);

  976. 				int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);

  977. 

  978. 				b3Float4 posA = gBodies[aIdx].m_pos;

  979. 				b3Float4 linVelA = gBodies[aIdx].m_linVel;

  980. 				b3Float4 angVelA = gBodies[aIdx].m_angVel;

  981. 				float invMassA = gBodies[aIdx].m_invMass;

  982. 				b3Mat3x3 invInertiaA = gShapes[aIdx].m_initInvInertia;

  983. 

  984. 				b3Float4 posB = gBodies[bIdx].m_pos;

  985. 				b3Float4 linVelB = gBodies[bIdx].m_linVel;

  986. 				b3Float4 angVelB = gBodies[bIdx].m_angVel;

  987. 				float invMassB = gBodies[bIdx].m_invMass;

  988. 				b3Mat3x3 invInertiaB = gShapes[bIdx].m_initInvInertia;

  989. 

  990. 				b3ContactConstraint4_t cs;

  991. 

  992.     			setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,

  993. 					&gContact[gIdx], cdata.m_dt, cdata.m_positionDrift, cdata.m_positionConstraintCoeff,

  994. 					&cs );

  995. 		

  996. 				cs.m_batchIdx = gContact[gIdx].m_batchIdx;

  997. 

  998. 				gConstraintOut[gIdx] = (b3GpuConstraint4&)cs;

  999. 			}

  1000. 

  1001. 			contactCOut->copyFromHost(gConstraintOut);

  1002. 

  1003. 		} else

  1004. 		{

  1005. 			B3_PROFILE("gpu m_contactToConstraintKernel");

  1006. 

  1007. 		

  1008. 			b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( shapeBuf->getBufferCL()),

  1009. 				b3BufferInfoCL( contactCOut->getBufferCL() )};

  1010. 			b3LauncherCL launcher( m_queue, m_contactToConstraintKernel,"m_contactToConstraintKernel" );

  1011. 			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );

  1012. 			//launcher.setConst(  cdata );

  1013.         

  1014. 			launcher.setConst(cdata.m_nContacts);

  1015. 			launcher.setConst(cdata.m_dt);

  1016. 			launcher.setConst(cdata.m_positionDrift);

  1017. 			launcher.setConst(cdata.m_positionConstraintCoeff);

  1018.         

  1019. 			launcher.launch1D( nContacts, 64 );	

  1020. 			clFinish(m_queue);

  1021. 

  1022. 		}

  1023. 	}

  1024. 

  1025. 	

  1026. }

  1027. 

  1028. /*

  1029. void b3Solver::sortContacts(  const b3OpenCLArray<b3RigidBodyData>* bodyBuf, 

  1030. 			b3OpenCLArray<b3Contact4>* contactsIn, void* additionalData, 

  1031. 			int nContacts, const b3Solver::ConstraintCfg& cfg )

  1032. {

  1033. 	

  1034. 	

  1035. 

  1036. 	const int sortAlignment = 512; // todo. get this out of sort

  1037. 	if( cfg.m_enableParallelSolve )

  1038. 	{

  1039. 		

  1040. 

  1041. 		int sortSize = NEXTMULTIPLEOF( nContacts, sortAlignment );

  1042. 

  1043. 		b3OpenCLArray<unsigned int>* countsNative = m_numConstraints;//BufferUtils::map<TYPE_CL, false>( data->m_device, &countsHost );

  1044. 		b3OpenCLArray<unsigned int>* offsetsNative = m_offsets;//BufferUtils::map<TYPE_CL, false>( data->m_device, &offsetsHost );

  1045. 

  1046. 		{	//	2. set cell idx

  1047. 			struct CB

  1048. 			{

  1049. 				int m_nContacts;

  1050. 				int m_staticIdx;

  1051. 				float m_scale;

  1052. 				int m_nSplit;

  1053. 			};

  1054. 

  1055. 			b3Assert( sortSize%64 == 0 );

  1056. 			CB cdata;

  1057. 			cdata.m_nContacts = nContacts;

  1058. 			cdata.m_staticIdx = cfg.m_staticIdx;

  1059. 			cdata.m_scale = 1.f/(N_OBJ_PER_SPLIT*cfg.m_averageExtent);

  1060. 			cdata.m_nSplit = B3_SOLVER_N_SPLIT;

  1061. 

  1062. 			

  1063. 			b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) };

  1064. 			b3LauncherCL launcher( m_queue, m_setSortDataKernel );

  1065. 			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );

  1066. 			launcher.setConst(  cdata );

  1067. 			launcher.launch1D( sortSize, 64 );

  1068. 		}

  1069. 

  1070. 		{	//	3. sort by cell idx

  1071. 			int n = B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT;

  1072. 			int sortBit = 32;

  1073. 			//if( n <= 0xffff ) sortBit = 16;

  1074. 			//if( n <= 0xff ) sortBit = 8;

  1075. 			m_sort32->execute(*m_sortDataBuffer,sortSize);

  1076. 		}

  1077. 		{	//	4. find entries

  1078. 			m_search->execute( *m_sortDataBuffer, nContacts, *countsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT, b3BoundSearchCL::COUNT);

  1079. 

  1080. 			m_scan->execute( *countsNative, *offsetsNative, B3_SOLVER_N_SPLIT*B3_SOLVER_N_SPLIT );

  1081. 		}

  1082. 

  1083. 		{	//	5. sort constraints by cellIdx

  1084. 			//	todo. preallocate this

  1085. //			b3Assert( contactsIn->getType() == TYPE_HOST );

  1086. //			b3OpenCLArray<b3Contact4>* out = BufferUtils::map<TYPE_CL, false>( data->m_device, contactsIn );	//	copying contacts to this buffer

  1087. 

  1088. 			{

  1089. 				

  1090. 

  1091. 				b3Int4 cdata; cdata.x = nContacts;

  1092. 				b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( m_contactBuffer->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) };

  1093. 				b3LauncherCL launcher( m_queue, m_reorderContactKernel );

  1094. 				launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );

  1095. 				launcher.setConst(  cdata );

  1096. 				launcher.launch1D( nContacts, 64 );

  1097. 			}

  1098. //			BufferUtils::unmap<true>( out, contactsIn, nContacts );

  1099. 		}

  1100. 	}

  1101. 

  1102. 	

  1103. }

  1104. 

  1105. */

  1106. 

  1107. void	b3Solver::batchContacts(  b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* nNative, b3OpenCLArray<unsigned int>* offsetsNative, int staticIdx )

  1108. {

  1109. 	

  1110. 	int numWorkItems = 64*B3_SOLVER_N_CELLS;

  1111. 	{

  1112. 		B3_PROFILE("batch generation");

  1113. 		

  1114. 		b3Int4 cdata;

  1115. 		cdata.x = nContacts;

  1116. 		cdata.y = 0;

  1117. 		cdata.z = staticIdx;

  1118. 

  1119. 		

  1120. #ifdef BATCH_DEBUG

  1121. 		SolverDebugInfo* debugInfo = new  SolverDebugInfo[numWorkItems];

  1122. 		adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems);

  1123. 		memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems);

  1124. 		gpuDebugInfo.write(debugInfo,numWorkItems);

  1125. #endif

  1126. 

  1127. 		

  1128. 

  1129. 

  1130. 		b3BufferInfoCL bInfo[] = { 

  1131. 			b3BufferInfoCL( contacts->getBufferCL() ), 

  1132. 			b3BufferInfoCL(  m_contactBuffer2->getBufferCL()),

  1133. 			b3BufferInfoCL( nNative->getBufferCL() ), 

  1134. 			b3BufferInfoCL( offsetsNative->getBufferCL() ),

  1135. #ifdef BATCH_DEBUG

  1136. 			,	b3BufferInfoCL(&gpuDebugInfo)

  1137. #endif

  1138. 		};

  1139. 

  1140. 		

  1141. 		

  1142. 

  1143. 		{

  1144. 			m_batchSizes.resize(nNative->size());

  1145. 			B3_PROFILE("batchingKernel");

  1146. 			//b3LauncherCL launcher( m_queue, m_batchingKernel);

  1147. 			cl_kernel k = useNewBatchingKernel ? m_batchingKernelNew : m_batchingKernel;

  1148. 

  1149. 			b3LauncherCL launcher( m_queue, k,"*batchingKernel");

  1150. 			if (!useNewBatchingKernel )

  1151. 			{

  1152. 				launcher.setBuffer( contacts->getBufferCL() );

  1153. 			}

  1154. 			launcher.setBuffer( m_contactBuffer2->getBufferCL() );

  1155. 			launcher.setBuffer( nNative->getBufferCL());

  1156. 			launcher.setBuffer( offsetsNative->getBufferCL());

  1157. 			

  1158. 			launcher.setBuffer(m_batchSizes.getBufferCL());

  1159. 			

  1160. 

  1161. 			//launcher.setConst(  cdata );

  1162.             launcher.setConst(staticIdx);

  1163.             

  1164. 			launcher.launch1D( numWorkItems, 64 );

  1165. 			//clFinish(m_queue);

  1166. 			//b3AlignedObjectArray<int> batchSizesCPU;

  1167. 			//m_batchSizes.copyToHost(batchSizesCPU);

  1168. 			//printf(".\n");

  1169. 		}

  1170. 

  1171. #ifdef BATCH_DEBUG

  1172. 	aaaa

  1173. 		b3Contact4* hostContacts = new b3Contact4[nContacts];

  1174. 		m_contactBuffer->read(hostContacts,nContacts);

  1175. 		clFinish(m_queue);

  1176. 

  1177. 		gpuDebugInfo.read(debugInfo,numWorkItems);

  1178. 		clFinish(m_queue);

  1179. 

  1180. 		for (int i=0;i<numWorkItems;i++)

  1181. 		{

  1182. 			if (debugInfo[i].m_valInt1>0)

  1183. 			{

  1184. 				printf("catch\n");

  1185. 			}

  1186. 			if (debugInfo[i].m_valInt2>0)

  1187. 			{

  1188. 				printf("catch22\n");

  1189. 			}

  1190. 

  1191. 			if (debugInfo[i].m_valInt3>0)

  1192. 			{

  1193. 				printf("catch666\n");

  1194. 			}

  1195. 

  1196. 			if (debugInfo[i].m_valInt4>0)

  1197. 			{

  1198. 				printf("catch777\n");

  1199. 			}

  1200. 		}

  1201. 		delete[] debugInfo;

  1202. #endif //BATCH_DEBUG

  1203. 

  1204. 	}

  1205. 

  1206. //	copy buffer to buffer

  1207. 	//b3Assert(m_contactBuffer->size()==nContacts);

  1208. 	//contacts->copyFromOpenCLArray( *m_contactBuffer);

  1209. 	//clFinish(m_queue);//needed?

  1210. 	

  1211. 	

  1212. 	

  1213. }