dali-physics/third-party/bullet3/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp

   1 #include "btReducedDeformableBody.h"
   2 #include "../btSoftBodyInternals.h"
   3 #include "btReducedDeformableBodyHelpers.h"
   4 #include "LinearMath/btTransformUtil.h"
   5 #include <iostream>
   6 #include <fstream>
   7
   8 btReducedDeformableBody::btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
   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;
  15   m_nodeIndexOffset = 0;
  16
  17   m_transform_lock = false;
  18   m_ksScale = 1.0;
  19   m_rhoScale = 1.0;
  20
  21   // rigid motion
  22   m_linearVelocity.setZero();
  23         m_angularVelocity.setZero();
  24   m_internalDeltaLinearVelocity.setZero();
  25   m_internalDeltaAngularVelocity.setZero();
  26   m_angularVelocityFromReduced.setZero();
  27   m_internalDeltaAngularVelocityFromReduced.setZero();
  28         m_angularFactor.setValue(1, 1, 1);
  29         m_linearFactor.setValue(1, 1, 1);
  30   // m_invInertiaLocal.setValue(1, 1, 1);
  31   m_invInertiaLocal.setIdentity();
  32   m_mass = 0.0;
  33   m_inverseMass = 0.0;
  34
  35   m_linearDamping = 0;
  36   m_angularDamping = 0;
  37
  38   // Rayleigh damping
  39   m_dampingAlpha = 0;
  40   m_dampingBeta = 0;
  41
  42   m_rigidTransformWorld.setIdentity();
  43 }
  44
  45 void btReducedDeformableBody::setReducedModes(int num_modes, int full_size)
  46 {
  47   m_nReduced = num_modes;
  48   m_nFull = full_size;
  49   m_reducedDofs.resize(m_nReduced, 0);
  50   m_reducedDofsBuffer.resize(m_nReduced, 0);
  51   m_reducedVelocity.resize(m_nReduced, 0);
  52   m_reducedVelocityBuffer.resize(m_nReduced, 0);
  53   m_reducedForceElastic.resize(m_nReduced, 0);
  54   m_reducedForceDamping.resize(m_nReduced, 0);
  55   m_reducedForceExternal.resize(m_nReduced, 0);
  56   m_internalDeltaReducedVelocity.resize(m_nReduced, 0);
  57   m_nodalMass.resize(full_size, 0);
  58   m_localMomentArm.resize(m_nFull);
  59 }
  60
  61 void btReducedDeformableBody::setMassProps(const tDenseArray& mass_array)
  62 {
  63   btScalar total_mass = 0;
  64   btVector3 CoM(0, 0, 0);
  65         for (int i = 0; i < m_nFull; ++i)
  66         {
  67                 m_nodalMass[i] = m_rhoScale * mass_array[i];
  68                 m_nodes[i].m_im = mass_array[i] > 0 ? 1.0 / (m_rhoScale * mass_array[i]) : 0;
  69                 total_mass += m_rhoScale * mass_array[i];
  70
  71     CoM += m_nodalMass[i] * m_nodes[i].m_x;
  72         }
  73   // total rigid body mass
  74   m_mass = total_mass;
  75   m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0;
  76   // original CoM
  77   m_initialCoM = CoM / total_mass;
  78 }
  79
  80 void btReducedDeformableBody::setInertiaProps()
  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();
  88   m_rigidTransformWorld.setOrigin(m_initialCoM);
  89   m_interpolationWorldTransform = m_rigidTransformWorld;
  90
  91   updateLocalInertiaTensorFromNodes();
  92
  93   // update world inertia tensor
  94   btMatrix3x3 rotation;
  95   rotation.setIdentity();
  96   updateInitialInertiaTensor(rotation);
  97   updateInertiaTensor();
  98   m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
  99 }
 100
 101 void btReducedDeformableBody::setRigidVelocity(const btVector3& v)
 102 {
 103   m_linearVelocity = v;
 104 }
 105
 106 void btReducedDeformableBody::setRigidAngularVelocity(const btVector3& omega)
 107 {
 108   m_angularVelocity = omega;
 109 }
 110
 111 void btReducedDeformableBody::setStiffnessScale(const btScalar ks)
 112 {
 113   m_ksScale = ks;
 114 }
 115
 116 void btReducedDeformableBody::setMassScale(const btScalar rho)
 117 {
 118   m_rhoScale = rho;
 119 }
 120
 121 void btReducedDeformableBody::setFixedNodes(const int n_node)
 122 {
 123   m_fixedNodes.push_back(n_node);
 124   m_nodes[n_node].m_im = 0;   // set inverse mass to be zero for the constraint solver.
 125 }
 126
 127 void btReducedDeformableBody::setDamping(const btScalar alpha, const btScalar beta)
 128 {
 129   m_dampingAlpha = alpha;
 130   m_dampingBeta = beta;
 131 }
 132
 133 void btReducedDeformableBody::internalInitialization()
 134 {
 135   // zeroing
 136   endOfTimeStepZeroing();
 137   // initialize rest position
 138   updateRestNodalPositions();
 139   // initialize local nodal moment arm form the CoM
 140   updateLocalMomentArm();
 141   // initialize projection matrix
 142   updateExternalForceProjectMatrix(false);
 143 }
 144
 145 void btReducedDeformableBody::updateLocalMomentArm()
 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
 166 void btReducedDeformableBody::updateExternalForceProjectMatrix(bool initialized)
 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
 173     m_projPA.resize(m_nReduced);
 174     m_projCq.resize(m_nReduced);
 175
 176     m_STP.resize(m_nReduced);
 177     m_MrInvSTP.resize(m_nReduced);
 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       {
 185         btMatrix3x3 mass_scaled_i = Diagonal(1) - Diagonal(m_nodalMass[i] / m_mass);
 186         btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
 187         btVector3 prod_i = mass_scaled_i * s_ri;
 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     {
 206       btMatrix3x3 r_star = Cross(m_localMomentArm[i]);
 207       btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
 208       btVector3 prod_i = r_star * m_invInertiaTensorWorld * r_star * s_ri;
 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
 219 void btReducedDeformableBody::endOfTimeStepZeroing()
 220 {
 221   for (int i = 0; i < m_nReduced; ++i)
 222   {
 223     m_reducedForceElastic[i] = 0;
 224     m_reducedForceDamping[i] = 0;
 225     m_reducedForceExternal[i] = 0;
 226     m_internalDeltaReducedVelocity[i] = 0;
 227     m_reducedDofsBuffer[i] = m_reducedDofs[i];
 228     m_reducedVelocityBuffer[i] = m_reducedVelocity[i];
 229   }
 230   // std::cout << "zeroed!\n";
 231 }
 232
 233 void btReducedDeformableBody::applyInternalVelocityChanges()
 234 {
 235   m_linearVelocity += m_internalDeltaLinearVelocity;
 236   m_angularVelocity += m_internalDeltaAngularVelocity;
 237   m_internalDeltaLinearVelocity.setZero();
 238   m_internalDeltaAngularVelocity.setZero();
 239   for (int r = 0; r < m_nReduced; ++r)
 240   {
 241     m_reducedVelocity[r] += m_internalDeltaReducedVelocity[r];
 242     m_internalDeltaReducedVelocity[r] = 0;
 243   }
 244 }
 245
 246 void btReducedDeformableBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform)
 247 {
 248         btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform);
 249 }
 250
 251 void btReducedDeformableBody::updateReducedDofs(btScalar solverdt)
 252 {
 253   for (int r = 0; r < m_nReduced; ++r)
 254   {
 255     m_reducedDofs[r] = m_reducedDofsBuffer[r] + solverdt * m_reducedVelocity[r];
 256   }
 257 }
 258
 259 void btReducedDeformableBody::mapToFullPosition(const btTransform& ref_trans)
 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
 272 void btReducedDeformableBody::updateReducedVelocity(btScalar solverdt)
 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;
 279     delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r]);
 280     // delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]);
 281     m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v;
 282   }
 283 }
 284
 285 void btReducedDeformableBody::mapToFullVelocity(const btTransform& ref_trans)
 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   {
 323     m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
 324   }
 325 }
 326
 327 const btVector3 btReducedDeformableBody::computeTotalAngularMomentum() const
 328 {
 329   btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity;
 330   btVector3 L_reduced(0, 0, 0);
 331   btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced);
 332
 333   for (int i = 0; i < m_nFull; ++i)
 334   {
 335     btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i];
 336     btMatrix3x3 r_star = Cross(r_com);
 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
 347     L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com));
 348     // L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced));
 349   }
 350   return L_rigid + L_reduced;
 351 }
 352
 353 const btVector3 btReducedDeformableBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const
 354 {
 355   btVector3 v_from_reduced(0, 0, 0);
 356   btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
 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     {
 362       v_from_reduced[k] += m_modes[r][3 * n_node + k] * m_reducedVelocity[r];
 363     }
 364   }
 365   // get new velocity
 366   btVector3 vel = m_angularVelocity.cross(r_com) +
 367                   ref_trans.getBasis() * v_from_reduced +
 368                   m_linearVelocity;
 369   return vel;
 370 }
 371
 372 const btVector3 btReducedDeformableBody::internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const
 373 {
 374   btVector3 deltaV_from_reduced(0, 0, 0);
 375   btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
 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     {
 382       deltaV_from_reduced[k] += m_modes[r][3 * n_node + k] * m_internalDeltaReducedVelocity[r];
 383     }
 384   }
 385
 386   // get delta velocity
 387   btVector3 deltaV = m_internalDeltaAngularVelocity.cross(r_com) +
 388                      ref_trans.getBasis() * deltaV_from_reduced +
 389                      m_internalDeltaLinearVelocity;
 390   return deltaV;
 391 }
 392
 393 void btReducedDeformableBody::proceedToTransform(btScalar dt, bool end_of_time_step)
 394 {
 395   btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform);
 396   updateInertiaTensor();
 397   // m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose();
 398   m_rigidTransformWorld = m_interpolationWorldTransform;
 399   m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld;
 400 }
 401
 402 void btReducedDeformableBody::transformTo(const btTransform& trs)
 403 {
 404         btTransform current_transform = getRigidTransform();
 405         btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(),
 406                             trs.getOrigin() - current_transform.getOrigin());
 407   transform(new_transform);
 408 }
 409
 410 void btReducedDeformableBody::transform(const btTransform& trs)
 411 {
 412   m_transform_lock = true;
 413
 414   // transform mesh
 415   {
 416     const btScalar margin = getCollisionShape()->getMargin();
 417     ATTRIBUTE_ALIGNED16(btDbvtVolume)
 418     vol;
 419
 420     btVector3 CoM = m_rigidTransformWorld.getOrigin();
 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
 432       m_ndbvt.update(n.m_leaf, vol);
 433     }
 434     updateNormals();
 435     updateBounds();
 436     updateConstants();
 437   }
 438
 439   // update modes
 440   updateModesByRotation(trs.getBasis());
 441
 442   // update inertia tensor
 443   updateInitialInertiaTensor(trs.getBasis());
 444   updateInertiaTensor();
 445   m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
 446
 447   // update rigid frame (No need to update the rotation. Nodes have already been updated.)
 448   m_rigidTransformWorld.setOrigin(m_initialCoM + trs.getOrigin());
 449   m_interpolationWorldTransform = m_rigidTransformWorld;
 450   m_initialCoM = m_rigidTransformWorld.getOrigin();
 451
 452   internalInitialization();
 453 }
 454
 455 void btReducedDeformableBody::scale(const btVector3& scl)
 456 {
 457   // Scaling the mesh after transform is applied is not allowed
 458   btAssert(!m_transform_lock);
 459
 460   // scale the mesh
 461   {
 462     const btScalar margin = getCollisionShape()->getMargin();
 463     ATTRIBUTE_ALIGNED16(btDbvtVolume)
 464     vol;
 465
 466     btVector3 CoM = m_rigidTransformWorld.getOrigin();
 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);
 474       m_ndbvt.update(n.m_leaf, vol);
 475     }
 476     updateNormals();
 477     updateBounds();
 478     updateConstants();
 479     initializeDmInverse();
 480   }
 481
 482   // update inertia tensor
 483   updateLocalInertiaTensorFromNodes();
 484
 485   btMatrix3x3 id;
 486   id.setIdentity();
 487   updateInitialInertiaTensor(id);   // there is no rotation, but the local inertia tensor has changed
 488   updateInertiaTensor();
 489   m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
 490
 491   internalInitialization();
 492 }
 493
 494 void btReducedDeformableBody::setTotalMass(btScalar mass, bool fromfaces)
 495 {
 496   // Changing the total mass after transform is applied is not allowed
 497   btAssert(!m_transform_lock);
 498
 499   btScalar scale_ratio = mass / m_mass;
 500
 501   // update nodal mass
 502   for (int i = 0; i < m_nFull; ++i)
 503   {
 504     m_nodalMass[i] *= scale_ratio;
 505   }
 506   m_mass = mass;
 507   m_inverseMass = mass > 0 ? 1.0 / mass : 0;
 508
 509   // update inertia tensors
 510   updateLocalInertiaTensorFromNodes();
 511
 512   btMatrix3x3 id;
 513   id.setIdentity();
 514   updateInitialInertiaTensor(id);   // there is no rotation, but the local inertia tensor has changed
 515   updateInertiaTensor();
 516   m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
 517
 518   internalInitialization();
 519 }
 520
 521 void btReducedDeformableBody::updateRestNodalPositions()
 522 {
 523   // update reset nodal position
 524   m_x0.resize(m_nFull);
 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
 533 void btReducedDeformableBody::updateLocalInertiaTensorFromNodes()
 534 {
 535   btMatrix3x3 inertia_tensor;
 536   inertia_tensor.setZero();
 537
 538   for (int p = 0; p < m_nFull; ++p)
 539   {
 540     btMatrix3x3 particle_inertia;
 541     particle_inertia.setZero();
 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
 553     particle_inertia[1][0] = particle_inertia[0][1];
 554     particle_inertia[2][0] = particle_inertia[0][2];
 555     particle_inertia[2][1] = particle_inertia[1][2];
 556
 557     inertia_tensor += particle_inertia;
 558   }
 559   m_invInertiaLocal = inertia_tensor.inverse();
 560 }
 561
 562 void btReducedDeformableBody::updateInitialInertiaTensor(const btMatrix3x3& rotation)
 563 {
 564   // m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
 565   m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose();
 566 }
 567
 568 void btReducedDeformableBody::updateModesByRotation(const btMatrix3x3& rotation)
 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]);
 575       nodal_disp = rotation * nodal_disp;
 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
 585 void btReducedDeformableBody::updateInertiaTensor()
 586 {
 587         m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose();
 588 }
 589
 590 void btReducedDeformableBody::applyDamping(btScalar timeStep)
 591 {
 592   m_linearVelocity *= btScalar(1) - m_linearDamping;
 593   m_angularDamping *= btScalar(1) - m_angularDamping;
 594 }
 595
 596 void btReducedDeformableBody::applyCentralImpulse(const btVector3& impulse)
 597 {
 598   m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
 599   #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
 600   clampVelocity(m_linearVelocity);
 601   #endif
 602 }
 603
 604 void btReducedDeformableBody::applyTorqueImpulse(const btVector3& torque)
 605 {
 606   m_angularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
 607   #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
 608   clampVelocity(m_angularVelocity);
 609   #endif
 610 }
 611
 612 void btReducedDeformableBody::internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos)
 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
 620   m_internalDeltaLinearVelocity += impulse * m_linearFactor * m_inverseMass;
 621   // delta angular velocity
 622   btVector3 torque = rel_pos.cross(impulse * m_linearFactor);
 623   m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
 624 }
 625
 626 btVector3 btReducedDeformableBody::getRelativePos(int n_node)
 627 {
 628   btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
 629   btVector3 ri = rotation * m_localMomentArm[n_node];
 630   return ri;
 631 }
 632
 633 btMatrix3x3 btReducedDeformableBody::getImpulseFactor(int n_node)
 634 {
 635   // relative position
 636   btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
 637   btVector3 ri = rotation * m_localMomentArm[n_node];
 638   btMatrix3x3 ri_skew = Cross(ri);
 639
 640   // calculate impulse factor
 641   // rigid part
 642   btScalar inv_mass = m_nodalMass[n_node] > btScalar(0) ? btScalar(1) / m_mass : btScalar(0);
 643   btMatrix3x3 K1 = Diagonal(inv_mass);
 644   K1 -= ri_skew * m_interpolateInvInertiaTensorWorld * ri_skew;
 645
 646   // reduced deformable part
 647   btMatrix3x3 SA;
 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   }
 659   btMatrix3x3 RSARinv = rotation * SA * rotation.transpose();
 660
 661
 662   TVStack omega_helper; // Sum_i m_i r*_i R S_i
 663   omega_helper.resize(m_nReduced);
 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     {
 669       btMatrix3x3 mi_rstar_i = rotation * Cross(m_localMomentArm[i]) * m_nodalMass[i];
 670       btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
 671       omega_helper[r] += mi_rstar_i * rotation * s_ri;
 672     }
 673   }
 674
 675   btMatrix3x3 sum_multiply_A;
 676   sum_multiply_A.setZero();
 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
 688   btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose();
 689
 690   return m_rigidOnly ? K1 : K1 + K2;
 691 }
 692
 693 void btReducedDeformableBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
 694 {
 695   if (!m_rigidOnly)
 696   {
 697     // apply impulse force
 698     applyFullSpaceNodalForce(impulse / dt, n_node);
 699
 700     // update delta damping force
 701     tDenseArray reduced_vel_tmp;
 702     reduced_vel_tmp.resize(m_nReduced);
 703     for (int r = 0; r < m_nReduced; ++r)
 704     {
 705       reduced_vel_tmp[r] = m_reducedVelocity[r] + m_internalDeltaReducedVelocity[r];
 706     }
 707     applyReducedDampingForce(reduced_vel_tmp);
 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!
 714       m_internalDeltaReducedVelocity[r] += dt * (m_reducedForceDamping[r] + m_reducedForceExternal[r]);
 715     }
 716   }
 717
 718   internalApplyRigidImpulse(impulse, rel_pos);
 719 }
 720
 721 void btReducedDeformableBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node)
 722 {
 723   // f_local = R^-1 * f_ext //TODO: interpoalted transfrom
 724   // btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext;
 725   btVector3 f_local = m_interpolationWorldTransform.getBasis().transpose() * f_ext;
 726
 727   // f_ext_r = [S^T * P]_{n_node} * f_local
 728   tDenseArray f_ext_r;
 729   f_ext_r.resize(m_nReduced, 0);
 730   for (int r = 0; r < m_nReduced; ++r)
 731   {
 732     m_reducedForceExternal[r] = 0;
 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
 738     m_reducedForceExternal[r] += f_ext_r[r];
 739   }
 740 }
 741
 742 void btReducedDeformableBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
 743 {
 744   // update rigid frame velocity
 745   m_linearVelocity += dt * gravity;
 746 }
 747
 748 void btReducedDeformableBody::applyReducedElasticForce(const tDenseArray& reduce_dofs)
 749 {
 750   for (int r = 0; r < m_nReduced; ++r)
 751   {
 752     m_reducedForceElastic[r] = - m_ksScale * m_Kr[r] * reduce_dofs[r];
 753   }
 754 }
 755
 756 void btReducedDeformableBody::applyReducedDampingForce(const tDenseArray& reduce_vel)
 757 {
 758   for (int r = 0; r < m_nReduced; ++r)
 759   {
 760     m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r];
 761   }
 762 }
 763
 764 btScalar btReducedDeformableBody::getTotalMass() const
 765 {
 766   return m_mass;
 767 }
 768
 769 btTransform& btReducedDeformableBody::getRigidTransform()
 770 {
 771   return m_rigidTransformWorld;
 772 }
 773
 774 const btVector3& btReducedDeformableBody::getLinearVelocity() const
 775 {
 776   return m_linearVelocity;
 777 }
 778
 779 const btVector3& btReducedDeformableBody::getAngularVelocity() const
 780 {
 781   return m_angularVelocity;
 782 }
 783
 784 void btReducedDeformableBody::disableReducedModes(const bool rigid_only)
 785 {
 786   m_rigidOnly = rigid_only;
 787 }
 788
 789 bool btReducedDeformableBody::isReducedModesOFF() const
 790 {
 791   return m_rigidOnly;
 792 }