diff options
Diffstat (limited to 'src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp')
| -rw-r--r-- | src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp | 792 |
1 files changed, 792 insertions, 0 deletions
diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp new file mode 100644 index 000000000..feb30d587 --- /dev/null +++ b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp @@ -0,0 +1,792 @@ +#include "btReducedDeformableBody.h" +#include "../btSoftBodyInternals.h" +#include "btReducedDeformableBodyHelpers.h" +#include "LinearMath/btTransformUtil.h" +#include <iostream> +#include <fstream> + +btReducedDeformableBody::btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) + : btSoftBody(worldInfo, node_count, x, m), m_rigidOnly(false) +{ + // reduced deformable + m_reducedModel = true; + m_nReduced = 0; + m_nFull = 0; + m_nodeIndexOffset = 0; + + m_transform_lock = false; + m_ksScale = 1.0; + m_rhoScale = 1.0; + + // rigid motion + m_linearVelocity.setZero(); + m_angularVelocity.setZero(); + m_internalDeltaLinearVelocity.setZero(); + m_internalDeltaAngularVelocity.setZero(); + m_angularVelocityFromReduced.setZero(); + m_internalDeltaAngularVelocityFromReduced.setZero(); + m_angularFactor.setValue(1, 1, 1); + m_linearFactor.setValue(1, 1, 1); + // m_invInertiaLocal.setValue(1, 1, 1); + m_invInertiaLocal.setIdentity(); + m_mass = 0.0; + m_inverseMass = 0.0; + + m_linearDamping = 0; + m_angularDamping = 0; + + // Rayleigh damping + m_dampingAlpha = 0; + m_dampingBeta = 0; + + m_rigidTransformWorld.setIdentity(); +} + +void btReducedDeformableBody::setReducedModes(int num_modes, int full_size) +{ + m_nReduced = num_modes; + m_nFull = full_size; + m_reducedDofs.resize(m_nReduced, 0); + m_reducedDofsBuffer.resize(m_nReduced, 0); + m_reducedVelocity.resize(m_nReduced, 0); + m_reducedVelocityBuffer.resize(m_nReduced, 0); + m_reducedForceElastic.resize(m_nReduced, 0); + m_reducedForceDamping.resize(m_nReduced, 0); + m_reducedForceExternal.resize(m_nReduced, 0); + m_internalDeltaReducedVelocity.resize(m_nReduced, 0); + m_nodalMass.resize(full_size, 0); + m_localMomentArm.resize(m_nFull); +} + +void btReducedDeformableBody::setMassProps(const tDenseArray& mass_array) +{ + btScalar total_mass = 0; + btVector3 CoM(0, 0, 0); + for (int i = 0; i < m_nFull; ++i) + { + m_nodalMass[i] = m_rhoScale * mass_array[i]; + m_nodes[i].m_im = mass_array[i] > 0 ? 1.0 / (m_rhoScale * mass_array[i]) : 0; + total_mass += m_rhoScale * mass_array[i]; + + CoM += m_nodalMass[i] * m_nodes[i].m_x; + } + // total rigid body mass + m_mass = total_mass; + m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0; + // original CoM + m_initialCoM = CoM / total_mass; +} + +void btReducedDeformableBody::setInertiaProps() +{ + // make sure the initial CoM is at the origin (0,0,0) + // for (int i = 0; i < m_nFull; ++i) + // { + // m_nodes[i].m_x -= m_initialCoM; + // } + // m_initialCoM.setZero(); + m_rigidTransformWorld.setOrigin(m_initialCoM); + m_interpolationWorldTransform = m_rigidTransformWorld; + + updateLocalInertiaTensorFromNodes(); + + // update world inertia tensor + btMatrix3x3 rotation; + rotation.setIdentity(); + updateInitialInertiaTensor(rotation); + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; +} + +void btReducedDeformableBody::setRigidVelocity(const btVector3& v) +{ + m_linearVelocity = v; +} + +void btReducedDeformableBody::setRigidAngularVelocity(const btVector3& omega) +{ + m_angularVelocity = omega; +} + +void btReducedDeformableBody::setStiffnessScale(const btScalar ks) +{ + m_ksScale = ks; +} + +void btReducedDeformableBody::setMassScale(const btScalar rho) +{ + m_rhoScale = rho; +} + +void btReducedDeformableBody::setFixedNodes(const int n_node) +{ + m_fixedNodes.push_back(n_node); + m_nodes[n_node].m_im = 0; // set inverse mass to be zero for the constraint solver. +} + +void btReducedDeformableBody::setDamping(const btScalar alpha, const btScalar beta) +{ + m_dampingAlpha = alpha; + m_dampingBeta = beta; +} + +void btReducedDeformableBody::internalInitialization() +{ + // zeroing + endOfTimeStepZeroing(); + // initialize rest position + updateRestNodalPositions(); + // initialize local nodal moment arm form the CoM + updateLocalMomentArm(); + // initialize projection matrix + updateExternalForceProjectMatrix(false); +} + +void btReducedDeformableBody::updateLocalMomentArm() +{ + TVStack delta_x; + delta_x.resize(m_nFull); + + for (int i = 0; i < m_nFull; ++i) + { + for (int k = 0; k < 3; ++k) + { + // compute displacement + delta_x[i][k] = 0; + for (int j = 0; j < m_nReduced; ++j) + { + delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j]; + } + } + // get new moment arm Sq + x0 + m_localMomentArm[i] = m_x0[i] - m_initialCoM + delta_x[i]; + } +} + +void btReducedDeformableBody::updateExternalForceProjectMatrix(bool initialized) +{ + // if not initialized, need to compute both P_A and Cq + // otherwise, only need to udpate Cq + if (!initialized) + { + // resize + m_projPA.resize(m_nReduced); + m_projCq.resize(m_nReduced); + + m_STP.resize(m_nReduced); + m_MrInvSTP.resize(m_nReduced); + + // P_A + for (int r = 0; r < m_nReduced; ++r) + { + m_projPA[r].resize(3 * m_nFull, 0); + for (int i = 0; i < m_nFull; ++i) + { + btMatrix3x3 mass_scaled_i = Diagonal(1) - Diagonal(m_nodalMass[i] / m_mass); + btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + btVector3 prod_i = mass_scaled_i * s_ri; + + for (int k = 0; k < 3; ++k) + m_projPA[r][3 * i + k] = prod_i[k]; + + // btScalar ratio = m_nodalMass[i] / m_mass; + // m_projPA[r] += btVector3(- m_modes[r][3 * i] * ratio, + // - m_modes[r][3 * i + 1] * ratio, + // - m_modes[r][3 * i + 2] * ratio); + } + } + } + + // C(q) is updated once per position update + for (int r = 0; r < m_nReduced; ++r) + { + m_projCq[r].resize(3 * m_nFull, 0); + for (int i = 0; i < m_nFull; ++i) + { + btMatrix3x3 r_star = Cross(m_localMomentArm[i]); + btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + btVector3 prod_i = r_star * m_invInertiaTensorWorld * r_star * s_ri; + + for (int k = 0; k < 3; ++k) + m_projCq[r][3 * i + k] = m_nodalMass[i] * prod_i[k]; + + // btVector3 si(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + // m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]); + } + } +} + +void btReducedDeformableBody::endOfTimeStepZeroing() +{ + for (int i = 0; i < m_nReduced; ++i) + { + m_reducedForceElastic[i] = 0; + m_reducedForceDamping[i] = 0; + m_reducedForceExternal[i] = 0; + m_internalDeltaReducedVelocity[i] = 0; + m_reducedDofsBuffer[i] = m_reducedDofs[i]; + m_reducedVelocityBuffer[i] = m_reducedVelocity[i]; + } + // std::cout << "zeroed!\n"; +} + +void btReducedDeformableBody::applyInternalVelocityChanges() +{ + m_linearVelocity += m_internalDeltaLinearVelocity; + m_angularVelocity += m_internalDeltaAngularVelocity; + m_internalDeltaLinearVelocity.setZero(); + m_internalDeltaAngularVelocity.setZero(); + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedVelocity[r] += m_internalDeltaReducedVelocity[r]; + m_internalDeltaReducedVelocity[r] = 0; + } +} + +void btReducedDeformableBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform) +{ + btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform); +} + +void btReducedDeformableBody::updateReducedDofs(btScalar solverdt) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedDofs[r] = m_reducedDofsBuffer[r] + solverdt * m_reducedVelocity[r]; + } +} + +void btReducedDeformableBody::mapToFullPosition(const btTransform& ref_trans) +{ + btVector3 origin = ref_trans.getOrigin(); + btMatrix3x3 rotation = ref_trans.getBasis(); + + + for (int i = 0; i < m_nFull; ++i) + { + m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin; + m_nodes[i].m_q = m_nodes[i].m_x; + } +} + +void btReducedDeformableBody::updateReducedVelocity(btScalar solverdt) +{ + // update reduced velocity + for (int r = 0; r < m_nReduced; ++r) + { + // the reduced mass is always identity! + btScalar delta_v = 0; + delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r]); + // delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]); + m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v; + } +} + +void btReducedDeformableBody::mapToFullVelocity(const btTransform& ref_trans) +{ + // compute the reduced contribution to the angular velocity + // btVector3 sum_linear(0, 0, 0); + // btVector3 sum_angular(0, 0, 0); + // m_linearVelocityFromReduced.setZero(); + // m_angularVelocityFromReduced.setZero(); + // for (int i = 0; i < m_nFull; ++i) + // { + // btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i]; + // btMatrix3x3 r_star = Cross(r_com); + + // btVector3 v_from_reduced(0, 0, 0); + // for (int k = 0; k < 3; ++k) + // { + // for (int r = 0; r < m_nReduced; ++r) + // { + // v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r]; + // } + // } + + // btVector3 delta_linear = m_nodalMass[i] * v_from_reduced; + // btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced); + // sum_linear += delta_linear; + // sum_angular += delta_angular; + // // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n"; + // // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n"; + // // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n"; + // // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n"; + // } + // m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear); + // m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular; + + // m_linearVelocity -= m_linearVelocityFromReduced; + // m_angularVelocity -= m_angularVelocityFromReduced; + + for (int i = 0; i < m_nFull; ++i) + { + m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i); + } +} + +const btVector3 btReducedDeformableBody::computeTotalAngularMomentum() const +{ + btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity; + btVector3 L_reduced(0, 0, 0); + btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced); + + for (int i = 0; i < m_nFull; ++i) + { + btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i]; + btMatrix3x3 r_star = Cross(r_com); + + btVector3 v_from_reduced(0, 0, 0); + for (int k = 0; k < 3; ++k) + { + for (int r = 0; r < m_nReduced; ++r) + { + v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r]; + } + } + + L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com)); + // L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced)); + } + return L_rigid + L_reduced; +} + +const btVector3 btReducedDeformableBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const +{ + btVector3 v_from_reduced(0, 0, 0); + btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node]; + // compute velocity contributed by the reduced velocity + for (int k = 0; k < 3; ++k) + { + for (int r = 0; r < m_nReduced; ++r) + { + v_from_reduced[k] += m_modes[r][3 * n_node + k] * m_reducedVelocity[r]; + } + } + // get new velocity + btVector3 vel = m_angularVelocity.cross(r_com) + + ref_trans.getBasis() * v_from_reduced + + m_linearVelocity; + return vel; +} + +const btVector3 btReducedDeformableBody::internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const +{ + btVector3 deltaV_from_reduced(0, 0, 0); + btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node]; + + // compute velocity contributed by the reduced velocity + for (int k = 0; k < 3; ++k) + { + for (int r = 0; r < m_nReduced; ++r) + { + deltaV_from_reduced[k] += m_modes[r][3 * n_node + k] * m_internalDeltaReducedVelocity[r]; + } + } + + // get delta velocity + btVector3 deltaV = m_internalDeltaAngularVelocity.cross(r_com) + + ref_trans.getBasis() * deltaV_from_reduced + + m_internalDeltaLinearVelocity; + return deltaV; +} + +void btReducedDeformableBody::proceedToTransform(btScalar dt, bool end_of_time_step) +{ + btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform); + updateInertiaTensor(); + // m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose(); + m_rigidTransformWorld = m_interpolationWorldTransform; + m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld; +} + +void btReducedDeformableBody::transformTo(const btTransform& trs) +{ + btTransform current_transform = getRigidTransform(); + btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(), + trs.getOrigin() - current_transform.getOrigin()); + transform(new_transform); +} + +void btReducedDeformableBody::transform(const btTransform& trs) +{ + m_transform_lock = true; + + // transform mesh + { + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + + btVector3 CoM = m_rigidTransformWorld.getOrigin(); + btVector3 translation = trs.getOrigin(); + btMatrix3x3 rotation = trs.getBasis(); + + for (int i = 0; i < m_nodes.size(); ++i) + { + Node& n = m_nodes[i]; + n.m_x = rotation * (n.m_x - CoM) + CoM + translation; + n.m_q = rotation * (n.m_q - CoM) + CoM + translation; + n.m_n = rotation * n.m_n; + vol = btDbvtVolume::FromCR(n.m_x, margin); + + m_ndbvt.update(n.m_leaf, vol); + } + updateNormals(); + updateBounds(); + updateConstants(); + } + + // update modes + updateModesByRotation(trs.getBasis()); + + // update inertia tensor + updateInitialInertiaTensor(trs.getBasis()); + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; + + // update rigid frame (No need to update the rotation. Nodes have already been updated.) + m_rigidTransformWorld.setOrigin(m_initialCoM + trs.getOrigin()); + m_interpolationWorldTransform = m_rigidTransformWorld; + m_initialCoM = m_rigidTransformWorld.getOrigin(); + + internalInitialization(); +} + +void btReducedDeformableBody::scale(const btVector3& scl) +{ + // Scaling the mesh after transform is applied is not allowed + btAssert(!m_transform_lock); + + // scale the mesh + { + const btScalar margin = getCollisionShape()->getMargin(); + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + + btVector3 CoM = m_rigidTransformWorld.getOrigin(); + + for (int i = 0; i < m_nodes.size(); ++i) + { + Node& n = m_nodes[i]; + n.m_x = (n.m_x - CoM) * scl + CoM; + n.m_q = (n.m_q - CoM) * scl + CoM; + vol = btDbvtVolume::FromCR(n.m_x, margin); + m_ndbvt.update(n.m_leaf, vol); + } + updateNormals(); + updateBounds(); + updateConstants(); + initializeDmInverse(); + } + + // update inertia tensor + updateLocalInertiaTensorFromNodes(); + + btMatrix3x3 id; + id.setIdentity(); + updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; + + internalInitialization(); +} + +void btReducedDeformableBody::setTotalMass(btScalar mass, bool fromfaces) +{ + // Changing the total mass after transform is applied is not allowed + btAssert(!m_transform_lock); + + btScalar scale_ratio = mass / m_mass; + + // update nodal mass + for (int i = 0; i < m_nFull; ++i) + { + m_nodalMass[i] *= scale_ratio; + } + m_mass = mass; + m_inverseMass = mass > 0 ? 1.0 / mass : 0; + + // update inertia tensors + updateLocalInertiaTensorFromNodes(); + + btMatrix3x3 id; + id.setIdentity(); + updateInitialInertiaTensor(id); // there is no rotation, but the local inertia tensor has changed + updateInertiaTensor(); + m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; + + internalInitialization(); +} + +void btReducedDeformableBody::updateRestNodalPositions() +{ + // update reset nodal position + m_x0.resize(m_nFull); + for (int i = 0; i < m_nFull; ++i) + { + m_x0[i] = m_nodes[i].m_x; + } +} + +// reference notes: +// https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-07-dynamics-fall-2009/lecture-notes/MIT16_07F09_Lec26.pdf +void btReducedDeformableBody::updateLocalInertiaTensorFromNodes() +{ + btMatrix3x3 inertia_tensor; + inertia_tensor.setZero(); + + for (int p = 0; p < m_nFull; ++p) + { + btMatrix3x3 particle_inertia; + particle_inertia.setZero(); + + btVector3 r = m_nodes[p].m_x - m_initialCoM; + + particle_inertia[0][0] = m_nodalMass[p] * (r[1] * r[1] + r[2] * r[2]); + particle_inertia[1][1] = m_nodalMass[p] * (r[0] * r[0] + r[2] * r[2]); + particle_inertia[2][2] = m_nodalMass[p] * (r[0] * r[0] + r[1] * r[1]); + + particle_inertia[0][1] = - m_nodalMass[p] * (r[0] * r[1]); + particle_inertia[0][2] = - m_nodalMass[p] * (r[0] * r[2]); + particle_inertia[1][2] = - m_nodalMass[p] * (r[1] * r[2]); + + particle_inertia[1][0] = particle_inertia[0][1]; + particle_inertia[2][0] = particle_inertia[0][2]; + particle_inertia[2][1] = particle_inertia[1][2]; + + inertia_tensor += particle_inertia; + } + m_invInertiaLocal = inertia_tensor.inverse(); +} + +void btReducedDeformableBody::updateInitialInertiaTensor(const btMatrix3x3& rotation) +{ + // m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose(); + m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose(); +} + +void btReducedDeformableBody::updateModesByRotation(const btMatrix3x3& rotation) +{ + for (int r = 0; r < m_nReduced; ++r) + { + for (int i = 0; i < m_nFull; ++i) + { + btVector3 nodal_disp(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + nodal_disp = rotation * nodal_disp; + + for (int k = 0; k < 3; ++k) + { + m_modes[r][3 * i + k] = nodal_disp[k]; + } + } + } +} + +void btReducedDeformableBody::updateInertiaTensor() +{ + m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose(); +} + +void btReducedDeformableBody::applyDamping(btScalar timeStep) +{ + m_linearVelocity *= btScalar(1) - m_linearDamping; + m_angularDamping *= btScalar(1) - m_angularDamping; +} + +void btReducedDeformableBody::applyCentralImpulse(const btVector3& impulse) +{ + m_linearVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif +} + +void btReducedDeformableBody::applyTorqueImpulse(const btVector3& torque) +{ + m_angularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif +} + +void btReducedDeformableBody::internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos) +{ + if (m_inverseMass == btScalar(0.)) + { + std::cout << "something went wrong...probably didn't initialize?\n"; + btAssert(false); + } + // delta linear velocity + m_internalDeltaLinearVelocity += impulse * m_linearFactor * m_inverseMass; + // delta angular velocity + btVector3 torque = rel_pos.cross(impulse * m_linearFactor); + m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor; +} + +btVector3 btReducedDeformableBody::getRelativePos(int n_node) +{ + btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis(); + btVector3 ri = rotation * m_localMomentArm[n_node]; + return ri; +} + +btMatrix3x3 btReducedDeformableBody::getImpulseFactor(int n_node) +{ + // relative position + btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis(); + btVector3 ri = rotation * m_localMomentArm[n_node]; + btMatrix3x3 ri_skew = Cross(ri); + + // calculate impulse factor + // rigid part + btScalar inv_mass = m_nodalMass[n_node] > btScalar(0) ? btScalar(1) / m_mass : btScalar(0); + btMatrix3x3 K1 = Diagonal(inv_mass); + K1 -= ri_skew * m_interpolateInvInertiaTensorWorld * ri_skew; + + // reduced deformable part + btMatrix3x3 SA; + SA.setZero(); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { + for (int r = 0; r < m_nReduced; ++r) + { + SA[i][j] += m_modes[r][3 * n_node + i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]); + } + } + } + btMatrix3x3 RSARinv = rotation * SA * rotation.transpose(); + + + TVStack omega_helper; // Sum_i m_i r*_i R S_i + omega_helper.resize(m_nReduced); + for (int r = 0; r < m_nReduced; ++r) + { + omega_helper[r].setZero(); + for (int i = 0; i < m_nFull; ++i) + { + btMatrix3x3 mi_rstar_i = rotation * Cross(m_localMomentArm[i]) * m_nodalMass[i]; + btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]); + omega_helper[r] += mi_rstar_i * rotation * s_ri; + } + } + + btMatrix3x3 sum_multiply_A; + sum_multiply_A.setZero(); + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { + for (int r = 0; r < m_nReduced; ++r) + { + sum_multiply_A[i][j] += omega_helper[r][i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]); + } + } + } + + btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose(); + + return m_rigidOnly ? K1 : K1 + K2; +} + +void btReducedDeformableBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt) +{ + if (!m_rigidOnly) + { + // apply impulse force + applyFullSpaceNodalForce(impulse / dt, n_node); + + // update delta damping force + tDenseArray reduced_vel_tmp; + reduced_vel_tmp.resize(m_nReduced); + for (int r = 0; r < m_nReduced; ++r) + { + reduced_vel_tmp[r] = m_reducedVelocity[r] + m_internalDeltaReducedVelocity[r]; + } + applyReducedDampingForce(reduced_vel_tmp); + // applyReducedDampingForce(m_internalDeltaReducedVelocity); + + // delta reduced velocity + for (int r = 0; r < m_nReduced; ++r) + { + // The reduced mass is always identity! + m_internalDeltaReducedVelocity[r] += dt * (m_reducedForceDamping[r] + m_reducedForceExternal[r]); + } + } + + internalApplyRigidImpulse(impulse, rel_pos); +} + +void btReducedDeformableBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node) +{ + // f_local = R^-1 * f_ext //TODO: interpoalted transfrom + // btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext; + btVector3 f_local = m_interpolationWorldTransform.getBasis().transpose() * f_ext; + + // f_ext_r = [S^T * P]_{n_node} * f_local + tDenseArray f_ext_r; + f_ext_r.resize(m_nReduced, 0); + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedForceExternal[r] = 0; + for (int k = 0; k < 3; ++k) + { + f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k]; + } + + m_reducedForceExternal[r] += f_ext_r[r]; + } +} + +void btReducedDeformableBody::applyRigidGravity(const btVector3& gravity, btScalar dt) +{ + // update rigid frame velocity + m_linearVelocity += dt * gravity; +} + +void btReducedDeformableBody::applyReducedElasticForce(const tDenseArray& reduce_dofs) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedForceElastic[r] = - m_ksScale * m_Kr[r] * reduce_dofs[r]; + } +} + +void btReducedDeformableBody::applyReducedDampingForce(const tDenseArray& reduce_vel) +{ + for (int r = 0; r < m_nReduced; ++r) + { + m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r]; + } +} + +btScalar btReducedDeformableBody::getTotalMass() const +{ + return m_mass; +} + +btTransform& btReducedDeformableBody::getRigidTransform() +{ + return m_rigidTransformWorld; +} + +const btVector3& btReducedDeformableBody::getLinearVelocity() const +{ + return m_linearVelocity; +} + +const btVector3& btReducedDeformableBody::getAngularVelocity() const +{ + return m_angularVelocity; +} + +void btReducedDeformableBody::disableReducedModes(const bool rigid_only) +{ + m_rigidOnly = rigid_only; +} + +bool btReducedDeformableBody::isReducedModesOFF() const +{ + return m_rigidOnly; +}
\ No newline at end of file |