Bullet Collision Detection & Physics Library
btReducedDeformableBody.cpp
Go to the documentation of this file.
2#include "../btSoftBodyInternals.h"
5#include <iostream>
6#include <fstream>
7
9 : btSoftBody(worldInfo, node_count, x, m), m_rigidOnly(false)
10{
11 // reduced deformable
12 m_reducedModel = true;
13 m_nReduced = 0;
14 m_nFull = 0;
16
17 m_transform_lock = false;
18 m_ksScale = 1.0;
19 m_rhoScale = 1.0;
20
21 // rigid motion
29 m_linearFactor.setValue(1, 1, 1);
30 // m_invInertiaLocal.setValue(1, 1, 1);
32 m_mass = 0.0;
33 m_inverseMass = 0.0;
34
37
38 // Rayleigh damping
40 m_dampingBeta = 0;
41
43}
44
46{
59}
60
62{
64 btVector3 CoM(0, 0, 0);
65 for (int i = 0; i < m_nFull; ++i)
66 {
68 m_nodes[i].m_im = mass_array[i] > 0 ? 1.0 / (m_rhoScale * mass_array[i]) : 0;
70
71 CoM += m_nodalMass[i] * m_nodes[i].m_x;
72 }
73 // total rigid body mass
75 m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0;
76 // original CoM
78}
79
81{
82 // make sure the initial CoM is at the origin (0,0,0)
83 // for (int i = 0; i < m_nFull; ++i)
84 // {
85 // m_nodes[i].m_x -= m_initialCoM;
86 // }
87 // m_initialCoM.setZero();
90
92
93 // update world inertia tensor
99}
100
102{
104}
105
107{
109}
110
112{
113 m_ksScale = ks;
114}
115
117{
118 m_rhoScale = rho;
119}
120
122{
124 m_nodes[n_node].m_im = 0; // set inverse mass to be zero for the constraint solver.
125}
126
128{
131}
132
134{
135 // zeroing
137 // initialize rest position
139 // initialize local nodal moment arm form the CoM
141 // initialize projection matrix
143}
144
146{
147 TVStack delta_x;
148 delta_x.resize(m_nFull);
149
150 for (int i = 0; i < m_nFull; ++i)
151 {
152 for (int k = 0; k < 3; ++k)
153 {
154 // compute displacement
155 delta_x[i][k] = 0;
156 for (int j = 0; j < m_nReduced; ++j)
157 {
158 delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j];
159 }
160 }
161 // get new moment arm Sq + x0
162 m_localMomentArm[i] = m_x0[i] - m_initialCoM + delta_x[i];
163 }
164}
165
167{
168 // if not initialized, need to compute both P_A and Cq
169 // otherwise, only need to udpate Cq
170 if (!initialized)
171 {
172 // resize
175
178
179 // P_A
180 for (int r = 0; r < m_nReduced; ++r)
181 {
182 m_projPA[r].resize(3 * m_nFull, 0);
183 for (int i = 0; i < m_nFull; ++i)
184 {
186 btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
188
189 for (int k = 0; k < 3; ++k)
190 m_projPA[r][3 * i + k] = prod_i[k];
191
192 // btScalar ratio = m_nodalMass[i] / m_mass;
193 // m_projPA[r] += btVector3(- m_modes[r][3 * i] * ratio,
194 // - m_modes[r][3 * i + 1] * ratio,
195 // - m_modes[r][3 * i + 2] * ratio);
196 }
197 }
198 }
199
200 // C(q) is updated once per position update
201 for (int r = 0; r < m_nReduced; ++r)
202 {
203 m_projCq[r].resize(3 * m_nFull, 0);
204 for (int i = 0; i < m_nFull; ++i)
205 {
207 btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
209
210 for (int k = 0; k < 3; ++k)
211 m_projCq[r][3 * i + k] = m_nodalMass[i] * prod_i[k];
212
213 // btVector3 si(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
214 // m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]);
215 }
216 }
217}
218
220{
221 for (int i = 0; i < m_nReduced; ++i)
222 {
229 }
230 // std::cout << "zeroed!\n";
231}
232
234{
239 for (int r = 0; r < m_nReduced; ++r)
240 {
243 }
244}
245
247{
249}
250
252{
253 for (int r = 0; r < m_nReduced; ++r)
254 {
256 }
257}
258
260{
261 btVector3 origin = ref_trans.getOrigin();
262 btMatrix3x3 rotation = ref_trans.getBasis();
263
264
265 for (int i = 0; i < m_nFull; ++i)
266 {
267 m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin;
268 m_nodes[i].m_q = m_nodes[i].m_x;
269 }
270}
271
273{
274 // update reduced velocity
275 for (int r = 0; r < m_nReduced; ++r)
276 {
277 // the reduced mass is always identity!
278 btScalar delta_v = 0;
280 // delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]);
282 }
283}
284
286{
287 // compute the reduced contribution to the angular velocity
288 // btVector3 sum_linear(0, 0, 0);
289 // btVector3 sum_angular(0, 0, 0);
290 // m_linearVelocityFromReduced.setZero();
291 // m_angularVelocityFromReduced.setZero();
292 // for (int i = 0; i < m_nFull; ++i)
293 // {
294 // btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
295 // btMatrix3x3 r_star = Cross(r_com);
296
297 // btVector3 v_from_reduced(0, 0, 0);
298 // for (int k = 0; k < 3; ++k)
299 // {
300 // for (int r = 0; r < m_nReduced; ++r)
301 // {
302 // v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
303 // }
304 // }
305
306 // btVector3 delta_linear = m_nodalMass[i] * v_from_reduced;
307 // btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced);
308 // sum_linear += delta_linear;
309 // sum_angular += delta_angular;
310 // // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n";
311 // // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n";
312 // // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n";
313 // // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n";
314 // }
315 // m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear);
316 // m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular;
317
318 // m_linearVelocity -= m_linearVelocityFromReduced;
319 // m_angularVelocity -= m_angularVelocityFromReduced;
320
321 for (int i = 0; i < m_nFull; ++i)
322 {
324 }
325}
326
328{
330 btVector3 L_reduced(0, 0, 0);
332
333 for (int i = 0; i < m_nFull; ++i)
334 {
337
338 btVector3 v_from_reduced(0, 0, 0);
339 for (int k = 0; k < 3; ++k)
340 {
341 for (int r = 0; r < m_nReduced; ++r)
342 {
343 v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
344 }
345 }
346
348 // L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced));
349 }
350 return L_rigid + L_reduced;
351}
352
354{
355 btVector3 v_from_reduced(0, 0, 0);
357 // compute velocity contributed by the reduced velocity
358 for (int k = 0; k < 3; ++k)
359 {
360 for (int r = 0; r < m_nReduced; ++r)
361 {
363 }
364 }
365 // get new velocity
367 ref_trans.getBasis() * v_from_reduced +
369 return vel;
370}
371
373{
376
377 // compute velocity contributed by the reduced velocity
378 for (int k = 0; k < 3; ++k)
379 {
380 for (int r = 0; r < m_nReduced; ++r)
381 {
383 }
384 }
385
386 // get delta velocity
388 ref_trans.getBasis() * deltaV_from_reduced +
390 return deltaV;
391}
392
394{
397 // m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose();
400}
401
403{
405 btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(),
406 trs.getOrigin() - current_transform.getOrigin());
408}
409
411{
412 m_transform_lock = true;
413
414 // transform mesh
415 {
416 const btScalar margin = getCollisionShape()->getMargin();
418 vol;
419
421 btVector3 translation = trs.getOrigin();
422 btMatrix3x3 rotation = trs.getBasis();
423
424 for (int i = 0; i < m_nodes.size(); ++i)
425 {
426 Node& n = m_nodes[i];
427 n.m_x = rotation * (n.m_x - CoM) + CoM + translation;
428 n.m_q = rotation * (n.m_q - CoM) + CoM + translation;
429 n.m_n = rotation * n.m_n;
430 vol = btDbvtVolume::FromCR(n.m_x, margin);
431
433 }
435 updateBounds();
437 }
438
439 // update modes
440 updateModesByRotation(trs.getBasis());
441
442 // update inertia tensor
446
447 // update rigid frame (No need to update the rotation. Nodes have already been updated.)
451
453}
454
456{
457 // Scaling the mesh after transform is applied is not allowed
459
460 // scale the mesh
461 {
462 const btScalar margin = getCollisionShape()->getMargin();
464 vol;
465
467
468 for (int i = 0; i < m_nodes.size(); ++i)
469 {
470 Node& n = m_nodes[i];
471 n.m_x = (n.m_x - CoM) * scl + CoM;
472 n.m_q = (n.m_q - CoM) * scl + CoM;
473 vol = btDbvtVolume::FromCR(n.m_x, margin);
475 }
477 updateBounds();
480 }
481
482 // update inertia tensor
484
485 btMatrix3x3 id;
486 id.setIdentity();
487 updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed
490
492}
493
495{
496 // Changing the total mass after transform is applied is not allowed
498
500
501 // update nodal mass
502 for (int i = 0; i < m_nFull; ++i)
503 {
505 }
506 m_mass = mass;
507 m_inverseMass = mass > 0 ? 1.0 / mass : 0;
508
509 // update inertia tensors
511
512 btMatrix3x3 id;
513 id.setIdentity();
514 updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed
517
519}
520
522{
523 // update reset nodal position
525 for (int i = 0; i < m_nFull; ++i)
526 {
527 m_x0[i] = m_nodes[i].m_x;
528 }
529}
530
531// reference notes:
532// https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-07-dynamics-fall-2009/lecture-notes/MIT16_07F09_Lec26.pdf
534{
537
538 for (int p = 0; p < m_nFull; ++p)
539 {
542
543 btVector3 r = m_nodes[p].m_x - m_initialCoM;
544
545 particle_inertia[0][0] = m_nodalMass[p] * (r[1] * r[1] + r[2] * r[2]);
546 particle_inertia[1][1] = m_nodalMass[p] * (r[0] * r[0] + r[2] * r[2]);
547 particle_inertia[2][2] = m_nodalMass[p] * (r[0] * r[0] + r[1] * r[1]);
548
549 particle_inertia[0][1] = - m_nodalMass[p] * (r[0] * r[1]);
550 particle_inertia[0][2] = - m_nodalMass[p] * (r[0] * r[2]);
551 particle_inertia[1][2] = - m_nodalMass[p] * (r[1] * r[2]);
552
556
558 }
560}
561
563{
564 // m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
566}
567
569{
570 for (int r = 0; r < m_nReduced; ++r)
571 {
572 for (int i = 0; i < m_nFull; ++i)
573 {
574 btVector3 nodal_disp(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
576
577 for (int k = 0; k < 3; ++k)
578 {
579 m_modes[r][3 * i + k] = nodal_disp[k];
580 }
581 }
582 }
583}
584
586{
588}
589
591{
594}
595
597{
599 #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
601 #endif
602}
603
605{
607 #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
609 #endif
610}
611
613{
614 if (m_inverseMass == btScalar(0.))
615 {
616 std::cout << "something went wrong...probably didn't initialize?\n";
617 btAssert(false);
618 }
619 // delta linear velocity
621 // delta angular velocity
624}
625
627{
630 return ri;
631}
632
634{
635 // relative position
639
640 // calculate impulse factor
641 // rigid part
645
646 // reduced deformable part
648 SA.setZero();
649 for (int i = 0; i < 3; ++i)
650 {
651 for (int j = 0; j < 3; ++j)
652 {
653 for (int r = 0; r < m_nReduced; ++r)
654 {
655 SA[i][j] += m_modes[r][3 * n_node + i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
656 }
657 }
658 }
660
661
662 TVStack omega_helper; // Sum_i m_i r*_i R S_i
664 for (int r = 0; r < m_nReduced; ++r)
665 {
666 omega_helper[r].setZero();
667 for (int i = 0; i < m_nFull; ++i)
668 {
670 btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
672 }
673 }
674
677 for (int i = 0; i < 3; ++i)
678 {
679 for (int j = 0; j < 3; ++j)
680 {
681 for (int r = 0; r < m_nReduced; ++r)
682 {
683 sum_multiply_A[i][j] += omega_helper[r][i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
684 }
685 }
686 }
687
689
690 return m_rigidOnly ? K1 : K1 + K2;
691}
692
694{
695 if (!m_rigidOnly)
696 {
697 // apply impulse force
699
700 // update delta damping force
703 for (int r = 0; r < m_nReduced; ++r)
704 {
706 }
708 // applyReducedDampingForce(m_internalDeltaReducedVelocity);
709
710 // delta reduced velocity
711 for (int r = 0; r < m_nReduced; ++r)
712 {
713 // The reduced mass is always identity!
715 }
716 }
717
719}
720
722{
723 // f_local = R^-1 * f_ext //TODO: interpoalted transfrom
724 // btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext;
726
727 // f_ext_r = [S^T * P]_{n_node} * f_local
730 for (int r = 0; r < m_nReduced; ++r)
731 {
733 for (int k = 0; k < 3; ++k)
734 {
735 f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k];
736 }
737
739 }
740}
741
743{
744 // update rigid frame velocity
746}
747
749{
750 for (int r = 0; r < m_nReduced; ++r)
751 {
753 }
754}
755
757{
758 for (int r = 0; r < m_nReduced; ++r)
759 {
761 }
762}
763
765{
766 return m_mass;
767}
768
770{
772}
773
775{
776 return m_linearVelocity;
777}
778
780{
781 return m_angularVelocity;
782}
783
785{
787}
788
790{
791 return m_rigidOnly;
792}
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition btScalar.h:99
#define btAssert(x)
Definition btScalar.h:153
static btMatrix3x3 Cross(const btVector3 &v)
static btMatrix3x3 Diagonal(btScalar x)
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void push_back(const T &_Val)
btTransform m_interpolationWorldTransform
m_interpolationWorldTransform is used for CCD and interpolation it can be either previous or future (...
const btCollisionShape * getCollisionShape() const
virtual btScalar getMargin() const =0
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:50
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btMatrix3x3 transpose() const
Return the transpose of the matrix.
void setIdentity()
Set the matrix to the identity.
void setZero()
Set the matrix to the identity.
void applyFullSpaceNodalForce(const btVector3 &f_ext, int n_node)
void predictIntegratedTransform(btScalar dt, btTransform &predictedTransform)
void applyTorqueImpulse(const btVector3 &torque)
const btVector3 computeNodeFullVelocity(const btTransform &ref_trans, int n_node) const
btVector3 getRelativePos(int n_node)
btAlignedObjectArray< int > m_fixedNodes
btReducedDeformableBody(btSoftBodyWorldInfo *worldInfo, int node_count, const btVector3 *x, const btScalar *m)
virtual void transform(const btTransform &trs)
void mapToFullVelocity(const btTransform &ref_trans)
const btVector3 & getAngularVelocity() const
void setRigidAngularVelocity(const btVector3 &omega)
void setReducedModes(int num_modes, int full_size)
void updateReducedVelocity(btScalar solverdt)
void disableReducedModes(const bool rigid_only)
const btVector3 & getLinearVelocity() const
void updateModesByRotation(const btMatrix3x3 &rotation)
virtual btMatrix3x3 getImpulseFactor(int n_node)
void setRigidVelocity(const btVector3 &v)
const btVector3 internalComputeNodeDeltaVelocity(const btTransform &ref_trans, int n_node) const
const btVector3 computeTotalAngularMomentum() const
void applyCentralImpulse(const btVector3 &impulse)
void updateExternalForceProjectMatrix(bool initialized)
void applyReducedElasticForce(const tDenseArray &reduce_dofs)
void internalApplyFullSpaceImpulse(const btVector3 &impulse, const btVector3 &rel_pos, int n_node, btScalar dt)
void applyReducedDampingForce(const tDenseArray &reduce_vel)
void setDamping(const btScalar alpha, const btScalar beta)
void setStiffnessScale(const btScalar ks)
void setMassScale(const btScalar rho)
void applyDamping(btScalar timeStep)
void updateInitialInertiaTensor(const btMatrix3x3 &rotation)
void internalApplyRigidImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
void setFixedNodes(const int n_node)
void setMassProps(const tDenseArray &mass_array)
virtual void transformTo(const btTransform &trs)
virtual void scale(const btVector3 &scl)
void applyRigidGravity(const btVector3 &gravity, btScalar dt)
void updateReducedDofs(btScalar solverdt)
void proceedToTransform(btScalar dt, bool end_of_time_step)
void mapToFullPosition(const btTransform &ref_trans)
virtual void setTotalMass(btScalar mass, bool fromfaces=false)
The btSoftBody is an class to simulate cloth and volumetric soft bodies.
Definition btSoftBody.h:75
bool m_reducedModel
Definition btSoftBody.h:861
void updateNormals()
tNodeArray m_nodes
Definition btSoftBody.h:814
void initializeDmInverse()
void updateConstants()
void updateBounds()
btDbvt m_ndbvt
Definition btSoftBody.h:835
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
void setIdentity()
Set this transformation to the identity.
btVector3 & getOrigin()
Return the origin vector translation.
void setOrigin(const btVector3 &origin)
Set the translational element.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition btVector3.h:380
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition btVector3.h:640
void setZero()
Definition btVector3.h:671
static btDbvtAabbMm FromCR(const btVector3 &c, btScalar r)
Definition btDbvt.h:473
void update(btDbvtNode *leaf, int lookahead=-1)
Definition btDbvt.cpp:544
btDbvtNode * m_leaf
Definition btSoftBody.h:277