diff options
Diffstat (limited to 'src')
20 files changed, 107 insertions, 2758 deletions
diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp deleted file mode 100644 index feb30d587..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp +++ /dev/null @@ -1,792 +0,0 @@ -#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 diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h deleted file mode 100644 index 8968fb0cb..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h +++ /dev/null @@ -1,257 +0,0 @@ -#ifndef BT_REDUCED_SOFT_BODY_H -#define BT_REDUCED_SOFT_BODY_H - -#include "../btSoftBody.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btVector3.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btTransform.h" - -// Reduced deformable body is a simplified deformable object embedded in a rigid frame. -class btReducedDeformableBody : public btSoftBody -{ - public: - // - // Typedefs - // - typedef btAlignedObjectArray<btVector3> TVStack; - // typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix; - typedef btAlignedObjectArray<btScalar> tDenseArray; - typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix; - - private: - // flag to turn off the reduced modes - bool m_rigidOnly; - - // Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass. - bool m_transform_lock; - - // scaling factors - btScalar m_rhoScale; // mass density scale - btScalar m_ksScale; // stiffness scale - - // projection matrix - tDenseMatrix m_projPA; // Eqn. 4.11 from Rahul Sheth's thesis - tDenseMatrix m_projCq; - tDenseArray m_STP; - tDenseArray m_MrInvSTP; - - TVStack m_localMomentArm; // Sq + x0 - - btVector3 m_internalDeltaLinearVelocity; - btVector3 m_internalDeltaAngularVelocity; - tDenseArray m_internalDeltaReducedVelocity; - - btVector3 m_linearVelocityFromReduced; // contribution to the linear velocity from reduced velocity - btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity - btVector3 m_internalDeltaAngularVelocityFromReduced; - - protected: - // rigid frame - btScalar m_mass; // total mass of the rigid frame - btScalar m_inverseMass; // inverse of the total mass of the rigid frame - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - btScalar m_linearDamping; // linear damping coefficient - btScalar m_angularDamping; // angular damping coefficient - btVector3 m_linearFactor; - btVector3 m_angularFactor; - // btVector3 m_invInertiaLocal; - btMatrix3x3 m_invInertiaLocal; - btTransform m_rigidTransformWorld; - btMatrix3x3 m_invInertiaTensorWorldInitial; - btMatrix3x3 m_invInertiaTensorWorld; - btMatrix3x3 m_interpolateInvInertiaTensorWorld; - btVector3 m_initialCoM; // initial center of mass (original of the m_rigidTransformWorld) - - // damping - btScalar m_dampingAlpha; - btScalar m_dampingBeta; - - public: - // - // Fields - // - - // reduced space - int m_nReduced; - int m_nFull; - tDenseMatrix m_modes; // modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes - tDenseArray m_reducedDofs; // Reduced degree of freedom - tDenseArray m_reducedDofsBuffer; // Reduced degree of freedom at t^n - tDenseArray m_reducedVelocity; // Reduced velocity array - tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n - tDenseArray m_reducedForceExternal; // reduced external force - tDenseArray m_reducedForceElastic; // reduced internal elastic force - tDenseArray m_reducedForceDamping; // reduced internal damping force - tDenseArray m_eigenvalues; // eigenvalues of the reduce deformable model - tDenseArray m_Kr; // reduced stiffness matrix - - // full space - TVStack m_x0; // Rest position - tDenseArray m_nodalMass; // Mass on each node - btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes - int m_nodeIndexOffset; // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world. - - // contacts - btAlignedObjectArray<int> m_contactNodesList; - - // - // Api - // - btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m); - - ~btReducedDeformableBody() {} - - // - // initializing helpers - // - void internalInitialization(); - - void setReducedModes(int num_modes, int full_size); - - void setMassProps(const tDenseArray& mass_array); - - void setInertiaProps(); - - void setRigidVelocity(const btVector3& v); - - void setRigidAngularVelocity(const btVector3& omega); - - void setStiffnessScale(const btScalar ks); - - void setMassScale(const btScalar rho); - - void setFixedNodes(const int n_node); - - void setDamping(const btScalar alpha, const btScalar beta); - - void disableReducedModes(const bool rigid_only); - - virtual void setTotalMass(btScalar mass, bool fromfaces = false); - - // - // various internal updates - // - virtual void transformTo(const btTransform& trs); - virtual void transform(const btTransform& trs); - // caution: - // need to use scale before using transform, because the scale is performed in the local frame - // (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info) - virtual void scale(const btVector3& scl); - - private: - void updateRestNodalPositions(); - - void updateInitialInertiaTensor(const btMatrix3x3& rotation); - - void updateLocalInertiaTensorFromNodes(); - - void updateInertiaTensor(); - - void updateModesByRotation(const btMatrix3x3& rotation); - - public: - void updateLocalMomentArm(); - - void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform); - - // update the external force projection matrix - void updateExternalForceProjectMatrix(bool initialized); - - void endOfTimeStepZeroing(); - - void applyInternalVelocityChanges(); - - // - // position and velocity update related - // - - // compute reduced degree of freedoms - void updateReducedDofs(btScalar solverdt); - - // compute reduced velocity update (for explicit time stepping) - void updateReducedVelocity(btScalar solverdt); - - // map to full degree of freedoms - void mapToFullPosition(const btTransform& ref_trans); - - // compute full space velocity from the reduced velocity - void mapToFullVelocity(const btTransform& ref_trans); - - // compute total angular momentum - const btVector3 computeTotalAngularMomentum() const; - - // get a single node's full space velocity from the reduced velocity - const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const; - - // get a single node's all delta velocity - const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const; - - // - // rigid motion related - // - void applyDamping(btScalar timeStep); - - void applyCentralImpulse(const btVector3& impulse); - - void applyTorqueImpulse(const btVector3& torque); - - void proceedToTransform(btScalar dt, bool end_of_time_step); - - // - // force related - // - - // apply impulse to the rigid frame - void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos); - - // apply impulse to nodes in the full space - void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt); - - // apply nodal external force in the full space - void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node); - - // apply gravity to the rigid frame - void applyRigidGravity(const btVector3& gravity, btScalar dt); - - // apply reduced elastic force - void applyReducedElasticForce(const tDenseArray& reduce_dofs); - - // apply reduced damping force - void applyReducedDampingForce(const tDenseArray& reduce_vel); - - // calculate the impulse factor - virtual btMatrix3x3 getImpulseFactor(int n_node); - - // get relative position from a node to the CoM of the rigid frame - btVector3 getRelativePos(int n_node); - - // - // accessors - // - bool isReducedModesOFF() const; - btScalar getTotalMass() const; - btTransform& getRigidTransform(); - const btVector3& getLinearVelocity() const; - const btVector3& getAngularVelocity() const; - - #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 - void clampVelocity(btVector3& v) const { - v.setX( - fmax(-BT_CLAMP_VELOCITY_TO, - fmin(BT_CLAMP_VELOCITY_TO, v.getX())) - ); - v.setY( - fmax(-BT_CLAMP_VELOCITY_TO, - fmin(BT_CLAMP_VELOCITY_TO, v.getY())) - ); - v.setZ( - fmax(-BT_CLAMP_VELOCITY_TO, - fmin(BT_CLAMP_VELOCITY_TO, v.getZ())) - ); - } - #endif -}; - -#endif // BT_REDUCED_SOFT_BODY_H
\ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp deleted file mode 100644 index d7e4cc2b8..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp +++ /dev/null @@ -1,215 +0,0 @@ -#include "btReducedDeformableBodyHelpers.h" -#include "../btSoftBodyHelpers.h" -#include <iostream> -#include <string> -#include <sstream> - -btReducedDeformableBody* btReducedDeformableBodyHelpers::createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only) { - std::string filename = file_path + vtk_file; - btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createFromVtkFile(worldInfo, filename.c_str()); - - rsb->setReducedModes(num_modes, rsb->m_nodes.size()); - btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, file_path.c_str()); - - rsb->disableReducedModes(rigid_only); - - return rsb; -} - -btReducedDeformableBody* btReducedDeformableBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file) -{ - std::ifstream fs; - fs.open(vtk_file); - btAssert(fs); - - typedef btAlignedObjectArray<int> Index; - std::string line; - btAlignedObjectArray<btVector3> X; - btVector3 position; - btAlignedObjectArray<Index> indices; - bool reading_points = false; - bool reading_tets = false; - size_t n_points = 0; - size_t n_tets = 0; - size_t x_count = 0; - size_t indices_count = 0; - while (std::getline(fs, line)) - { - std::stringstream ss(line); - if (line.size() == (size_t)(0)) - { - } - else if (line.substr(0, 6) == "POINTS") - { - reading_points = true; - reading_tets = false; - ss.ignore(128, ' '); // ignore "POINTS" - ss >> n_points; - X.resize(n_points); - } - else if (line.substr(0, 5) == "CELLS") - { - reading_points = false; - reading_tets = true; - ss.ignore(128, ' '); // ignore "CELLS" - ss >> n_tets; - indices.resize(n_tets); - } - else if (line.substr(0, 10) == "CELL_TYPES") - { - reading_points = false; - reading_tets = false; - } - else if (reading_points) - { - btScalar p; - ss >> p; - position.setX(p); - ss >> p; - position.setY(p); - ss >> p; - position.setZ(p); - //printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ()); - X[x_count++] = position; - } - else if (reading_tets) - { - int d; - ss >> d; - if (d != 4) - { - printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n"); - fs.close(); - return 0; - } - ss.ignore(128, ' '); // ignore "4" - Index tet; - tet.resize(4); - for (size_t i = 0; i < 4; i++) - { - ss >> tet[i]; - //printf("%d ", tet[i]); - } - //printf("\n"); - indices[indices_count++] = tet; - } - } - btReducedDeformableBody* rsb = new btReducedDeformableBody(&worldInfo, n_points, &X[0], 0); - - for (int i = 0; i < n_tets; ++i) - { - const Index& ni = indices[i]; - rsb->appendTetra(ni[0], ni[1], ni[2], ni[3]); - { - rsb->appendLink(ni[0], ni[1], 0, true); - rsb->appendLink(ni[1], ni[2], 0, true); - rsb->appendLink(ni[2], ni[0], 0, true); - rsb->appendLink(ni[0], ni[3], 0, true); - rsb->appendLink(ni[1], ni[3], 0, true); - rsb->appendLink(ni[2], ni[3], 0, true); - } - } - - btSoftBodyHelpers::generateBoundaryFaces(rsb); - rsb->initializeDmInverse(); - rsb->m_tetraScratches.resize(rsb->m_tetras.size()); - rsb->m_tetraScratchesTn.resize(rsb->m_tetras.size()); - printf("Nodes: %u\r\n", rsb->m_nodes.size()); - printf("Links: %u\r\n", rsb->m_links.size()); - printf("Faces: %u\r\n", rsb->m_faces.size()); - printf("Tetras: %u\r\n", rsb->m_tetras.size()); - - fs.close(); - - return rsb; -} - -void btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path) -{ - // read in eigenmodes, stiffness and mass matrices - std::string eigenvalues_file = std::string(file_path) + "eigenvalues.bin"; - btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_eigenvalues, rsb->m_nReduced, eigenvalues_file.c_str()); - - std::string Kr_file = std::string(file_path) + "K_r_diag_mat.bin"; - btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Kr, rsb->m_nReduced, Kr_file.c_str()); - - // std::string Mr_file = std::string(file_path) + "M_r_diag_mat.bin"; - // btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Mr, rsb->m_nReduced, Mr_file.c_str()); - - std::string modes_file = std::string(file_path) + "modes.bin"; - btReducedDeformableBodyHelpers::readBinaryMat(rsb->m_modes, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str()); - - // read in full nodal mass - std::string M_file = std::string(file_path) + "M_diag_mat.bin"; - btAlignedObjectArray<btScalar> mass_array; - btReducedDeformableBodyHelpers::readBinaryVec(mass_array, rsb->m_nFull, M_file.c_str()); - rsb->setMassProps(mass_array); - - // calculate the inertia tensor in the local frame - rsb->setInertiaProps(); - - // other internal initialization - rsb->internalInitialization(); -} - -// read in a vector from the binary file -void btReducedDeformableBodyHelpers::readBinaryVec(btReducedDeformableBody::tDenseArray& vec, - const unsigned int n_size, // #entries read - const char* file) -{ - std::ifstream f_in(file, std::ios::in | std::ios::binary); - // first get size - unsigned int size; - f_in.read((char*)&size, sizeof(uint32_t)); - btAssert(size >= n_size); // make sure the #requested mode is smaller than the #available modes - - // read data - vec.resize(n_size); - double temp; - for (unsigned int i = 0; i < n_size; ++i) - { - f_in.read((char*)&temp, sizeof(double)); - vec[i] = btScalar(temp); - } - f_in.close(); -} - -// read in a matrix from the binary file -void btReducedDeformableBodyHelpers::readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, - const unsigned int n_modes, // #modes, outer array size - const unsigned int n_full, // inner array size - const char* file) -{ - std::ifstream f_in(file, std::ios::in | std::ios::binary); - // first get size - unsigned int v_size; - f_in.read((char*)&v_size, sizeof(uint32_t)); - btAssert(v_size >= n_modes * n_full); // make sure the #requested mode is smaller than the #available modes - - // read data - mat.resize(n_modes); - for (int i = 0; i < n_modes; ++i) - { - for (int j = 0; j < n_full; ++j) - { - double temp; - f_in.read((char*)&temp, sizeof(double)); - - if (mat[i].size() != n_modes) - mat[i].resize(n_full); - mat[i][j] = btScalar(temp); - } - } - f_in.close(); -} - -void btReducedDeformableBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin) -{ - btScalar lx = btScalar(2.) * (half_extents[0] + margin[0]); - btScalar ly = btScalar(2.) * (half_extents[1] + margin[1]); - btScalar lz = btScalar(2.) * (half_extents[2] + margin[2]); - - inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz), - mass / (btScalar(12.0)) * (lx * lx + lz * lz), - mass / (btScalar(12.0)) * (lx * lx + ly * ly)); -} diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h deleted file mode 100644 index 2b259a931..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef BT_REDUCED_SOFT_BODY_HELPERS_H -#define BT_REDUCED_SOFT_BODY_HELPERS_H - -#include "btReducedDeformableBody.h" -#include <string> - -struct btReducedDeformableBodyHelpers -{ - // create a reduced deformable object - static btReducedDeformableBody* createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only); - // read in geometry info from Vtk file - static btReducedDeformableBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); - // read in all reduced files - static void readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path); - // read in a binary vector - static void readBinaryVec(btReducedDeformableBody::tDenseArray& vec, const unsigned int n_size, const char* file); - // read in a binary matrix - static void readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, const unsigned int n_modes, const unsigned int n_full, const char* file); - - // calculate the local inertia tensor for a box shape reduced deformable object - static void calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin); -}; - - -#endif // BT_REDUCED_SOFT_BODY_HELPERS_H
\ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp deleted file mode 100644 index 1418cc247..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp +++ /dev/null @@ -1,325 +0,0 @@ -#include "btReducedDeformableBodySolver.h" -#include "btReducedDeformableBody.h" - -btReducedDeformableBodySolver::btReducedDeformableBodySolver() -{ - m_ascendOrder = true; - m_reducedSolver = true; - m_dampingAlpha = 0; - m_dampingBeta = 0; - m_gravity = btVector3(0, 0, 0); -} - -void btReducedDeformableBodySolver::setGravity(const btVector3& gravity) -{ - m_gravity = gravity; -} - -void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt) -{ - m_softBodies.copyFromArray(bodies); - bool nodeUpdated = updateNodes(); - - if (nodeUpdated) - { - m_dv.resize(m_numNodes, btVector3(0, 0, 0)); - m_ddv.resize(m_numNodes, btVector3(0, 0, 0)); - m_residual.resize(m_numNodes, btVector3(0, 0, 0)); - m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0)); - } - - // need to setZero here as resize only set value for newly allocated items - for (int i = 0; i < m_numNodes; ++i) - { - m_dv[i].setZero(); - m_ddv[i].setZero(); - m_residual[i].setZero(); - } - - if (dt > 0) - { - m_dt = dt; - } - m_objective->reinitialize(nodeUpdated, dt); - - int N = bodies.size(); - if (nodeUpdated) - { - m_staticConstraints.resize(N); - m_nodeRigidConstraints.resize(N); - // m_faceRigidConstraints.resize(N); - } - for (int i = 0; i < N; ++i) - { - m_staticConstraints[i].clear(); - m_nodeRigidConstraints[i].clear(); - // m_faceRigidConstraints[i].clear(); - } - - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - rsb->m_contactNodesList.clear(); - } - - // set node index offsets - int sum = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - rsb->m_nodeIndexOffset = sum; - sum += rsb->m_nodes.size(); - } - - btDeformableBodySolver::updateSoftBodies(); -} - -void btReducedDeformableBodySolver::predictMotion(btScalar solverdt) -{ - applyExplicitForce(solverdt); - - // predict new mesh location - predictReduceDeformableMotion(solverdt); - - //TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion -} - -void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt) -{ - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - if (!rsb->isActive()) - { - continue; - } - - // clear contacts variables - rsb->m_nodeRigidContacts.resize(0); - rsb->m_faceRigidContacts.resize(0); - rsb->m_faceNodeContacts.resize(0); - - // calculate inverse mass matrix for all nodes - for (int j = 0; j < rsb->m_nodes.size(); ++j) - { - if (rsb->m_nodes[j].m_im > 0) - { - rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse(); - } - } - - // rigid motion: t, R at time^* - rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform()); - - // update reduced dofs at time^* - // rsb->updateReducedDofs(solverdt); - - // update local moment arm at time^* - // rsb->updateLocalMomentArm(); - // rsb->updateExternalForceProjectMatrix(true); - - // predict full space velocity at time^* (needed for constraints) - rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform()); - - // update full space nodal position at time^* - rsb->mapToFullPosition(rsb->getInterpolationWorldTransform()); - - // update bounding box - rsb->updateBounds(); - - // update tree - rsb->updateNodeTree(true, true); - if (!rsb->m_fdbvt.empty()) - { - rsb->updateFaceTree(true, true); - } - } -} - -void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt) -{ - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - - // apply gravity to the rigid frame, get m_linearVelocity at time^* - rsb->applyRigidGravity(m_gravity, solverdt); - - if (!rsb->isReducedModesOFF()) - { - // add internal force (elastic force & damping force) - rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer); - rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer); - - // get reduced velocity at time^* - rsb->updateReducedVelocity(solverdt); - } - - // apply damping (no need at this point) - // rsb->applyDamping(solverdt); - } -} - -void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep) -{ - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - - // rigid motion - rsb->proceedToTransform(timeStep, true); - - if (!rsb->isReducedModesOFF()) - { - // update reduced dofs for time^n+1 - rsb->updateReducedDofs(timeStep); - - // update local moment arm for time^n+1 - rsb->updateLocalMomentArm(); - rsb->updateExternalForceProjectMatrix(true); - } - - // update mesh nodal positions for time^n+1 - rsb->mapToFullPosition(rsb->getRigidTransform()); - - // update mesh nodal velocity - rsb->mapToFullVelocity(rsb->getRigidTransform()); - - // end of time step clean up and update - rsb->endOfTimeStepZeroing(); - - // update the rendering mesh - rsb->interpolateRenderMesh(); - } -} - -void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal) -{ - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - if (!rsb->isActive()) - { - continue; - } - - // set fixed constraints - for (int j = 0; j < rsb->m_fixedNodes.size(); ++j) - { - int i_node = rsb->m_fixedNodes[j]; - if (rsb->m_nodes[i_node].m_im == 0) - { - for (int k = 0; k < 3; ++k) - { - btVector3 dir(0, 0, 0); - dir[k] = 1; - btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt); - m_staticConstraints[i].push_back(static_constraint); - } - } - } - btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size()); - - // set Deformable Node vs. Rigid constraint - for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j) - { - const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j]; - // skip fixed points - if (contact.m_node->m_im == 0) - { - continue; - } - btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt); - m_nodeRigidConstraints[i].push_back(constraint); - rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset); - } - // std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n"; - // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n"; - - } -} - -btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) -{ - btScalar residualSquare = 0; - - for (int i = 0; i < m_softBodies.size(); ++i) - { - btAlignedObjectArray<int> m_orderNonContactConstraintPool; - btAlignedObjectArray<int> m_orderContactConstraintPool; - - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - - // shuffle the order of applying constraint - m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size()); - m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size()); - if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) - { - // fixed constraint order - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j; - } - // contact constraint order - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j; - } - - m_ascendOrder = m_ascendOrder ? false : true; - } - else - { - for (int j = 0; j < m_staticConstraints[i].size(); ++j) - { - m_orderNonContactConstraintPool[j] = j; - } - // contact constraint order - for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) - { - m_orderContactConstraintPool[j] = j; - } - } - - // handle fixed constraint - for (int k = 0; k < m_staticConstraints[i].size(); ++k) - { - btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]]; - btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - // handle contact constraint - - // node vs rigid contact - // std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n'; - for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k) - { - btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]]; - btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); - residualSquare = btMax(residualSquare, localResidualSquare); - } - - // face vs rigid contact - // for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k) - // { - // btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k]; - // btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); - // residualSquare = btMax(residualSquare, localResidualSquare); - // } - } - - - return residualSquare; -} - -void btReducedDeformableBodySolver::deformableBodyInternalWriteBack() -{ - // reduced deformable update - for (int i = 0; i < m_softBodies.size(); ++i) - { - btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]); - rsb->applyInternalVelocityChanges(); - } - m_ascendOrder = true; -}
\ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h deleted file mode 100644 index 04c171f31..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H -#define BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H - -#include "BulletSoftBody/btDeformableBodySolver.h" -#include "btReducedDeformableContactConstraint.h" - -class btReducedDeformableBody; - -class btReducedDeformableBodySolver : public btDeformableBodySolver -{ - protected: - bool m_ascendOrder; - btScalar m_dampingAlpha; - btScalar m_dampingBeta; - - btVector3 m_gravity; - - void predictReduceDeformableMotion(btScalar solverdt); - - void applyExplicitForce(btScalar solverdt); - - public: - btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableStaticConstraint> > m_staticConstraints; - btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints; - btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableFaceRigidContactConstraint> > m_faceRigidConstraints; - - btReducedDeformableBodySolver(); - ~btReducedDeformableBodySolver() {} - - virtual void setGravity(const btVector3& gravity); - - virtual SolverTypes getSolverType() const - { - return REDUCED_DEFORMABLE_SOLVER; - } - - // resize/clear data structures - virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt); - - virtual void predictMotion(btScalar solverdt); - - virtual void applyTransforms(btScalar timeStep); - - // set up contact constraints - virtual void setConstraints(const btContactSolverInfo& infoGlobal); - - // solve all constraints (fixed and contact) - virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); - - // apply all the delta velocities - virtual void deformableBodyInternalWriteBack(); - - // virtual void setProjection() {} - - // virtual void setLagrangeMultiplier() {} - - // virtual void setupDeformableSolve(bool implicit); - -}; - -#endif // BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
\ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp deleted file mode 100644 index 3c78d2d22..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp +++ /dev/null @@ -1,579 +0,0 @@ -#include "btReducedDeformableContactConstraint.h" -#include <iostream> - -// ================= static constraints =================== -btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint( - btReducedDeformableBody* rsb, - btSoftBody::Node* node, - const btVector3& ri, - const btVector3& x0, - const btVector3& dir, - const btContactSolverInfo& infoGlobal, - btScalar dt) - : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal) -{ - m_erp = 0.2; - m_appliedImpulse = 0; - - // get impulse factor - m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index); - m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection); - - btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection); - btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection); - - m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor; -} - -btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) -{ - // target velocity of fixed constraint is 0 - btVector3 deltaVa = getDeltaVa(); - btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection); - btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor; - m_appliedImpulse = m_appliedImpulse + deltaImpulse; - - btVector3 impulse = deltaImpulse * m_impulseDirection; - applyImpulse(impulse); - - // calculate residual - btScalar residualSquare = m_impulseFactor * deltaImpulse; - residualSquare *= residualSquare; - - return residualSquare; -} - -// this calls reduced deformable body's internalApplyFullSpaceImpulse -void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse) -{ - // apply full space impulse - m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt); -} - -btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const -{ - return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index); -} - -// ================= base contact constraints =================== -btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint( - btReducedDeformableBody* rsb, - const btSoftBody::DeformableRigidContact& c, - const btContactSolverInfo& infoGlobal, - btScalar dt) - : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal) -{ - m_nodeQueryIndex = 0; - m_appliedNormalImpulse = 0; - m_appliedTangentImpulse = 0; - m_rhs = 0; - m_rhs_tangent = 0; - m_cfm = infoGlobal.m_deformable_cfm; - m_cfm_friction = 0; - m_erp = infoGlobal.m_deformable_erp; - m_erp_friction = infoGlobal.m_deformable_erp; - m_friction = infoGlobal.m_friction; - - m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject(); - m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK); -} - -void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body) -{ - if (!m_collideMultibody) - { - m_solverBodyId = bodyId; - m_solverBody = &solver_body; - m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass(); - btVector3 torqueAxis = -m_relPosA.cross(m_contactNormalA); - m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis; - - m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass(); - btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent); - m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent; - } -} - -btVector3 btReducedDeformableRigidContactConstraint::getVa() const -{ - btVector3 Va(0, 0, 0); - if (!m_collideStatic) - { - Va = btDeformableRigidContactConstraint::getVa(); - } - return Va; -} - -btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) -{ - // btVector3 Va = getVa(); - // btVector3 deltaVa = Va - m_bufferVelocityA; - // if (!m_collideStatic) - // { - // std::cout << "moving collision!!!\n"; - // std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n"; - // std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t' - // << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t' - // << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n'; - // } - btVector3 deltaVa = getDeltaVa(); - btVector3 deltaVb = getDeltaVb(); - - // if (!m_collideStatic) - // { - // std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n'; - // std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n'; - // } - - // get delta relative velocity and magnitude (i.e., how much impulse has been applied?) - btVector3 deltaV_rel = deltaVa - deltaVb; - btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA); - - // if (!m_collideStatic) - // { - // std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n"; - // std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n"; - // std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n'; - // } - - // get the normal impulse to be applied - btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor; - // if (!m_collideStatic) - // { - // std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << "\n"; - // std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n'; - // } - - { - // cumulative impulse that has been applied - btScalar sum = m_appliedNormalImpulse + deltaImpulse; - // if the cumulative impulse is pushing the object into the rigid body, set it zero - if (sum < 0) - { - deltaImpulse = -m_appliedNormalImpulse; - m_appliedNormalImpulse = 0; - } - else - { - m_appliedNormalImpulse = sum; - } - } - - // if (!m_collideStatic) - // { - // std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n'; - // std::cout << "deltaImpulse: " << deltaImpulse << '\n'; - // } - - // residual is the nodal normal velocity change in current iteration - btScalar residualSquare = deltaImpulse * m_normalImpulseFactor; // get residual - residualSquare *= residualSquare; - - - // apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|) - btScalar deltaImpulse_tangent = 0; - btScalar deltaImpulse_tangent2 = 0; - { - // calculate how much impulse is needed - // btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent); - // btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv; - // deltaImpulse_tangent = m_rhs_tangent - impulse_changed; - - // btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent; - btScalar lower_limit = - m_appliedNormalImpulse * m_friction; - btScalar upper_limit = m_appliedNormalImpulse * m_friction; - // if (sum > upper_limit) - // { - // deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse; - // m_appliedTangentImpulse = upper_limit; - // } - // else if (sum < lower_limit) - // { - // deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse; - // m_appliedTangentImpulse = lower_limit; - // } - // else - // { - // m_appliedTangentImpulse = sum; - // } - // - calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent, - m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel); - - if (m_collideMultibody) - { - calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2, - m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel); - } - - - if (!m_collideStatic) - { - // std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t" << m_contactTangent[1] << "\t" << m_contactTangent[2] << "\n"; - // std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n'; - // std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n'; - // std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n'; - } - } - - // get the total impulse vector - btVector3 impulse_normal = deltaImpulse * m_contactNormalA; - btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent); - btVector3 impulse_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2); - btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2; - - applyImpulse(impulse); - - // apply impulse to the rigid/multibodies involved and change their velocities - if (!m_collideStatic) - { - // std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t' - // << m_linearComponentNormal[1] << '\t' - // << m_linearComponentNormal[2] << '\n'; - // std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t' - // << m_angularComponentNormal[1] << '\t' - // << m_angularComponentNormal[2] << '\n'; - - if (!m_collideMultibody) // collision with rigid body - { - // std::cout << "rigid impulse applied!!\n"; - // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' - // << m_solverBody->getDeltaLinearVelocity()[1] << '\t' - // << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; - // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' - // << m_solverBody->getDeltaAngularVelocity()[1] << '\t' - // << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; - - m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse); - m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent); - - // std::cout << "after\n"; - // std::cout << "rigid impulse applied!!\n"; - // std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t' - // << m_solverBody->getDeltaLinearVelocity()[1] << '\t' - // << m_solverBody->getDeltaLinearVelocity()[2] << '\n'; - // std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t' - // << m_solverBody->getDeltaAngularVelocity()[1] << '\t' - // << m_solverBody->getDeltaAngularVelocity()[2] << '\n'; - } - else // collision with multibody - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj); - if (multibodyLinkCol) - { - const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // apply normal component of the impulse - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse); - - // const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - // std::cout << "deltaV_normal: "; - // for (int i = 0; i < ndof; ++i) - // { - // std::cout << i << "\t" << deltaV_normal[i] << '\n'; - // } - - if (impulse_tangent.norm() > SIMD_EPSILON) - { - // apply tangential component of the impulse - const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, deltaImpulse_tangent); - const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2); - } - } - } - } - return residualSquare; -} - -void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent, - btScalar& appliedImpulse, - const btScalar rhs_tangent, - const btScalar tangentImpulseFactorInv, - const btVector3& tangent, - const btScalar lower_limit, - const btScalar upper_limit, - const btVector3& deltaV_rel) -{ - btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent); - btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv; - deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed; - - btScalar sum = appliedImpulse + deltaImpulse_tangent; - if (sum > upper_limit) - { - deltaImpulse_tangent = upper_limit - appliedImpulse; - appliedImpulse = upper_limit; - } - else if (sum < lower_limit) - { - deltaImpulse_tangent = lower_limit - appliedImpulse; - appliedImpulse = lower_limit; - } - else - { - appliedImpulse = sum; - } -} - -// ================= node vs rigid constraints =================== -btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint( - btReducedDeformableBody* rsb, - const btSoftBody::DeformableNodeRigidContact& contact, - const btContactSolverInfo& infoGlobal, - btScalar dt) - : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt) -{ - m_contactNormalA = contact.m_cti.m_normal; - m_contactNormalB = -contact.m_cti.m_normal; - - if (contact.m_node->index < rsb->m_nodes.size()) - { - m_nodeQueryIndex = contact.m_node->index; - } - else - { - m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset; - } - - if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - m_relPosA = contact.m_c1; - } - else - { - m_relPosA = btVector3(0,0,0); - } - m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin(); - - if (m_collideStatic) // colliding with static object, only consider reduced deformable body's impulse factor - { - m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex); - } - else // colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors - { - m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0; - } - - m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA); - m_tangentImpulseFactor = 0; - - warmStarting(); -} - -void btReducedDeformableNodeRigidContactConstraint::warmStarting() -{ - btVector3 va = getVa(); - btVector3 vb = getVb(); - m_bufferVelocityA = va; - m_bufferVelocityB = vb; - - // we define the (+) direction of errors to be the outward surface normal of the rigid object - btVector3 v_rel = vb - va; - // get tangent direction of the relative velocity - btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA; - if (v_tangent.norm() < SIMD_EPSILON) - { - m_contactTangent = btVector3(0, 0, 0); - // tangent impulse factor - m_tangentImpulseFactor = 0; - m_tangentImpulseFactorInv = 0; - } - else - { - if (!m_collideMultibody) - { - m_contactTangent = v_tangent.normalized(); - m_contactTangent2.setZero(); - // tangent impulse factor 1 - m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent); - m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor; - // tangent impulse factor 2 - m_tangentImpulseFactor2 = 0; - m_tangentImpulseFactorInv2 = 0; - } - else // multibody requires 2 contact directions - { - m_contactTangent = m_contact->t1; - m_contactTangent2 = m_contact->t2; - - // tangent impulse factor 1 - m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent); - m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor; - // tangent impulse factor 2 - m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2); - m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2; - } - } - - - // initial guess for normal impulse - { - btScalar velocity_error = btDot(v_rel, m_contactNormalA); // magnitude of relative velocity - btScalar position_error = 0; - if (m_penetration > 0) - { - velocity_error += m_penetration / m_dt; - } - else - { - // add penetration correction vel - position_error = m_penetration * m_erp / m_dt; - } - // get the initial estimate of impulse magnitude to be applied - m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor; - } - - // initial guess for tangential impulse - { - btScalar velocity_error = btDot(v_rel, m_contactTangent); - m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv; - - if (m_collideMultibody) - { - btScalar velocity_error2 = btDot(v_rel, m_contactTangent2); - m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2; - } - } - - // warm starting - // applyImpulse(m_rhs * m_contactNormalA); - // if (!m_collideStatic) - // { - // const btSoftBody::sCti& cti = m_contact->m_cti; - // if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - // { - // m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs); - // } - // } -} - -btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const -{ - return m_node->m_v; -} - -btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const -{ - btVector3 deltaVa(0, 0, 0); - if (!m_collideStatic) - { - if (!m_collideMultibody) // for rigid body - { - deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA); - } - else // for multibody - { - btMultiBodyLinkCollider* multibodyLinkCol = 0; - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj); - if (multibodyLinkCol) - { - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0]; - const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0]; - const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0]; - const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); - // add in the normal component of the va - btScalar vel = 0; - for (int k = 0; k < ndof; ++k) - { - vel += local_dv[k] * J_n[k]; - } - deltaVa = m_contact->m_cti.m_normal * vel; - - // add in the tangential components of the va - vel = 0; - for (int k = 0; k < ndof; ++k) - { - vel += local_dv[k] * J_t1[k]; - } - deltaVa += m_contact->t1 * vel; - - vel = 0; - for (int k = 0; k < ndof; ++k) - { - vel += local_dv[k] * J_t2[k]; - } - deltaVa += m_contact->t2 * vel; - } - } - } - return deltaVa; -} - -btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const -{ - // std::cout << "node: " << m_node->index << '\n'; - return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex); -} - -btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const -{ - return m_node->m_splitv; -} - -btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const -{ - return m_total_normal_dv + m_total_tangent_dv; -} - -void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) -{ - m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt); - // m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt); - // m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform()); - // if (!m_collideStatic) - // { - // // std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n'; - // // std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n'; - // btVector3 v_after = getDeltaVb() + m_node->m_v; - // // std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n'; - // } - // std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n'; -} - -// ================= face vs rigid constraints =================== -btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint( - btReducedDeformableBody* rsb, - const btSoftBody::DeformableFaceRigidContact& contact, - const btContactSolverInfo& infoGlobal, - btScalar dt, - bool useStrainLimiting) - : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt) -{} - -btVector3 btReducedDeformableFaceRigidContactConstraint::getVb() const -{ - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2]; - return vb; -} - -btVector3 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const -{ - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2]; - return vb; -} - -btVector3 btReducedDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const -{ - btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv; - const btSoftBody::DeformableFaceRigidContact* contact = getContact(); - if (m_face->m_n[0] == node) - { - return face_dv * contact->m_weights[0]; - } - if (m_face->m_n[1] == node) - { - return face_dv * contact->m_weights[1]; - } - btAssert(node == m_face->m_n[2]); - return face_dv * contact->m_weights[2]; -} - -void btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse) -{ - // -}
\ No newline at end of file diff --git a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h b/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h deleted file mode 100644 index 10d0938c5..000000000 --- a/src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h +++ /dev/null @@ -1,194 +0,0 @@ -#include "../btDeformableContactConstraint.h" -#include "btReducedDeformableBody.h" - -// ================= static constraints =================== -class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint -{ - public: - btReducedDeformableBody* m_rsb; - btScalar m_dt; - btVector3 m_ri; - btVector3 m_targetPos; - btVector3 m_impulseDirection; - btMatrix3x3 m_impulseFactorMatrix; - btScalar m_impulseFactor; - btScalar m_rhs; - btScalar m_appliedImpulse; - btScalar m_erp; - - btReducedDeformableStaticConstraint(btReducedDeformableBody* rsb, - btSoftBody::Node* node, - const btVector3& ri, - const btVector3& x0, - const btVector3& dir, - const btContactSolverInfo& infoGlobal, - btScalar dt); - // btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other); - btReducedDeformableStaticConstraint() {} - virtual ~btReducedDeformableStaticConstraint() {} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - // this calls reduced deformable body's applyFullSpaceImpulse - virtual void applyImpulse(const btVector3& impulse); - - btVector3 getDeltaVa() const; - - // virtual void applySplitImpulse(const btVector3& impulse) {} -}; - -// ================= base contact constraints =================== -class btReducedDeformableRigidContactConstraint : public btDeformableRigidContactConstraint -{ - public: - bool m_collideStatic; // flag for collision with static object - bool m_collideMultibody; // flag for collision with multibody - - int m_nodeQueryIndex; - int m_solverBodyId; // for debugging - - btReducedDeformableBody* m_rsb; - btSolverBody* m_solverBody; - btScalar m_dt; - - btScalar m_appliedNormalImpulse; - btScalar m_appliedTangentImpulse; - btScalar m_appliedTangentImpulse2; - btScalar m_normalImpulseFactor; - btScalar m_tangentImpulseFactor; - btScalar m_tangentImpulseFactor2; - btScalar m_tangentImpulseFactorInv; - btScalar m_tangentImpulseFactorInv2; - btScalar m_rhs; - btScalar m_rhs_tangent; - btScalar m_rhs_tangent2; - - btScalar m_cfm; - btScalar m_cfm_friction; - btScalar m_erp; - btScalar m_erp_friction; - btScalar m_friction; - - btVector3 m_contactNormalA; // surface normal for rigid body (opposite direction as impulse) - btVector3 m_contactNormalB; // surface normal for reduced deformable body (opposite direction as impulse) - btVector3 m_contactTangent; // tangential direction of the relative velocity - btVector3 m_contactTangent2; // 2nd tangential direction of the relative velocity - btVector3 m_relPosA; // relative position of the contact point for A (rigid) - btVector3 m_relPosB; // relative position of the contact point for B - btMatrix3x3 m_impulseFactor; // total impulse matrix - - btVector3 m_bufferVelocityA; // velocity at the beginning of the iteration - btVector3 m_bufferVelocityB; - btVector3 m_linearComponentNormal; // linear components for the solver body - btVector3 m_angularComponentNormal; // angular components for the solver body - // since 2nd contact direction only applies to multibody, these components will never be used - btVector3 m_linearComponentTangent; - btVector3 m_angularComponentTangent; - - btReducedDeformableRigidContactConstraint(btReducedDeformableBody* rsb, - const btSoftBody::DeformableRigidContact& c, - const btContactSolverInfo& infoGlobal, - btScalar dt); - // btReducedDeformableRigidContactConstraint(const btReducedDeformableRigidContactConstraint& other); - btReducedDeformableRigidContactConstraint() {} - virtual ~btReducedDeformableRigidContactConstraint() {} - - void setSolverBody(const int bodyId, btSolverBody& solver_body); - - virtual void warmStarting() {} - - virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); - - void calculateTangentialImpulse(btScalar& deltaImpulse_tangent, - btScalar& appliedImpulse, - const btScalar rhs_tangent, - const btScalar tangentImpulseFactorInv, - const btVector3& tangent, - const btScalar lower_limit, - const btScalar upper_limit, - const btVector3& deltaV_rel); - - virtual void applyImpulse(const btVector3& impulse) {} - - virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later - - virtual btVector3 getVa() const; - virtual btVector3 getDeltaVa() const = 0; - virtual btVector3 getDeltaVb() const = 0; -}; - -// ================= node vs rigid constraints =================== -class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformableRigidContactConstraint -{ - public: - btSoftBody::Node* m_node; - - btReducedDeformableNodeRigidContactConstraint(btReducedDeformableBody* rsb, - const btSoftBody::DeformableNodeRigidContact& contact, - const btContactSolverInfo& infoGlobal, - btScalar dt); - // btReducedDeformableNodeRigidContactConstraint(const btReducedDeformableNodeRigidContactConstraint& other); - btReducedDeformableNodeRigidContactConstraint() {} - virtual ~btReducedDeformableNodeRigidContactConstraint() {} - - virtual void warmStarting(); - - // get the velocity of the deformable node in contact - virtual btVector3 getVb() const; - - // get the velocity change of the rigid body - virtual btVector3 getDeltaVa() const; - - // get velocity change of the node in contat - virtual btVector3 getDeltaVb() const; - - // get the split impulse velocity of the deformable face at the contact point - virtual btVector3 getSplitVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableNodeRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact); - } - - // this calls reduced deformable body's applyFullSpaceImpulse - virtual void applyImpulse(const btVector3& impulse); -}; - -// ================= face vs rigid constraints =================== -class btReducedDeformableFaceRigidContactConstraint : public btReducedDeformableRigidContactConstraint -{ - public: - btSoftBody::Face* m_face; - bool m_useStrainLimiting; - - btReducedDeformableFaceRigidContactConstraint(btReducedDeformableBody* rsb, - const btSoftBody::DeformableFaceRigidContact& contact, - const btContactSolverInfo& infoGlobal, - btScalar dt, - bool useStrainLimiting); - // btReducedDeformableFaceRigidContactConstraint(const btReducedDeformableFaceRigidContactConstraint& other); - btReducedDeformableFaceRigidContactConstraint() {} - virtual ~btReducedDeformableFaceRigidContactConstraint() {} - - // get the velocity of the deformable face at the contact point - virtual btVector3 getVb() const; - - // get the split impulse velocity of the deformable face at the contact point - virtual btVector3 getSplitVb() const; - - // get the velocity change of the input soft body node in the constraint - virtual btVector3 getDv(const btSoftBody::Node*) const; - - // cast the contact to the desired type - const btSoftBody::DeformableFaceRigidContact* getContact() const - { - return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact); - } - - // this calls reduced deformable body's applyFullSpaceImpulse - virtual void applyImpulse(const btVector3& impulse); -};
\ No newline at end of file diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index c12eef5fe..945276801 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -24,11 +24,6 @@ SET(BulletSoftBody_SRCS btDeformableMultiBodyDynamicsWorld.cpp btDeformableContactConstraint.cpp poly34.cpp - - BulletReducedDeformableBody/btReducedDeformableBody.cpp - BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp - BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp - BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp ) @@ -68,11 +63,6 @@ SET(BulletSoftBody_HDRS poly34.h btSoftBodySolverVertexBuffer.h - - BulletReducedDeformableBody/btReducedDeformableBody.h - BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h - BulletReducedDeformableBody/btReducedDeformableBodySolver.h - BulletReducedDeformableBody/btReducedDeformableContactConstraint.h ) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 60b6fe388..eb05b9f01 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -25,18 +25,12 @@ #include "btDeformableNeoHookeanForce.h" #include "btDeformableContactProjection.h" #include "btPreconditioner.h" -// #include "btDeformableMultiBodyDynamicsWorld.h" +#include "btDeformableMultiBodyDynamicsWorld.h" #include "LinearMath/btQuickprof.h" class btDeformableBackwardEulerObjective { public: - enum _ - { - Mass_preconditioner, - KKT_preconditioner - }; - typedef btAlignedObjectArray<btVector3> TVStack; btScalar m_dt; btAlignedObjectArray<btDeformableLagrangianForce*> m_lf; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 4e9df5f83..27f10a92f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -23,7 +23,6 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); - m_reducedSolver = false; } btDeformableBodySolver::~btDeformableBodySolver() @@ -402,7 +401,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt) } } } - applyExplicitForce(); + m_objective->applyExplicitForce(m_residual); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -505,89 +504,3 @@ void btDeformableBodySolver::setLineSearch(bool lineSearch) { m_lineSearch = lineSearch; } - -void btDeformableBodySolver::applyExplicitForce() -{ - m_objective->applyExplicitForce(m_residual); -} - -void btDeformableBodySolver::applyTransforms(btScalar timeStep) -{ - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btSoftBody::Node& node = psb->m_nodes[j]; - btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / timeStep; - for (int c = 0; c < 3; c++) - { - if (node.m_v[c] > clampDeltaV) - { - node.m_v[c] = clampDeltaV; - } - if (node.m_v[c] < -clampDeltaV) - { - node.m_v[c] = -clampDeltaV; - } - } - node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); - node.m_q = node.m_x; - node.m_vn = node.m_v; - } - // enforce anchor constraints - for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) - { - btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; - btSoftBody::Node* n = a.m_node; - n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; - - // update multibody anchor info - if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); - if (multibodyLinkCol) - { - btVector3 nrm; - const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); - const btTransform& wtr = multibodyLinkCol->getWorldTransform(); - psb->m_worldInfo->m_sparsesdf.Evaluate( - wtr.invXform(n->m_x), - shp, - nrm, - 0); - a.m_cti.m_normal = wtr.getBasis() * nrm; - btVector3 normal = a.m_cti.m_normal; - btVector3 t1 = generateUnitOrthogonalVector(normal); - btVector3 t2 = btCross(normal, t1); - btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); - - btScalar* J_n = &jacobianData_normal.m_jacobians[0]; - btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; - btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; - - btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; - btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - - btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), - t1.getX(), t1.getY(), t1.getZ(), - t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - a.m_c0 = rot.transpose() * local_impulse_matrix * rot; - a.jacobianData_normal = jacobianData_normal; - a.jacobianData_t1 = jacobianData_t1; - a.jacobianData_t2 = jacobianData_t2; - a.t1 = t1; - a.t2 = t2; - } - } - } - psb->interpolateRenderMesh(); - } -}
\ No newline at end of file diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 68354f199..ae674d6e8 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -24,8 +24,8 @@ #include "btConjugateResidual.h" #include "btConjugateGradient.h" struct btCollisionObjectWrapper; -// class btDeformableBackwardEulerObjective; -// class btDeformableMultiBodyDynamicsWorld; +class btDeformableBackwardEulerObjective; +class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { @@ -46,7 +46,6 @@ protected: int m_maxNewtonIterations; // max number of newton iterations btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance bool m_lineSearch; // If true, use newton's method with line search under implicit scheme - bool m_reducedSolver; // flag for reduced soft body solver public: // handles data related to objective function btDeformableBackwardEulerObjective* m_objective; @@ -69,18 +68,11 @@ public: // solve the momentum equation virtual void solveDeformableConstraints(btScalar solverdt); - // set gravity (get from deformable world) - virtual void setGravity(const btVector3& gravity) - { - // for full deformable object, we don't store gravity in the solver - // this function is overriden in the reduced deformable object - } - // resize/clear data structures - virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt); + void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt); // set up contact constraints - virtual void setConstraints(const btContactSolverInfo& infoGlobal); + void setConstraints(const btContactSolverInfo& infoGlobal); // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion virtual void predictMotion(btScalar solverdt); @@ -93,7 +85,7 @@ public: void backupVelocity(); // set m_dv and m_backupVelocity to desired value to prepare for momentum solve - virtual void setupDeformableSolve(bool implicit); + void setupDeformableSolve(bool implicit); // set the current velocity to that backed up in m_backupVelocity void revertVelocity(); @@ -158,62 +150,6 @@ public: // used in line search btScalar kineticEnergy(); - // add explicit force to the velocity in the objective class - virtual void applyExplicitForce(); - - // execute position/velocity update and apply anchor constraints in the integrateTransforms from the Dynamics world - virtual void applyTransforms(btScalar timeStep); - - virtual void setStrainLimiting(bool opt) - { - m_objective->m_projection.m_useStrainLimiting = opt; - } - - virtual void setPreconditioner(int opt) - { - switch (opt) - { - case btDeformableBackwardEulerObjective::Mass_preconditioner: - m_objective->m_preconditioner = m_objective->m_massPreconditioner; - break; - - case btDeformableBackwardEulerObjective::KKT_preconditioner: - m_objective->m_preconditioner = m_objective->m_KKTPreconditioner; - break; - - default: - btAssert(false); - break; - } - } - - virtual btAlignedObjectArray<btDeformableLagrangianForce*>* getLagrangianForceArray() - { - return &(m_objective->m_lf); - } - - virtual const btAlignedObjectArray<btSoftBody::Node*>* getIndices() - { - return m_objective->getIndices(); - } - - virtual void setProjection() - { - m_objective->m_projection.setProjection(); - } - - virtual void setLagrangeMultiplier() - { - m_objective->m_projection.setLagrangeMultiplier(); - } - - virtual bool isReducedSolver() - { - return m_reducedSolver; - } - - virtual void deformableBodyInternalWriteBack() {} - // unused functions virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {} virtual void solveConstraints(btScalar dt) {} diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 2a0e5ec34..631fd5fbe 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -14,16 +14,11 @@ */ #include "btDeformableMultiBodyConstraintSolver.h" -#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h" #include <iostream> -#include <thread> // override the iterations method to include deformable/multibody contact btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { { - // pair deformable body with solver body - pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal); - ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); @@ -42,10 +37,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal); - - // std::cout << "------------Iteration " << iteration << "------------\n"; - // std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n"; - if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF @@ -60,9 +51,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_analyticsData.m_numBodies = numBodies; m_analyticsData.m_numContactManifolds = numManifolds; m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual; - - m_deformableSolver->deformableBodyInternalWriteBack(); - // std::cout << "[===================Next Step===================]\n"; break; } } @@ -90,12 +78,6 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { - // reduced soft body solver directly modifies the solver body - if (m_deformableSolver->isReducedSolver()) - { - return; - } - for (int i = 0; i < numBodies; i++) { int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); @@ -112,12 +94,6 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject* void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) { - // reduced soft body solver directly modifies the solver body - if (m_deformableSolver->isReducedSolver()) - { - return; - } - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) { btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; @@ -129,53 +105,6 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS } } - -void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal) -{ - if (!m_deformableSolver->isReducedSolver()) - { - return; - } - - btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver); - - for (int i = 0; i < numDeformableBodies; ++i) - { - for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k) - { - btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k]; - - if (!constraint.m_contact->m_cti.m_colObj->isStaticObject()) - { - btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj); - - // object index in the solver body pool - int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep); - - const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]); - if (body && body->getInvMass()) - { - // std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n"; - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - constraint.setSolverBody(bodyId, solverBody); - } - } - } - - // for (int j = 0; j < numBodies; j++) - // { - // int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep); - - // btRigidBody* body = btRigidBody::upcast(bodies[j]); - // if (body && body->getInvMass()) - // { - // btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - // m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody); - // } - // } - } -} - void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index 28733fa49..94aabce83 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -43,9 +43,6 @@ protected: // write the velocity of the underlying rigid body to the the the solver body void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal); - // let each deformable body knows which solver body is in constact - void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal); - virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 030cbaf90..bc7b31273 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -94,7 +94,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t beforeSolverCallbacks(timeStep); - // ///solve contact constraints and then deformable bodies momemtum equation + ///solve contact constraints and then deformable bodies momemtum equation solveConstraints(timeStep); afterSolverCallbacks(timeStep); @@ -298,7 +298,83 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) BT_PROFILE("integrateTransforms"); positionCorrection(timeStep); btMultiBodyDynamicsWorld::integrateTransforms(timeStep); - m_deformableBodySolver->applyTransforms(timeStep); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / timeStep; + for (int c = 0; c < 3; c++) + { + if (node.m_v[c] > clampDeltaV) + { + node.m_v[c] = clampDeltaV; + } + if (node.m_v[c] < -clampDeltaV) + { + node.m_v[c] = -clampDeltaV; + } + } + node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv); + node.m_q = node.m_x; + node.m_vn = node.m_v; + } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } + } + psb->interpolateRenderMesh(); + } } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) @@ -315,9 +391,9 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // set up the directions in which the velocity does not change in the momentum solve if (m_useProjection) - m_deformableBodySolver->setProjection(); + m_deformableBodySolver->m_objective->m_projection.setProjection(); else - m_deformableBodySolver->setLagrangeMultiplier(); + m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); // for explicit scheme, m_backupVelocity = v_{n+1}^* // for implicit scheme, m_backupVelocity = v_n @@ -443,12 +519,6 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time m_deformableBodySolver->predictMotion(timeStep); } -void btDeformableMultiBodyDynamicsWorld::setGravity(const btVector3& gravity) -{ - btDiscreteDynamicsWorld::setGravity(gravity); - m_deformableBodySolver->setGravity(gravity); -} - void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { m_internalTime += timeStep; @@ -463,14 +533,14 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) if (m_useProjection) { m_deformableBodySolver->m_useProjection = true; - m_deformableBodySolver->setStrainLimiting(true); - m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner); + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; } else { m_deformableBodySolver->m_useProjection = false; - m_deformableBodySolver->setStrainLimiting(false); - m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner); + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; } } @@ -612,7 +682,7 @@ void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray(); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; bool added = false; for (int i = 0; i < forces.size(); ++i) { @@ -626,14 +696,14 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL if (!added) { force->addSoftBody(psb); - force->setIndices(m_deformableBodySolver->getIndices()); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); forces.push_back(force); } } void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray(); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; int removed_index = -1; for (int i = 0; i < forces.size(); ++i) { @@ -651,7 +721,7 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb) { - btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray(); + btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf; for (int i = 0; i < forces.size(); ++i) { forces[i]->removeSoftBody(psb); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 5971ecd9a..4b7069aac 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -19,7 +19,7 @@ #include "btSoftMultiBodyDynamicsWorld.h" #include "btDeformableLagrangianForce.h" #include "btDeformableMassSpringForce.h" -// #include "btDeformableBodySolver.h" +#include "btDeformableBodySolver.h" #include "btDeformableMultiBodyConstraintSolver.h" #include "btSoftBodyHelpers.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" @@ -121,8 +121,6 @@ public: return m_sbi; } - virtual void setGravity(const btVector3& gravity); - void reinitialize(btScalar timeStep); void applyRigidBodyGravity(btScalar timeStep); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 933f3edc5..eafde0d22 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -231,9 +231,6 @@ void btSoftBody::initDefaults() m_gravityFactor = 1; m_cacheBarycenter = false; m_fdbvnt = 0; - - // reduced flag - m_reducedModel = false; } // diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index f91640acb..043f27db0 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -799,7 +799,6 @@ public: typedef btAlignedObjectArray<Material*> tMaterialArray; typedef btAlignedObjectArray<Joint*> tJointArray; typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray; - typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix; // // Fields @@ -858,8 +857,6 @@ public: btScalar m_restLengthScale; - bool m_reducedModel; // Reduced deformable model flag - // // Api // @@ -1006,15 +1003,15 @@ public: /* Get best fit rigid transform */ btTransform getRigidTransform(); /* Transform to given pose */ - virtual void transformTo(const btTransform& trs); + void transformTo(const btTransform& trs); /* Transform */ - virtual void transform(const btTransform& trs); + void transform(const btTransform& trs); /* Translate */ - virtual void translate(const btVector3& trs); + void translate(const btVector3& trs); /* Rotate */ - virtual void rotate(const btQuaternion& rot); + void rotate(const btQuaternion& rot); /* Scale */ - virtual void scale(const btVector3& scl); + void scale(const btVector3& scl); /* Get link resting lengths scale */ btScalar getRestLengthScale(); /* Scale resting length of all springs */ @@ -1104,13 +1101,6 @@ public: void setZeroVelocity(); bool wantsSleeping(); - virtual btMatrix3x3 getImpulseFactor(int n_node) - { - btMatrix3x3 tmp; - tmp.setIdentity(); - return tmp; - } - // // Functionality to deal with new accelerated solvers. // diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 137258675..ec052a206 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1691,19 +1691,12 @@ struct btSoftColliders if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - const btVector3 ra = n.m_x - wtr.getOrigin(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - if (psb->m_reducedModel) - { - c.m_c0 = MassMatrix(imb, iwi, ra); //impulse factor K of the rigid body only (not the inverse) - } - else - { - c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); - // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); - } + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); + // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) @@ -1731,16 +1724,7 @@ struct btSoftColliders t1.getX(), t1.getY(), t1.getZ(), t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - - btMatrix3x3 local_impulse_matrix; - if (psb->m_reducedModel) - { - local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof); - } - else - { - local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); - } + btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); c.m_c0 = rot.transpose() * local_impulse_matrix * rot; c.jacobianData_normal = jacobianData_normal; c.jacobianData_t1 = jacobianData_t1; diff --git a/src/BulletSoftBody/btSoftBodySolvers.h b/src/BulletSoftBody/btSoftBodySolvers.h index 7eafc6c31..cdd069849 100644 --- a/src/BulletSoftBody/btSoftBodySolvers.h +++ b/src/BulletSoftBody/btSoftBodySolvers.h @@ -36,8 +36,7 @@ public: CL_SIMD_SOLVER, DX_SOLVER, DX_SIMD_SOLVER, - DEFORMABLE_SOLVER, - REDUCED_DEFORMABLE_SOLVER + DEFORMABLE_SOLVER }; protected: |