3 Copyright (c) 2013 Advanced Micro Devices, Inc.
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:
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.
15 //Originally written by Erwin Coumans
17 bool useGpuInitSolverBodies = true;
18 bool useGpuInfo1 = true;
19 bool useGpuInfo2 = true;
20 bool useGpuSolveJointConstraintRows = true;
21 bool useGpuWriteBackVelocities = true;
22 bool gpuBreakConstraints = true;
24 #include "b3GpuPgsConstraintSolver.h"
26 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
28 #include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h"
30 #include "Bullet3Common/b3AlignedObjectArray.h"
31 #include <string.h> //for memset
32 #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
33 #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
34 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
36 #include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h"
38 #include "Bullet3OpenCL/RigidBody/kernels/jointSolver.h" //solveConstraintRowsCL
39 #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
41 #define B3_JOINT_SOLVER_PATH "src/Bullet3OpenCL/RigidBody/kernels/jointSolver.cl"
43 struct b3GpuPgsJacobiSolverInternalData
46 cl_device_id m_device;
47 cl_command_queue m_queue;
49 b3PrefixScanCL* m_prefixScan;
51 cl_kernel m_solveJointConstraintRowsKernels;
52 cl_kernel m_initSolverBodiesKernel;
53 cl_kernel m_getInfo1Kernel;
54 cl_kernel m_initBatchConstraintsKernel;
55 cl_kernel m_getInfo2Kernel;
56 cl_kernel m_writeBackVelocitiesKernel;
57 cl_kernel m_breakViolatedConstraintsKernel;
59 b3OpenCLArray<unsigned int>* m_gpuConstraintRowOffsets;
61 b3OpenCLArray<b3GpuSolverBody>* m_gpuSolverBodies;
62 b3OpenCLArray<b3BatchConstraint>* m_gpuBatchConstraints;
63 b3OpenCLArray<b3GpuSolverConstraint>* m_gpuConstraintRows;
64 b3OpenCLArray<unsigned int>* m_gpuConstraintInfo1;
66 // b3AlignedObjectArray<b3GpuSolverBody> m_cpuSolverBodies;
67 b3AlignedObjectArray<b3BatchConstraint> m_cpuBatchConstraints;
68 b3AlignedObjectArray<b3GpuSolverConstraint> m_cpuConstraintRows;
69 b3AlignedObjectArray<unsigned int> m_cpuConstraintInfo1;
70 b3AlignedObjectArray<unsigned int> m_cpuConstraintRowOffsets;
72 b3AlignedObjectArray<b3RigidBodyData> m_cpuBodies;
73 b3AlignedObjectArray<b3InertiaData> m_cpuInertias;
75 b3AlignedObjectArray<b3GpuGenericConstraint> m_cpuConstraints;
77 b3AlignedObjectArray<int> m_batchSizes;
81 static b3Transform getWorldTransform(b3RigidBodyData* rb)
84 newTrans.setOrigin(rb->m_pos);
85 newTrans.setRotation(rb->m_quat);
89 static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia)
91 return inertia->m_invInertiaWorld;
96 static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb)
101 static const b3Vector3& getAngularVelocity(b3RigidBodyData* rb)
106 b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& rel_pos)
108 //we also calculate lin/ang velocity for kinematic objects
109 return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos);
112 b3GpuPgsConstraintSolver::b3GpuPgsConstraintSolver(cl_context ctx, cl_device_id device, cl_command_queue queue, bool usePgs)
115 m_gpuData = new b3GpuPgsJacobiSolverInternalData();
116 m_gpuData->m_context = ctx;
117 m_gpuData->m_device = device;
118 m_gpuData->m_queue = queue;
120 m_gpuData->m_prefixScan = new b3PrefixScanCL(ctx, device, queue);
122 m_gpuData->m_gpuConstraintRowOffsets = new b3OpenCLArray<unsigned int>(m_gpuData->m_context, m_gpuData->m_queue);
124 m_gpuData->m_gpuSolverBodies = new b3OpenCLArray<b3GpuSolverBody>(m_gpuData->m_context, m_gpuData->m_queue);
125 m_gpuData->m_gpuBatchConstraints = new b3OpenCLArray<b3BatchConstraint>(m_gpuData->m_context, m_gpuData->m_queue);
126 m_gpuData->m_gpuConstraintRows = new b3OpenCLArray<b3GpuSolverConstraint>(m_gpuData->m_context, m_gpuData->m_queue);
127 m_gpuData->m_gpuConstraintInfo1 = new b3OpenCLArray<unsigned int>(m_gpuData->m_context, m_gpuData->m_queue);
131 cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, &errNum, "", B3_JOINT_SOLVER_PATH);
132 //cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_gpuData->m_context,m_gpuData->m_device,0,&errNum,"",B3_JOINT_SOLVER_PATH,true);
133 b3Assert(errNum == CL_SUCCESS);
134 m_gpuData->m_solveJointConstraintRowsKernels = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "solveJointConstraintRows", &errNum, prog);
135 b3Assert(errNum == CL_SUCCESS);
136 m_gpuData->m_initSolverBodiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "initSolverBodies", &errNum, prog);
137 b3Assert(errNum == CL_SUCCESS);
138 m_gpuData->m_getInfo1Kernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "getInfo1Kernel", &errNum, prog);
139 b3Assert(errNum == CL_SUCCESS);
140 m_gpuData->m_initBatchConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "initBatchConstraintsKernel", &errNum, prog);
141 b3Assert(errNum == CL_SUCCESS);
142 m_gpuData->m_getInfo2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "getInfo2Kernel", &errNum, prog);
143 b3Assert(errNum == CL_SUCCESS);
144 m_gpuData->m_writeBackVelocitiesKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "writeBackVelocitiesKernel", &errNum, prog);
145 b3Assert(errNum == CL_SUCCESS);
146 m_gpuData->m_breakViolatedConstraintsKernel = b3OpenCLUtils::compileCLKernelFromString(m_gpuData->m_context, m_gpuData->m_device, solveConstraintRowsCL, "breakViolatedConstraintsKernel", &errNum, prog);
147 b3Assert(errNum == CL_SUCCESS);
149 clReleaseProgram(prog);
153 b3GpuPgsConstraintSolver::~b3GpuPgsConstraintSolver()
155 clReleaseKernel(m_gpuData->m_solveJointConstraintRowsKernels);
156 clReleaseKernel(m_gpuData->m_initSolverBodiesKernel);
157 clReleaseKernel(m_gpuData->m_getInfo1Kernel);
158 clReleaseKernel(m_gpuData->m_initBatchConstraintsKernel);
159 clReleaseKernel(m_gpuData->m_getInfo2Kernel);
160 clReleaseKernel(m_gpuData->m_writeBackVelocitiesKernel);
161 clReleaseKernel(m_gpuData->m_breakViolatedConstraintsKernel);
163 delete m_gpuData->m_prefixScan;
164 delete m_gpuData->m_gpuConstraintRowOffsets;
165 delete m_gpuData->m_gpuSolverBodies;
166 delete m_gpuData->m_gpuBatchConstraints;
167 delete m_gpuData->m_gpuConstraintRows;
168 delete m_gpuData->m_gpuConstraintInfo1;
173 struct b3BatchConstraint
175 int m_bodyAPtrAndSignBit;
176 int m_bodyBPtrAndSignBit;
177 int m_originalConstraintIndex;
181 static b3AlignedObjectArray<b3BatchConstraint> batchConstraints;
183 void b3GpuPgsConstraintSolver::recomputeBatches()
185 m_gpuData->m_batchSizes.clear();
188 b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlySetup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
190 B3_PROFILE("GPU solveGroupCacheFriendlySetup");
191 batchConstraints.resize(numConstraints);
192 m_gpuData->m_gpuBatchConstraints->resize(numConstraints);
194 m_maxOverrideNumSolverIterations = 0;
196 /* m_gpuData->m_gpuBodies->resize(numBodies);
197 m_gpuData->m_gpuBodies->copyFromHostPointer(bodies,numBodies);
199 b3OpenCLArray<b3InertiaData> gpuInertias(m_gpuData->m_context,m_gpuData->m_queue);
200 gpuInertias.resize(numBodies);
201 gpuInertias.copyFromHostPointer(inertias,numBodies);
204 m_gpuData->m_gpuSolverBodies->resize(numBodies);
206 m_tmpSolverBodyPool.resize(numBodies);
208 if (useGpuInitSolverBodies)
210 B3_PROFILE("m_initSolverBodiesKernel");
212 b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initSolverBodiesKernel, "m_initSolverBodiesKernel");
213 launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
214 launcher.setBuffer(gpuBodies->getBufferCL());
215 launcher.setConst(numBodies);
216 launcher.launch1D(numBodies);
217 clFinish(m_gpuData->m_queue);
219 // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
223 gpuBodies->copyToHost(m_gpuData->m_cpuBodies);
224 for (int i = 0; i < numBodies; i++)
226 b3RigidBodyData& body = m_gpuData->m_cpuBodies[i];
227 b3GpuSolverBody& solverBody = m_tmpSolverBodyPool[i];
228 initSolverBody(i, &solverBody, &body);
229 solverBody.m_originalBodyIndex = i;
231 m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool);
235 // int totalBodies = 0;
236 int totalNumRows = 0;
237 //b3RigidBody* rb0=0,*rb1=0;
243 m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
245 // b3OpenCLArray<b3GpuGenericConstraint> gpuConstraints(m_gpuData->m_context,m_gpuData->m_queue);
249 B3_PROFILE("info1 and init batchConstraint");
251 m_gpuData->m_gpuConstraintInfo1->resize(numConstraints);
255 B3_PROFILE("getInfo1Kernel");
257 b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo1Kernel, "m_getInfo1Kernel");
258 launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
259 launcher.setBuffer(gpuConstraints->getBufferCL());
260 launcher.setConst(numConstraints);
261 launcher.launch1D(numConstraints);
262 clFinish(m_gpuData->m_queue);
265 if (m_gpuData->m_batchSizes.size() == 0)
267 B3_PROFILE("initBatchConstraintsKernel");
269 m_gpuData->m_gpuConstraintRowOffsets->resize(numConstraints);
270 unsigned int total = 0;
271 m_gpuData->m_prefixScan->execute(*m_gpuData->m_gpuConstraintInfo1, *m_gpuData->m_gpuConstraintRowOffsets, numConstraints, &total);
272 unsigned int lastElem = m_gpuData->m_gpuConstraintInfo1->at(numConstraints - 1);
273 totalNumRows = total + lastElem;
276 B3_PROFILE("init batch constraints");
277 b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_initBatchConstraintsKernel, "m_initBatchConstraintsKernel");
278 launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
279 launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
280 launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
281 launcher.setBuffer(gpuConstraints->getBufferCL());
282 launcher.setBuffer(gpuBodies->getBufferCL());
283 launcher.setConst(numConstraints);
284 launcher.launch1D(numConstraints);
285 clFinish(m_gpuData->m_queue);
287 //assume the batching happens on CPU, so copy the data
288 m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
294 gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
295 //calculate the total number of contraint rows
296 for (int i = 0; i < numConstraints; i++)
298 unsigned int& info1 = m_tmpConstraintSizesPool[i];
299 // unsigned int info1;
300 if (m_gpuData->m_cpuConstraints[i].isEnabled())
302 m_gpuData->m_cpuConstraints[i].getInfo1(&info1, &m_gpuData->m_cpuBodies[0]);
309 totalNumRows += info1;
312 m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
313 m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool);
315 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
316 m_gpuData->m_gpuConstraintRows->resize(totalNumRows);
318 // b3GpuConstraintArray verify;
323 B3_PROFILE("getInfo2Kernel");
324 b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_getInfo2Kernel, "m_getInfo2Kernel");
325 launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
326 launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
327 launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
328 launcher.setBuffer(gpuConstraints->getBufferCL());
329 launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
330 launcher.setBuffer(gpuBodies->getBufferCL());
331 launcher.setBuffer(gpuInertias->getBufferCL());
332 launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
333 launcher.setConst(infoGlobal.m_timeStep);
334 launcher.setConst(infoGlobal.m_erp);
335 launcher.setConst(infoGlobal.m_globalCfm);
336 launcher.setConst(infoGlobal.m_damping);
337 launcher.setConst(infoGlobal.m_numIterations);
338 launcher.setConst(numConstraints);
339 launcher.launch1D(numConstraints);
340 clFinish(m_gpuData->m_queue);
342 if (m_gpuData->m_batchSizes.size() == 0)
343 m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
344 //m_gpuData->m_gpuConstraintRows->copyToHost(verify);
345 //m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool);
350 gpuInertias->copyToHost(m_gpuData->m_cpuInertias);
352 ///setup the b3SolverConstraints
354 for (int i = 0; i < numConstraints; i++)
356 const int& info1 = m_tmpConstraintSizesPool[i];
360 int constraintIndex = batchConstraints[i].m_originalConstraintIndex;
361 int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[constraintIndex];
363 b3GpuSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[constraintRowOffset];
364 b3GpuGenericConstraint& constraint = m_gpuData->m_cpuConstraints[i];
366 b3RigidBodyData& rbA = m_gpuData->m_cpuBodies[constraint.getRigidBodyA()];
367 //b3RigidBody& rbA = constraint.getRigidBodyA();
368 // b3RigidBody& rbB = constraint.getRigidBodyB();
369 b3RigidBodyData& rbB = m_gpuData->m_cpuBodies[constraint.getRigidBodyB()];
371 int solverBodyIdA = constraint.getRigidBodyA(); //getOrInitSolverBody(constraint.getRigidBodyA(),bodies,inertias);
372 int solverBodyIdB = constraint.getRigidBodyB(); //getOrInitSolverBody(constraint.getRigidBodyB(),bodies,inertias);
374 b3GpuSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
375 b3GpuSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
379 batchConstraints[i].m_bodyAPtrAndSignBit = solverBodyIdA;
385 batchConstraints[i].m_bodyAPtrAndSignBit = -solverBodyIdA;
390 batchConstraints[i].m_bodyBPtrAndSignBit = solverBodyIdB;
396 batchConstraints[i].m_bodyBPtrAndSignBit = -solverBodyIdB;
399 int overrideNumSolverIterations = 0; //constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
400 if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
401 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
404 for (j = 0; j < info1; j++)
406 memset(¤tConstraintRow[j], 0, sizeof(b3GpuSolverConstraint));
407 currentConstraintRow[j].m_angularComponentA.setValue(0, 0, 0);
408 currentConstraintRow[j].m_angularComponentB.setValue(0, 0, 0);
409 currentConstraintRow[j].m_appliedImpulse = 0.f;
410 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
411 currentConstraintRow[j].m_cfm = 0.f;
412 currentConstraintRow[j].m_contactNormal.setValue(0, 0, 0);
413 currentConstraintRow[j].m_friction = 0.f;
414 currentConstraintRow[j].m_frictionIndex = 0;
415 currentConstraintRow[j].m_jacDiagABInv = 0.f;
416 currentConstraintRow[j].m_lowerLimit = 0.f;
417 currentConstraintRow[j].m_upperLimit = 0.f;
419 currentConstraintRow[j].m_originalContactPoint = 0;
420 currentConstraintRow[j].m_overrideNumSolverIterations = 0;
421 currentConstraintRow[j].m_relpos1CrossNormal.setValue(0, 0, 0);
422 currentConstraintRow[j].m_relpos2CrossNormal.setValue(0, 0, 0);
423 currentConstraintRow[j].m_rhs = 0.f;
424 currentConstraintRow[j].m_rhsPenetration = 0.f;
425 currentConstraintRow[j].m_solverBodyIdA = 0;
426 currentConstraintRow[j].m_solverBodyIdB = 0;
428 currentConstraintRow[j].m_lowerLimit = -B3_INFINITY;
429 currentConstraintRow[j].m_upperLimit = B3_INFINITY;
430 currentConstraintRow[j].m_appliedImpulse = 0.f;
431 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
432 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
433 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
434 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
437 bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
438 bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
439 bodyAPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
440 bodyAPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
441 bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f, 0.f, 0.f);
442 bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f, 0.f, 0.f);
443 bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
444 bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
446 b3GpuConstraintInfo2 info2;
447 info2.fps = 1.f / infoGlobal.m_timeStep;
448 info2.erp = infoGlobal.m_erp;
449 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
450 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
451 info2.m_J2linearAxis = 0;
452 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
453 info2.rowskip = sizeof(b3GpuSolverConstraint) / sizeof(b3Scalar); //check this
454 ///the size of b3GpuSolverConstraint needs be a multiple of b3Scalar
455 b3Assert(info2.rowskip * sizeof(b3Scalar) == sizeof(b3GpuSolverConstraint));
456 info2.m_constraintError = ¤tConstraintRow->m_rhs;
457 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
458 info2.m_damping = infoGlobal.m_damping;
459 info2.cfm = ¤tConstraintRow->m_cfm;
460 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
461 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
462 info2.m_numIterations = infoGlobal.m_numIterations;
463 m_gpuData->m_cpuConstraints[i].getInfo2(&info2, &m_gpuData->m_cpuBodies[0]);
465 ///finalize the constraint setup
466 for (j = 0; j < info1; j++)
468 b3GpuSolverConstraint& solverConstraint = currentConstraintRow[j];
470 if (solverConstraint.m_upperLimit >= m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold())
472 solverConstraint.m_upperLimit = m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold();
475 if (solverConstraint.m_lowerLimit <= -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold())
477 solverConstraint.m_lowerLimit = -m_gpuData->m_cpuConstraints[i].getBreakingImpulseThreshold();
480 // solverConstraint.m_originalContactPoint = constraint;
482 b3Matrix3x3& invInertiaWorldA = m_gpuData->m_cpuInertias[constraint.getRigidBodyA()].m_invInertiaWorld;
484 //b3Vector3 angularFactorA(1,1,1);
485 const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
486 solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1; //*angularFactorA;
489 b3Matrix3x3& invInertiaWorldB = m_gpuData->m_cpuInertias[constraint.getRigidBodyB()].m_invInertiaWorld;
491 const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
492 solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2; //*constraint.getRigidBodyB().getAngularFactor();
496 //it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal
497 //because it gets multiplied iMJlB
498 b3Vector3 iMJlA = solverConstraint.m_contactNormal * rbA.m_invMass;
499 b3Vector3 iMJaA = invInertiaWorldA * solverConstraint.m_relpos1CrossNormal;
500 b3Vector3 iMJlB = solverConstraint.m_contactNormal * rbB.m_invMass; //sign of normal?
501 b3Vector3 iMJaB = invInertiaWorldB * solverConstraint.m_relpos2CrossNormal;
503 b3Scalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
504 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
505 sum += iMJlB.dot(solverConstraint.m_contactNormal);
506 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
507 b3Scalar fsum = b3Fabs(sum);
508 b3Assert(fsum > B3_EPSILON);
509 solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f;
513 ///todo: add force/torque accelerators
516 b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.m_linVel) + solverConstraint.m_relpos1CrossNormal.dot(rbA.m_angVel);
517 b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.m_linVel) + solverConstraint.m_relpos2CrossNormal.dot(rbB.m_angVel);
519 rel_vel = vel1Dotn + vel2Dotn;
521 b3Scalar restitution = 0.f;
522 b3Scalar positionalError = solverConstraint.m_rhs; //already filled in by getConstraintInfo2
523 b3Scalar velocityError = restitution - rel_vel * info2.m_damping;
524 b3Scalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
525 b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
526 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
527 solverConstraint.m_appliedImpulse = 0.f;
533 m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool);
534 m_gpuData->m_gpuConstraintInfo1->copyFromHost(m_tmpConstraintSizesPool);
536 if (m_gpuData->m_batchSizes.size() == 0)
537 m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
539 m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
541 m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool);
546 #ifdef B3_SUPPORT_CONTACT_CONSTRAINTS
550 for (i = 0; i < numManifolds; i++)
552 b3Contact4& manifold = manifoldPtr[i];
553 convertContact(bodies, inertias, &manifold, infoGlobal);
556 #endif //B3_SUPPORT_CONTACT_CONSTRAINTS
559 // b3ContactSolverInfo info = infoGlobal;
561 // int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
562 // int numConstraintPool = m_tmpSolverContactConstraintPool.size();
563 // int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
568 ///a straight copy from GPU/OpenCL kernel, for debugging
569 __inline void internalApplyImpulse(b3GpuSolverBody* body, const b3Vector3& linearComponent, const b3Vector3& angularComponent, float impulseMagnitude)
571 body->m_deltaLinearVelocity += linearComponent * impulseMagnitude * body->m_linearFactor;
572 body->m_deltaAngularVelocity += angularComponent * (impulseMagnitude * body->m_angularFactor);
575 void resolveSingleConstraintRowGeneric2(b3GpuSolverBody* body1, b3GpuSolverBody* body2, b3GpuSolverConstraint* c)
577 float deltaImpulse = c->m_rhs - b3Scalar(c->m_appliedImpulse) * c->m_cfm;
578 float deltaVel1Dotn = b3Dot(c->m_contactNormal, body1->m_deltaLinearVelocity) + b3Dot(c->m_relpos1CrossNormal, body1->m_deltaAngularVelocity);
579 float deltaVel2Dotn = -b3Dot(c->m_contactNormal, body2->m_deltaLinearVelocity) + b3Dot(c->m_relpos2CrossNormal, body2->m_deltaAngularVelocity);
581 deltaImpulse -= deltaVel1Dotn * c->m_jacDiagABInv;
582 deltaImpulse -= deltaVel2Dotn * c->m_jacDiagABInv;
584 float sum = b3Scalar(c->m_appliedImpulse) + deltaImpulse;
585 if (sum < c->m_lowerLimit)
587 deltaImpulse = c->m_lowerLimit - b3Scalar(c->m_appliedImpulse);
588 c->m_appliedImpulse = c->m_lowerLimit;
590 else if (sum > c->m_upperLimit)
592 deltaImpulse = c->m_upperLimit - b3Scalar(c->m_appliedImpulse);
593 c->m_appliedImpulse = c->m_upperLimit;
597 c->m_appliedImpulse = sum;
600 internalApplyImpulse(body1, c->m_contactNormal * body1->m_invMass, c->m_angularComponentA, deltaImpulse);
601 internalApplyImpulse(body2, -c->m_contactNormal * body2->m_invMass, c->m_angularComponentB, deltaImpulse);
604 void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb)
606 solverBody->m_deltaLinearVelocity.setValue(0.f, 0.f, 0.f);
607 solverBody->m_deltaAngularVelocity.setValue(0.f, 0.f, 0.f);
608 solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
609 solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
612 // solverBody->m_worldTransform = getWorldTransform(rb);
613 solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass, rb->m_invMass, rb->m_invMass));
614 solverBody->m_originalBodyIndex = bodyIndex;
615 solverBody->m_angularFactor = b3MakeVector3(1, 1, 1);
616 solverBody->m_linearFactor = b3MakeVector3(1, 1, 1);
617 solverBody->m_linearVelocity = getLinearVelocity(rb);
618 solverBody->m_angularVelocity = getAngularVelocity(rb);
621 void b3GpuPgsConstraintSolver::averageVelocities()
625 b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints1, int numConstraints, const b3ContactSolverInfo& infoGlobal)
627 //only create the batches once.
628 //@todo: incrementally update batches when constraints are added/activated and/or removed/deactivated
629 B3_PROFILE("GpuSolveGroupCacheFriendlyIterations");
631 bool createBatches = m_gpuData->m_batchSizes.size() == 0;
635 m_gpuData->m_batchSizes.resize(0);
638 m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
640 B3_PROFILE("batch joints");
641 b3Assert(batchConstraints.size() == numConstraints);
642 int simdWidth = numConstraints + 1;
643 int numBodies = m_tmpSolverBodyPool.size();
644 sortConstraintByBatch3(&batchConstraints[0], numConstraints, simdWidth, m_staticIdx, numBodies);
646 m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
651 /*b3AlignedObjectArray<b3BatchConstraint> cpuCheckBatches;
652 m_gpuData->m_gpuBatchConstraints->copyToHost(cpuCheckBatches);
653 b3Assert(cpuCheckBatches.size()==batchConstraints.size());
656 //>copyFromHost(batchConstraints);
658 int maxIterations = infoGlobal.m_numIterations;
660 bool useBatching = true;
664 if (!useGpuSolveJointConstraintRows)
666 B3_PROFILE("copy to host");
667 m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
668 m_gpuData->m_gpuBatchConstraints->copyToHost(batchConstraints);
669 m_gpuData->m_gpuConstraintRows->copyToHost(m_tmpSolverNonContactConstraintPool);
670 m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1);
671 m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets);
672 gpuConstraints1->copyToHost(m_gpuData->m_cpuConstraints);
675 for (int iteration = 0; iteration < maxIterations; iteration++)
678 int constraintOffset = 0;
679 int numBatches = m_gpuData->m_batchSizes.size();
680 for (int bb = 0; bb < numBatches; bb++)
682 int numConstraintsInBatch = m_gpuData->m_batchSizes[bb];
684 if (useGpuSolveJointConstraintRows)
686 B3_PROFILE("solveJointConstraintRowsKernels");
689 __kernel void solveJointConstraintRows(__global b3GpuSolverBody* solverBodies,
690 __global b3BatchConstraint* batchConstraints,
691 __global b3SolverConstraint* rows,
692 __global unsigned int* numConstraintRowsInfo1,
693 __global unsigned int* rowOffsets,
694 __global b3GpuGenericConstraint* constraints,
696 int numConstraintsInBatch*/
698 b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_solveJointConstraintRowsKernels, "m_solveJointConstraintRowsKernels");
699 launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
700 launcher.setBuffer(m_gpuData->m_gpuBatchConstraints->getBufferCL());
701 launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
702 launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
703 launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
704 launcher.setBuffer(gpuConstraints1->getBufferCL()); //to detect disabled constraints
705 launcher.setConst(batchOffset);
706 launcher.setConst(numConstraintsInBatch);
708 launcher.launch1D(numConstraintsInBatch);
712 for (int b = 0; b < numConstraintsInBatch; b++)
714 const b3BatchConstraint& c = batchConstraints[batchOffset + b];
715 /*printf("-----------\n");
716 printf("bb=%d\n",bb);
717 printf("c.batchId = %d\n", c.m_batchId);
719 b3Assert(c.m_batchId == bb);
720 b3GpuGenericConstraint* constraint = &m_gpuData->m_cpuConstraints[c.m_originalConstraintIndex];
721 if (constraint->m_flags & B3_CONSTRAINT_FLAG_ENABLED)
723 int numConstraintRows = m_gpuData->m_cpuConstraintInfo1[c.m_originalConstraintIndex];
724 int constraintOffset = m_gpuData->m_cpuConstraintRowOffsets[c.m_originalConstraintIndex];
726 for (int jj = 0; jj < numConstraintRows; jj++)
729 b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[constraintOffset + jj];
730 //resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
731 resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA], &m_tmpSolverBodyPool[constraint.m_solverBodyIdB], &constraint);
736 batchOffset += numConstraintsInBatch;
737 constraintOffset += numConstraintsInBatch;
739 } //for (int iteration...
741 if (!useGpuSolveJointConstraintRows)
744 B3_PROFILE("copy from host");
745 m_gpuData->m_gpuSolverBodies->copyFromHost(m_tmpSolverBodyPool);
746 m_gpuData->m_gpuBatchConstraints->copyFromHost(batchConstraints);
747 m_gpuData->m_gpuConstraintRows->copyFromHost(m_tmpSolverNonContactConstraintPool);
750 //B3_PROFILE("copy to host");
751 //m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
753 //int sz = sizeof(b3GpuSolverBody);
754 //printf("cpu sizeof(b3GpuSolverBody)=%d\n",sz);
758 for (int iteration = 0; iteration < maxIterations; iteration++)
760 int numJoints = m_tmpSolverNonContactConstraintPool.size();
761 for (int j = 0; j < numJoints; j++)
763 b3GpuSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
764 resolveSingleConstraintRowGeneric2(&m_tmpSolverBodyPool[constraint.m_solverBodyIdA], &m_tmpSolverBodyPool[constraint.m_solverBodyIdB], &constraint);
774 clFinish(m_gpuData->m_queue);
778 static b3AlignedObjectArray<int> bodyUsed;
779 static b3AlignedObjectArray<int> curUsed;
781 inline int b3GpuPgsConstraintSolver::sortConstraintByBatch3(b3BatchConstraint* cs, int numConstraints, int simdWidth, int staticIdx, int numBodies)
783 //int sz = sizeof(b3BatchConstraint);
785 B3_PROFILE("sortConstraintByBatch3");
787 static int maxSwaps = 0;
790 curUsed.resize(2 * simdWidth);
792 static int maxNumConstraints = 0;
793 if (maxNumConstraints < numConstraints)
795 maxNumConstraints = numConstraints;
796 //printf("maxNumConstraints = %d\n",maxNumConstraints );
799 int numUsedArray = numBodies / 32 + 1;
800 bodyUsed.resize(numUsedArray);
802 for (int q = 0; q < numUsedArray; q++)
810 for (int i = 0; i < numConstraints; i++)
811 cs[i].m_batchId = -1;
814 int numValidConstraints = 0;
815 // int unprocessedConstraintIndex = 0;
820 B3_PROFILE("cpu batch innerloop");
822 while (numValidConstraints < numConstraints)
825 int nCurrentBatch = 0;
827 for (int i = 0; i < curBodyUsed; i++)
828 bodyUsed[curUsed[i] / 32] = 0;
832 for (int i = numValidConstraints; i < numConstraints; i++)
835 b3Assert(idx < numConstraints);
836 // check if it can go
837 int bodyAS = cs[idx].m_bodyAPtrAndSignBit;
838 int bodyBS = cs[idx].m_bodyBPtrAndSignBit;
839 int bodyA = abs(bodyAS);
840 int bodyB = abs(bodyBS);
841 bool aIsStatic = (bodyAS < 0) || bodyAS == staticIdx;
842 bool bIsStatic = (bodyBS < 0) || bodyBS == staticIdx;
843 int aUnavailable = 0;
844 int bUnavailable = 0;
847 aUnavailable = bodyUsed[bodyA / 32] & (1 << (bodyA & 31));
852 bUnavailable = bodyUsed[bodyB / 32] & (1 << (bodyB & 31));
855 if (aUnavailable == 0 && bUnavailable == 0) // ok
859 bodyUsed[bodyA / 32] |= (1 << (bodyA & 31));
860 curUsed[curBodyUsed++] = bodyA;
864 bodyUsed[bodyB / 32] |= (1 << (bodyB & 31));
865 curUsed[curBodyUsed++] = bodyB;
868 cs[idx].m_batchId = batchIdx;
870 if (i != numValidConstraints)
872 b3Swap(cs[i], cs[numValidConstraints]);
876 numValidConstraints++;
879 if (nCurrentBatch == simdWidth)
882 for (int i = 0; i < curBodyUsed; i++)
883 bodyUsed[curUsed[i] / 32] = 0;
889 m_gpuData->m_batchSizes.push_back(nCurrentBatch);
895 // debugPrintf( "nBatches: %d\n", batchIdx );
896 for (int i = 0; i < numConstraints; i++)
898 b3Assert(cs[i].m_batchId != -1);
902 if (maxSwaps < numSwaps)
905 //printf("maxSwaps = %d\n", maxSwaps);
911 /// b3PgsJacobiSolver Sequentially applies impulses
912 b3Scalar b3GpuPgsConstraintSolver::solveGroup(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias,
913 int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
915 B3_PROFILE("solveJoints");
916 //you need to provide at least some bodies
918 solveGroupCacheFriendlySetup(gpuBodies, gpuInertias, numBodies, gpuConstraints, numConstraints, infoGlobal);
920 solveGroupCacheFriendlyIterations(gpuConstraints, numConstraints, infoGlobal);
922 solveGroupCacheFriendlyFinish(gpuBodies, gpuInertias, numBodies, gpuConstraints, numConstraints, infoGlobal);
927 void b3GpuPgsConstraintSolver::solveJoints(int numBodies, b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias,
928 int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints)
930 b3ContactSolverInfo infoGlobal;
931 infoGlobal.m_splitImpulse = false;
932 infoGlobal.m_timeStep = 1.f / 60.f;
933 infoGlobal.m_numIterations = 4; //4;
934 // infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS|B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION;
935 //infoGlobal.m_solverMode|=B3_SOLVER_USE_2_FRICTION_DIRECTIONS|B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS;
936 infoGlobal.m_solverMode |= B3_SOLVER_USE_2_FRICTION_DIRECTIONS;
938 //if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
939 //if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
941 solveGroup(gpuBodies, gpuInertias, numBodies, gpuConstraints, numConstraints, infoGlobal);
944 //b3AlignedObjectArray<b3RigidBodyData> testBodies;
946 b3Scalar b3GpuPgsConstraintSolver::solveGroupCacheFriendlyFinish(b3OpenCLArray<b3RigidBodyData>* gpuBodies, b3OpenCLArray<b3InertiaData>* gpuInertias, int numBodies, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
948 B3_PROFILE("solveGroupCacheFriendlyFinish");
949 // int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
953 if (gpuBreakConstraints)
955 B3_PROFILE("breakViolatedConstraintsKernel");
956 b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_breakViolatedConstraintsKernel, "m_breakViolatedConstraintsKernel");
957 launcher.setBuffer(gpuConstraints->getBufferCL());
958 launcher.setBuffer(m_gpuData->m_gpuConstraintInfo1->getBufferCL());
959 launcher.setBuffer(m_gpuData->m_gpuConstraintRowOffsets->getBufferCL());
960 launcher.setBuffer(m_gpuData->m_gpuConstraintRows->getBufferCL());
961 launcher.setConst(numConstraints);
962 launcher.launch1D(numConstraints);
966 gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
967 m_gpuData->m_gpuBatchConstraints->copyToHost(m_gpuData->m_cpuBatchConstraints);
968 m_gpuData->m_gpuConstraintRows->copyToHost(m_gpuData->m_cpuConstraintRows);
969 gpuConstraints->copyToHost(m_gpuData->m_cpuConstraints);
970 m_gpuData->m_gpuConstraintInfo1->copyToHost(m_gpuData->m_cpuConstraintInfo1);
971 m_gpuData->m_gpuConstraintRowOffsets->copyToHost(m_gpuData->m_cpuConstraintRowOffsets);
973 for (int cid = 0; cid < numConstraints; cid++)
975 int originalConstraintIndex = batchConstraints[cid].m_originalConstraintIndex;
976 int constraintRowOffset = m_gpuData->m_cpuConstraintRowOffsets[originalConstraintIndex];
977 int numRows = m_gpuData->m_cpuConstraintInfo1[originalConstraintIndex];
980 // printf("cid=%d, breakingThreshold =%f\n",cid,breakingThreshold);
981 for (int i = 0; i < numRows; i++)
983 int rowIndex = constraintRowOffset + i;
984 int orgConstraintIndex = m_gpuData->m_cpuConstraintRows[rowIndex].m_originalConstraintIndex;
985 float breakingThreshold = m_gpuData->m_cpuConstraints[orgConstraintIndex].m_breakingImpulseThreshold;
986 // printf("rows[%d].m_appliedImpulse=%f\n",rowIndex,rows[rowIndex].m_appliedImpulse);
987 if (b3Fabs(m_gpuData->m_cpuConstraintRows[rowIndex].m_appliedImpulse) >= breakingThreshold)
989 m_gpuData->m_cpuConstraints[orgConstraintIndex].m_flags = 0; //&= ~B3_CONSTRAINT_FLAG_ENABLED;
995 gpuConstraints->copyFromHost(m_gpuData->m_cpuConstraints);
1000 if (useGpuWriteBackVelocities)
1002 B3_PROFILE("GPU write back velocities and transforms");
1004 b3LauncherCL launcher(m_gpuData->m_queue, m_gpuData->m_writeBackVelocitiesKernel, "m_writeBackVelocitiesKernel");
1005 launcher.setBuffer(gpuBodies->getBufferCL());
1006 launcher.setBuffer(m_gpuData->m_gpuSolverBodies->getBufferCL());
1007 launcher.setConst(numBodies);
1008 launcher.launch1D(numBodies);
1009 clFinish(m_gpuData->m_queue);
1010 // m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
1011 // m_gpuData->m_gpuBodies->copyToHostPointer(bodies,numBodies);
1012 //m_gpuData->m_gpuBodies->copyToHost(testBodies);
1016 B3_PROFILE("CPU write back velocities and transforms");
1018 m_gpuData->m_gpuSolverBodies->copyToHost(m_tmpSolverBodyPool);
1019 gpuBodies->copyToHost(m_gpuData->m_cpuBodies);
1020 for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
1022 int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
1023 //printf("bodyIndex=%d\n",bodyIndex);
1024 b3Assert(i == bodyIndex);
1026 b3RigidBodyData* body = &m_gpuData->m_cpuBodies[bodyIndex];
1027 if (body->m_invMass)
1029 if (infoGlobal.m_splitImpulse)
1030 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1032 m_tmpSolverBodyPool[i].writebackVelocity();
1036 body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity;
1037 body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity;
1044 if (infoGlobal.m_splitImpulse)
1046 body->m_pos = m_tmpSolverBodyPool[i].m_worldTransform.getOrigin();
1048 orn = m_tmpSolverBodyPool[i].m_worldTransform.getRotation();
1055 gpuBodies->copyFromHost(m_gpuData->m_cpuBodies);
1059 clFinish(m_gpuData->m_queue);
1061 m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1062 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1063 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1064 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1066 m_tmpSolverBodyPool.resizeNoInitialize(0);