dali-physics/third-party/bullet3/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp

   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
  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
  33 b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
  34 {
  35         delete m_data;
  36 }
  37
  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
  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
  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 }
  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
  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
  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
 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
 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 {
 228         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
 238         unsigned short int getType() { return 0; }
 239
 240         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
 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
 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
 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
 439 const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
 440 {
 441         return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
 442 }
 443
 444 int b3CpuRigidBodyPipeline::getNumBodies() const
 445 {
 446         return m_data->m_rigidBodies.size();
 447 }