diff options
author | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-07-18 11:34:06 -0700 |
---|---|---|
committer | Xuchen Han <xuchenhan@xuchenhan-macbookpro.roam.corp.google.com> | 2019-08-02 13:12:59 -0700 |
commit | dc10336d45eeaf5d06de4bdabd7df3bc4d990d6f (patch) | |
tree | 2e933d9b87b74552bb21d1c5a9be8bf2902d2b4c | |
parent | 3430192db798c773036d247a460da520d0c26762 (diff) | |
download | bullet3-dc10336d45eeaf5d06de4bdabd7df3bc4d990d6f.tar.gz |
code clean up + check in examples
-rw-r--r-- | examples/BasicDemo/BasicExample.cpp | 6 | ||||
-rw-r--r-- | examples/DeformableContact/DeformableContact.cpp | 412 | ||||
-rw-r--r-- | examples/DeformableContact/DeformableContact.h | 20 | ||||
-rw-r--r-- | examples/DeformableDemo/DeformableDemo.cpp | 90 | ||||
-rw-r--r-- | examples/ExampleBrowser/ExampleEntries.cpp | 8 | ||||
-rw-r--r-- | examples/Pinch/Pinch.cpp | 297 | ||||
-rw-r--r-- | examples/Pinch/Pinch.h | 20 | ||||
-rw-r--r-- | examples/VolumetricDeformable/VolumetricDeformable.cpp | 294 | ||||
-rw-r--r-- | examples/VolumetricDeformable/VolumetricDeformable.h | 20 | ||||
-rw-r--r-- | src/BulletSoftBody/btBackwardEulerObjective.cpp | 47 | ||||
-rw-r--r-- | src/BulletSoftBody/btBackwardEulerObjective.h | 118 | ||||
-rw-r--r-- | src/BulletSoftBody/btCGProjection.h | 49 | ||||
-rw-r--r-- | src/BulletSoftBody/btConjugateGradient.h | 8 | ||||
-rw-r--r-- | src/BulletSoftBody/btContactProjection.cpp | 201 | ||||
-rw-r--r-- | src/BulletSoftBody/btContactProjection.h | 160 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.cpp | 78 | ||||
-rw-r--r-- | src/BulletSoftBody/btDeformableBodySolver.h | 69 | ||||
-rw-r--r-- | src/BulletSoftBody/btPreconditioner.h | 67 |
18 files changed, 1552 insertions, 412 deletions
diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp index 67f670d07..136aefb20 100644 --- a/examples/BasicDemo/BasicExample.cpp +++ b/examples/BasicDemo/BasicExample.cpp @@ -16,9 +16,9 @@ subject to the following restrictions: #include "BasicExample.h" #include "btBulletDynamicsCommon.h" -#define ARRAY_SIZE_Y 5 -#define ARRAY_SIZE_X 5 -#define ARRAY_SIZE_Z 5 +#define ARRAY_SIZE_Y 1 +#define ARRAY_SIZE_X 1 +#define ARRAY_SIZE_Z 1 #include "LinearMath/btVector3.h" #include "LinearMath/btAlignedObjectArray.h" diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp new file mode 100644 index 000000000..b70751e2f --- /dev/null +++ b/examples/DeformableContact/DeformableContact.cpp @@ -0,0 +1,412 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///create 125 (5x5x5) dynamic object +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + +//maximum number of objects (and allow user to shoot additional boxes) +#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024) + +///scaling of the objects (0.1 = 20 centimeter boxes ) +#define SCALING 1. +#define START_POS_X -5 +#define START_POS_Y -5 +#define START_POS_Z -3 + +#include "DeformableContact.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" +#include "../SoftDemo/BunnyMesh.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "../Utils/b3ResourcePath.h" +///The DeformableContact demo deformable bodies self-collision +static bool g_floatingBase = true; +static float friction = 1.; +class DeformableContact : public CommonMultiBodyBase +{ + btMultiBody* m_multiBody; + btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks; +public: + + struct TetraBunny + { + #include "../SoftDemo/bunny.inl" + }; + + + DeformableContact(struct GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) + { + } + + virtual ~DeformableContact() + { + } + + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 15; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + virtual void stepSimulation(float deltaTime); + + btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false); + + void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); + + + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonMultiBodyBase::renderScene(); + btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void DeformableContact::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + btMultiBodyConstraintSolver* sol; + sol = new btMultiBodyConstraintSolver; + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -40, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body,1,1+2); + } + + + { + bool damping = true; + bool gyro = true; + int numLinks = 3; + bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = true; + btVector3 linkHalfExtents(1, 1, 1); + btVector3 baseHalfExtents(1, 1, 1); + + btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); + + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if (!damping) + { + mbC->setLinearDamping(0.f); + mbC->setAngularDamping(0.f); + } + else + { + mbC->setLinearDamping(0.1f); + mbC->setAngularDamping(0.9f); + } + + if (numLinks > 0) + { + btScalar q0 = 0.f * SIMD_PI / 180.f; + if (!spherical) + { + mbC->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents); + } + + // create a patch of cloth + { + const btScalar s = 6; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btVector3(+s, 0, -s), + btVector3(-s, 0, +s), + btVector3(+s, 0, +s), + 20,20, + 1 + 2 + 4 + 8, true); + + psb->getCollisionShape()->setMargin(0.25); + psb->generateBendingConstraints(2); + psb->setTotalMass(.5); + psb->setDampingCoefficient(0.01); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0; + getDeformableDynamicsWorld()->addSoftBody(psb); + + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableContact::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + +void DeformableContact::stepSimulation(float deltaTime) +{ +// getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime); + m_dynamicsWorld->stepSimulation(deltaTime, 4, 1./240.); +} + + +btMultiBody* DeformableContact::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = .05f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + + btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + // pMultiBody->setBaseVel(vel); + + //init the links + btVector3 hingeJointAxis(1, 0, 0); + float linkMass = .05f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI / 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for (int i = 0; i < numLinks; ++i) + { + if (!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); + else + //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void DeformableContact::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray<btQuaternion> world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray<btVector3> local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + + if (1) + { + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + + pWorld->addCollisionObject(col, 2, 1 + 2); + + col->setFriction(friction); + pMultiBody->setBaseCollider(col); + } + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + btVector3 posr = local_origin[i + 1]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + + btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(friction); + pWorld->addCollisionObject(col, 2, 1 + 2); + + pMultiBody->getLink(i).m_collider = col; + } +} +class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableContact(options.m_guiHelper); +} + + diff --git a/examples/DeformableContact/DeformableContact.h b/examples/DeformableContact/DeformableContact.h new file mode 100644 index 000000000..591c6bab1 --- /dev/null +++ b/examples/DeformableContact/DeformableContact.h @@ -0,0 +1,20 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef _DEFORMABLE_CONTACT_H +#define _DEFORMABLE_CONTACT_H + +class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_CONTACT_H diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index 990bdd7b9..7ce94a314 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -35,6 +35,7 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodyHelpers.h" #include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include <stdio.h> //printf debugging #include "../CommonInterfaces/CommonRigidBodyBase.h" @@ -59,17 +60,23 @@ public: void resetCamera() { - float dist = 15; + float dist = 20; float pitch = -45; float yaw = 100; - float targetPos[3] = {0, -5, 0}; + float targetPos[3] = {0, -3, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } void Ctor_RbUpStack(int count) { - float mass = 1; + float mass = 0.2; btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); @@ -84,18 +91,30 @@ public: btCollisionShape* shape[] = { new btBoxShape(btVector3(1, 1, 1)), - cylinderCompound, - new btSphereShape(0.75) - +// new btSphereShape(0.75), +// cylinderCompound }; - static const int nshapes = sizeof(shape) / sizeof(shape[0]); - for (int i = 0; i < count; ++i) - { - btTransform startTransform; - startTransform.setIdentity(); - startTransform.setOrigin(btVector3(0, 3 + 3 * i, 0)); - createRigidBody(mass, startTransform, shape[i % nshapes]); - } +// static const int nshapes = sizeof(shape) / sizeof(shape[0]); +// for (int i = 0; i < count; ++i) +// { +// btTransform startTransform; +// startTransform.setIdentity(); +// startTransform.setOrigin(btVector3(0, 2+ 2 * i, 0)); +// startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); +// createRigidBody(mass, startTransform, shape[i % nshapes]); +// } + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(1, 2, 1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(1, 2, -1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(-1, 2, 1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(-1, 2, -1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(0, 4, 0)); + createRigidBody(mass, startTransform, shape[0]); } virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const @@ -143,14 +162,16 @@ void DeformableDemo::initPhysics() btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) - btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); m_solver = sol; - m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, deformableBodySolver); + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); deformableBodySolver->setWorld(getDeformableDynamicsWorld()); // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); - getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + +// getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { @@ -161,8 +182,8 @@ void DeformableDemo::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -50, 0)); - groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0)); + groundTransform.setOrigin(btVector3(0, -30, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -177,7 +198,7 @@ void DeformableDemo::initPhysics() btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); - body->setFriction(.5); + body->setFriction(1); //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); @@ -185,24 +206,37 @@ void DeformableDemo::initPhysics() // create a piece of cloth { + bool onGround = true; const btScalar s = 4; - btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), btVector3(+s, 0, -s), btVector3(-s, 0, +s), btVector3(+s, 0, +s), -// 3, 3, - 20,20, +// 3,3, + 20,20, 1 + 2 + 4 + 8, true); -// 0, true); +// 0, true); + + if (onGround) + psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btVector3(+s, 0, -s), + btVector3(-s, 0, +s), + btVector3(+s, 0, +s), + 20,20, + 0, true); - psb->getCollisionShape()->setMargin(0.25); + psb->getCollisionShape()->setMargin(0.1); psb->generateBendingConstraints(2); - psb->setTotalMass(2); - psb->setDampingCoefficient(0.02); + psb->setTotalMass(10); + psb->setSpringStiffness(10); + psb->setDampingCoefficient(0.01); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 1; getDeformableDynamicsWorld()->addSoftBody(psb); // add a few rigid bodies - Ctor_RbUpStack(10); + Ctor_RbUpStack(1); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 6d3db549e..f29cef2ba 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -48,6 +48,9 @@ #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../DeformableDemo/DeformableDemo.h" +#include "../Pinch/Pinch.h" +#include "../DeformableContact/DeformableContact.h" +#include "../VolumetricDeformable/VolumetricDeformable.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -119,7 +122,10 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc), - ExampleEntry(0, "Deformable", "Deformable test", DeformableCreateFunc), + ExampleEntry(0, "Deformable-RigidBody Contact", "Deformable test", DeformableCreateFunc), + ExampleEntry(0, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), + ExampleEntry(0, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), + ExampleEntry(0, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableContactCreateFunc), ExampleEntry(1, "Constraints", "Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.", AllConstraintCreateFunc), diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp new file mode 100644 index 000000000..cedc51750 --- /dev/null +++ b/examples/Pinch/Pinch.cpp @@ -0,0 +1,297 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///create 125 (5x5x5) dynamic object +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + +//maximum number of objects (and allow user to shoot additional boxes) +#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024) + +///scaling of the objects (0.1 = 20 centimeter boxes ) +#define SCALING 1. +#define START_POS_X -5 +#define START_POS_Y -5 +#define START_POS_Z -3 + +#include "Pinch.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The Pinch shows the use of rolling friction. +///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. +///Generally it is best to leave the rolling friction coefficient zero (or close to zero). + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +struct TetraBunny +{ +#include "../SoftDemo/bunny.inl" +}; + + +class Pinch : public CommonRigidBodyBase +{ +public: + Pinch(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~Pinch() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 25; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void createGrip() + { + int count = 2; + float mass = 2; + btCollisionShape* shape[] = { + new btBoxShape(btVector3(3, 3, 0.5)), + }; + static const int nshapes = sizeof(shape) / sizeof(shape[0]); + for (int i = 0; i < count; ++i) + { + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(10, 0, 0)); + startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + createRigidBody(mass, startTransform, shape[i % nshapes]); + } + } + + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world) +{ + btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies(); + if (rbs.size()<2) + return; + btRigidBody* rb0 = rbs[0]; + btScalar pressTime = 0.9; + btTransform rbTransform; + rbTransform.setIdentity(); + btVector3 translation = btVector3(0.5,3,4); + btVector3 velocity = btVector3(0,5,0); + if (time < pressTime) + { + velocity = btVector3(0,0,-2); + translation += velocity * time; + } + else + translation += btVector3(0,0,-2) * pressTime + (time-pressTime)*velocity; + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb0->setCenterOfMassTransform(rbTransform); + rb0->setAngularVelocity(btVector3(0,0,0)); + rb0->setLinearVelocity(velocity); + + btRigidBody* rb1 = rbs[1]; + translation = btVector3(0.5,3,-4); + velocity = btVector3(0,5,0); + if (time < pressTime) + { + velocity = btVector3(0,0,2); + translation += velocity * time; + } + else + translation += btVector3(0,0,2) * pressTime + (time-pressTime)*velocity; + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb1->setCenterOfMassTransform(rbTransform); + rb1->setAngularVelocity(btVector3(0,0,0)); + rb1->setLinearVelocity(velocity); + + rb0->setFriction(2); + rb1->setFriction(2); +} + +void Pinch::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + + getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + //create a ground + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -25, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a soft block + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 4, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(1); + psb->setSpringStiffness(10); + psb->setDampingCoefficient(0.01); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0.5; + // add a grippers + createGrip(); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void Pinch::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options) +{ + return new Pinch(options.m_guiHelper); +} + + diff --git a/examples/Pinch/Pinch.h b/examples/Pinch/Pinch.h new file mode 100644 index 000000000..6c7e2fb3c --- /dev/null +++ b/examples/Pinch/Pinch.h @@ -0,0 +1,20 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef _PINCH_H +#define _PINCH_H + +class CommonExampleInterface* PinchCreateFunc(struct CommonExampleOptions& options); + +#endif //_PINCH_H diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp new file mode 100644 index 000000000..b30e9bc24 --- /dev/null +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -0,0 +1,294 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +///create 125 (5x5x5) dynamic object +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_Z 5 + +//maximum number of objects (and allow user to shoot additional boxes) +#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024) + +///scaling of the objects (0.1 = 20 centimeter boxes ) +#define SCALING 1. +#define START_POS_X -5 +#define START_POS_Y -5 +#define START_POS_Z -3 + +#include "VolumetricDeformable.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableRigidDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include <stdio.h> //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The VolumetricDeformable shows the use of rolling friction. +///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. +///Generally it is best to leave the rolling friction coefficient zero (or close to zero). + + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class VolumetricDeformable : public CommonRigidBodyBase +{ +public: + VolumetricDeformable(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~VolumetricDeformable() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, 3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void createStaticBox(const btVector3& halfEdge, const btVector3& translation) + { + btCollisionShape* box = new btBoxShape(halfEdge); + m_collisionShapes.push_back(box); + + btTransform Transform; + Transform.setIdentity(); + Transform.setOrigin(translation); + Transform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + btVector3 localInertia(0, 0, 0); + if (isDynamic) + box->calculateLocalInertia(mass, localInertia); + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(Transform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, box, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + void Ctor_RbUpStack(int count) + { + float mass = 0.02; + + btCompoundShape* cylinderCompound = new btCompoundShape; + btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); + btCollisionShape* boxShape = new btBoxShape(btVector3(2, .5, .5)); + btTransform localTransform; + localTransform.setIdentity(); + cylinderCompound->addChildShape(localTransform, boxShape); + btQuaternion orn(SIMD_HALF_PI, 0, 0); + localTransform.setRotation(orn); + // localTransform.setOrigin(btVector3(1,1,1)); + cylinderCompound->addChildShape(localTransform, cylinderShape); + + btCollisionShape* shape[] = { + new btBoxShape(btVector3(1, 1, 1)), + }; + static const int nshapes = sizeof(shape) / sizeof(shape[0]); + for (int i = 0; i < count; ++i) + { + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(i, 10 + 2 * i, i-1)); + createRigidBody(mass, startTransform, shape[i % nshapes]); + } + } + + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableRigidDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableRigidDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void VolumetricDeformable::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver(); + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(50.), btScalar(150.))); + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -50, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0)); + createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0)); + createStaticBox(btVector3(5, 5, 1), btVector3(0,0,5)); + createStaticBox(btVector3(5, 5, 1), btVector3(0,0,-5)); + + // create volumetric soft body + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 5, 0)); +// psb->setVolumeMass(10); + psb->getCollisionShape()->setMargin(0.25); +// psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->setSpringStiffness(1); + psb->setDampingCoefficient(0.01); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0.5; + } + // add a few rigid bodies + Ctor_RbUpStack(4); + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void VolumetricDeformable::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options) +{ + return new VolumetricDeformable(options.m_guiHelper); +} + + diff --git a/examples/VolumetricDeformable/VolumetricDeformable.h b/examples/VolumetricDeformable/VolumetricDeformable.h new file mode 100644 index 000000000..af1cc38b6 --- /dev/null +++ b/examples/VolumetricDeformable/VolumetricDeformable.h @@ -0,0 +1,20 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +#ifndef _VOLUMETRIC_DEFORMABLE_H +#define _VOLUMETRIC_DEFORMABLE_H + +class CommonExampleInterface* VolumetricDeformableCreateFunc(struct CommonExampleOptions& options); + +#endif //_VOLUMETRIC_DEFORMABLE__H diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index 9174107ba..024cc66b6 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -77,6 +77,51 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv) } } +void btBackwardEulerObjective::applyForce(TVStack& force, bool setZero) +{ + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } + } + if (setZero) + { + for (int i = 0; i < force.size(); ++i) + force[i].setZero(); + } +} + +void btBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const +{ + // add implicit force + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledImplicitForce(dt, residual); + } +} + +btScalar btBackwardEulerObjective::computeNorm(const TVStack& residual) const +{ + btScalar norm_squared = 0; + for (int i = 0; i < residual.size(); ++i) + { + norm_squared += residual[i].length2(); + } + return std::sqrt(norm_squared+SIMD_EPSILON); +} + +void btBackwardEulerObjective::applyExplicitForce(TVStack& force) +{ + for (int i = 0; i < m_lf.size(); ++i) + m_lf[i]->addScaledExplicitForce(m_dt, force); + applyForce(force, true); +} + void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) { size_t counter = 0; @@ -85,7 +130,7 @@ void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - dv[counter] = psb->m_nodes[j].m_im * residual[counter] * 1; + dv[counter] = psb->m_nodes[j].m_im * residual[counter]; ++counter; } } diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index e9642f6ae..68f7f66fe 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -12,66 +12,10 @@ #include "btLagrangianForce.h" #include "btMassSpring.h" #include "btContactProjection.h" +#include "btPreconditioner.h" #include "btDeformableRigidDynamicsWorld.h" class btDeformableRigidDynamicsWorld; - -class Preconditioner -{ -public: - using TVStack = btAlignedObjectArray<btVector3>; - virtual void operator()(const TVStack& x, TVStack& b) = 0; - virtual void reinitialize(bool nodeUpdated) = 0; -}; - -class DefaultPreconditioner : public Preconditioner -{ -public: - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - for (int i = 0; i < b.size(); ++i) - b[i] = x[i]; - } - virtual void reinitialize(bool nodeUpdated) - { - - } -}; - -class MassPreconditioner : public Preconditioner -{ - btAlignedObjectArray<btScalar> m_inv_mass; - const btAlignedObjectArray<btSoftBody *>& m_softBodies; -public: - MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies) - : m_softBodies(softBodies) - { - } - - virtual void reinitialize(bool nodeUpdated) - { - if (nodeUpdated) - { - m_inv_mass.clear(); - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - m_inv_mass.push_back(psb->m_nodes[j].m_im); - } - } - } - - virtual void operator()(const TVStack& x, TVStack& b) - { - btAssert(b.size() == x.size()); - btAssert(m_inv_mass.size() == x.size()); - for (int i = 0; i < b.size(); ++i) - b[i] = x[i] * m_inv_mass[i]; - } -}; - class btBackwardEulerObjective { public: @@ -91,74 +35,60 @@ public: void initialize(){} - void computeResidual(btScalar dt, TVStack& residual) const - { - // add implicit force - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->addScaledImplicitForce(dt, residual); - } - } + // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual + void computeResidual(btScalar dt, TVStack& residual) const; - void applyExplicitForce(TVStack& force) - { - for (int i = 0; i < m_lf.size(); ++i) - m_lf[i]->addScaledExplicitForce(m_dt, force); - - size_t counter = 0; - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; - psb->m_nodes[j].m_v += one_over_mass * force[counter]; - force[counter].setZero(); - counter++; - } - } - } + // add explicit force to the velocity + void applyExplicitForce(TVStack& force); - btScalar computeNorm(const TVStack& residual) const - { - btScalar norm_squared = 0; - for (int i = 0; i < residual.size(); ++i) - { - norm_squared += residual[i].length2(); - } - return std::sqrt(norm_squared+SIMD_EPSILON); - } + // apply force to velocity and optionally reset the force to zero + void applyForce(TVStack& force, bool setZero); + + // compute the norm of the residual + btScalar computeNorm(const TVStack& residual) const; + // compute one step of the solve (there is only one solve if the system is linear) void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); + // perform A*x = b void multiply(const TVStack& x, TVStack& b) const; + // update the constraints treated as projections void updateProjection(const TVStack& dv) { projection.update(dv, m_backupVelocity); } + + // set initial guess for CG solve void initialGuess(TVStack& dv, const TVStack& residual); + // reset data structure void reinitialize(bool nodeUpdated); + // enforce constraints in CG solve void enforceConstraint(TVStack& x) { projection.enforceConstraint(x); updateVelocity(x); } + // add dv to velocity void updateVelocity(const TVStack& dv); - void setConstraintDirections() + //set constraints as projections + void setConstraints() { - projection.setConstraintDirections(); + projection.setConstraints(); } + + // update the projections and project the residual void project(TVStack& r, const TVStack& dv) { updateProjection(dv); projection(r); } + // perform precondition M^(-1) x = b void precondition(const TVStack& x, TVStack& b) { m_preconditioner->operator()(x,b); diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 46979c8f0..ff5eecd8c 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -17,12 +17,12 @@ struct Constraint btAlignedObjectArray<const btSoftBody::RContact*> m_contact; btAlignedObjectArray<btVector3> m_direction; btAlignedObjectArray<btScalar> m_value; + // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve + btAlignedObjectArray<btScalar> m_accumulated_normal_impulse; Constraint(const btSoftBody::RContact& rcontact) { - m_contact.push_back(&rcontact); - m_direction.push_back(rcontact.m_cti.m_normal); - m_value.push_back(0); + append(rcontact); } Constraint(const btVector3 dir) @@ -30,31 +30,57 @@ struct Constraint m_contact.push_back(nullptr); m_direction.push_back(dir); m_value.push_back(0); + m_accumulated_normal_impulse.push_back(0); } Constraint() { + m_contact.push_back(nullptr); + m_direction.push_back(btVector3(0,0,0)); + m_value.push_back(0); + m_accumulated_normal_impulse.push_back(0); + } + + void append(const btSoftBody::RContact& rcontact) + { + m_contact.push_back(&rcontact); + m_direction.push_back(rcontact.m_cti.m_normal); + m_value.push_back(0); + m_accumulated_normal_impulse.push_back(0); + } + + ~Constraint() + { } }; struct Friction { - btAlignedObjectArray<bool> m_static; - btAlignedObjectArray<btScalar> m_impulse; - btAlignedObjectArray<btScalar> m_dv; - btAlignedObjectArray<btVector3> m_direction; - btAlignedObjectArray<btVector3> m_direction_prev; + + btAlignedObjectArray<bool> m_static; // whether the friction is static + btAlignedObjectArray<btScalar> m_impulse; // the impulse magnitude the node feels + btAlignedObjectArray<btScalar> m_dv; // the dv magnitude of the node + btAlignedObjectArray<btVector3> m_direction; // the direction of the friction for the node + btAlignedObjectArray<bool> m_static_prev; btAlignedObjectArray<btScalar> m_impulse_prev; btAlignedObjectArray<btScalar> m_dv_prev; + btAlignedObjectArray<btVector3> m_direction_prev; - btAlignedObjectArray<bool> m_released; + btAlignedObjectArray<bool> m_released; // whether the contact is released - btAlignedObjectArray<btScalar> m_accumulated_normal_impulse; + + + // the total impulse the node applied to the rb in the tangential direction in the cg solve btAlignedObjectArray<btVector3> m_accumulated_tangent_impulse; Friction() { + append(); + } + + void append() + { m_static.push_back(false); m_static_prev.push_back(false); @@ -67,7 +93,6 @@ struct Friction m_dv.push_back(0); m_dv_prev.push_back(0); - m_accumulated_normal_impulse.push_back(0); m_accumulated_tangent_impulse.push_back(btVector3(0,0,0)); m_released.push_back(false); } @@ -100,7 +125,7 @@ public: // apply the constraints virtual void operator()(TVStack& x) = 0; - virtual void setConstraintDirections() = 0; + virtual void setConstraints() = 0; // update the constraints virtual void update(const TVStack& dv, const TVStack& backup_v) = 0; diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index a2b69f48d..f23b26385 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -18,12 +18,10 @@ class btConjugateGradient using TVStack = btAlignedObjectArray<btVector3>; TVStack r,p,z,temp; int max_iterations; - btScalar tolerance; public: btConjugateGradient(const int max_it_in) : max_iterations(max_it_in) - , tolerance(std::numeric_limits<float>::epsilon() * 16) { } @@ -121,12 +119,6 @@ public: return ans; } - void setZero(TVStack& a) - { - for (int i = 0; i < a.size(); ++i) - a[i].setZero(); - } - void multAndAddTo(btScalar s, const TVStack& a, TVStack& result) { // result += s*a diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 041792d08..cf2e05e59 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -61,9 +61,9 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v); btScalar vel = 0.0; - for (int j = 0; j < ndof; ++j) + for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac[k]; } va = cti.m_normal * vel * m_dt; } @@ -75,41 +75,41 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit btVector3 impulse = c->m_c0 * vr; const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn); const btVector3 impulse_tangent = impulse - impulse_normal; + + // start friction handling + // copy old data + friction.m_impulse_prev[j] = friction.m_impulse[j]; + friction.m_dv_prev[j] = friction.m_dv[j]; + friction.m_static_prev[j] = friction.m_static[j]; + + // get the current tangent direction btScalar local_tangent_norm = impulse_tangent.norm(); btVector3 local_tangent_dir = btVector3(0,0,0); if (local_tangent_norm > SIMD_EPSILON) local_tangent_dir = impulse_tangent.normalized(); // accumulated impulse on the rb in this and all prev cg iterations - friction.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal); - btScalar accumulated_normal = friction.m_accumulated_normal_impulse[j]; + constraint.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal); + const btScalar& accumulated_normal = constraint.m_accumulated_normal_impulse[j]; + + // the total tangential impulse required to stop sliding btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent; btScalar tangent_norm = tangent.norm(); - // start friction handling - // copy old data - friction.m_impulse_prev[j] = friction.m_impulse[j]; - friction.m_dv_prev[j] = friction.m_dv[j]; - friction.m_static_prev[j] = friction.m_static[j]; - if (accumulated_normal < 0 && tangent_norm > SIMD_EPSILON) + + if (accumulated_normal < 0) { friction.m_direction[j] = -local_tangent_dir; - btScalar compare1 = -accumulated_normal*c->m_c3; - btScalar compare2 = tangent_norm; // do not allow switching from static friction to dynamic friction // it causes cg to explode if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false) { friction.m_static[j] = false; friction.m_impulse[j] = -accumulated_normal*c->m_c3; - friction.m_dv[j] = 0; - impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]); } else { friction.m_static[j] = true; - friction.m_impulse[j] = 0; - friction.m_dv[j] = local_tangent_norm * c->m_c2/m_dt; - impulse = impulse_normal + impulse_tangent; + friction.m_impulse[j] = local_tangent_norm; } } else @@ -117,27 +117,29 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit friction.m_released[j] = true; friction.m_static[j] = false; friction.m_impulse[j] = 0; - friction.m_dv[j] = 0; friction.m_direction[j] = btVector3(0,0,0); - impulse = impulse_normal; } + friction.m_dv[j] = friction.m_impulse[j] * c->m_c2/m_dt; friction.m_accumulated_tangent_impulse[j] = -friction.m_impulse[j] * friction.m_direction[j]; + + // the incremental impulse applied to rb in the tangential direction + btVector3 incremental_tangent = (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]); + if (1) // in the same CG solve, the set of constraits doesn't change -// if (dn <= SIMD_EPSILON) { // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient // dv = new_impulse + accumulated velocity change in previous CG iterations // so we have the invariant node->m_v = backupVelocity + dv; - btVector3 dv = -impulse * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]]; - btScalar dvn = dv.dot(cti.m_normal); +// btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]]; +// btScalar dvn = dv.dot(cti.m_normal); + btScalar dvn = -accumulated_normal * c->m_c2/m_dt; constraint.m_value[j] = dvn; // the incremental impulse: // in the normal direction it's the normal component of "impulse" // in the tangent direction it's the difference between the frictional impulse in the iteration and the previous iteration - impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]); -// impulse = impulse_normal; + impulse = impulse_normal + incremental_tangent; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) @@ -147,7 +149,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit { if (multibodyLinkCol) { - double multiplier = 1; + double multiplier = 0.5; multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); } } @@ -158,8 +160,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit } } - -void btContactProjection::setConstraintDirections() +void btContactProjection::setConstraints() { // set Dirichlet constraint for (int i = 0; i < m_softBodies.size(); ++i) @@ -188,6 +189,7 @@ void btContactProjection::setConstraintDirections() { btSoftBody* psb = m_softBodies[i]; btMultiBodyJacobianData jacobianData; + btAlignedObjectArray<btScalar> jacobian; for (int j = 0; j < psb->m_rcontacts.size(); ++j) { @@ -214,6 +216,7 @@ void btContactProjection::setConstraintDirections() } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { + jacobian.clear(); multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); if (multibodyLinkCol) { @@ -227,9 +230,11 @@ void btContactProjection::setConstraintDirections() multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], deltaV, jacobianData.scratch_r, jacobianData.scratch_v); btScalar vel = 0.0; + jacobian.resize(ndof); for (int j = 0; j < ndof; ++j) { vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + jacobian[j] = jac[j]; } va = cti.m_normal * vel * m_dt; } @@ -262,22 +267,10 @@ void btContactProjection::setConstraintDirections() btScalar dot_prod = dirs[0].dot(cti.m_normal); if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon) { - constraints[j].m_contact.push_back(&c); - constraints[j].m_direction.push_back(cti.m_normal); - constraints[j].m_value.push_back(0); - + // group the constraints + constraints[j].append(c); // push in an empty friction - frictions[j].m_direction.push_back(btVector3(0,0,0)); - frictions[j].m_direction_prev.push_back(btVector3(0,0,0)); - frictions[j].m_impulse.push_back(0); - frictions[j].m_impulse_prev.push_back(0); - frictions[j].m_dv.push_back(0); - frictions[j].m_dv_prev.push_back(0); - frictions[j].m_static.push_back(false); - frictions[j].m_static_prev.push_back(false); - frictions[j].m_accumulated_normal_impulse.push_back(0); - frictions[j].m_accumulated_tangent_impulse.push_back(btVector3(0,0,0)); - frictions[j].m_released.push_back(false); + frictions[j].append(); merged = true; break; } @@ -295,3 +288,127 @@ void btContactProjection::setConstraintDirections() } } } + +void btContactProjection::enforceConstraint(TVStack& x) +{ + const int dim = 3; + for (auto& it : m_constraints) + { + const btAlignedObjectArray<Constraint>& constraints = it.second; + size_t i = m_indices[it.first]; + const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; + btAssert(constraints.size() <= dim); + btAssert(constraints.size() > 0); + if (constraints.size() == 1) + { + x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; + for (int j = 0; j < constraints[0].m_direction.size(); ++j) + x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j]; + } + else if (constraints.size() == 2) + { + btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]); + btAssert(free_dir.norm() > SIMD_EPSILON) + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + for (int j = 0; j < constraints.size(); ++j) + { + for (int k = 0; k < constraints[j].m_direction.size(); ++k) + { + x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; + } + } + + } + else + { + x[i].setZero(); + for (int j = 0; j < constraints.size(); ++j) + { + for (int k = 0; k < constraints[j].m_direction.size(); ++k) + { + x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; + } + } + } + + // apply friction if the node is not constrained in all directions + if (constraints.size() < 3) + { + for (int f = 0; f < frictions.size(); ++f) + { + const Friction& friction= frictions[f]; + for (int j = 0; j < friction.m_direction.size(); ++j) + { + // clear the old constraint + if (friction.m_static_prev[j] == true) + { + x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; + } + // add the new constraint + if (friction.m_static[j] == true) + { + x[i] += friction.m_direction[j] * friction.m_dv[j]; + } + } + } + } + } +} + +void btContactProjection::operator()(TVStack& x) +{ + const int dim = 3; + for (auto& it : m_constraints) + { + const btAlignedObjectArray<Constraint>& constraints = it.second; + size_t i = m_indices[it.first]; + btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; + btAssert(constraints.size() <= dim); + btAssert(constraints.size() > 0); + if (constraints.size() == 1) + { + x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; + } + else if (constraints.size() == 2) + { + btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]); + btAssert(free_dir.norm() > SIMD_EPSILON) + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + else + x[i].setZero(); + + // apply friction if the node is not constrained in all directions + if (constraints.size() < 3) + { + bool has_static_constraint = false; + for (int f = 0; f < frictions.size(); ++f) + { + Friction& friction= frictions[f]; + for (int j = 0; j < friction.m_static.size(); ++j) + has_static_constraint = has_static_constraint || friction.m_static[j]; + } + + for (int f = 0; f < frictions.size(); ++f) + { + Friction& friction= frictions[f]; + for (int j = 0; j < friction.m_direction.size(); ++j) + { + // clear the old friction force + if (friction.m_static_prev[j] == false) + { + x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j]; + } + + // only add to the rhs if there is no static friction constraint on the node + if (friction.m_static[j] == false && !has_static_constraint) + { + x[i] += friction.m_direction[j] * friction.m_impulse[j]; + } + } + } + } + } +} diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 566fd9f92..725cc106b 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -18,171 +18,21 @@ public: btContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt) : btCGProjection(softBodies, dt) { - } virtual ~btContactProjection() { - } - // apply the constraints - virtual void operator()(TVStack& x) - { - const int dim = 3; - for (auto& it : m_constraints) - { - const btAlignedObjectArray<Constraint>& constraints = it.second; - size_t i = m_indices[it.first]; - btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; - btAssert(constraints.size() <= dim); - btAssert(constraints.size() > 0); - if (constraints.size() == 1) - { - x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; - Friction& friction= frictions[0]; - - bool has_static_constraint = false; - for (int j = 0; j < friction.m_static.size(); ++j) - has_static_constraint = has_static_constraint || friction.m_static[j]; - - for (int j = 0; j < friction.m_direction.size(); ++j) - { - // clear the old friction force - if (friction.m_static_prev[j] == false) - { - x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j]; - } - - // only add to the rhs if there is no static friction constraint on the node - if (friction.m_static[j] == false && !has_static_constraint) - { - x[i] += friction.m_direction[j] * friction.m_impulse[j]; - } - } - } - else if (constraints.size() == 2) - { - // TODO : friction - btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]); - btAssert(free_dir.norm() > SIMD_EPSILON) - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - - bool has_static_constraint = false; - for (int f = 0; f < 2; ++f) - { - Friction& friction= frictions[f]; - for (int j = 0; j < friction.m_static.size(); ++j) - has_static_constraint = has_static_constraint || friction.m_static[j]; - } - - for (int f = 0; f < 2; ++f) - { - Friction& friction= frictions[f]; - for (int j = 0; j < friction.m_direction.size(); ++j) - { - // clear the old friction force - if (friction.m_static_prev[j] == false) - { - x[i] -= friction.m_direction_prev[j] * friction.m_impulse_prev[j]; - } - - // only add to the rhs if there is no static friction constraint on the node - if (friction.m_static[j] == false && !has_static_constraint) - { - x[i] += friction.m_direction[j] * friction.m_impulse[j]; - } - } - } - } - else - x[i].setZero(); - } - } - + // apply the constraints to the rhs + virtual void operator()(TVStack& x); - virtual void enforceConstraint(TVStack& x) - { - const int dim = 3; - for (auto& it : m_constraints) - { - const btAlignedObjectArray<Constraint>& constraints = it.second; - size_t i = m_indices[it.first]; - const btAlignedObjectArray<Friction>& frictions = m_frictions[it.first]; - btAssert(constraints.size() <= dim); - btAssert(constraints.size() > 0); - if (constraints.size() == 1) - { - x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; - for (int j = 0; j < constraints[0].m_direction.size(); ++j) - x[i] += constraints[0].m_value[j] * constraints[0].m_direction[j]; - - const Friction& friction= frictions[0]; - for (int j = 0; j < friction.m_direction.size(); ++j) - { - // clear the old constraint - if (friction.m_static_prev[j] == true) - { -// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; - } - // add the new constraint - if (friction.m_static[j] == true) - { - x[i] += friction.m_direction[j] * friction.m_dv[j]; - } - } - } - else if (constraints.size() == 2) - { - // TODO: friction - btVector3 free_dir = btCross(constraints[0].m_direction[0], constraints[1].m_direction[0]); - btAssert(free_dir.norm() > SIMD_EPSILON) - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - for (int j = 0; j < constraints.size(); ++j) - { - for (int k = 0; k < constraints[j].m_direction.size(); ++k) - { - x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; - } - } - - for (int f = 0; f < 2; ++f) - { - const Friction& friction= frictions[f]; - for (int j = 0; j < friction.m_direction.size(); ++j) - { - // clear the old constraint - if (friction.m_static_prev[j] == true) - { -// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; - } - // add the new constraint - if (friction.m_static[j] == true) - { - x[i] += friction.m_direction[j] * friction.m_dv[j]; - } - } - } - } - else - { - x[i].setZero(); - for (int j = 0; j < constraints.size(); ++j) - { - for (int k = 0; k < constraints[j].m_direction.size(); ++k) - { - x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; - } - } - } - } - } + // apply constraints to x in Ax=b + virtual void enforceConstraint(TVStack& x); // update the constraints virtual void update(const TVStack& dv, const TVStack& backupVelocity); - virtual void setConstraintDirections(); + virtual void setConstraints(); }; #endif /* btContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index a8e1e5cd5..f98dfacbb 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -122,14 +122,15 @@ void btDeformableBodySolver::solveConstraints(float solverdt) // apply explicit force m_objective->applyExplicitForce(m_residual); - // remove contact constraints with separating velocity - setConstraintDirections(); + // add constraints to the solver + setConstraints(); backupVelocity(); for (int i = 0; i < m_solveIterations; ++i) { m_objective->computeResidual(solverdt, m_residual); + m_objective->initialGuess(m_dv, m_residual); m_objective->computeStep(m_dv, m_residual, solverdt); updateVelocity(); } @@ -153,9 +154,9 @@ void btDeformableBodySolver::reinitialize(bool nodeUpdated) m_objective->reinitialize(nodeUpdated); } -void btDeformableBodySolver::setConstraintDirections() +void btDeformableBodySolver::setConstraints() { - m_objective->setConstraintDirections(); + m_objective->setConstraints(); } void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world) @@ -173,9 +174,76 @@ void btDeformableBodySolver::updateVelocity() btSoftBody* psb = m_softBodySet[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { -// psb->m_nodes[j].m_v += m_dv[counter]; psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; ++counter; } } } + + +void btDeformableBodySolver::advect(btScalar dt) +{ + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + auto& node = psb->m_nodes[j]; + node.m_x = node.m_q + dt * node.m_v; + } + } +} + +void btDeformableBodySolver::backupVelocity() +{ + // serial implementation + int counter = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_backupVelocity[counter++] = psb->m_nodes[j].m_v; + } + } +} + +bool btDeformableBodySolver::updateNodes() +{ + int numNodes = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + numNodes += m_softBodySet[i]->m_nodes.size(); + if (numNodes != m_numNodes) + { + m_numNodes = numNodes; + m_backupVelocity.resize(numNodes); + return true; + } + return false; +} + + +void btDeformableBodySolver::predictMotion(float solverdt) +{ + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody *psb = m_softBodySet[i]; + + if (psb->isActive()) + { + psb->predictMotion(solverdt); + } + } +} + +void btDeformableBodySolver::updateSoftBodies() +{ + for (int i = 0; i < m_softBodySet.size(); i++) + { + btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; + if (psb->isActive()) + { + psb->integrateMotion(); // normal is updated here + } + } +} diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 17f088c72..a1cd45077 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -51,17 +51,7 @@ public: return true; } - virtual void updateSoftBodies() - { - for (int i = 0; i < m_softBodySet.size(); i++) - { - btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; - if (psb->isActive()) - { - psb->integrateMotion(); // normal is updated here - } - } - } + virtual void updateSoftBodies(); virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false) { @@ -76,64 +66,17 @@ public: void reinitialize(bool nodeUpdated); - void setConstraintDirections(); + void setConstraints(); - void advect(btScalar dt) - { - size_t counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = m_softBodySet[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - auto& node = psb->m_nodes[j]; - node.m_x = node.m_q + dt * node.m_v; - } - } - } + void advect(btScalar dt); - void backupVelocity() - { - // serial implementation - int counter = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = m_softBodySet[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - m_backupVelocity[counter++] = psb->m_nodes[j].m_v; - } - } - } + void backupVelocity(); void updateVelocity(); - bool updateNodes() - { - int numNodes = 0; - for (int i = 0; i < m_softBodySet.size(); ++i) - numNodes += m_softBodySet[i]->m_nodes.size(); - if (numNodes != m_numNodes) - { - m_numNodes = numNodes; - m_backupVelocity.resize(numNodes); - return true; - } - return false; - } + bool updateNodes(); - virtual void predictMotion(float solverdt) - { - for (int i = 0; i < m_softBodySet.size(); ++i) - { - btSoftBody *psb = m_softBodySet[i]; - - if (psb->isActive()) - { - psb->predictMotion(solverdt); - } - } - } + virtual void predictMotion(float solverdt); virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h new file mode 100644 index 000000000..ad190a3a8 --- /dev/null +++ b/src/BulletSoftBody/btPreconditioner.h @@ -0,0 +1,67 @@ +// +// btPreconditioner.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/18/19. +// + +#ifndef BT_PRECONDITIONER_H +#define BT_PRECONDITIONER_H + +class Preconditioner +{ +public: + using TVStack = btAlignedObjectArray<btVector3>; + virtual void operator()(const TVStack& x, TVStack& b) = 0; + virtual void reinitialize(bool nodeUpdated) = 0; +}; + +class DefaultPreconditioner : public Preconditioner +{ +public: + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i]; + } + virtual void reinitialize(bool nodeUpdated) + { + + } +}; + +class MassPreconditioner : public Preconditioner +{ + btAlignedObjectArray<btScalar> m_inv_mass; + const btAlignedObjectArray<btSoftBody *>& m_softBodies; +public: + MassPreconditioner(const btAlignedObjectArray<btSoftBody *>& softBodies) + : m_softBodies(softBodies) + { + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + m_inv_mass.clear(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + m_inv_mass.push_back(psb->m_nodes[j].m_im); + } + } + } + + virtual void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + btAssert(m_inv_mass.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i] * m_inv_mass[i]; + } +}; + +#endif /* BT_PRECONDITIONER_H */ |