From 32836b06945bac5da3bdd50cfed51f6387e2f5f1 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 4 Jul 2019 12:49:45 -0700 Subject: [PATCH] set up deformable world and solver (does not support contact or friction yet) --- examples/DeformableDemo/DeformableDemo.cpp | 269 ++++++++++++++++++ examples/DeformableDemo/DeformableDemo.h | 20 ++ examples/ExampleBrowser/ExampleEntries.cpp | 3 + src/.DS_Store | Bin 0 -> 6148 bytes src/BulletDynamics/Dynamics/btDynamicsWorld.h | 3 +- src/BulletSoftBody/btBackwardEulerObjective.h | 163 +++++++++++ src/BulletSoftBody/btConjugateGradient.h | 208 ++++++++++++++ src/BulletSoftBody/btDeformableBodySolver.h | 269 ++++++++++++++++++ .../btDeformableRigidDynamicsWorld.cpp | 39 +++ .../btDeformableRigidDynamicsWorld.h | 89 ++++++ src/BulletSoftBody/btLagrangianForce.h | 53 ++++ src/BulletSoftBody/btMassSpring.h | 113 ++++++++ src/BulletSoftBody/btSoftBody.cpp | 1 + src/BulletSoftBody/btSoftBody.h | 6 + src/BulletSoftBody/btSoftBodySolvers.h | 3 +- 15 files changed, 1237 insertions(+), 2 deletions(-) create mode 100644 examples/DeformableDemo/DeformableDemo.cpp create mode 100644 examples/DeformableDemo/DeformableDemo.h create mode 100644 src/.DS_Store create mode 100644 src/BulletSoftBody/btBackwardEulerObjective.h create mode 100644 src/BulletSoftBody/btConjugateGradient.h create mode 100644 src/BulletSoftBody/btDeformableBodySolver.h create mode 100644 src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp create mode 100644 src/BulletSoftBody/btDeformableRigidDynamicsWorld.h create mode 100644 src/BulletSoftBody/btLagrangianForce.h create mode 100644 src/BulletSoftBody/btMassSpring.h diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp new file mode 100644 index 000000000..7edc3c4a3 --- /dev/null +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -0,0 +1,269 @@ +/* +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 "DeformableDemo.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 //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The DeformableDemo 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). +class DeformableDemo : public CommonRigidBodyBase +{ +public: + DeformableDemo(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableDemo() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { +// float dist = 30; +// float pitch = -14; +// float yaw = 0; +// float targetPos[3] = {0, 0, 0}; + float dist = 45; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0,0, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + 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 DeformableDemo::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup +// m_collisionConfiguration = new btDefaultCollisionConfiguration(); + 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) + btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; + m_solver = sol; + + m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration, deformableBodySolver); + // 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); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a few basic rigid bodies +// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(25.))); + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.), btScalar(25.), btScalar(50.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -30, 0)); +// groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03)); + //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(.5); + + //add the body to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } +// +// { +// ///create a few basic rigid bodies +// btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(100.), btScalar(100.), btScalar(50.))); +// +// m_collisionShapes.push_back(groundShape); +// +// btTransform groundTransform; +// groundTransform.setIdentity(); +// groundTransform.setOrigin(btVector3(0, 0, -54)); +// //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(.1); +// //add the body to the dynamics world +// m_dynamicsWorld->addRigidBody(body); +// } +// +// { +// // add a simple deformable body +// const btVector3 s(3,2,1); // side length +// const btVector3 p(0,30,0); // origin; +// const btVector3 h = s * 0.5; +// const btVector3 c[] = {p + h * btVector3(-1, -1, -1), +// p + h * btVector3(+1, -1, -1), +// p + h * btVector3(-1, +1, -1), +// p + h * btVector3(+1, +1, -1), +// p + h * btVector3(-1, -1, +1), +// p + h * btVector3(+1, -1, +1), +// p + h * btVector3(-1, +1, +1), +// p + h * btVector3(+1, +1, +1)}; +// btSoftBody* psb = btSoftBodyHelpers::CreateFromConvexHull(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), c, 8); +// psb->generateBendingConstraints(2); +// psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; +// psb->setTotalMass(150); +// getDeformableDynamicsWorld()->addSoftBody(psb); +// } + { + const btScalar s = 8; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getSoftDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btVector3(+s, 0, -s), + btVector3(-s, 0, +s), + btVector3(+s, 0, +s), + 10, 10, + // 31,31, + 1 + 2 + 4 + 8, true); +// 0, true); + + psb->getCollisionShape()->setMargin(0.5); +// btSoftBody::Material* pm = psb->appendMaterial(); +// pm->m_kLST = 0.4 * 1000; +// pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2); + psb->setTotalMass(1); + psb->setDampingCoefficient(0.01); + getDeformableDynamicsWorld()->addSoftBody(psb); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableDemo::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* DeformableCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableDemo(options.m_guiHelper); +} diff --git a/examples/DeformableDemo/DeformableDemo.h b/examples/DeformableDemo/DeformableDemo.h new file mode 100644 index 000000000..c688cea9d --- /dev/null +++ b/examples/DeformableDemo/DeformableDemo.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_DEMO_H +#define _DEFORMABLE_DEMO_H + +class CommonExampleInterface* DeformableCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_DEMO_H diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e80b4eee9..6d3db549e 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -47,6 +47,7 @@ #include "../FractureDemo/FractureDemo.h" #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" +#include "../DeformableDemo/DeformableDemo.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -118,6 +119,8 @@ 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(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/src/.DS_Store b/src/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..ee616d543128a1988ca9f9dc9bb19e1f51a11544 GIT binary patch literal 6148 zcmeHKQESvd5T3oBn)IMV5VYXKf)9PUhZFUxZy~mQDp;D*K4?7?61_mkmR!<9j)Tx2 z&>x_$T7QS_pXd+i4{&C8uUKmPSgXt&v)^QPW_I)Kv6}?|5!A_301p5hRKk{v%^!sN zNiRvudI*KCkwOI_DENrT@-HwzYqtOa)R5!v_w@^K9TWH*k4JlPu40Tki}&(uRP}nN zqBYmPbN8O(&Uf6-LdV?=hiV*FQB{wEs2q5Y!z@YTsNBhtYPgjat9iPg7H`V+s7lMX z6uhNWjseDw;_WOQ54;DjRZ&G*k(RoisqtUZUf{>y1sPL`K@UHSZ3e>!y?XK`t5^L>4A_~q-j??*p=9_u6!`2W>% z-{K5T(72YL{je-@ReZv@S&dmPBQwAZFax)h0lQy0o!dG=o)ihr2Bz|HBn1TO_0nuIwR|0$`d$wMBIXY`4 t>IYO3ifas>r=X#aVvMDu_z|iW^h+`jeT$7j^q}yEfTn>PX5dB{_yxA-W?TRO literal 0 HcmV?d00001 diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h index eadd8c12e..10853b761 100644 --- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -34,7 +34,8 @@ enum btDynamicsWorldType BT_CONTINUOUS_DYNAMICS_WORLD = 3, BT_SOFT_RIGID_DYNAMICS_WORLD = 4, BT_GPU_DYNAMICS_WORLD = 5, - BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6 + BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6, + BT_DEFORMABLE_RIGID_DYNAMICS_WORLD = 7 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h new file mode 100644 index 000000000..2dca39bb6 --- /dev/null +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -0,0 +1,163 @@ +// +// btBackwardEulerObjective.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/1/19. +// + +#ifndef BT_BACKWARD_EULER_OBJECTIVE_H +#define BT_BACKWARD_EULER_OBJECTIVE_H +#include +#include "btConjugateGradient.h" +#include "btLagrangianForce.h" +#include "btMassSpring.h" + +struct DirichletDofProjection +{ + using TVStack = btAlignedObjectArray; + const btAlignedObjectArray& m_softBodies; + DirichletDofProjection(const btAlignedObjectArray& softBodies) + : m_softBodies(softBodies) + {} + + void operator()(TVStack& x) + { + size_t counter = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im == 0) + { + x[counter].setZero(); + } + ++counter; + } + } + } +}; + +class btBackwardEulerObjective +{ +public: + using TVStack = btAlignedObjectArray; + struct EmptyProjection + { + void operator()(TVStack& x) + {} + }; + struct DefaultPreconditioner + { + void operator()(const TVStack& x, TVStack& b) + { + btAssert(b.size() == x.size()); + for (int i = 0; i < b.size(); ++i) + b[i] = x[i]; + } + }; + btScalar m_dt; + btConjugateGradient cg; + + btAlignedObjectArray m_lf; + btAlignedObjectArray& m_softBodies; + std::function project; + std::function precondition; + + btBackwardEulerObjective(btAlignedObjectArray& softBodies) + : cg(20) + , m_softBodies(softBodies) + , project(EmptyProjection()) + , precondition(DefaultPreconditioner()) + { + // this should really be specified in initialization instead of here + btMassSpring* mass_spring = new btMassSpring(m_softBodies); + m_lf.push_back(mass_spring); + } + + virtual ~btBackwardEulerObjective() {} + + void initialize(){} + + void computeResidual(btScalar dt, TVStack& residual) const + { + // gravity is treated explicitly in predictUnconstraintMotion + + // add force + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledForce(dt, residual); + } + } + + 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); + } + + void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt) + { + m_dt = dt; + // TODO:figure out what the tolerance should be + btScalar tolerance = std::numeric_limits::epsilon()*16 * computeNorm(residual); + cg.solve(*this, dv, residual, tolerance); + } + + void multiply(const TVStack& x, TVStack& b) const + { + for (int i = 0; i < b.size(); ++i) + b[i].setZero(); + + // add in the mass term + 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) + { + const auto& node = psb->m_nodes[j]; + b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; + ++counter; + } + } + + for (int i = 0; i < m_lf.size(); ++i) + { + // damping force is implicit and elastic force is explicit + m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); + } + } + + void reinitialize(bool nodeUpdated) + { + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + } + + if(nodeUpdated) + { + DirichletDofProjection dirichlet(m_softBodies); + setProjection(dirichlet); + } + } + + template + void setProjection(Func project_func) + { + project = project_func; + } + + template + void setPreconditioner(Func preconditioner_func) + { + precondition = preconditioner_func; + } +}; + +#endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h new file mode 100644 index 000000000..76297226d --- /dev/null +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -0,0 +1,208 @@ +// +// btConjugateGradient.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/1/19. +// + +#ifndef BT_CONJUGATE_GRADIENT_H +#define BT_CONJUGATE_GRADIENT_H +#include +template +class btConjugateGradient +{ + using TVStack = btAlignedObjectArray; + 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::epsilon() * 16) + { + } + + virtual ~btConjugateGradient(){} + +// // return the number of iterations taken +// int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance) +// { +// btAssert(x.size() == b.size()); +// reinitialize(b); +// +// // r = M * (b - A * x) --with assigned dof zeroed out +// A.multiply(x, temp); +// temp = sub(b, temp); +// A.project(temp); +// A.precondition(temp, r); +// +// btScalar r_dot_r = squaredNorm(r), r_dot_r_new; +// btScalar r_norm = std::sqrt(r_dot_r); +// if (r_norm < tolerance) { +// std::cout << "Iteration = 0" << std::endl; +// std::cout << "Two norm of the residual = " << r_norm << std::endl; +// return 0; +// } +// +// p = r; +// // q = M * A * q; +// A.multiply(p, temp); +// A.precondition(temp, q); +// +// // alpha = |r|^2 / (p^T * A * p) +// btScalar alpha = r_dot_r / dot(p, q), beta; +// +// for (int k = 1; k < max_iterations; k++) { +//// x += alpha * p; +//// r -= alpha * q; +// multAndAddTo(alpha, p, x); +// multAndAddTo(-alpha, q, r); +// +// // zero out the dofs of r +// A.project(r); +// +// r_dot_r_new = squaredNorm(r); +// r_norm = std::sqrt(r_dot_r_new); +// +// if (r_norm < tolerance) { +// std::cout << "ConjugateGradient iterations " << k << std::endl; +// return k; +// +// beta = r_dot_r_new / r_dot_r; +// r_dot_r = r_dot_r_new; +//// p = r + beta * p; +// p = multAndAdd(beta, p, r); +// +// // q = M * A * q; +// A.multiply(p, temp); +// A.precondition(temp, q); +// +// alpha = r_dot_r / dot(p, q); +// } +// +// setZero(q); +// setZero(r); +// } +// std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl; +// return max_iterations; +// } + + // return the number of iterations taken + int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance) + { + btAssert(x.size() == b.size()); + reinitialize(b); + + // r = b - A * x --with assigned dof zeroed out + A.multiply(x, temp); + r = sub(b, temp); + A.project(r); + + btScalar r_norm = std::sqrt(squaredNorm(r)); + if (r_norm < tolerance) { + std::cout << "Iteration = 0" << std::endl; + std::cout << "Two norm of the residual = " << r_norm << std::endl; + return 0; + } + + // z = M^(-1) * r + A.precondition(r, z); + // p = z; + p = z; + // temp = A*p + A.multiply(p, temp); + + btScalar r_dot_z = dot(z,r), r_dot_z_new; + // alpha = r^T * z / (p^T * A * p) + btScalar alpha = r_dot_z / dot(p, temp), beta; + + for (int k = 1; k < max_iterations; k++) { + // x += alpha * p; + // r -= alpha * temp; + multAndAddTo(alpha, p, x); + multAndAddTo(-alpha, temp, r); + + // zero out the dofs of r + A.project(r); + + r_norm = std::sqrt(squaredNorm(r)); + + if (r_norm < tolerance) { + std::cout << "ConjugateGradient iterations " << k << std::endl; + return k; + } + + // z = M^(-1) * r + A.precondition(r, z); + r_dot_z_new = dot(r,z); + beta = r_dot_z_new/ r_dot_z; + r_dot_z = r_dot_z_new; + // p = z + beta * p; + p = multAndAdd(beta, p, z); + // temp = A * p; + A.multiply(p, temp); + // alpha = r^T * z / (p^T * A * p) + alpha = r_dot_z / dot(p, temp); + } + std::cout << "ConjugateGradient max iterations reached " << max_iterations << std::endl; + return max_iterations; + } + + void reinitialize(const TVStack& b) + { + r.resize(b.size()); + p.resize(b.size()); + z.resize(b.size()); + temp.resize(b.size()); + } + + TVStack sub(const TVStack& a, const TVStack& b) + { + // c = a-b + btAssert(a.size() == b.size()) + TVStack c; + c.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + c[i] = a[i] - b[i]; + return c; + } + + btScalar squaredNorm(const TVStack& a) + { + return dot(a,a); + } + + btScalar dot(const TVStack& a, const TVStack& b) + { + btScalar ans(0); + for (int i = 0; i < a.size(); ++i) + ans += a[i].dot(b[i]); + 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 + btAssert(a.size() == result.size()) + for (int i = 0; i < a.size(); ++i) + result[i] += s * a[i]; + } + + TVStack multAndAdd(btScalar s, const TVStack& a, const TVStack& b) + { + // result = a*s + b + TVStack result; + result.resize(a.size()); + for (int i = 0; i < a.size(); ++i) + result[i] = s * a[i] + b[i]; + return result; + } +}; +#endif /* btConjugateGradient_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h new file mode 100644 index 000000000..4e36a2112 --- /dev/null +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -0,0 +1,269 @@ +// +// btDeformableBodySolver.h +// BulletSoftBody +// +// Created by Chuyuan Fu on 7/1/19. +// + +#ifndef BT_DEFORMABLE_BODY_SOLVERS_H +#define BT_DEFORMABLE_BODY_SOLVERS_H + +#include "btSoftBodySolvers.h" +#include "btBackwardEulerObjective.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" + +struct btCollisionObjectWrapper; + +class btDeformableBodySolver : public btSoftBodySolver +{ + using TVStack = btAlignedObjectArray; +protected: + /** Variable to define whether we need to update solver constants on the next iteration */ + bool m_updateSolverConstants; + int m_numNodes; + TVStack m_dv; + TVStack m_residual; + btAlignedObjectArray m_softBodySet; + btBackwardEulerObjective m_objective; + int m_solveIterations; + int m_impulseIterations; + +public: + btDeformableBodySolver() + : m_numNodes(0) + , m_objective(m_softBodySet) + , m_solveIterations(1) + , m_impulseIterations(1) + { + } + + virtual ~btDeformableBodySolver() + { + } + + virtual SolverTypes getSolverType() const + { + return DEFORMABLE_SOLVER; + } + + virtual bool checkInitialized() + { + 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 optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false) + { + m_softBodySet.copyFromArray(softBodies); + } + + virtual void copyBackToSoftBodies(bool bMove = true) {} + + virtual void solveConstraints(float solverdt) + { + bool nodeUpdated = updateNodes(); + reinitialize(nodeUpdated); + + for (int i = 0; i < m_solveIterations; ++i) + { + // get the velocity after contact solve + // TODO: perform contact solve here + for (int j = 0; j < m_impulseIterations; ++j) + { + for (int s = 0; s < m_softBodySet.size(); ++s) + VSolve_RContacts(m_softBodySet[s], 0, solverdt); + } + + // advect with v_n+1 ** to update position based states + // where v_n+1 ** is the velocity after contact response + + // only need to advect x here if elastic force is implicit +// prepareSolve(solverdt); + + m_objective.computeResidual(solverdt, m_residual); + m_objective.computeStep(m_dv, m_residual, solverdt); + + updateVelocity(); + } + advect(solverdt); + } + + void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + { + m_dv.resize(m_numNodes); + m_residual.resize(m_numNodes); + } + for (int i = 0; i < m_dv.size(); ++i) + { + m_dv[i].setZero(); + m_residual[i].setZero(); + } + m_objective.reinitialize(nodeUpdated); + } + + void prepareSolve(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 * psb->m_dampingCoefficient; + } + } + } + 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 += dt * m_dv[counter++]; + } + } + } + + void updateVelocity() + { + // 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) + { + psb->m_nodes[j].m_v += m_dv[counter++]; + } + } + } + + 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; + return true; + } + return false; + } + 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 copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} + + virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) + { + softBody->defaultCollisionHandler(collisionObjectWrap); + } + + virtual void processCollision(btSoftBody *, btSoftBody *) { + // TODO + } + + void VSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar dt) + { + const btScalar mrg = psb->getCollisionShape()->getMargin(); + btMultiBodyJacobianData jacobianData; + for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i) + { + const btSoftBody::RContact& c = psb->m_rcontacts[i]; + const btSoftBody::sCti& cti = c.m_cti; + if (cti.m_colObj->hasContactResponse()) + { + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + btScalar* deltaV; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1) * dt : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac = &jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0]; + 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) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + } + va = cti.m_normal * vel * dt; + } + } + + const btVector3 vb = c.m_node->m_x - c.m_node->m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn <= SIMD_EPSILON) + { + const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); + const btVector3 fv = vr - (cti.m_normal * dn); + // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient + const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst); + c.m_node->m_v -= impulse * c.m_c2 / dt; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse, c.m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + if (multibodyLinkCol) + { + double multiplier = 0.5; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + } + } + } + } + } + } + +}; + +#endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp new file mode 100644 index 000000000..73d4f67f6 --- /dev/null +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -0,0 +1,39 @@ +// +// btDeformableRigidDynamicsWorld.cpp +// BulletSoftBody +// +// Created by Xuchen Han on 7/1/19. +// + +#include +#include "btDeformableRigidDynamicsWorld.h" +#include "btDeformableBodySolver.h" + +void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) +{ + // Let the solver grab the soft bodies and if necessary optimize for it + m_deformableBodySolver->optimize(getSoftDynamicsWorld()->getSoftBodyArray()); + + if (!m_deformableBodySolver->checkInitialized()) + { + btAssert("Solver initialization failed\n"); + } + + btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); + + ///solve deformable bodies constraints + solveDeformableBodiesConstraints(timeStep); + + + ///update soft bodies + m_deformableBodySolver->updateSoftBodies(); + + // End solver-wise simulation step + // /////////////////////////////// +} + +void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) +{ + m_deformableBodySolver->solveConstraints(timeStep); +} + diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h new file mode 100644 index 000000000..55ed41d86 --- /dev/null +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -0,0 +1,89 @@ +/* + 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 BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H +#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H + +#include "btSoftRigidDynamicsWorld.h" +#include "btLagrangianForce.h" +#include "btMassSpring.h" +#include "btDeformableBodySolver.h" +typedef btAlignedObjectArray btSoftBodyArray; + +class btDeformableBodySolver; +class btLagrangianForce; + +class btDeformableRigidDynamicsWorld : public btSoftRigidDynamicsWorld +{ + using TVStack = btAlignedObjectArray; + ///Solver classes that encapsulate multiple deformable bodies for solving + btDeformableBodySolver* m_deformableBodySolver; + + +protected: + virtual void internalSingleStepSimulation(btScalar timeStep); + + void solveDeformableBodiesConstraints(btScalar timeStep); + +public: + btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) + : btSoftRigidDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration, 0), + m_deformableBodySolver(deformableBodySolver) + { + } + + virtual ~btDeformableRigidDynamicsWorld() + { + } + + virtual btSoftRigidDynamicsWorld* getSoftDynamicsWorld() + { + return (btSoftRigidDynamicsWorld*)(this); + } + + virtual const btSoftRigidDynamicsWorld* getSoftDynamicsWorld() const + { + return (const btSoftRigidDynamicsWorld*)(this); + } + + virtual btDynamicsWorldType getWorldType() const + { + return BT_DEFORMABLE_RIGID_DYNAMICS_WORLD; + } + + virtual void predictUnconstraintMotion(btScalar timeStep) + { + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(float(timeStep)); + } + // virtual void internalStepSingleStepSimulation(btScalar timeStep); + + // virtual void solveDeformableBodiesConstraints(btScalar timeStep); + + virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter) + { + getSoftDynamicsWorld()->getSoftBodyArray().push_back(body); + + // Set the soft body solver that will deal with this body + // to be the world's solver + body->setSoftBodySolver(m_deformableBodySolver); + + btCollisionWorld::addCollisionObject(body, + collisionFilterGroup, + collisionFilterMask); + } +}; + +#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btLagrangianForce.h b/src/BulletSoftBody/btLagrangianForce.h new file mode 100644 index 000000000..40e63207e --- /dev/null +++ b/src/BulletSoftBody/btLagrangianForce.h @@ -0,0 +1,53 @@ +// +// btLagrangianForce.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/1/19. +// + +#ifndef BT_LAGRANGIAN_FORCE_H +#define BT_LAGRANGIAN_FORCE_H + +#include "btSoftBody.h" +#include + +class btLagrangianForce +{ +public: + using TVStack = btAlignedObjectArray; + const btAlignedObjectArray& m_softBodies; + std::unordered_map m_indices; + + btLagrangianForce(const btAlignedObjectArray& softBodies) + : m_softBodies(softBodies) + { + } + + virtual ~btLagrangianForce(){} + + virtual void addScaledForce(btScalar scale, TVStack& force) = 0; + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + updateId(); + } + + void updateId() + { + size_t index = 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) + { + m_indices[&(psb->m_nodes[j])] = index++; + } + } + } +}; +#endif /* btLagrangianForce_h */ diff --git a/src/BulletSoftBody/btMassSpring.h b/src/BulletSoftBody/btMassSpring.h new file mode 100644 index 000000000..ffe21c801 --- /dev/null +++ b/src/BulletSoftBody/btMassSpring.h @@ -0,0 +1,113 @@ +// +// btMassSpring.h +// BulletSoftBody +// +// Created by Chuyuan Fu on 7/1/19. +// + +#ifndef BT_MASS_SPRING_H +#define BT_MASS_SPRING_H + +#include "btLagrangianForce.h" + +class btMassSpring : public btLagrangianForce +{ +public: + using TVStack = btLagrangianForce::TVStack; + btMassSpring(const btAlignedObjectArray& softBodies) : btLagrangianForce(softBodies) + { + + } + + virtual void addScaledForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes == force.size()) + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const auto& link = psb->m_links[j]; + const auto node1 = link.m_n[0]; + const auto node2 = link.m_n[1]; + btScalar kLST = link.Feature::m_material->m_kLST; // this is probly wrong, TODO: figure out how to get stiffness + btScalar r = link.m_rl; + size_t id1 = m_indices[node1]; + size_t id2 = m_indices[node2]; + + // elastic force + btVector3 dir = (node2->m_x - node1->m_x); + btVector3 dir_normalized = dir.normalized(); + btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r); + force[id1] += scaled_force; + force[id2] -= scaled_force; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly + scaled_force = scale * v_diff * k_damp; + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes == dx.size()); + btAssert(numNodes == df.size()); + + // implicit elastic force + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const auto& link = psb->m_links[j]; + const auto node1 = link.m_n[0]; + const auto node2 = link.m_n[1]; + btScalar kLST = link.Feature::m_material->m_kLST; + size_t id1 = m_indices[node1]; + size_t id2 = m_indices[node2]; + btVector3 local_scaled_df = scale * kLST * (dx[id2] - dx[id1]); + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicity damping force + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const auto& link = psb->m_links[j]; + const auto node1 = link.m_n[0]; + const auto node2 = link.m_n[1]; + btScalar k_damp = psb->m_dampingCoefficient; // TODO: FIX THIS HACK and set k_damp properly + size_t id1 = m_indices[node1]; + size_t id2 = m_indices[node2]; + btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + + int getNumNodes() + { + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + numNodes += m_softBodies[i]->m_nodes.size(); + } + return numNodes; + } +}; + +#endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 58796a88d..066072426 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -110,6 +110,7 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); + m_dampingCoefficient = 1; } // diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 9b35b799d..01cde89e3 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -704,6 +704,7 @@ public: btDbvt m_fdbvt; // Faces tree btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters + btScalar m_dampingCoefficient; // Damping Coefficient btAlignedObjectArray m_clusterConnectivity; //cluster connectivity, for self-collision @@ -735,6 +736,11 @@ public: { return m_worldInfo; } + + void setDampingCoefficient(btScalar damping_coeff) + { + m_dampingCoefficient = damping_coeff; + } ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) diff --git a/src/BulletSoftBody/btSoftBodySolvers.h b/src/BulletSoftBody/btSoftBodySolvers.h index dcf508265..405475529 100644 --- a/src/BulletSoftBody/btSoftBodySolvers.h +++ b/src/BulletSoftBody/btSoftBodySolvers.h @@ -35,7 +35,8 @@ public: CL_SOLVER, CL_SIMD_SOLVER, DX_SOLVER, - DX_SIMD_SOLVER + DX_SIMD_SOLVER, + DEFORMABLE_SOLVER }; protected: