Bullet Collision Detection & Physics Library: btMultiBodyInplaceSolverIslandCallback.h Source File

Bullet Collision Detection & Physics Library

btMultiBodyInplaceSolverIslandCallback.h
Go to the documentation of this file.
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{
33public:
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{
55public:
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 {
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 */
btMultiBodyConstraintSolver.h
btMultiBodyDynamicsWorld.h
btGetConstraintIslandId2
int btGetConstraintIslandId2(const btTypedConstraint *lhs)
Definition: btMultiBodyInplaceSolverIslandCallback.h:22
btGetMultiBodyConstraintIslandId
int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint *lhs)
Definition: btMultiBodyInplaceSolverIslandCallback.h:43
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSimulationIslandManager.h
btAlignedObjectArray< btCollisionObject * >
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btAlignedObjectArray::clear
void clear()
clear the array, deallocated memory. Generally it is better to use array.resize(0),...
Definition: btAlignedObjectArray.h:176
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:51
btCollisionObject::CO_SOFT_BODY
@ CO_SOFT_BODY
Definition: btCollisionObject.h:152
btCollisionObject::getInternalType
int getInternalType() const
reserved for Bullet internal usage
Definition: btCollisionObject.h:374
btCollisionObject::getIslandTag
int getIslandTag() const
Definition: btCollisionObject.h:448
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btMultiBodyConstraintSolver
Definition: btMultiBodyConstraintSolver.h:30
btMultiBodyConstraintSolver::solveMultiBodyGroup
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
Definition: btMultiBodyConstraintSolver.cpp:1740
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:57
btMultiBodyConstraint::getIslandIdA
virtual int getIslandIdA() const =0
btMultiBodyConstraint::getIslandIdB
virtual int getIslandIdB() const =0
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:65
btSequentialImpulseConstraintSolver::m_analyticsData
btSolverAnalyticsData m_analyticsData
Definition: btSequentialImpulseConstraintSolver.h:212
btSortConstraintOnIslandPredicate2
Definition: btMultiBodyInplaceSolverIslandCallback.h:32
btSortConstraintOnIslandPredicate2::operator()
bool operator()(const btTypedConstraint *lhs, const btTypedConstraint *rhs) const
Definition: btMultiBodyInplaceSolverIslandCallback.h:34
btSortMultiBodyConstraintOnIslandPredicate
Definition: btMultiBodyInplaceSolverIslandCallback.h:54
btSortMultiBodyConstraintOnIslandPredicate::operator()
bool operator()(const btMultiBodyConstraint *lhs, const btMultiBodyConstraint *rhs) const
Definition: btMultiBodyInplaceSolverIslandCallback.h:56
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:76
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
MultiBodyInplaceSolverIslandCallback
Definition: btMultiBodyInplaceSolverIslandCallback.h:66
MultiBodyInplaceSolverIslandCallback::m_constraints
btAlignedObjectArray< btTypedConstraint * > m_constraints
Definition: btMultiBodyInplaceSolverIslandCallback.h:81
MultiBodyInplaceSolverIslandCallback::processConstraints
virtual void processConstraints(int islandId=-1)
Definition: btMultiBodyInplaceSolverIslandCallback.h:223
MultiBodyInplaceSolverIslandCallback::setup
virtual void setup(btContactSolverInfo *solverInfo, btTypedConstraint **sortedConstraints, int numConstraints, btMultiBodyConstraint **sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw *debugDrawer)
Definition: btMultiBodyInplaceSolverIslandCallback.h:104
MultiBodyInplaceSolverIslandCallback::m_multiBodyConstraints
btAlignedObjectArray< btMultiBodyConstraint * > m_multiBodyConstraints
Definition: btMultiBodyInplaceSolverIslandCallback.h:82
MultiBodyInplaceSolverIslandCallback::processIsland
virtual void processIsland(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifolds, int numManifolds, int islandId)
Definition: btMultiBodyInplaceSolverIslandCallback.h:127
MultiBodyInplaceSolverIslandCallback::m_sortedConstraints
btTypedConstraint ** m_sortedConstraints
Definition: btMultiBodyInplaceSolverIslandCallback.h:73
MultiBodyInplaceSolverIslandCallback::m_solverInfo
btContactSolverInfo * m_solverInfo
Definition: btMultiBodyInplaceSolverIslandCallback.h:68
MultiBodyInplaceSolverIslandCallback::m_numConstraints
int m_numConstraints
Definition: btMultiBodyInplaceSolverIslandCallback.h:74
MultiBodyInplaceSolverIslandCallback::m_multiBodySortedConstraints
btMultiBodyConstraint ** m_multiBodySortedConstraints
Definition: btMultiBodyInplaceSolverIslandCallback.h:70
MultiBodyInplaceSolverIslandCallback::setMultiBodyConstraintSolver
void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver *solver)
Definition: btMultiBodyInplaceSolverIslandCallback.h:122
MultiBodyInplaceSolverIslandCallback::m_manifolds
btAlignedObjectArray< btPersistentManifold * > m_manifolds
Definition: btMultiBodyInplaceSolverIslandCallback.h:80
MultiBodyInplaceSolverIslandCallback::m_dispatcher
btDispatcher * m_dispatcher
Definition: btMultiBodyInplaceSolverIslandCallback.h:76
MultiBodyInplaceSolverIslandCallback::m_debugDrawer
btIDebugDraw * m_debugDrawer
Definition: btMultiBodyInplaceSolverIslandCallback.h:75
MultiBodyInplaceSolverIslandCallback::operator=
MultiBodyInplaceSolverIslandCallback & operator=(const MultiBodyInplaceSolverIslandCallback &other)
Definition: btMultiBodyInplaceSolverIslandCallback.h:97
MultiBodyInplaceSolverIslandCallback::m_bodies
btAlignedObjectArray< btCollisionObject * > m_bodies
Definition: btMultiBodyInplaceSolverIslandCallback.h:78
MultiBodyInplaceSolverIslandCallback::m_islandAnalyticsData
btAlignedObjectArray< btSolverAnalyticsData > m_islandAnalyticsData
Definition: btMultiBodyInplaceSolverIslandCallback.h:84
MultiBodyInplaceSolverIslandCallback::m_numMultiBodyConstraints
int m_numMultiBodyConstraints
Definition: btMultiBodyInplaceSolverIslandCallback.h:71
MultiBodyInplaceSolverIslandCallback::m_softBodies
btAlignedObjectArray< btCollisionObject * > m_softBodies
Definition: btMultiBodyInplaceSolverIslandCallback.h:79
MultiBodyInplaceSolverIslandCallback::MultiBodyInplaceSolverIslandCallback
MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver *solver, btDispatcher *dispatcher)
Definition: btMultiBodyInplaceSolverIslandCallback.h:86
MultiBodyInplaceSolverIslandCallback::m_solver
btMultiBodyConstraintSolver * m_solver
Definition: btMultiBodyInplaceSolverIslandCallback.h:69
btContactSolverInfoData::m_reportSolverAnalytics
int m_reportSolverAnalytics
Definition: btContactSolverInfo.h:71
btContactSolverInfoData::m_minimumSolverBatchSize
int m_minimumSolverBatchSize
Definition: btContactSolverInfo.h:64
btContactSolverInfo
Definition: btContactSolverInfo.h:76
btSimulationIslandManager::IslandCallback
Definition: btSimulationIslandManager.h:53
btSolverAnalyticsData::m_islandId
int m_islandId
Definition: btSequentialImpulseConstraintSolver.h:41