1 #include "b3CpuRigidBodyPipeline.h"
2
3 #include "Bullet3Dynamics/shared/b3IntegrateTransforms.h"
4 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
5 #include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
6 #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
7 #include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
8 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
9 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
10 #include "Bullet3Common/b3Vector3.h"
11 #include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
12 #include "Bullet3Dynamics/shared/b3Inertia.h"
13
14 struct b3CpuRigidBodyPipelineInternalData
15 {
16 b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
17 b3AlignedObjectArray<b3Inertia> m_inertias;
18 b3AlignedObjectArray<b3Aabb> m_aabbWorldSpace;
19
20 b3DynamicBvhBroadphase* m_bp;
21 b3CpuNarrowPhase* m_np;
22 b3Config m_config;
23 };
24
b3CpuRigidBodyPipeline(class b3CpuNarrowPhase * narrowphase,struct b3DynamicBvhBroadphase * broadphaseDbvt,const b3Config & config)25 b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
26 {
27 m_data = new b3CpuRigidBodyPipelineInternalData;
28 m_data->m_np = narrowphase;
29 m_data->m_bp = broadphaseDbvt;
30 m_data->m_config = config;
31 }
32
~b3CpuRigidBodyPipeline()33 b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
34 {
35 delete m_data;
36 }
37
updateAabbWorldSpace()38 void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
39 {
40 for (int i = 0; i < this->getNumBodies(); i++)
41 {
42 b3RigidBodyData* body = &m_data->m_rigidBodies[i];
43 b3Float4 position = body->m_pos;
44 b3Quat orientation = body->m_quat;
45
46 int collidableIndex = body->m_collidableIdx;
47 b3Collidable& collidable = m_data->m_np->getCollidableCpu(collidableIndex);
48 int shapeIndex = collidable.m_shapeIndex;
49
50 if (shapeIndex >= 0)
51 {
52 b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
53 b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
54 float margin = 0.f;
55 b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &worldAabb.m_minVec, &worldAabb.m_maxVec);
56 m_data->m_bp->setAabb(i, worldAabb.m_minVec, worldAabb.m_maxVec, 0);
57 }
58 }
59 }
60
computeOverlappingPairs()61 void b3CpuRigidBodyPipeline::computeOverlappingPairs()
62 {
63 int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
64 m_data->m_bp->calculateOverlappingPairs();
65 numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
66 printf("numPairs=%d\n", numPairs);
67 }
68
computeContactPoints()69 void b3CpuRigidBodyPipeline::computeContactPoints()
70 {
71 b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
72
73 m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
74 }
stepSimulation(float deltaTime)75 void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
76 {
77 //update world space aabb's
78 updateAabbWorldSpace();
79
80 //compute overlapping pairs
81 computeOverlappingPairs();
82
83 //compute contacts
84 computeContactPoints();
85
86 //solve contacts
87
88 //update transforms
89 integrate(deltaTime);
90 }
91
b3CalcRelVel(const b3Vector3 & l0,const b3Vector3 & l1,const b3Vector3 & a0,const b3Vector3 & a1,const b3Vector3 & linVel0,const b3Vector3 & angVel0,const b3Vector3 & linVel1,const b3Vector3 & angVel1)92 static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
93 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
94 {
95 return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
96 }
97
b3SetLinearAndAngular(const b3Vector3 & n,const b3Vector3 & r0,const b3Vector3 & r1,b3Vector3 & linear,b3Vector3 & angular0,b3Vector3 & angular1)98 static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
99 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
100 {
101 linear = -n;
102 angular0 = -b3Cross(r0, n);
103 angular1 = b3Cross(r1, n);
104 }
105
b3SolveContact(b3ContactConstraint4 & cs,const b3Vector3 & posA,b3Vector3 & linVelA,b3Vector3 & angVelA,float invMassA,const b3Matrix3x3 & invInertiaA,const b3Vector3 & posB,b3Vector3 & linVelB,b3Vector3 & angVelB,float invMassB,const b3Matrix3x3 & invInertiaB,float maxRambdaDt[4],float minRambdaDt[4])106 static inline void b3SolveContact(b3ContactConstraint4& cs,
107 const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
108 const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
109 float maxRambdaDt[4], float minRambdaDt[4])
110 {
111 b3Vector3 dLinVelA;
112 dLinVelA.setZero();
113 b3Vector3 dAngVelA;
114 dAngVelA.setZero();
115 b3Vector3 dLinVelB;
116 dLinVelB.setZero();
117 b3Vector3 dAngVelB;
118 dAngVelB.setZero();
119
120 for (int ic = 0; ic < 4; ic++)
121 {
122 // dont necessary because this makes change to 0
123 if (cs.m_jacCoeffInv[ic] == 0.f) continue;
124
125 {
126 b3Vector3 angular0, angular1, linear;
127 b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
128 b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
129 b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
130
131 float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
132 linVelA, angVelA, linVelB, angVelB) +
133 cs.m_b[ic];
134 rambdaDt *= cs.m_jacCoeffInv[ic];
135
136 {
137 float prevSum = cs.m_appliedRambdaDt[ic];
138 float updated = prevSum;
139 updated += rambdaDt;
140 updated = b3Max(updated, minRambdaDt[ic]);
141 updated = b3Min(updated, maxRambdaDt[ic]);
142 rambdaDt = updated - prevSum;
143 cs.m_appliedRambdaDt[ic] = updated;
144 }
145
146 b3Vector3 linImp0 = invMassA * linear * rambdaDt;
147 b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
148 b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
149 b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
150 #ifdef _WIN32
151 b3Assert(_finite(linImp0.getX()));
152 b3Assert(_finite(linImp1.getX()));
153 #endif
154 {
155 linVelA += linImp0;
156 angVelA += angImp0;
157 linVelB += linImp1;
158 angVelB += angImp1;
159 }
160 }
161 }
162 }
163
b3SolveFriction(b3ContactConstraint4 & cs,const b3Vector3 & posA,b3Vector3 & linVelA,b3Vector3 & angVelA,float invMassA,const b3Matrix3x3 & invInertiaA,const b3Vector3 & posB,b3Vector3 & linVelB,b3Vector3 & angVelB,float invMassB,const b3Matrix3x3 & invInertiaB,float maxRambdaDt[4],float minRambdaDt[4])164 static inline void b3SolveFriction(b3ContactConstraint4& cs,
165 const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
166 const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
167 float maxRambdaDt[4], float minRambdaDt[4])
168 {
169 if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
170 const b3Vector3& center = (const b3Vector3&)cs.m_center;
171
172 b3Vector3 n = -(const b3Vector3&)cs.m_linear;
173
174 b3Vector3 tangent[2];
175
176 b3PlaneSpace1(n, tangent[0], tangent[1]);
177
178 b3Vector3 angular0, angular1, linear;
179 b3Vector3 r0 = center - posA;
180 b3Vector3 r1 = center - posB;
181 for (int i = 0; i < 2; i++)
182 {
183 b3SetLinearAndAngular(tangent[i], r0, r1, linear, angular0, angular1);
184 float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
185 linVelA, angVelA, linVelB, angVelB);
186 rambdaDt *= cs.m_fJacCoeffInv[i];
187
188 {
189 float prevSum = cs.m_fAppliedRambdaDt[i];
190 float updated = prevSum;
191 updated += rambdaDt;
192 updated = b3Max(updated, minRambdaDt[i]);
193 updated = b3Min(updated, maxRambdaDt[i]);
194 rambdaDt = updated - prevSum;
195 cs.m_fAppliedRambdaDt[i] = updated;
196 }
197
198 b3Vector3 linImp0 = invMassA * linear * rambdaDt;
199 b3Vector3 linImp1 = invMassB * (-linear) * rambdaDt;
200 b3Vector3 angImp0 = (invInertiaA * angular0) * rambdaDt;
201 b3Vector3 angImp1 = (invInertiaB * angular1) * rambdaDt;
202 #ifdef _WIN32
203 b3Assert(_finite(linImp0.getX()));
204 b3Assert(_finite(linImp1.getX()));
205 #endif
206 linVelA += linImp0;
207 angVelA += angImp0;
208 linVelB += linImp1;
209 angVelB += angImp1;
210 }
211
212 { // angular damping for point constraint
213 b3Vector3 ab = (posB - posA).normalized();
214 b3Vector3 ac = (center - posA).normalized();
215 if (b3Dot(ab, ac) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
216 {
217 float angNA = b3Dot(n, angVelA);
218 float angNB = b3Dot(n, angVelB);
219
220 angVelA -= (angNA * 0.1f) * n;
221 angVelB -= (angNB * 0.1f) * n;
222 }
223 }
224 }
225
226 struct b3SolveTask // : public ThreadPool::Task
227 {
b3SolveTaskb3SolveTask228 b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
229 b3AlignedObjectArray<b3Inertia>& shapes,
230 b3AlignedObjectArray<b3ContactConstraint4>& constraints,
231 int start, int nConstraints,
232 int maxNumBatches,
233 b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
234 : m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
235 {
236 }
237
getTypeb3SolveTask238 unsigned short int getType() { return 0; }
239
runb3SolveTask240 void run(int tIdx)
241 {
242 b3AlignedObjectArray<int> usedBodies;
243 //printf("run..............\n");
244
245 for (int bb = 0; bb < m_maxNumBatches; bb++)
246 {
247 usedBodies.resize(0);
248 for (int ic = m_nConstraints - 1; ic >= 0; ic--)
249 //for(int ic=0; ic<m_nConstraints; ic++)
250 {
251 int i = m_start + ic;
252 if (m_constraints[i].m_batchIdx != bb)
253 continue;
254
255 float frictionCoeff = b3GetFrictionCoeff(&m_constraints[i]);
256 int aIdx = (int)m_constraints[i].m_bodyA;
257 int bIdx = (int)m_constraints[i].m_bodyB;
258 //int localBatch = m_constraints[i].m_batchIdx;
259 b3RigidBodyData& bodyA = m_bodies[aIdx];
260 b3RigidBodyData& bodyB = m_bodies[bIdx];
261
262 #if 0
263 if ((bodyA.m_invMass) && (bodyB.m_invMass))
264 {
265 // printf("aIdx=%d, bIdx=%d\n", aIdx,bIdx);
266 }
267 if (bIdx==10)
268 {
269 //printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
270 }
271 #endif
272 if (aIdx == 10)
273 {
274 //printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
275 }
276 if (usedBodies.size() < (aIdx + 1))
277 {
278 usedBodies.resize(aIdx + 1, 0);
279 }
280
281 if (usedBodies.size() < (bIdx + 1))
282 {
283 usedBodies.resize(bIdx + 1, 0);
284 }
285
286 if (bodyA.m_invMass)
287 {
288 b3Assert(usedBodies[aIdx] == 0);
289 usedBodies[aIdx]++;
290 }
291
292 if (bodyB.m_invMass)
293 {
294 b3Assert(usedBodies[bIdx] == 0);
295 usedBodies[bIdx]++;
296 }
297
298 if (!m_solveFriction)
299 {
300 float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
301 float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
302
303 b3SolveContact(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,
304 (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
305 maxRambdaDt, minRambdaDt);
306 }
307 else
308 {
309 float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
310 float minRambdaDt[4] = {0.f, 0.f, 0.f, 0.f};
311
312 float sum = 0;
313 for (int j = 0; j < 4; j++)
314 {
315 sum += m_constraints[i].m_appliedRambdaDt[j];
316 }
317 frictionCoeff = 0.7f;
318 for (int j = 0; j < 4; j++)
319 {
320 maxRambdaDt[j] = frictionCoeff * sum;
321 minRambdaDt[j] = -maxRambdaDt[j];
322 }
323
324 b3SolveFriction(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,
325 (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
326 maxRambdaDt, minRambdaDt);
327 }
328 }
329
330 if (m_wgUsedBodies)
331 {
332 if (m_wgUsedBodies[m_curWgidx].size() < usedBodies.size())
333 {
334 m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
335 }
336 for (int i = 0; i < usedBodies.size(); i++)
337 {
338 if (usedBodies[i])
339 {
340 //printf("cell %d uses body %d\n", m_curWgidx,i);
341 m_wgUsedBodies[m_curWgidx][i] = 1;
342 }
343 }
344 }
345 }
346 }
347
348 b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
349 b3AlignedObjectArray<b3Inertia>& m_shapes;
350 b3AlignedObjectArray<b3ContactConstraint4>& m_constraints;
351 b3AlignedObjectArray<int>* m_wgUsedBodies;
352 int m_curWgidx;
353 int m_start;
354 int m_nConstraints;
355 bool m_solveFriction;
356 int m_maxNumBatches;
357 };
358
solveContactConstraints()359 void b3CpuRigidBodyPipeline::solveContactConstraints()
360 {
361 int m_nIterations = 4;
362
363 b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
364 // const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
365 int n = contactConstraints.size();
366 //convert contacts...
367
368 int maxNumBatches = 250;
369
370 for (int iter = 0; iter < m_nIterations; iter++)
371 {
372 b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
373 task.m_solveFriction = false;
374 task.run(0);
375 }
376
377 for (int iter = 0; iter < m_nIterations; iter++)
378 {
379 b3SolveTask task(m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n, maxNumBatches, 0, 0);
380 task.m_solveFriction = true;
381 task.run(0);
382 }
383 }
384
integrate(float deltaTime)385 void b3CpuRigidBodyPipeline::integrate(float deltaTime)
386 {
387 float angDamping = 0.f;
388 b3Vector3 gravityAcceleration = b3MakeVector3(0, -9, 0);
389
390 //integrate transforms (external forces/gravity should be moved into constraint solver)
391 for (int i = 0; i < m_data->m_rigidBodies.size(); i++)
392 {
393 b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
394 }
395 }
396
registerPhysicsInstance(float mass,const float * position,const float * orientation,int collidableIndex,int userData)397 int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
398 {
399 b3RigidBodyData body;
400 int bodyIndex = m_data->m_rigidBodies.size();
401 body.m_invMass = mass ? 1.f / mass : 0.f;
402 body.m_angVel.setValue(0, 0, 0);
403 body.m_collidableIdx = collidableIndex;
404 body.m_frictionCoeff = 0.3f;
405 body.m_linVel.setValue(0, 0, 0);
406 body.m_pos.setValue(position[0], position[1], position[2]);
407 body.m_quat.setValue(orientation[0], orientation[1], orientation[2], orientation[3]);
408 body.m_restituitionCoeff = 0.f;
409
410 m_data->m_rigidBodies.push_back(body);
411
412 if (collidableIndex >= 0)
413 {
414 b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
415
416 b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
417 b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
418 b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
419
420 b3Scalar margin = 0.01f;
421 b3Transform t;
422 t.setIdentity();
423 t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
424 t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
425 b3TransformAabb(localAabbMin, localAabbMax, margin, t, worldAabb.m_minVec, worldAabb.m_maxVec);
426
427 m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
428 // b3Vector3 aabbMin,aabbMax;
429 // m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
430 }
431 else
432 {
433 b3Error("registerPhysicsInstance using invalid collidableIndex\n");
434 }
435
436 return bodyIndex;
437 }
438
getBodyBuffer() const439 const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
440 {
441 return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
442 }
443
getNumBodies() const444 int b3CpuRigidBodyPipeline::getNumBodies() const
445 {
446 return m_data->m_rigidBodies.size();
447 }
448