diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp index ca7bf89c6..0987927a2 100644 --- a/examples/ReducedDeformableDemo/FreeFall.cpp +++ b/examples/ReducedDeformableDemo/FreeFall.cpp @@ -33,7 +33,7 @@ static btScalar damping_alpha = 0.0; static btScalar damping_beta = 0.0; static btScalar COLLIDING_VELOCITY = 0; static int start_mode = 6; -static int num_modes = 40; +static int num_modes = 20; class FreeFall : public CommonDeformableBodyBase { @@ -56,14 +56,14 @@ public: void resetCamera() { - // float dist = 6; - // float pitch = -20; - // float yaw = 90; - // float targetPos[3] = {0, 2, 0}; - float dist = 20; - float pitch = -30; - float yaw = 125; - float targetPos[3] = {-2, 0, 2}; + float dist = 10; + float pitch = -20; + float yaw = 90; + float targetPos[3] = {0, 2, 0}; + // float dist = 20; + // float pitch = -30; + // float yaw = 125; + // float targetPos[3] = {-2, 0, 2}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } @@ -143,6 +143,7 @@ void FreeFall::initPhysics() m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + // m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER; // create volumetric reduced deformable body { @@ -150,11 +151,12 @@ void FreeFall::initPhysics() getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.01); + rsb->scale(btVector3(1, 1, 0.5)); btTransform init_transform; init_transform.setIdentity(); init_transform.setOrigin(btVector3(0, 10, 0)); - // init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 2.0)); + init_transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 4.0)); rsb->transform(init_transform); rsb->setStiffnessScale(20); @@ -173,7 +175,7 @@ void FreeFall::initPhysics() // rsb->setRigidAngularVelocity(btVector3(1, 0, 0)); } // add a few rigid bodies - Ctor_RbUpStack(); + // Ctor_RbUpStack(); // create a static rigid box as the ground { // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50))); diff --git a/examples/ReducedDeformableDemo/ReducedCollide.cpp b/examples/ReducedDeformableDemo/ReducedCollide.cpp index 4f3b5d141..678b0938e 100644 --- a/examples/ReducedDeformableDemo/ReducedCollide.cpp +++ b/examples/ReducedDeformableDemo/ReducedCollide.cpp @@ -30,10 +30,10 @@ // static btScalar E = 50; // static btScalar nu = 0.3; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.0001; +static btScalar damping_beta = 0.0; static btScalar COLLIDING_VELOCITY = 4; static int start_mode = 6; -static int num_modes = 1; +static int num_modes = 20; class ReducedCollide : public CommonDeformableBodyBase { @@ -75,10 +75,10 @@ public: void Ctor_RbUpStack() { - // float mass = 55; - float mass = 8; + float mass = 55; + // float mass = 8; - btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1)); + btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5)); btVector3 localInertia(0, 0, 0); if (mass != 0.f) shape->calculateLocalInertia(mass, localInertia); @@ -205,10 +205,11 @@ void ReducedCollide::initPhysics() // create volumetric reduced deformable body { - btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedBeam(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); + btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); getDeformableDynamicsWorld()->addSoftBody(rsb); rsb->getCollisionShape()->setMargin(0.1); + rsb->scale(btVector3(1, 1, 1)); btTransform init_transform; init_transform.setIdentity(); @@ -216,7 +217,7 @@ void ReducedCollide::initPhysics() // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0)); rsb->transform(init_transform); - rsb->setStiffnessScale(100); + rsb->setStiffnessScale(20); rsb->setDamping(damping_alpha, damping_beta); rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects @@ -298,9 +299,9 @@ void ReducedCollide::initPhysics() getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1; getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2; getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); - getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6; + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false; - getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 200; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); // { diff --git a/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp b/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp index 67aa3eeed..b0d832e51 100644 --- a/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp +++ b/examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp @@ -39,7 +39,7 @@ static btScalar sGripperVerticalVelocity = 0.f; static btScalar sGripperClosingTargetVelocity = 0.f; static btScalar damping_alpha = 0.0; -static btScalar damping_beta = 0.0; +static btScalar damping_beta = 0.001; static int start_mode = 6; static int num_modes = 20; static float friction = 1.; @@ -126,14 +126,14 @@ public: if (dofIndex == 6) { motor->setVelocityTarget(-fingerTargetVelocities[1], 1); - motor->setMaxAppliedImpulse(70); + motor->setMaxAppliedImpulse(100); } if (dofIndex == 7) { motor->setVelocityTarget(fingerTargetVelocities[1], 1); - motor->setMaxAppliedImpulse(70); + motor->setMaxAppliedImpulse(100); } - motor->setMaxAppliedImpulse(50); + motor->setMaxAppliedImpulse(200); } } dofIndex += mb->getLink(link).m_dofCount; @@ -320,15 +320,15 @@ void ReducedMotorGrasp::initPhysics() btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedCube(getDeformableDynamicsWorld()->getWorldInfo(), start_mode, num_modes); getDeformableDynamicsWorld()->addSoftBody(rsb); - rsb->getCollisionShape()->setMargin(0.05); + rsb->getCollisionShape()->setMargin(0.01); btTransform init_transform; init_transform.setIdentity(); init_transform.setOrigin(btVector3(0, 0.5, 0)); - init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0)); + // init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0)); rsb->transform(init_transform); - rsb->setStiffnessScale(20); + rsb->setStiffnessScale(25); rsb->setDamping(damping_alpha, damping_beta); // rsb->setRigidVelocity(btVector3(0, 1, 0)); @@ -359,8 +359,8 @@ void ReducedMotorGrasp::initPhysics() SliderParams slider("Moving velocity", &sGripperVerticalVelocity); // slider.m_minVal = -.02; // slider.m_maxVal = .02; - slider.m_minVal = -.2; - slider.m_maxVal = .2; + slider.m_minVal = -.5; + slider.m_maxVal = .5; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp index 104dc2def..4f0c9c9fe 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp @@ -8,7 +8,7 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m) : btSoftBody(worldInfo, node_count, x, m) { - m_rigidOnly = true; //! only use rigid frame to debug + m_rigidOnly = false; //! only use rigid frame to debug // reduced deformable m_reducedModel = true; @@ -26,7 +26,8 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co m_internalDeltaAngularVelocity.setZero(); m_angularFactor.setValue(1, 1, 1); m_linearFactor.setValue(1, 1, 1); - m_invInertiaLocal.setValue(1, 1, 1); + // m_invInertiaLocal.setValue(1, 1, 1); + m_invInertiaLocal.setIdentity(); m_mass = 0.0; m_inverseMass = 0.0; @@ -76,18 +77,18 @@ void btReducedSoftBody::setInertiaProps(const btVector3& inertia) { // TODO: only support box shape now // set local inertia - m_invInertiaLocal.setValue( - inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), - inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), - inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + // m_invInertiaLocal.setValue( + // inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), + // inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + // inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + updateLocalInertiaTensorFromNodes(); // update world inertia tensor btMatrix3x3 rotation; rotation.setIdentity(); updateInitialInertiaTensor(rotation); - // updateInitialInertiaTensorFromNodes(); updateInertiaTensor(); m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld; } @@ -367,7 +368,37 @@ void btReducedSoftBody::transform(const btTransform& trs) void btReducedSoftBody::scale(const btVector3& scl) { // scale the mesh - btSoftBody::scale(scl); + { + 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 btReducedSoftBody::updateRestNodalPositions() @@ -380,7 +411,7 @@ void btReducedSoftBody::updateRestNodalPositions() } } -void btReducedSoftBody::updateInitialInertiaTensorFromNodes() +void btReducedSoftBody::updateLocalInertiaTensorFromNodes() { btMatrix3x3 inertia_tensor; inertia_tensor.setZero(); @@ -396,12 +427,13 @@ void btReducedSoftBody::updateInitialInertiaTensorFromNodes() } } } - m_invInertiaTensorWorldInitial = inertia_tensor.inverse(); + m_invInertiaLocal = inertia_tensor.inverse(); } void btReducedSoftBody::updateInitialInertiaTensor(const btMatrix3x3& rotation) { - m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose(); + // m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose(); + m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose(); } void btReducedSoftBody::updateModesByRotation(const btMatrix3x3& rotation) @@ -658,7 +690,7 @@ void btReducedSoftBody::applyReducedDampingForce(const tDenseArray& reduce_vel) { for (int r = 0; r < m_nReduced; ++r) { - m_reducedForceDamping[r] = - m_dampingBeta * m_Kr[r] * reduce_vel[r]; + m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r]; } } diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h index ce46b6d67..fa750c6bb 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h @@ -46,7 +46,8 @@ class btReducedSoftBody : public btSoftBody btScalar m_angularDamping; // angular damping coefficient btVector3 m_linearFactor; btVector3 m_angularFactor; - btVector3 m_invInertiaLocal; + // btVector3 m_invInertiaLocal; + btMatrix3x3 m_invInertiaLocal; btTransform m_rigidTransformWorld; btMatrix3x3 m_invInertiaTensorWorldInitial; btMatrix3x3 m_invInertiaTensorWorld; @@ -122,8 +123,15 @@ class btReducedSoftBody : public btSoftBody // // various internal updates // - virtual void scale(const btVector3& scl); 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); + virtual void transformTo(const btTransform& trs) + { + btAssert(false); // not supported yet + } virtual void translate(const btVector3& trs) { btAssert(false); // use transform(). @@ -138,7 +146,7 @@ class btReducedSoftBody : public btSoftBody void updateInitialInertiaTensor(const btMatrix3x3& rotation); - void updateInitialInertiaTensorFromNodes(); + void updateLocalInertiaTensorFromNodes(); void updateInertiaTensor(); diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp index e05989c54..9a839f876 100644 --- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp +++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp @@ -273,11 +273,10 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de btReducedSoftBody* rsb = static_cast(m_softBodies[i]); // shuffle the order of applying constraint - // if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) + m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size()); + m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size()); + if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { - - m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size()); - m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size()); // fixed constraint order for (int j = 0; j < m_staticConstraints[i].size(); ++j) { @@ -287,11 +286,22 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j; - std::cout << m_orderContactConstraintPool[j] << '\n'; } 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)