Bullet Collision Detection & Physics Library: btReducedDeformableBodySolver.cpp Source File

Bullet Collision Detection & Physics Library

btReducedDeformableBodySolver.cpp
Go to the documentation of this file.
1#include "btReducedDeformableBodySolver.h"
2#include "btReducedDeformableBody.h"
3
4btReducedDeformableBodySolver::btReducedDeformableBodySolver()
5{
6 m_ascendOrder = true;
7 m_reducedSolver = true;
8 m_dampingAlpha = 0;
9 m_dampingBeta = 0;
10 m_gravity = btVector3(0, 0, 0);
11}
12
13 void btReducedDeformableBodySolver::setGravity(const btVector3& gravity)
14{
15 m_gravity = gravity;
16}
17
18 void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt)
19{
20 m_softBodies.copyFromArray(bodies);
21 bool nodeUpdated = updateNodes();
22
23 if (nodeUpdated)
24 {
25 m_dv.resize(m_numNodes, btVector3(0, 0, 0));
26 m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
27 m_residual.resize(m_numNodes, btVector3(0, 0, 0));
28 m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
29 }
30
31 // need to setZero here as resize only set value for newly allocated items
32 for (int i = 0; i < m_numNodes; ++i)
33 {
34 m_dv[i].setZero();
35 m_ddv[i].setZero();
36 m_residual[i].setZero();
37 }
38
39 if (dt > 0)
40 {
41 m_dt = dt;
42 }
43 m_objective->reinitialize(nodeUpdated, dt);
44
45 int N = bodies.size();
46 if (nodeUpdated)
47 {
48 m_staticConstraints.resize(N);
49 m_nodeRigidConstraints.resize(N);
50 // m_faceRigidConstraints.resize(N);
51 }
52 for (int i = 0; i < N; ++i)
53 {
54 m_staticConstraints[i].clear();
55 m_nodeRigidConstraints[i].clear();
56 // m_faceRigidConstraints[i].clear();
57 }
58
59 for (int i = 0; i < m_softBodies.size(); ++i)
60 {
61 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
62 rsb->m_contactNodesList.clear();
63 }
64
65 // set node index offsets
66 int sum = 0;
67 for (int i = 0; i < m_softBodies.size(); ++i)
68 {
69 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
70 rsb->m_nodeIndexOffset = sum;
71 sum += rsb->m_nodes.size();
72 }
73
74 btDeformableBodySolver::updateSoftBodies();
75}
76
77 void btReducedDeformableBodySolver::predictMotion(btScalar solverdt)
78{
79 applyExplicitForce(solverdt);
80
81 // predict new mesh location
82 predictReduceDeformableMotion(solverdt);
83
84 //TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion
85}
86
87 void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt)
88{
89 for (int i = 0; i < m_softBodies.size(); ++i)
90 {
91 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
92 if (!rsb->isActive())
93 {
94 continue;
95 }
96
97 // clear contacts variables
98 rsb->m_nodeRigidContacts.resize(0);
99 rsb->m_faceRigidContacts.resize(0);
100 rsb->m_faceNodeContacts.resize(0);
101
102 // calculate inverse mass matrix for all nodes
103 for (int j = 0; j < rsb->m_nodes.size(); ++j)
104 {
105 if (rsb->m_nodes[j].m_im > 0)
106 {
107 rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse();
108 }
109 }
110
111 // rigid motion: t, R at time^*
112 rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
113
114 // update reduced dofs at time^*
115 // rsb->updateReducedDofs(solverdt);
116
117 // update local moment arm at time^*
118 // rsb->updateLocalMomentArm();
119 // rsb->updateExternalForceProjectMatrix(true);
120
121 // predict full space velocity at time^* (needed for constraints)
122 rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
123
124 // update full space nodal position at time^*
125 rsb->mapToFullPosition(rsb->getInterpolationWorldTransform());
126
127 // update bounding box
128 rsb->updateBounds();
129
130 // update tree
131 rsb->updateNodeTree(true, true);
132 if (!rsb->m_fdbvt.empty())
133 {
134 rsb->updateFaceTree(true, true);
135 }
136 }
137}
138
139 void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt)
140{
141 for (int i = 0; i < m_softBodies.size(); ++i)
142 {
143 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
144
145 // apply gravity to the rigid frame, get m_linearVelocity at time^*
146 rsb->applyRigidGravity(m_gravity, solverdt);
147
148 if (!rsb->isReducedModesOFF())
149 {
150 // add internal force (elastic force & damping force)
151 rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
152 rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
153
154 // get reduced velocity at time^*
155 rsb->updateReducedVelocity(solverdt);
156 }
157
158 // apply damping (no need at this point)
159 // rsb->applyDamping(solverdt);
160 }
161}
162
163 void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep)
164{
165 for (int i = 0; i < m_softBodies.size(); ++i)
166 {
167 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
168
169 // rigid motion
170 rsb->proceedToTransform(timeStep, true);
171
172 if (!rsb->isReducedModesOFF())
173 {
174 // update reduced dofs for time^n+1
175 rsb->updateReducedDofs(timeStep);
176
177 // update local moment arm for time^n+1
178 rsb->updateLocalMomentArm();
179 rsb->updateExternalForceProjectMatrix(true);
180 }
181
182 // update mesh nodal positions for time^n+1
183 rsb->mapToFullPosition(rsb->getRigidTransform());
184
185 // update mesh nodal velocity
186 rsb->mapToFullVelocity(rsb->getRigidTransform());
187
188 // end of time step clean up and update
189 rsb->endOfTimeStepZeroing();
190
191 // update the rendering mesh
192 rsb->interpolateRenderMesh();
193 }
194}
195
196 void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
197{
198 for (int i = 0; i < m_softBodies.size(); ++i)
199 {
200 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
201 if (!rsb->isActive())
202 {
203 continue;
204 }
205
206 // set fixed constraints
207 for (int j = 0; j < rsb->m_fixedNodes.size(); ++j)
208 {
209 int i_node = rsb->m_fixedNodes[j];
210 if (rsb->m_nodes[i_node].m_im == 0)
211 {
212 for (int k = 0; k < 3; ++k)
213 {
214 btVector3 dir(0, 0, 0);
215 dir[k] = 1;
216 btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt);
217 m_staticConstraints[i].push_back(static_constraint);
218 }
219 }
220 }
221 btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size());
222
223 // set Deformable Node vs. Rigid constraint
224 for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
225 {
226 const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
227 // skip fixed points
228 if (contact.m_node->m_im == 0)
229 {
230 continue;
231 }
232 btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
233 m_nodeRigidConstraints[i].push_back(constraint);
234 rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
235 }
236 // std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
237 // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
238
239 }
240}
241
242 btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
243{
244 btScalar residualSquare = 0;
245
246 for (int i = 0; i < m_softBodies.size(); ++i)
247 {
248 btAlignedObjectArray<int> m_orderNonContactConstraintPool;
249 btAlignedObjectArray<int> m_orderContactConstraintPool;
250
251 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
252
253 // shuffle the order of applying constraint
254 m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
255 m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
256 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
257 {
258 // fixed constraint order
259 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
260 {
261 m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j;
262 }
263 // contact constraint order
264 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
265 {
266 m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
267 }
268
269 m_ascendOrder = m_ascendOrder ? false : true;
270 }
271 else
272 {
273 for (int j = 0; j < m_staticConstraints[i].size(); ++j)
274 {
275 m_orderNonContactConstraintPool[j] = j;
276 }
277 // contact constraint order
278 for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
279 {
280 m_orderContactConstraintPool[j] = j;
281 }
282 }
283
284 // handle fixed constraint
285 for (int k = 0; k < m_staticConstraints[i].size(); ++k)
286 {
287 btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]];
288 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
289 residualSquare = btMax(residualSquare, localResidualSquare);
290 }
291
292 // handle contact constraint
293
294 // node vs rigid contact
295 // std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
296 for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
297 {
298 btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];
299 btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
300 residualSquare = btMax(residualSquare, localResidualSquare);
301 }
302
303 // face vs rigid contact
304 // for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k)
305 // {
306 // btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k];
307 // btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
308 // residualSquare = btMax(residualSquare, localResidualSquare);
309 // }
310 }
311
312
313 return residualSquare;
314}
315
316 void btReducedDeformableBodySolver::deformableBodyInternalWriteBack()
317{
318 // reduced deformable update
319 for (int i = 0; i < m_softBodies.size(); ++i)
320 {
321 btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
322 rsb->applyInternalVelocityChanges();
323 }
324 m_ascendOrder = true;
325}
SOLVER_RANDMIZE_ORDER
@ SOLVER_RANDMIZE_ORDER
Definition: btContactSolverInfo.h:23
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btReducedDeformableBodySolver.h
btReducedDeformableBody.h
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btAssert
#define btAssert(x)
Definition: btScalar.h:153
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:95
btAlignedObjectArray< btSoftBody * >
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btAlignedObjectArray::copyFromArray
void copyFromArray(const btAlignedObjectArray &otherArray)
Definition: btAlignedObjectArray.h:496
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::isActive
bool isActive() const
Definition: btCollisionObject.h:306
btCollisionObject::getInterpolationWorldTransform
const btTransform & getInterpolationWorldTransform() const
Definition: btCollisionObject.h:410
btDeformableBackwardEulerObjective::reinitialize
void reinitialize(bool nodeUpdated, btScalar dt)
Definition: btDeformableBackwardEulerObjective.cpp:34
btDeformableBodySolver::m_ddv
TVStack m_ddv
Definition: btDeformableBodySolver.h:38
btDeformableBodySolver::updateSoftBodies
virtual void updateSoftBodies()
Perform necessary per-step updates of soft bodies such as recomputing normals and bounding boxes.
Definition: btDeformableBodySolver.cpp:485
btDeformableBodySolver::m_numNodes
int m_numNodes
Definition: btDeformableBodySolver.h:35
btDeformableBodySolver::m_objective
btDeformableBackwardEulerObjective * m_objective
Definition: btDeformableBodySolver.h:52
btDeformableBodySolver::updateNodes
bool updateNodes()
Definition: btDeformableBodySolver.cpp:375
btDeformableBodySolver::m_reducedSolver
bool m_reducedSolver
Definition: btDeformableBodySolver.h:49
btDeformableBodySolver::m_dv
TVStack m_dv
Definition: btDeformableBodySolver.h:36
btDeformableBodySolver::m_residual
TVStack m_residual
Definition: btDeformableBodySolver.h:39
btDeformableBodySolver::m_softBodies
btAlignedObjectArray< btSoftBody * > m_softBodies
Definition: btDeformableBodySolver.h:40
btDeformableBodySolver::m_dt
btScalar m_dt
Definition: btDeformableBodySolver.h:42
btDeformableBodySolver::m_backupVelocity
TVStack m_backupVelocity
Definition: btDeformableBodySolver.h:41
btDeformableBodySolver::applyExplicitForce
virtual void applyExplicitForce()
Definition: btDeformableBodySolver.cpp:509
btReducedDeformableBodySolver::m_dampingBeta
btScalar m_dampingBeta
Definition: btReducedDeformableBodySolver.h:14
btReducedDeformableBodySolver::predictMotion
virtual void predictMotion(btScalar solverdt)
Predict motion of soft bodies into next timestep.
Definition: btReducedDeformableBodySolver.cpp:77
btReducedDeformableBodySolver::btReducedDeformableBodySolver
btReducedDeformableBodySolver()
Definition: btReducedDeformableBodySolver.cpp:4
btReducedDeformableBodySolver::solveContactConstraints
virtual btScalar solveContactConstraints(btCollisionObject **deformableBodies, int numDeformableBodies, const btContactSolverInfo &infoGlobal)
Definition: btReducedDeformableBodySolver.cpp:242
btReducedDeformableBodySolver::setConstraints
virtual void setConstraints(const btContactSolverInfo &infoGlobal)
Definition: btReducedDeformableBodySolver.cpp:196
btReducedDeformableBodySolver::m_ascendOrder
bool m_ascendOrder
Definition: btReducedDeformableBodySolver.h:12
btReducedDeformableBodySolver::setGravity
virtual void setGravity(const btVector3 &gravity)
Definition: btReducedDeformableBodySolver.cpp:13
btReducedDeformableBodySolver::m_gravity
btVector3 m_gravity
Definition: btReducedDeformableBodySolver.h:16
btReducedDeformableBodySolver::deformableBodyInternalWriteBack
virtual void deformableBodyInternalWriteBack()
Definition: btReducedDeformableBodySolver.cpp:316
btReducedDeformableBodySolver::m_nodeRigidConstraints
btAlignedObjectArray< btAlignedObjectArray< btReducedDeformableNodeRigidContactConstraint > > m_nodeRigidConstraints
Definition: btReducedDeformableBodySolver.h:24
btReducedDeformableBodySolver::reinitialize
virtual void reinitialize(const btAlignedObjectArray< btSoftBody * > &bodies, btScalar dt)
Definition: btReducedDeformableBodySolver.cpp:18
btReducedDeformableBodySolver::m_staticConstraints
btAlignedObjectArray< btAlignedObjectArray< btReducedDeformableStaticConstraint > > m_staticConstraints
Definition: btReducedDeformableBodySolver.h:23
btReducedDeformableBodySolver::applyTransforms
virtual void applyTransforms(btScalar timeStep)
Definition: btReducedDeformableBodySolver.cpp:163
btReducedDeformableBodySolver::m_dampingAlpha
btScalar m_dampingAlpha
Definition: btReducedDeformableBodySolver.h:13
btReducedDeformableBodySolver::predictReduceDeformableMotion
void predictReduceDeformableMotion(btScalar solverdt)
Definition: btReducedDeformableBodySolver.cpp:87
btReducedDeformableBody
Definition: btReducedDeformableBody.h:12
btReducedDeformableBody::predictIntegratedTransform
void predictIntegratedTransform(btScalar dt, btTransform &predictedTransform)
Definition: btReducedDeformableBody.cpp:246
btReducedDeformableBody::applyInternalVelocityChanges
void applyInternalVelocityChanges()
Definition: btReducedDeformableBody.cpp:233
btReducedDeformableBody::getRelativePos
btVector3 getRelativePos(int n_node)
Definition: btReducedDeformableBody.cpp:626
btReducedDeformableBody::m_fixedNodes
btAlignedObjectArray< int > m_fixedNodes
Definition: btReducedDeformableBody.h:93
btReducedDeformableBody::mapToFullVelocity
void mapToFullVelocity(const btTransform &ref_trans)
Definition: btReducedDeformableBody.cpp:285
btReducedDeformableBody::endOfTimeStepZeroing
void endOfTimeStepZeroing()
Definition: btReducedDeformableBody.cpp:219
btReducedDeformableBody::m_nodeIndexOffset
int m_nodeIndexOffset
Definition: btReducedDeformableBody.h:94
btReducedDeformableBody::m_reducedVelocityBuffer
tDenseArray m_reducedVelocityBuffer
Definition: btReducedDeformableBody.h:83
btReducedDeformableBody::updateReducedVelocity
void updateReducedVelocity(btScalar solverdt)
Definition: btReducedDeformableBody.cpp:272
btReducedDeformableBody::isReducedModesOFF
bool isReducedModesOFF() const
Definition: btReducedDeformableBody.cpp:789
btReducedDeformableBody::m_x0
TVStack m_x0
Definition: btReducedDeformableBody.h:91
btReducedDeformableBody::updateExternalForceProjectMatrix
void updateExternalForceProjectMatrix(bool initialized)
Definition: btReducedDeformableBody.cpp:166
btReducedDeformableBody::applyReducedElasticForce
void applyReducedElasticForce(const tDenseArray &reduce_dofs)
Definition: btReducedDeformableBody.cpp:748
btReducedDeformableBody::updateLocalMomentArm
void updateLocalMomentArm()
Definition: btReducedDeformableBody.cpp:145
btReducedDeformableBody::applyReducedDampingForce
void applyReducedDampingForce(const tDenseArray &reduce_vel)
Definition: btReducedDeformableBody.cpp:756
btReducedDeformableBody::applyRigidGravity
void applyRigidGravity(const btVector3 &gravity, btScalar dt)
Definition: btReducedDeformableBody.cpp:742
btReducedDeformableBody::updateReducedDofs
void updateReducedDofs(btScalar solverdt)
Definition: btReducedDeformableBody.cpp:251
btReducedDeformableBody::proceedToTransform
void proceedToTransform(btScalar dt, bool end_of_time_step)
Definition: btReducedDeformableBody.cpp:393
btReducedDeformableBody::mapToFullPosition
void mapToFullPosition(const btTransform &ref_trans)
Definition: btReducedDeformableBody.cpp:259
btReducedDeformableBody::m_contactNodesList
btAlignedObjectArray< int > m_contactNodesList
Definition: btReducedDeformableBody.h:97
btReducedDeformableBody::getRigidTransform
btTransform & getRigidTransform()
Definition: btReducedDeformableBody.cpp:769
btReducedDeformableBody::m_reducedDofsBuffer
tDenseArray m_reducedDofsBuffer
Definition: btReducedDeformableBody.h:81
btReducedDeformableNodeRigidContactConstraint
Definition: btReducedDeformableContactConstraint.h:122
btReducedDeformableRigidContactConstraint::solveConstraint
virtual btScalar solveConstraint(const btContactSolverInfo &infoGlobal)
Definition: btReducedDeformableContactConstraint.cpp:107
btReducedDeformableStaticConstraint
Definition: btReducedDeformableContactConstraint.h:6
btReducedDeformableStaticConstraint::solveConstraint
virtual btScalar solveConstraint(const btContactSolverInfo &infoGlobal)
Definition: btReducedDeformableContactConstraint.cpp:28
btSoftBody::DeformableNodeRigidContact
Definition: btSoftBody.h:379
btSoftBody::DeformableNodeRigidContact::m_node
Node * m_node
Definition: btSoftBody.h:381
btSoftBody::interpolateRenderMesh
void interpolateRenderMesh()
Definition: btSoftBody.cpp:3815
btSoftBody::updateFaceTree
void updateFaceTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1300
btSoftBody::updateNodeTree
void updateNodeTree(bool use_velocity, bool margin)
Definition: btSoftBody.h:1257
btSoftBody::m_faceNodeContacts
btAlignedObjectArray< DeformableFaceNodeContact > m_faceNodeContacts
Definition: btSoftBody.h:826
btSoftBody::m_faceRigidContacts
btAlignedObjectArray< DeformableFaceRigidContact > m_faceRigidContacts
Definition: btSoftBody.h:827
btSoftBody::m_nodeRigidContacts
btAlignedObjectArray< DeformableNodeRigidContact > m_nodeRigidContacts
Definition: btSoftBody.h:825
btSoftBody::m_fdbvt
btDbvt m_fdbvt
Definition: btSoftBody.h:836
btSoftBody::m_nodes
tNodeArray m_nodes
Definition: btSoftBody.h:814
btSoftBody::updateBounds
void updateBounds()
Definition: btSoftBody.cpp:2966
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:62
btContactSolverInfo
Definition: btContactSolverInfo.h:76
btDbvt::empty
bool empty() const
Definition: btDbvt.h:314
btSoftBody::Node::m_im
btScalar m_im
Definition: btSoftBody.h:275
btSoftBody::Node::index
int index
Definition: btSoftBody.h:280