dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyInplaceSolverIslandCallback.h

   1 /*
   2  Bullet Continuous Collision Detection and Physics Library
   3  Copyright (c) 2019 Google Inc. http://bulletphysics.org
   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  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.
  10  2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  11  3. This notice may not be removed or altered from any source distribution.
  12  */
  13
  14 #ifndef BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
  15 #define BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H
  16
  17 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
  18 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
  19 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
  20 #include "btMultiBodyConstraintSolver.h"
  21
  22 SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs)
  23 {
  24     int islandId;
  25
  26     const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
  27     const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
  28     islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
  29     return islandId;
  30 }
  31 class btSortConstraintOnIslandPredicate2
  32 {
  33 public:
  34     bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const
  35     {
  36         int rIslandId0, lIslandId0;
  37         rIslandId0 = btGetConstraintIslandId2(rhs);
  38         lIslandId0 = btGetConstraintIslandId2(lhs);
  39         return lIslandId0 < rIslandId0;
  40     }
  41 };
  42
  43 SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
  44 {
  45     int islandId;
  46
  47     int islandTagA = lhs->getIslandIdA();
  48     int islandTagB = lhs->getIslandIdB();
  49     islandId = islandTagA >= 0 ? islandTagA : islandTagB;
  50     return islandId;
  51 }
  52
  53 class btSortMultiBodyConstraintOnIslandPredicate
  54 {
  55 public:
  56     bool operator()(const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs) const
  57     {
  58         int rIslandId0, lIslandId0;
  59         rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
  60         lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
  61         return lIslandId0 < rIslandId0;
  62     }
  63 };
  64
  65 struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
  66 {
  67
  68     btContactSolverInfo* m_solverInfo;
  69     btMultiBodyConstraintSolver* m_solver;
  70     btMultiBodyConstraint** m_multiBodySortedConstraints;
  71     int m_numMultiBodyConstraints;
  72
  73     btTypedConstraint** m_sortedConstraints;
  74     int m_numConstraints;
  75     btIDebugDraw* m_debugDrawer;
  76     btDispatcher* m_dispatcher;
  77
  78     btAlignedObjectArray<btCollisionObject*> m_bodies;
  79         btAlignedObjectArray<btCollisionObject*> m_softBodies;
  80     btAlignedObjectArray<btPersistentManifold*> m_manifolds;
  81     btAlignedObjectArray<btTypedConstraint*> m_constraints;
  82     btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
  83
  84     btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
  85
  86     MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
  87                                          btDispatcher* dispatcher)
  88     : m_solverInfo(NULL),
  89     m_solver(solver),
  90     m_multiBodySortedConstraints(NULL),
  91     m_numConstraints(0),
  92     m_debugDrawer(NULL),
  93     m_dispatcher(dispatcher)
  94     {
  95     }
  96
  97     MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other)
  98     {
  99         btAssert(0);
 100         (void)other;
 101         return *this;
 102     }
 103
 104     SIMD_FORCE_INLINE virtual void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
 105     {
 106         m_islandAnalyticsData.clear();
 107         btAssert(solverInfo);
 108         m_solverInfo = solverInfo;
 109
 110         m_multiBodySortedConstraints = sortedMultiBodyConstraints;
 111         m_numMultiBodyConstraints = numMultiBodyConstraints;
 112         m_sortedConstraints = sortedConstraints;
 113         m_numConstraints = numConstraints;
 114
 115         m_debugDrawer = debugDrawer;
 116         m_bodies.resize(0);
 117         m_manifolds.resize(0);
 118         m_constraints.resize(0);
 119         m_multiBodyConstraints.resize(0);
 120     }
 121
 122     void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver)
 123     {
 124         m_solver = solver;
 125     }
 126
 127     virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId)
 128     {
 129         if (islandId < 0)
 130         {
 131             ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
 132             m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
 133             if (m_solverInfo->m_reportSolverAnalytics&1)
 134             {
 135                 m_solver->m_analyticsData.m_islandId = islandId;
 136                 m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
 137             }
 138         }
 139         else
 140         {
 141             //also add all non-contact constraints/joints for this island
 142             btTypedConstraint** startConstraint = 0;
 143             btMultiBodyConstraint** startMultiBodyConstraint = 0;
 144
 145             int numCurConstraints = 0;
 146             int numCurMultiBodyConstraints = 0;
 147
 148             int i;
 149
 150             //find the first constraint for this island
 151
 152             for (i = 0; i < m_numConstraints; i++)
 153             {
 154                 if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
 155                 {
 156                     startConstraint = &m_sortedConstraints[i];
 157                     break;
 158                 }
 159             }
 160             //count the number of constraints in this island
 161             for (; i < m_numConstraints; i++)
 162             {
 163                 if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
 164                 {
 165                     numCurConstraints++;
 166                 }
 167             }
 168
 169             for (i = 0; i < m_numMultiBodyConstraints; i++)
 170             {
 171                 if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
 172                 {
 173                     startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
 174                     break;
 175                 }
 176             }
 177             //count the number of multi body constraints in this island
 178             for (; i < m_numMultiBodyConstraints; i++)
 179             {
 180                 if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
 181                 {
 182                     numCurMultiBodyConstraints++;
 183                 }
 184             }
 185
 186             //if (m_solverInfo->m_minimumSolverBatchSize<=1)
 187             //{
 188             //    m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
 189             //} else
 190             {
 191                 for (i = 0; i < numBodies; i++)
 192                                 {
 193                                         bool isSoftBodyType = (bodies[i]->getInternalType() & btCollisionObject::CO_SOFT_BODY);
 194                                         if (!isSoftBodyType)
 195                                         {
 196                                                 m_bodies.push_back(bodies[i]);
 197                                         }
 198                                         else
 199                                         {
 200                                                 m_softBodies.push_back(bodies[i]);
 201                                         }
 202                                 }
 203                 for (i = 0; i < numManifolds; i++)
 204                     m_manifolds.push_back(manifolds[i]);
 205                 for (i = 0; i < numCurConstraints; i++)
 206                     m_constraints.push_back(startConstraint[i]);
 207
 208                 for (i = 0; i < numCurMultiBodyConstraints; i++)
 209                     m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
 210
 211                 if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
 212                 {
 213                     processConstraints(islandId);
 214                 }
 215                 else
 216                 {
 217                     //printf("deferred\n");
 218                 }
 219             }
 220         }
 221     }
 222
 223     virtual void processConstraints(int islandId=-1)
 224     {
 225         btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
 226         btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
 227         btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0;
 228         btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
 229
 230         //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
 231
 232         m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
 233         if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
 234         {
 235             m_solver->m_analyticsData.m_islandId = islandId;
 236             m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
 237         }
 238         m_bodies.resize(0);
 239                 m_softBodies.resize(0);
 240         m_manifolds.resize(0);
 241         m_constraints.resize(0);
 242         m_multiBodyConstraints.resize(0);
 243     }
 244 };
 245
 246
 247 #endif /*BT_MULTIBODY_INPLACE_SOLVER_ISLAND_CALLBACK_H */