From ec0e9892b52b02a48d85ab4c7e636cb24d7c41eb Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 6 Mar 2020 16:04:34 -0800 Subject: [PATCH] add slider demo --- .../CommonInterfaces/CommonRigidBodyBase.h | 17 ++- .../DeformableDemo/VolumetricDeformable.cpp | 94 +++++++++++- .../CollisionDispatch/btCollisionWorld.h | 30 ++++ .../btDeformableMultiBodyDynamicsWorld.h | 143 ++++++++++++++++++ .../btDeformableNeoHookeanForce.h | 48 +++++- 5 files changed, 327 insertions(+), 5 deletions(-) diff --git a/examples/CommonInterfaces/CommonRigidBodyBase.h b/examples/CommonInterfaces/CommonRigidBodyBase.h index fea34aec0..daaae2ba4 100644 --- a/examples/CommonInterfaces/CommonRigidBodyBase.h +++ b/examples/CommonInterfaces/CommonRigidBodyBase.h @@ -7,7 +7,7 @@ #include "CommonGUIHelperInterface.h" #include "CommonRenderInterface.h" #include "CommonCameraInterface.h" - +#include "BulletSoftBody/btSoftBody.h" #include "CommonGraphicsAppInterface.h" #include "CommonWindowInterface.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" @@ -343,6 +343,21 @@ struct CommonRigidBodyBase : public CommonExampleInterface p2p->m_setting.m_tau = 0.001f; } } + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (psb) + { + m_savedState = psb->getActivationState(); + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_dynamicsWorld->addConstraint(p2p, true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } // pickObject(pickPos, rayCallback.m_collisionObject); m_oldPickingPos = rayToWorld; diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 1f23071d4..4be1e090f 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -20,12 +20,16 @@ #include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "../CommonInterfaces/CommonParameterInterface.h" #include //printf debugging #include "../CommonInterfaces/CommonRigidBodyBase.h" #include "../Utils/b3ResourcePath.h" ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects. +static btScalar E = 100; +static btScalar nu = 0.3; +static btScalar damping = 0.1; struct TetraCube { @@ -35,6 +39,7 @@ struct TetraCube class VolumetricDeformable : public CommonRigidBodyBase { btAlignedObjectArray m_forces; + btDeformableNeoHookeanForce* m_neohookean; public: VolumetricDeformable(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper) @@ -58,6 +63,9 @@ public: void stepSimulation(float deltaTime) { + m_neohookean->setPoissonRatio(nu); + m_neohookean->setYoungsModulus(E); + m_neohookean->setDamping(damping); //use a smaller internal timestep, there are stability issues float internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); @@ -146,6 +154,8 @@ public: } } } + + virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld); }; void VolumetricDeformable::initPhysics() @@ -220,6 +230,7 @@ void VolumetricDeformable::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0.5; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(psb); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); @@ -227,6 +238,7 @@ void VolumetricDeformable::initPhysics() m_forces.push_back(gravity_force); btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.05); + m_neohookean = neohookean; getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); @@ -234,9 +246,87 @@ void VolumetricDeformable::initPhysics() getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); // add a few rigid bodies - Ctor_RbUpStack(4); - + Ctor_RbUpStack(4); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + { + SliderParams slider("Young's Modulus", &E); + slider.m_minVal = 0; + slider.m_maxVal = 200; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Poisson Ratio", &nu); + slider.m_minVal = 0; + slider.m_maxVal = 0.4; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Damping", &damping); + slider.m_minVal = 0.01; + slider.m_maxVal = 1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } +} + +bool VolumetricDeformable::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) +{ + if (getDeformableDynamicsWorld() == 0) + return false; + + btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld); + + rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest; + getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback); + if (rayCallback.hasHit()) + { + btVector3 pickPos = rayCallback.m_hitPointWorld; + btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject); + if (body) + { + //other exclusions? + if (!(body->isStaticObject() || body->isKinematicObject())) + { + m_pickedBody = body; + m_savedState = m_pickedBody->getActivationState(); + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); + //printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ()); + btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); + m_dynamicsWorld->addConstraint(p2p, true); + m_pickedConstraint = p2p; + btScalar mousePickClamping = 30.f; + p2p->m_setting.m_impulseClamp = mousePickClamping; + //very weak constraint for picking + p2p->m_setting.m_tau = 0.001f; + } + } + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (psb) + { + m_savedState = psb->getActivationState(); + m_pickedBody->setActivationState(DISABLE_DEACTIVATION); +// btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos; +// btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot); +// m_dynamicsWorld->addConstraint(p2p, true); +// m_pickedConstraint = p2p; +// btScalar mousePickClamping = 30.f; +// p2p->m_setting.m_impulseClamp = mousePickClamping; +// //very weak constraint for picking +// p2p->m_setting.m_tau = 0.001f; + } + + // pickObject(pickPos, rayCallback.m_collisionObject); + m_oldPickingPos = rayToWorld; + m_hitPos = pickPos; + m_oldPickingDist = (pickPos - rayFromWorld).length(); + // printf("hit !\n"); + //add p2p + } + return false; } void VolumetricDeformable::exitPhysics() diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index fd0e5b9bb..5f3e8c70a 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -263,6 +263,36 @@ public: return rayResult.m_hitFraction; } }; + + struct ClosestRayResultCallbackWithInfo : public ClosestRayResultCallback + { + ClosestRayResultCallbackWithInfo(const btVector3& rayFromWorld, const btVector3& rayToWorld) + : ClosestRayResultCallback(rayFromWorld, rayToWorld) + { + } + LocalShapeInfo* m_localShapeInfo; + + virtual btScalar addSingleResult(LocalRayResult& rayResult, bool normalInWorldSpace) + { + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_collisionObject = rayResult.m_collisionObject; + m_localShapeInfo = rayResult.m_localShapeInfo; + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } + else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; + } + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } + }; struct AllHitsRayResultCallback : public RayResultCallback { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index e55b02d01..5d78d4097 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -161,6 +161,149 @@ public: void applyRepulsionForce(btScalar timeStep); void performGeometricCollisions(btScalar timeStep); + + struct btDeformableSingleRayCallback : public btBroadphaseRayCallback + { + btVector3 m_rayFromWorld; + btVector3 m_rayToWorld; + btTransform m_rayFromTrans; + btTransform m_rayToTrans; + btVector3 m_hitNormal; + + const btDeformableMultiBodyDynamicsWorld* m_world; + btCollisionWorld::RayResultCallback& m_resultCallback; + + btDeformableSingleRayCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld, const btDeformableMultiBodyDynamicsWorld* world, btCollisionWorld::RayResultCallback& resultCallback) + : m_rayFromWorld(rayFromWorld), + m_rayToWorld(rayToWorld), + m_world(world), + m_resultCallback(resultCallback) + { + m_rayFromTrans.setIdentity(); + m_rayFromTrans.setOrigin(m_rayFromWorld); + m_rayToTrans.setIdentity(); + m_rayToTrans.setOrigin(m_rayToWorld); + + btVector3 rayDir = (rayToWorld - rayFromWorld); + + rayDir.normalize(); + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + m_signs[0] = m_rayDirectionInverse[0] < 0.0; + m_signs[1] = m_rayDirectionInverse[1] < 0.0; + m_signs[2] = m_rayDirectionInverse[2] < 0.0; + + m_lambda_max = rayDir.dot(m_rayToWorld - m_rayFromWorld); + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + ///terminate further ray tests, once the closestHitFraction reached zero + if (m_resultCallback.m_closestHitFraction == btScalar(0.f)) + return false; + + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + + //only perform raycast if filterMask matches + if (m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + //btVector3 collisionObjectAabbMin,collisionObjectAabbMax; +#if 0 +#ifdef RECALCULATE_AABB + btVector3 collisionObjectAabbMin,collisionObjectAabbMax; + collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax); +#else + //getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax); + const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin; + const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax; +#endif +#endif + //btScalar hitLambda = m_resultCallback.m_closestHitFraction; + //culling already done by broadphase + //if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal)) + { + m_world->rayTestSingle(m_rayFromTrans, m_rayToTrans, + collisionObject, + collisionObject->getCollisionShape(), + collisionObject->getWorldTransform(), + m_resultCallback); + } + } + return true; + } + }; + + + + void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const + { + BT_PROFILE("rayTest"); + /// use the broadphase to accelerate the search for objects, based on their aabb + /// and for each object with ray-aabb overlap, perform an exact ray test + btDeformableSingleRayCallback rayCB(rayFromWorld, rayToWorld, this, resultCallback); + +#ifndef USE_BRUTEFORCE_RAYBROADPHASE + m_broadphasePairCache->rayTest(rayFromWorld, rayToWorld, rayCB); +#else + for (int i = 0; i < this->getNumCollisionObjects(); i++) + { + rayCB.process(m_collisionObjects[i]->getBroadphaseHandle()); + } +#endif //USE_BRUTEFORCE_RAYBROADPHASE + } + + void rayTestSingle(const btTransform& rayFromTrans, const btTransform& rayToTrans, + btCollisionObject* collisionObject, + const btCollisionShape* collisionShape, + const btTransform& colObjWorldTransform, + RayResultCallback& resultCallback) const + { + if (collisionShape->isSoftBody()) + { + btSoftBody* softBody = btSoftBody::upcast(collisionObject); + if (softBody) + { + btSoftBody::sRayCast softResult; + if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) + { + if (softResult.fraction <= resultCallback.m_closestHitFraction) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = 0; + shapeInfo.m_triangleIndex = softResult.index; + // get the normal + btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); + btVector3 normal = -rayDir; + normal.normalize(); + + if (softResult.feature == btSoftBody::eFeature::Face) + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) + { + // normal always point toward origin of the ray + normal = -normal; + } + } + + btCollisionWorld::LocalRayResult rayResult(collisionObject, + &shapeInfo, + normal, + softResult.fraction); + bool normalInWorldSpace = true; + resultCallback.addSingleResult(rayResult, normalInWorldSpace); + } + } + } + } + else + { + btCollisionWorld::rayTestSingle(rayFromTrans, rayToTrans, collisionObject, collisionShape, colObjWorldTransform, resultCallback); + } + } }; #endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 3d06e304d..2fb1c0b99 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -24,21 +24,65 @@ class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { public: typedef btAlignedObjectArray TVStack; - btScalar m_mu, m_lambda; + btScalar m_mu, m_lambda; // Lame Parameters + btScalar m_E, m_nu; // Young's modulus and Poisson ratio btScalar m_mu_damp, m_lambda_damp; btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) { btScalar damping = 0.05; m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); } btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) { m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); } - + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3*m_lambda + 2*m_mu)/(m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1- 2*m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping) + { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); + } + virtual void addScaledForces(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force);