From 32836b06945bac5da3bdd50cfed51f6387e2f5f1 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 4 Jul 2019 12:49:45 -0700 Subject: 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 000000000..ee616d543 Binary files /dev/null and b/src/.DS_Store differ 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: -- cgit v1.2.1 From 35ce2ae0e2dc5b28185fc90355f8033085b7b76a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 4 Jul 2019 23:07:02 -0700 Subject: add contact constraint as projections in CG --- .../Dynamics/btDiscreteDynamicsWorld.h | 8 +- src/BulletSoftBody/btBackwardEulerObjective.h | 78 +++++------ src/BulletSoftBody/btCGProjection.h | 80 ++++++++++++ src/BulletSoftBody/btConjugateGradient.h | 8 +- src/BulletSoftBody/btContactProjection.cpp | 142 +++++++++++++++++++++ src/BulletSoftBody/btContactProjection.h | 56 ++++++++ src/BulletSoftBody/btDeformableBodySolver.h | 116 +++++------------ .../btDeformableRigidDynamicsWorld.cpp | 42 +++++- .../btDeformableRigidDynamicsWorld.h | 3 +- 9 files changed, 392 insertions(+), 141 deletions(-) create mode 100644 src/BulletSoftBody/btCGProjection.h create mode 100644 src/BulletSoftBody/btContactProjection.cpp create mode 100644 src/BulletSoftBody/btContactProjection.h diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 7fe961921..51b3d906d 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -17,7 +17,6 @@ subject to the following restrictions: #define BT_DISCRETE_DYNAMICS_WORLD_H #include "btDynamicsWorld.h" - class btDispatcher; class btOverlappingPairCache; class btConstraintSolver; @@ -26,6 +25,7 @@ class btTypedConstraint; class btActionInterface; class btPersistentManifold; class btIDebugDraw; + struct InplaceSolverIslandCallback; #include "LinearMath/btAlignedObjectArray.h" @@ -76,7 +76,7 @@ protected: virtual void calculateSimulationIslands(); - virtual void solveConstraints(btContactSolverInfo & solverInfo); + virtual void updateActivationState(btScalar timeStep); @@ -95,7 +95,7 @@ protected: void serializeRigidBodies(btSerializer * serializer); void serializeDynamicsWorldInfo(btSerializer * serializer); - + public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -107,6 +107,8 @@ public: ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); + virtual void solveConstraints(btContactSolverInfo & solverInfo); + virtual void synchronizeMotionStates(); ///this can be useful to synchronize a single rigid body -> graphics object diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index 2dca39bb6..f8f0596b5 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -11,42 +11,14 @@ #include "btConjugateGradient.h" #include "btLagrangianForce.h" #include "btMassSpring.h" +#include "btContactProjection.h" +#include "btDeformableRigidDynamicsWorld.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 btDeformableRigidDynamicsWorld; class btBackwardEulerObjective { public: using TVStack = btAlignedObjectArray; - struct EmptyProjection - { - void operator()(TVStack& x) - {} - }; struct DefaultPreconditioner { void operator()(const TVStack& x, TVStack& b) @@ -58,19 +30,19 @@ public: }; btScalar m_dt; btConjugateGradient cg; - + btDeformableRigidDynamicsWorld* m_world; btAlignedObjectArray m_lf; btAlignedObjectArray& m_softBodies; - std::function project; std::function precondition; + btContactProjection projection; - btBackwardEulerObjective(btAlignedObjectArray& softBodies) + btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : cg(20) , m_softBodies(softBodies) - , project(EmptyProjection()) , precondition(DefaultPreconditioner()) + , projection(m_softBodies, backup_v) { - // this should really be specified in initialization instead of here + // TODO: this should really be specified in initialization instead of here btMassSpring* mass_spring = new btMassSpring(m_softBodies); m_lf.push_back(mass_spring); } @@ -103,7 +75,6 @@ public: 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); } @@ -133,24 +104,33 @@ public: } } + void updateProjection(const TVStack& dv) + { + projection.update(m_dt, dv); + } + void reinitialize(bool nodeUpdated) { - for (int i = 0; i < m_lf.size(); ++i) + if(nodeUpdated) { - m_lf[i]->reinitialize(nodeUpdated); + projection.setSoftBodies(m_softBodies); } - - if(nodeUpdated) + for (int i = 0; i < m_lf.size(); ++i) { - DirichletDofProjection dirichlet(m_softBodies); - setProjection(dirichlet); + m_lf[i]->reinitialize(nodeUpdated); + projection.reinitialize(nodeUpdated); } } - template - void setProjection(Func project_func) + void enforceConstraint(TVStack& x) + { + projection.enforceConstraint(x); + } + + void project(TVStack& r, const TVStack& dv) { - project = project_func; + updateProjection(dv); + projection(r); } template @@ -158,6 +138,12 @@ public: { precondition = preconditioner_func; } + + virtual void setWorld(btDeformableRigidDynamicsWorld* world) + { + m_world = world; + projection.setWorld(world); + } }; #endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h new file mode 100644 index 000000000..89743ef27 --- /dev/null +++ b/src/BulletSoftBody/btCGProjection.h @@ -0,0 +1,80 @@ +// btCGProjection.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/4/19. +// + +#ifndef BT_CG_PROJECTION_H +#define BT_CG_PROJECTION_H + +#include "btSoftBody.h" +#include + +class btDeformableRigidDynamicsWorld; +class btCGProjection +{ +public: +// static const int dim = 3; + using TVStack = btAlignedObjectArray; + using TVArrayStack = btAlignedObjectArray >; + using TArrayStack = btAlignedObjectArray >; + btAlignedObjectArray m_softBodies; + btDeformableRigidDynamicsWorld* m_world; + std::unordered_map m_indices; + TVArrayStack m_constrainedDirections; + TArrayStack m_constrainedValues; + const TVStack& m_backupVelocity; + + btCGProjection(btAlignedObjectArray& softBodies, const TVStack& backup_v) + : m_softBodies(softBodies) + , m_backupVelocity(backup_v) + { + + } + + virtual ~btCGProjection() + { + + } + + // apply the constraints + virtual void operator()(TVStack& x) = 0; + + // update the constraints + virtual void update(btScalar dt, const TVStack& dv) = 0; + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + updateId(); + m_constrainedValues.resize(m_indices.size()); + m_constrainedDirections.resize(m_indices.size()); + } + + void updateId() + { + size_t index = 0; + m_indices.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_indices[&(psb->m_nodes[j])] = index++; + } + } + } + + void setSoftBodies(btAlignedObjectArray softBodies) + { + m_softBodies.copyFromArray(softBodies); + } + + virtual void setWorld(btDeformableRigidDynamicsWorld* world) + { + m_world = world; + } +}; + + +#endif /* btCGProjection_h */ diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 76297226d..2754a208d 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -89,7 +89,7 @@ public: // } // return the number of iterations taken - int solve(const TM& A, TVStack& x, const TVStack& b, btScalar tolerance) + int solve(TM& A, TVStack& x, const TVStack& b, btScalar tolerance) { btAssert(x.size() == b.size()); reinitialize(b); @@ -97,7 +97,8 @@ public: // r = b - A * x --with assigned dof zeroed out A.multiply(x, temp); r = sub(b, temp); - A.project(r); + A.project(r,x); + A.enforceConstraint(x); btScalar r_norm = std::sqrt(squaredNorm(r)); if (r_norm < tolerance) { @@ -124,7 +125,8 @@ public: multAndAddTo(-alpha, temp, r); // zero out the dofs of r - A.project(r); + A.project(r,x); + A.enforceConstraint(x); r_norm = std::sqrt(squaredNorm(r)); diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp new file mode 100644 index 000000000..b50918af4 --- /dev/null +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -0,0 +1,142 @@ +// +// btContactProjection.cpp +// BulletSoftBody +// +// Created by Xuchen Han on 7/4/19. +// + +#include "btContactProjection.h" +#include "btDeformableRigidDynamicsWorld.h" +void btContactProjection::update(btScalar dt, const TVStack& dv) +{ + 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) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter]; + ++counter; + } + } + + ///solve rigid body constraints + m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo()); + + // clear the old constraints + for (int i = 0; i < m_constrainedDirections.size(); ++i) + { + m_constrainedDirections[i].clear(); + m_constrainedValues[i].clear(); + } + + // Set dirichlet constraints + 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) + { + m_constrainedDirections[counter].push_back(btVector3(1,0,0)); + m_constrainedDirections[counter].push_back(btVector3(0,1,0)); + m_constrainedDirections[counter].push_back(btVector3(0,0,1)); + m_constrainedValues[counter].push_back(0); + m_constrainedValues[counter].push_back(0); + m_constrainedValues[counter].push_back(0); + } + ++counter; + } + } + + // loop through contacts to create contact constraints + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + btMultiBodyJacobianData jacobianData; + const btScalar mrg = psb->getCollisionShape()->getMargin(); + for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i) + { + const btSoftBody::RContact& c = psb->m_rcontacts[i]; + + // skip anchor points + if (c.m_node->m_im == 0) + continue; + + 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; + + // grab the velocity of the rigid body + 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; + } + } + + // TODO: rethink what the velocity of the soft body node should be + // const btVector3 vb = c.m_node->m_x - c.m_node->m_q; + const btVector3 vb = c.m_node->m_v * dt; + 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))); + const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4))); + + //c.m_node->m_v -= impulse * c.m_c2 / dt; + // TODO: only contact is considered here, add friction later + btVector3 normal = cti.m_normal.normalized(); + btVector3 dv = -impulse * c.m_c2; + btScalar dvn = dv.dot(normal); + m_constrainedDirections[m_indices[c.m_node]].push_back(normal); + m_constrainedValues[m_indices[c.m_node]].push_back(dvn); + + 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); + } + } + } + } + } + } +} diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h new file mode 100644 index 000000000..dc2c8800e --- /dev/null +++ b/src/BulletSoftBody/btContactProjection.h @@ -0,0 +1,56 @@ +// +// btContactProjection.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/4/19. +// + +#ifndef BT_CONTACT_PROJECTION_H +#define BT_CONTACT_PROJECTION_H +#include "btCGProjection.h" +#include "btSoftBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include +class btContactProjection : public btCGProjection +{ +public: + btContactProjection(btAlignedObjectArray& softBodies, const btAlignedObjectArray& backup_v) + : btCGProjection(softBodies, backup_v) + { + + } + + virtual ~btContactProjection() + { + + } + + // apply the constraints + virtual void operator()(TVStack& x) + { + for (int i = 0; i < x.size(); ++i) + { + for (int j = 0; j < m_constrainedDirections[i].size(); ++j) + { + x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; + } + } + } + + virtual void enforceConstraint(TVStack& x) + { + for (int i = 0; i < x.size(); ++i) + { + for (int j = 0; j < m_constrainedDirections[i].size(); ++j) + { + x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; + x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j]; + } + } + } + + // update the constraints + virtual void update(btScalar dt, const TVStack& dv); +}; +#endif /* btContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 4e36a2112..85441a803 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -10,12 +10,14 @@ #include "btSoftBodySolvers.h" #include "btBackwardEulerObjective.h" - +#include "btDeformableRigidDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" struct btCollisionObjectWrapper; +class btDeformableRigidDynamicsWorld; + class btDeformableBodySolver : public btSoftBodySolver { using TVStack = btAlignedObjectArray; @@ -29,13 +31,16 @@ protected: btBackwardEulerObjective m_objective; int m_solveIterations; int m_impulseIterations; + btDeformableRigidDynamicsWorld* m_world; + btAlignedObjectArray m_backupVelocity; public: btDeformableBodySolver() : m_numNodes(0) - , m_objective(m_softBodySet) + , m_objective(m_softBodySet, m_backupVelocity) , m_solveIterations(1) , m_impulseIterations(1) + , m_world(nullptr) { } @@ -76,23 +81,11 @@ public: { bool nodeUpdated = updateNodes(); reinitialize(nodeUpdated); - + backupVelocity(); 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); @@ -107,7 +100,9 @@ public: { m_dv.resize(m_numNodes); m_residual.resize(m_numNodes); + m_backupVelocity.resize(m_numNodes); } + for (int i = 0; i < m_dv.size(); ++i) { m_dv[i].setZero(); @@ -151,7 +146,22 @@ public: 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 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; } } } @@ -168,6 +178,7 @@ public: } return false; } + virtual void predictMotion(float solverdt) { for (int i = 0; i < m_softBodySet.size(); ++i) @@ -192,78 +203,11 @@ public: // TODO } - void VSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar dt) + virtual void setWorld(btDeformableRigidDynamicsWorld* world) { - 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); - } - } - } - } - } + m_world = world; + m_objective.setWorld(world); } - }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 73d4f67f6..1b21ed5b3 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -18,12 +18,52 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS { btAssert("Solver initialization failed\n"); } + + // from btDiscreteDynamicsWorld singleStepSimulation + if (0 != m_internalPreTickCallback) + { + (*m_internalPreTickCallback)(this, timeStep); + } + + ///apply gravity, predict motion + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + + + btDispatcherInfo& dispatchInfo = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDispatchInfo(); + + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDebugDrawer(); + + // only used in CCD +// createPredictiveContacts(timeStep); + + ///perform collision detection + btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::performDiscreteCollisionDetection(); - btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); + btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::calculateSimulationIslands(); + + btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + +// btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); + predictUnconstraintMotion(timeStep); + //integrate transforms + btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep); + + ///update vehicle simulation + btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::updateActions(timeStep); + + btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::updateActivationState(timeStep); ///update soft bodies m_deformableBodySolver->updateSoftBodies(); diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 55ed41d86..7f70fef56 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -31,7 +31,6 @@ class btDeformableRigidDynamicsWorld : public btSoftRigidDynamicsWorld ///Solver classes that encapsulate multiple deformable bodies for solving btDeformableBodySolver* m_deformableBodySolver; - protected: virtual void internalSingleStepSimulation(btScalar timeStep); @@ -65,7 +64,7 @@ public: virtual void predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); +// btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); m_deformableBodySolver->predictMotion(float(timeStep)); } // virtual void internalStepSingleStepSimulation(btScalar timeStep); -- cgit v1.2.1 From b8997c36b2b1a25e2d3085ded367b65352a45e31 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 6 Jul 2019 20:55:41 -0700 Subject: update contact projection --- src/BulletSoftBody/btBackwardEulerObjective.h | 1 + src/BulletSoftBody/btContactProjection.cpp | 5 ++-- src/BulletSoftBody/btDeformableBodySolver.h | 27 ++++++++++++++++++++-- .../btDeformableRigidDynamicsWorld.cpp | 9 ++++++-- .../btDeformableRigidDynamicsWorld.h | 2 +- 5 files changed, 37 insertions(+), 7 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index f8f0596b5..cef07aea1 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -101,6 +101,7 @@ public: { // damping force is implicit and elastic force is explicit m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); +// m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); } } diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index b50918af4..2666ff789 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -76,7 +76,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) 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); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1) + btVector3(0,-10,0)*dt) * dt : btVector3(0, 0, 0); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -117,7 +117,8 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) //c.m_node->m_v -= impulse * c.m_c2 / dt; // TODO: only contact is considered here, add friction later btVector3 normal = cti.m_normal.normalized(); - btVector3 dv = -impulse * c.m_c2; + btVector3 diff = c.m_node->m_v - m_backupVelocity[m_indices[c.m_node]]; + btVector3 dv = -impulse * c.m_c2/dt + diff; btScalar dvn = dv.dot(normal); m_constrainedDirections[m_indices[c.m_node]].push_back(normal); m_constrainedValues[m_indices[c.m_node]].push_back(dvn); diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 85441a803..5a0a3a05a 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -8,6 +8,7 @@ #ifndef BT_DEFORMABLE_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H +#include #include "btSoftBodySolvers.h" #include "btBackwardEulerObjective.h" #include "btDeformableRigidDynamicsWorld.h" @@ -87,6 +88,7 @@ public: // only need to advect x here if elastic force is implicit // prepareSolve(solverdt); m_objective.computeResidual(solverdt, m_residual); + moveTempVelocity(solverdt, m_residual); m_objective.computeStep(m_dv, m_residual, solverdt); updateVelocity(); @@ -94,6 +96,20 @@ public: advect(solverdt); } + void moveTempVelocity(btScalar dt, const TVStack& f) + { + 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_v += node.m_im * dt * f[counter++]; + } + } + } + void reinitialize(bool nodeUpdated) { if (nodeUpdated) @@ -119,7 +135,7 @@ public: 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; + node.m_x = node.m_q + dt * node.m_v; } } } @@ -132,7 +148,13 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { auto& node = psb->m_nodes[j]; - node.m_x += dt * m_dv[counter++]; +// node.m_x += dt * m_dv[counter++]; + node.m_x += dt * node.m_v; + if (j == 4) + { + std::cout << "x " << psb->m_nodes[j].m_x.getY() << std::endl; + std::cout << "v " << psb->m_nodes[j].m_v.getY() << std::endl; + } } } } @@ -147,6 +169,7 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + ++counter; } } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 1b21ed5b3..c0afc98d4 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -26,7 +26,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS } ///apply gravity, predict motion - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + predictUnconstraintMotion(timeStep); btDispatcherInfo& dispatchInfo = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDispatchInfo(); @@ -56,7 +56,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); - predictUnconstraintMotion(timeStep); +// predictUnconstraintMotion(timeStep); //integrate transforms btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep); @@ -68,6 +68,11 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///update soft bodies m_deformableBodySolver->updateSoftBodies(); + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) + { + btRigidBody* body = m_nonStaticRigidBodies[i]; + std::cout << "rb v = " << body->getLinearVelocity().getY() << std::endl; + } // End solver-wise simulation step // /////////////////////////////// } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 7f70fef56..98dc4602c 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -64,7 +64,7 @@ public: virtual void predictUnconstraintMotion(btScalar timeStep) { -// btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); m_deformableBodySolver->predictMotion(float(timeStep)); } // virtual void internalStepSingleStepSimulation(btScalar timeStep); -- cgit v1.2.1 From 786b0436ec3eda6fd46066adfa34d35f8a346e9b Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sun, 7 Jul 2019 14:31:19 -0700 Subject: fixed gravity issue in rigid body and deformable body contact solve --- examples/DeformableDemo/DeformableDemo.cpp | 125 +++++++++------------ src/BulletSoftBody/btBackwardEulerObjective.h | 2 +- src/BulletSoftBody/btCGProjection.h | 4 +- src/BulletSoftBody/btContactProjection.cpp | 19 +--- src/BulletSoftBody/btContactProjection.h | 4 +- src/BulletSoftBody/btDeformableBodySolver.h | 24 +--- .../btDeformableRigidDynamicsWorld.cpp | 15 ++- 7 files changed, 72 insertions(+), 121 deletions(-) diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index 7edc3c4a3..990bdd7b9 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -59,16 +59,45 @@ public: void resetCamera() { -// float dist = 30; -// float pitch = -14; -// float yaw = 0; -// float targetPos[3] = {0, 0, 0}; - float dist = 45; + float dist = 15; float pitch = -45; float yaw = 100; - float targetPos[3] = {0,0, 0}; + float targetPos[3] = {0, -5, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } + + + void Ctor_RbUpStack(int count) + { + float mass = 1; + + 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)), + cylinderCompound, + new btSphereShape(0.75) + + }; + 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]); + } + } + virtual const btDeformableRigidDynamicsWorld* getDeformableDynamicsWorld() const { ///just make it a btSoftRigidDynamicsWorld please @@ -105,7 +134,6 @@ 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) @@ -119,22 +147,22 @@ void DeformableDemo::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableRigidDynamicsWorld(m_dispatcher, m_broadphase, m_solver, 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); 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.))); + ///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, -30, 0)); -// groundTransform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI * 0.03)); + 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.); @@ -151,76 +179,30 @@ void DeformableDemo::initPhysics() btRigidBody* body = new btRigidBody(rbInfo); body->setFriction(.5); - //add the body to the dynamics world + //add the ground 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); -// } + + // create a piece of cloth { - const btScalar s = 8; + const btScalar s = 4; 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, +// 3, 3, + 20,20, 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->getCollisionShape()->setMargin(0.25); psb->generateBendingConstraints(2); - psb->setTotalMass(1); - psb->setDampingCoefficient(0.01); + psb->setTotalMass(2); + psb->setDampingCoefficient(0.02); getDeformableDynamicsWorld()->addSoftBody(psb); + + // add a few rigid bodies + Ctor_RbUpStack(10); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } @@ -263,7 +245,10 @@ void DeformableDemo::exitPhysics() } + class CommonExampleInterface* DeformableCreateFunc(struct CommonExampleOptions& options) { return new DeformableDemo(options.m_guiHelper); } + + diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index cef07aea1..5e81fc0f2 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -40,7 +40,7 @@ public: : cg(20) , m_softBodies(softBodies) , precondition(DefaultPreconditioner()) - , projection(m_softBodies, backup_v) + , projection(m_softBodies) { // TODO: this should really be specified in initialization instead of here btMassSpring* mass_spring = new btMassSpring(m_softBodies); diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 89743ef27..533e19c7a 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -23,11 +23,9 @@ public: std::unordered_map m_indices; TVArrayStack m_constrainedDirections; TArrayStack m_constrainedValues; - const TVStack& m_backupVelocity; - btCGProjection(btAlignedObjectArray& softBodies, const TVStack& backup_v) + btCGProjection(btAlignedObjectArray& softBodies) : m_softBodies(softBodies) - , m_backupVelocity(backup_v) { } diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 2666ff789..8f9f9225a 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -9,17 +9,6 @@ #include "btDeformableRigidDynamicsWorld.h" void btContactProjection::update(btScalar dt, const TVStack& dv) { - 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) - { - psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter]; - ++counter; - } - } - ///solve rigid body constraints m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo()); @@ -31,7 +20,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) } // Set dirichlet constraints - counter = 0; + size_t counter = 0; for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; @@ -76,7 +65,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1) + btVector3(0,-10,0)*dt) * dt : btVector3(0, 0, 0); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * dt : btVector3(0, 0, 0); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -114,11 +103,9 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) // const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))); const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4))); - //c.m_node->m_v -= impulse * c.m_c2 / dt; // TODO: only contact is considered here, add friction later btVector3 normal = cti.m_normal.normalized(); - btVector3 diff = c.m_node->m_v - m_backupVelocity[m_indices[c.m_node]]; - btVector3 dv = -impulse * c.m_c2/dt + diff; + btVector3 dv = -impulse * c.m_c2/dt; btScalar dvn = dv.dot(normal); m_constrainedDirections[m_indices[c.m_node]].push_back(normal); m_constrainedValues[m_indices[c.m_node]].push_back(dvn); diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index dc2c8800e..e1c3bc50d 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -15,8 +15,8 @@ class btContactProjection : public btCGProjection { public: - btContactProjection(btAlignedObjectArray& softBodies, const btAlignedObjectArray& backup_v) - : btCGProjection(softBodies, backup_v) + btContactProjection(btAlignedObjectArray& softBodies) + : btCGProjection(softBodies) { } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 5a0a3a05a..9afec42dc 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -82,13 +82,11 @@ public: { bool nodeUpdated = updateNodes(); reinitialize(nodeUpdated); - backupVelocity(); for (int i = 0; i < m_solveIterations; ++i) { // only need to advect x here if elastic force is implicit // prepareSolve(solverdt); m_objective.computeResidual(solverdt, m_residual); - moveTempVelocity(solverdt, m_residual); m_objective.computeStep(m_dv, m_residual, solverdt); updateVelocity(); @@ -116,7 +114,6 @@ public: { m_dv.resize(m_numNodes); m_residual.resize(m_numNodes); - m_backupVelocity.resize(m_numNodes); } for (int i = 0; i < m_dv.size(); ++i) @@ -150,11 +147,6 @@ public: auto& node = psb->m_nodes[j]; // node.m_x += dt * m_dv[counter++]; node.m_x += dt * node.m_v; - if (j == 4) - { - std::cout << "x " << psb->m_nodes[j].m_x.getY() << std::endl; - std::cout << "v " << psb->m_nodes[j].m_v.getY() << std::endl; - } } } } @@ -168,27 +160,13 @@ public: btSoftBody* psb = m_softBodySet[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + psb->m_nodes[j].m_v += m_dv[counter]; ++counter; } } } - 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; - } - } - } - bool updateNodes() { int numNodes = 0; diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index c0afc98d4..82b24ccb8 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -53,6 +53,14 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS // btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); + // incorporate gravity into velocity and clear force + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + clearForces(); + ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); @@ -67,12 +75,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///update soft bodies m_deformableBodySolver->updateSoftBodies(); - - for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) - { - btRigidBody* body = m_nonStaticRigidBodies[i]; - std::cout << "rb v = " << body->getLinearVelocity().getY() << std::endl; - } + // End solver-wise simulation step // /////////////////////////////// } -- cgit v1.2.1 From 13d4e1cc2bb79e28e63990fbf77fb4c1eb8aeb10 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 9 Jul 2019 14:26:04 -0700 Subject: bug fixes in constraints projections; cpplized various functions --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 88 ++++++++++++ src/BulletSoftBody/btBackwardEulerObjective.h | 68 ++------- src/BulletSoftBody/btCGProjection.h | 17 ++- src/BulletSoftBody/btConjugateGradient.h | 68 +-------- src/BulletSoftBody/btContactProjection.cpp | 151 ++++++++++++++------ src/BulletSoftBody/btContactProjection.h | 8 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 157 +++++++++++++++++++++ src/BulletSoftBody/btDeformableBodySolver.h | 89 +++--------- .../btDeformableRigidDynamicsWorld.cpp | 39 ++++- .../btDeformableRigidDynamicsWorld.h | 19 +-- src/BulletSoftBody/btMassSpring.h | 7 +- src/BulletSoftBody/btSoftBodyInternals.h | 2 +- 12 files changed, 450 insertions(+), 263 deletions(-) create mode 100644 src/BulletSoftBody/btBackwardEulerObjective.cpp create mode 100644 src/BulletSoftBody/btDeformableBodySolver.cpp diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp new file mode 100644 index 000000000..22614c994 --- /dev/null +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -0,0 +1,88 @@ +// +// btBackwardEulerObjective.cpp +// BulletSoftBody +// +// Created by Xuchen Han on 7/9/19. +// + +#include "btBackwardEulerObjective.h" + +btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) +: cg(20) +, m_softBodies(softBodies) +, precondition(DefaultPreconditioner()) +, projection(m_softBodies, m_dt) +, m_backupVelocity(backup_v) +{ + // TODO: this should really be specified in initialization instead of here + btMassSpring* mass_spring = new btMassSpring(m_softBodies); + m_lf.push_back(mass_spring); +} + +void btBackwardEulerObjective::reinitialize(bool nodeUpdated) +{ + if(nodeUpdated) + { + projection.setSoftBodies(m_softBodies); + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + projection.reinitialize(nodeUpdated); + } +} + + +void btBackwardEulerObjective::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) + { + // add damping matrix + m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); + + // add stiffness matrix when fully implicity + m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); + } +} + +void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt) +{ + m_dt = dt; + btScalar tolerance = std::numeric_limits::epsilon()* 16 * computeNorm(residual); + cg.solve(*this, dv, residual, tolerance); + } + +void btBackwardEulerObjective::updateVelocity(const TVStack& dv) +{ + for (int i = 0; i < m_softBodies.size(); ++i) + { + int 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) + { + // only the velocity of the constrained nodes needs to be updated during CG solve + if (projection.m_constrainedDirections.size() > 0) + psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter]; + ++counter; + } + } + } +} diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index 5e81fc0f2..424291700 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -35,17 +35,9 @@ public: btAlignedObjectArray& m_softBodies; std::function precondition; btContactProjection projection; + const TVStack& m_backupVelocity; - btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) - : cg(20) - , m_softBodies(softBodies) - , precondition(DefaultPreconditioner()) - , projection(m_softBodies) - { - // TODO: this should really be specified in initialization instead of here - btMassSpring* mass_spring = new btMassSpring(m_softBodies); - m_lf.push_back(mass_spring); - } + btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); virtual ~btBackwardEulerObjective() {} @@ -54,7 +46,6 @@ public: void computeResidual(btScalar dt, TVStack& residual) const { // gravity is treated explicitly in predictUnconstraintMotion - // add force for (int i = 0; i < m_lf.size(); ++i) { @@ -72,62 +63,29 @@ public: return std::sqrt(norm_squared); } - void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt) - { - m_dt = dt; - btScalar tolerance = std::numeric_limits::epsilon()*16 * computeNorm(residual); - cg.solve(*this, dv, residual, tolerance); - } + void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); - 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); -// m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); - } - } + void multiply(const TVStack& x, TVStack& b) const; void updateProjection(const TVStack& dv) { - projection.update(m_dt, dv); + projection.update(dv, m_backupVelocity); } - void reinitialize(bool nodeUpdated) - { - if(nodeUpdated) - { - projection.setSoftBodies(m_softBodies); - } - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->reinitialize(nodeUpdated); - projection.reinitialize(nodeUpdated); - } - } + void reinitialize(bool nodeUpdated); void enforceConstraint(TVStack& x) { projection.enforceConstraint(x); + updateVelocity(x); } + void updateVelocity(const TVStack& dv); + + void setConstraintDirections() + { + projection.setConstraintDirections(); + } void project(TVStack& r, const TVStack& dv) { updateProjection(dv); diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 533e19c7a..8648e99f8 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -23,30 +23,39 @@ public: std::unordered_map m_indices; TVArrayStack m_constrainedDirections; TArrayStack m_constrainedValues; + const btScalar& m_dt; - btCGProjection(btAlignedObjectArray& softBodies) + btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) + , m_dt(dt) { - } virtual ~btCGProjection() { - } // apply the constraints virtual void operator()(TVStack& x) = 0; + virtual void setConstraintDirections() = 0; + // update the constraints - virtual void update(btScalar dt, const TVStack& dv) = 0; + virtual void update(const TVStack& dv, const TVStack& backup_v) = 0; virtual void reinitialize(bool nodeUpdated) { if (nodeUpdated) updateId(); + + // resize and clear the old constraints m_constrainedValues.resize(m_indices.size()); m_constrainedDirections.resize(m_indices.size()); + for (int i = 0; i < m_constrainedDirections.size(); ++i) + { + m_constrainedDirections[i].clear(); + m_constrainedValues[i].clear(); + } } void updateId() diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 2754a208d..0a88e6b94 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -8,6 +8,10 @@ #ifndef BT_CONJUGATE_GRADIENT_H #define BT_CONJUGATE_GRADIENT_H #include +#include +#include +#include + template class btConjugateGradient { @@ -25,69 +29,6 @@ public: 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(TM& A, TVStack& x, const TVStack& b, btScalar tolerance) { @@ -109,7 +50,6 @@ public: // z = M^(-1) * r A.precondition(r, z); - // p = z; p = z; // temp = A*p A.multiply(p, temp); diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 8f9f9225a..4db3e019e 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -7,44 +7,16 @@ #include "btContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" -void btContactProjection::update(btScalar dt, const TVStack& dv) +void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) { ///solve rigid body constraints m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo()); - - // clear the old constraints - for (int i = 0; i < m_constrainedDirections.size(); ++i) - { - m_constrainedDirections[i].clear(); - m_constrainedValues[i].clear(); - } - - // Set dirichlet constraints - 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) - { - m_constrainedDirections[counter].push_back(btVector3(1,0,0)); - m_constrainedDirections[counter].push_back(btVector3(0,1,0)); - m_constrainedDirections[counter].push_back(btVector3(0,0,1)); - m_constrainedValues[counter].push_back(0); - m_constrainedValues[counter].push_back(0); - m_constrainedValues[counter].push_back(0); - } - ++counter; - } - } - + // loop through contacts to create contact constraints for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; btMultiBodyJacobianData jacobianData; - const btScalar mrg = psb->getCollisionShape()->getMargin(); for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i) { const btSoftBody::RContact& c = psb->m_rcontacts[i]; @@ -65,7 +37,7 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) 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); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -86,29 +58,25 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) { vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; } - va = cti.m_normal * vel * dt; + va = cti.m_normal * vel * m_dt; } } - // TODO: rethink what the velocity of the soft body node should be - // const btVector3 vb = c.m_node->m_x - c.m_node->m_q; - const btVector3 vb = c.m_node->m_v * dt; + const btVector3 vb = c.m_node->m_v * m_dt; const btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); - if (dn <= SIMD_EPSILON) + if (1) // in the same CG solve, the set of constraits doesn't change +// 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))); - const btVector3 impulse = c.m_c0 * ((vr - (fv * c.m_c3))+ (cti.m_normal * (dp * c.m_c4))); - + const btVector3 impulse = c.m_c0 *(cti.m_normal * dn); // TODO: only contact is considered here, add friction later - btVector3 normal = cti.m_normal.normalized(); - btVector3 dv = -impulse * c.m_c2/dt; - btScalar dvn = dv.dot(normal); - m_constrainedDirections[m_indices[c.m_node]].push_back(normal); - m_constrainedValues[m_indices[c.m_node]].push_back(dvn); + + // 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); + m_constrainedValues[m_indices[c.m_node]][0]=(dvn); if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -128,3 +96,94 @@ void btContactProjection::update(btScalar dt, const TVStack& dv) } } } + +void btContactProjection::setConstraintDirections() +{ + // set Dirichlet constraint + 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) + { + m_constrainedDirections[counter].push_back(btVector3(1,0,0)); + m_constrainedDirections[counter].push_back(btVector3(0,1,0)); + m_constrainedDirections[counter].push_back(btVector3(0,0,1)); + m_constrainedValues[counter].push_back(0); + m_constrainedValues[counter].push_back(0); + m_constrainedValues[counter].push_back(0); + } + ++counter; + } + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + btMultiBodyJacobianData jacobianData; + + int j = 0; + while (j < psb->m_rcontacts.size()) + { + const btSoftBody::RContact& c = psb->m_rcontacts[j]; + // skip anchor points + if (c.m_node->m_im == 0) + { + psb->m_rcontacts.removeAtIndex(j); + continue; + } + + 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; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_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 * m_dt; + } + } + + const btVector3 vb = c.m_node->m_v * m_dt; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) + { + ++j; + m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal); + m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1); + continue; + } + } + psb->m_rcontacts.removeAtIndex(j); + } + } +} diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index e1c3bc50d..5598ada0a 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -15,8 +15,8 @@ class btContactProjection : public btCGProjection { public: - btContactProjection(btAlignedObjectArray& softBodies) - : btCGProjection(softBodies) + btContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) + : btCGProjection(softBodies, dt) { } @@ -51,6 +51,8 @@ public: } // update the constraints - virtual void update(btScalar dt, const TVStack& dv); + virtual void update(const TVStack& dv, const TVStack& backupVelocity); + + virtual void setConstraintDirections(); }; #endif /* btContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp new file mode 100644 index 000000000..d3d448db0 --- /dev/null +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -0,0 +1,157 @@ +// +// btDeformableBodySolver.cpp +// BulletSoftBody +// +// Created by Xuchen Han on 7/9/19. +// + +#include +#include "btDeformableBodySolver.h" + +void btDeformableBodySolver::postStabilize() +{ + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + btMultiBodyJacobianData jacobianData; + const btScalar mrg = psb->getCollisionShape()->getMargin(); + for (int j = 0; j < psb->m_rcontacts.size(); ++j) + { + const btSoftBody::RContact& c = psb->m_rcontacts[j]; + // skip anchor points + if (c.m_node->m_im == 0) + continue; + + 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; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_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 * m_dt; + } + } + + const btVector3 vb = c.m_node->m_v * m_dt; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + + const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); + + // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient + + btScalar dvn = dn * c.m_c4; + const btVector3 impulse = c.m_c0 * ((cti.m_normal * (dn * c.m_c4))); + // TODO: only contact is considered here, add friction later + if (dp < 0) + { + c.m_node->m_x -= dp * cti.m_normal * c.m_c4; + + //// + // 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); + // } + // } + } + } + } +} + +void btDeformableBodySolver::solveConstraints(float solverdt) +{ + m_dt = solverdt; + bool nodeUpdated = updateNodes(); + reinitialize(nodeUpdated); + backupVelocity(); + postStabilize(); + for (int i = 0; i < m_solveIterations; ++i) + { + m_objective->computeResidual(solverdt, m_residual); + m_objective->computeStep(m_dv, m_residual, solverdt); + updateVelocity(); + } + advect(solverdt); +} + +void btDeformableBodySolver::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); + + // remove contact constraints with separating velocity + setConstraintDirections(); +} + +void btDeformableBodySolver::setConstraintDirections() +{ + m_objective->setConstraintDirections(); +} + +void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world) +{ + m_world = world; + m_objective->setWorld(world); +} + +void btDeformableBodySolver::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]; + psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; + ++counter; + } + } +} diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 9afec42dc..540a2b6cb 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -16,7 +16,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" struct btCollisionObjectWrapper; - +class btBackwardEulerObjective; class btDeformableRigidDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver @@ -29,25 +29,17 @@ protected: TVStack m_dv; TVStack m_residual; btAlignedObjectArray m_softBodySet; - btBackwardEulerObjective m_objective; + btBackwardEulerObjective* m_objective; int m_solveIterations; int m_impulseIterations; btDeformableRigidDynamicsWorld* m_world; btAlignedObjectArray m_backupVelocity; + btScalar m_dt; public: - btDeformableBodySolver() - : m_numNodes(0) - , m_objective(m_softBodySet, m_backupVelocity) - , m_solveIterations(1) - , m_impulseIterations(1) - , m_world(nullptr) - { - } + btDeformableBodySolver(); - virtual ~btDeformableBodySolver() - { - } + virtual ~btDeformableBodySolver(); virtual SolverTypes getSolverType() const { @@ -78,21 +70,9 @@ public: virtual void copyBackToSoftBodies(bool bMove = true) {} - virtual void solveConstraints(float solverdt) - { - bool nodeUpdated = updateNodes(); - reinitialize(nodeUpdated); - for (int i = 0; i < m_solveIterations; ++i) - { - // 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); - } + virtual void solveConstraints(float solverdt); + + void postStabilize(); void moveTempVelocity(btScalar dt, const TVStack& f) { @@ -108,34 +88,10 @@ public: } } - 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 reinitialize(bool nodeUpdated); + + void setConstraintDirections(); - 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; - } - } - } void advect(btScalar dt) { size_t counter = 0; @@ -145,13 +101,13 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { auto& node = psb->m_nodes[j]; -// node.m_x += dt * m_dv[counter++]; - node.m_x += dt * node.m_v; + node.m_x += dt * m_dv[counter++]; +// node.m_x = node.m_q + dt * node.m_v; } } } - void updateVelocity() + void backupVelocity() { // serial implementation int counter = 0; @@ -160,13 +116,13 @@ public: btSoftBody* psb = m_softBodySet[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - psb->m_nodes[j].m_v += m_dv[counter]; - - ++counter; + m_backupVelocity[counter++] = psb->m_nodes[j].m_v; } } } + void updateVelocity(); + bool updateNodes() { int numNodes = 0; @@ -175,6 +131,7 @@ public: if (numNodes != m_numNodes) { m_numNodes = numNodes; + m_backupVelocity.resize(numNodes); return true; } return false; @@ -200,15 +157,11 @@ public: softBody->defaultCollisionHandler(collisionObjectWrap); } - virtual void processCollision(btSoftBody *, btSoftBody *) { - // TODO + virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { + softBody->defaultCollisionHandler(otherSoftBody); } - virtual void setWorld(btDeformableRigidDynamicsWorld* world) - { - m_world = world; - m_objective.setWorld(world); - } + virtual void setWorld(btDeformableRigidDynamicsWorld* world); }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 82b24ccb8..0bbb2e995 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -9,6 +9,20 @@ #include "btDeformableRigidDynamicsWorld.h" #include "btDeformableBodySolver.h" +btDeformableBodySolver::btDeformableBodySolver() +: m_numNodes(0) +, m_solveIterations(1) +, m_impulseIterations(1) +, m_world(nullptr) +{ + m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity); +} + +btDeformableBodySolver::~btDeformableBodySolver() +{ + delete m_objective; +} + void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { // Let the solver grab the soft bodies and if necessary optimize for it @@ -50,9 +64,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS (*m_internalTickCallback)(this, timeStep); } - -// btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::internalSingleStepSimulation(timeStep); - // incorporate gravity into velocity and clear force for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) { @@ -64,7 +75,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); -// predictUnconstraintMotion(timeStep); //integrate transforms btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep); @@ -76,6 +86,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///update soft bodies m_deformableBodySolver->updateSoftBodies(); + clearForces(); // End solver-wise simulation step // /////////////////////////////// } @@ -85,3 +96,23 @@ void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar t m_deformableBodySolver->solveConstraints(timeStep); } +void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) +{ + 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); +} + +void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + m_deformableBodySolver->predictMotion(float(timeStep)); +} + + diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 98dc4602c..11ad969a2 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -62,27 +62,12 @@ public: return BT_DEFORMABLE_RIGID_DYNAMICS_WORLD; } - virtual void predictUnconstraintMotion(btScalar timeStep) - { - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); - m_deformableBodySolver->predictMotion(float(timeStep)); - } + virtual void predictUnconstraintMotion(btScalar 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); - } + virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); }; #endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btMassSpring.h b/src/BulletSoftBody/btMassSpring.h index ffe21c801..9fd3c405a 100644 --- a/src/BulletSoftBody/btMassSpring.h +++ b/src/BulletSoftBody/btMassSpring.h @@ -37,7 +37,12 @@ public: size_t id2 = m_indices[node2]; // elastic force + + // fully implicit btVector3 dir = (node2->m_x - node1->m_x); + + // explicit elastic force +// btVector3 dir = (node2->m_q - node1->m_q); btVector3 dir_normalized = dir.normalized(); btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r); force[id1] += scaled_force; @@ -89,7 +94,7 @@ public: 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 + btScalar k_damp = psb->m_dampingCoefficient; size_t id1 = m_indices[node1]; size_t id2 = m_indices[node2]; btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 7efe514f3..41911b2bc 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -880,7 +880,7 @@ struct btSoftColliders const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 ra = n.m_q - wtr.getOrigin(); const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); const btVector3 vb = n.m_x - n.m_q; const btVector3 vr = vb - va; -- cgit v1.2.1 From c4e316f005f98ec8007a9278b4af93662a0631c7 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 9 Jul 2019 15:27:17 -0700 Subject: btDeformableRigidWorld now inherits from btMultiBodyDynamicsWorld instead of btSoftRigidDynamicsWorld --- src/BulletDynamics/Dynamics/btDynamicsWorld.h | 2 +- .../Featherstone/btMultiBodyDynamicsWorld.h | 5 +- src/BulletSoftBody/btContactProjection.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 14 +++++ .../btDeformableRigidDynamicsWorld.cpp | 34 ++++------- .../btDeformableRigidDynamicsWorld.h | 67 ++++++++++++++++++---- 6 files changed, 86 insertions(+), 38 deletions(-) diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 10853b761..3c55234a8 100644 --- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -35,7 +35,7 @@ enum btDynamicsWorldType BT_SOFT_RIGID_DYNAMICS_WORLD = 4, BT_GPU_DYNAMICS_WORLD = 5, BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6, - BT_DEFORMABLE_RIGID_DYNAMICS_WORLD = 7 + BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index e36c2f7aa..23641a37b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -47,7 +47,7 @@ protected: virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); - virtual void solveConstraints(btContactSolverInfo& solverInfo); + virtual void serializeMultiBodies(btSerializer* serializer); @@ -55,7 +55,8 @@ public: btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); virtual ~btMultiBodyDynamicsWorld(); - + + virtual void solveConstraints(btContactSolverInfo& solverInfo); virtual void addMultiBody(btMultiBody* body, int group = btBroadphaseProxy::DefaultFilter, int mask = btBroadphaseProxy::AllFilter); virtual void removeMultiBody(btMultiBody* body); diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 4db3e019e..c7a947fbe 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -10,7 +10,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) { ///solve rigid body constraints - m_world->btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::solveConstraints(m_world->getSolverInfo()); + m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); // loop through contacts to create contact constraints for (int i = 0; i < m_softBodies.size(); ++i) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index d3d448db0..9f94e9f6b 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -8,6 +8,20 @@ #include #include "btDeformableBodySolver.h" +btDeformableBodySolver::btDeformableBodySolver() +: m_numNodes(0) +, m_solveIterations(1) +, m_impulseIterations(1) +, m_world(nullptr) +{ + m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity); +} + +btDeformableBodySolver::~btDeformableBodySolver() +{ + delete m_objective; +} + void btDeformableBodySolver::postStabilize() { for (int i = 0; i < m_softBodySet.size(); ++i) diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 0bbb2e995..ded4762e0 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -9,24 +9,11 @@ #include "btDeformableRigidDynamicsWorld.h" #include "btDeformableBodySolver.h" -btDeformableBodySolver::btDeformableBodySolver() -: m_numNodes(0) -, m_solveIterations(1) -, m_impulseIterations(1) -, m_world(nullptr) -{ - m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity); -} - -btDeformableBodySolver::~btDeformableBodySolver() -{ - delete m_objective; -} void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { // Let the solver grab the soft bodies and if necessary optimize for it - m_deformableBodySolver->optimize(getSoftDynamicsWorld()->getSoftBodyArray()); + m_deformableBodySolver->optimize(m_softBodies); if (!m_deformableBodySolver->checkInitialized()) { @@ -42,22 +29,21 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///apply gravity, predict motion predictUnconstraintMotion(timeStep); - - btDispatcherInfo& dispatchInfo = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDispatchInfo(); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getDebugDrawer(); + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); // only used in CCD // createPredictiveContacts(timeStep); ///perform collision detection - btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::performDiscreteCollisionDetection(); + btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); - btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::calculateSimulationIslands(); + btMultiBodyDynamicsWorld::calculateSimulationIslands(); - btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; if (0 != m_internalTickCallback) { @@ -76,12 +62,12 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS solveDeformableBodiesConstraints(timeStep); //integrate transforms - btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::integrateTransforms(timeStep); + btMultiBodyDynamicsWorld::integrateTransforms(timeStep); ///update vehicle simulation - btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::updateActions(timeStep); + btMultiBodyDynamicsWorld::updateActions(timeStep); - btSoftRigidDynamicsWorld::btDiscreteDynamicsWorld::updateActivationState(timeStep); + btMultiBodyDynamicsWorld::updateActivationState(timeStep); ///update soft bodies m_deformableBodySolver->updateSoftBodies(); @@ -98,7 +84,7 @@ void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar t void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask) { - getSoftDynamicsWorld()->getSoftBodyArray().push_back(body); + m_softBodies.push_back(body); // Set the soft body solver that will deal with this body // to be the world's solver diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 11ad969a2..ca1fff885 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -16,50 +16,76 @@ #ifndef BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H #define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H -#include "btSoftRigidDynamicsWorld.h" +#include "btSoftMultiBodyDynamicsWorld.h" #include "btLagrangianForce.h" #include "btMassSpring.h" #include "btDeformableBodySolver.h" +#include "btSoftBodyHelpers.h" + typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; class btLagrangianForce; +typedef btAlignedObjectArray btSoftBodyArray; -class btDeformableRigidDynamicsWorld : public btSoftRigidDynamicsWorld +class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld { using TVStack = btAlignedObjectArray; ///Solver classes that encapsulate multiple deformable bodies for solving btDeformableBodySolver* m_deformableBodySolver; - + btSoftBodyArray m_softBodies; + int m_drawFlags; + bool m_drawNodeTree; + bool m_drawFaceTree; + bool m_drawClusterTree; + btSoftBodyWorldInfo m_sbi; + bool m_ownsSolver; + 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), + btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) + : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), m_deformableBodySolver(deformableBodySolver) { + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); + + m_sbi.air_density = (btScalar)1.2; + m_sbi.water_density = 0; + m_sbi.water_offset = 0; + m_sbi.water_normal = btVector3(0, 0, 0); + m_sbi.m_gravity.setValue(0, -10, 0); + + m_sbi.m_sparsesdf.Initialize(); } virtual ~btDeformableRigidDynamicsWorld() { } - virtual btSoftRigidDynamicsWorld* getSoftDynamicsWorld() + virtual btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() { - return (btSoftRigidDynamicsWorld*)(this); + return (btMultiBodyDynamicsWorld*)(this); } - virtual const btSoftRigidDynamicsWorld* getSoftDynamicsWorld() const + virtual const btMultiBodyDynamicsWorld* getMultiBodyDynamicsWorld() const { - return (const btSoftRigidDynamicsWorld*)(this); + return (const btMultiBodyDynamicsWorld*)(this); } virtual btDynamicsWorldType getWorldType() const { - return BT_DEFORMABLE_RIGID_DYNAMICS_WORLD; + return BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD; } virtual void predictUnconstraintMotion(btScalar timeStep); @@ -68,6 +94,27 @@ public: // virtual void solveDeformableBodiesConstraints(btScalar timeStep); virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); + + btSoftBodyArray& getSoftBodyArray() + { + return m_softBodies; + } + + const btSoftBodyArray& getSoftBodyArray() const + { + return m_softBodies; + } + + btSoftBodyWorldInfo& getWorldInfo() + { + return m_sbi; + } + const btSoftBodyWorldInfo& getWorldInfo() const + { + return m_sbi; + } + int getDrawFlags() const { return (m_drawFlags); } + void setDrawFlags(int f) { m_drawFlags = f; } }; #endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H -- cgit v1.2.1 From 77d670ae416d77d078e09099e57f743afd239acb Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 10 Jul 2019 16:08:46 -0700 Subject: separate external force solve from constraint solve and eliminate damping in external force solve --- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 575 +++++++++++---------- .../Featherstone/btMultiBodyDynamicsWorld.h | 2 + 2 files changed, 295 insertions(+), 282 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 1131e5378..718106e77 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -421,288 +421,7 @@ void btMultiBodyDynamicsWorld::forwardKinematics() } void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { - forwardKinematics(); - - BT_PROFILE("solveConstraints"); - - clearMultiBodyConstraintForces(); - - m_sortedConstraints.resize(m_constraints.size()); - int i; - for (i = 0; i < getNumConstraints(); i++) - { - m_sortedConstraints[i] = m_constraints[i]; - } - m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); - btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - - m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); - for (i = 0; i < m_multiBodyConstraints.size(); i++) - { - m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; - } - m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); - - btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; - - m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); - m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); - -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - { - BT_PROFILE("btMultiBody addForce"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - - bod->addBaseForce(m_gravity * bod->getBaseMass()); - - for (int j = 0; j < bod->getNumLinks(); ++j) - { - bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); - } - } //if (!isSleeping) - } - } -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - - { - BT_PROFILE("btMultiBody stepVelocities"); - for (int i = 0; i < this->m_multiBodies.size(); i++) - { - btMultiBody* bod = m_multiBodies[i]; - - bool isSleeping = false; - - if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) - { - isSleeping = true; - } - for (int b = 0; b < bod->getNumLinks(); b++) - { - if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) - isSleeping = true; - } - - if (!isSleeping) - { - //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) - m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) - m_scratch_v.resize(bod->getNumLinks() + 1); - m_scratch_m.resize(bod->getNumLinks() + 1); - bool doNotUpdatePos = false; - bool isConstraintPass = false; - { - if (!bod->isUsingRK4Integration()) - { - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, - m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - else - { - // - int numDofs = bod->getNumDofs() + 6; - int numPosVars = bod->getNumPosVars() + 7; - btAlignedObjectArray scratch_r2; - scratch_r2.resize(2 * numPosVars + 8 * numDofs); - //convenience - btScalar* pMem = &scratch_r2[0]; - btScalar* scratch_q0 = pMem; - pMem += numPosVars; - btScalar* scratch_qx = pMem; - pMem += numPosVars; - btScalar* scratch_qd0 = pMem; - pMem += numDofs; - btScalar* scratch_qd1 = pMem; - pMem += numDofs; - btScalar* scratch_qd2 = pMem; - pMem += numDofs; - btScalar* scratch_qd3 = pMem; - pMem += numDofs; - btScalar* scratch_qdd0 = pMem; - pMem += numDofs; - btScalar* scratch_qdd1 = pMem; - pMem += numDofs; - btScalar* scratch_qdd2 = pMem; - pMem += numDofs; - btScalar* scratch_qdd3 = pMem; - pMem += numDofs; - btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]); - - ///// - //copy q0 to scratch_q0 and qd0 to scratch_qd0 - scratch_q0[0] = bod->getWorldToBaseRot().x(); - scratch_q0[1] = bod->getWorldToBaseRot().y(); - scratch_q0[2] = bod->getWorldToBaseRot().z(); - scratch_q0[3] = bod->getWorldToBaseRot().w(); - scratch_q0[4] = bod->getBasePos().x(); - scratch_q0[5] = bod->getBasePos().y(); - scratch_q0[6] = bod->getBasePos().z(); - // - for (int link = 0; link < bod->getNumLinks(); ++link) - { - for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) - scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; - } - // - for (int dof = 0; dof < numDofs; ++dof) - scratch_qd0[dof] = bod->getVelocityVector()[dof]; - //// - struct - { - btMultiBody* bod; - btScalar *scratch_qx, *scratch_q0; - - void operator()() - { - for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) - scratch_qx[dof] = scratch_q0[dof]; - } - } pResetQx = {bod, scratch_qx, scratch_q0}; - // - struct - { - void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size) - { - for (int i = 0; i < size; ++i) - pVal[i] = pCurVal[i] + dt * pDer[i]; - } - - } pEulerIntegrate; - // - struct - { - void operator()(btMultiBody* pBody, const btScalar* pData) - { - btScalar* pVel = const_cast(pBody->getVelocityVector()); - - for (int i = 0; i < pBody->getNumDofs() + 6; ++i) - pVel[i] = pData[i]; - } - } pCopyToVelocityVector; - // - struct - { - void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) - { - for (int i = 0; i < size; ++i) - pDst[i] = pSrc[start + i]; - } - } pCopy; - // - - btScalar h = solverInfo.m_timeStep; -#define output &m_scratch_r[bod->getNumDofs()] - //calc qdd0 from: q0 & qd0 - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd0, 0, numDofs); - //calc q1 = q0 + h/2 * qd0 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0); - //calc qd1 = qd0 + h/2 * qdd0 - pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); - // - //calc qdd1 from: q1 & qd1 - pCopyToVelocityVector(bod, scratch_qd1); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd1, 0, numDofs); - //calc q2 = q0 + h/2 * qd1 - pResetQx(); - bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1); - //calc qd2 = qd0 + h/2 * qdd1 - pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); - // - //calc qdd2 from: q2 & qd2 - pCopyToVelocityVector(bod, scratch_qd2); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd2, 0, numDofs); - //calc q3 = q0 + h * qd2 - pResetQx(); - bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); - //calc qd3 = qd0 + h * qdd2 - pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); - // - //calc qdd3 from: q3 & qd3 - pCopyToVelocityVector(bod, scratch_qd3); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - pCopy(output, scratch_qdd3, 0, numDofs); - - // - //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) - //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) - btAlignedObjectArray delta_q; - delta_q.resize(numDofs); - btAlignedObjectArray delta_qd; - delta_qd.resize(numDofs); - for (int i = 0; i < numDofs; ++i) - { - delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]); - delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]); - //delta_q[i] = h*scratch_qd0[i]; - //delta_qd[i] = h*scratch_qdd0[i]; - } - // - pCopyToVelocityVector(bod, scratch_qd0); - bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); - // - if (!doNotUpdatePos) - { - btScalar* pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - - for (int i = 0; i < numDofs; ++i) - pRealBuf[i] = delta_q[i]; - - //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); - bod->setPosUpdated(true); - } - - //ugly hack which resets the cached data to t0 (needed for constraint solver) - { - for (int link = 0; link < bod->getNumLinks(); ++link) - bod->getLink(link).updateCacheMultiDof(); - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, - isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); - } - } - } - -#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - bod->clearForcesAndTorques(); -#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY - } //if (!isSleeping) - } - } + solveExternalForces(solverInfo); /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); @@ -760,6 +479,298 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } +void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverInfo) +{ + forwardKinematics(); + + BT_PROFILE("solveConstraints"); + + clearMultiBodyConstraintForces(); + + m_sortedConstraints.resize(m_constraints.size()); + int i; + for (i = 0; i < getNumConstraints(); i++) + { + m_sortedConstraints[i] = m_constraints[i]; + } + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + + m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); + for (i = 0; i < m_multiBodyConstraints.size(); i++) + { + m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; + } + m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); + + btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; + + m_solverMultiBodyIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer()); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + +#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + { + BT_PROFILE("btMultiBody addForce"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + + bod->addBaseForce(m_gravity * bod->getBaseMass()); + + for (int j = 0; j < bod->getNumLinks(); ++j) + { + bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); + } + } //if (!isSleeping) + } + } +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + + { + BT_PROFILE("btMultiBody stepVelocities"); + for (int i = 0; i < this->m_multiBodies.size(); i++) + { + btMultiBody* bod = m_multiBodies[i]; + + bool isSleeping = false; + + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) + m_scratch_r.resize(bod->getNumLinks() + 1); //multidof? ("Y"s use it and it is used to store qdd) + m_scratch_v.resize(bod->getNumLinks() + 1); + m_scratch_m.resize(bod->getNumLinks() + 1); + bool doNotUpdatePos = false; + bool isConstraintPass = false; + { + if (!bod->isUsingRK4Integration()) + { + const btScalar linearDamp = bod->getLinearDamping(); +// const btScalar angularDamp = bod->getAngularDamping(); + bod->setLinearDamping(0); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, + m_scratch_r, m_scratch_v, m_scratch_m,isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + bod->setLinearDamping(linearDamp); +// bod->setAngularDamping(angularDamp); + } + else + { + // + int numDofs = bod->getNumDofs() + 6; + int numPosVars = bod->getNumPosVars() + 7; + btAlignedObjectArray scratch_r2; + scratch_r2.resize(2 * numPosVars + 8 * numDofs); + //convenience + btScalar* pMem = &scratch_r2[0]; + btScalar* scratch_q0 = pMem; + pMem += numPosVars; + btScalar* scratch_qx = pMem; + pMem += numPosVars; + btScalar* scratch_qd0 = pMem; + pMem += numDofs; + btScalar* scratch_qd1 = pMem; + pMem += numDofs; + btScalar* scratch_qd2 = pMem; + pMem += numDofs; + btScalar* scratch_qd3 = pMem; + pMem += numDofs; + btScalar* scratch_qdd0 = pMem; + pMem += numDofs; + btScalar* scratch_qdd1 = pMem; + pMem += numDofs; + btScalar* scratch_qdd2 = pMem; + pMem += numDofs; + btScalar* scratch_qdd3 = pMem; + pMem += numDofs; + btAssert((pMem - (2 * numPosVars + 8 * numDofs)) == &scratch_r2[0]); + + ///// + //copy q0 to scratch_q0 and qd0 to scratch_qd0 + scratch_q0[0] = bod->getWorldToBaseRot().x(); + scratch_q0[1] = bod->getWorldToBaseRot().y(); + scratch_q0[2] = bod->getWorldToBaseRot().z(); + scratch_q0[3] = bod->getWorldToBaseRot().w(); + scratch_q0[4] = bod->getBasePos().x(); + scratch_q0[5] = bod->getBasePos().y(); + scratch_q0[6] = bod->getBasePos().z(); + // + for (int link = 0; link < bod->getNumLinks(); ++link) + { + for (int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) + scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; + } + // + for (int dof = 0; dof < numDofs; ++dof) + scratch_qd0[dof] = bod->getVelocityVector()[dof]; + //// + struct + { + btMultiBody* bod; + btScalar *scratch_qx, *scratch_q0; + + void operator()() + { + for (int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) + scratch_qx[dof] = scratch_q0[dof]; + } + } pResetQx = {bod, scratch_qx, scratch_q0}; + // + struct + { + void operator()(btScalar dt, const btScalar* pDer, const btScalar* pCurVal, btScalar* pVal, int size) + { + for (int i = 0; i < size; ++i) + pVal[i] = pCurVal[i] + dt * pDer[i]; + } + + } pEulerIntegrate; + // + struct + { + void operator()(btMultiBody* pBody, const btScalar* pData) + { + btScalar* pVel = const_cast(pBody->getVelocityVector()); + + for (int i = 0; i < pBody->getNumDofs() + 6; ++i) + pVel[i] = pData[i]; + } + } pCopyToVelocityVector; + // + struct + { + void operator()(const btScalar* pSrc, btScalar* pDst, int start, int size) + { + for (int i = 0; i < size; ++i) + pDst[i] = pSrc[start + i]; + } + } pCopy; + // + + btScalar h = solverInfo.m_timeStep; +#define output &m_scratch_r[bod->getNumDofs()] + //calc qdd0 from: q0 & qd0 + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd0, 0, numDofs); + //calc q1 = q0 + h/2 * qd0 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd0); + //calc qd1 = qd0 + h/2 * qdd0 + pEulerIntegrate(btScalar(.5) * h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); + // + //calc qdd1 from: q1 & qd1 + pCopyToVelocityVector(bod, scratch_qd1); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd1, 0, numDofs); + //calc q2 = q0 + h/2 * qd1 + pResetQx(); + bod->stepPositionsMultiDof(btScalar(.5) * h, scratch_qx, scratch_qd1); + //calc qd2 = qd0 + h/2 * qdd1 + pEulerIntegrate(btScalar(.5) * h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); + // + //calc qdd2 from: q2 & qd2 + pCopyToVelocityVector(bod, scratch_qd2); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd2, 0, numDofs); + //calc q3 = q0 + h * qd2 + pResetQx(); + bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); + //calc qd3 = qd0 + h * qdd2 + pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); + // + //calc qdd3 from: q3 & qd3 + pCopyToVelocityVector(bod, scratch_qd3); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0., m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + pCopy(output, scratch_qdd3, 0, numDofs); + + // + //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) + //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) + btAlignedObjectArray delta_q; + delta_q.resize(numDofs); + btAlignedObjectArray delta_qd; + delta_qd.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + delta_q[i] = h / btScalar(6.) * (scratch_qd0[i] + 2 * scratch_qd1[i] + 2 * scratch_qd2[i] + scratch_qd3[i]); + delta_qd[i] = h / btScalar(6.) * (scratch_qdd0[i] + 2 * scratch_qdd1[i] + 2 * scratch_qdd2[i] + scratch_qdd3[i]); + //delta_q[i] = h*scratch_qd0[i]; + //delta_qd[i] = h*scratch_qdd0[i]; + } + // + pCopyToVelocityVector(bod, scratch_qd0); + bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); + // + if (!doNotUpdatePos) + { + btScalar* pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); + + for (int i = 0; i < numDofs; ++i) + pRealBuf[i] = delta_q[i]; + + //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); + bod->setPosUpdated(true); + } + + //ugly hack which resets the cached data to t0 (needed for constraint solver) + { + for (int link = 0; link < bod->getNumLinks(); ++link) + bod->getLink(link).updateCacheMultiDof(); + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(0, m_scratch_r, m_scratch_v, m_scratch_m, + isConstraintPass,getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } + } + } + +#ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + bod->clearForcesAndTorques(); +#endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY + } //if (!isSleeping) + } + } +} + + void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 23641a37b..019d1bd87 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -111,6 +111,8 @@ public: virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); virtual void setConstraintSolver(btConstraintSolver* solver); virtual void getAnalyticsData(btAlignedObjectArray& m_islandAnalyticsData) const; + + virtual void solveExternalForces(btContactSolverInfo& solverInfo); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H -- cgit v1.2.1 From b7e512a5f9e099c3aefaf5a5ffb79a37ababde29 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 10 Jul 2019 16:09:38 -0700 Subject: sync gravity with substeps --- src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index ded4762e0..5fdf01a18 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -50,13 +50,24 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS (*m_internalTickCallback)(this, timeStep); } - // incorporate gravity into velocity and clear force + // TODO: This is an ugly hack to get the desired gravity behavior. + // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep + // when there are multiple substeps + + clearForces(); + clearMultiBodyForces(); + btMultiBodyDynamicsWorld::applyGravity(); + // integrate rigid body gravity for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) { btRigidBody* rb = m_nonStaticRigidBodies[i]; rb->integrateVelocities(timeStep); } + // integrate multibody gravity + btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo()); clearForces(); + clearMultiBodyForces(); ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); -- cgit v1.2.1 From b28f1fdac3d38171d37d1f938df8a0dff3afd8af Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 11 Jul 2019 10:14:58 -0700 Subject: add support for more than one constraint for a single deformable node --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 4 +- src/BulletSoftBody/btCGProjection.h | 3 ++ src/BulletSoftBody/btContactProjection.cpp | 56 +++++++++++++++++++++++++ src/BulletSoftBody/btContactProjection.h | 42 +++++++++++++++---- 4 files changed, 96 insertions(+), 9 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index 22614c994..69f5cd6ac 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -8,7 +8,7 @@ #include "btBackwardEulerObjective.h" btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) -: cg(20) +: cg(50) , m_softBodies(softBodies) , precondition(DefaultPreconditioner()) , projection(m_softBodies, m_dt) @@ -66,7 +66,7 @@ void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, m_dt = dt; btScalar tolerance = std::numeric_limits::epsilon()* 16 * computeNorm(residual); cg.solve(*this, dv, residual, tolerance); - } +} void btBackwardEulerObjective::updateVelocity(const TVStack& dv) { diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 8648e99f8..ff1d6cbdd 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -23,6 +23,8 @@ public: std::unordered_map m_indices; TVArrayStack m_constrainedDirections; TArrayStack m_constrainedValues; + btAlignedObjectArray m_constrainedId; + const btScalar& m_dt; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) @@ -56,6 +58,7 @@ public: m_constrainedDirections[i].clear(); m_constrainedValues[i].clear(); } + m_constrainedId.clear(); } void updateId() diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index c7a947fbe..c45cccf5d 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -114,6 +114,7 @@ void btContactProjection::setConstraintDirections() m_constrainedValues[counter].push_back(0); m_constrainedValues[counter].push_back(0); m_constrainedValues[counter].push_back(0); + m_constrainedId.push_back(counter); } ++counter; } @@ -180,10 +181,65 @@ void btContactProjection::setConstraintDirections() ++j; m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal); m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1); + m_constrainedId.push_back(m_indices[c.m_node]); continue; } } psb->m_rcontacts.removeAtIndex(j); } } + + // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions + counter = 0; + const int dim = 3; + 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 (m_constrainedDirections[counter].size() > dim) + { + btAlignedObjectArray prunedConstraints; + // always keep the first constrained direction + prunedConstraints.push_back(m_constrainedDirections[counter][0]); + // find the direction most orthogonal to the first direction and keep it + size_t selected = 1; + btScalar min_dotProductAbs = std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][selected])); + for (int j = 2; j < m_constrainedDirections[counter].size(); ++j) + { + btScalar dotProductAbs =std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][j])); + if (dotProductAbs < min_dotProductAbs) + { + selected = j; + min_dotProductAbs = dotProductAbs; + } + } + if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON) + { + m_constrainedDirections[counter++] = prunedConstraints; + continue; + } + prunedConstraints.push_back(m_constrainedDirections[counter][selected]); + + // find the direction most orthogonal to the previous two directions and keep it + size_t selected2 = (selected == 1) ? 2 : 1; + btVector3 normal = btCross(prunedConstraints[0], prunedConstraints[1]); + normal.normalize(); + btScalar max_dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][selected2])); + for (int j = 3; j < m_constrainedDirections[counter].size(); ++j) + { + btScalar dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][j])); + if (dotProductAbs > min_dotProductAbs) + { + selected2 = j; + max_dotProductAbs = dotProductAbs; + } + } + prunedConstraints.push_back(m_constrainedDirections[counter][selected2]); + m_constrainedDirections[counter] = prunedConstraints; + m_constrainedValues[counter].resize(dim); + } + ++counter; + } + } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 5598ada0a..c0897e9bf 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -29,24 +29,52 @@ public: // apply the constraints virtual void operator()(TVStack& x) { - for (int i = 0; i < x.size(); ++i) + const int dim = 3; + for (int j = 0; j < m_constrainedId.size(); ++j) { - for (int j = 0; j < m_constrainedDirections[i].size(); ++j) + int i = m_constrainedId[j]; + btAssert(m_constrainedDirections[i].size() <= dim); + if (m_constrainedDirections[i].size() <= 1) { - x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; + for (int j = 0; j < m_constrainedDirections[i].size(); ++j) + { + x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; + } } + else if (m_constrainedDirections[i].size() == 2) + { + btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]); + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + else + x[i].setZero(); } } virtual void enforceConstraint(TVStack& x) { - for (int i = 0; i < x.size(); ++i) + const int dim = 3; + for (int j = 0; j < m_constrainedId.size(); ++j) { - for (int j = 0; j < m_constrainedDirections[i].size(); ++j) + int i = m_constrainedId[j]; + btAssert(m_constrainedDirections[i].size() <= dim); + if (m_constrainedDirections[i].size() <= 1) + { + for (int j = 0; j < m_constrainedDirections[i].size(); ++j) + { + x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; + x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j]; + } + } + else if (m_constrainedDirections[i].size() == 2) { - x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; - x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j]; + btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]); + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir + m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1]; } + else + x[i] = m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1] + m_constrainedDirections[i][2] * m_constrainedValues[i][2]; } } -- cgit v1.2.1 From 4e5f4b9fe98ac7631f9ca4cce8a713e23f0cc892 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 11 Jul 2019 11:26:30 -0700 Subject: reformulate how constraints are managed in the projection class --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 34 +++--- src/BulletSoftBody/btCGProjection.h | 53 +++++++-- src/BulletSoftBody/btContactProjection.cpp | 149 ++++++++++++------------ src/BulletSoftBody/btContactProjection.h | 45 ++++--- src/BulletSoftBody/btDeformableBodySolver.cpp | 26 ++--- 5 files changed, 171 insertions(+), 136 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index 69f5cd6ac..bdb781897 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -70,19 +70,27 @@ void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, void btBackwardEulerObjective::updateVelocity(const TVStack& dv) { - for (int i = 0; i < m_softBodies.size(); ++i) + // only the velocity of the constrained nodes needs to be updated during CG solve + for (auto it : projection.m_constraints) { - int 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) - { - // only the velocity of the constrained nodes needs to be updated during CG solve - if (projection.m_constrainedDirections.size() > 0) - psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter]; - ++counter; - } - } + int i = projection.m_indices[it.first]; + it.first->m_v = m_backupVelocity[i] + dv[i]; } } + +// for (int i = 0; i < m_softBodies.size(); ++i) +// { +// int 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) +// { +// // only the velocity of the constrained nodes needs to be updated during CG solve +// if (projection.m_constraints[&(psb->m_nodes[j])].size() > 0) +// psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter]; +// ++counter; +// } +// } +// } + diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index ff1d6cbdd..a238e998b 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -11,6 +11,34 @@ #include class btDeformableRigidDynamicsWorld; + +struct Constraint +{ + const btSoftBody::RContact* m_contact; + const btVector3 m_direction; + btScalar m_value; + + Constraint(const btSoftBody::RContact& rcontact) + : m_contact(&rcontact) + , m_direction(rcontact.m_cti.m_normal) + , m_value(0) + { + } + + Constraint(const btVector3 dir) + : m_contact(nullptr) + , m_direction(dir) + , m_value(0) + {} + + Constraint() + : m_contact(nullptr) + { + + } + +}; + class btCGProjection { public: @@ -21,11 +49,11 @@ public: btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; std::unordered_map m_indices; - TVArrayStack m_constrainedDirections; - TArrayStack m_constrainedValues; - btAlignedObjectArray m_constrainedId; - +// TVArrayStack m_constrainedDirections; +// TArrayStack m_constrainedValues; +// btAlignedObjectArray m_constrainedId; const btScalar& m_dt; + std::unordered_map > m_constraints; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) @@ -51,14 +79,15 @@ public: updateId(); // resize and clear the old constraints - m_constrainedValues.resize(m_indices.size()); - m_constrainedDirections.resize(m_indices.size()); - for (int i = 0; i < m_constrainedDirections.size(); ++i) - { - m_constrainedDirections[i].clear(); - m_constrainedValues[i].clear(); - } - m_constrainedId.clear(); +// m_constrainedValues.resize(m_indices.size()); +// m_constrainedDirections.resize(m_indices.size()); +// for (int i = 0; i < m_constrainedDirections.size(); ++i) +// { +// m_constrainedDirections[i].clear(); +// m_constrainedValues[i].clear(); +// } +// m_constrainedId.clear(); + m_constraints.clear(); } void updateId() diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index c45cccf5d..3016a9e85 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -12,20 +12,21 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit ///solve rigid body constraints m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); - // loop through contacts to create contact constraints - for (int i = 0; i < m_softBodies.size(); ++i) + // loop through constraints to set constrained values + for (auto it : m_constraints) { - btSoftBody* psb = m_softBodies[i]; - btMultiBodyJacobianData jacobianData; - for (int i = 0, ni = psb->m_rcontacts.size(); i < ni; ++i) + btAlignedObjectArray& constraints = it.second; + for (int i = 0; i < constraints.size(); ++i) { - const btSoftBody::RContact& c = psb->m_rcontacts[i]; - - // skip anchor points - if (c.m_node->m_im == 0) + Constraint& constraint = constraints[i]; + if (constraint.m_contact == nullptr) + { + // nothing needs to be done for dirichelet constraints continue; - - const btSoftBody::sCti& cti = c.m_cti; + } + const btSoftBody::RContact* c = constraint.m_contact; + const btSoftBody::sCti& cti = c->m_cti; + btMultiBodyJacobianData jacobianData; if (cti.m_colObj->hasContactResponse()) { btVector3 va(0, 0, 0); @@ -37,7 +38,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -49,7 +50,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit 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); + 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); @@ -62,26 +63,26 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit } } - const btVector3 vb = c.m_node->m_v * m_dt; + const btVector3 vb = c->m_node->m_v * m_dt; const btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); if (1) // in the same CG solve, the set of constraits doesn't change -// if (dn <= SIMD_EPSILON) + // if (dn <= SIMD_EPSILON) { // 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 *(cti.m_normal * dn); + const btVector3 impulse = c->m_c0 *(cti.m_normal * dn); // TODO: only contact is considered here, add friction later // 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]]; + 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); - m_constrainedValues[m_indices[c.m_node]][0]=(dvn); + constraint.m_value = dvn; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) - rigidCol->applyImpulse(impulse, c.m_c1); + rigidCol->applyImpulse(impulse, c->m_c1); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -97,24 +98,23 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit } } + void btContactProjection::setConstraintDirections() { // set Dirichlet constraint size_t counter = 0; for (int i = 0; i < m_softBodies.size(); ++i) { - const btSoftBody* psb = m_softBodies[i]; + btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { if (psb->m_nodes[j].m_im == 0) { - m_constrainedDirections[counter].push_back(btVector3(1,0,0)); - m_constrainedDirections[counter].push_back(btVector3(0,1,0)); - m_constrainedDirections[counter].push_back(btVector3(0,0,1)); - m_constrainedValues[counter].push_back(0); - m_constrainedValues[counter].push_back(0); - m_constrainedValues[counter].push_back(0); - m_constrainedId.push_back(counter); + btAlignedObjectArray c; + c.push_back(Constraint(btVector3(1,0,0))); + c.push_back(Constraint(btVector3(0,1,0))); + c.push_back(Constraint(btVector3(0,0,1))); + m_constraints[&(psb->m_nodes[j])] = c; } ++counter; } @@ -125,14 +125,12 @@ void btContactProjection::setConstraintDirections() btSoftBody* psb = m_softBodies[i]; btMultiBodyJacobianData jacobianData; - int j = 0; - while (j < psb->m_rcontacts.size()) + for (int j = 0; j < psb->m_rcontacts.size(); ++j) { const btSoftBody::RContact& c = psb->m_rcontacts[j]; // skip anchor points if (c.m_node->m_im == 0) { - psb->m_rcontacts.removeAtIndex(j); continue; } @@ -178,68 +176,69 @@ void btContactProjection::setConstraintDirections() const btScalar dn = btDot(vr, cti.m_normal); if (dn < SIMD_EPSILON) { - ++j; - m_constrainedDirections[m_indices[c.m_node]].push_back(cti.m_normal); - m_constrainedValues[m_indices[c.m_node]].resize(m_constrainedValues[m_indices[c.m_node]].size()+1); - m_constrainedId.push_back(m_indices[c.m_node]); + if (m_constraints.find(c.m_node) == m_constraints.end()) + { + btAlignedObjectArray constraints; + constraints.push_back(Constraint(c)); + m_constraints[c.m_node] = constraints; + } + else + { + m_constraints[c.m_node].push_back(Constraint(c)); + } continue; } } - psb->m_rcontacts.removeAtIndex(j); } } // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions counter = 0; const int dim = 3; - for (int i = 0; i < m_softBodies.size(); ++i) + for (auto it : m_constraints) { - const btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) + const btAlignedObjectArray& c = it.second; + if (c.size() > dim) { - if (m_constrainedDirections[counter].size() > dim) + btAlignedObjectArray prunedConstraints; + // always keep the first constrained direction + prunedConstraints.push_back(c[0]); + + // find the direction most orthogonal to the first direction and keep it + size_t selected = 1; + btScalar min_dotProductAbs = std::abs(prunedConstraints[0].m_direction.dot(c[selected].m_direction)); + for (int j = 2; j < c.size(); ++j) { - btAlignedObjectArray prunedConstraints; - // always keep the first constrained direction - prunedConstraints.push_back(m_constrainedDirections[counter][0]); - // find the direction most orthogonal to the first direction and keep it - size_t selected = 1; - btScalar min_dotProductAbs = std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][selected])); - for (int j = 2; j < m_constrainedDirections[counter].size(); ++j) + btScalar dotProductAbs =std::abs(prunedConstraints[0].m_direction.dot(c[j].m_direction)); + if (dotProductAbs < min_dotProductAbs) { - btScalar dotProductAbs =std::abs(prunedConstraints[0].dot(m_constrainedDirections[counter][j])); - if (dotProductAbs < min_dotProductAbs) - { - selected = j; - min_dotProductAbs = dotProductAbs; - } + selected = j; + min_dotProductAbs = dotProductAbs; } - if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON) - { - m_constrainedDirections[counter++] = prunedConstraints; - continue; - } - prunedConstraints.push_back(m_constrainedDirections[counter][selected]); - - // find the direction most orthogonal to the previous two directions and keep it - size_t selected2 = (selected == 1) ? 2 : 1; - btVector3 normal = btCross(prunedConstraints[0], prunedConstraints[1]); - normal.normalize(); - btScalar max_dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][selected2])); - for (int j = 3; j < m_constrainedDirections[counter].size(); ++j) + } + if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON) + { + it.second = prunedConstraints; + continue; + } + prunedConstraints.push_back(c[selected]); + + // find the direction most orthogonal to the previous two directions and keep it + size_t selected2 = (selected == 1) ? 2 : 1; + btVector3 normal = btCross(prunedConstraints[0].m_direction, prunedConstraints[1].m_direction); + normal.normalize(); + btScalar max_dotProductAbs = std::abs(normal.dot(c[selected2].m_direction)); + for (int j = 3; j < c.size(); ++j) + { + btScalar dotProductAbs = std::abs(normal.dot(c[j].m_direction)); + if (dotProductAbs > min_dotProductAbs) { - btScalar dotProductAbs = std::abs(normal.dot(m_constrainedDirections[counter][j])); - if (dotProductAbs > min_dotProductAbs) - { - selected2 = j; - max_dotProductAbs = dotProductAbs; - } + selected2 = j; + max_dotProductAbs = dotProductAbs; } - prunedConstraints.push_back(m_constrainedDirections[counter][selected2]); - m_constrainedDirections[counter] = prunedConstraints; - m_constrainedValues[counter].resize(dim); } - ++counter; + prunedConstraints.push_back(c[selected2]); + it.second = prunedConstraints; } } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index c0897e9bf..a497c933c 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -30,20 +30,19 @@ public: virtual void operator()(TVStack& x) { const int dim = 3; - for (int j = 0; j < m_constrainedId.size(); ++j) + for (auto it : m_constraints) { - int i = m_constrainedId[j]; - btAssert(m_constrainedDirections[i].size() <= dim); - if (m_constrainedDirections[i].size() <= 1) + const btAlignedObjectArray& constraints = it.second; + size_t i = m_indices[it.first]; + btAssert(constraints.size() <= dim); + btAssert(constraints.size() > 0); + if (constraints.size() == 1) { - for (int j = 0; j < m_constrainedDirections[i].size(); ++j) - { - x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; - } + x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; } - else if (m_constrainedDirections[i].size() == 2) + else if (constraints.size() == 2) { - btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]); + btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); free_dir.normalize(); x[i] = x[i].dot(free_dir) * free_dir; } @@ -51,30 +50,30 @@ public: x[i].setZero(); } } + virtual void enforceConstraint(TVStack& x) { const int dim = 3; - for (int j = 0; j < m_constrainedId.size(); ++j) + for (auto it : m_constraints) { - int i = m_constrainedId[j]; - btAssert(m_constrainedDirections[i].size() <= dim); - if (m_constrainedDirections[i].size() <= 1) + const btAlignedObjectArray& constraints = it.second; + size_t i = m_indices[it.first]; + btAssert(constraints.size() <= dim); + btAssert(constraints.size() > 0); + if (constraints.size() == 1) { - for (int j = 0; j < m_constrainedDirections[i].size(); ++j) - { - x[i] -= x[i].dot(m_constrainedDirections[i][j]) * m_constrainedDirections[i][j]; - x[i] += m_constrainedValues[i][j] * m_constrainedDirections[i][j]; - } + x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; + x[i] += constraints[0].m_value * constraints[0].m_direction; } - else if (m_constrainedDirections[i].size() == 2) + else if (constraints.size() == 2) { - btVector3 free_dir = btCross(m_constrainedDirections[i][0], m_constrainedDirections[i][1]); + btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir + m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1]; + x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value; } else - x[i] = m_constrainedDirections[i][0] * m_constrainedValues[i][0] + m_constrainedDirections[i][1] * m_constrainedValues[i][1] + m_constrainedDirections[i][2] * m_constrainedValues[i][2]; + x[i] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction; } } diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 9f94e9f6b..b457a262f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -89,20 +89,20 @@ void btDeformableBodySolver::postStabilize() c.m_node->m_x -= dp * cti.m_normal * c.m_c4; //// - // if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - // { - // if (rigidCol) - // rigidCol->applyImpulse(impulse, c.m_c1); - // } +// 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); +// } +// } } - // else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - // { - // if (multibodyLinkCol) - // { - // double multiplier = 0.5; - // multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); - // } - // } } } } -- cgit v1.2.1 From 696c96f392f41ee41045d345082032d6bfcebcd8 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 12 Jul 2019 10:03:38 -0700 Subject: bug fix in projection; start friction --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btCGProjection.h | 29 +++++++------ src/BulletSoftBody/btContactProjection.cpp | 56 ++++++++++++++++++++----- src/BulletSoftBody/btContactProjection.h | 14 ++++++- src/BulletSoftBody/btDeformableBodySolver.cpp | 1 + src/BulletSoftBody/btSoftBodyInternals.h | 3 +- 6 files changed, 75 insertions(+), 30 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index bdb781897..8801e68df 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -8,7 +8,7 @@ #include "btBackwardEulerObjective.h" btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) -: cg(50) +: cg(20) , m_softBodies(softBodies) , precondition(DefaultPreconditioner()) , projection(m_softBodies, m_dt) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index a238e998b..6c104ef2f 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -15,7 +15,7 @@ class btDeformableRigidDynamicsWorld; struct Constraint { const btSoftBody::RContact* m_contact; - const btVector3 m_direction; + btVector3 m_direction; btScalar m_value; Constraint(const btSoftBody::RContact& rcontact) @@ -36,7 +36,17 @@ struct Constraint { } - +}; + +struct Friction +{ + btVector3 m_dv; + btVector3 m_direction; + Friction() + { + m_dv.setZero(); + m_direction.setZero(); + } }; class btCGProjection @@ -49,11 +59,9 @@ public: btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; std::unordered_map m_indices; -// TVArrayStack m_constrainedDirections; -// TArrayStack m_constrainedValues; -// btAlignedObjectArray m_constrainedId; const btScalar& m_dt; std::unordered_map > m_constraints; + std::unordered_map m_frictions; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) @@ -77,17 +85,8 @@ public: { if (nodeUpdated) updateId(); - - // resize and clear the old constraints -// m_constrainedValues.resize(m_indices.size()); -// m_constrainedDirections.resize(m_indices.size()); -// for (int i = 0; i < m_constrainedDirections.size(); ++i) -// { -// m_constrainedDirections[i].clear(); -// m_constrainedValues[i].clear(); -// } -// m_constrainedId.clear(); m_constraints.clear(); + m_frictions.clear(); } void updateId() diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 3016a9e85..b90c749f3 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -7,14 +7,16 @@ #include "btContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" +#include void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) { ///solve rigid body constraints m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values - for (auto it : m_constraints) + for (auto& it : m_constraints) { + Friction& friction = m_frictions[it.first]; btAlignedObjectArray& constraints = it.second; for (int i = 0; i < constraints.size(); ++i) { @@ -66,11 +68,27 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit const btVector3 vb = c->m_node->m_v * m_dt; const btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); - if (1) // in the same CG solve, the set of constraits doesn't change - // if (dn <= SIMD_EPSILON) + btVector3 impulse = c->m_c0 * vr; + const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; + + if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON) + { + btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3, impulse_tangent.norm()); + + impulse_tangent_magnitude = 0; + + const btVector3 tangent_dir = impulse_tangent.normalized(); + impulse_tangent = impulse_tangent_magnitude * tangent_dir; + friction.m_direction = impulse_tangent; + friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]).dot(tangent_dir)*tangent_dir; + } + impulse = impulse_normal + impulse_tangent; +// 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 - const btVector3 impulse = c->m_c0 *(cti.m_normal * dn); + // TODO: only contact is considered here, add friction later // dv = new_impulse + accumulated velocity change in previous CG iterations @@ -82,7 +100,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) - rigidCol->applyImpulse(impulse, c->m_c1); + rigidCol->applyImpulse(impulse_normal, c->m_c1); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -102,7 +120,6 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit void btContactProjection::setConstraintDirections() { // set Dirichlet constraint - size_t counter = 0; for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; @@ -116,7 +133,6 @@ void btContactProjection::setConstraintDirections() c.push_back(Constraint(btVector3(0,0,1))); m_constraints[&(psb->m_nodes[j])] = c; } - ++counter; } } @@ -181,6 +197,7 @@ void btContactProjection::setConstraintDirections() btAlignedObjectArray constraints; constraints.push_back(Constraint(c)); m_constraints[c.m_node] = constraints; + m_frictions[c.m_node] = Friction(); } else { @@ -193,11 +210,10 @@ void btContactProjection::setConstraintDirections() } // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions - counter = 0; const int dim = 3; - for (auto it : m_constraints) + for (auto& it : m_constraints) { - const btAlignedObjectArray& c = it.second; + btAlignedObjectArray& c = it.second; if (c.size() > dim) { btAlignedObjectArray prunedConstraints; @@ -216,7 +232,7 @@ void btContactProjection::setConstraintDirections() min_dotProductAbs = dotProductAbs; } } - if (std::abs(min_dotProductAbs-1) < SIMD_EPSILON) + if (std::abs(std::abs(min_dotProductAbs)-1) < SIMD_EPSILON) { it.second = prunedConstraints; continue; @@ -240,5 +256,23 @@ void btContactProjection::setConstraintDirections() prunedConstraints.push_back(c[selected2]); it.second = prunedConstraints; } + else + { + // prune out collinear constraints + const btVector3& first_dir = c[0].m_direction; + int i = 1; + while (i < c.size()) + { + if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON) + c.removeAtIndex(i); + else + ++i; + } + if (c.size() == 3) + { + if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON) + c.removeAtIndex(2); + } + } } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index a497c933c..29d0e6632 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -30,18 +30,22 @@ public: virtual void operator()(TVStack& x) { const int dim = 3; - for (auto it : m_constraints) + for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; + const Friction& friction = 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) * constraints[0].m_direction; + if (friction.m_direction.norm() > SIMD_EPSILON) + x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; } else if (constraints.size() == 2) { + // TODO : friction btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); free_dir.normalize(); x[i] = x[i].dot(free_dir) * free_dir; @@ -55,16 +59,22 @@ public: virtual void enforceConstraint(TVStack& x) { const int dim = 3; - for (auto it : m_constraints) + for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; + const Friction& friction = 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) * constraints[0].m_direction; x[i] += constraints[0].m_value * constraints[0].m_direction; + if (friction.m_direction.norm() > SIMD_EPSILON) + { + x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; + x[i] += friction.m_dv; + } } else if (constraints.size() == 2) { diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index b457a262f..b7c531385 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -122,6 +122,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) updateVelocity(); } advect(solverdt); +// postStabilize(); } void btDeformableBodySolver::reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 41911b2bc..12d96171f 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -891,7 +891,8 @@ struct btSoftColliders c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); c.m_c1 = ra; c.m_c2 = ima * psb->m_sst.sdt; - c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; +// c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; psb->m_rcontacts.push_back(c); if (m_rigidBody) -- cgit v1.2.1 From 98cd9a85e4416686e099969332691c91b60a3468 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 12 Jul 2019 10:35:50 -0700 Subject: generalize preconditioner, now supports mass preconditioning --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 10 ++-- src/BulletSoftBody/btBackwardEulerObjective.h | 78 +++++++++++++++++++++---- 2 files changed, 73 insertions(+), 15 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index 8801e68df..12c7a7add 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -8,14 +8,15 @@ #include "btBackwardEulerObjective.h" btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) -: cg(20) +: cg(50) , m_softBodies(softBodies) -, precondition(DefaultPreconditioner()) , projection(m_softBodies, m_dt) , m_backupVelocity(backup_v) { // TODO: this should really be specified in initialization instead of here btMassSpring* mass_spring = new btMassSpring(m_softBodies); +// m_preconditioner = new MassPreconditioner(m_softBodies); + m_preconditioner = new DefaultPreconditioner(); m_lf.push_back(mass_spring); } @@ -28,8 +29,9 @@ void btBackwardEulerObjective::reinitialize(bool nodeUpdated) for (int i = 0; i < m_lf.size(); ++i) { m_lf[i]->reinitialize(nodeUpdated); - projection.reinitialize(nodeUpdated); } + projection.reinitialize(nodeUpdated); + m_preconditioner->reinitialize(nodeUpdated); } @@ -64,7 +66,7 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt) { m_dt = dt; - btScalar tolerance = std::numeric_limits::epsilon()* 16 * computeNorm(residual); + btScalar tolerance = std::numeric_limits::epsilon()* 1024 * computeNorm(residual); cg.solve(*this, dv, residual, tolerance); } diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index 424291700..9b94341b7 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -15,25 +15,82 @@ #include "btDeformableRigidDynamicsWorld.h" class btDeformableRigidDynamicsWorld; -class btBackwardEulerObjective + +class Preconditioner { public: using TVStack = btAlignedObjectArray; - struct DefaultPreconditioner + 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 m_inv_mass; + const btAlignedObjectArray& m_softBodies; +public: + MassPreconditioner(const btAlignedObjectArray& softBodies) + : m_softBodies(softBodies) { - void operator()(const TVStack& x, TVStack& b) + } + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) { - btAssert(b.size() == x.size()); - for (int i = 0; i < b.size(); ++i) - b[i] = x[i]; + 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: + using TVStack = btAlignedObjectArray; +// 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; btDeformableRigidDynamicsWorld* m_world; btAlignedObjectArray m_lf; btAlignedObjectArray& m_softBodies; - std::function precondition; + Preconditioner* m_preconditioner; btContactProjection projection; const TVStack& m_backupVelocity; @@ -92,10 +149,9 @@ public: projection(r); } - template - void setPreconditioner(Func preconditioner_func) + void precondition(const TVStack& x, TVStack& b) { - precondition = preconditioner_func; + m_preconditioner->operator()(x,b); } virtual void setWorld(btDeformableRigidDynamicsWorld* world) -- cgit v1.2.1 From ac628f4d394f39d10cd178e005b63ad7dda0e1f0 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 12 Jul 2019 13:26:58 -0700 Subject: add two way coupled penetration resolution; not momentum conserving, but seem to work fine --- src/BulletSoftBody/btDeformableBodySolver.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index b7c531385..41a5df70d 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -87,21 +87,22 @@ void btDeformableBodySolver::postStabilize() if (dp < 0) { c.m_node->m_x -= dp * cti.m_normal * c.m_c4; +// c.m_node->m_x -= impulse * c.m_c2; //// -// 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); -// } -// } + 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); + } + } } } } -- cgit v1.2.1 From bac7d461c5a9d318a04b9741cccf7f1759132bf3 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 15 Jul 2019 10:48:20 -0700 Subject: fix bugs in poststablize and projection of colinear constraints --- src/BulletSoftBody/btContactProjection.cpp | 31 +++++++------- src/BulletSoftBody/btContactProjection.h | 25 ++++++++--- src/BulletSoftBody/btDeformableBodySolver.cpp | 37 +++++++++-------- src/BulletSoftBody/btDeformableBodySolver.h | 5 +-- src/BulletSoftBody/btSoftBodyInternals.h | 60 ++++++++++++++------------- 5 files changed, 89 insertions(+), 69 deletions(-) diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index b90c749f3..6082f4dad 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -11,6 +11,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) { ///solve rigid body constraints + m_world->getSolverInfo().m_numIterations = 5; m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values @@ -258,21 +259,21 @@ void btContactProjection::setConstraintDirections() } else { - // prune out collinear constraints - const btVector3& first_dir = c[0].m_direction; - int i = 1; - while (i < c.size()) - { - if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON) - c.removeAtIndex(i); - else - ++i; - } - if (c.size() == 3) - { - if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON) - c.removeAtIndex(2); - } +// // prune out collinear constraints +// const btVector3& first_dir = c[0].m_direction; +// int i = 1; +// while (i < c.size()) +// { +// if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON) +// c.removeAtIndex(i); +// else +// ++i; +// } +// if (c.size() == 3) +// { +// if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON) +// c.removeAtIndex(2); +// } } } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 29d0e6632..8ce3cb578 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -47,8 +47,13 @@ public: { // TODO : friction btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; + if (free_dir.norm() < SIMD_EPSILON) + x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } } else x[i].setZero(); @@ -69,7 +74,8 @@ public: if (constraints.size() == 1) { x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; - x[i] += constraints[0].m_value * constraints[0].m_direction; + btVector3 diff = constraints[0].m_value * constraints[0].m_direction; + x[i] += diff; if (friction.m_direction.norm() > SIMD_EPSILON) { x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; @@ -79,8 +85,17 @@ public: else if (constraints.size() == 2) { btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value; + if (free_dir.norm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; + btVector3 diff = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction; + x[i] += diff; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value; + } } else x[i] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 41a5df70d..e9bcf226a 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -77,8 +77,8 @@ void btDeformableBodySolver::postStabilize() const btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); - const btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); - + btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); +// dp += mrg; // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient btScalar dvn = dn * c.m_c4; @@ -86,23 +86,27 @@ void btDeformableBodySolver::postStabilize() // TODO: only contact is considered here, add friction later if (dp < 0) { - c.m_node->m_x -= dp * cti.m_normal * c.m_c4; -// c.m_node->m_x -= impulse * c.m_c2; - - //// - 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) + bool two_way = false; + if (two_way) { - if (multibodyLinkCol) + c.m_node->m_x -= impulse * c.m_c2; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - double multiplier = 0.5; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + 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); + } } } + else + c.m_node->m_x -= dp * cti.m_normal * c.m_c4; } } } @@ -115,7 +119,6 @@ void btDeformableBodySolver::solveConstraints(float solverdt) bool nodeUpdated = updateNodes(); reinitialize(nodeUpdated); backupVelocity(); - postStabilize(); for (int i = 0; i < m_solveIterations; ++i) { m_objective->computeResidual(solverdt, m_residual); @@ -123,7 +126,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) updateVelocity(); } advect(solverdt); -// postStabilize(); + postStabilize(); } void btDeformableBodySolver::reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 540a2b6cb..3fac0743d 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -2,7 +2,7 @@ // btDeformableBodySolver.h // BulletSoftBody // -// Created by Chuyuan Fu on 7/1/19. +// Created by Xuchen Han on 7/1/19. // #ifndef BT_DEFORMABLE_BODY_SOLVERS_H @@ -101,8 +101,7 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { auto& node = psb->m_nodes[j]; - node.m_x += dt * m_dv[counter++]; -// node.m_x = node.m_q + dt * node.m_v; + node.m_x = node.m_q + dt * node.m_v; } } } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 12d96171f..b892498ea 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -869,35 +869,37 @@ struct btSoftColliders const btScalar m = n.m_im > 0 ? dynmargin : stamargin; btSoftBody::RContact c; - if ((!n.m_battach) && - psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) - { - const btScalar ima = n.m_im; - const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; - const btScalar ms = ima + imb; - if (ms > 0) - { - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_q - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); - c.m_c1 = ra; - c.m_c2 = ima * psb->m_sst.sdt; -// c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; - c.m_c3 = fc; - c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); - } + if (!n.m_battach) + { + if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + // c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c3 = fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } } } btSoftBody* psb; -- cgit v1.2.1 From befab4eab6ac8ef8aea141d8e89a463648e9de41 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 15 Jul 2019 14:52:24 -0700 Subject: reorganize the contact constraints --- src/BulletSoftBody/btCGProjection.h | 32 ++-- src/BulletSoftBody/btContactProjection.cpp | 240 ++++++++++++----------------- src/BulletSoftBody/btContactProjection.h | 56 ++++--- 3 files changed, 148 insertions(+), 180 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 6c104ef2f..91621e49b 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -14,37 +14,41 @@ class btDeformableRigidDynamicsWorld; struct Constraint { - const btSoftBody::RContact* m_contact; - btVector3 m_direction; - btScalar m_value; + btAlignedObjectArray m_contact; + btAlignedObjectArray m_direction; + btAlignedObjectArray m_value; Constraint(const btSoftBody::RContact& rcontact) - : m_contact(&rcontact) - , m_direction(rcontact.m_cti.m_normal) - , m_value(0) { + m_contact.push_back(&rcontact); + m_direction.push_back(rcontact.m_cti.m_normal); + m_value.push_back(0); } Constraint(const btVector3 dir) - : m_contact(nullptr) - , m_direction(dir) - , m_value(0) - {} + { + m_contact.push_back(nullptr); + m_direction.push_back(dir); + m_value.push_back(0); + } Constraint() - : m_contact(nullptr) { - } }; struct Friction { - btVector3 m_dv; + bool m_static; + btScalar m_value; btVector3 m_direction; + + bool m_static_prev; + btScalar m_value_prev; + btVector3 m_direction_prev; Friction() { - m_dv.setZero(); + m_direction_prev.setZero(); m_direction.setZero(); } }; diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 6082f4dad..9388ac7e4 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -22,93 +22,97 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit for (int i = 0; i < constraints.size(); ++i) { Constraint& constraint = constraints[i]; - if (constraint.m_contact == nullptr) + for (int j = 0; j < constraint.m_contact.size(); ++j) { - // nothing needs to be done for dirichelet constraints - continue; - } - const btSoftBody::RContact* c = constraint.m_contact; - const btSoftBody::sCti& cti = c->m_cti; - btMultiBodyJacobianData jacobianData; - if (cti.m_colObj->hasContactResponse()) - { - btVector3 va(0, 0, 0); - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - btScalar* deltaV; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + if (constraint.m_contact[j] == nullptr) { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0); + // nothing needs to be done for dirichelet constraints + continue; } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + const btSoftBody::RContact* c = constraint.m_contact[j]; + const btSoftBody::sCti& cti = c->m_cti; + btMultiBodyJacobianData jacobianData; + if (cti.m_colObj->hasContactResponse()) { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + btScalar* deltaV; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - 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) + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + 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 * m_dt; } - va = cti.m_normal * vel * m_dt; } - } - - const btVector3 vb = c->m_node->m_v * m_dt; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); - btVector3 impulse = c->m_c0 * vr; - const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - - if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON) - { - btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3, impulse_tangent.norm()); - impulse_tangent_magnitude = 0; + const btVector3 vb = c->m_node->m_v * m_dt; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + btVector3 impulse = c->m_c0 * vr; + const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn); + btVector3 impulse_tangent = impulse - impulse_normal; - const btVector3 tangent_dir = impulse_tangent.normalized(); - impulse_tangent = impulse_tangent_magnitude * tangent_dir; - friction.m_direction = impulse_tangent; - friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]).dot(tangent_dir)*tangent_dir; - } - impulse = impulse_normal + impulse_tangent; -// 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 - - // TODO: only contact is considered here, add friction later - - // 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); - constraint.m_value = dvn; - - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON) { - if (rigidCol) - rigidCol->applyImpulse(impulse_normal, c->m_c1); + btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3*1000, impulse_tangent.norm()); + +// impulse_tangent_magnitude = 0; + + const btVector3 tangent_dir = impulse_tangent.normalized(); + impulse_tangent = impulse_tangent_magnitude * tangent_dir; + friction.m_direction = impulse_tangent; + friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]); } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + impulse = impulse_normal + impulse_tangent; + + // if (1) // in the same CG solve, the set of constraits doesn't change + if (dn <= SIMD_EPSILON) { - if (multibodyLinkCol) + // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient + + // TODO: only contact is considered here, add friction later + + // 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); + constraint.m_value[j] = dvn; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse_normal, c->m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { - double multiplier = 0.5; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + if (multibodyLinkCol) + { + double multiplier = 0.5; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + } } } } @@ -202,78 +206,30 @@ void btContactProjection::setConstraintDirections() } else { - m_constraints[c.m_node].push_back(Constraint(c)); + // group colinear constraints into one + const btScalar angle_epsilon = 0.015192247; // less than 10 degree + bool merged = false; + btAlignedObjectArray& constraints = m_constraints[c.m_node]; + for (int j = 0; j < constraints.size(); ++j) + { + const btAlignedObjectArray& dirs = constraints[j].m_direction; + 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); + merged = true; + break; + } + } + const int dim = 3; + // hard coded no more than 3 constraint directions + if (!merged && constraints.size() < dim) + constraints.push_back(Constraint(c)); } - continue; } } } } - - // for particles with more than three constrained directions, prune constrained directions so that there are at most three constrained directions - const int dim = 3; - for (auto& it : m_constraints) - { - btAlignedObjectArray& c = it.second; - if (c.size() > dim) - { - btAlignedObjectArray prunedConstraints; - // always keep the first constrained direction - prunedConstraints.push_back(c[0]); - - // find the direction most orthogonal to the first direction and keep it - size_t selected = 1; - btScalar min_dotProductAbs = std::abs(prunedConstraints[0].m_direction.dot(c[selected].m_direction)); - for (int j = 2; j < c.size(); ++j) - { - btScalar dotProductAbs =std::abs(prunedConstraints[0].m_direction.dot(c[j].m_direction)); - if (dotProductAbs < min_dotProductAbs) - { - selected = j; - min_dotProductAbs = dotProductAbs; - } - } - if (std::abs(std::abs(min_dotProductAbs)-1) < SIMD_EPSILON) - { - it.second = prunedConstraints; - continue; - } - prunedConstraints.push_back(c[selected]); - - // find the direction most orthogonal to the previous two directions and keep it - size_t selected2 = (selected == 1) ? 2 : 1; - btVector3 normal = btCross(prunedConstraints[0].m_direction, prunedConstraints[1].m_direction); - normal.normalize(); - btScalar max_dotProductAbs = std::abs(normal.dot(c[selected2].m_direction)); - for (int j = 3; j < c.size(); ++j) - { - btScalar dotProductAbs = std::abs(normal.dot(c[j].m_direction)); - if (dotProductAbs > min_dotProductAbs) - { - selected2 = j; - max_dotProductAbs = dotProductAbs; - } - } - prunedConstraints.push_back(c[selected2]); - it.second = prunedConstraints; - } - else - { -// // prune out collinear constraints -// const btVector3& first_dir = c[0].m_direction; -// int i = 1; -// while (i < c.size()) -// { -// if (std::abs(std::abs(first_dir.dot(c[i].m_direction)) - 1) < 4*SIMD_EPSILON) -// c.removeAtIndex(i); -// else -// ++i; -// } -// if (c.size() == 3) -// { -// if (std::abs(std::abs(c[1].m_direction.dot(c[2].m_direction)) - 1) < 4*SIMD_EPSILON) -// c.removeAtIndex(2); -// } - } - } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 8ce3cb578..1e5c52a60 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -39,21 +39,20 @@ public: btAssert(constraints.size() > 0); if (constraints.size() == 1) { - x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; + x[i] -= x[i].dot(constraints[0].m_direction[0]) * constraints[0].m_direction[0]; if (friction.m_direction.norm() > SIMD_EPSILON) - x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; + { + btVector3 dir = friction.m_direction.normalized(); + x[i] -= x[i].dot(dir) * dir; + } } else if (constraints.size() == 2) { // TODO : friction - btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); - if (free_dir.norm() < SIMD_EPSILON) - x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; - else - { - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir; - } + 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(); @@ -73,32 +72,41 @@ public: btAssert(constraints.size() > 0); if (constraints.size() == 1) { - x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; - btVector3 diff = constraints[0].m_value * constraints[0].m_direction; - x[i] += diff; + 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]; if (friction.m_direction.norm() > SIMD_EPSILON) { - x[i] -= x[i].dot(friction.m_direction) * friction.m_direction; + btVector3 dir = friction.m_direction.normalized(); + x[i] -= x[i].dot(dir) * dir; x[i] += friction.m_dv; } } else if (constraints.size() == 2) { - btVector3 free_dir = btCross(constraints[0].m_direction, constraints[1].m_direction); - if (free_dir.norm() < SIMD_EPSILON) + 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) { - x[i] -= x[i].dot(constraints[0].m_direction) * constraints[0].m_direction; - btVector3 diff = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction; - x[i] += diff; + for (int k = 0; k < constraints[j].m_direction.size(); ++k) + { + x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; + } } - else + } + else + { + x[i].setZero(); + for (int j = 0; j < constraints.size(); ++j) { - free_dir.normalize(); - x[i] = x[i].dot(free_dir) * free_dir + constraints[0].m_direction * constraints[0].m_value + constraints[1].m_direction * constraints[1].m_value; + 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] = constraints[0].m_value * constraints[0].m_direction + constraints[1].m_value * constraints[1].m_direction + constraints[2].m_value * constraints[2].m_direction; } } -- cgit v1.2.1 From 2fc376e8f58fbf983b79c73da8ea2fd86cc9a170 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 16 Jul 2019 15:28:33 -0700 Subject: bug fix in friction; accumulate friction impulses in cg; forbid switching from static to dynamic friction --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 18 +----- src/BulletSoftBody/btCGProjection.h | 28 ++++++--- src/BulletSoftBody/btConjugateGradient.h | 2 - src/BulletSoftBody/btContactProjection.cpp | 83 ++++++++++++++++++++----- src/BulletSoftBody/btContactProjection.h | 43 ++++++++++--- 5 files changed, 121 insertions(+), 53 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index 12c7a7add..e8c4c3edc 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -8,7 +8,7 @@ #include "btBackwardEulerObjective.h" btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) -: cg(50) +: cg(20) , m_softBodies(softBodies) , projection(m_softBodies, m_dt) , m_backupVelocity(backup_v) @@ -79,20 +79,4 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv) it.first->m_v = m_backupVelocity[i] + dv[i]; } } - -// for (int i = 0; i < m_softBodies.size(); ++i) -// { -// int 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) -// { -// // only the velocity of the constrained nodes needs to be updated during CG solve -// if (projection.m_constraints[&(psb->m_nodes[j])].size() > 0) -// psb->m_nodes[j].m_v = m_backupVelocity[counter] + dv[counter]; -// ++counter; -// } -// } -// } diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 91621e49b..b0f798dad 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -39,17 +39,27 @@ struct Constraint struct Friction { - bool m_static; - btScalar m_value; - btVector3 m_direction; + btAlignedObjectArray m_static; + btAlignedObjectArray m_value; + btAlignedObjectArray m_direction; - bool m_static_prev; - btScalar m_value_prev; - btVector3 m_direction_prev; + btAlignedObjectArray m_static_prev; + btAlignedObjectArray m_value_prev; + btAlignedObjectArray m_direction_prev; + + btAlignedObjectArray m_accumulated_impulse; Friction() { - m_direction_prev.setZero(); - m_direction.setZero(); + m_static.push_back(false); + m_static_prev.push_back(false); + + m_direction.push_back(btVector3(0,0,0)); + m_direction_prev.push_back(btVector3(0,0,0)); + + m_value.push_back(0); + m_value_prev.push_back(0); + + m_accumulated_impulse.push_back(btVector3(0,0,0)); } }; @@ -65,7 +75,7 @@ public: std::unordered_map m_indices; const btScalar& m_dt; std::unordered_map > m_constraints; - std::unordered_map m_frictions; + std::unordered_map > m_frictions; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 0a88e6b94..a2b69f48d 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -63,11 +63,9 @@ public: // r -= alpha * temp; multAndAddTo(alpha, p, x); multAndAddTo(-alpha, temp, r); - // zero out the dofs of r A.project(r,x); A.enforceConstraint(x); - r_norm = std::sqrt(squaredNorm(r)); if (r_norm < tolerance) { diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 9388ac7e4..e356da264 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -17,11 +17,12 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit // loop through constraints to set constrained values for (auto& it : m_constraints) { - Friction& friction = m_frictions[it.first]; + btAlignedObjectArray& frictions = m_frictions[it.first]; btAlignedObjectArray& constraints = it.second; for (int i = 0; i < constraints.size(); ++i) { Constraint& constraint = constraints[i]; + Friction& friction = frictions[i]; for (int j = 0; j < constraint.m_contact.size(); ++j) { if (constraint.m_contact[j] == nullptr) @@ -75,36 +76,64 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn); btVector3 impulse_tangent = impulse - impulse_normal; - if (dn < 0 && impulse_tangent.norm() > SIMD_EPSILON) + + // accumulated impulse on the rb in this and all prev cg iterations + friction.m_accumulated_impulse[j] += impulse; + btScalar accumulated_normal = friction.m_accumulated_impulse[j].dot(cti.m_normal); + btVector3 accumulated_tangent = friction.m_accumulated_impulse[j] - accumulated_normal * cti.m_normal; + + // start friction handling + // copy old data + friction.m_direction_prev[j] = friction.m_direction[j]; + friction.m_value_prev[j] = friction.m_value[j]; + friction.m_static_prev[j] = friction.m_static[j]; + if (accumulated_normal < 0 && accumulated_tangent.norm() > SIMD_EPSILON) { - btScalar impulse_tangent_magnitude = std::min(impulse_normal.norm()*c->m_c3*1000, impulse_tangent.norm()); - -// impulse_tangent_magnitude = 0; + // do not allow switching from static friction to dynamic friction + // it causes cg to explode + if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false) + { + friction.m_static[j] = false; + friction.m_value[j] = -accumulated_normal*c->m_c3; + } + else + { + friction.m_static[j] = true; + friction.m_value[j] = accumulated_tangent.norm(); + } - const btVector3 tangent_dir = impulse_tangent.normalized(); - impulse_tangent = impulse_tangent_magnitude * tangent_dir; - friction.m_direction = impulse_tangent; - friction.m_dv = -impulse_tangent * c->m_c2/m_dt + (c->m_node->m_v - backupVelocity[m_indices[c->m_node]]); + const btVector3 tangent_dir = accumulated_tangent.normalized(); + impulse_tangent = friction.m_value[j] * tangent_dir; + friction.m_direction[j] = -tangent_dir; + } + else + { + friction.m_static[j] = false; + friction.m_value[j] = 0; + impulse_tangent.setZero(); } - impulse = impulse_normal + impulse_tangent; - // if (1) // in the same CG solve, the set of constraits doesn't change - if (dn <= SIMD_EPSILON) + + 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 - // TODO: only contact is considered here, add friction later - // 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); 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_value_prev[j] * friction.m_direction_prev[j]) - (friction.m_value[j] * friction.m_direction[j]); + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) - rigidCol->applyImpulse(impulse_normal, c->m_c1); + rigidCol->applyImpulse(impulse, c->m_c1); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -137,6 +166,13 @@ void btContactProjection::setConstraintDirections() c.push_back(Constraint(btVector3(0,1,0))); c.push_back(Constraint(btVector3(0,0,1))); m_constraints[&(psb->m_nodes[j])] = c; + + btAlignedObjectArray f; + f.push_back(Friction()); + f.push_back(Friction()); + f.push_back(Friction()); + m_frictions[&(psb->m_nodes[j])] = f; + // no friction constraints for dirichlet } } } @@ -202,7 +238,9 @@ void btContactProjection::setConstraintDirections() btAlignedObjectArray constraints; constraints.push_back(Constraint(c)); m_constraints[c.m_node] = constraints; - m_frictions[c.m_node] = Friction(); + btAlignedObjectArray frictions; + frictions.push_back(Friction()); + m_frictions[c.m_node] = frictions; } else { @@ -210,6 +248,7 @@ void btContactProjection::setConstraintDirections() const btScalar angle_epsilon = 0.015192247; // less than 10 degree bool merged = false; btAlignedObjectArray& constraints = m_constraints[c.m_node]; + btAlignedObjectArray& frictions = m_frictions[c.m_node]; for (int j = 0; j < constraints.size(); ++j) { const btAlignedObjectArray& dirs = constraints[j].m_direction; @@ -219,6 +258,15 @@ void btContactProjection::setConstraintDirections() constraints[j].m_contact.push_back(&c); constraints[j].m_direction.push_back(cti.m_normal); constraints[j].m_value.push_back(0); + + // 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_value.push_back(0); + frictions[j].m_value_prev.push_back(0); + frictions[j].m_static.push_back(false); + frictions[j].m_static_prev.push_back(false); + frictions[j].m_accumulated_impulse.push_back(btVector3(0,0,0)); merged = true; break; } @@ -226,7 +274,10 @@ void btContactProjection::setConstraintDirections() const int dim = 3; // hard coded no more than 3 constraint directions if (!merged && constraints.size() < dim) + { constraints.push_back(Constraint(c)); + frictions.push_back(Friction()); + } } } } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 1e5c52a60..b52bb2d72 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -34,16 +34,31 @@ public: { const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; - const Friction& friction = m_frictions[it.first]; + btAlignedObjectArray& 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]; - if (friction.m_direction.norm() > SIMD_EPSILON) + 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) { - btVector3 dir = friction.m_direction.normalized(); - x[i] -= x[i].dot(dir) * dir; + // clear the old friction force + if (friction.m_static_prev[j] == false) + { + x[i] -= friction.m_direction_prev[j] * friction.m_value_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_value[j]; + } } } else if (constraints.size() == 2) @@ -67,7 +82,7 @@ public: { const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; - const Friction& friction = m_frictions[it.first]; + const btAlignedObjectArray& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -75,15 +90,25 @@ public: 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]; - if (friction.m_direction.norm() > SIMD_EPSILON) + + const Friction& friction= frictions[0]; + for (int j = 0; j < friction.m_direction.size(); ++j) { - btVector3 dir = friction.m_direction.normalized(); - x[i] -= x[i].dot(dir) * dir; - x[i] += friction.m_dv; + // clear the old constraint + if (friction.m_static_prev[j] == true) + { + x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j]; + } + // add the new constraint + if (friction.m_static[j] == true) + { + x[i] += friction.m_direction[j] * friction.m_value[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(); -- cgit v1.2.1 From 7846dd38dd5172d3b2876a95fa4a515755195668 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 17 Jul 2019 15:58:55 -0700 Subject: switch explicit elastic force --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 16 +++++-- src/BulletSoftBody/btBackwardEulerObjective.h | 36 +++++++++----- src/BulletSoftBody/btCGProjection.h | 16 +++++-- src/BulletSoftBody/btContactProjection.cpp | 34 ++++++++----- src/BulletSoftBody/btContactProjection.h | 53 +++++++++++++++++++-- src/BulletSoftBody/btDeformableBodySolver.cpp | 11 +++-- src/BulletSoftBody/btDeformableBodySolver.h | 14 ------ .../btDeformableRigidDynamicsWorld.cpp | 7 ++- .../btDeformableRigidDynamicsWorld.h | 9 ++-- src/BulletSoftBody/btLagrangianForce.h | 4 +- src/BulletSoftBody/btMassSpring.h | 55 +++++++++++++++++----- src/BulletSoftBody/btSoftBody.cpp | 12 ++++- src/BulletSoftBody/btSoftBody.h | 3 +- 13 files changed, 193 insertions(+), 77 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index e8c4c3edc..3f27df5a9 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -57,9 +57,6 @@ void btBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const { // add damping matrix m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); - - // add stiffness matrix when fully implicity - m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); } } @@ -80,3 +77,16 @@ void btBackwardEulerObjective::updateVelocity(const TVStack& dv) } } +void btBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) +{ + 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) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter] * 1; + ++counter; + } + } +} diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index 9b94341b7..e9642f6ae 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -76,15 +76,6 @@ class btBackwardEulerObjective { public: using TVStack = btAlignedObjectArray; -// 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; btDeformableRigidDynamicsWorld* m_world; @@ -102,11 +93,29 @@ public: void computeResidual(btScalar dt, TVStack& residual) const { - // gravity is treated explicitly in predictUnconstraintMotion - // add force + // add implicit force for (int i = 0; i < m_lf.size(); ++i) { - m_lf[i]->addScaledForce(dt, residual); + m_lf[i]->addScaledImplicitForce(dt, residual); + } + } + + 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++; + } } } @@ -117,7 +126,7 @@ public: { norm_squared += residual[i].length2(); } - return std::sqrt(norm_squared); + return std::sqrt(norm_squared+SIMD_EPSILON); } void computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt); @@ -128,6 +137,7 @@ public: { projection.update(dv, m_backupVelocity); } + void initialGuess(TVStack& dv, const TVStack& residual); void reinitialize(bool nodeUpdated); diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index b0f798dad..a00fe384b 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -40,13 +40,17 @@ struct Constraint struct Friction { btAlignedObjectArray m_static; - btAlignedObjectArray m_value; + btAlignedObjectArray m_impulse; + btAlignedObjectArray m_dv; btAlignedObjectArray m_direction; btAlignedObjectArray m_static_prev; - btAlignedObjectArray m_value_prev; + btAlignedObjectArray m_impulse_prev; + btAlignedObjectArray m_dv_prev; btAlignedObjectArray m_direction_prev; + btAlignedObjectArray m_released; + btAlignedObjectArray m_accumulated_impulse; Friction() { @@ -56,10 +60,14 @@ struct Friction m_direction.push_back(btVector3(0,0,0)); m_direction_prev.push_back(btVector3(0,0,0)); - m_value.push_back(0); - m_value_prev.push_back(0); + m_impulse.push_back(0); + m_impulse_prev.push_back(0); + + m_dv.push_back(0); + m_dv_prev.push_back(0); m_accumulated_impulse.push_back(btVector3(0,0,0)); + m_released.push_back(false); } }; diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index e356da264..3250e010b 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -85,35 +85,41 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit // start friction handling // copy old data friction.m_direction_prev[j] = friction.m_direction[j]; - friction.m_value_prev[j] = friction.m_value[j]; + 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 && accumulated_tangent.norm() > SIMD_EPSILON) { + + btScalar compare1 = -accumulated_normal*c->m_c3; + btScalar compare2 = accumulated_tangent.norm(); // do not allow switching from static friction to dynamic friction // it causes cg to explode - if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false) + if (-accumulated_normal*c->m_c3 < accumulated_tangent.norm() && friction.m_static_prev[j] == false && friction.m_released[j] == false) { friction.m_static[j] = false; - friction.m_value[j] = -accumulated_normal*c->m_c3; + friction.m_impulse[j] = -accumulated_normal*c->m_c3; + friction.m_dv[j] = -accumulated_normal*c->m_c3 *c->m_c2/m_dt ; } else { friction.m_static[j] = true; - friction.m_value[j] = accumulated_tangent.norm(); + friction.m_impulse[j] = accumulated_tangent.norm(); + friction.m_dv[j] = accumulated_tangent.norm() * c->m_c2/m_dt; } const btVector3 tangent_dir = accumulated_tangent.normalized(); - impulse_tangent = friction.m_value[j] * tangent_dir; friction.m_direction[j] = -tangent_dir; } else { + friction.m_released[j] = true; friction.m_static[j] = false; - friction.m_value[j] = 0; - impulse_tangent.setZero(); + friction.m_impulse[j] = 0; + friction.m_dv[j] = 0; } - + friction.m_accumulated_impulse[j] = accumulated_normal * cti.m_normal - 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) { @@ -128,8 +134,8 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit // 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_value_prev[j] * friction.m_direction_prev[j]) - (friction.m_value[j] * friction.m_direction[j]); - + impulse = impulse_normal + (friction.m_impulse_prev[j] * friction.m_direction_prev[j]) - (friction.m_impulse[j] * friction.m_direction[j]); +// impulse = impulse_normal; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { if (rigidCol) @@ -172,7 +178,6 @@ void btContactProjection::setConstraintDirections() f.push_back(Friction()); f.push_back(Friction()); m_frictions[&(psb->m_nodes[j])] = f; - // no friction constraints for dirichlet } } } @@ -262,11 +267,14 @@ void btContactProjection::setConstraintDirections() // 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_value.push_back(0); - frictions[j].m_value_prev.push_back(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_impulse.push_back(btVector3(0,0,0)); + frictions[j].m_released.push_back(false); merged = true; break; } diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index b52bb2d72..969745d93 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -51,13 +51,13 @@ public: // clear the old friction force if (friction.m_static_prev[j] == false) { - x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j]; + 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_value[j]; + x[i] += friction.m_direction[j] * friction.m_impulse[j]; } } } @@ -68,6 +68,33 @@ public: 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(); @@ -97,12 +124,12 @@ public: // clear the old constraint if (friction.m_static_prev[j] == true) { - x[i] -= friction.m_direction_prev[j] * friction.m_value_prev[j]; + 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_value[j]; + x[i] += friction.m_direction[j] * friction.m_dv[j]; } } } @@ -120,6 +147,24 @@ public: 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 { diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index e9bcf226a..a8e1e5cd5 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -118,7 +118,15 @@ void btDeformableBodySolver::solveConstraints(float solverdt) m_dt = solverdt; bool nodeUpdated = updateNodes(); reinitialize(nodeUpdated); + + // apply explicit force + m_objective->applyExplicitForce(m_residual); + + // remove contact constraints with separating velocity + setConstraintDirections(); + backupVelocity(); + for (int i = 0; i < m_solveIterations; ++i) { m_objective->computeResidual(solverdt, m_residual); @@ -143,9 +151,6 @@ void btDeformableBodySolver::reinitialize(bool nodeUpdated) m_residual[i].setZero(); } m_objective->reinitialize(nodeUpdated); - - // remove contact constraints with separating velocity - setConstraintDirections(); } void btDeformableBodySolver::setConstraintDirections() diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 3fac0743d..17f088c72 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -74,20 +74,6 @@ public: void postStabilize(); - void moveTempVelocity(btScalar dt, const TVStack& f) - { - 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_v += node.m_im * dt * f[counter++]; - } - } - } - void reinitialize(bool nodeUpdated); void setConstraintDirections(); diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 5fdf01a18..173f71206 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -12,6 +12,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { + m_internalTime += timeStep; // Let the solver grab the soft bodies and if necessary optimize for it m_deformableBodySolver->optimize(m_softBodies); @@ -54,7 +55,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep // when there are multiple substeps - clearForces(); clearMultiBodyForces(); btMultiBodyDynamicsWorld::applyGravity(); @@ -69,9 +69,14 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS clearForces(); clearMultiBodyForces(); + + for (int i = 0; i < before_solver_callbacks.size(); ++i) + before_solver_callbacks[i](m_internalTime, this); ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); + + //integrate transforms btMultiBodyDynamicsWorld::integrateTransforms(timeStep); diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index ca1fff885..5bbebeda3 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -21,7 +21,7 @@ #include "btMassSpring.h" #include "btDeformableBodySolver.h" #include "btSoftBodyHelpers.h" - +#include typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; @@ -40,6 +40,7 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld bool m_drawClusterTree; btSoftBodyWorldInfo m_sbi; bool m_ownsSolver; + btScalar m_internalTime; protected: virtual void internalSingleStepSimulation(btScalar timeStep); @@ -67,8 +68,9 @@ public: m_sbi.m_gravity.setValue(0, -10, 0); m_sbi.m_sparsesdf.Initialize(); + m_internalTime = 0.0; } - + btAlignedObjectArray > before_solver_callbacks; virtual ~btDeformableRigidDynamicsWorld() { } @@ -89,9 +91,6 @@ public: } virtual void predictUnconstraintMotion(btScalar timeStep); - // virtual void internalStepSingleStepSimulation(btScalar timeStep); - - // virtual void solveDeformableBodiesConstraints(btScalar timeStep); virtual void addSoftBody(btSoftBody* body, int collisionFilterGroup = btBroadphaseProxy::DefaultFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter); diff --git a/src/BulletSoftBody/btLagrangianForce.h b/src/BulletSoftBody/btLagrangianForce.h index 40e63207e..b95a30ce9 100644 --- a/src/BulletSoftBody/btLagrangianForce.h +++ b/src/BulletSoftBody/btLagrangianForce.h @@ -25,12 +25,14 @@ public: virtual ~btLagrangianForce(){} - virtual void addScaledForce(btScalar scale, TVStack& force) = 0; + virtual void addScaledImplicitForce(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 addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + virtual void reinitialize(bool nodeUpdated) { if (nodeUpdated) diff --git a/src/BulletSoftBody/btMassSpring.h b/src/BulletSoftBody/btMassSpring.h index 9fd3c405a..78cdcf7e6 100644 --- a/src/BulletSoftBody/btMassSpring.h +++ b/src/BulletSoftBody/btMassSpring.h @@ -19,7 +19,42 @@ public: } - virtual void addScaledForce(btScalar scale, TVStack& force) + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(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]; + size_t id1 = m_indices[node1]; + size_t id2 = m_indices[node2]; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btScalar k_damp = psb->m_dampingCoefficient; + btVector3 scaled_force = scale * v_diff * k_damp; + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); btAssert(numNodes == force.size()) @@ -31,7 +66,7 @@ public: 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 kLST = link.Feature::m_material->m_kLST; btScalar r = link.m_rl; size_t id1 = m_indices[node1]; size_t id2 = m_indices[node2]; @@ -39,21 +74,14 @@ public: // elastic force // fully implicit - btVector3 dir = (node2->m_x - node1->m_x); +// btVector3 dir = (node2->m_x - node1->m_x); // explicit elastic force -// btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir = (node2->m_q - node1->m_q); 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; } } } @@ -64,7 +92,7 @@ public: btAssert(numNodes == dx.size()); btAssert(numNodes == df.size()); - // implicit elastic force + // implicit elastic force differential for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; @@ -83,9 +111,10 @@ public: } } + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { - // implicity damping force + // implicit damping force differential for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 066072426..974246448 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1782,7 +1782,7 @@ void btSoftBody::predictMotion(btScalar dt) m_sst.radmrg = getCollisionShape()->getMargin(); m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; /* Forces */ - addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); applyForces(); /* Integrate */ for (i = 0, ni = m_nodes.size(); i < ni; ++i) @@ -1805,7 +1805,7 @@ void btSoftBody::predictMotion(btScalar dt) } } } - n.m_v += deltaV; + n.m_v += deltaV; n.m_x += n.m_v * m_sst.sdt; n.m_f = btVector3(0, 0, 0); } @@ -2775,6 +2775,14 @@ void btSoftBody::dampClusters() } } +void btSoftBody::setSpringStiffness(btScalar k) +{ + for (int i = 0; i < m_links.size(); ++i) + { + m_links[i].Feature::m_material->m_kLST = k; + } +} + // void btSoftBody::Joint::Prepare(btScalar dt, int) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 01cde89e3..e8fd3da39 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1011,6 +1011,7 @@ public: void solveClusters(btScalar sor); void applyClusters(bool drift); void dampClusters(); + void setSpringStiffness(btScalar k); void applyForces(); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); @@ -1021,7 +1022,7 @@ public: static vsolver_t getSolver(eVSolver::_ solver); virtual int calculateSerializeBufferSize() const; - + ///fills the dataBuffer and returns the struct name (and 0 on failure) virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; -- cgit v1.2.1 From 3430192db798c773036d247a460da520d0c26762 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 18 Jul 2019 09:16:01 -0700 Subject: reformulate friction --- src/BulletSoftBody/btBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btCGProjection.h | 10 +++-- src/BulletSoftBody/btContactProjection.cpp | 51 +++++++++++++------------ src/BulletSoftBody/btContactProjection.h | 4 +- 4 files changed, 36 insertions(+), 31 deletions(-) diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp index 3f27df5a9..9174107ba 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btBackwardEulerObjective.cpp @@ -8,7 +8,7 @@ #include "btBackwardEulerObjective.h" btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) -: cg(20) +: cg(10) , m_softBodies(softBodies) , projection(m_softBodies, m_dt) , m_backupVelocity(backup_v) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index a00fe384b..46979c8f0 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -43,22 +43,23 @@ struct Friction btAlignedObjectArray m_impulse; btAlignedObjectArray m_dv; btAlignedObjectArray m_direction; + btAlignedObjectArray m_direction_prev; btAlignedObjectArray m_static_prev; btAlignedObjectArray m_impulse_prev; btAlignedObjectArray m_dv_prev; - btAlignedObjectArray m_direction_prev; btAlignedObjectArray m_released; - btAlignedObjectArray m_accumulated_impulse; + btAlignedObjectArray m_accumulated_normal_impulse; + btAlignedObjectArray m_accumulated_tangent_impulse; Friction() { m_static.push_back(false); m_static_prev.push_back(false); - m_direction.push_back(btVector3(0,0,0)); m_direction_prev.push_back(btVector3(0,0,0)); + m_direction.push_back(btVector3(0,0,0)); m_impulse.push_back(0); m_impulse_prev.push_back(0); @@ -66,7 +67,8 @@ struct Friction m_dv.push_back(0); m_dv_prev.push_back(0); - m_accumulated_impulse.push_back(btVector3(0,0,0)); + m_accumulated_normal_impulse.push_back(0); + m_accumulated_tangent_impulse.push_back(btVector3(0,0,0)); m_released.push_back(false); } }; diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 3250e010b..041792d08 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -11,7 +11,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) { ///solve rigid body constraints - m_world->getSolverInfo().m_numIterations = 5; + m_world->getSolverInfo().m_numIterations = 10; m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values @@ -73,43 +73,44 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit const btVector3 vr = vb - va; const btScalar dn = btDot(vr, cti.m_normal); btVector3 impulse = c->m_c0 * vr; - const btVector3 impulse_normal = c->m_c0 *(cti.m_normal * dn); - btVector3 impulse_tangent = impulse - impulse_normal; - + const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn); + const btVector3 impulse_tangent = impulse - impulse_normal; + 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_impulse[j] += impulse; - btScalar accumulated_normal = friction.m_accumulated_impulse[j].dot(cti.m_normal); - btVector3 accumulated_tangent = friction.m_accumulated_impulse[j] - accumulated_normal * cti.m_normal; - + friction.m_accumulated_normal_impulse[j] += impulse_normal.dot(cti.m_normal); + btScalar accumulated_normal = friction.m_accumulated_normal_impulse[j]; + btVector3 tangent = friction.m_accumulated_tangent_impulse[j] + impulse_tangent; + btScalar tangent_norm = tangent.norm(); // start friction handling // copy old data - friction.m_direction_prev[j] = friction.m_direction[j]; 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 && accumulated_tangent.norm() > SIMD_EPSILON) + if (accumulated_normal < 0 && tangent_norm > SIMD_EPSILON) { - + friction.m_direction[j] = -local_tangent_dir; btScalar compare1 = -accumulated_normal*c->m_c3; - btScalar compare2 = accumulated_tangent.norm(); + 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 < accumulated_tangent.norm() && friction.m_static_prev[j] == false && friction.m_released[j] == false) + 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] = -accumulated_normal*c->m_c3 *c->m_c2/m_dt ; + 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] = accumulated_tangent.norm(); - friction.m_dv[j] = accumulated_tangent.norm() * c->m_c2/m_dt; + friction.m_impulse[j] = 0; + friction.m_dv[j] = local_tangent_norm * c->m_c2/m_dt; + impulse = impulse_normal + impulse_tangent; } - - const btVector3 tangent_dir = accumulated_tangent.normalized(); - friction.m_direction[j] = -tangent_dir; } else { @@ -117,9 +118,10 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit 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_accumulated_impulse[j] = accumulated_normal * cti.m_normal - friction.m_impulse[j] * friction.m_direction[j]; + friction.m_accumulated_tangent_impulse[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) { @@ -134,7 +136,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit // 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 + (friction.m_impulse_prev[j] * friction.m_direction_prev[j])-(friction.m_impulse[j] * friction.m_direction[j]); // impulse = impulse_normal; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -145,7 +147,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit { if (multibodyLinkCol) { - double multiplier = 0.5; + double multiplier = 1; multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); } } @@ -273,7 +275,8 @@ void btContactProjection::setConstraintDirections() 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_impulse.push_back(btVector3(0,0,0)); + 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); merged = true; break; diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h index 969745d93..566fd9f92 100644 --- a/src/BulletSoftBody/btContactProjection.h +++ b/src/BulletSoftBody/btContactProjection.h @@ -124,7 +124,7 @@ public: // clear the old constraint if (friction.m_static_prev[j] == true) { - x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; +// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; } // add the new constraint if (friction.m_static[j] == true) @@ -156,7 +156,7 @@ public: // clear the old constraint if (friction.m_static_prev[j] == true) { - x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; +// x[i] -= friction.m_direction_prev[j] * friction.m_dv_prev[j]; } // add the new constraint if (friction.m_static[j] == true) -- cgit v1.2.1 From dc10336d45eeaf5d06de4bdabd7df3bc4d990d6f Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 18 Jul 2019 11:34:06 -0700 Subject: code clean up + check in examples --- examples/BasicDemo/BasicExample.cpp | 6 +- examples/DeformableContact/DeformableContact.cpp | 412 +++++++++++++++++++++ examples/DeformableContact/DeformableContact.h | 20 + examples/DeformableDemo/DeformableDemo.cpp | 90 +++-- examples/ExampleBrowser/ExampleEntries.cpp | 8 +- examples/Pinch/Pinch.cpp | 297 +++++++++++++++ examples/Pinch/Pinch.h | 20 + .../VolumetricDeformable/VolumetricDeformable.cpp | 294 +++++++++++++++ .../VolumetricDeformable/VolumetricDeformable.h | 20 + src/BulletSoftBody/btBackwardEulerObjective.cpp | 47 ++- src/BulletSoftBody/btBackwardEulerObjective.h | 118 ++---- src/BulletSoftBody/btCGProjection.h | 49 ++- src/BulletSoftBody/btConjugateGradient.h | 8 - src/BulletSoftBody/btContactProjection.cpp | 201 +++++++--- src/BulletSoftBody/btContactProjection.h | 160 +------- src/BulletSoftBody/btDeformableBodySolver.cpp | 78 +++- src/BulletSoftBody/btDeformableBodySolver.h | 69 +--- src/BulletSoftBody/btPreconditioner.h | 67 ++++ 18 files changed, 1552 insertions(+), 412 deletions(-) create mode 100644 examples/DeformableContact/DeformableContact.cpp create mode 100644 examples/DeformableContact/DeformableContact.h create mode 100644 examples/Pinch/Pinch.cpp create mode 100644 examples/Pinch/Pinch.h create mode 100644 examples/VolumetricDeformable/VolumetricDeformable.cpp create mode 100644 examples/VolumetricDeformable/VolumetricDeformable.h create mode 100644 src/BulletSoftBody/btPreconditioner.h 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 //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 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 world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray 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 //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 //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& 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 //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; - 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 m_inv_mass; - const btAlignedObjectArray& m_softBodies; -public: - MassPreconditioner(const btAlignedObjectArray& 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 m_contact; btAlignedObjectArray m_direction; btAlignedObjectArray m_value; + // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve + btAlignedObjectArray 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,30 +30,56 @@ 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 m_static; - btAlignedObjectArray m_impulse; - btAlignedObjectArray m_dv; - btAlignedObjectArray m_direction; - btAlignedObjectArray m_direction_prev; + + btAlignedObjectArray m_static; // whether the friction is static + btAlignedObjectArray m_impulse; // the impulse magnitude the node feels + btAlignedObjectArray m_dv; // the dv magnitude of the node + btAlignedObjectArray m_direction; // the direction of the friction for the node + btAlignedObjectArray m_static_prev; btAlignedObjectArray m_impulse_prev; btAlignedObjectArray m_dv_prev; + btAlignedObjectArray m_direction_prev; - btAlignedObjectArray m_released; + btAlignedObjectArray m_released; // whether the contact is released - btAlignedObjectArray m_accumulated_normal_impulse; + + + // the total impulse the node applied to the rb in the tangential direction in the cg solve btAlignedObjectArray 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; 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) { } @@ -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 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& constraints = it.second; + size_t i = m_indices[it.first]; + const btAlignedObjectArray& 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& constraints = it.second; + size_t i = m_indices[it.first]; + btAlignedObjectArray& 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& 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& constraints = it.second; - size_t i = m_indices[it.first]; - btAlignedObjectArray& 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& constraints = it.second; - size_t i = m_indices[it.first]; - const btAlignedObjectArray& 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 &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; + 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 m_inv_mass; + const btAlignedObjectArray& m_softBodies; +public: + MassPreconditioner(const btAlignedObjectArray& 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 */ -- cgit v1.2.1 From a90cad2a9650ad57d597eaf72cc8b0e091e51ad5 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sun, 21 Jul 2019 18:32:54 -0700 Subject: deformable code refactor --- .../Dynamics/btDiscreteDynamicsWorld.h | 10 + src/BulletSoftBody/btBackwardEulerObjective.cpp | 137 ------ src/BulletSoftBody/btBackwardEulerObjective.h | 28 +- src/BulletSoftBody/btCGProjection.h | 51 ++- src/BulletSoftBody/btConjugateGradient.h | 4 +- src/BulletSoftBody/btContactProjection.cpp | 156 +++++-- src/BulletSoftBody/btContactProjection.h | 38 -- .../btDeformableBackwardEulerObjective.cpp | 134 ++++++ .../btDeformableBackwardEulerObjective.h | 100 +++++ src/BulletSoftBody/btDeformableBodySolver.cpp | 190 +++----- src/BulletSoftBody/btDeformableBodySolver.h | 35 +- .../btDeformableContactProjection.cpp | 488 +++++++++++++++++++++ src/BulletSoftBody/btDeformableContactProjection.h | 43 ++ src/BulletSoftBody/btDeformableGravityForce.h | 60 +++ src/BulletSoftBody/btDeformableLagrangianForce.h | 63 +++ src/BulletSoftBody/btDeformableMassSpringForce.h | 107 +++++ .../btDeformableRigidDynamicsWorld.cpp | 148 ++++--- .../btDeformableRigidDynamicsWorld.h | 20 +- src/BulletSoftBody/btLagrangianForce.h | 55 --- src/BulletSoftBody/btMassSpring.h | 147 ------- src/BulletSoftBody/btSoftBody.cpp | 13 +- 21 files changed, 1355 insertions(+), 672 deletions(-) delete mode 100644 src/BulletSoftBody/btBackwardEulerObjective.cpp delete mode 100644 src/BulletSoftBody/btContactProjection.h create mode 100644 src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp create mode 100644 src/BulletSoftBody/btDeformableBackwardEulerObjective.h create mode 100644 src/BulletSoftBody/btDeformableContactProjection.cpp create mode 100644 src/BulletSoftBody/btDeformableContactProjection.h create mode 100644 src/BulletSoftBody/btDeformableGravityForce.h create mode 100644 src/BulletSoftBody/btDeformableLagrangianForce.h create mode 100644 src/BulletSoftBody/btDeformableMassSpringForce.h delete mode 100644 src/BulletSoftBody/btLagrangianForce.h delete mode 100644 src/BulletSoftBody/btMassSpring.h diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 51b3d906d..73607c61f 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -229,6 +229,16 @@ public: { return m_latencyMotionStateInterpolation; } + + btAlignedObjectArray& getNonStaticRigidBodies() + { + return m_nonStaticRigidBodies; + } + + const btAlignedObjectArray& getNonStaticRigidBodies() const + { + return m_nonStaticRigidBodies; + } }; #endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btBackwardEulerObjective.cpp b/src/BulletSoftBody/btBackwardEulerObjective.cpp deleted file mode 100644 index 024cc66b6..000000000 --- a/src/BulletSoftBody/btBackwardEulerObjective.cpp +++ /dev/null @@ -1,137 +0,0 @@ -// -// btBackwardEulerObjective.cpp -// BulletSoftBody -// -// Created by Xuchen Han on 7/9/19. -// - -#include "btBackwardEulerObjective.h" - -btBackwardEulerObjective::btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) -: cg(10) -, m_softBodies(softBodies) -, projection(m_softBodies, m_dt) -, m_backupVelocity(backup_v) -{ - // TODO: this should really be specified in initialization instead of here - btMassSpring* mass_spring = new btMassSpring(m_softBodies); -// m_preconditioner = new MassPreconditioner(m_softBodies); - m_preconditioner = new DefaultPreconditioner(); - m_lf.push_back(mass_spring); -} - -void btBackwardEulerObjective::reinitialize(bool nodeUpdated) -{ - if(nodeUpdated) - { - projection.setSoftBodies(m_softBodies); - } - for (int i = 0; i < m_lf.size(); ++i) - { - m_lf[i]->reinitialize(nodeUpdated); - } - projection.reinitialize(nodeUpdated); - m_preconditioner->reinitialize(nodeUpdated); -} - - -void btBackwardEulerObjective::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) - { - // add damping matrix - m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); - } -} - -void btBackwardEulerObjective::computeStep(TVStack& dv, const TVStack& residual, const btScalar& dt) -{ - m_dt = dt; - btScalar tolerance = std::numeric_limits::epsilon()* 1024 * computeNorm(residual); - cg.solve(*this, dv, residual, tolerance); -} - -void btBackwardEulerObjective::updateVelocity(const TVStack& dv) -{ - // only the velocity of the constrained nodes needs to be updated during CG solve - for (auto it : projection.m_constraints) - { - int i = projection.m_indices[it.first]; - it.first->m_v = m_backupVelocity[i] + dv[i]; - } -} - -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; - for (int i = 0; i < m_softBodies.size(); ++i) - { - 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]; - ++counter; - } - } -} diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h index 68f7f66fe..16ec0a542 100644 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ b/src/BulletSoftBody/btBackwardEulerObjective.h @@ -1,5 +1,5 @@ // -// btBackwardEulerObjective.h +// btDeformableBackwardEulerObjective.h // BulletSoftBody // // Created by Xuchen Han on 7/1/19. @@ -11,27 +11,26 @@ #include "btConjugateGradient.h" #include "btLagrangianForce.h" #include "btMassSpring.h" -#include "btContactProjection.h" +#include "btDeformableContactProjection.h" #include "btPreconditioner.h" #include "btDeformableRigidDynamicsWorld.h" class btDeformableRigidDynamicsWorld; -class btBackwardEulerObjective +class btDeformableBackwardEulerObjective { public: using TVStack = btAlignedObjectArray; btScalar m_dt; - btConjugateGradient cg; btDeformableRigidDynamicsWorld* m_world; btAlignedObjectArray m_lf; btAlignedObjectArray& m_softBodies; Preconditioner* m_preconditioner; - btContactProjection projection; + btDeformableContactProjection projection; const TVStack& m_backupVelocity; - btBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); + btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); - virtual ~btBackwardEulerObjective() {} + virtual ~btDeformableBackwardEulerObjective() {} void initialize(){} @@ -53,18 +52,14 @@ public: // 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); + void setDt(btScalar dt); + // enforce constraints in CG solve void enforceConstraint(TVStack& x) { @@ -82,10 +77,11 @@ public: } // update the projections and project the residual - void project(TVStack& r, const TVStack& dv) + void project(TVStack& r) { - updateProjection(dv); - projection(r); + projection.update(); + // TODO rename + projection.project(r); } // perform precondition M^(-1) x = b diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index ff5eecd8c..09055c7d1 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -8,53 +8,61 @@ #define BT_CG_PROJECTION_H #include "btSoftBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include class btDeformableRigidDynamicsWorld; -struct Constraint +struct DeformableContactConstraint { btAlignedObjectArray m_contact; btAlignedObjectArray m_direction; btAlignedObjectArray m_value; // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve btAlignedObjectArray m_accumulated_normal_impulse; + btAlignedObjectArray m_normal_jacobian; - Constraint(const btSoftBody::RContact& rcontact) + DeformableContactConstraint(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian) { - append(rcontact); + append(rcontact, jacobian); } - Constraint(const btVector3 dir) + DeformableContactConstraint(const btVector3 dir) { m_contact.push_back(nullptr); m_direction.push_back(dir); m_value.push_back(0); m_accumulated_normal_impulse.push_back(0); + btMultiBodyJacobianData j; + m_normal_jacobian.push_back(j); } - Constraint() + DeformableContactConstraint() { 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); + btMultiBodyJacobianData j; + m_normal_jacobian.push_back(j); } - void append(const btSoftBody::RContact& rcontact) + void append(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian) { 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); + m_normal_jacobian.push_back(jacobian); } - ~Constraint() + ~DeformableContactConstraint() { } }; -struct Friction +struct DeformableFrictionConstraint { btAlignedObjectArray m_static; // whether the friction is static @@ -69,14 +77,22 @@ struct Friction btAlignedObjectArray m_direction_prev; btAlignedObjectArray m_released; // whether the contact is released - + btAlignedObjectArray m_complementary_jacobian; + btAlignedObjectArray m_complementaryDirection; // the total impulse the node applied to the rb in the tangential direction in the cg solve btAlignedObjectArray m_accumulated_tangent_impulse; - Friction() + + DeformableFrictionConstraint() + { + append(); + } + + DeformableFrictionConstraint(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian) { append(); + addJacobian(complementaryDir, jacobian); } void append() @@ -96,6 +112,13 @@ struct Friction m_accumulated_tangent_impulse.push_back(btVector3(0,0,0)); m_released.push_back(false); } + + void addJacobian(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian) + { + m_complementary_jacobian.push_back(jacobian); + m_complementaryDirection.push_back(complementaryDir); + } + }; class btCGProjection @@ -109,8 +132,6 @@ public: btDeformableRigidDynamicsWorld* m_world; std::unordered_map m_indices; const btScalar& m_dt; - std::unordered_map > m_constraints; - std::unordered_map > m_frictions; btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) @@ -123,19 +144,17 @@ public: } // apply the constraints - virtual void operator()(TVStack& x) = 0; + virtual void project(TVStack& x) = 0; virtual void setConstraints() = 0; // update the constraints - virtual void update(const TVStack& dv, const TVStack& backup_v) = 0; + virtual void update() = 0; virtual void reinitialize(bool nodeUpdated) { if (nodeUpdated) updateId(); - m_constraints.clear(); - m_frictions.clear(); } void updateId() diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index f23b26385..0c8384a00 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -36,7 +36,7 @@ public: // r = b - A * x --with assigned dof zeroed out A.multiply(x, temp); r = sub(b, temp); - A.project(r,x); + A.project(r); A.enforceConstraint(x); btScalar r_norm = std::sqrt(squaredNorm(r)); @@ -62,7 +62,7 @@ public: multAndAddTo(alpha, p, x); multAndAddTo(-alpha, temp, r); // zero out the dofs of r - A.project(r,x); + A.project(r); A.enforceConstraint(x); r_norm = std::sqrt(squaredNorm(r)); diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index cf2e05e59..99ab6f473 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -1,14 +1,47 @@ // -// btContactProjection.cpp +// btDeformableContactProjection.cpp // BulletSoftBody // // Created by Xuchen Han on 7/4/19. // -#include "btContactProjection.h" +#include "btDeformableContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" #include -void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocity) +static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + 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, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); +} + +static btVector3 generateUnitOrthogonalVector(const btVector3& u) +{ + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; +} + +void btDeformableContactProjection::update() { ///solve rigid body constraints m_world->getSolverInfo().m_numIterations = 10; @@ -32,13 +65,17 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit } const btSoftBody::RContact* c = constraint.m_contact[j]; const btSoftBody::sCti& cti = c->m_cti; - btMultiBodyJacobianData jacobianData; + + // normal jacobian is precompute but tangent jacobian is not + const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j]; + const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j]; + if (cti.m_colObj->hasContactResponse()) { btVector3 va(0, 0, 0); btRigidBody* rigidCol = 0; btMultiBodyLinkCollider* multibodyLinkCol = 0; - btScalar* deltaV; + const btScalar* deltaV_normal; // grab the velocity of the rigid body if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) @@ -52,20 +89,25 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit 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); + const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0]; + deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // add in the normal component of the va btScalar vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac[k]; + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k]; } va = cti.m_normal * vel * m_dt; + + // add in complementary direction of va + const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0]; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k]; + } + va += friction.m_complementaryDirection[j] * vel * m_dt; } } @@ -109,7 +151,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit else { friction.m_static[j] = true; - friction.m_impulse[j] = local_tangent_norm; + friction.m_impulse[j] = tangent_norm; } } else @@ -125,15 +167,22 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit // 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]); + // TODO cleanup if (1) // in the same CG solve, the set of constraits doesn't change { // 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_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; + + // the following is equivalent + /* + 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); + */ + constraint.m_value[j] = dvn; // the incremental impulse: @@ -147,10 +196,20 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { + if (multibodyLinkCol) { - double multiplier = 0.5; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV, -impulse.length() * multiplier); + double multiplier = 1; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); + + if (incremental_tangent.norm() > SIMD_EPSILON) + { + btMultiBodyJacobianData jacobian_tangent; + btVector3 tangent = incremental_tangent.normalized(); + findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); + const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); + } } } } @@ -160,7 +219,7 @@ void btContactProjection::update(const TVStack& dv, const TVStack& backupVelocit } } -void btContactProjection::setConstraints() +void btDeformableContactProjection::setConstraints() { // set Dirichlet constraint for (int i = 0; i < m_softBodies.size(); ++i) @@ -188,8 +247,8 @@ void btContactProjection::setConstraints() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; - btMultiBodyJacobianData jacobianData; - btAlignedObjectArray jacobian; + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_complementary; for (int j = 0; j < psb->m_rcontacts.size(); ++j) { @@ -206,7 +265,6 @@ void btContactProjection::setConstraints() btVector3 va(0, 0, 0); btRigidBody* rigidCol = 0; btMultiBodyLinkCollider* multibodyLinkCol = 0; - btScalar* deltaV; // grab the velocity of the rigid body if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) @@ -216,25 +274,18 @@ void btContactProjection::setConstraints() } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { - jacobian.clear(); 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); - + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal); btScalar vel = 0.0; - jacobian.resize(ndof); + const btScalar* jac = &jacobianData_normal.m_jacobians[0]; + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; for (int j = 0; j < ndof; ++j) { vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; - jacobian[j] = jac[j]; + std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl; + std::cout << jac[j] << std::endl; } va = cti.m_normal * vel * m_dt; } @@ -245,13 +296,25 @@ void btContactProjection::setConstraints() const btScalar dn = btDot(vr, cti.m_normal); if (dn < SIMD_EPSILON) { + // find complementary jacobian + btVector3 complementaryDirection; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + complementaryDirection = generateUnitOrthogonalVector(cti.m_normal); + findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection); + } + } + if (m_constraints.find(c.m_node) == m_constraints.end()) { btAlignedObjectArray constraints; - constraints.push_back(Constraint(c)); + constraints.push_back(Constraint(c, jacobianData_normal)); m_constraints[c.m_node] = constraints; btAlignedObjectArray frictions; - frictions.push_back(Friction()); + frictions.push_back(Friction(complementaryDirection, jacobianData_complementary)); m_frictions[c.m_node] = frictions; } else @@ -268,9 +331,10 @@ void btContactProjection::setConstraints() if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon) { // group the constraints - constraints[j].append(c); + constraints[j].append(c, jacobianData_normal); // push in an empty friction frictions[j].append(); + frictions[j].addJacobian(complementaryDirection, jacobianData_complementary); merged = true; break; } @@ -279,8 +343,8 @@ void btContactProjection::setConstraints() // hard coded no more than 3 constraint directions if (!merged && constraints.size() < dim) { - constraints.push_back(Constraint(c)); - frictions.push_back(Friction()); + constraints.push_back(Constraint(c, jacobianData_normal)); + frictions.push_back(Friction(complementaryDirection, jacobianData_complementary)); } } } @@ -289,7 +353,7 @@ void btContactProjection::setConstraints() } } -void btContactProjection::enforceConstraint(TVStack& x) +void btDeformableContactProjection::enforceConstraint(TVStack& x) { const int dim = 3; for (auto& it : m_constraints) @@ -356,7 +420,7 @@ void btContactProjection::enforceConstraint(TVStack& x) } } -void btContactProjection::operator()(TVStack& x) +void btDeformableContactProjection::project(TVStack& x) { const int dim = 3; for (auto& it : m_constraints) @@ -412,3 +476,13 @@ void btContactProjection::operator()(TVStack& x) } } } + +void btDeformableContactProjection::reinitialize(bool nodeUpdated) +{ + btCGProjection::reinitialize(nodeUpdated); + m_constraints.clear(); + m_frictions.clear(); +} + + + diff --git a/src/BulletSoftBody/btContactProjection.h b/src/BulletSoftBody/btContactProjection.h deleted file mode 100644 index 725cc106b..000000000 --- a/src/BulletSoftBody/btContactProjection.h +++ /dev/null @@ -1,38 +0,0 @@ -// -// btContactProjection.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/4/19. -// - -#ifndef BT_CONTACT_PROJECTION_H -#define BT_CONTACT_PROJECTION_H -#include "btCGProjection.h" -#include "btSoftBody.h" -#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" -#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -#include -class btContactProjection : public btCGProjection -{ -public: - btContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) - : btCGProjection(softBodies, dt) - { - } - - virtual ~btContactProjection() - { - } - - // apply the constraints to the rhs - virtual void operator()(TVStack& x); - - // 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 setConstraints(); -}; -#endif /* btContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp new file mode 100644 index 000000000..31b0905ac --- /dev/null +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -0,0 +1,134 @@ +// +// btDeformableBackwardEulerObjective.cpp +// BulletSoftBody +// +// Created by Xuchen Han on 7/9/19. +// + +#include "btDeformableBackwardEulerObjective.h" + +btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) +: m_softBodies(softBodies) +, projection(m_softBodies, m_dt) +, m_backupVelocity(backup_v) +{ + // TODO: this should really be specified in initialization instead of here + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies); + btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0)); + m_preconditioner = new DefaultPreconditioner(); + m_lf.push_back(mass_spring); + m_lf.push_back(gravity); +} + +void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated) +{ + if(nodeUpdated) + { + projection.setSoftBodies(m_softBodies); + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->reinitialize(nodeUpdated); + } + projection.reinitialize(nodeUpdated); + m_preconditioner->reinitialize(nodeUpdated); +} + +void btDeformableBackwardEulerObjective::setDt(btScalar dt) +{ + m_dt = dt; +} + +void btDeformableBackwardEulerObjective::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) + { + // add damping matrix + m_lf[i]->addScaledForceDifferential(-m_dt, x, b); + } +} + +void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) +{ + // only the velocity of the constrained nodes needs to be updated during CG solve + for (auto it : projection.m_constraints) + { + int i = projection.m_indices[it.first]; + it.first->m_v = m_backupVelocity[i] + dv[i]; + } +} + +void btDeformableBackwardEulerObjective::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 btDeformableBackwardEulerObjective::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 btDeformableBackwardEulerObjective::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 btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) +{ + for (int i = 0; i < m_lf.size(); ++i) + m_lf[i]->addScaledExplicitForce(m_dt, force); + applyForce(force, true); +} + +void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack& residual) +{ + 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) + { + dv[counter] = psb->m_nodes[j].m_im * residual[counter]; + ++counter; + } + } +} diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h new file mode 100644 index 000000000..bfd4ca3af --- /dev/null +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -0,0 +1,100 @@ +// +// btDeformableBackwardEulerObjective.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 "btDeformableLagrangianForce.h" +#include "btDeformableMassSpringForce.h" +#include "btDeformableGravityForce.h" +#include "btDeformableContactProjection.h" +#include "btPreconditioner.h" +#include "btDeformableRigidDynamicsWorld.h" + +class btDeformableRigidDynamicsWorld; +class btDeformableBackwardEulerObjective +{ +public: + using TVStack = btAlignedObjectArray; + btScalar m_dt; + btDeformableRigidDynamicsWorld* m_world; + btAlignedObjectArray m_lf; + btAlignedObjectArray& m_softBodies; + Preconditioner* m_preconditioner; + btDeformableContactProjection projection; + const TVStack& m_backupVelocity; + + btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); + + virtual ~btDeformableBackwardEulerObjective() {} + + void initialize(){} + + // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual + void computeResidual(btScalar dt, TVStack& residual) const; + + // add explicit force to the velocity + void applyExplicitForce(TVStack& force); + + // 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; + + // set initial guess for CG solve + void initialGuess(TVStack& dv, const TVStack& residual); + + // reset data structure + void reinitialize(bool nodeUpdated); + + void setDt(btScalar dt); + + // enforce constraints in CG solve + void enforceConstraint(TVStack& x) + { + projection.enforceConstraint(x); + updateVelocity(x); + } + + // add dv to velocity + void updateVelocity(const TVStack& dv); + + //set constraints as projections + void setConstraints() + { + projection.setConstraints(); + } + + // update the projections and project the residual + void project(TVStack& r) + { + projection.update(); + projection.project(r); + } + + // perform precondition M^(-1) x = b + void precondition(const TVStack& x, TVStack& b) + { + m_preconditioner->operator()(x,b); + } + + virtual void setWorld(btDeformableRigidDynamicsWorld* world) + { + m_world = world; + projection.setWorld(world); + } +}; + +#endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index f98dfacbb..96d2e0029 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -10,11 +10,9 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) -, m_solveIterations(1) -, m_impulseIterations(1) -, m_world(nullptr) +, cg(10) { - m_objective = new btBackwardEulerObjective(m_softBodySet, m_backupVelocity); + m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); } btDeformableBodySolver::~btDeformableBodySolver() @@ -22,124 +20,31 @@ btDeformableBodySolver::~btDeformableBodySolver() delete m_objective; } -void btDeformableBodySolver::postStabilize() -{ - for (int i = 0; i < m_softBodySet.size(); ++i) - { - btSoftBody* psb = m_softBodySet[i]; - btMultiBodyJacobianData jacobianData; - const btScalar mrg = psb->getCollisionShape()->getMargin(); - for (int j = 0; j < psb->m_rcontacts.size(); ++j) - { - const btSoftBody::RContact& c = psb->m_rcontacts[j]; - // skip anchor points - if (c.m_node->m_im == 0) - continue; - - 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; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_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 * m_dt; - } - } - - const btVector3 vb = c.m_node->m_v * m_dt; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); - - btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); -// dp += mrg; - // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient - - btScalar dvn = dn * c.m_c4; - const btVector3 impulse = c.m_c0 * ((cti.m_normal * (dn * c.m_c4))); - // TODO: only contact is considered here, add friction later - if (dp < 0) - { - bool two_way = false; - if (two_way) - { - c.m_node->m_x -= impulse * c.m_c2; - - 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); - } - } - } - else - c.m_node->m_x -= dp * cti.m_normal * c.m_c4; - } - } - } - } -} - void btDeformableBodySolver::solveConstraints(float solverdt) { - m_dt = solverdt; - bool nodeUpdated = updateNodes(); - reinitialize(nodeUpdated); - - // apply explicit force - m_objective->applyExplicitForce(m_residual); + m_objective->setDt(solverdt); // add constraints to the solver setConstraints(); + // save v_{n+1}^* velocity after explicit forces 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(); - } - advect(solverdt); - postStabilize(); + m_objective->computeResidual(solverdt, m_residual); +// m_objective->initialGuess(m_dv, m_residual); + computeStep(m_dv, m_residual); + updateVelocity(); } -void btDeformableBodySolver::reinitialize(bool nodeUpdated) +void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) { + btScalar tolerance = std::numeric_limits::epsilon()* 1024 * m_objective->computeNorm(residual); + cg.solve(*m_objective, dv, residual, tolerance); +} + +void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies) +{ + m_softBodySet.copyFromArray(softBodies); + bool nodeUpdated = updateNodes(); if (nodeUpdated) { m_dv.resize(m_numNodes); @@ -161,7 +66,6 @@ void btDeformableBodySolver::setConstraints() void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world) { - m_world = world; m_objective->setWorld(world); } @@ -180,20 +84,6 @@ void btDeformableBodySolver::updateVelocity() } } - -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 @@ -231,11 +121,53 @@ void btDeformableBodySolver::predictMotion(float solverdt) if (psb->isActive()) { - psb->predictMotion(solverdt); + // apply explicit forces to velocity + m_objective->applyExplicitForce(m_residual); + // predict motion for collision detection + predictDeformableMotion(psb, solverdt); } } } +void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar dt) +{ + int i, ni; + + /* Prepare */ + psb->m_sst.sdt = dt * psb->m_cfg.timescale; + psb->m_sst.isdt = 1 / psb->m_sst.sdt; + psb->m_sst.velmrg = psb->m_sst.sdt * 3; + psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); + psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; + /* Integrate */ + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + n.m_q = n.m_x; + n.m_x += n.m_v * dt; + } + /* Bounds */ + psb->updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg); + psb->m_ndbvt.update(n.m_leaf, + vol, + n.m_v * psb->m_sst.velmrg, + psb->m_sst.updmrg); + } + + /* Clear contacts */ + psb->m_rcontacts.resize(0); + psb->m_scontacts.resize(0); + /* Optimize dbvt's */ + psb->m_ndbvt.optimizeIncremental(1); +} + void btDeformableBodySolver::updateSoftBodies() { for (int i = 0; i < m_softBodySet.size(); i++) @@ -243,7 +175,7 @@ void btDeformableBodySolver::updateSoftBodies() btSoftBody *psb = (btSoftBody *)m_softBodySet[i]; if (psb->isActive()) { - psb->integrateMotion(); // normal is updated here + psb->updateNormals(); // normal is updated here } } } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index a1cd45077..570cce90f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -10,31 +10,27 @@ #include #include "btSoftBodySolvers.h" -#include "btBackwardEulerObjective.h" +#include "btDeformableBackwardEulerObjective.h" #include "btDeformableRigidDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" struct btCollisionObjectWrapper; -class btBackwardEulerObjective; +class btDeformableBackwardEulerObjective; class btDeformableRigidDynamicsWorld; 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; - btDeformableRigidDynamicsWorld* m_world; + btDeformableBackwardEulerObjective* m_objective; btAlignedObjectArray m_backupVelocity; btScalar m_dt; + btConjugateGradient cg; public: btDeformableBodySolver(); @@ -45,30 +41,20 @@ public: { return DEFORMABLE_SOLVER; } - - virtual bool checkInitialized() - { - return true; - } virtual void updateSoftBodies(); - - virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false) - { - m_softBodySet.copyFromArray(softBodies); - } virtual void copyBackToSoftBodies(bool bMove = true) {} - virtual void solveConstraints(float solverdt); + void extracted(float solverdt); - void postStabilize(); + virtual void solveConstraints(float solverdt); - void reinitialize(bool nodeUpdated); + void reinitialize(const btAlignedObjectArray& softBodies); void setConstraints(); - void advect(btScalar dt); + void predictDeformableMotion(btSoftBody* psb, btScalar dt); void backupVelocity(); @@ -76,6 +62,8 @@ public: bool updateNodes(); + void computeStep(TVStack& dv, const TVStack& residual); + virtual void predictMotion(float solverdt); virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} @@ -88,7 +76,8 @@ public: virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { softBody->defaultCollisionHandler(otherSoftBody); } - + virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false){} + virtual bool checkInitialized(){return true;} virtual void setWorld(btDeformableRigidDynamicsWorld* world); }; diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp new file mode 100644 index 000000000..e81cb2c50 --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -0,0 +1,488 @@ +// +// btDeformableContactProjection.cpp +// BulletSoftBody +// +// Created by Xuchen Han on 7/4/19. +// + +#include "btDeformableContactProjection.h" +#include "btDeformableRigidDynamicsWorld.h" +#include +static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + 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, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); +} + +static btVector3 generateUnitOrthogonalVector(const btVector3& u) +{ + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; +} + +void btDeformableContactProjection::update() +{ + ///solve rigid body constraints + m_world->getSolverInfo().m_numIterations = 10; + m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); + + // loop through constraints to set constrained values + for (auto& it : m_constraints) + { + btAlignedObjectArray& frictions = m_frictions[it.first]; + btAlignedObjectArray& constraints = it.second; + for (int i = 0; i < constraints.size(); ++i) + { + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; + for (int j = 0; j < constraint.m_contact.size(); ++j) + { + if (constraint.m_contact[j] == nullptr) + { + // nothing needs to be done for dirichelet constraints + continue; + } + const btSoftBody::RContact* c = constraint.m_contact[j]; + const btSoftBody::sCti& cti = c->m_cti; + + // normal jacobian is precompute but tangent jacobian is not + const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j]; + const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j]; + + if (cti.m_colObj->hasContactResponse()) + { + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + const btScalar* deltaV_normal; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_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; + const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0]; + deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k]; + } + va = cti.m_normal * vel * m_dt; + + // add in complementary direction of va + const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0]; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k]; + } + va += friction.m_complementaryDirection[j] * vel * m_dt; + } + } + + const btVector3 vb = c->m_node->m_v * m_dt; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + 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 + 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(); + + if (accumulated_normal < 0) + { + friction.m_direction[j] = -local_tangent_dir; + // 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; + } + else + { + friction.m_static[j] = true; + friction.m_impulse[j] = tangent_norm; + } + } + else + { + friction.m_released[j] = true; + friction.m_static[j] = false; + friction.m_impulse[j] = 0; + friction.m_direction[j] = btVector3(0,0,0); + } + 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]); + + // TODO cleanup + if (1) // in the same CG solve, the set of constraits doesn't change + { + // 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; + + btScalar dvn = -accumulated_normal * c->m_c2/m_dt; + + // the following is equivalent + /* + 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); + */ + + 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 + incremental_tangent; + 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 = 1; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); + + if (incremental_tangent.norm() > SIMD_EPSILON) + { + btMultiBodyJacobianData jacobian_tangent; + btVector3 tangent = incremental_tangent.normalized(); + findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); + const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); + } + } + } + } + } + } + } + } +} + +void btDeformableContactProjection::setConstraints() +{ + // set Dirichlet constraint + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im == 0) + { + btAlignedObjectArray c; + c.push_back(DeformableContactConstraint(btVector3(1,0,0))); + c.push_back(DeformableContactConstraint(btVector3(0,1,0))); + c.push_back(DeformableContactConstraint(btVector3(0,0,1))); + m_constraints[&(psb->m_nodes[j])] = c; + + btAlignedObjectArray f; + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); + m_frictions[&(psb->m_nodes[j])] = f; + } + } + } + + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_complementary; + + for (int j = 0; j < psb->m_rcontacts.size(); ++j) + { + const btSoftBody::RContact& c = psb->m_rcontacts[j]; + // skip anchor points + if (c.m_node->m_im == 0) + { + continue; + } + + const btSoftBody::sCti& cti = c.m_cti; + if (cti.m_colObj->hasContactResponse()) + { + btVector3 va(0, 0, 0); + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal); + btScalar vel = 0.0; + const btScalar* jac = &jacobianData_normal.m_jacobians[0]; + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + for (int j = 0; j < ndof; ++j) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl; + std::cout << jac[j] << std::endl; + } + va = cti.m_normal * vel * m_dt; + } + } + + const btVector3 vb = c.m_node->m_v * m_dt; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, cti.m_normal); + if (dn < SIMD_EPSILON) + { + // find complementary jacobian + btVector3 complementaryDirection; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + complementaryDirection = generateUnitOrthogonalVector(cti.m_normal); + findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection); + } + } + + if (m_constraints.find(c.m_node) == m_constraints.end()) + { + btAlignedObjectArray constraints; + constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); + m_constraints[c.m_node] = constraints; + btAlignedObjectArray frictions; + frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); + m_frictions[c.m_node] = frictions; + } + else + { + // group colinear constraints into one + const btScalar angle_epsilon = 0.015192247; // less than 10 degree + bool merged = false; + btAlignedObjectArray& constraints = m_constraints[c.m_node]; + btAlignedObjectArray& frictions = m_frictions[c.m_node]; + for (int j = 0; j < constraints.size(); ++j) + { + const btAlignedObjectArray& dirs = constraints[j].m_direction; + btScalar dot_prod = dirs[0].dot(cti.m_normal); + if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon) + { + // group the constraints + constraints[j].append(c, jacobianData_normal); + // push in an empty friction + frictions[j].append(); + frictions[j].addJacobian(complementaryDirection, jacobianData_complementary); + merged = true; + break; + } + } + const int dim = 3; + // hard coded no more than 3 constraint directions + if (!merged && constraints.size() < dim) + { + constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); + frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); + } + } + } + } + } + } +} + +void btDeformableContactProjection::enforceConstraint(TVStack& x) +{ + const int dim = 3; + for (auto& it : m_constraints) + { + const btAlignedObjectArray& constraints = it.second; + size_t i = m_indices[it.first]; + const btAlignedObjectArray& 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 DeformableFrictionConstraint& 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 btDeformableContactProjection::project(TVStack& x) +{ + const int dim = 3; + for (auto& it : m_constraints) + { + const btAlignedObjectArray& constraints = it.second; + size_t i = m_indices[it.first]; + btAlignedObjectArray& 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) + { + DeformableFrictionConstraint& 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) + { + DeformableFrictionConstraint& 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]; + } + } + } + } + } +} + +void btDeformableContactProjection::reinitialize(bool nodeUpdated) +{ + btCGProjection::reinitialize(nodeUpdated); + m_constraints.clear(); + m_frictions.clear(); +} + + + diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h new file mode 100644 index 000000000..aa7af70fb --- /dev/null +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -0,0 +1,43 @@ +// +// btDeformableContactProjection.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/4/19. +// + +#ifndef BT_CONTACT_PROJECTION_H +#define BT_CONTACT_PROJECTION_H +#include "btCGProjection.h" +#include "btSoftBody.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include +class btDeformableContactProjection : public btCGProjection +{ +public: + std::unordered_map > m_constraints; + std::unordered_map > m_frictions; + + btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) + : btCGProjection(softBodies, dt) + { + } + + virtual ~btDeformableContactProjection() + { + } + + // apply the constraints to the rhs + virtual void project(TVStack& x); + + // apply constraints to x in Ax=b + virtual void enforceConstraint(TVStack& x); + + // update the constraints + virtual void update(); + + virtual void setConstraints(); + + virtual void reinitialize(bool nodeUpdated); +}; +#endif /* btContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h new file mode 100644 index 000000000..545615a47 --- /dev/null +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -0,0 +1,60 @@ +// +// btDeformableGravityForce.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/21/19. +// + +#ifndef BT_DEFORMABLE_GRAVITY_FORCE_H +#define BT_DEFORMABLE_GRAVITY_FORCE_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableGravityForce : public btDeformableLagrangianForce +{ +public: + using TVStack = btDeformableLagrangianForce::TVStack; + btVector3 m_gravity; + + btDeformableGravityForce(const btAlignedObjectArray& softBodies, const btVector3& g) : btDeformableLagrangianForce(softBodies), m_gravity(g) + { + + } + + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledGravityForce(scale, force); + } + + virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + + } + + virtual void addScaledGravityForce(btScalar scale, TVStack& force) + { + int numNodes = getNumNodes(); + btAssert(numNodes == force.size()) + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& n = psb->m_nodes[j]; + size_t id = m_indices[&n]; + btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; + btVector3 scaled_force = scale * m_gravity * mass; + force[id] += scaled_force; + } + } + } + + + + +}; +#endif /* BT_DEFORMABLE_GRAVITY_FORCE_H */ diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h new file mode 100644 index 000000000..acb9a28ff --- /dev/null +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -0,0 +1,63 @@ +// +// btDeformableLagrangianForce.h +// BulletSoftBody +// +// Created by Xuchen Han on 7/1/19. +// + +#ifndef BT_DEFORMABLE_LAGRANGIAN_FORCE_H +#define BT_DEFORMABLE_LAGRANGIAN_FORCE_H + +#include "btSoftBody.h" +#include + +class btDeformableLagrangianForce +{ +public: + using TVStack = btAlignedObjectArray; + const btAlignedObjectArray& m_softBodies; + std::unordered_map m_indices; + + btDeformableLagrangianForce(const btAlignedObjectArray& softBodies) + : m_softBodies(softBodies) + { + } + + virtual ~btDeformableLagrangianForce(){} + + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0; + + virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + + virtual void reinitialize(bool nodeUpdated) + { + if (nodeUpdated) + updateId(); + } + + virtual 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++; + } + } + } + + virtual int getNumNodes() + { + int numNodes = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + numNodes += m_softBodies[i]->m_nodes.size(); + } + return numNodes; + } +}; +#endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h new file mode 100644 index 000000000..31389c20f --- /dev/null +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -0,0 +1,107 @@ +// +// btDeformableMassSpringForce.h +// BulletSoftBody +// +// Created by Xuchen Gan on 7/1/19. +// + +#ifndef BT_MASS_SPRING_H +#define BT_MASS_SPRING_H + +#include "btDeformableLagrangianForce.h" + +class btDeformableMassSpringForce : public btDeformableLagrangianForce +{ +public: + using TVStack = btDeformableLagrangianForce::TVStack; + btDeformableMassSpringForce(const btAlignedObjectArray& softBodies) : btDeformableLagrangianForce(softBodies) + { + + } + + virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + { + addScaledDampingForce(scale, force); + } + + virtual void addScaledExplicitForce(btScalar scale, TVStack& force) + { + addScaledElasticForce(scale, force); + } + + virtual void addScaledDampingForce(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]; + size_t id1 = m_indices[node1]; + size_t id2 = m_indices[node2]; + + // damping force + btVector3 v_diff = (node2->m_v - node1->m_v); + btScalar k_damp = psb->m_dampingCoefficient; + btVector3 scaled_force = scale * v_diff * k_damp; + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledElasticForce(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; + btScalar r = link.m_rl; + size_t id1 = m_indices[node1]; + size_t id2 = m_indices[node2]; + + // elastic force + // explicit elastic force + btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir_normalized = dir.normalized(); + btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r); + force[id1] += scaled_force; + force[id2] -= scaled_force; + } + } + } + + virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + // implicit damping force differential + 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; + 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; + } + } + } +}; + +#endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 173f71206..3af3c8499 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -12,87 +12,78 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - m_internalTime += timeStep; - // Let the solver grab the soft bodies and if necessary optimize for it - m_deformableBodySolver->optimize(m_softBodies); + reinitialize(timeStep); - if (!m_deformableBodySolver->checkInitialized()) - { - btAssert("Solver initialization failed\n"); - } - - // from btDiscreteDynamicsWorld singleStepSimulation - if (0 != m_internalPreTickCallback) - { - (*m_internalPreTickCallback)(this, timeStep); - } + // add gravity to velocity of rigid and multi bodys + applyRigidBodyGravity(timeStep); - ///apply gravity, predict motion + ///apply gravity and explicit force to velocity, predict motion predictUnconstraintMotion(timeStep); - btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); - - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); - - // only used in CCD -// createPredictiveContacts(timeStep); - ///perform collision detection btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); btMultiBodyDynamicsWorld::calculateSimulationIslands(); - btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; - - if (0 != m_internalTickCallback) - { - (*m_internalTickCallback)(this, timeStep); - } - - // TODO: This is an ugly hack to get the desired gravity behavior. - // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again - // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep - // when there are multiple substeps - clearForces(); - clearMultiBodyForces(); - btMultiBodyDynamicsWorld::applyGravity(); - // integrate rigid body gravity - for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) - { - btRigidBody* rb = m_nonStaticRigidBodies[i]; - rb->integrateVelocities(timeStep); - } - // integrate multibody gravity - btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo()); - clearForces(); - clearMultiBodyForces(); - + beforeSolverCallbacks(timeStep); - for (int i = 0; i < before_solver_callbacks.size(); ++i) - before_solver_callbacks[i](m_internalTime, this); ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); - + positionCorrection(); - //integrate transforms - btMultiBodyDynamicsWorld::integrateTransforms(timeStep); + integrateTransforms(timeStep); ///update vehicle simulation btMultiBodyDynamicsWorld::updateActions(timeStep); btMultiBodyDynamicsWorld::updateActivationState(timeStep); - - ///update soft bodies - m_deformableBodySolver->updateSoftBodies(); - - clearForces(); // End solver-wise simulation step // /////////////////////////////// } +void btDeformableRigidDynamicsWorld::positionCorrection() +{ + // perform position correction for all geometric collisions + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + const btScalar mrg = psb->getCollisionShape()->getMargin(); + for (int j = 0; j < psb->m_rcontacts.size(); ++j) + { + const btSoftBody::RContact& c = psb->m_rcontacts[j]; + // skip anchor points + if (c.m_node->m_im == 0) + continue; + + const btSoftBody::sCti& cti = c.m_cti; + if (cti.m_colObj->hasContactResponse()) + { + btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); + if (dp < 0) + { + // m_c4 is the collision hardness + c.m_node->m_q -= dp * cti.m_normal * c.m_c4; + } + } + } + } +} + +void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) +{ + btMultiBodyDynamicsWorld::integrateTransforms(dt); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[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 btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) { m_deformableBodySolver->solveConstraints(timeStep); @@ -117,4 +108,45 @@ void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep m_deformableBodySolver->predictMotion(float(timeStep)); } +void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep) +{ + m_internalTime += timeStep; + m_deformableBodySolver->reinitialize(m_softBodies); + btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); + btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; +} +void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) +{ + // TODO: This is an ugly hack to get the desired gravity behavior. + // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep + // when there are multiple substeps + clearForces(); + clearMultiBodyForces(); + btMultiBodyDynamicsWorld::applyGravity(); + // integrate rigid body gravity + for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i) + { + btRigidBody* rb = m_nonStaticRigidBodies[i]; + rb->integrateVelocities(timeStep); + } + // integrate multibody gravity + btMultiBodyDynamicsWorld::solveExternalForces(btMultiBodyDynamicsWorld::getSolverInfo()); + clearForces(); + clearMultiBodyForces(); +} + +void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) +{ + if (0 != m_internalTickCallback) + { + (*m_internalTickCallback)(this, timeStep); + } + + for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) + m_beforeSolverCallbacks[i](m_internalTime, this); +} diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 5bbebeda3..c5cf280ef 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -17,15 +17,15 @@ #define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H #include "btSoftMultiBodyDynamicsWorld.h" -#include "btLagrangianForce.h" -#include "btMassSpring.h" +#include "btDeformableLagrangianForce.h" +#include "btDeformableMassSpringForce.h" #include "btDeformableBodySolver.h" #include "btSoftBodyHelpers.h" #include typedef btAlignedObjectArray btSoftBodyArray; class btDeformableBodySolver; -class btLagrangianForce; +class btDeformableLagrangianForce; typedef btAlignedObjectArray btSoftBodyArray; class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld @@ -45,6 +45,10 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld protected: virtual void internalSingleStepSimulation(btScalar timeStep); + virtual void integrateTransforms(btScalar timeStep); + + void positionCorrection(); + void solveDeformableBodiesConstraints(btScalar timeStep); public: @@ -70,7 +74,7 @@ public: m_sbi.m_sparsesdf.Initialize(); m_internalTime = 0.0; } - btAlignedObjectArray > before_solver_callbacks; + btAlignedObjectArray > m_beforeSolverCallbacks; virtual ~btDeformableRigidDynamicsWorld() { } @@ -108,10 +112,18 @@ public: { return m_sbi; } + const btSoftBodyWorldInfo& getWorldInfo() const { return m_sbi; } + + void reinitialize(btScalar timeStep); + + void applyRigidBodyGravity(btScalar timeStep); + + void beforeSolverCallbacks(btScalar timeStep); + int getDrawFlags() const { return (m_drawFlags); } void setDrawFlags(int f) { m_drawFlags = f; } }; diff --git a/src/BulletSoftBody/btLagrangianForce.h b/src/BulletSoftBody/btLagrangianForce.h deleted file mode 100644 index b95a30ce9..000000000 --- a/src/BulletSoftBody/btLagrangianForce.h +++ /dev/null @@ -1,55 +0,0 @@ -// -// 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 addScaledImplicitForce(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 addScaledExplicitForce(btScalar scale, TVStack& force) = 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 deleted file mode 100644 index 78cdcf7e6..000000000 --- a/src/BulletSoftBody/btMassSpring.h +++ /dev/null @@ -1,147 +0,0 @@ -// -// 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 addScaledImplicitForce(btScalar scale, TVStack& force) - { - addScaledDampingForce(scale, force); - } - - virtual void addScaledExplicitForce(btScalar scale, TVStack& force) - { - addScaledElasticForce(scale, force); - } - - virtual void addScaledDampingForce(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]; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; - - // damping force - btVector3 v_diff = (node2->m_v - node1->m_v); - btScalar k_damp = psb->m_dampingCoefficient; - btVector3 scaled_force = scale * v_diff * k_damp; - force[id1] += scaled_force; - force[id2] -= scaled_force; - } - } - } - - virtual void addScaledElasticForce(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; - btScalar r = link.m_rl; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; - - // elastic force - - // fully implicit -// btVector3 dir = (node2->m_x - node1->m_x); - - // explicit elastic force - btVector3 dir = (node2->m_q - node1->m_q); - btVector3 dir_normalized = dir.normalized(); - btVector3 scaled_force = scale * kLST * (dir - dir_normalized * r); - 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 differential - 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) - { - // implicit damping force differential - 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; - 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 974246448..876b6b6e4 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1782,7 +1782,7 @@ void btSoftBody::predictMotion(btScalar dt) m_sst.radmrg = getCollisionShape()->getMargin(); m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; /* Forces */ - addVelocity(m_worldInfo->m_gravity * m_sst.sdt); +// addVelocity(m_worldInfo->m_gravity * m_sst.sdt); applyForces(); /* Integrate */ for (i = 0, ni = m_nodes.size(); i < ni; ++i) @@ -1806,7 +1806,7 @@ void btSoftBody::predictMotion(btScalar dt) } } n.m_v += deltaV; - n.m_x += n.m_v * m_sst.sdt; + n.m_x += n.m_v * m_sst.sdt; n.m_f = btVector3(0, 0, 0); } /* Clusters */ @@ -2270,10 +2270,11 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); - //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); - const btTransform& wtr = colObjWrap->getWorldTransform(); - //todo: check which transform is needed here + const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + + // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg + const btTransform &wtr = tmpRigid ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); + // TODO: get the correct transform for multibody btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( -- cgit v1.2.1 From 243b9fc8c7e4727eb42947389a99c0254d16032d Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 23 Jul 2019 12:15:23 -0700 Subject: combat friction drift in positionCorrect by changing velocity and change it back (effectively only changing position) --- examples/DeformableContact/DeformableContact.cpp | 23 ++-- examples/DeformableDemo/DeformableDemo.cpp | 21 ++-- examples/Pinch/Pinch.cpp | 136 ++++++++++++++++++--- .../VolumetricDeformable/VolumetricDeformable.cpp | 4 +- .../btSimulationIslandManager.cpp | 7 +- src/BulletSoftBody/btContactProjection.cpp | 54 ++++---- src/BulletSoftBody/btDeformableBodySolver.cpp | 15 +++ src/BulletSoftBody/btDeformableBodySolver.h | 6 +- .../btDeformableContactProjection.cpp | 91 +++++++------- src/BulletSoftBody/btDeformableGravityForce.h | 1 + src/BulletSoftBody/btDeformableMassSpringForce.h | 3 + .../btDeformableRigidDynamicsWorld.cpp | 67 +++++++--- .../btDeformableRigidDynamicsWorld.h | 4 +- src/BulletSoftBody/btSoftBody.cpp | 3 +- src/BulletSoftBody/btSoftBodyInternals.h | 8 +- 15 files changed, 291 insertions(+), 152 deletions(-) diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp index b70751e2f..a43fc04ca 100644 --- a/examples/DeformableContact/DeformableContact.cpp +++ b/examples/DeformableContact/DeformableContact.cpp @@ -54,13 +54,6 @@ class DeformableContact : public CommonMultiBodyBase btMultiBody* m_multiBody; btAlignedObjectArray m_jointFeedbacks; public: - - struct TetraBunny - { - #include "../SoftDemo/bunny.inl" - }; - - DeformableContact(struct GUIHelperInterface* helper) : CommonMultiBodyBase(helper) { @@ -147,7 +140,7 @@ void DeformableContact::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0, -40, 0)); - groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1)); //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: btScalar mass(0.); @@ -170,16 +163,16 @@ void DeformableContact::initPhysics() { - bool damping = true; - bool gyro = true; - int numLinks = 3; + bool damping = false; + bool gyro = false; + int numLinks = 0; 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); + btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 2.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); mbC->setCanSleep(canSleep); mbC->setHasSelfCollision(selfCollide); @@ -216,12 +209,13 @@ void DeformableContact::initPhysics() // create a patch of cloth { - const btScalar s = 6; + const btScalar s = 4; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), btVector3(+s, 0, -s), btVector3(-s, 0, +s), btVector3(+s, 0, +s), - 20,20, +// 20,20, + 3,3, 1 + 2 + 4 + 8, true); psb->getCollisionShape()->setMargin(0.25); @@ -232,7 +226,6 @@ void DeformableContact::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 0; getDeformableDynamicsWorld()->addSoftBody(psb); - } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index 7ce94a314..aa58fbd6f 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -105,15 +105,15 @@ public: // } btTransform startTransform; startTransform.setIdentity(); - startTransform.setOrigin(btVector3(1, 2, 1)); + startTransform.setOrigin(btVector3(1, 1.5, 1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(1, 2, -1)); + startTransform.setOrigin(btVector3(1, 1.5, -1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(-1, 2, 1)); + startTransform.setOrigin(btVector3(-1, 1.5, 1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(-1, 2, -1)); + startTransform.setOrigin(btVector3(-1, 1.5, -1)); createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(0, 4, 0)); + startTransform.setOrigin(btVector3(0, 3.5, 0)); createRigidBody(mass, startTransform, shape[0]); } @@ -182,7 +182,7 @@ void DeformableDemo::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -30, 0)); + groundTransform.setOrigin(btVector3(0, -32, 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.); @@ -206,7 +206,7 @@ void DeformableDemo::initPhysics() // create a piece of cloth { - bool onGround = true; + bool onGround = false; const btScalar s = 4; btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), btVector3(+s, 0, -s), @@ -222,13 +222,14 @@ void DeformableDemo::initPhysics() btVector3(+s, 0, -s), btVector3(-s, 0, +s), btVector3(+s, 0, +s), - 20,20, +// 20,20, + 2,2, 0, true); psb->getCollisionShape()->setMargin(0.1); psb->generateBendingConstraints(2); - psb->setTotalMass(10); - psb->setSpringStiffness(10); + 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 diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp index cedc51750..cc3008f9c 100644 --- a/examples/Pinch/Pinch.cpp +++ b/examples/Pinch/Pinch.cpp @@ -73,9 +73,9 @@ public: void resetCamera() { float dist = 25; - float pitch = -45; + float pitch = -30; float yaw = 100; - float targetPos[3] = {0, -3, 0}; + float targetPos[3] = {0, -0, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } @@ -137,17 +137,56 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world) return; btRigidBody* rb0 = rbs[0]; btScalar pressTime = 0.9; + btScalar liftTime = 2.5; + btScalar shiftTime = 3.5; + btScalar holdTime = 4.5*1000; + btScalar dropTime = 5.3*1000; btTransform rbTransform; rbTransform.setIdentity(); - btVector3 translation = btVector3(0.5,3,4); - btVector3 velocity = btVector3(0,5,0); + btVector3 translation; + btVector3 velocity; + + btVector3 initialTranslationLeft = btVector3(0.5,3,4); + btVector3 initialTranslationRight = btVector3(0.5,3,-4); + btVector3 pinchVelocityLeft = btVector3(0,0,-2); + btVector3 pinchVelocityRight = btVector3(0,0,2); + btVector3 liftVelocity = btVector3(0,5,0); + btVector3 shiftVelocity = btVector3(0,0,5); + btVector3 holdVelocity = btVector3(0,0,0); + btVector3 openVelocityLeft = btVector3(0,0,4); + btVector3 openVelocityRight = btVector3(0,0,-4); + if (time < pressTime) { - velocity = btVector3(0,0,-2); - translation += velocity * time; + velocity = pinchVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime); } else - translation += btVector3(0,0,-2) * pressTime + (time-pressTime)*velocity; + { + velocity = holdVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime); + } rbTransform.setOrigin(translation); rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); rb0->setCenterOfMassTransform(rbTransform); @@ -155,23 +194,45 @@ void dynamics(btScalar time, btDeformableRigidDynamicsWorld* world) 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; + velocity = pinchVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime); } else - translation += btVector3(0,0,2) * pressTime + (time-pressTime)*velocity; + { + velocity = holdVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime); + } 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); + rb0->setFriction(20); + rb1->setFriction(20); } void Pinch::initPhysics() @@ -194,7 +255,7 @@ void Pinch::initPhysics() m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); getDeformableDynamicsWorld()->getWorldInfo().m_gravity.setValue(0, -10, 0); - getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //create a ground @@ -229,21 +290,58 @@ void Pinch::initPhysics() // create a soft block { + btScalar verts[24] = {0.f, 0.f, 0.f, + 1.f, 0.f, 0.f, + 0.f, 1.f, 0.f, + 0.f, 0.f, 1.f, + 1.f, 1.f, 0.f, + 0.f, 1.f, 1.f, + 1.f, 0.f, 1.f, + 1.f, 1.f, 1.f + }; + int triangles[60] = {0, 6, 3, + 0,1,6, + 7,5,3, + 7,3,6, + 4,7,6, + 4,6,1, + 7,2,5, + 7,4,2, + 0,3,2, + 2,3,5, + 0,2,4, + 0,4,1, + 0,6,5, + 0,6,4, + 3,4,2, + 3,4,7, + 2,7,3, + 2,7,1, + 4,5,0, + 4,5,6, + }; +// btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20); +//// 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->scale(btVector3(5, 5, 5)); +// psb->translate(btVector3(-2.5, 4, -2.5)); +// psb->getCollisionShape()->setMargin(0.1); +// psb->setTotalMass(1); + psb->setSpringStiffness(4); + psb->setDampingCoefficient(0.02); 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; + psb->m_cfg.kDF = 2; + getDeformableDynamicsWorld()->addSoftBody(psb); // add a grippers createGrip(); } diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp index b30e9bc24..9bcf63ab2 100644 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -77,8 +77,8 @@ public: void stepSimulation(float deltaTime) { //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 240.f; - m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + float internalTimeStep = 1. / 480.f; + m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep); } void createStaticBox(const btVector3& halfEdge, const btVector3& translation) diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index e5097ccbb..11fa97cdd 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); +// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if (colObj0->getActivationState() == ACTIVE_TAG || @@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); +// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { @@ -278,7 +278,8 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } - btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); +// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) { diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp index 99ab6f473..e81cb2c50 100644 --- a/src/BulletSoftBody/btContactProjection.cpp +++ b/src/BulletSoftBody/btContactProjection.cpp @@ -50,12 +50,12 @@ void btDeformableContactProjection::update() // loop through constraints to set constrained values for (auto& it : m_constraints) { - btAlignedObjectArray& frictions = m_frictions[it.first]; - btAlignedObjectArray& constraints = it.second; + btAlignedObjectArray& frictions = m_frictions[it.first]; + btAlignedObjectArray& constraints = it.second; for (int i = 0; i < constraints.size(); ++i) { - Constraint& constraint = constraints[i]; - Friction& friction = frictions[i]; + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; for (int j = 0; j < constraint.m_contact.size(); ++j) { if (constraint.m_contact[j] == nullptr) @@ -229,16 +229,16 @@ void btDeformableContactProjection::setConstraints() { if (psb->m_nodes[j].m_im == 0) { - btAlignedObjectArray c; - c.push_back(Constraint(btVector3(1,0,0))); - c.push_back(Constraint(btVector3(0,1,0))); - c.push_back(Constraint(btVector3(0,0,1))); + btAlignedObjectArray c; + c.push_back(DeformableContactConstraint(btVector3(1,0,0))); + c.push_back(DeformableContactConstraint(btVector3(0,1,0))); + c.push_back(DeformableContactConstraint(btVector3(0,0,1))); m_constraints[&(psb->m_nodes[j])] = c; - btAlignedObjectArray f; - f.push_back(Friction()); - f.push_back(Friction()); - f.push_back(Friction()); + btAlignedObjectArray f; + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); + f.push_back(DeformableFrictionConstraint()); m_frictions[&(psb->m_nodes[j])] = f; } } @@ -310,11 +310,11 @@ void btDeformableContactProjection::setConstraints() if (m_constraints.find(c.m_node) == m_constraints.end()) { - btAlignedObjectArray constraints; - constraints.push_back(Constraint(c, jacobianData_normal)); + btAlignedObjectArray constraints; + constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); m_constraints[c.m_node] = constraints; - btAlignedObjectArray frictions; - frictions.push_back(Friction(complementaryDirection, jacobianData_complementary)); + btAlignedObjectArray frictions; + frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); m_frictions[c.m_node] = frictions; } else @@ -322,8 +322,8 @@ void btDeformableContactProjection::setConstraints() // group colinear constraints into one const btScalar angle_epsilon = 0.015192247; // less than 10 degree bool merged = false; - btAlignedObjectArray& constraints = m_constraints[c.m_node]; - btAlignedObjectArray& frictions = m_frictions[c.m_node]; + btAlignedObjectArray& constraints = m_constraints[c.m_node]; + btAlignedObjectArray& frictions = m_frictions[c.m_node]; for (int j = 0; j < constraints.size(); ++j) { const btAlignedObjectArray& dirs = constraints[j].m_direction; @@ -343,8 +343,8 @@ void btDeformableContactProjection::setConstraints() // hard coded no more than 3 constraint directions if (!merged && constraints.size() < dim) { - constraints.push_back(Constraint(c, jacobianData_normal)); - frictions.push_back(Friction(complementaryDirection, jacobianData_complementary)); + constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); + frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); } } } @@ -358,9 +358,9 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) const int dim = 3; for (auto& it : m_constraints) { - const btAlignedObjectArray& constraints = it.second; + const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; - const btAlignedObjectArray& frictions = m_frictions[it.first]; + const btAlignedObjectArray& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -401,7 +401,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) { for (int f = 0; f < frictions.size(); ++f) { - const Friction& friction= frictions[f]; + const DeformableFrictionConstraint& friction= frictions[f]; for (int j = 0; j < friction.m_direction.size(); ++j) { // clear the old constraint @@ -425,9 +425,9 @@ void btDeformableContactProjection::project(TVStack& x) const int dim = 3; for (auto& it : m_constraints) { - const btAlignedObjectArray& constraints = it.second; + const btAlignedObjectArray& constraints = it.second; size_t i = m_indices[it.first]; - btAlignedObjectArray& frictions = m_frictions[it.first]; + btAlignedObjectArray& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -450,14 +450,14 @@ void btDeformableContactProjection::project(TVStack& x) bool has_static_constraint = false; for (int f = 0; f < frictions.size(); ++f) { - Friction& friction= frictions[f]; + DeformableFrictionConstraint& 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]; + DeformableFrictionConstraint& friction= frictions[f]; for (int j = 0; j < friction.m_direction.size(); ++j) { // clear the old friction force diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 96d2e0029..e3009dcbb 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -29,6 +29,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) // save v_{n+1}^* velocity after explicit forces backupVelocity(); + m_objective->computeResidual(solverdt, m_residual); // m_objective->initialGuess(m_dv, m_residual); computeStep(m_dv, m_residual); @@ -98,6 +99,20 @@ void btDeformableBodySolver::backupVelocity() } } +void btDeformableBodySolver::revertVelocity() +{ + // 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_backupVelocity[counter++]; + } + } +} + bool btDeformableBodySolver::updateNodes() { int numNodes = 0; diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 570cce90f..0b4a4819f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -27,12 +27,14 @@ protected: TVStack m_dv; TVStack m_residual; btAlignedObjectArray m_softBodySet; - btDeformableBackwardEulerObjective* m_objective; + btAlignedObjectArray m_backupVelocity; btScalar m_dt; btConjugateGradient cg; public: + btDeformableBackwardEulerObjective* m_objective; + btDeformableBodySolver(); virtual ~btDeformableBodySolver(); @@ -57,7 +59,7 @@ public: void predictDeformableMotion(btSoftBody* psb, btScalar dt); void backupVelocity(); - + void revertVelocity(); void updateVelocity(); bool updateNodes(); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index e81cb2c50..25f667711 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -143,6 +143,8 @@ void btDeformableContactProjection::update() friction.m_direction[j] = -local_tangent_dir; // do not allow switching from static friction to dynamic friction // it causes cg to explode + btScalar comp1 = -accumulated_normal*c->m_c3; + btScalar comp2 = tangent_norm; if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false) { friction.m_static[j] = false; @@ -167,49 +169,44 @@ void btDeformableContactProjection::update() // 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]); - // TODO cleanup - if (1) // in the same CG solve, the set of constraits doesn't change - { - // 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; - btScalar dvn = -accumulated_normal * c->m_c2/m_dt; - - // the following is equivalent - /* - 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); - */ - - constraint.m_value[j] = dvn; + // dv = new_impulse + accumulated velocity change in previous CG iterations + // so we have the invariant node->m_v = backupVelocity + dv; + + btScalar dvn = -accumulated_normal * c->m_c2/m_dt; + + // the following is equivalent + /* + 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); + */ + + 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 + incremental_tangent; + 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) + { - // 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 + incremental_tangent; - 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 = 1; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); - if (multibodyLinkCol) + if (incremental_tangent.norm() > SIMD_EPSILON) { - double multiplier = 1; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); - - if (incremental_tangent.norm() > SIMD_EPSILON) - { - btMultiBodyJacobianData jacobian_tangent; - btVector3 tangent = incremental_tangent.normalized(); - findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); - const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); - } + btMultiBodyJacobianData jacobian_tangent; + btVector3 tangent = incremental_tangent.normalized(); + findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); + const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); } } } @@ -404,14 +401,10 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) const DeformableFrictionConstraint& 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 + // add the friction constraint if (friction.m_static[j] == true) { + x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; x[i] += friction.m_direction[j] * friction.m_dv[j]; } } @@ -467,9 +460,15 @@ void btDeformableContactProjection::project(TVStack& x) } // only add to the rhs if there is no static friction constraint on the node - if (friction.m_static[j] == false && !has_static_constraint) + if (friction.m_static[j] == false) + { + if (!has_static_constraint) + x[i] += friction.m_direction[j] * friction.m_impulse[j]; + } + else { - x[i] += friction.m_direction[j] * friction.m_impulse[j]; + // otherwise clear the constraint in the friction direction + x[i] -= x[i].dot(friction.m_direction[j]) * friction.m_direction[j]; } } } diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 545615a47..d8571fa73 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -23,6 +23,7 @@ public: virtual void addScaledImplicitForce(btScalar scale, TVStack& force) { +// addScaledGravityForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 31389c20f..70ad6e69c 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -22,6 +22,7 @@ public: virtual void addScaledImplicitForce(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force); +// addScaledElasticForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -102,6 +103,8 @@ public: } } } + + }; #endif /* btMassSpring_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 3af3c8499..216c1a24d 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -13,7 +13,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { reinitialize(timeStep); - +// beforeSolverCallbacks(timeStep); // add gravity to velocity of rigid and multi bodys applyRigidBodyGravity(timeStep); @@ -30,7 +30,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS ///solve deformable bodies constraints solveDeformableBodiesConstraints(timeStep); - positionCorrection(); + afterSolverCallbacks(timeStep); integrateTransforms(timeStep); @@ -42,36 +42,57 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS // /////////////////////////////// } -void btDeformableRigidDynamicsWorld::positionCorrection() +void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) { - // perform position correction for all geometric collisions - for (int i = 0; i < m_softBodies.size(); ++i) + // perform position correction for all constraints + for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints) { - btSoftBody* psb = m_softBodies[i]; - const btScalar mrg = psb->getCollisionShape()->getMargin(); - for (int j = 0; j < psb->m_rcontacts.size(); ++j) + btAlignedObjectArray& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first]; + btAlignedObjectArray& constraints = it.second; + for (int i = 0; i < constraints.size(); ++i) { - const btSoftBody::RContact& c = psb->m_rcontacts[j]; - // skip anchor points - if (c.m_node->m_im == 0) - continue; - - const btSoftBody::sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) + DeformableContactConstraint& constraint = constraints[i]; + DeformableFrictionConstraint& friction = frictions[i]; + for (int j = 0; j < constraint.m_contact.size(); ++j) { - btScalar dp = btMin((btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg); - if (dp < 0) + const btSoftBody::RContact* c = constraint.m_contact[j]; + // skip anchor points + if (c == nullptr || c->m_node->m_im == 0) + continue; + const btSoftBody::sCti& cti = c->m_cti; + btRigidBody* rigidCol = 0; + btVector3 va(0, 0, 0); + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - // m_c4 is the collision hardness - c.m_node->m_q -= dp * cti.m_normal * c.m_c4; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); + } + + if (cti.m_colObj->hasContactResponse()) + { + btScalar dp = cti.m_offset; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (friction.m_static[j] == true) + { + c->m_node->m_v = va; + } + if (dp < 0) + { + c->m_node->m_v -= dp * cti.m_normal / dt; + } } } } } } + void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) { + m_deformableBodySolver->backupVelocity(); + positionCorrection(dt); btMultiBodyDynamicsWorld::integrateTransforms(dt); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -82,6 +103,7 @@ void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) node.m_x = node.m_q + dt * node.m_v; } } + m_deformableBodySolver->revertVelocity(); } void btDeformableRigidDynamicsWorld::solveDeformableBodiesConstraints(btScalar timeStep) @@ -146,7 +168,12 @@ void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) { (*m_internalTickCallback)(this, timeStep); } - + for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) + m_beforeSolverCallbacks[i](m_internalTime, this); +} + +void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) +{ for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) m_beforeSolverCallbacks[i](m_internalTime, this); } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index c5cf280ef..f1be4fd85 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -47,7 +47,7 @@ protected: virtual void integrateTransforms(btScalar timeStep); - void positionCorrection(); + void positionCorrection(btScalar dt); void solveDeformableBodiesConstraints(btScalar timeStep); @@ -124,6 +124,8 @@ public: void beforeSolverCallbacks(btScalar timeStep); + void afterSolverCallbacks(btScalar timeStep); + int getDrawFlags() const { return (m_drawFlags); } void setDrawFlags(int f) { m_drawFlags = f; } }; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 876b6b6e4..bd7587c47 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2286,7 +2286,8 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; - cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); +// cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + cti.m_offset = dst; return (true); } return (false); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index b892498ea..9b3e45ea4 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -878,15 +878,11 @@ struct btSoftColliders const btScalar ms = ima + imb; if (ms > 0) { + psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti); const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_x - wtr.getOrigin(); - const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); - const btVector3 vb = n.m_x - n.m_q; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, c.m_cti.m_normal); - const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btVector3 ra = n.m_q - wtr.getOrigin(); const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); c.m_node = &n; c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); -- cgit v1.2.1 From 233a381e7cc39b1768771f0d3660f985dd1bcf95 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 24 Jul 2019 16:01:47 -0700 Subject: add correct impulse matrix to multibody-deformable contact --- examples/DeformableContact/DeformableContact.cpp | 46 +- examples/ExampleBrowser/ExampleEntries.cpp | 2 + examples/MultiBodyBaseline/MultiBodyBaseline.cpp | 358 +++++++++++++++ examples/MultiBodyBaseline/MultiBodyBaseline.h | 20 + .../VolumetricDeformable/VolumetricDeformable.cpp | 4 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 4 + .../Featherstone/btMultiBodyDynamicsWorld.h | 1 + src/BulletSoftBody/btCGProjection.h | 27 +- src/BulletSoftBody/btContactProjection.cpp | 488 --------------------- .../btDeformableContactProjection.cpp | 72 ++- src/BulletSoftBody/btDeformableContactProjection.h | 2 +- .../btDeformableRigidDynamicsWorld.cpp | 37 ++ src/BulletSoftBody/btSoftBody.h | 10 +- src/BulletSoftBody/btSoftBodyInternals.h | 141 +++++- 14 files changed, 618 insertions(+), 594 deletions(-) create mode 100644 examples/MultiBodyBaseline/MultiBodyBaseline.cpp create mode 100644 examples/MultiBodyBaseline/MultiBodyBaseline.h delete mode 100644 src/BulletSoftBody/btContactProjection.cpp diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp index a43fc04ca..f51978ae3 100644 --- a/examples/DeformableContact/DeformableContact.cpp +++ b/examples/DeformableContact/DeformableContact.cpp @@ -69,10 +69,10 @@ public: void resetCamera() { - float dist = 15; + float dist = 30; float pitch = -30; float yaw = 100; - float targetPos[3] = {0, -3, 0}; + float targetPos[3] = {0, -10, 0}; m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } @@ -140,7 +140,7 @@ void DeformableContact::initPhysics() btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0, -40, 0)); - groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1)); + 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.); @@ -163,16 +163,16 @@ void DeformableContact::initPhysics() { - bool damping = false; + bool damping = true; bool gyro = false; - int numLinks = 0; - bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + int numLinks = 4; + bool spherical = false; //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, 2.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); + 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); @@ -180,13 +180,13 @@ void DeformableContact::initPhysics() // if (!damping) { - mbC->setLinearDamping(0.f); - mbC->setAngularDamping(0.f); + mbC->setLinearDamping(0.0f); + mbC->setAngularDamping(0.0f); } else { - mbC->setLinearDamping(0.1f); - mbC->setAngularDamping(0.9f); + mbC->setLinearDamping(0.04f); + mbC->setAngularDamping(0.04f); } if (numLinks > 0) @@ -209,22 +209,24 @@ void DeformableContact::initPhysics() // create a patch of cloth { + btScalar h = 0; const btScalar s = 4; - btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), - btVector3(+s, 0, -s), - btVector3(-s, 0, +s), - btVector3(+s, 0, +s), -// 20,20, - 3,3, + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 20,20, +// 3,3, 1 + 2 + 4 + 8, true); psb->getCollisionShape()->setMargin(0.25); psb->generateBendingConstraints(2); - psb->setTotalMass(.5); + psb->setTotalMass(5); + psb->setSpringStiffness(2); 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; + psb->m_cfg.kDF = .1; getDeformableDynamicsWorld()->addSoftBody(psb); } @@ -271,7 +273,7 @@ void DeformableContact::exitPhysics() void DeformableContact::stepSimulation(float deltaTime) { // getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime); - m_dynamicsWorld->stepSimulation(deltaTime, 4, 1./240.); + m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.); } @@ -279,7 +281,7 @@ btMultiBody* DeformableContact::createFeatherstoneMultiBody_testMultiDof(btMulti { //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = .05f; + float baseMass = .1f; if (baseMass) { @@ -300,7 +302,7 @@ btMultiBody* DeformableContact::createFeatherstoneMultiBody_testMultiDof(btMulti //init the links btVector3 hingeJointAxis(1, 0, 0); - float linkMass = .05f; + float linkMass = .1f; btVector3 linkInertiaDiag(0.f, 0.f, 0.f); btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index f29cef2ba..1bbadd941 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -50,6 +50,7 @@ #include "../DeformableDemo/DeformableDemo.h" #include "../Pinch/Pinch.h" #include "../DeformableContact/DeformableContact.h" +#include "../MultiBodyBaseline/MultiBodyBaseline.h" #include "../VolumetricDeformable/VolumetricDeformable.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" @@ -126,6 +127,7 @@ static ExampleEntry gDefaultExamples[] = 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(0, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), 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/MultiBodyBaseline/MultiBodyBaseline.cpp b/examples/MultiBodyBaseline/MultiBodyBaseline.cpp new file mode 100644 index 000000000..663c2d33a --- /dev/null +++ b/examples/MultiBodyBaseline/MultiBodyBaseline.cpp @@ -0,0 +1,358 @@ +/* +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 "MultiBodyBaseline.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 //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 MultiBodyBaseline demo deformable bodies self-collision +static bool g_floatingBase = true; +static float friction = 1.; +class MultiBodyBaseline : public CommonMultiBodyBase +{ + btMultiBody* m_multiBody; + btAlignedObjectArray m_jointFeedbacks; +public: + MultiBodyBaseline(struct GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) + { + } + + virtual ~MultiBodyBaseline() + { + } + + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 30; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -10, 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); + +}; + +void MultiBodyBaseline::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(); + btMultiBodyConstraintSolver* sol; + sol = new btMultiBodyConstraintSolver; + m_solver = sol; + + btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration); + m_dynamicsWorld = world; + // m_dynamicsWorld->setDebugDrawer(&gDebugDraw); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + + { + ///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.)); + //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 = false; + int numLinks = 4; + bool spherical = false; //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.0f); + mbC->setAngularDamping(0.0f); + } + else + { + mbC->setLinearDamping(0.04f); + mbC->setAngularDamping(0.04f); + } + + 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); + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void MultiBodyBaseline::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 MultiBodyBaseline::stepSimulation(float deltaTime) +{ +// getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime); + m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.); +} + + +btMultiBody* MultiBodyBaseline::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 = .1f; + + 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 = .1f; + 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 MultiBodyBaseline::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray 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* MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options) +{ + return new MultiBodyBaseline(options.m_guiHelper); +} + + diff --git a/examples/MultiBodyBaseline/MultiBodyBaseline.h b/examples/MultiBodyBaseline/MultiBodyBaseline.h new file mode 100644 index 000000000..35734c63c --- /dev/null +++ b/examples/MultiBodyBaseline/MultiBodyBaseline.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 _MULTIBODY_BASELINE_H +#define _MULTIBODY_BASELINE_H + +class CommonExampleInterface* MultiBodyBaselineCreateFunc(struct CommonExampleOptions& options); + +#endif //_MULTIBODY_BASELINE_H diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp index 9bcf63ab2..b30e9bc24 100644 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -77,8 +77,8 @@ public: void stepSimulation(float deltaTime) { //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 480.f; - m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep); + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } void createStaticBox(const btVector3& halfEdge, const btVector3& translation) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 718106e77..d4a9a754f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -422,7 +422,11 @@ void btMultiBodyDynamicsWorld::forwardKinematics() void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { solveExternalForces(solverInfo); + solveInternalConstraints(solverInfo); +} +void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo) +{ /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 019d1bd87..4f48f07d2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -113,6 +113,7 @@ public: virtual void getAnalyticsData(btAlignedObjectArray& m_islandAnalyticsData) const; virtual void solveExternalForces(btContactSolverInfo& solverInfo); + virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 09055c7d1..928ee46f8 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -21,11 +21,10 @@ struct DeformableContactConstraint btAlignedObjectArray m_value; // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve btAlignedObjectArray m_accumulated_normal_impulse; - btAlignedObjectArray m_normal_jacobian; - DeformableContactConstraint(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian) + DeformableContactConstraint(const btSoftBody::RContact& rcontact) { - append(rcontact, jacobian); + append(rcontact); } DeformableContactConstraint(const btVector3 dir) @@ -34,8 +33,6 @@ struct DeformableContactConstraint m_direction.push_back(dir); m_value.push_back(0); m_accumulated_normal_impulse.push_back(0); - btMultiBodyJacobianData j; - m_normal_jacobian.push_back(j); } DeformableContactConstraint() @@ -44,17 +41,14 @@ struct DeformableContactConstraint m_direction.push_back(btVector3(0,0,0)); m_value.push_back(0); m_accumulated_normal_impulse.push_back(0); - btMultiBodyJacobianData j; - m_normal_jacobian.push_back(j); } - void append(const btSoftBody::RContact& rcontact, const btMultiBodyJacobianData& jacobian) + 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); - m_normal_jacobian.push_back(jacobian); } ~DeformableContactConstraint() @@ -77,8 +71,6 @@ struct DeformableFrictionConstraint btAlignedObjectArray m_direction_prev; btAlignedObjectArray m_released; // whether the contact is released - btAlignedObjectArray m_complementary_jacobian; - btAlignedObjectArray m_complementaryDirection; // the total impulse the node applied to the rb in the tangential direction in the cg solve @@ -89,12 +81,6 @@ struct DeformableFrictionConstraint append(); } - DeformableFrictionConstraint(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian) - { - append(); - addJacobian(complementaryDir, jacobian); - } - void append() { m_static.push_back(false); @@ -112,13 +98,6 @@ struct DeformableFrictionConstraint m_accumulated_tangent_impulse.push_back(btVector3(0,0,0)); m_released.push_back(false); } - - void addJacobian(const btVector3& complementaryDir, const btMultiBodyJacobianData& jacobian) - { - m_complementary_jacobian.push_back(jacobian); - m_complementaryDirection.push_back(complementaryDir); - } - }; class btCGProjection diff --git a/src/BulletSoftBody/btContactProjection.cpp b/src/BulletSoftBody/btContactProjection.cpp deleted file mode 100644 index e81cb2c50..000000000 --- a/src/BulletSoftBody/btContactProjection.cpp +++ /dev/null @@ -1,488 +0,0 @@ -// -// btDeformableContactProjection.cpp -// BulletSoftBody -// -// Created by Xuchen Han on 7/4/19. -// - -#include "btDeformableContactProjection.h" -#include "btDeformableRigidDynamicsWorld.h" -#include -static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, - btMultiBodyJacobianData& jacobianData, - const btVector3& contact_point, - const btVector3& dir) -{ - 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, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); - multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); -} - -static btVector3 generateUnitOrthogonalVector(const btVector3& u) -{ - btScalar ux = u.getX(); - btScalar uy = u.getY(); - btScalar uz = u.getZ(); - btScalar ax = std::abs(ux); - btScalar ay = std::abs(uy); - btScalar az = std::abs(uz); - btVector3 v; - if (ax <= ay && ax <= az) - v = btVector3(0, -uz, uy); - else if (ay <= ax && ay <= az) - v = btVector3(-uz, 0, ux); - else - v = btVector3(-uy, ux, 0); - v.normalize(); - return v; -} - -void btDeformableContactProjection::update() -{ - ///solve rigid body constraints - m_world->getSolverInfo().m_numIterations = 10; - m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); - - // loop through constraints to set constrained values - for (auto& it : m_constraints) - { - btAlignedObjectArray& frictions = m_frictions[it.first]; - btAlignedObjectArray& constraints = it.second; - for (int i = 0; i < constraints.size(); ++i) - { - DeformableContactConstraint& constraint = constraints[i]; - DeformableFrictionConstraint& friction = frictions[i]; - for (int j = 0; j < constraint.m_contact.size(); ++j) - { - if (constraint.m_contact[j] == nullptr) - { - // nothing needs to be done for dirichelet constraints - continue; - } - const btSoftBody::RContact* c = constraint.m_contact[j]; - const btSoftBody::sCti& cti = c->m_cti; - - // normal jacobian is precompute but tangent jacobian is not - const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j]; - const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j]; - - if (cti.m_colObj->hasContactResponse()) - { - btVector3 va(0, 0, 0); - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - const btScalar* deltaV_normal; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_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; - const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0]; - deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - - // add in the normal component of the va - btScalar vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k]; - } - va = cti.m_normal * vel * m_dt; - - // add in complementary direction of va - const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0]; - vel = 0.0; - for (int k = 0; k < ndof; ++k) - { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k]; - } - va += friction.m_complementaryDirection[j] * vel * m_dt; - } - } - - const btVector3 vb = c->m_node->m_v * m_dt; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); - 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 - 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(); - - if (accumulated_normal < 0) - { - friction.m_direction[j] = -local_tangent_dir; - // 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; - } - else - { - friction.m_static[j] = true; - friction.m_impulse[j] = tangent_norm; - } - } - else - { - friction.m_released[j] = true; - friction.m_static[j] = false; - friction.m_impulse[j] = 0; - friction.m_direction[j] = btVector3(0,0,0); - } - 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]); - - // TODO cleanup - if (1) // in the same CG solve, the set of constraits doesn't change - { - // 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; - - btScalar dvn = -accumulated_normal * c->m_c2/m_dt; - - // the following is equivalent - /* - 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); - */ - - 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 + incremental_tangent; - 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 = 1; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); - - if (incremental_tangent.norm() > SIMD_EPSILON) - { - btMultiBodyJacobianData jacobian_tangent; - btVector3 tangent = incremental_tangent.normalized(); - findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); - const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); - } - } - } - } - } - } - } - } -} - -void btDeformableContactProjection::setConstraints() -{ - // set Dirichlet constraint - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - if (psb->m_nodes[j].m_im == 0) - { - btAlignedObjectArray c; - c.push_back(DeformableContactConstraint(btVector3(1,0,0))); - c.push_back(DeformableContactConstraint(btVector3(0,1,0))); - c.push_back(DeformableContactConstraint(btVector3(0,0,1))); - m_constraints[&(psb->m_nodes[j])] = c; - - btAlignedObjectArray f; - f.push_back(DeformableFrictionConstraint()); - f.push_back(DeformableFrictionConstraint()); - f.push_back(DeformableFrictionConstraint()); - m_frictions[&(psb->m_nodes[j])] = f; - } - } - } - - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - btMultiBodyJacobianData jacobianData_normal; - btMultiBodyJacobianData jacobianData_complementary; - - for (int j = 0; j < psb->m_rcontacts.size(); ++j) - { - const btSoftBody::RContact& c = psb->m_rcontacts[j]; - // skip anchor points - if (c.m_node->m_im == 0) - { - continue; - } - - const btSoftBody::sCti& cti = c.m_cti; - if (cti.m_colObj->hasContactResponse()) - { - btVector3 va(0, 0, 0); - btRigidBody* rigidCol = 0; - btMultiBodyLinkCollider* multibodyLinkCol = 0; - - // grab the velocity of the rigid body - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) - { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0); - } - else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal); - btScalar vel = 0.0; - const btScalar* jac = &jacobianData_normal.m_jacobians[0]; - const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - for (int j = 0; j < ndof; ++j) - { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; - std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl; - std::cout << jac[j] << std::endl; - } - va = cti.m_normal * vel * m_dt; - } - } - - const btVector3 vb = c.m_node->m_v * m_dt; - const btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - // find complementary jacobian - btVector3 complementaryDirection; - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - complementaryDirection = generateUnitOrthogonalVector(cti.m_normal); - findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection); - } - } - - if (m_constraints.find(c.m_node) == m_constraints.end()) - { - btAlignedObjectArray constraints; - constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); - m_constraints[c.m_node] = constraints; - btAlignedObjectArray frictions; - frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); - m_frictions[c.m_node] = frictions; - } - else - { - // group colinear constraints into one - const btScalar angle_epsilon = 0.015192247; // less than 10 degree - bool merged = false; - btAlignedObjectArray& constraints = m_constraints[c.m_node]; - btAlignedObjectArray& frictions = m_frictions[c.m_node]; - for (int j = 0; j < constraints.size(); ++j) - { - const btAlignedObjectArray& dirs = constraints[j].m_direction; - btScalar dot_prod = dirs[0].dot(cti.m_normal); - if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon) - { - // group the constraints - constraints[j].append(c, jacobianData_normal); - // push in an empty friction - frictions[j].append(); - frictions[j].addJacobian(complementaryDirection, jacobianData_complementary); - merged = true; - break; - } - } - const int dim = 3; - // hard coded no more than 3 constraint directions - if (!merged && constraints.size() < dim) - { - constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); - frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); - } - } - } - } - } - } -} - -void btDeformableContactProjection::enforceConstraint(TVStack& x) -{ - const int dim = 3; - for (auto& it : m_constraints) - { - const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices[it.first]; - const btAlignedObjectArray& 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 DeformableFrictionConstraint& 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 btDeformableContactProjection::project(TVStack& x) -{ - const int dim = 3; - for (auto& it : m_constraints) - { - const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices[it.first]; - btAlignedObjectArray& 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) - { - DeformableFrictionConstraint& 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) - { - DeformableFrictionConstraint& 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]; - } - } - } - } - } -} - -void btDeformableContactProjection::reinitialize(bool nodeUpdated) -{ - btCGProjection::reinitialize(nodeUpdated); - m_constraints.clear(); - m_frictions.clear(); -} - - - diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 25f667711..1a9cad120 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -45,7 +45,7 @@ void btDeformableContactProjection::update() { ///solve rigid body constraints m_world->getSolverInfo().m_numIterations = 10; - m_world->btMultiBodyDynamicsWorld::solveConstraints(m_world->getSolverInfo()); + m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values for (auto& it : m_constraints) @@ -66,10 +66,6 @@ void btDeformableContactProjection::update() const btSoftBody::RContact* c = constraint.m_contact[j]; const btSoftBody::sCti& cti = c->m_cti; - // normal jacobian is precompute but tangent jacobian is not - const btMultiBodyJacobianData& jacobianData_normal = constraint.m_normal_jacobian[j]; - const btMultiBodyJacobianData& jacobianData_complementary = friction.m_complementary_jacobian[j]; - if (cti.m_colObj->hasContactResponse()) { btVector3 va(0, 0, 0); @@ -89,25 +85,31 @@ void btDeformableContactProjection::update() if (multibodyLinkCol) { const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - const btScalar* jac_normal = &jacobianData_normal.m_jacobians[0]; - deltaV_normal = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; + deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; // add in the normal component of the va btScalar vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_normal[k]; + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k]; } va = cti.m_normal * vel * m_dt; - - // add in complementary direction of va - const btScalar* jac_complementary = &jacobianData_complementary.m_jacobians[0]; + + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k]; + } + va += c->t1 * vel * m_dt; vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * jac_complementary[k]; + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k]; } - va += friction.m_complementaryDirection[j] * vel * m_dt; + va += c->t2 * vel * m_dt; } } @@ -143,8 +145,6 @@ void btDeformableContactProjection::update() friction.m_direction[j] = -local_tangent_dir; // do not allow switching from static friction to dynamic friction // it causes cg to explode - btScalar comp1 = -accumulated_normal*c->m_c3; - btScalar comp2 = tangent_norm; if (-accumulated_normal*c->m_c3 < tangent_norm && friction.m_static_prev[j] == false && friction.m_released[j] == false) { friction.m_static[j] = false; @@ -194,19 +194,15 @@ void btDeformableContactProjection::update() } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { - if (multibodyLinkCol) { - double multiplier = 1; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, -impulse_normal.length() * multiplier); - + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); if (incremental_tangent.norm() > SIMD_EPSILON) { - btMultiBodyJacobianData jacobian_tangent; - btVector3 tangent = incremental_tangent.normalized(); - findJacobian(multibodyLinkCol, jacobian_tangent, c->m_node->m_x, tangent); - const btScalar* deltaV_tangent = &jacobian_tangent.m_deltaVelocitiesUnitImpulse[0]; - multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_tangent, incremental_tangent.length() * multiplier); + const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1)); + const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t2, impulse.dot(c->t2)); } } } @@ -274,15 +270,12 @@ void btDeformableContactProjection::setConstraints() multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); if (multibodyLinkCol) { - findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, cti.m_normal); btScalar vel = 0.0; - const btScalar* jac = &jacobianData_normal.m_jacobians[0]; + const btScalar* jac = &c.jacobianData_normal.m_jacobians[0]; const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; for (int j = 0; j < ndof; ++j) { vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; - std::cout << multibodyLinkCol->m_multiBody->getVelocityVector()[j] << std::endl; - std::cout << jac[j] << std::endl; } va = cti.m_normal * vel * m_dt; } @@ -293,25 +286,13 @@ void btDeformableContactProjection::setConstraints() const btScalar dn = btDot(vr, cti.m_normal); if (dn < SIMD_EPSILON) { - // find complementary jacobian - btVector3 complementaryDirection; - if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) - { - multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); - if (multibodyLinkCol) - { - complementaryDirection = generateUnitOrthogonalVector(cti.m_normal); - findJacobian(multibodyLinkCol, jacobianData_complementary, c.m_node->m_x, complementaryDirection); - } - } - if (m_constraints.find(c.m_node) == m_constraints.end()) { btAlignedObjectArray constraints; - constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); + constraints.push_back(DeformableContactConstraint(c)); m_constraints[c.m_node] = constraints; btAlignedObjectArray frictions; - frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); + frictions.push_back(DeformableFrictionConstraint()); m_frictions[c.m_node] = frictions; } else @@ -328,10 +309,9 @@ void btDeformableContactProjection::setConstraints() if (std::abs(std::abs(dot_prod) - 1) < angle_epsilon) { // group the constraints - constraints[j].append(c, jacobianData_normal); + constraints[j].append(c); // push in an empty friction frictions[j].append(); - frictions[j].addJacobian(complementaryDirection, jacobianData_complementary); merged = true; break; } @@ -340,8 +320,8 @@ void btDeformableContactProjection::setConstraints() // hard coded no more than 3 constraint directions if (!merged && constraints.size() < dim) { - constraints.push_back(DeformableContactConstraint(c, jacobianData_normal)); - frictions.push_back(DeformableFrictionConstraint(complementaryDirection, jacobianData_complementary)); + constraints.push_back(DeformableContactConstraint(c)); + frictions.push_back(DeformableFrictionConstraint()); } } } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index aa7af70fb..e73d8faa4 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -40,4 +40,4 @@ public: virtual void reinitialize(bool nodeUpdated); }; -#endif /* btContactProjection_h */ +#endif /* btDeformableContactProjection_h */ diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 216c1a24d..f9c9968c9 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -69,6 +69,43 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; + + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k]; + } + va = cti.m_normal * vel; + + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k]; + } + va += c->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k]; + } + va += c->t2 * vel; + } + } + else + { + // The object interacting with deformable node is not supported for position correction + btAssert(false); + } if (cti.m_colObj->hasContactResponse()) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index e8fd3da39..95249b093 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -26,7 +26,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" #include "btSparseSDF.h" #include "BulletCollision/BroadphaseCollision/btDbvt.h" - +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" //#ifdef BT_USE_DOUBLE_PRECISION //#define btRigidBodyData btRigidBodyDoubleData //#define btRigidBodyDataName "btRigidBodyDoubleData" @@ -300,6 +301,13 @@ public: btScalar m_c2; // ima*dt btScalar m_c3; // Friction btScalar m_c4; // Hardness + + // jacobians and unit impulse responses for multibody + btMultiBodyJacobianData jacobianData_normal; + btMultiBodyJacobianData jacobianData_t1; + btMultiBodyJacobianData jacobianData_t2; + btVector3 t1; + btVector3 t2; }; /* SContact */ struct SContact diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 9b3e45ea4..8cf0ed3c0 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -25,7 +25,41 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" #include "BulletCollision/CollisionShapes/btConvexInternalShape.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include //for memset +#include +static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, + btMultiBodyJacobianData& jacobianData, + const btVector3& contact_point, + const btVector3& dir) +{ + 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, contact_point, dir, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0], &jacobianData.m_deltaVelocitiesUnitImpulse[0], jacobianData.scratch_r, jacobianData.scratch_v); +} +static btVector3 generateUnitOrthogonalVector(const btVector3& u) +{ + btScalar ux = u.getX(); + btScalar uy = u.getY(); + btScalar uz = u.getZ(); + btScalar ax = std::abs(ux); + btScalar ay = std::abs(uy); + btScalar az = std::abs(uz); + btVector3 v; + if (ax <= ay && ax <= az) + v = btVector3(0, -uz, uy); + else if (ay <= ax && ay <= az) + v = btVector3(-uz, 0, ux); + else + v = btVector3(-uy, ux, 0); + v.normalize(); + return v; +} // // btSymMatrix // @@ -298,6 +332,46 @@ static inline btMatrix3x3 Diagonal(btScalar x) m[2] = btVector3(0, 0, x); return (m); } + +static inline btMatrix3x3 Diagonal(const btVector3& v) +{ + btMatrix3x3 m; + m[0] = btVector3(v.getX(), 0, 0); + m[1] = btVector3(0, v.getY(), 0); + m[2] = btVector3(0, 0, v.getZ()); + return (m); +} + +static inline btScalar Dot(const btScalar* a,const btScalar* b, int ndof) +{ + btScalar result = 0; + for (int i = 0; i < ndof; ++i) + result += a[i] * b[i]; + return result; +} + +static inline btMatrix3x3 OuterProduct(const btScalar* v1,const btScalar* v2,const btScalar* v3, + const btScalar* u1, const btScalar* u2, const btScalar* u3, int ndof) +{ + btMatrix3x3 m; + btScalar a11 = Dot(v1,u1,ndof); + btScalar a12 = Dot(v1,u2,ndof); + btScalar a13 = Dot(v1,u3,ndof); + + btScalar a21 = Dot(v2,u1,ndof); + btScalar a22 = Dot(v2,u2,ndof); + btScalar a23 = Dot(v2,u3,ndof); + + btScalar a31 = Dot(v3,u1,ndof); + btScalar a32 = Dot(v3,u2,ndof); + btScalar a33 = Dot(v3,u3,ndof); + m[0] = btVector3(a11, a12, a13); + m[1] = btVector3(a21, a22, a23); + m[2] = btVector3(a31, a32, a33); + return (m); +} + + // static inline btMatrix3x3 Add(const btMatrix3x3& a, const btMatrix3x3& b) @@ -879,21 +953,68 @@ struct btSoftColliders if (ms > 0) { psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti); - const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); - static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); - const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_q - wtr.getOrigin(); - const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + auto& cti = c.m_cti; c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); - c.m_c1 = ra; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); c.m_c2 = ima * psb->m_sst.sdt; - // c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_q - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + if (m_rigidBody) + m_rigidBody->activate(); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 normal = cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal, t1, t2); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btVector3 u_dot_J(0,0,0); + for (int i = 0; i < ndof; ++i) + { + u_dot_J += btVector3(J_n[i] * u_n[i], J_t1[i] * u_t1[i], J_t2[i] * u_t2[i]); + } + btVector3 impulse_matrix_diag; + btScalar dt = psb->m_sst.sdt; + impulse_matrix_diag.setX(1/((u_dot_J.getX() + n.m_im) * dt)); + impulse_matrix_diag.setY(1/((u_dot_J.getY() + n.m_im) * dt)); + impulse_matrix_diag.setZ(1/((u_dot_J.getZ() + n.m_im) * dt)); + btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + } + } psb->m_rcontacts.push_back(c); - if (m_rigidBody) - m_rigidBody->activate(); } } } -- cgit v1.2.1 From ec403f790d271af9b33eabaab07cd454eb1437c3 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 25 Jul 2019 13:51:44 -0700 Subject: factor out force; now btDeformableLagrangianceForce can be specified at configuration time and to specific softbody --- examples/DeformableContact/DeformableContact.cpp | 11 ++- examples/DeformableDemo/DeformableDemo.cpp | 7 +- examples/Pinch/Pinch.cpp | 9 +- .../VolumetricDeformable/VolumetricDeformable.cpp | 7 +- src/BulletSoftBody/btBackwardEulerObjective.h | 100 --------------------- src/BulletSoftBody/btCGProjection.h | 21 +---- .../btDeformableBackwardEulerObjective.cpp | 13 +-- .../btDeformableBackwardEulerObjective.h | 19 ++++ .../btDeformableContactProjection.cpp | 17 ++-- src/BulletSoftBody/btDeformableContactProjection.h | 4 +- src/BulletSoftBody/btDeformableGravityForce.h | 11 ++- src/BulletSoftBody/btDeformableLagrangianForce.h | 39 ++++---- src/BulletSoftBody/btDeformableMassSpringForce.h | 24 ++--- .../btDeformableRigidDynamicsWorld.cpp | 33 +++++-- .../btDeformableRigidDynamicsWorld.h | 2 + src/BulletSoftBody/btSoftBody.cpp | 10 +-- src/BulletSoftBody/btSoftBody.h | 2 +- src/BulletSoftBody/btSoftBodyInternals.h | 25 +++--- 18 files changed, 150 insertions(+), 204 deletions(-) delete mode 100644 src/BulletSoftBody/btBackwardEulerObjective.h diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp index f51978ae3..c40564867 100644 --- a/examples/DeformableContact/DeformableContact.cpp +++ b/examples/DeformableContact/DeformableContact.cpp @@ -127,8 +127,9 @@ void DeformableContact::initPhysics() 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); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { @@ -169,8 +170,8 @@ void DeformableContact::initPhysics() bool spherical = false; //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); + btVector3 linkHalfExtents(.4, 1, .4); + btVector3 baseHalfExtents(.4, 1, .4); btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 10.f,0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); @@ -228,6 +229,8 @@ void DeformableContact::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = .1; getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index aa58fbd6f..2411670cf 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -168,8 +168,9 @@ void DeformableDemo::initPhysics() 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); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; // getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -235,6 +236,8 @@ void DeformableDemo::initPhysics() psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 1; getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); // add a few rigid bodies Ctor_RbUpStack(1); diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp index cc3008f9c..4252a47b8 100644 --- a/examples/Pinch/Pinch.cpp +++ b/examples/Pinch/Pinch.cpp @@ -252,8 +252,9 @@ void Pinch::initPhysics() 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); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -336,12 +337,14 @@ void Pinch::initPhysics() // psb->translate(btVector3(-2.5, 4, -2.5)); // psb->getCollisionShape()->setMargin(0.1); // psb->setTotalMass(1); - psb->setSpringStiffness(4); + psb->setSpringStiffness(2); psb->setDampingCoefficient(0.02); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); // add a grippers createGrip(); } diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp index b30e9bc24..15fb712f6 100644 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -186,8 +186,9 @@ void VolumetricDeformable::initPhysics() 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); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { @@ -240,6 +241,8 @@ void VolumetricDeformable::initPhysics() 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; + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } // add a few rigid bodies Ctor_RbUpStack(4); diff --git a/src/BulletSoftBody/btBackwardEulerObjective.h b/src/BulletSoftBody/btBackwardEulerObjective.h deleted file mode 100644 index 16ec0a542..000000000 --- a/src/BulletSoftBody/btBackwardEulerObjective.h +++ /dev/null @@ -1,100 +0,0 @@ -// -// btDeformableBackwardEulerObjective.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" -#include "btDeformableContactProjection.h" -#include "btPreconditioner.h" -#include "btDeformableRigidDynamicsWorld.h" - -class btDeformableRigidDynamicsWorld; -class btDeformableBackwardEulerObjective -{ -public: - using TVStack = btAlignedObjectArray; - btScalar m_dt; - btDeformableRigidDynamicsWorld* m_world; - btAlignedObjectArray m_lf; - btAlignedObjectArray& m_softBodies; - Preconditioner* m_preconditioner; - btDeformableContactProjection projection; - const TVStack& m_backupVelocity; - - btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); - - virtual ~btDeformableBackwardEulerObjective() {} - - void initialize(){} - - // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual - void computeResidual(btScalar dt, TVStack& residual) const; - - // add explicit force to the velocity - void applyExplicitForce(TVStack& force); - - // 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; - - // set initial guess for CG solve - void initialGuess(TVStack& dv, const TVStack& residual); - - // reset data structure - void reinitialize(bool nodeUpdated); - - void setDt(btScalar dt); - - // enforce constraints in CG solve - void enforceConstraint(TVStack& x) - { - projection.enforceConstraint(x); - updateVelocity(x); - } - - // add dv to velocity - void updateVelocity(const TVStack& dv); - - //set constraints as projections - void setConstraints() - { - projection.setConstraints(); - } - - // update the projections and project the residual - void project(TVStack& r) - { - projection.update(); - // TODO rename - projection.project(r); - } - - // perform precondition M^(-1) x = b - void precondition(const TVStack& x, TVStack& b) - { - m_preconditioner->operator()(x,b); - } - - virtual void setWorld(btDeformableRigidDynamicsWorld* world) - { - m_world = world; - projection.setWorld(world); - } -}; - -#endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 928ee46f8..b4a887fa6 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -109,12 +109,13 @@ public: using TArrayStack = btAlignedObjectArray >; btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; - std::unordered_map m_indices; + const std::unordered_map* m_indices; const btScalar& m_dt; - btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) + btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) : m_softBodies(softBodies) , m_dt(dt) + , m_indices(indices) { } @@ -132,22 +133,6 @@ public: virtual void reinitialize(bool nodeUpdated) { - if (nodeUpdated) - updateId(); - } - - void updateId() - { - size_t index = 0; - m_indices.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_indices[&(psb->m_nodes[j])] = index++; - } - } } void setSoftBodies(btAlignedObjectArray softBodies) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 31b0905ac..acc9db75f 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -9,21 +9,22 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) -, projection(m_softBodies, m_dt) +, projection(m_softBodies, m_dt, &m_indices) , m_backupVelocity(backup_v) { // TODO: this should really be specified in initialization instead of here - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies); - btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0)); +// btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies); +// btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0)); m_preconditioner = new DefaultPreconditioner(); - m_lf.push_back(mass_spring); - m_lf.push_back(gravity); +// m_lf.push_back(mass_spring); +// m_lf.push_back(gravity); } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated) { if(nodeUpdated) { + updateId(); projection.setSoftBodies(m_softBodies); } for (int i = 0; i < m_lf.size(); ++i) @@ -69,7 +70,7 @@ void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) // only the velocity of the constrained nodes needs to be updated during CG solve for (auto it : projection.m_constraints) { - int i = projection.m_indices[it.first]; + int i = m_indices[it.first]; it.first->m_v = m_backupVelocity[i] + dv[i]; } } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index bfd4ca3af..e22b42b0b 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -28,6 +28,7 @@ public: Preconditioner* m_preconditioner; btDeformableContactProjection projection; const TVStack& m_backupVelocity; + std::unordered_map m_indices; btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); @@ -95,6 +96,24 @@ public: m_world = world; projection.setWorld(world); } + + virtual 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++; + } + } + } + + std::unordered_map* getIndices() + { + return &m_indices; + } }; #endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 1a9cad120..4d6ea4e84 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -88,26 +88,27 @@ void btDeformableContactProjection::update() const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; - // add in the normal component of the va btScalar vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k]; + vel += local_v[k] * J_n[k]; } va = cti.m_normal * vel * m_dt; + // add in the tangential components of the va vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k]; + vel += local_v[k] * J_t1[k]; } va += c->t1 * vel * m_dt; vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k]; + vel += local_v[k] * J_t2[k]; } va += c->t2 * vel * m_dt; } @@ -177,7 +178,7 @@ void btDeformableContactProjection::update() // the following is equivalent /* - btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices[c->m_node]]; + btVector3 dv = -impulse_normal * c->m_c2/m_dt + c->m_node->m_v - backupVelocity[m_indices->at(c->m_node)]; btScalar dvn = dv.dot(cti.m_normal); */ @@ -196,9 +197,11 @@ void btDeformableContactProjection::update() { if (multibodyLinkCol) { + // apply normal component of the impulse multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_normal, impulse.dot(cti.m_normal)); if (incremental_tangent.norm() > SIMD_EPSILON) { + // apply tangential component of the impulse const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV_t1, impulse.dot(c->t1)); const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; @@ -336,7 +339,7 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices[it.first]; + size_t i = m_indices->at(it.first); const btAlignedObjectArray& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); @@ -399,7 +402,7 @@ void btDeformableContactProjection::project(TVStack& x) for (auto& it : m_constraints) { const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices[it.first]; + size_t i = m_indices->at(it.first); btAlignedObjectArray& frictions = m_frictions[it.first]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index e73d8faa4..ea3b00f62 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -18,8 +18,8 @@ public: std::unordered_map > m_constraints; std::unordered_map > m_frictions; - btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) - : btCGProjection(softBodies, dt) + btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) + : btCGProjection(softBodies, dt, indices) { } diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index d8571fa73..398662e1f 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -16,7 +16,7 @@ public: using TVStack = btDeformableLagrangianForce::TVStack; btVector3 m_gravity; - btDeformableGravityForce(const btAlignedObjectArray& softBodies, const btVector3& g) : btDeformableLagrangianForce(softBodies), m_gravity(g) + btDeformableGravityForce(const btVector3& g) : m_gravity(g) { } @@ -39,14 +39,14 @@ public: virtual void addScaledGravityForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); - btAssert(numNodes == force.size()) + btAssert(numNodes <= force.size()) for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { btSoftBody::Node& n = psb->m_nodes[j]; - size_t id = m_indices[&n]; + size_t id = m_indices->at(&n); btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; btVector3 scaled_force = scale * m_gravity * mass; force[id] += scaled_force; @@ -54,7 +54,10 @@ public: } } - + virtual btDeformableLagrangianForceType getForceType() + { + return BT_GRAVITY_FORCE; + } }; diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index acb9a28ff..fa4184a14 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -10,16 +10,20 @@ #include "btSoftBody.h" #include +enum btDeformableLagrangianForceType +{ + BT_GRAVITY_FORCE = 1, + BT_MASSSPRING_FORCE = 2 +}; class btDeformableLagrangianForce { public: using TVStack = btAlignedObjectArray; - const btAlignedObjectArray& m_softBodies; - std::unordered_map m_indices; + btAlignedObjectArray m_softBodies; + const std::unordered_map* m_indices; - btDeformableLagrangianForce(const btAlignedObjectArray& softBodies) - : m_softBodies(softBodies) + btDeformableLagrangianForce() { } @@ -31,23 +35,10 @@ public: virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + virtual btDeformableLagrangianForceType getForceType() = 0; + virtual void reinitialize(bool nodeUpdated) { - if (nodeUpdated) - updateId(); - } - - virtual 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++; - } - } } virtual int getNumNodes() @@ -59,5 +50,15 @@ public: } return numNodes; } + + virtual void addSoftBody(btSoftBody* psb) + { + m_softBodies.push_back(psb); + } + + virtual void setIndices(const std::unordered_map* indices) + { + m_indices = indices; + } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 70ad6e69c..c9fca134c 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -14,15 +14,13 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce { public: using TVStack = btDeformableLagrangianForce::TVStack; - btDeformableMassSpringForce(const btAlignedObjectArray& softBodies) : btDeformableLagrangianForce(softBodies) + btDeformableMassSpringForce() { - } virtual void addScaledImplicitForce(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force); -// addScaledElasticForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -33,7 +31,7 @@ public: virtual void addScaledDampingForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); - btAssert(numNodes == force.size()) + btAssert(numNodes <= force.size()) for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; @@ -42,8 +40,8 @@ public: const auto& link = psb->m_links[j]; const auto node1 = link.m_n[0]; const auto node2 = link.m_n[1]; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; + size_t id1 = m_indices->at(node1); + size_t id2 = m_indices->at(node2); // damping force btVector3 v_diff = (node2->m_v - node1->m_v); @@ -58,7 +56,7 @@ public: virtual void addScaledElasticForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); - btAssert(numNodes == force.size()) + btAssert(numNodes <= force.size()) for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; @@ -69,8 +67,8 @@ public: const auto node2 = link.m_n[1]; btScalar kLST = link.Feature::m_material->m_kLST; btScalar r = link.m_rl; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; + size_t id1 = m_indices->at(node1); + size_t id2 = m_indices->at(node2); // elastic force // explicit elastic force @@ -95,8 +93,8 @@ public: const auto node1 = link.m_n[0]; const auto node2 = link.m_n[1]; btScalar k_damp = psb->m_dampingCoefficient; - size_t id1 = m_indices[node1]; - size_t id2 = m_indices[node2]; + size_t id1 = m_indices->at(node1); + size_t id2 = m_indices->at(node2); btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); df[id1] += local_scaled_df; df[id2] -= local_scaled_df; @@ -104,6 +102,10 @@ public: } } + virtual btDeformableLagrangianForceType getForceType() + { + return BT_MASSSPRING_FORCE; + } }; diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index f9c9968c9..b91d9d2c4 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -60,13 +60,12 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) if (c == nullptr || c->m_node->m_im == 0) continue; const btSoftBody::sCti& cti = c->m_cti; - btRigidBody* rigidCol = 0; btVector3 va(0, 0, 0); // grab the velocity of the rigid body if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) @@ -78,25 +77,25 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0]; const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0]; const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0]; - + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); // add in the normal component of the va btScalar vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_n[k]; + vel += local_v[k] * J_n[k]; } va = cti.m_normal * vel; vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t1[k]; + vel += local_v[k] * J_t1[k]; } va += c->t1 * vel; vel = 0.0; for (int k = 0; k < ndof; ++k) { - vel += multibodyLinkCol->m_multiBody->getVelocityVector()[k] * J_t2[k]; + vel += local_v[k] * J_t2[k]; } va += c->t2 * vel; } @@ -110,7 +109,6 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) if (cti.m_colObj->hasContactResponse()) { btScalar dp = cti.m_offset; - rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); if (friction.m_static[j] == true) { c->m_node->m_v = va; @@ -214,3 +212,24 @@ void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) m_beforeSolverCallbacks[i](m_internalTime, this); } + +void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) +{ + btAlignedObjectArray& forces = m_deformableBodySolver->m_objective->m_lf; + bool added = false; + for (int i = 0; i < forces.size(); ++i) + { + if (forces[i]->getForceType() == force->getForceType()) + { + forces[i]->addSoftBody(psb); + added = true; + break; + } + } + if (!added) + { + force->addSoftBody(psb); + force->setIndices(m_deformableBodySolver->m_objective->getIndices()); + forces.push_back(force); + } +} diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index f1be4fd85..c6b3dcbc6 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -126,6 +126,8 @@ public: void afterSolverCallbacks(btScalar timeStep); + void addForce(btSoftBody* psb, btDeformableLagrangianForce* force); + int getDrawFlags() const { return (m_drawFlags); } void setDrawFlags(int f) { m_drawFlags = f; } }; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index bd7587c47..c31bdb7ed 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2266,14 +2266,15 @@ btVector3 btSoftBody::evaluateCom() const bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, - btSoftBody::sCti& cti) const + btSoftBody::sCti& cti, bool predict) const { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg - const btTransform &wtr = tmpRigid ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); + const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); +// const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); // TODO: get the correct transform for multibody btScalar dst = @@ -2282,11 +2283,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, shp, nrm, margin); - if (dst < 0) + if (dst < 0 || !predict) { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; -// cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); cti.m_offset = dst; return (true); } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 95249b093..5e33d74d0 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1005,7 +1005,7 @@ public: btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; void updateNormals(); void updateBounds(); void updatePose(); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 8cf0ed3c0..2ec8c1b55 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -945,14 +945,17 @@ struct btSoftColliders if (!n.m_battach) { - if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + // check for collision at x_{n+1}^* + if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) +// if (psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false)); { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; const btScalar ms = ima + imb; if (ms > 0) { - psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti); + // resolve contact at x_n + psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); auto& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); @@ -982,8 +985,13 @@ struct btSoftColliders btVector3 t2 = btCross(normal, t1); btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); + + // findJacobian is hella expensive, avoid calling if possible + if (fc != 0) + { + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); + } btScalar* J_n = &jacobianData_normal.m_jacobians[0]; btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; @@ -995,16 +1003,7 @@ struct btSoftColliders btMatrix3x3 rot(normal, t1, t2); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btVector3 u_dot_J(0,0,0); - for (int i = 0; i < ndof; ++i) - { - u_dot_J += btVector3(J_n[i] * u_n[i], J_t1[i] * u_t1[i], J_t2[i] * u_t2[i]); - } - btVector3 impulse_matrix_diag; btScalar dt = psb->m_sst.sdt; - impulse_matrix_diag.setX(1/((u_dot_J.getX() + n.m_im) * dt)); - impulse_matrix_diag.setY(1/((u_dot_J.getY() + n.m_im) * dt)); - impulse_matrix_diag.setZ(1/((u_dot_J.getZ() + n.m_im) * dt)); btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); c.m_c0 = rot.transpose() * local_impulse_matrix * rot; c.jacobianData_normal = jacobianData_normal; -- cgit v1.2.1 From f1e7ce9ce14ce220c80a572c555eef0150f76df7 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 31 Jul 2019 20:40:22 -0700 Subject: add multibody interpolation transform so that collision detection is consistent with rigidbody --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 130 +++++++++++++++++++-- src/BulletDynamics/Featherstone/btMultiBody.h | 25 +++- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 22 +++- .../Featherstone/btMultiBodyDynamicsWorld.h | 4 +- src/BulletDynamics/Featherstone/btMultiBodyLink.h | 35 ++++-- .../btDeformableBackwardEulerObjective.cpp | 5 - .../btDeformableContactProjection.cpp | 2 +- .../btDeformableRigidDynamicsWorld.cpp | 2 +- src/BulletSoftBody/btSoftBody.cpp | 8 +- src/BulletSoftBody/btSoftBodyInternals.h | 9 +- 10 files changed, 196 insertions(+), 46 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 3e210d752..80ad19891 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -27,6 +27,7 @@ #include "btMultiBodyJointFeedback.h" #include "LinearMath/btTransformUtil.h" #include "LinearMath/btSerializer.h" +#include //#include "Bullet3Common/b3Logging.h" // #define INCLUDE_GYRO_TERM @@ -100,6 +101,8 @@ btMultiBody::btMultiBody(int n_links, m_baseName(0), m_basePos(0, 0, 0), m_baseQuat(0, 0, 0, 1), + m_basePos_interpolate(0, 0, 0), + m_baseQuat_interpolate(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -449,6 +452,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const return m_links[i].m_cachedRotParentToThis; } +const btVector3 &btMultiBody::getInterpolateRVector(int i) const +{ + return m_links[i].m_cachedRVector_interpolate; +} + +const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const +{ + return m_links[i].m_cachedRotParentToThis_interpolate; +} + btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const { btAssert(i >= -1); @@ -1581,17 +1594,37 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar //printf("]\n"); ///////////////// } +void btMultiBody::predictPositionsMultiDof(btScalar dt) +{ + stepPositionsMultiDof(dt, 0, 0, true); +} -void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) +void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd, bool predict) { int num_links = getNumLinks(); // step position by adding dt * velocity //btVector3 v = getBaseVel(); //m_basePos += dt * v; // - btScalar *pBasePos = (pq ? &pq[4] : m_basePos); - btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - // + btScalar *pBasePos; + btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + if (!predict) + { + pBasePos = (pq ? &pq[4] : m_basePos); + } // + else + { + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + } + + + pBasePos[0] += dt * pBaseVel[0]; pBasePos[1] += dt * pBaseVel[1]; pBasePos[2] += dt * pBaseVel[2]; @@ -1645,7 +1678,18 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat = pq ? pq : m_baseQuat; + btScalar *pBaseQuat; + if (!predict) + pBaseQuat = pq ? pq : m_baseQuat; + else + { + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; + } btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) // btQuaternion baseQuat; @@ -1670,7 +1714,12 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd // Finally we can update m_jointPos for each of the m_links for (int i = 0; i < num_links; ++i) { - btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]); + btScalar *pJointPos; + if (!predict) + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); + else + pJointPos = &m_links[i].m_jointPos_interpolate[0]; + btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); switch (m_links[i].m_jointType) @@ -1678,12 +1727,23 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd case btMultibodyLink::ePrismatic: case btMultibodyLink::eRevolute: { + //reset to current pos + if (predict) + { + pJointPos[0] = m_links[i].m_jointPos[0]; + } btScalar jointVel = pJointVel[0]; pJointPos[0] += dt * jointVel; break; } case btMultibodyLink::eSpherical: { + //reset to current pos + if (predict) + { + for (int i = 0; i < 4; ++i) + pJointPos[i] = m_links[i].m_jointPos[i]; + } btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); btQuaternion jointOri; @@ -1697,6 +1757,11 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } case btMultibodyLink::ePlanar: { + if (predict) + { + for (int i = 0; i < 3; ++i) + pJointPos[i] = m_links[i].m_jointPos[i]; + } pJointPos[0] += dt * getJointVelMultiDof(i)[0]; btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); @@ -1711,7 +1776,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } } - m_links[i].updateCacheMultiDof(pq); + m_links[i].updateCacheMultiDof(pq, predict); if (pq) pq += m_links[i].m_posVarCount; @@ -2006,6 +2071,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray &world_to_local, btAlignedObjectArray &local_origin) +{ + world_to_local.resize(getNumLinks() + 1); + local_origin.resize(getNumLinks() + 1); + + world_to_local[0] = getInterpolateWorldToBaseRot(); + local_origin[0] = getInterpolateBasePos(); + + if (getBaseCollider()) + { + btVector3 posr = local_origin[0]; + // float pos[4]={posr.x(),posr.y(),posr.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()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + getBaseCollider()->setInterpolationWorldTransform(tr); + } + + for (int k = 0; k < getNumLinks(); k++) + { + const int parent = getParent(k); + world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1]; + local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k))); + } + + for (int m = 0; m < getNumLinks(); m++) + { + btMultiBodyLinkCollider *col = getLink(m).m_collider; + if (col) + { + int link = col->m_link; + btAssert(link == m); + + int index = link + 1; + + btVector3 posr = local_origin[index]; + // float pos[4]={posr.x(),posr.y(),posr.z(),1}; + btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()}; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + + col->setInterpolationWorldTransform(tr); + } + } +} + int btMultiBody::calculateSerializeBufferSize() const { int sz = sizeof(btMultiBodyData); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index c0b0d003b..fe60af991 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -193,12 +193,24 @@ public: const btQuaternion &getWorldToBaseRot() const { return m_baseQuat; - } // rotates world vectors into base frame + } + + const btVector3 &getInterpolateBasePos() const + { + return m_basePos_interpolate; + } // in world frame + const btQuaternion &getInterpolateWorldToBaseRot() const + { + return m_baseQuat_interpolate; + } + + // rotates world vectors into base frame btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame void setBasePos(const btVector3 &pos) { m_basePos = pos; + m_basePos_interpolate = pos; } void setBaseWorldTransform(const btTransform &tr) @@ -224,6 +236,7 @@ public: void setWorldToBaseRot(const btQuaternion &rot) { m_baseQuat = rot; //m_baseQuat asumed to ba alias!? + m_baseQuat_interpolate = rot; } void setBaseOmega(const btVector3 &omega) { @@ -273,6 +286,8 @@ public: const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. + const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords + const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. // // transform vectors in local frame of link i to world frame (or vice versa) @@ -420,7 +435,10 @@ public: } // timestep the positions (given current velocities). - void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); + void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false); + + // predict the positions + void predictPositionsMultiDof(btScalar dt); // // contacts @@ -581,6 +599,7 @@ public: void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; void updateCollisionObjectWorldTransforms(btAlignedObjectArray & world_to_local, btAlignedObjectArray & local_origin); + void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray & world_to_local, btAlignedObjectArray & local_origin); virtual int calculateSerializeBufferSize() const; @@ -664,7 +683,9 @@ private: const char *m_baseName; //memory needs to be manager by user! btVector3 m_basePos; // position of COM of base (world frame) + btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame) btQuaternion m_baseQuat; // rotates world points into base frame + btQuaternion m_baseQuat_interpolate; btScalar m_baseMass; // mass of the base btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index d4a9a754f..d1ac5e26f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) m_multiBodies.remove(body); } +void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + integrateMultiBodyTransforms(timeStep, /*predict = */ true); + +} void btMultiBodyDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); @@ -778,8 +784,11 @@ void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverIn void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { btDiscreteDynamicsWorld::integrateTransforms(timeStep); + integrateMultiBodyTransforms(timeStep); +} - { +void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict) +{ BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies @@ -802,7 +811,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) int nLinks = bod->getNumLinks(); ///base + num m_links - + if (!predict) { if (!bod->isPosUpdated()) bod->stepPositionsMultiDof(timeStep); @@ -815,18 +824,21 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) bod->setPosUpdated(false); } } + else + bod->predictPositionsMultiDof(timeStep); m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); - - bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + if (predict) + bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + else + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); } else { bod->clearVelocities(); } } - } } void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 4f48f07d2..e82c17b31 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -96,7 +96,9 @@ public: virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); - + void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false); + + virtual void predictUnconstraintMotion(btScalar timeStep); virtual void debugDrawWorld(); virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 92d41dfac..e7966b852 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -111,6 +111,10 @@ struct btMultibodyLink btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. + + // predicted verstion + btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame. btVector3 m_appliedForce; // In WORLD frame btVector3 m_appliedTorque; // In WORLD frame @@ -119,6 +123,7 @@ struct btMultibodyLink btVector3 m_appliedConstraintTorque; // In WORLD frame btScalar m_jointPos[7]; + btScalar m_jointPos_interpolate[7]; //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. //It gets set to zero after each internal stepSimulation call @@ -186,44 +191,50 @@ struct btMultibodyLink } // routine to update m_cachedRotParentToThis and m_cachedRVector - void updateCacheMultiDof(btScalar *pq = 0) + void updateCacheMultiDof(btScalar *pq = 0, bool predict = false) { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - + btScalar *pJointPos; + + if (!predict) + pJointPos = (pq ? pq : &m_jointPos[0]); + else + pJointPos = &m_jointPos_interpolate[0]; + btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis; + btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector; switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); break; } case ePrismatic: { // m_cachedRotParentToThis never changes, so no need to update - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); break; } case eSpherical: { - m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } case ePlanar: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); break; } case eFixed: { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index acc9db75f..9ffd63a3c 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -12,12 +12,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned , projection(m_softBodies, m_dt, &m_indices) , m_backupVelocity(backup_v) { - // TODO: this should really be specified in initialization instead of here -// btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies); -// btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0)); m_preconditioner = new DefaultPreconditioner(); -// m_lf.push_back(mass_spring); -// m_lf.push_back(gravity); } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 4d6ea4e84..734fbb451 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -44,7 +44,7 @@ static btVector3 generateUnitOrthogonalVector(const btVector3& u) void btDeformableContactProjection::update() { ///solve rigid body constraints - m_world->getSolverInfo().m_numIterations = 10; + m_world->getSolverInfo().m_numIterations = 1; m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index b91d9d2c4..6ff5a9a41 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -161,7 +161,7 @@ void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collision void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); + btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); m_deformableBodySolver->predictMotion(float(timeStep)); } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c31bdb7ed..06e0b41d0 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2270,12 +2270,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, { btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); - const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); - + const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg - const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); -// const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); - // TODO: get the correct transform for multibody + const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); +// const btTransform &wtr = colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 2ec8c1b55..34302ccc4 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -985,13 +985,8 @@ struct btSoftColliders btVector3 t2 = btCross(normal, t1); btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal); - - // findJacobian is hella expensive, avoid calling if possible - if (fc != 0) - { - findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); - } + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); btScalar* J_n = &jacobianData_normal.m_jacobians[0]; btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; -- cgit v1.2.1 From 3dc8abcf36b03aeeedb7643a4fea9c238a59bfba Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 10:19:31 -0700 Subject: only call buildIslands once for multibody in each timestep --- .../CollisionDispatch/btSimulationIslandManager.cpp | 6 +++--- src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp | 8 ++++++-- src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h | 1 + src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 8 ++++++++ src/BulletSoftBody/btDeformableBackwardEulerObjective.h | 5 +---- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- 6 files changed, 20 insertions(+), 10 deletions(-) diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 11fa97cdd..57e1bb494 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -233,7 +233,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } -// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { if (colObj0->getActivationState() == ACTIVE_TAG || @@ -257,7 +257,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } -// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { @@ -278,7 +278,7 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisi // printf("error in island management\n"); } -// btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index d1ac5e26f..2e6dbc440 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -428,14 +428,18 @@ void btMultiBodyDynamicsWorld::forwardKinematics() void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { solveExternalForces(solverInfo); + buildIslands(); solveInternalConstraints(solverInfo); } +void btMultiBodyDynamicsWorld::buildIslands() +{ + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); +} + void btMultiBodyDynamicsWorld::solveInternalConstraints(btContactSolverInfo& solverInfo) { /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback); - m_solverMultiBodyIslandCallback->processConstraints(); m_constraintSolver->allSolved(solverInfo, m_debugDrawer); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index e82c17b31..04707bf2d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -116,6 +116,7 @@ public: virtual void solveExternalForces(btContactSolverInfo& solverInfo); virtual void solveInternalConstraints(btContactSolverInfo& solverInfo); + void buildIslands(); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 9ffd63a3c..7b084af0e 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -128,3 +128,11 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack } } } + +//set constraints as projections +void btDeformableBackwardEulerObjective::setConstraints() +{ + // build islands for multibody solve + m_world->btMultiBodyDynamicsWorld::buildIslands(); + projection.setConstraints(); +} diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index e22b42b0b..a736315b7 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -73,10 +73,7 @@ public: void updateVelocity(const TVStack& dv); //set constraints as projections - void setConstraints() - { - projection.setConstraints(); - } + void setConstraints(); // update the projections and project the residual void project(TVStack& r) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index e3009dcbb..a1616a460 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -31,7 +31,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) backupVelocity(); m_objective->computeResidual(solverdt, m_residual); -// m_objective->initialGuess(m_dv, m_residual); + computeStep(m_dv, m_residual); updateVelocity(); } -- cgit v1.2.1 From 54303e02b10f3fd5bb3624e0cc0ccdfc0de23016 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 13:21:06 -0700 Subject: perform position correction only when objects are penetrating --- src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 10 ++++++---- src/BulletSoftBody/btSoftBody.cpp | 9 +++++---- src/BulletSoftBody/btSoftBodyInternals.h | 1 - 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 6ff5a9a41..0b8807366 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -109,12 +109,14 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) if (cti.m_colObj->hasContactResponse()) { btScalar dp = cti.m_offset; - if (friction.m_static[j] == true) - { - c->m_node->m_v = va; - } + + // only perform position correction when penetrating if (dp < 0) { + if (friction.m_static[j] == true) + { + c->m_node->m_v = va; + } c->m_node->m_v -= dp * cti.m_normal / dt; } } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 06e0b41d0..8a62d31bb 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2271,9 +2271,9 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, btVector3 nrm; const btCollisionShape* shp = colObjWrap->getCollisionShape(); const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); - // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg + // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect + // but resolve contact at x_n const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform(); -// const btTransform &wtr = colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( @@ -2281,13 +2281,14 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, shp, nrm, margin); - if (dst < 0 || !predict) + if (!predict) { cti.m_colObj = colObjWrap->getCollisionObject(); cti.m_normal = wtr.getBasis() * nrm; cti.m_offset = dst; - return (true); } + if (dst < 0) + return true; return (false); } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 34302ccc4..61dff4234 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -947,7 +947,6 @@ struct btSoftColliders { // check for collision at x_{n+1}^* if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) -// if (psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false)); { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; -- cgit v1.2.1 From 8cc7cb59d7af86084018608968cf5f6e5c5b18dd Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 14:06:42 -0700 Subject: clean up examples --- examples/BasicDemo/BasicExample.cpp | 6 +++--- examples/ExampleBrowser/ExampleEntries.cpp | 13 +++++++------ src/BulletDynamics/Featherstone/btMultiBody.cpp | 1 - src/BulletSoftBody/btDeformableBodySolver.h | 2 +- src/BulletSoftBody/btDeformableContactProjection.h | 1 - src/BulletSoftBody/btSoftBodyInternals.h | 1 - 6 files changed, 11 insertions(+), 13 deletions(-) diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp index 136aefb20..67f670d07 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 1 -#define ARRAY_SIZE_X 1 -#define ARRAY_SIZE_Z 1 +#define ARRAY_SIZE_Y 5 +#define ARRAY_SIZE_X 5 +#define ARRAY_SIZE_Z 5 #include "LinearMath/btVector3.h" #include "LinearMath/btAlignedObjectArray.h" diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 1bbadd941..f186f642a 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -122,12 +122,6 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Basic Example", "Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc), 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-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(0, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), 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), @@ -201,6 +195,13 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Spheres & Plane C-API (Bullet2)", "Collision C-API using Bullet 2.x backend", CollisionTutorialBullet2CreateFunc, TUT_SPHERE_PLANE_BULLET2), //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), + ExampleEntry(0, "Deformabe Body"), + ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableCreateFunc), + ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), + ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), + ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableContactCreateFunc), + // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), + #ifdef INCLUDE_CLOTH_DEMOS ExampleEntry(0, "Soft Body"), ExampleEntry(1, "Cloth", "Simulate a patch of cloth.", SoftDemoCreateFunc, 0), diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 80ad19891..1857bd55f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -27,7 +27,6 @@ #include "btMultiBodyJointFeedback.h" #include "LinearMath/btTransformUtil.h" #include "LinearMath/btSerializer.h" -#include //#include "Bullet3Common/b3Logging.h" // #define INCLUDE_GYRO_TERM diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 0b4a4819f..d721869a1 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -8,7 +8,7 @@ #ifndef BT_DEFORMABLE_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H -#include + #include "btSoftBodySolvers.h" #include "btDeformableBackwardEulerObjective.h" #include "btDeformableRigidDynamicsWorld.h" diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index ea3b00f62..2ca571195 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -11,7 +11,6 @@ #include "btSoftBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -#include class btDeformableContactProjection : public btCGProjection { public: diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 61dff4234..5756fffc7 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -28,7 +28,6 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include //for memset -#include static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, -- cgit v1.2.1 From 753b2d9f156bfa7fcf35be0da3c3fbc0b9c85ee3 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 15:02:15 -0700 Subject: add new demos to CMakeList --- examples/ExampleBrowser/CMakeLists.txt | 8 ++++++++ examples/ExampleBrowser/premake4.lua | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index a04e9f9b4..cfae5fcc5 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -359,6 +359,14 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/MultiBodyConstraintFeedback.cpp ../SoftDemo/SoftDemo.cpp ../SoftDemo/SoftDemo.h + ../Pinch/Pinch.cpp + ../Pinch/Pinch.h + ../DeformableContact/DeformableContact.cpp + ../DeformableContact/DeformableContact.h + ../DeformableDemo/DeformableDemo.cpp + ../DeformableDemo/DeformableDemo.h + ../VolumetricDeformable/VolumetricDeformable.cpp + ../VolumetricDeformable/VolumetricDeformable.h ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index b976136df..ff81b34b7 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -188,6 +188,10 @@ project "App_BulletExampleBrowser" "../RenderingExamples/*", "../VoronoiFracture/*", "../SoftDemo/*", + "../DeformableDemo/*", + "../DeformableContact/*", + "../VolumetricDeformable/*", + "../Pinch/*", "../RollingFrictionDemo/*", "../rbdl/*", "../FractureDemo/*", -- cgit v1.2.1 From f624b60c198ca4f39f349ece1230ba5ce60ee7e6 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 15:19:37 -0700 Subject: get rid of auto --- .../btDeformableBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btDeformableContactProjection.cpp | 1 + src/BulletSoftBody/btDeformableMassSpringForce.h | 18 +++++++++--------- src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 2 +- src/BulletSoftBody/btSoftBodyInternals.h | 3 ++- 5 files changed, 14 insertions(+), 12 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 7b084af0e..a9b77e368 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -47,7 +47,7 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - const auto& node = psb->m_nodes[j]; + const btSoftBody::Node& node = psb->m_nodes[j]; b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; ++counter; } diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 734fbb451..4ae07290c 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -8,6 +8,7 @@ #include "btDeformableContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" #include +#include static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index c9fca134c..549985321 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -37,9 +37,9 @@ public: 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]; + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; size_t id1 = m_indices->at(node1); size_t id2 = m_indices->at(node2); @@ -62,9 +62,9 @@ public: 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]; + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; btScalar kLST = link.Feature::m_material->m_kLST; btScalar r = link.m_rl; size_t id1 = m_indices->at(node1); @@ -89,9 +89,9 @@ public: 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]; + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; btScalar k_damp = psb->m_dampingCoefficient; size_t id1 = m_indices->at(node1); size_t id2 = m_indices->at(node2); diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 0b8807366..78c5f24a9 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -136,7 +136,7 @@ void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) btSoftBody* psb = m_softBodies[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - auto& node = psb->m_nodes[j]; + btSoftBody::Node& node = psb->m_nodes[j]; node.m_x = node.m_q + dt * node.m_v; } } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 5756fffc7..9c732250b 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -28,6 +28,7 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include //for memset +#include static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, @@ -954,7 +955,7 @@ struct btSoftColliders { // resolve contact at x_n psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); - auto& cti = c.m_cti; + btSoftBody::sCti& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); c.m_c2 = ima * psb->m_sst.sdt; -- cgit v1.2.1 From 7f33d8cdb9acde79144030544c7f039403c113c6 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 15:27:10 -0700 Subject: get rid of 'using' --- src/BulletSoftBody/btCGProjection.h | 9 ++++++--- src/BulletSoftBody/btConjugateGradient.h | 3 ++- src/BulletSoftBody/btDeformableBackwardEulerObjective.h | 3 ++- src/BulletSoftBody/btDeformableBodySolver.h | 3 ++- src/BulletSoftBody/btDeformableGravityForce.h | 3 ++- src/BulletSoftBody/btDeformableLagrangianForce.h | 3 ++- src/BulletSoftBody/btDeformableMassSpringForce.h | 3 ++- src/BulletSoftBody/btDeformableRigidDynamicsWorld.h | 3 ++- src/BulletSoftBody/btPreconditioner.h | 3 ++- 9 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index b4a887fa6..ca7417f8f 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -104,9 +104,12 @@ class btCGProjection { public: // static const int dim = 3; - using TVStack = btAlignedObjectArray; - using TVArrayStack = btAlignedObjectArray >; - using TArrayStack = btAlignedObjectArray >; + typedef btAlignedObjectArray TVStack; + typedef btAlignedObjectArray > TVArrayStack; + typedef btAlignedObjectArray > TArrayStack; +// using TVStack = btAlignedObjectArray; +// using TVArrayStack = btAlignedObjectArray >; +// using TArrayStack = btAlignedObjectArray >; btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; const std::unordered_map* m_indices; diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 0c8384a00..fdef6b22d 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -15,7 +15,8 @@ template class btConjugateGradient { - using TVStack = btAlignedObjectArray; +// using TVStack = btAlignedObjectArray; + typedef btAlignedObjectArray TVStack; TVStack r,p,z,temp; int max_iterations; diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index a736315b7..22a685631 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -20,7 +20,8 @@ class btDeformableRigidDynamicsWorld; class btDeformableBackwardEulerObjective { public: - using TVStack = btAlignedObjectArray; +// using TVStack = btAlignedObjectArray; + typedef btAlignedObjectArray TVStack; btScalar m_dt; btDeformableRigidDynamicsWorld* m_world; btAlignedObjectArray m_lf; diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index d721869a1..5528d8f84 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -21,7 +21,8 @@ class btDeformableRigidDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { - using TVStack = btAlignedObjectArray; +// using TVStack = btAlignedObjectArray; + typedef btAlignedObjectArray TVStack; protected: int m_numNodes; TVStack m_dv; diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 398662e1f..88d9107e1 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -13,7 +13,8 @@ class btDeformableGravityForce : public btDeformableLagrangianForce { public: - using TVStack = btDeformableLagrangianForce::TVStack; +// using TVStack = btDeformableLagrangianForce::TVStack; + typedef btAlignedObjectArray TVStack; btVector3 m_gravity; btDeformableGravityForce(const btVector3& g) : m_gravity(g) diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index fa4184a14..ff75eeb1c 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -19,7 +19,8 @@ enum btDeformableLagrangianForceType class btDeformableLagrangianForce { public: - using TVStack = btAlignedObjectArray; +// using TVStack = btAlignedObjectArray; + typedef btAlignedObjectArray TVStack; btAlignedObjectArray m_softBodies; const std::unordered_map* m_indices; diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 549985321..d3ccd70dc 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -13,7 +13,8 @@ class btDeformableMassSpringForce : public btDeformableLagrangianForce { public: - using TVStack = btDeformableLagrangianForce::TVStack; +// using TVStack = btDeformableLagrangianForce::TVStack; + typedef btAlignedObjectArray TVStack; btDeformableMassSpringForce() { } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index c6b3dcbc6..779a88ee3 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -30,7 +30,8 @@ typedef btAlignedObjectArray btSoftBodyArray; class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld { - using TVStack = btAlignedObjectArray; + typedef btAlignedObjectArray TVStack; +// using TVStack = btAlignedObjectArray; ///Solver classes that encapsulate multiple deformable bodies for solving btDeformableBodySolver* m_deformableBodySolver; btSoftBodyArray m_softBodies; diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index ad190a3a8..97cec43c5 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -11,7 +11,8 @@ class Preconditioner { public: - using TVStack = btAlignedObjectArray; + typedef btAlignedObjectArray TVStack; +// using TVStack = btAlignedObjectArray; virtual void operator()(const TVStack& x, TVStack& b) = 0; virtual void reinitialize(bool nodeUpdated) = 0; }; -- cgit v1.2.1 From 9a5ef6c8494668c54ced7a523a07934678049cf3 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 15:41:10 -0700 Subject: update CMakeList --- src/BulletSoftBody/CMakeLists.txt | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index d43df1c67..ec5fd447e 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -17,6 +17,11 @@ SET(BulletSoftBody_SRCS btSoftSoftCollisionAlgorithm.cpp btDefaultSoftBodySolver.cpp + btDeformableBackwardEulerObjective.cpp + btDeformableBodySolver.cpp + btDeformableContactProjection.cpp + btDeformableRigidDynamicsWorld.cpp + ) SET(BulletSoftBody_HDRS @@ -33,6 +38,18 @@ SET(BulletSoftBody_HDRS btSoftBodySolvers.h btDefaultSoftBodySolver.h + + btCGrojection.h + btConjugateGradient.h + btDeformableGravityForce.h + btDeformableMassSpringForce.h + btDeformableLagrangianForce.h + btPreconditioner.h + + btDeformableBackwardEulerObjective.h + btDeformableBodySolver.h + btDeformableContactProjection.h + btDeformableRigidDynamicsWorld.h btSoftBodySolverVertexBuffer.h ) -- cgit v1.2.1 From dae230912b2ddc69f8ce5ec813fa6178e1269449 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 15:51:38 -0700 Subject: typo fix --- src/BulletSoftBody/CMakeLists.txt | 2 +- src/BulletSoftBody/btSoftBodyInternals.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index ec5fd447e..40155786e 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -39,7 +39,7 @@ SET(BulletSoftBody_HDRS btSoftBodySolvers.h btDefaultSoftBodySolver.h - btCGrojection.h + btCGProjection.h btConjugateGradient.h btDeformableGravityForce.h btDeformableMassSpringForce.h diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 9c732250b..77bcdc5d8 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -28,7 +28,7 @@ subject to the following restrictions: #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" #include //for memset -#include +#include static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, -- cgit v1.2.1 From 8c04a78c9b84bfd13235c1a1333c70f32d231059 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 17:46:26 -0700 Subject: switch from std::unordered_map to btHashMap --- src/BulletSoftBody/btCGProjection.h | 14 +++----- .../btDeformableBackwardEulerObjective.cpp | 9 ++--- .../btDeformableBackwardEulerObjective.h | 13 +++++--- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.h | 1 + .../btDeformableContactProjection.cpp | 38 +++++++++++----------- src/BulletSoftBody/btDeformableContactProjection.h | 13 +++++--- src/BulletSoftBody/btDeformableGravityForce.h | 2 +- src/BulletSoftBody/btDeformableLagrangianForce.h | 9 ++--- src/BulletSoftBody/btDeformableMassSpringForce.h | 12 +++---- .../btDeformableRigidDynamicsWorld.cpp | 7 ++-- src/BulletSoftBody/btSoftBody.h | 1 + src/BulletSoftBody/btSoftBodyInternals.h | 4 ++- 13 files changed, 68 insertions(+), 57 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index ca7417f8f..144208908 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -10,7 +10,6 @@ #include "btSoftBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" -#include class btDeformableRigidDynamicsWorld; @@ -21,6 +20,7 @@ struct DeformableContactConstraint btAlignedObjectArray m_value; // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve btAlignedObjectArray m_accumulated_normal_impulse; + btSoftBody::Node* node; DeformableContactConstraint(const btSoftBody::RContact& rcontact) { @@ -58,7 +58,6 @@ struct DeformableContactConstraint struct DeformableFrictionConstraint { - btAlignedObjectArray m_static; // whether the friction is static btAlignedObjectArray m_impulse; // the impulse magnitude the node feels btAlignedObjectArray m_dv; // the dv magnitude of the node @@ -75,6 +74,7 @@ struct DeformableFrictionConstraint // the total impulse the node applied to the rb in the tangential direction in the cg solve btAlignedObjectArray m_accumulated_tangent_impulse; + btSoftBody::Node* node; DeformableFrictionConstraint() { @@ -103,22 +103,18 @@ struct DeformableFrictionConstraint class btCGProjection { public: -// static const int dim = 3; typedef btAlignedObjectArray TVStack; typedef btAlignedObjectArray > TVArrayStack; typedef btAlignedObjectArray > TArrayStack; -// using TVStack = btAlignedObjectArray; -// using TVArrayStack = btAlignedObjectArray >; -// using TArrayStack = btAlignedObjectArray >; btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; - const std::unordered_map* m_indices; + const btAlignedObjectArray* m_nodes; const btScalar& m_dt; - btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) + btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const btAlignedObjectArray* nodes) : m_softBodies(softBodies) , m_dt(dt) - , m_indices(indices) + , m_nodes(nodes) { } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index a9b77e368..50db01ec9 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -9,7 +9,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) -, projection(m_softBodies, m_dt, &m_indices) +, projection(m_softBodies, m_dt, &m_nodes) , m_backupVelocity(backup_v) { m_preconditioner = new DefaultPreconditioner(); @@ -63,10 +63,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { // only the velocity of the constrained nodes needs to be updated during CG solve - for (auto it : projection.m_constraints) +// for (auto it : projection.m_constraints) + for (int i = 0; i < projection.m_constraints.size(); ++i) { - int i = m_indices[it.first]; - it.first->m_v = m_backupVelocity[i] + dv[i]; + int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); + m_nodes[index]->m_v = m_backupVelocity[index] + dv[index]; } } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 22a685631..42d85f538 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -29,7 +29,7 @@ public: Preconditioner* m_preconditioner; btDeformableContactProjection projection; const TVStack& m_backupVelocity; - std::unordered_map m_indices; + btAlignedObjectArray m_nodes; btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); @@ -97,20 +97,23 @@ public: virtual void updateId() { - size_t index = 0; + size_t id = 0; + m_nodes.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_indices[&(psb->m_nodes[j])] = index++; + psb->m_nodes[j].index = id; + m_nodes.push_back(&psb->m_nodes[j]); + ++id; } } } - std::unordered_map* getIndices() + const btAlignedObjectArray* getIndices() const { - return &m_indices; + return &m_nodes; } }; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index a1616a460..4be5d4680 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -50,6 +50,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray cg; + public: btDeformableBackwardEulerObjective* m_objective; diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 4ae07290c..5a3f9e555 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -8,7 +8,7 @@ #include "btDeformableContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" #include -#include +#include static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol, btMultiBodyJacobianData& jacobianData, const btVector3& contact_point, @@ -49,10 +49,10 @@ void btDeformableContactProjection::update() m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo()); // loop through constraints to set constrained values - for (auto& it : m_constraints) + for (int index = 0; index < m_constraints.size(); ++index) { - btAlignedObjectArray& frictions = m_frictions[it.first]; - btAlignedObjectArray& constraints = it.second; + btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; + btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); for (int i = 0; i < constraints.size(); ++i) { DeformableContactConstraint& constraint = constraints[i]; @@ -230,13 +230,13 @@ void btDeformableContactProjection::setConstraints() c.push_back(DeformableContactConstraint(btVector3(1,0,0))); c.push_back(DeformableContactConstraint(btVector3(0,1,0))); c.push_back(DeformableContactConstraint(btVector3(0,0,1))); - m_constraints[&(psb->m_nodes[j])] = c; + m_constraints.insert(psb->m_nodes[j].index, c); btAlignedObjectArray f; f.push_back(DeformableFrictionConstraint()); f.push_back(DeformableFrictionConstraint()); f.push_back(DeformableFrictionConstraint()); - m_frictions[&(psb->m_nodes[j])] = f; + m_frictions.insert(psb->m_nodes[j].index, f); } } } @@ -290,22 +290,22 @@ void btDeformableContactProjection::setConstraints() const btScalar dn = btDot(vr, cti.m_normal); if (dn < SIMD_EPSILON) { - if (m_constraints.find(c.m_node) == m_constraints.end()) + if (m_constraints.find(c.m_node->index) == NULL) { btAlignedObjectArray constraints; constraints.push_back(DeformableContactConstraint(c)); - m_constraints[c.m_node] = constraints; + m_constraints.insert(c.m_node->index,constraints); btAlignedObjectArray frictions; frictions.push_back(DeformableFrictionConstraint()); - m_frictions[c.m_node] = frictions; + m_frictions.insert(c.m_node->index,frictions); } else { // group colinear constraints into one const btScalar angle_epsilon = 0.015192247; // less than 10 degree bool merged = false; - btAlignedObjectArray& constraints = m_constraints[c.m_node]; - btAlignedObjectArray& frictions = m_frictions[c.m_node]; + btAlignedObjectArray& constraints = *m_constraints[c.m_node->index]; + btAlignedObjectArray& frictions = *m_frictions[c.m_node->index]; for (int j = 0; j < constraints.size(); ++j) { const btAlignedObjectArray& dirs = constraints[j].m_direction; @@ -337,11 +337,11 @@ void btDeformableContactProjection::setConstraints() void btDeformableContactProjection::enforceConstraint(TVStack& x) { const int dim = 3; - for (auto& it : m_constraints) + for (int index = 0; index < m_constraints.size(); ++index) { - const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices->at(it.first); - const btAlignedObjectArray& frictions = m_frictions[it.first]; + const btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); + const btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) @@ -400,11 +400,11 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) void btDeformableContactProjection::project(TVStack& x) { const int dim = 3; - for (auto& it : m_constraints) + for (int index = 0; index < m_constraints.size(); ++index) { - const btAlignedObjectArray& constraints = it.second; - size_t i = m_indices->at(it.first); - btAlignedObjectArray& frictions = m_frictions[it.first]; + const btAlignedObjectArray& constraints = *m_constraints.getAtIndex(index); + size_t i = m_constraints.getKeyAtIndex(index).getUid1(); + btAlignedObjectArray& frictions = *m_frictions[m_constraints.getKeyAtIndex(index)]; btAssert(constraints.size() <= dim); btAssert(constraints.size() > 0); if (constraints.size() == 1) diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 2ca571195..9e61fed34 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -11,14 +11,19 @@ #include "btSoftBody.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" +#include "LinearMath/btHashMap.h" class btDeformableContactProjection : public btCGProjection { public: - std::unordered_map > m_constraints; - std::unordered_map > m_frictions; +// std::unordered_map > m_constraints; +// std::unordered_map > m_frictions; - btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const std::unordered_map* indices) - : btCGProjection(softBodies, dt, indices) + // map from node index to constraints + btHashMap > m_constraints; + btHashMap >m_frictions; + + btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const btAlignedObjectArray* nodes) + : btCGProjection(softBodies, dt, nodes) { } diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 88d9107e1..0c2ae535d 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -47,7 +47,7 @@ public: for (int j = 0; j < psb->m_nodes.size(); ++j) { btSoftBody::Node& n = psb->m_nodes[j]; - size_t id = m_indices->at(&n); + size_t id = n.index; btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; btVector3 scaled_force = scale * m_gravity * mass; force[id] += scaled_force; diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index ff75eeb1c..6e1c54352 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -9,7 +9,8 @@ #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H #include "btSoftBody.h" -#include +#include + enum btDeformableLagrangianForceType { BT_GRAVITY_FORCE = 1, @@ -22,7 +23,7 @@ public: // using TVStack = btAlignedObjectArray; typedef btAlignedObjectArray TVStack; btAlignedObjectArray m_softBodies; - const std::unordered_map* m_indices; + const btAlignedObjectArray* m_nodes; btDeformableLagrangianForce() { @@ -57,9 +58,9 @@ public: m_softBodies.push_back(psb); } - virtual void setIndices(const std::unordered_map* indices) + virtual void setIndices(const btAlignedObjectArray* nodes) { - m_indices = indices; + m_nodes = nodes; } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index d3ccd70dc..841101c8f 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -41,8 +41,8 @@ public: const btSoftBody::Link& link = psb->m_links[j]; btSoftBody::Node* node1 = link.m_n[0]; btSoftBody::Node* node2 = link.m_n[1]; - size_t id1 = m_indices->at(node1); - size_t id2 = m_indices->at(node2); + size_t id1 = node1->index; + size_t id2 = node2->index; // damping force btVector3 v_diff = (node2->m_v - node1->m_v); @@ -68,8 +68,8 @@ public: btSoftBody::Node* node2 = link.m_n[1]; btScalar kLST = link.Feature::m_material->m_kLST; btScalar r = link.m_rl; - size_t id1 = m_indices->at(node1); - size_t id2 = m_indices->at(node2); + size_t id1 = node1->index; + size_t id2 = node2->index; // elastic force // explicit elastic force @@ -94,8 +94,8 @@ public: btSoftBody::Node* node1 = link.m_n[0]; btSoftBody::Node* node2 = link.m_n[1]; btScalar k_damp = psb->m_dampingCoefficient; - size_t id1 = m_indices->at(node1); - size_t id2 = m_indices->at(node2); + size_t id1 = node1->index; + size_t id2 = node2->index; btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); df[id1] += local_scaled_df; df[id2] -= local_scaled_df; diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 78c5f24a9..6e1ed75bd 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -45,10 +45,11 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) { // perform position correction for all constraints - for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints) +// for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints) + for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) { - btAlignedObjectArray& frictions = m_deformableBodySolver->m_objective->projection.m_frictions[it.first]; - btAlignedObjectArray& constraints = it.second; + btAlignedObjectArray& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)]; + btAlignedObjectArray& constraints = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index); for (int i = 0; i < constraints.size(); ++i) { DeformableContactConstraint& constraint = constraints[i]; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 5e33d74d0..87633a83c 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -258,6 +258,7 @@ public: btScalar m_area; // Area btDbvtNode* m_leaf; // Leaf data int m_battach : 1; // Attached + int index; }; /* Link */ ATTRIBUTE_ALIGNED16(struct) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 77bcdc5d8..eb487a56f 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -995,7 +995,9 @@ struct btSoftColliders btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; - btMatrix3x3 rot(normal, t1, t2); // world frame to local frame + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; btScalar dt = psb->m_sst.sdt; btMatrix3x3 local_impulse_matrix = Diagonal(1/dt) * (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); -- cgit v1.2.1 From 021cbb2a0ea0e5e7a4b19908bfd8663b2300b82a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 2 Aug 2019 23:50:15 -0700 Subject: include numeric_limits --- src/BulletSoftBody/btDeformableBodySolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 4be5d4680..df12c213a 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -6,6 +6,7 @@ // #include +#include #include "btDeformableBodySolver.h" btDeformableBodySolver::btDeformableBodySolver() -- cgit v1.2.1 From c5d84c1a0b5f1120f0a392b8894b0d685193864b Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 3 Aug 2019 00:10:36 -0700 Subject: get rid of nullptr and std::function --- examples/Pinch/Pinch.cpp | 2 +- src/BulletSoftBody/btCGProjection.h | 4 ++-- src/BulletSoftBody/btDeformableContactProjection.cpp | 2 +- .../btDeformableRigidDynamicsWorld.cpp | 20 +++++++++++++++----- src/BulletSoftBody/btDeformableRigidDynamicsWorld.h | 10 +++++++++- 5 files changed, 28 insertions(+), 10 deletions(-) diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp index 4252a47b8..1e4afb748 100644 --- a/examples/Pinch/Pinch.cpp +++ b/examples/Pinch/Pinch.cpp @@ -256,7 +256,7 @@ void Pinch::initPhysics() m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - getDeformableDynamicsWorld()->m_beforeSolverCallbacks.push_back(dynamics); + getDeformableDynamicsWorld()->setSolverCallback(dynamics); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //create a ground diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 144208908..e11ecf817 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -29,7 +29,7 @@ struct DeformableContactConstraint DeformableContactConstraint(const btVector3 dir) { - m_contact.push_back(nullptr); + m_contact.push_back(NULL); m_direction.push_back(dir); m_value.push_back(0); m_accumulated_normal_impulse.push_back(0); @@ -37,7 +37,7 @@ struct DeformableContactConstraint DeformableContactConstraint() { - m_contact.push_back(nullptr); + m_contact.push_back(NULL); m_direction.push_back(btVector3(0,0,0)); m_value.push_back(0); m_accumulated_normal_impulse.push_back(0); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 5a3f9e555..edee3126e 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -59,7 +59,7 @@ void btDeformableContactProjection::update() DeformableFrictionConstraint& friction = frictions[i]; for (int j = 0; j < constraint.m_contact.size(); ++j) { - if (constraint.m_contact[j] == nullptr) + if (constraint.m_contact[j] == NULL) { // nothing needs to be done for dirichelet constraints continue; diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 6e1ed75bd..de39778ad 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -58,7 +58,7 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) { const btSoftBody::RContact* c = constraint.m_contact[j]; // skip anchor points - if (c == nullptr || c->m_node->m_im == 0) + if (c == NULL || c->m_node->m_im == 0) continue; const btSoftBody::sCti& cti = c->m_cti; btVector3 va(0, 0, 0); @@ -206,14 +206,24 @@ void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) { (*m_internalTickCallback)(this, timeStep); } - for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) - m_beforeSolverCallbacks[i](m_internalTime, this); + + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } + +// for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) +// m_beforeSolverCallbacks[i](m_internalTime, this); } void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) { - for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) - m_beforeSolverCallbacks[i](m_internalTime, this); + if (0 != m_solverCallback) + { + (*m_solverCallback)(m_internalTime, this); + } +// for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) +// m_beforeSolverCallbacks[i](m_internalTime, this); } void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 779a88ee3..47284e7fb 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -75,7 +75,15 @@ public: m_sbi.m_sparsesdf.Initialize(); m_internalTime = 0.0; } - btAlignedObjectArray > m_beforeSolverCallbacks; +// btAlignedObjectArray > m_beforeSolverCallbacks; + typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world); + btSolverCallback m_solverCallback; + + void setSolverCallback(btSolverCallback cb) + { + m_solverCallback = cb; + } + virtual ~btDeformableRigidDynamicsWorld() { } -- cgit v1.2.1 From 02d3a9469f7431d9e4e120eafcf7ccba76596e6a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 5 Aug 2019 11:54:17 -0700 Subject: code clean up + Zlib copyright header --- src/BulletSoftBody/btCGProjection.h | 19 ++++++++------ src/BulletSoftBody/btConjugateGradient.h | 22 ++++++++++------ .../btDeformableBackwardEulerObjective.cpp | 21 ++++++++++------ .../btDeformableBackwardEulerObjective.h | 18 +++++++++----- src/BulletSoftBody/btDeformableBodySolver.cpp | 22 ++++++++++------ src/BulletSoftBody/btDeformableBodySolver.h | 20 +++++++++------ .../btDeformableContactProjection.cpp | 18 +++++++++----- src/BulletSoftBody/btDeformableContactProjection.h | 21 +++++++++------- src/BulletSoftBody/btDeformableGravityForce.h | 21 +++++++++------- src/BulletSoftBody/btDeformableLagrangianForce.h | 18 +++++++++----- src/BulletSoftBody/btDeformableMassSpringForce.h | 18 +++++++++----- .../btDeformableRigidDynamicsWorld.cpp | 29 +++++++++++----------- .../btDeformableRigidDynamicsWorld.h | 8 +++--- src/BulletSoftBody/btPreconditioner.h | 18 +++++++++----- 14 files changed, 170 insertions(+), 103 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index e11ecf817..667c715c0 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -1,8 +1,15 @@ -// btCGProjection.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/4/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_CG_PROJECTION_H #define BT_CG_PROJECTION_H @@ -20,7 +27,6 @@ struct DeformableContactConstraint btAlignedObjectArray m_value; // the magnitude of the total impulse the node applied to the rb in the normal direction in the cg solve btAlignedObjectArray m_accumulated_normal_impulse; - btSoftBody::Node* node; DeformableContactConstraint(const btSoftBody::RContact& rcontact) { @@ -74,7 +80,6 @@ struct DeformableFrictionConstraint // the total impulse the node applied to the rb in the tangential direction in the cg solve btAlignedObjectArray m_accumulated_tangent_impulse; - btSoftBody::Node* node; DeformableFrictionConstraint() { diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index fdef6b22d..dfec3dde5 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -1,9 +1,15 @@ -// -// btConjugateGradient.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/1/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_CONJUGATE_GRADIENT_H #define BT_CONJUGATE_GRADIENT_H @@ -12,7 +18,7 @@ #include #include -template +template class btConjugateGradient { // using TVStack = btAlignedObjectArray; @@ -29,7 +35,7 @@ public: virtual ~btConjugateGradient(){} // return the number of iterations taken - int solve(TM& A, TVStack& x, const TVStack& b, btScalar tolerance) + int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance) { btAssert(x.size() == b.size()); reinitialize(b); diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 50db01ec9..618648b47 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -1,9 +1,15 @@ -// -// btDeformableBackwardEulerObjective.cpp -// BulletSoftBody -// -// Created by Xuchen Han on 7/9/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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. + */ #include "btDeformableBackwardEulerObjective.h" @@ -63,7 +69,6 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { // only the velocity of the constrained nodes needs to be updated during CG solve -// for (auto it : projection.m_constraints) for (int i = 0; i < projection.m_constraints.size(); ++i) { int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); @@ -112,7 +117,9 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) { for (int i = 0; i < m_lf.size(); ++i) + { m_lf[i]->addScaledExplicitForce(m_dt, force); + } applyForce(force, true); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 42d85f538..db448ebe9 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -1,9 +1,15 @@ -// -// btDeformableBackwardEulerObjective.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/1/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_BACKWARD_EULER_OBJECTIVE_H #define BT_BACKWARD_EULER_OBJECTIVE_H diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index df12c213a..27cb8e467 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -1,9 +1,15 @@ -// -// btDeformableBodySolver.cpp -// BulletSoftBody -// -// Created by Xuchen Han on 7/9/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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. + */ #include #include @@ -11,7 +17,7 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) -, cg(10) +, m_cg(10) { m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); } @@ -40,7 +46,7 @@ void btDeformableBodySolver::solveConstraints(float solverdt) void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) { btScalar tolerance = std::numeric_limits::epsilon()* 1024 * m_objective->computeNorm(residual); - cg.solve(*m_objective, dv, residual, tolerance); + m_cg.solve(*m_objective, dv, residual, tolerance); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies) diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 9f1216eab..7186f8fb2 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -1,9 +1,15 @@ -// -// btDeformableBodySolver.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/1/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_BODY_SOLVERS_H #define BT_DEFORMABLE_BODY_SOLVERS_H @@ -31,7 +37,7 @@ protected: btAlignedObjectArray m_backupVelocity; btScalar m_dt; - btConjugateGradient cg; + btConjugateGradient m_cg; public: diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index edee3126e..dff5398c9 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -1,9 +1,15 @@ -// -// btDeformableContactProjection.cpp -// BulletSoftBody -// -// Created by Xuchen Han on 7/4/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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. + */ #include "btDeformableContactProjection.h" #include "btDeformableRigidDynamicsWorld.h" diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 9e61fed34..51ea92689 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -1,9 +1,15 @@ -// -// btDeformableContactProjection.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/4/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_CONTACT_PROJECTION_H #define BT_CONTACT_PROJECTION_H @@ -15,9 +21,6 @@ class btDeformableContactProjection : public btCGProjection { public: -// std::unordered_map > m_constraints; -// std::unordered_map > m_frictions; - // map from node index to constraints btHashMap > m_constraints; btHashMap >m_frictions; diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 0c2ae535d..d18eac57d 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -1,9 +1,15 @@ -// -// btDeformableGravityForce.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/21/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_GRAVITY_FORCE_H #define BT_DEFORMABLE_GRAVITY_FORCE_H @@ -13,18 +19,15 @@ class btDeformableGravityForce : public btDeformableLagrangianForce { public: -// using TVStack = btDeformableLagrangianForce::TVStack; typedef btAlignedObjectArray TVStack; btVector3 m_gravity; btDeformableGravityForce(const btVector3& g) : m_gravity(g) { - } virtual void addScaledImplicitForce(btScalar scale, TVStack& force) { -// addScaledGravityForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 6e1c54352..d2bbcef40 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -1,9 +1,15 @@ -// -// btDeformableLagrangianForce.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/1/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_LAGRANGIAN_FORCE_H #define BT_DEFORMABLE_LAGRANGIAN_FORCE_H diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 841101c8f..2d70ef0ae 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -1,9 +1,15 @@ -// -// btDeformableMassSpringForce.h -// BulletSoftBody -// -// Created by Xuchen Gan on 7/1/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_MASS_SPRING_H #define BT_MASS_SPRING_H diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index de39778ad..ef48f97b1 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -1,9 +1,15 @@ -// -// btDeformableRigidDynamicsWorld.cpp -// BulletSoftBody -// -// Created by Xuchen Han on 7/1/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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. + */ #include #include "btDeformableRigidDynamicsWorld.h" @@ -13,7 +19,6 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { reinitialize(timeStep); -// beforeSolverCallbacks(timeStep); // add gravity to velocity of rigid and multi bodys applyRigidBodyGravity(timeStep); @@ -165,7 +170,7 @@ void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collision void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); - m_deformableBodySolver->predictMotion(float(timeStep)); + m_deformableBodySolver->predictMotion(timeStep); } void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep) @@ -181,8 +186,7 @@ void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep) void btDeformableRigidDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) { - // TODO: This is an ugly hack to get the desired gravity behavior. - // gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again + // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep // when there are multiple substeps clearForces(); @@ -211,9 +215,6 @@ void btDeformableRigidDynamicsWorld::beforeSolverCallbacks(btScalar timeStep) { (*m_solverCallback)(m_internalTime, this); } - -// for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) -// m_beforeSolverCallbacks[i](m_internalTime, this); } void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) @@ -222,8 +223,6 @@ void btDeformableRigidDynamicsWorld::afterSolverCallbacks(btScalar timeStep) { (*m_solverCallback)(m_internalTime, this); } -// for (int i = 0; i < m_beforeSolverCallbacks.size(); ++i) -// m_beforeSolverCallbacks[i](m_internalTime, this); } void btDeformableRigidDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force) diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 47284e7fb..7d771d455 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -43,6 +43,9 @@ class btDeformableRigidDynamicsWorld : public btMultiBodyDynamicsWorld bool m_ownsSolver; btScalar m_internalTime; + typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world); + btSolverCallback m_solverCallback; + protected: virtual void internalSingleStepSimulation(btScalar timeStep); @@ -55,7 +58,7 @@ protected: public: btDeformableRigidDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) : btMultiBodyDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration), - m_deformableBodySolver(deformableBodySolver) + m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) { m_drawFlags = fDrawFlags::Std; m_drawNodeTree = true; @@ -76,8 +79,7 @@ public: m_internalTime = 0.0; } // btAlignedObjectArray > m_beforeSolverCallbacks; - typedef void (*btSolverCallback)(btScalar time, btDeformableRigidDynamicsWorld* world); - btSolverCallback m_solverCallback; + void setSolverCallback(btSolverCallback cb) { diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index 97cec43c5..8860a8cfe 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -1,9 +1,15 @@ -// -// btPreconditioner.h -// BulletSoftBody -// -// Created by Xuchen Han on 7/18/19. -// +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_PRECONDITIONER_H #define BT_PRECONDITIONER_H -- cgit v1.2.1 From 73f5eb6a8f477c46b18a61476e81dc4dd9cb6612 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 5 Aug 2019 16:49:04 -0700 Subject: add profiling and code clean up --- src/BulletSoftBody/btConjugateGradient.h | 5 ++--- src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 11 ++++++----- src/BulletSoftBody/btDeformableBackwardEulerObjective.h | 5 +++-- src/BulletSoftBody/btDeformableBodySolver.cpp | 3 +++ src/BulletSoftBody/btDeformableContactProjection.cpp | 8 +++++--- src/BulletSoftBody/btDeformableMassSpringForce.h | 4 ++-- src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 7 +++++-- src/BulletSoftBody/btDeformableRigidDynamicsWorld.h | 4 +--- 8 files changed, 27 insertions(+), 20 deletions(-) diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index dfec3dde5..b8dac7184 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -17,15 +17,13 @@ #include #include #include - +#include "LinearMath/btQuickprof.h" template class btConjugateGradient { -// using TVStack = btAlignedObjectArray; typedef btAlignedObjectArray TVStack; TVStack r,p,z,temp; int max_iterations; - public: btConjugateGradient(const int max_it_in) : max_iterations(max_it_in) @@ -37,6 +35,7 @@ public: // return the number of iterations taken int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance) { + BT_PROFILE("CGSolve"); btAssert(x.size() == b.size()); reinitialize(b); diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 618648b47..44783fb3d 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -12,6 +12,7 @@ */ #include "btDeformableBackwardEulerObjective.h" +#include "LinearMath/btQuickprof.h" btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) @@ -23,6 +24,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated) { + BT_PROFILE("reinitialize"); if(nodeUpdated) { updateId(); @@ -43,9 +45,7 @@ void btDeformableBackwardEulerObjective::setDt(btScalar dt) void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) const { - for (int i = 0; i < b.size(); ++i) - b[i].setZero(); - + BT_PROFILE("multiply"); // add in the mass term size_t counter = 0; for (int i = 0; i < m_softBodies.size(); ++i) @@ -54,11 +54,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) for (int j = 0; j < psb->m_nodes.size(); ++j) { const btSoftBody::Node& node = psb->m_nodes[j]; - b[counter] += (node.m_im == 0) ? btVector3(0,0,0) : x[counter] / node.m_im; + 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) { // add damping matrix @@ -97,6 +97,7 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const { + BT_PROFILE("computeResidual"); // add implicit force for (int i = 0; i < m_lf.size(); ++i) { diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index db448ebe9..82af57f6d 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -13,7 +13,6 @@ #ifndef BT_BACKWARD_EULER_OBJECTIVE_H #define BT_BACKWARD_EULER_OBJECTIVE_H -#include #include "btConjugateGradient.h" #include "btDeformableLagrangianForce.h" #include "btDeformableMassSpringForce.h" @@ -21,6 +20,7 @@ #include "btDeformableContactProjection.h" #include "btPreconditioner.h" #include "btDeformableRigidDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" class btDeformableRigidDynamicsWorld; class btDeformableBackwardEulerObjective @@ -36,7 +36,6 @@ public: btDeformableContactProjection projection; const TVStack& m_backupVelocity; btAlignedObjectArray m_nodes; - btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); virtual ~btDeformableBackwardEulerObjective() {} @@ -72,6 +71,7 @@ public: // enforce constraints in CG solve void enforceConstraint(TVStack& x) { + BT_PROFILE("enforceConstraint"); projection.enforceConstraint(x); updateVelocity(x); } @@ -85,6 +85,7 @@ public: // update the projections and project the residual void project(TVStack& r) { + BT_PROFILE("project"); projection.update(); projection.project(r); } diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 27cb8e467..9382106e9 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -14,6 +14,7 @@ #include #include #include "btDeformableBodySolver.h" +#include "LinearMath/btQuickprof.h" btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) @@ -29,6 +30,7 @@ btDeformableBodySolver::~btDeformableBodySolver() void btDeformableBodySolver::solveConstraints(float solverdt) { + BT_PROFILE("solveConstraints"); m_objective->setDt(solverdt); // add constraints to the solver @@ -70,6 +72,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArraysetConstraints(); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index dff5398c9..7470c7a93 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -224,6 +224,7 @@ void btDeformableContactProjection::update() void btDeformableContactProjection::setConstraints() { + BT_PROFILE("setConstraints"); // set Dirichlet constraint for (int i = 0; i < m_softBodies.size(); ++i) { @@ -233,12 +234,14 @@ void btDeformableContactProjection::setConstraints() if (psb->m_nodes[j].m_im == 0) { btAlignedObjectArray c; + c.reserve(3); c.push_back(DeformableContactConstraint(btVector3(1,0,0))); c.push_back(DeformableContactConstraint(btVector3(0,1,0))); c.push_back(DeformableContactConstraint(btVector3(0,0,1))); m_constraints.insert(psb->m_nodes[j].index, c); btAlignedObjectArray f; + f.reserve(3); f.push_back(DeformableFrictionConstraint()); f.push_back(DeformableFrictionConstraint()); f.push_back(DeformableFrictionConstraint()); @@ -246,13 +249,12 @@ void btDeformableContactProjection::setConstraints() } } } - for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; btMultiBodyJacobianData jacobianData_normal; btMultiBodyJacobianData jacobianData_complementary; - + std::cout <m_rcontacts.size() << std::endl; for (int j = 0; j < psb->m_rcontacts.size(); ++j) { const btSoftBody::RContact& c = psb->m_rcontacts[j]; @@ -296,6 +298,7 @@ void btDeformableContactProjection::setConstraints() const btScalar dn = btDot(vr, cti.m_normal); if (dn < SIMD_EPSILON) { + if (m_constraints.find(c.m_node->index) == NULL) { btAlignedObjectArray constraints; @@ -369,7 +372,6 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x) x[i] += constraints[j].m_value[k] * constraints[j].m_direction[k]; } } - } else { diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 2d70ef0ae..ca8ddf983 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -94,15 +94,15 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; + btScalar scaled_k_damp = psb->m_dampingCoefficient * scale; for (int j = 0; j < psb->m_links.size(); ++j) { const btSoftBody::Link& link = psb->m_links[j]; btSoftBody::Node* node1 = link.m_n[0]; btSoftBody::Node* node2 = link.m_n[1]; - btScalar k_damp = psb->m_dampingCoefficient; size_t id1 = node1->index; size_t id2 = node2->index; - btVector3 local_scaled_df = scale * k_damp * (dv[id2] - dv[id1]); + btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); df[id1] += local_scaled_df; df[id2] -= local_scaled_df; } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index ef48f97b1..116d4e0bc 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -14,10 +14,11 @@ #include #include "btDeformableRigidDynamicsWorld.h" #include "btDeformableBodySolver.h" - +#include "LinearMath/btQuickprof.h" void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { + BT_PROFILE("internalSingleStepSimulation"); reinitialize(timeStep); // add gravity to velocity of rigid and multi bodys applyRigidBodyGravity(timeStep); @@ -50,7 +51,7 @@ void btDeformableRigidDynamicsWorld::internalSingleStepSimulation(btScalar timeS void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) { // perform position correction for all constraints -// for (auto& it : m_deformableBodySolver->m_objective->projection.m_constraints) + BT_PROFILE("positionCorrection"); for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index) { btAlignedObjectArray& frictions = *m_deformableBodySolver->m_objective->projection.m_frictions[m_deformableBodySolver->m_objective->projection.m_constraints.getKeyAtIndex(index)]; @@ -134,6 +135,7 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt) void btDeformableRigidDynamicsWorld::integrateTransforms(btScalar dt) { + BT_PROFILE("integrateTransforms"); m_deformableBodySolver->backupVelocity(); positionCorrection(dt); btMultiBodyDynamicsWorld::integrateTransforms(dt); @@ -169,6 +171,7 @@ void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collision void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { + BT_PROFILE("predictUnconstraintMotion"); btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep); m_deformableBodySolver->predictMotion(timeStep); } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 7d771d455..cc4fdae05 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -78,9 +78,7 @@ public: m_sbi.m_sparsesdf.Initialize(); m_internalTime = 0.0; } -// btAlignedObjectArray > m_beforeSolverCallbacks; - - + void setSolverCallback(btSolverCallback cb) { m_solverCallback = cb; -- cgit v1.2.1 From 6a599bde87e53c22fb71cb6d35c718e9ea8083dc Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 6 Aug 2019 10:16:56 -0700 Subject: setDt in reinitialize and remove unused variables --- src/BulletSoftBody/btCGProjection.h | 6 +++--- .../btDeformableBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 20 ++++++-------------- src/BulletSoftBody/btDeformableBodySolver.h | 2 +- src/BulletSoftBody/btDeformableContactProjection.cpp | 1 - src/BulletSoftBody/btDeformableContactProjection.h | 4 ++-- .../btDeformableRigidDynamicsWorld.cpp | 2 +- 7 files changed, 14 insertions(+), 23 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 667c715c0..430327ea0 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -113,13 +113,13 @@ public: typedef btAlignedObjectArray > TArrayStack; btAlignedObjectArray m_softBodies; btDeformableRigidDynamicsWorld* m_world; - const btAlignedObjectArray* m_nodes; +// const btAlignedObjectArray* m_nodes; const btScalar& m_dt; - btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const btAlignedObjectArray* nodes) + btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) , m_dt(dt) - , m_nodes(nodes) +// , m_nodes(nodes) { } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 44783fb3d..3e6656c0d 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -16,7 +16,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) -, projection(m_softBodies, m_dt, &m_nodes) +, projection(m_softBodies, m_dt) , m_backupVelocity(backup_v) { m_preconditioner = new DefaultPreconditioner(); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 9382106e9..242b0a882 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -31,8 +31,6 @@ btDeformableBodySolver::~btDeformableBodySolver() void btDeformableBodySolver::solveConstraints(float solverdt) { BT_PROFILE("solveConstraints"); - m_objective->setDt(solverdt); - // add constraints to the solver setConstraints(); @@ -51,22 +49,16 @@ void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) m_cg.solve(*m_objective, dv, residual, tolerance); } -void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies) +void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) { + m_objective->setDt(dt); m_softBodySet.copyFromArray(softBodies); bool nodeUpdated = updateNodes(); - if (nodeUpdated) - { - m_dv.resize(m_numNodes); - m_residual.resize(m_numNodes); - m_backupVelocity.resize(m_numNodes); - } - for (int i = 0; i < m_dv.size(); ++i) - { - m_dv[i].setZero(); - m_residual[i].setZero(); - } + m_dv.resize(m_numNodes, btVector3(0,0,0)); + m_residual.resize(m_numNodes, btVector3(0,0,0)); + m_backupVelocity.resize(m_numNodes, btVector3(0,0,0)); + m_objective->reinitialize(nodeUpdated); } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 7186f8fb2..cdcc6ed80 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -60,7 +60,7 @@ public: virtual void solveConstraints(float solverdt); - void reinitialize(const btAlignedObjectArray& softBodies); + void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); void setConstraints(); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 7470c7a93..27fee5cf8 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -254,7 +254,6 @@ void btDeformableContactProjection::setConstraints() btSoftBody* psb = m_softBodies[i]; btMultiBodyJacobianData jacobianData_normal; btMultiBodyJacobianData jacobianData_complementary; - std::cout <m_rcontacts.size() << std::endl; for (int j = 0; j < psb->m_rcontacts.size(); ++j) { const btSoftBody::RContact& c = psb->m_rcontacts[j]; diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 51ea92689..9589b165e 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -25,8 +25,8 @@ public: btHashMap > m_constraints; btHashMap >m_frictions; - btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt, const btAlignedObjectArray* nodes) - : btCGProjection(softBodies, dt, nodes) + btDeformableContactProjection(btAlignedObjectArray& softBodies, const btScalar& dt) + : btCGProjection(softBodies, dt) { } diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 116d4e0bc..5afc88323 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -179,7 +179,7 @@ void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep void btDeformableRigidDynamicsWorld::reinitialize(btScalar timeStep) { m_internalTime += timeStep; - m_deformableBodySolver->reinitialize(m_softBodies); + m_deformableBodySolver->reinitialize(m_softBodies, timeStep); btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; -- cgit v1.2.1 From e5231b5cc57d617a38bb0144e5a34661b54a8811 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 6 Aug 2019 10:52:19 -0700 Subject: restore behaviors of btSoftBody --- examples/DeformableContact/DeformableContact.cpp | 1 + examples/DeformableDemo/DeformableDemo.cpp | 1 + examples/Pinch/Pinch.cpp | 1 + .../VolumetricDeformable/VolumetricDeformable.cpp | 1 + .../btDeformableBackwardEulerObjective.h | 1 - src/BulletSoftBody/btDeformableBodySolver.cpp | 3 - src/BulletSoftBody/btSoftBody.cpp | 262 +++++++++++++-------- src/BulletSoftBody/btSoftBody.h | 4 +- src/BulletSoftBody/btSoftBodyInternals.h | 60 ++++- 9 files changed, 221 insertions(+), 113 deletions(-) diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp index c40564867..102ad40a9 100644 --- a/examples/DeformableContact/DeformableContact.cpp +++ b/examples/DeformableContact/DeformableContact.cpp @@ -228,6 +228,7 @@ void DeformableContact::initPhysics() 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; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp index 2411670cf..a3cdbf847 100644 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ b/examples/DeformableDemo/DeformableDemo.cpp @@ -235,6 +235,7 @@ void DeformableDemo::initPhysics() 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; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp index 1e4afb748..c6e26ddb1 100644 --- a/examples/Pinch/Pinch.cpp +++ b/examples/Pinch/Pinch.cpp @@ -342,6 +342,7 @@ void Pinch::initPhysics() psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp index 15fb712f6..b30e4ac4d 100644 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ b/examples/VolumetricDeformable/VolumetricDeformable.cpp @@ -241,6 +241,7 @@ void VolumetricDeformable::initPhysics() 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; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 82af57f6d..30c2d995e 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -26,7 +26,6 @@ class btDeformableRigidDynamicsWorld; class btDeformableBackwardEulerObjective { public: -// using TVStack = btAlignedObjectArray; typedef btAlignedObjectArray TVStack; btScalar m_dt; btDeformableRigidDynamicsWorld* m_world; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 242b0a882..fc792bd7f 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -75,7 +75,6 @@ void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world) void btDeformableBodySolver::updateVelocity() { - // serial implementation int counter = 0; for (int i = 0; i < m_softBodySet.size(); ++i) { @@ -90,7 +89,6 @@ void btDeformableBodySolver::updateVelocity() void btDeformableBodySolver::backupVelocity() { - // serial implementation int counter = 0; for (int i = 0; i < m_softBodySet.size(); ++i) { @@ -104,7 +102,6 @@ void btDeformableBodySolver::backupVelocity() void btDeformableBodySolver::revertVelocity() { - // serial implementation int counter = 0; for (int i = 0; i < m_softBodySet.size(); ++i) { diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 8a62d31bb..86a48bf62 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1758,115 +1758,115 @@ void btSoftBody::setSolver(eSolverPresets::_ preset) } } -// void btSoftBody::predictMotion(btScalar dt) { - int i, ni; - - /* Update */ - if (m_bUpdateRtCst) - { - m_bUpdateRtCst = false; - updateConstants(); - m_fdbvt.clear(); - if (m_cfg.collisions & fCollision::VF_SS) - { - initializeFaceTree(); - } - } - - /* Prepare */ - m_sst.sdt = dt * m_cfg.timescale; - m_sst.isdt = 1 / m_sst.sdt; - m_sst.velmrg = m_sst.sdt * 3; - m_sst.radmrg = getCollisionShape()->getMargin(); - m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; - /* Forces */ -// addVelocity(m_worldInfo->m_gravity * m_sst.sdt); - applyForces(); - /* Integrate */ - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - n.m_q = n.m_x; - btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; - { - btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; - btScalar clampDeltaV = maxDisplacement / m_sst.sdt; - for (int c = 0; c < 3; c++) - { - if (deltaV[c] > clampDeltaV) - { - deltaV[c] = clampDeltaV; - } - if (deltaV[c] < -clampDeltaV) - { - deltaV[c] = -clampDeltaV; - } - } - } + int i, ni; + + /* Update */ + if (m_bUpdateRtCst) + { + m_bUpdateRtCst = false; + updateConstants(); + m_fdbvt.clear(); + if (m_cfg.collisions & fCollision::VF_SS) + { + initializeFaceTree(); + } + } + + /* Prepare */ + m_sst.sdt = dt * m_cfg.timescale; + m_sst.isdt = 1 / m_sst.sdt; + m_sst.velmrg = m_sst.sdt * 3; + m_sst.radmrg = getCollisionShape()->getMargin(); + m_sst.updmrg = m_sst.radmrg * (btScalar)0.25; + /* Forces */ + addVelocity(m_worldInfo->m_gravity * m_sst.sdt); + applyForces(); + /* Integrate */ + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + n.m_q = n.m_x; + btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt; + { + btScalar maxDisplacement = m_worldInfo->m_maxDisplacement; + btScalar clampDeltaV = maxDisplacement / m_sst.sdt; + for (int c = 0; c < 3; c++) + { + if (deltaV[c] > clampDeltaV) + { + deltaV[c] = clampDeltaV; + } + if (deltaV[c] < -clampDeltaV) + { + deltaV[c] = -clampDeltaV; + } + } + } n.m_v += deltaV; n.m_x += n.m_v * m_sst.sdt; - n.m_f = btVector3(0, 0, 0); - } - /* Clusters */ - updateClusters(); - /* Bounds */ - updateBounds(); - /* Nodes */ - ATTRIBUTE_ALIGNED16(btDbvtVolume) - vol; - for (i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); - m_ndbvt.update(n.m_leaf, - vol, - n.m_v * m_sst.velmrg, - m_sst.updmrg); - } - /* Faces */ - if (!m_fdbvt.empty()) - { - for (int i = 0; i < m_faces.size(); ++i) - { - Face& f = m_faces[i]; - const btVector3 v = (f.m_n[0]->m_v + - f.m_n[1]->m_v + - f.m_n[2]->m_v) / - 3; - vol = VolumeOf(f, m_sst.radmrg); - m_fdbvt.update(f.m_leaf, - vol, - v * m_sst.velmrg, - m_sst.updmrg); - } - } - /* Pose */ - updatePose(); - /* Match */ - if (m_pose.m_bframe && (m_cfg.kMT > 0)) - { - const btMatrix3x3 posetrs = m_pose.m_rot; - for (int i = 0, ni = m_nodes.size(); i < ni; ++i) - { - Node& n = m_nodes[i]; - if (n.m_im > 0) - { - const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; - n.m_x = Lerp(n.m_x, x, m_cfg.kMT); - } - } - } - /* Clear contacts */ - m_rcontacts.resize(0); - m_scontacts.resize(0); - /* Optimize dbvt's */ - m_ndbvt.optimizeIncremental(1); - m_fdbvt.optimizeIncremental(1); - m_cdbvt.optimizeIncremental(1); + n.m_f = btVector3(0, 0, 0); + } + /* Clusters */ + updateClusters(); + /* Bounds */ + updateBounds(); + /* Nodes */ + ATTRIBUTE_ALIGNED16(btDbvtVolume) + vol; + for (i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + m_ndbvt.update(n.m_leaf, + vol, + n.m_v * m_sst.velmrg, + m_sst.updmrg); + } + /* Faces */ + if (!m_fdbvt.empty()) + { + for (int i = 0; i < m_faces.size(); ++i) + { + Face& f = m_faces[i]; + const btVector3 v = (f.m_n[0]->m_v + + f.m_n[1]->m_v + + f.m_n[2]->m_v) / + 3; + vol = VolumeOf(f, m_sst.radmrg); + m_fdbvt.update(f.m_leaf, + vol, + v * m_sst.velmrg, + m_sst.updmrg); + } + } + /* Pose */ + updatePose(); + /* Match */ + if (m_pose.m_bframe && (m_cfg.kMT > 0)) + { + const btMatrix3x3 posetrs = m_pose.m_rot; + for (int i = 0, ni = m_nodes.size(); i < ni; ++i) + { + Node& n = m_nodes[i]; + if (n.m_im > 0) + { + const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com; + n.m_x = Lerp(n.m_x, x, m_cfg.kMT); + } + } + } + /* Clear contacts */ + m_rcontacts.resize(0); + m_scontacts.resize(0); + /* Optimize dbvt's */ + m_ndbvt.optimizeIncremental(1); + m_fdbvt.optimizeIncremental(1); + m_cdbvt.optimizeIncremental(1); } + // void btSoftBody::solveConstraints() { @@ -2262,8 +2262,35 @@ btVector3 btSoftBody::evaluateCom() const return (com); } -// bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap, + const btVector3& x, + btScalar margin, + btSoftBody::sCti& cti) const +{ + btVector3 nrm; + const btCollisionShape* shp = colObjWrap->getCollisionShape(); + // const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject()); + //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); + //todo: check which transform is needed here + + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(x), + shp, + nrm, + margin); + if (dst < 0) + { + cti.m_colObj = colObjWrap->getCollisionObject(); + cti.m_normal = wtr.getBasis() * nrm; + cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst); + return (true); + } + return (false); +} +// +bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict) const @@ -3262,6 +3289,33 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap collider.ProcessColObj(this, pcoWrap); } break; + case fCollision::SDF_RD: + { + btSoftColliders::CollideSDF_RD docollide; + btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); + btTransform wtr = pcoWrap->getWorldTransform(); + + const btTransform ctr = pcoWrap->getWorldTransform(); + const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); + const btScalar basemargin = getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(), + mins, + maxs); + volume = btDbvtVolume::FromMM(mins, maxs); + volume.Expand(btVector3(basemargin, basemargin, basemargin)); + docollide.psb = this; + docollide.m_colObj1Wrap = pcoWrap; + docollide.m_rigidBody = prb1; + + docollide.dynmargin = basemargin + timemargin; + docollide.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide); + } + break; } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 87633a83c..f2934c94a 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -160,6 +160,7 @@ public: RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft + SDF_RD = 0x0003, ///DF based rigid vs deformable SVSmask = 0x0030, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling @@ -1006,7 +1007,8 @@ public: btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; - bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const; + bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const; void updateNormals(); void updateBounds(); void updatePose(); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index eb487a56f..80545ebad 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -928,10 +928,62 @@ struct btSoftColliders psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this); } }; + // + // CollideSDF_RS + // + struct CollideSDF_RS : btDbvt::ICollide + { + void Process(const btDbvtNode* leaf) + { + btSoftBody::Node* node = (btSoftBody::Node*)leaf->data; + DoNode(*node); + } + void DoNode(btSoftBody::Node& n) const + { + const btScalar m = n.m_im > 0 ? dynmargin : stamargin; + btSoftBody::RContact c; + + if ((!n.m_battach) && + psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti)) + { + const btScalar ima = n.m_im; + const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + const btScalar ms = ima + imb; + if (ms > 0) + { + const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; + const btVector3 ra = n.m_x - wtr.getOrigin(); + const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0); + const btVector3 vb = n.m_x - n.m_q; + const btVector3 vr = vb - va; + const btScalar dn = btDot(vr, c.m_cti.m_normal); + const btVector3 fv = vr - c.m_cti.m_normal * dn; + const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_c2 = ima * psb->m_sst.sdt; + c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc; + c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + if (m_rigidBody) + m_rigidBody->activate(); + } + } + } + btSoftBody* psb; + const btCollisionObjectWrapper* m_colObj1Wrap; + btRigidBody* m_rigidBody; + btScalar dynmargin; + btScalar stamargin; + }; + // - // CollideSDF_RS + // CollideSDF_RD // - struct CollideSDF_RS : btDbvt::ICollide + struct CollideSDF_RD : btDbvt::ICollide { void Process(const btDbvtNode* leaf) { @@ -946,7 +998,7 @@ struct btSoftColliders if (!n.m_battach) { // check for collision at x_{n+1}^* - if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true)) { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; @@ -954,7 +1006,7 @@ struct btSoftColliders if (ms > 0) { // resolve contact at x_n - psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); + psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false); btSoftBody::sCti& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); -- cgit v1.2.1 From 02c5b99b2f894252205b39bc46405c7a3b9a2084 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 6 Aug 2019 11:42:48 -0700 Subject: add algorithm overview --- src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 5afc88323..2fce9e7a5 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -11,6 +11,22 @@ 3. This notice may not be removed or altered from any source distribution. */ +/* ====== Overview of the Deformable Algorithm ====== */ + +/* +A single step of the deformable body simulation contains the following main components: +1. Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces. +2. Detect collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*. +3. Then velocities of deformable bodies v_{n+1} are solved in + M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass, + by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}. +4. Contact constraints are solved as projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact. +5. Position is updated via x_{n+1} = x_n + dt * v_{n+1}. +6. Apply position correction to prevent numerical drift. + +The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf + */ + #include #include "btDeformableRigidDynamicsWorld.h" #include "btDeformableBodySolver.h" -- cgit v1.2.1 From 9a7e30d09ffe20a533ad76b1729e66232aa5de89 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 8 Aug 2019 15:00:55 -0700 Subject: move deformable examples to a single folder and rename them; change license to google 2016 --- examples/CMakeLists.txt | 2 +- examples/DeformableContact/DeformableContact.cpp | 411 --------------------- examples/DeformableContact/DeformableContact.h | 20 - examples/DeformableDemo/DeformableDemo.cpp | 293 --------------- examples/DeformableDemo/DeformableDemo.h | 20 - examples/DeformableDemo/DeformableMultibody.cpp | 409 ++++++++++++++++++++ examples/DeformableDemo/DeformableMultibody.h | 19 + examples/DeformableDemo/DeformableRigid.cpp | 291 +++++++++++++++ examples/DeformableDemo/DeformableRigid.h | 19 + examples/DeformableDemo/Pinch.cpp | 397 ++++++++++++++++++++ examples/DeformableDemo/Pinch.h | 19 + examples/DeformableDemo/VolumetricDeformable.cpp | 296 +++++++++++++++ examples/DeformableDemo/VolumetricDeformable.h | 19 + examples/ExampleBrowser/CMakeLists.txt | 16 +- examples/ExampleBrowser/ExampleEntries.cpp | 13 +- examples/ExampleBrowser/premake4.lua | 3 - examples/Pinch/Pinch.cpp | 399 -------------------- examples/Pinch/Pinch.h | 20 - .../VolumetricDeformable/VolumetricDeformable.cpp | 298 --------------- .../VolumetricDeformable/VolumetricDeformable.h | 20 - .../btDeformableRigidDynamicsWorld.h | 4 +- 21 files changed, 1485 insertions(+), 1503 deletions(-) delete mode 100644 examples/DeformableContact/DeformableContact.cpp delete mode 100644 examples/DeformableContact/DeformableContact.h delete mode 100644 examples/DeformableDemo/DeformableDemo.cpp delete mode 100644 examples/DeformableDemo/DeformableDemo.h create mode 100644 examples/DeformableDemo/DeformableMultibody.cpp create mode 100644 examples/DeformableDemo/DeformableMultibody.h create mode 100644 examples/DeformableDemo/DeformableRigid.cpp create mode 100644 examples/DeformableDemo/DeformableRigid.h create mode 100644 examples/DeformableDemo/Pinch.cpp create mode 100644 examples/DeformableDemo/Pinch.h create mode 100644 examples/DeformableDemo/VolumetricDeformable.cpp create mode 100644 examples/DeformableDemo/VolumetricDeformable.h delete mode 100644 examples/Pinch/Pinch.cpp delete mode 100644 examples/Pinch/Pinch.h delete mode 100644 examples/VolumetricDeformable/VolumetricDeformable.cpp delete mode 100644 examples/VolumetricDeformable/VolumetricDeformable.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 1918a9003..ba9efac19 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -1,4 +1,4 @@ -SUBDIRS( HelloWorld BasicDemo ) +SUBDIRS( HelloWorld BasicDemo) IF(BUILD_BULLET3) SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow TwoJoint ) ENDIF() diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp deleted file mode 100644 index 102ad40a9..000000000 --- a/examples/DeformableContact/DeformableContact.cpp +++ /dev/null @@ -1,411 +0,0 @@ -/* -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 //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 m_jointFeedbacks; -public: - DeformableContact(struct GUIHelperInterface* helper) - : CommonMultiBodyBase(helper) - { - } - - virtual ~DeformableContact() - { - } - - void initPhysics(); - - void exitPhysics(); - - void resetCamera() - { - float dist = 30; - float pitch = -30; - float yaw = 100; - float targetPos[3] = {0, -10, 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()); - btVector3 gravity = btVector3(0, -10, 0); - m_dynamicsWorld->setGravity(gravity); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - 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.)); - //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 = false; - int numLinks = 4; - bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals - bool canSleep = false; - bool selfCollide = true; - btVector3 linkHalfExtents(.4, 1, .4); - btVector3 baseHalfExtents(.4, 1, .4); - - 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.0f); - mbC->setAngularDamping(0.0f); - } - else - { - mbC->setLinearDamping(0.04f); - mbC->setAngularDamping(0.04f); - } - - 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 - { - btScalar h = 0; - const btScalar s = 4; - btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), - btVector3(+s, h, -s), - btVector3(-s, h, +s), - btVector3(+s, h, +s), - 20,20, -// 3,3, - 1 + 2 + 4 + 8, true); - - psb->getCollisionShape()->setMargin(0.25); - psb->generateBendingConstraints(2); - psb->setTotalMass(5); - psb->setSpringStiffness(2); - 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; - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - getDeformableDynamicsWorld()->addSoftBody(psb); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); - } - - 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, 5, 1./250.); -} - - -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 = .1f; - - 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 = .1f; - 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 world_to_local; - world_to_local.resize(pMultiBody->getNumLinks() + 1); - - btAlignedObjectArray 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 deleted file mode 100644 index 591c6bab1..000000000 --- a/examples/DeformableContact/DeformableContact.h +++ /dev/null @@ -1,20 +0,0 @@ -/* -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 deleted file mode 100644 index a3cdbf847..000000000 --- a/examples/DeformableDemo/DeformableDemo.cpp +++ /dev/null @@ -1,293 +0,0 @@ -/* -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 "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.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 = 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 Ctor_RbUpStack(int count) - { - float mass = 0.2; - - 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)), -// 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, 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, 1.5, 1)); - createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(1, 1.5, -1)); - createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(-1, 1.5, 1)); - createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(-1, 1.5, -1)); - createRigidBody(mass, startTransform, shape[0]); - startTransform.setOrigin(btVector3(0, 3.5, 0)); - createRigidBody(mass, startTransform, shape[0]); - } - - 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 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 - btVector3 gravity = btVector3(0, -10, 0); - m_dynamicsWorld->setGravity(gravity); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - -// 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, -32, 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(1); - - //add the ground to the dynamics world - m_dynamicsWorld->addRigidBody(body); - } - - // create a piece of cloth - { - bool onGround = false; - const btScalar s = 4; - 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, - 1 + 2 + 4 + 8, 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, - 2,2, - 0, true); - - psb->getCollisionShape()->setMargin(0.1); - 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 = 1; - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - getDeformableDynamicsWorld()->addSoftBody(psb); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); - - // add a few rigid bodies - Ctor_RbUpStack(1); - } - 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 deleted file mode 100644 index c688cea9d..000000000 --- a/examples/DeformableDemo/DeformableDemo.h +++ /dev/null @@ -1,20 +0,0 @@ -/* -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/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp new file mode 100644 index 000000000..78adf7bf1 --- /dev/null +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -0,0 +1,409 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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 "DeformableMultibody.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 //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 DeformableMultibody demo deformable bodies self-collision +static bool g_floatingBase = true; +static float friction = 1.; +class DeformableMultibody : public CommonMultiBodyBase +{ + btMultiBody* m_multiBody; + btAlignedObjectArray m_jointFeedbacks; +public: + DeformableMultibody(struct GUIHelperInterface* helper) + : CommonMultiBodyBase(helper) + { + } + + virtual ~DeformableMultibody() + { + } + + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 30; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -10, 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 DeformableMultibody::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()); + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + 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.)); + //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 = false; + int numLinks = 4; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = true; + btVector3 linkHalfExtents(.4, 1, .4); + btVector3 baseHalfExtents(.4, 1, .4); + + 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.0f); + mbC->setAngularDamping(0.0f); + } + else + { + mbC->setLinearDamping(0.04f); + mbC->setAngularDamping(0.04f); + } + + 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 + { + btScalar h = 0; + const btScalar s = 4; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 20,20, +// 3,3, + 1 + 2 + 4 + 8, true); + + psb->getCollisionShape()->setMargin(0.25); + psb->generateBendingConstraints(2); + psb->setTotalMass(5); + psb->setSpringStiffness(2); + 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; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableMultibody::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 DeformableMultibody::stepSimulation(float deltaTime) +{ +// getDeformableDynamicsWorld()->getMultiBodyDynamicsWorld()->stepSimulation(deltaTime); + m_dynamicsWorld->stepSimulation(deltaTime, 5, 1./250.); +} + + +btMultiBody* DeformableMultibody::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 = .1f; + + 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 = .1f; + 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 DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray 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* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableMultibody(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableMultibody.h b/examples/DeformableDemo/DeformableMultibody.h new file mode 100644 index 000000000..8bc9020f3 --- /dev/null +++ b/examples/DeformableDemo/DeformableMultibody.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_MULTIBODY_H +#define _DEFORMABLE_MULTIBODY_H + +class CommonExampleInterface* DeformableMultibodyCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_MULTIBODY_H diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp new file mode 100644 index 000000000..c26bcc7d5 --- /dev/null +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -0,0 +1,291 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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 "DeformableRigid.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 //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The DeformableRigid 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 DeformableRigid : public CommonRigidBodyBase +{ +public: + DeformableRigid(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableRigid() + { + } + 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 Ctor_RbUpStack(int count) + { + float mass = 0.2; + + 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)), +// 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, 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, 1.5, 1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(1, 1.5, -1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(-1, 1.5, 1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(-1, 1.5, -1)); + createRigidBody(mass, startTransform, shape[0]); + startTransform.setOrigin(btVector3(0, 3.5, 0)); + createRigidBody(mass, startTransform, shape[0]); + } + + 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 DeformableRigid::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 + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + +// 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, -32, 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(1); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + bool onGround = false; + const btScalar s = 4; + 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, + 1 + 2 + 4 + 8, 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, + 2,2, + 0, true); + + psb->getCollisionShape()->setMargin(0.1); + 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 = 1; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + + // add a few rigid bodies + Ctor_RbUpStack(1); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableRigid::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* DeformableRigidCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableRigid(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableRigid.h b/examples/DeformableDemo/DeformableRigid.h new file mode 100644 index 000000000..1f2bb0911 --- /dev/null +++ b/examples/DeformableDemo/DeformableRigid.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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_RIGID_H +#define _DEFORMABLE_RIGID_H + +class CommonExampleInterface* DeformableRigidCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_RIGID_H diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp new file mode 100644 index 000000000..aaae1df3f --- /dev/null +++ b/examples/DeformableDemo/Pinch.cpp @@ -0,0 +1,397 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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 //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 = -30; + float yaw = 100; + float targetPos[3] = {0, -0, 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& rbs = world->getNonStaticRigidBodies(); + if (rbs.size()<2) + return; + btRigidBody* rb0 = rbs[0]; + btScalar pressTime = 0.9; + btScalar liftTime = 2.5; + btScalar shiftTime = 3.5; + btScalar holdTime = 4.5*1000; + btScalar dropTime = 5.3*1000; + btTransform rbTransform; + rbTransform.setIdentity(); + btVector3 translation; + btVector3 velocity; + + btVector3 initialTranslationLeft = btVector3(0.5,3,4); + btVector3 initialTranslationRight = btVector3(0.5,3,-4); + btVector3 pinchVelocityLeft = btVector3(0,0,-2); + btVector3 pinchVelocityRight = btVector3(0,0,2); + btVector3 liftVelocity = btVector3(0,5,0); + btVector3 shiftVelocity = btVector3(0,0,5); + btVector3 holdVelocity = btVector3(0,0,0); + btVector3 openVelocityLeft = btVector3(0,0,4); + btVector3 openVelocityRight = btVector3(0,0,-4); + + if (time < pressTime) + { + velocity = pinchVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityLeft; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime); + } + 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]; + if (time < pressTime) + { + velocity = pinchVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityRight; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime); + } + 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(20); + rb1->setFriction(20); +} + +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 + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + getDeformableDynamicsWorld()->setSolverCallback(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 + { + btScalar verts[24] = {0.f, 0.f, 0.f, + 1.f, 0.f, 0.f, + 0.f, 1.f, 0.f, + 0.f, 0.f, 1.f, + 1.f, 1.f, 0.f, + 0.f, 1.f, 1.f, + 1.f, 0.f, 1.f, + 1.f, 1.f, 1.f + }; + int triangles[60] = {0, 6, 3, + 0,1,6, + 7,5,3, + 7,3,6, + 4,7,6, + 4,6,1, + 7,2,5, + 7,4,2, + 0,3,2, + 2,3,5, + 0,2,4, + 0,4,1, + 0,6,5, + 0,6,4, + 3,4,2, + 3,4,7, + 2,7,3, + 2,7,1, + 4,5,0, + 4,5,6, + }; +// btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20); +//// + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 4, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(1); +// psb->scale(btVector3(5, 5, 5)); +// psb->translate(btVector3(-2.5, 4, -2.5)); +// psb->getCollisionShape()->setMargin(0.1); +// psb->setTotalMass(1); + psb->setSpringStiffness(2); + psb->setDampingCoefficient(0.02); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + // 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/DeformableDemo/Pinch.h b/examples/DeformableDemo/Pinch.h new file mode 100644 index 000000000..3d01b262f --- /dev/null +++ b/examples/DeformableDemo/Pinch.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp new file mode 100644 index 000000000..6f2b88f8d --- /dev/null +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -0,0 +1,296 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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 //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 + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + 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; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + } + // 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/DeformableDemo/VolumetricDeformable.h b/examples/DeformableDemo/VolumetricDeformable.h new file mode 100644 index 000000000..04e30d9c6 --- /dev/null +++ b/examples/DeformableDemo/VolumetricDeformable.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2016 Google Inc. http://bulletphysics.org + 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/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index cfae5fcc5..19334b2db 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -359,14 +359,14 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/MultiBodyConstraintFeedback.cpp ../SoftDemo/SoftDemo.cpp ../SoftDemo/SoftDemo.h - ../Pinch/Pinch.cpp - ../Pinch/Pinch.h - ../DeformableContact/DeformableContact.cpp - ../DeformableContact/DeformableContact.h - ../DeformableDemo/DeformableDemo.cpp - ../DeformableDemo/DeformableDemo.h - ../VolumetricDeformable/VolumetricDeformable.cpp - ../VolumetricDeformable/VolumetricDeformable.h + ../DeformableDemo/Pinch.cpp + ../DeformableDemo/Pinch.h + ../DeformableDemo/DeformableMultibody.cpp + ../DeformableDemo/DeformableMultibody.h + ../DeformableDemo/DeformableRigid.cpp + ../DeformableDemo/DeformableRigid.h + ../DeformableDemo/VolumetricDeformable.cpp + ../DeformableDemo/VolumetricDeformable.h ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index f186f642a..481d36c5d 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -47,11 +47,10 @@ #include "../FractureDemo/FractureDemo.h" #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" -#include "../DeformableDemo/DeformableDemo.h" -#include "../Pinch/Pinch.h" -#include "../DeformableContact/DeformableContact.h" -#include "../MultiBodyBaseline/MultiBodyBaseline.h" -#include "../VolumetricDeformable/VolumetricDeformable.h" +#include "../DeformableDemo/DeformableRigid.h" +#include "../DeformableDemo/Pinch.h" +#include "../DeformableDemo/DeformableMultibody.h" +#include "../DeformableDemo/VolumetricDeformable.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -196,10 +195,10 @@ static ExampleEntry gDefaultExamples[] = //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), ExampleEntry(0, "Deformabe Body"), - ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableCreateFunc), + ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc), ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), - ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableContactCreateFunc), + ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), #ifdef INCLUDE_CLOTH_DEMOS diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index ff81b34b7..985a0457b 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -189,9 +189,6 @@ project "App_BulletExampleBrowser" "../VoronoiFracture/*", "../SoftDemo/*", "../DeformableDemo/*", - "../DeformableContact/*", - "../VolumetricDeformable/*", - "../Pinch/*", "../RollingFrictionDemo/*", "../rbdl/*", "../FractureDemo/*", diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp deleted file mode 100644 index c6e26ddb1..000000000 --- a/examples/Pinch/Pinch.cpp +++ /dev/null @@ -1,399 +0,0 @@ -/* -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 //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 = -30; - float yaw = 100; - float targetPos[3] = {0, -0, 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& rbs = world->getNonStaticRigidBodies(); - if (rbs.size()<2) - return; - btRigidBody* rb0 = rbs[0]; - btScalar pressTime = 0.9; - btScalar liftTime = 2.5; - btScalar shiftTime = 3.5; - btScalar holdTime = 4.5*1000; - btScalar dropTime = 5.3*1000; - btTransform rbTransform; - rbTransform.setIdentity(); - btVector3 translation; - btVector3 velocity; - - btVector3 initialTranslationLeft = btVector3(0.5,3,4); - btVector3 initialTranslationRight = btVector3(0.5,3,-4); - btVector3 pinchVelocityLeft = btVector3(0,0,-2); - btVector3 pinchVelocityRight = btVector3(0,0,2); - btVector3 liftVelocity = btVector3(0,5,0); - btVector3 shiftVelocity = btVector3(0,0,5); - btVector3 holdVelocity = btVector3(0,0,0); - btVector3 openVelocityLeft = btVector3(0,0,4); - btVector3 openVelocityRight = btVector3(0,0,-4); - - if (time < pressTime) - { - velocity = pinchVelocityLeft; - translation = initialTranslationLeft + pinchVelocityLeft * time; - } - else if (time < liftTime) - { - velocity = liftVelocity; - translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime); - - } - else if (time < shiftTime) - { - velocity = shiftVelocity; - translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); - } - else if (time < holdTime) - { - velocity = btVector3(0,0,0); - translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); - } - else if (time < dropTime) - { - velocity = openVelocityLeft; - translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime); - } - else - { - velocity = holdVelocity; - translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime); - } - 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]; - if (time < pressTime) - { - velocity = pinchVelocityRight; - translation = initialTranslationRight + pinchVelocityRight * time; - } - else if (time < liftTime) - { - velocity = liftVelocity; - translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime); - - } - else if (time < shiftTime) - { - velocity = shiftVelocity; - translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); - } - else if (time < holdTime) - { - velocity = btVector3(0,0,0); - translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); - } - else if (time < dropTime) - { - velocity = openVelocityRight; - translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime); - } - else - { - velocity = holdVelocity; - translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime); - } - 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(20); - rb1->setFriction(20); -} - -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 - btVector3 gravity = btVector3(0, -10, 0); - m_dynamicsWorld->setGravity(gravity); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - - getDeformableDynamicsWorld()->setSolverCallback(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 - { - btScalar verts[24] = {0.f, 0.f, 0.f, - 1.f, 0.f, 0.f, - 0.f, 1.f, 0.f, - 0.f, 0.f, 1.f, - 1.f, 1.f, 0.f, - 0.f, 1.f, 1.f, - 1.f, 0.f, 1.f, - 1.f, 1.f, 1.f - }; - int triangles[60] = {0, 6, 3, - 0,1,6, - 7,5,3, - 7,3,6, - 4,7,6, - 4,6,1, - 7,2,5, - 7,4,2, - 0,3,2, - 2,3,5, - 0,2,4, - 0,4,1, - 0,6,5, - 0,6,4, - 3,4,2, - 3,4,7, - 2,7,3, - 2,7,1, - 4,5,0, - 4,5,6, - }; -// btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(getDeformableDynamicsWorld()->getWorldInfo(), &verts[0], &triangles[0], 20); -//// - btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), - TetraCube::getElements(), - 0, - TetraCube::getNodes(), - false, true, true); - - psb->scale(btVector3(2, 2, 2)); - psb->translate(btVector3(0, 4, 0)); - psb->getCollisionShape()->setMargin(0.1); - psb->setTotalMass(1); -// psb->scale(btVector3(5, 5, 5)); -// psb->translate(btVector3(-2.5, 4, -2.5)); -// psb->getCollisionShape()->setMargin(0.1); -// psb->setTotalMass(1); - psb->setSpringStiffness(2); - psb->setDampingCoefficient(0.02); - psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects - psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 2; - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - getDeformableDynamicsWorld()->addSoftBody(psb); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); - // 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 deleted file mode 100644 index 6c7e2fb3c..000000000 --- a/examples/Pinch/Pinch.h +++ /dev/null @@ -1,20 +0,0 @@ -/* -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 deleted file mode 100644 index b30e4ac4d..000000000 --- a/examples/VolumetricDeformable/VolumetricDeformable.cpp +++ /dev/null @@ -1,298 +0,0 @@ -/* -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 //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 - btVector3 gravity = btVector3(0, -10, 0); - m_dynamicsWorld->setGravity(gravity); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - 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; - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce()); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); - } - // 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 deleted file mode 100644 index af1cc38b6..000000000 --- a/examples/VolumetricDeformable/VolumetricDeformable.h +++ /dev/null @@ -1,20 +0,0 @@ -/* -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/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index cc4fdae05..6489d6ebc 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -1,13 +1,11 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - + Copyright (c) 2016 Google Inc. http://bulletphysics.org 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. -- cgit v1.2.1 From 96e8dcef0f57c730ea1f268d6cd575dac22f5d7e Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 8 Aug 2019 16:45:19 -0700 Subject: fix bug caused by not reseting to zero --- src/BulletSoftBody/btDeformableBodySolver.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index fc792bd7f..4dd45edc8 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -55,9 +55,19 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArrayreinitialize(nodeUpdated); } -- cgit v1.2.1 From 436b6c6963d4639599908d4643d4b5f341f86f6c Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 8 Aug 2019 17:14:13 -0700 Subject: separate multibody position prediction into standalone function --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 204 ++++++++++++++++----- src/BulletDynamics/Featherstone/btMultiBody.h | 2 +- .../Featherstone/btMultiBodyDynamicsWorld.cpp | 67 +++++-- .../Featherstone/btMultiBodyDynamicsWorld.h | 3 +- src/BulletDynamics/Featherstone/btMultiBodyLink.h | 64 ++++++- 5 files changed, 259 insertions(+), 81 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 1857bd55f..4d634b699 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -1595,35 +1595,167 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar } void btMultiBody::predictPositionsMultiDof(btScalar dt) { - stepPositionsMultiDof(dt, 0, 0, true); + int num_links = getNumLinks(); + // step position by adding dt * velocity + //btVector3 v = getBaseVel(); + //m_basePos += dt * v; + // + btScalar *pBasePos; + btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) + + // reset to current position + for (int i = 0; i < 3; ++i) + { + m_basePos_interpolate[i] = m_basePos[i]; + } + pBasePos = m_basePos_interpolate; + + pBasePos[0] += dt * pBaseVel[0]; + pBasePos[1] += dt * pBaseVel[1]; + pBasePos[2] += dt * pBaseVel[2]; + + /////////////////////////////// + //local functor for quaternion integration (to avoid error prone redundancy) + struct + { + //"exponential map" based on btTransformUtil::integrateTransform(..) + void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt) + { + //baseBody => quat is alias and omega is global coor + //!baseBody => quat is alibi and omega is local coor + + btVector3 axis; + btVector3 angvel; + + if (!baseBody) + angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok + else + angvel = omega; + + btScalar fAngle = angvel.length(); + //limit the angular motion + if (fAngle * dt > ANGULAR_MOTION_THRESHOLD) + { + fAngle = btScalar(0.5) * SIMD_HALF_PI / dt; + } + + if (fAngle < btScalar(0.001)) + { + // use Taylor's expansions of sync function + axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle); + } + else + { + // sync(fAngle) = sin(c*fAngle)/t + axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle); + } + + if (!baseBody) + quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat; + else + quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5))); + //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse(); + + quat.normalize(); + } + } pQuatUpdateFun; + /////////////////////////////// + + //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); + // + btScalar *pBaseQuat; + + // reset to current orientation + for (int i = 0; i < 4; ++i) + { + m_baseQuat_interpolate[i] = m_baseQuat[i]; + } + pBaseQuat = m_baseQuat_interpolate; + + btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) + // + btQuaternion baseQuat; + baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]); + btVector3 baseOmega; + baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]); + pQuatUpdateFun(baseOmega, baseQuat, true, dt); + pBaseQuat[0] = baseQuat.x(); + pBaseQuat[1] = baseQuat.y(); + pBaseQuat[2] = baseQuat.z(); + pBaseQuat[3] = baseQuat.w(); + + // Finally we can update m_jointPos for each of the m_links + for (int i = 0; i < num_links; ++i) + { + btScalar *pJointPos; + pJointPos = &m_links[i].m_jointPos_interpolate[0]; + + btScalar *pJointVel = getJointVelMultiDof(i); + + switch (m_links[i].m_jointType) + { + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + //reset to current pos + pJointPos[0] = m_links[i].m_jointPos[0]; + btScalar jointVel = pJointVel[0]; + pJointPos[0] += dt * jointVel; + break; + } + case btMultibodyLink::eSpherical: + { + //reset to current pos + + for (int i = 0; i < 4; ++i) + { + pJointPos[i] = m_links[i].m_jointPos[i]; + } + + btVector3 jointVel; + jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); + btQuaternion jointOri; + jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]); + pQuatUpdateFun(jointVel, jointOri, false, dt); + pJointPos[0] = jointOri.x(); + pJointPos[1] = jointOri.y(); + pJointPos[2] = jointOri.z(); + pJointPos[3] = jointOri.w(); + break; + } + case btMultibodyLink::ePlanar: + { + for (int i = 0; i < 3; ++i) + { + pJointPos[i] = m_links[i].m_jointPos[i]; + } + pJointPos[0] += dt * getJointVelMultiDof(i)[0]; + + btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); + btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2); + pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt; + pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt; + break; + } + default: + { + } + } + + m_links[i].updateInterpolationCacheMultiDof(); + } } -void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd, bool predict) +void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd) { int num_links = getNumLinks(); // step position by adding dt * velocity //btVector3 v = getBaseVel(); //m_basePos += dt * v; // - btScalar *pBasePos; + btScalar *pBasePos = (pq ? &pq[4] : m_basePos); btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety) - if (!predict) - { - pBasePos = (pq ? &pq[4] : m_basePos); - } // - else - { - // reset to current position - for (int i = 0; i < 3; ++i) - { - m_basePos_interpolate[i] = m_basePos[i]; - } - pBasePos = m_basePos_interpolate; - } - - - pBasePos[0] += dt * pBaseVel[0]; pBasePos[1] += dt * pBaseVel[1]; pBasePos[2] += dt * pBaseVel[2]; @@ -1677,18 +1809,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt); // - btScalar *pBaseQuat; - if (!predict) - pBaseQuat = pq ? pq : m_baseQuat; - else - { - // reset to current orientation - for (int i = 0; i < 4; ++i) - { - m_baseQuat_interpolate[i] = m_baseQuat[i]; - } - pBaseQuat = m_baseQuat_interpolate; - } + btScalar *pBaseQuat = pq ? pq : m_baseQuat; btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety) // btQuaternion baseQuat; @@ -1714,10 +1835,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd for (int i = 0; i < num_links; ++i) { btScalar *pJointPos; - if (!predict) - pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); - else - pJointPos = &m_links[i].m_jointPos_interpolate[0]; + pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]); btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i)); @@ -1727,10 +1845,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd case btMultibodyLink::eRevolute: { //reset to current pos - if (predict) - { - pJointPos[0] = m_links[i].m_jointPos[0]; - } btScalar jointVel = pJointVel[0]; pJointPos[0] += dt * jointVel; break; @@ -1738,11 +1852,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd case btMultibodyLink::eSpherical: { //reset to current pos - if (predict) - { - for (int i = 0; i < 4; ++i) - pJointPos[i] = m_links[i].m_jointPos[i]; - } btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]); btQuaternion jointOri; @@ -1756,11 +1865,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } case btMultibodyLink::ePlanar: { - if (predict) - { - for (int i = 0; i < 3; ++i) - pJointPos[i] = m_links[i].m_jointPos[i]; - } pJointPos[0] += dt * getJointVelMultiDof(i)[0]; btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2); @@ -1775,7 +1879,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd } } - m_links[i].updateCacheMultiDof(pq, predict); + m_links[i].updateCacheMultiDof(pq); if (pq) pq += m_links[i].m_posVarCount; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index fe60af991..91b5c3edb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -435,7 +435,7 @@ public: } // timestep the positions (given current velocities). - void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false); + void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); // predict the positions void predictPositionsMultiDof(btScalar dt); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 2e6dbc440..1be76ad86 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -36,7 +36,7 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep); - integrateMultiBodyTransforms(timeStep, /*predict = */ true); + predictMultiBodyTransforms(timeStep); } void btMultiBodyDynamicsWorld::calculateSimulationIslands() @@ -791,7 +791,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) integrateMultiBodyTransforms(timeStep); } -void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict) +void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep) { BT_PROFILE("btMultiBody stepPositions"); //integrate and update the Featherstone hierarchies @@ -815,28 +815,21 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b int nLinks = bod->getNumLinks(); ///base + num m_links - if (!predict) - { - if (!bod->isPosUpdated()) - bod->stepPositionsMultiDof(timeStep); - else - { - btScalar* pRealBuf = const_cast(bod->getVelocityVector()); - pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); - - bod->stepPositionsMultiDof(1, 0, pRealBuf); - bod->setPosUpdated(false); - } - } + if (!bod->isPosUpdated()) + bod->stepPositionsMultiDof(timeStep); else - bod->predictPositionsMultiDof(timeStep); + { + btScalar* pRealBuf = const_cast(bod->getVelocityVector()); + pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs() * bod->getNumDofs(); + + bod->stepPositionsMultiDof(1, 0, pRealBuf); + bod->setPosUpdated(false); + } + m_scratch_world_to_local.resize(nLinks + 1); m_scratch_local_origin.resize(nLinks + 1); - if (predict) - bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); - else - bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); } else { @@ -845,6 +838,40 @@ void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, b } } +void btMultiBodyDynamicsWorld::predictMultiBodyTransforms(btScalar timeStep) +{ + BT_PROFILE("btMultiBody stepPositions"); + //integrate and update the Featherstone hierarchies + + for (int b = 0; b < m_multiBodies.size(); b++) + { + btMultiBody* bod = m_multiBodies[b]; + bool isSleeping = false; + if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) + { + isSleeping = true; + } + for (int b = 0; b < bod->getNumLinks(); b++) + { + if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING) + isSleeping = true; + } + + if (!isSleeping) + { + int nLinks = bod->getNumLinks(); + bod->predictPositionsMultiDof(timeStep); + m_scratch_world_to_local.resize(nLinks + 1); + m_scratch_local_origin.resize(nLinks + 1); + bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin); + } + else + { + bod->clearVelocities(); + } + } +} + void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint) { m_multiBodyConstraints.push_back(constraint); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 04707bf2d..653ec36ca 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -96,7 +96,8 @@ public: virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint); virtual void integrateTransforms(btScalar timeStep); - void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false); + void integrateMultiBodyTransforms(btScalar timeStep); + void predictMultiBodyTransforms(btScalar timeStep); virtual void predictUnconstraintMotion(btScalar timeStep); virtual void debugDrawWorld(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index e7966b852..1cd6b8c07 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -191,16 +191,11 @@ struct btMultibodyLink } // routine to update m_cachedRotParentToThis and m_cachedRVector - void updateCacheMultiDof(btScalar *pq = 0, bool predict = false) + void updateCacheMultiDof(btScalar *pq = 0) { - btScalar *pJointPos; - - if (!predict) - pJointPos = (pq ? pq : &m_jointPos[0]); - else - pJointPos = &m_jointPos_interpolate[0]; - btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis; - btVector3& cachedVector = predict ? m_cachedRVector_interpolate : m_cachedRVector; + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + btQuaternion& cachedRot = m_cachedRotParentToThis; + btVector3& cachedVector =m_cachedRVector; switch (m_jointType) { case eRevolute: @@ -245,6 +240,57 @@ struct btMultibodyLink } } } + + void updateInterpolationCacheMultiDof() + { + btScalar *pJointPos = &m_jointPos_interpolate[0]; + + btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate; + btVector3& cachedVector = m_cachedRVector_interpolate; + switch (m_jointType) + { + case eRevolute: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + + break; + } + case ePrismatic: + { + // m_cachedRotParentToThis never changes, so no need to update + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + + break; + } + case eSpherical: + { + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + case ePlanar: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); + + break; + } + case eFixed: + { + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + default: + { + //invalid type + btAssert(0); + } + } + } }; #endif //BT_MULTIBODY_LINK_H -- cgit v1.2.1 From 817e64a769219621de0debc4979630953dbee60b Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 8 Aug 2019 17:26:19 -0700 Subject: remove one softbody array copy --- .../ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a | Bin 1641184 -> 0 bytes src/BulletSoftBody/btCGProjection.h | 8 +------- .../btDeformableBackwardEulerObjective.cpp | 4 ++-- .../btDeformableBackwardEulerObjective.h | 4 ++-- src/BulletSoftBody/btDeformableBodySolver.cpp | 3 +-- 5 files changed, 6 insertions(+), 13 deletions(-) delete mode 100644 examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a diff --git a/examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a b/examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a deleted file mode 100644 index 377e345f2..000000000 Binary files a/examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a and /dev/null differ diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 430327ea0..6b5caf581 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -111,7 +111,7 @@ public: typedef btAlignedObjectArray TVStack; typedef btAlignedObjectArray > TVArrayStack; typedef btAlignedObjectArray > TArrayStack; - btAlignedObjectArray m_softBodies; + btAlignedObjectArray& m_softBodies; btDeformableRigidDynamicsWorld* m_world; // const btAlignedObjectArray* m_nodes; const btScalar& m_dt; @@ -119,7 +119,6 @@ public: btCGProjection(btAlignedObjectArray& softBodies, const btScalar& dt) : m_softBodies(softBodies) , m_dt(dt) -// , m_nodes(nodes) { } @@ -139,11 +138,6 @@ public: { } - void setSoftBodies(btAlignedObjectArray softBodies) - { - m_softBodies.copyFromArray(softBodies); - } - virtual void setWorld(btDeformableRigidDynamicsWorld* world) { m_world = world; diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 3e6656c0d..fbc663f35 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -22,13 +22,13 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned m_preconditioner = new DefaultPreconditioner(); } -void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated) +void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) { BT_PROFILE("reinitialize"); + setDt(dt); if(nodeUpdated) { updateId(); - projection.setSoftBodies(m_softBodies); } for (int i = 0; i < m_lf.size(); ++i) { diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index 30c2d995e..c681300a7 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -62,8 +62,8 @@ public: // set initial guess for CG solve void initialGuess(TVStack& dv, const TVStack& residual); - // reset data structure - void reinitialize(bool nodeUpdated); + // reset data structure and reset dt + void reinitialize(bool nodeUpdated, btScalar dt); void setDt(btScalar dt); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 4dd45edc8..b77ba9da3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -51,7 +51,6 @@ void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) { - m_objective->setDt(dt); m_softBodySet.copyFromArray(softBodies); bool nodeUpdated = updateNodes(); @@ -69,7 +68,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArrayreinitialize(nodeUpdated); + m_objective->reinitialize(nodeUpdated, dt); } void btDeformableBodySolver::setConstraints() -- cgit v1.2.1 From 7adb6fdff3bd182f27668836a5b9d0ecc109856a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 8 Aug 2019 17:43:49 -0700 Subject: 2016 -> 2019 --- examples/DeformableDemo/DeformableMultibody.cpp | 2 +- examples/DeformableDemo/DeformableMultibody.h | 2 +- examples/DeformableDemo/DeformableRigid.cpp | 2 +- examples/DeformableDemo/DeformableRigid.h | 2 +- examples/DeformableDemo/Pinch.cpp | 2 +- examples/DeformableDemo/Pinch.h | 2 +- examples/DeformableDemo/VolumetricDeformable.cpp | 2 +- examples/DeformableDemo/VolumetricDeformable.h | 2 +- src/BulletSoftBody/btCGProjection.h | 2 +- src/BulletSoftBody/btConjugateGradient.h | 2 +- src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btDeformableBackwardEulerObjective.h | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.h | 2 +- src/BulletSoftBody/btDeformableContactProjection.cpp | 2 +- src/BulletSoftBody/btDeformableContactProjection.h | 2 +- src/BulletSoftBody/btDeformableGravityForce.h | 2 +- src/BulletSoftBody/btDeformableLagrangianForce.h | 2 +- src/BulletSoftBody/btDeformableMassSpringForce.h | 2 +- src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp | 2 +- src/BulletSoftBody/btDeformableRigidDynamicsWorld.h | 2 +- src/BulletSoftBody/btPreconditioner.h | 2 +- 22 files changed, 22 insertions(+), 22 deletions(-) diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 78adf7bf1..e947a5585 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/examples/DeformableDemo/DeformableMultibody.h b/examples/DeformableDemo/DeformableMultibody.h index 8bc9020f3..acd9d421d 100644 --- a/examples/DeformableDemo/DeformableMultibody.h +++ b/examples/DeformableDemo/DeformableMultibody.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index c26bcc7d5..403ce4907 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/examples/DeformableDemo/DeformableRigid.h b/examples/DeformableDemo/DeformableRigid.h index 1f2bb0911..0d0e0dc5e 100644 --- a/examples/DeformableDemo/DeformableRigid.h +++ b/examples/DeformableDemo/DeformableRigid.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index aaae1df3f..df0964447 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/examples/DeformableDemo/Pinch.h b/examples/DeformableDemo/Pinch.h index 3d01b262f..1616ec39a 100644 --- a/examples/DeformableDemo/Pinch.h +++ b/examples/DeformableDemo/Pinch.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 6f2b88f8d..74d9678a8 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/examples/DeformableDemo/VolumetricDeformable.h b/examples/DeformableDemo/VolumetricDeformable.h index 04e30d9c6..fe5d326a5 100644 --- a/examples/DeformableDemo/VolumetricDeformable.h +++ b/examples/DeformableDemo/VolumetricDeformable.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index 6b5caf581..f38fa545c 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index b8dac7184..22a9f3ada 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index fbc663f35..746048562 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index c681300a7..f87f7f509 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index b77ba9da3..eb8e62237 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index cdcc6ed80..94102afa9 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 27fee5cf8..84d35c062 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 9589b165e..64d448b5e 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index d18eac57d..270222b7e 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index d2bbcef40..eb4d032bf 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index ca8ddf983..f97ef0a03 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp index 2fce9e7a5..3b3b62edc 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h index 6489d6ebc..de5eb7ef4 100644 --- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index 8860a8cfe..663731a58 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -1,6 +1,6 @@ /* Bullet Continuous Collision Detection and Physics Library - Copyright (c) 2016 Google Inc. http://bulletphysics.org + Copyright (c) 2019 Google Inc. http://bulletphysics.org 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, -- cgit v1.2.1 From 12653f9f191c1706d8cffd2dec93b1febc3edab3 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 9 Aug 2019 10:14:35 -0700 Subject: add back files accidentally removed --- .../ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a | Bin 0 -> 1641184 bytes 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a diff --git a/examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a b/examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a new file mode 100644 index 000000000..377e345f2 Binary files /dev/null and b/examples/ThirdPartyLibs/openvr/bin/osx64/libopenvr_api.a differ -- cgit v1.2.1 From 69a02302aa01c6e65975f302f4e115191cf01616 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 10 Aug 2019 12:12:59 -0700 Subject: fix gripper in pinch example --- examples/DeformableDemo/Pinch.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index df0964447..44e5f4e5f 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -87,7 +87,7 @@ public: void createGrip() { int count = 2; - float mass = 2; + float mass = 1e6; btCollisionShape* shape[] = { new btBoxShape(btVector3(3, 3, 0.5)), }; -- cgit v1.2.1 From b90097803e498e22fd61b7c7f44ceb05a91af0e2 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 13 Aug 2019 14:56:26 -0700 Subject: update loading softbody --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b8e0275cd..13a492674 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7861,8 +7861,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; bool hasStatus = true; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - double scale = 0.1; - double mass = 0.1; + double scale = 1; + double mass = 1; double collisionMargin = 0.02; const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments; if (m_data->m_verboseOutput) @@ -7944,9 +7944,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar //turn on softbody vs softbody collision psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->randomizeConstraints(); + psb->scale(btVector3(scale, scale, scale)); psb->rotate(initialOrn); psb->translate(initialPos); - psb->scale(btVector3(scale, scale, scale)); psb->setTotalMass(mass, true); psb->getCollisionShape()->setMargin(collisionMargin); -- cgit v1.2.1 From 10108cd3eabdffcd4d70a6ff7ebae998cdd80a3a Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 13 Aug 2019 16:53:51 -0700 Subject: update obj loader --- Extras/obj2sdf/obj2sdf.cpp | 41 +-- .../ImportMJCFDemo/BulletMJCFImporter.cpp | 26 +- .../ImportMeshUtility/b3ImportMeshUtility.cpp | 7 +- .../Importers/ImportObjDemo/LoadMeshFromObj.cpp | 14 +- examples/Importers/ImportObjDemo/LoadMeshFromObj.h | 2 +- .../Wavefront2GLInstanceGraphicsShape.cpp | 103 +++--- .../Wavefront2GLInstanceGraphicsShape.h | 2 +- .../ImportURDFDemo/BulletUrdfImporter.cpp | 28 +- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 34 +- examples/ThirdPartyLibs/Wavefront/README.md | 143 +++++--- .../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp | 397 +++++++++++---------- .../ThirdPartyLibs/Wavefront/tiny_obj_loader.h | 43 ++- 12 files changed, 475 insertions(+), 365 deletions(-) diff --git a/Extras/obj2sdf/obj2sdf.cpp b/Extras/obj2sdf/obj2sdf.cpp index b6a638c7f..8d3db3955 100644 --- a/Extras/obj2sdf/obj2sdf.cpp +++ b/Extras/obj2sdf/obj2sdf.cpp @@ -77,9 +77,10 @@ int main(int argc, char* argv[]) b3FileUtils::extractPath(fileNameWithPath, materialPrefixPath, MAX_PATH_LEN); std::vector shapes; + tinyobj::attrib_t attribute; b3BulletDefaultFileIO fileIO; - std::string err = tinyobj::LoadObj(shapes, fileNameWithPath, materialPrefixPath,&fileIO); + std::string err = tinyobj::LoadObj(attribute, shapes, fileNameWithPath, materialPrefixPath,&fileIO); char sdfFileName[MAX_PATH_LEN]; sprintf(sdfFileName, "%s%s.sdf", materialPrefixPath, "newsdf"); @@ -117,20 +118,20 @@ int main(int argc, char* argv[]) int curTexcoords = shapeC->texcoords.size() / 2; int faceCount = shape.mesh.indices.size(); - int vertexCount = shape.mesh.positions.size(); + int vertexCount = attribute.vertices.size(); for (int v = 0; v < vertexCount; v++) { - shapeC->positions.push_back(shape.mesh.positions[v]); + shapeC->positions.push_back(attribute.vertices[v]); } - int numNormals = int(shape.mesh.normals.size()); + int numNormals = int(attribute.normals.size()); for (int vn = 0; vn < numNormals; vn++) { - shapeC->normals.push_back(shape.mesh.normals[vn]); + shapeC->normals.push_back(attribute.normals[vn]); } - int numTexCoords = int(shape.mesh.texcoords.size()); + int numTexCoords = int(attribute.texcoords.size()); for (int vt = 0; vt < numTexCoords; vt++) { - shapeC->texcoords.push_back(shape.mesh.texcoords[vt]); + shapeC->texcoords.push_back(attribute.texcoords[vt]); } for (int face = 0; face < faceCount; face += 3) @@ -140,9 +141,9 @@ int main(int argc, char* argv[]) continue; } - shapeC->indices.push_back(shape.mesh.indices[face] + curPositions); - shapeC->indices.push_back(shape.mesh.indices[face + 1] + curPositions); - shapeC->indices.push_back(shape.mesh.indices[face + 2] + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face].vertex_index + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face + 1].vertex_index + curPositions); + shapeC->indices.push_back(shape.mesh.indices[face + 2].vertex_index + curPositions); } } } @@ -329,7 +330,7 @@ int main(int argc, char* argv[]) } int faceCount = shape.mesh.indices.size(); - int vertexCount = shape.mesh.positions.size(); + int vertexCount = attribute.vertices.size(); tinyobj::material_t mat = shape.material; if (shape.name.length()) { @@ -339,7 +340,7 @@ int main(int argc, char* argv[]) } for (int v = 0; v < vertexCount / 3; v++) { - fprintf(f, "v %f %f %f\n", shape.mesh.positions[v * 3 + 0], shape.mesh.positions[v * 3 + 1], shape.mesh.positions[v * 3 + 2]); + fprintf(f, "v %f %f %f\n", attribute.vertices[v * 3 + 0], attribute.vertices[v * 3 + 1], attribute.vertices[v * 3 + 2]); } if (mat.name.length()) @@ -352,18 +353,18 @@ int main(int argc, char* argv[]) } fprintf(f, "\n"); - int numNormals = int(shape.mesh.normals.size()); + int numNormals = int(attribute.normals.size()); for (int vn = 0; vn < numNormals / 3; vn++) { - fprintf(f, "vn %f %f %f\n", shape.mesh.normals[vn * 3 + 0], shape.mesh.normals[vn * 3 + 1], shape.mesh.normals[vn * 3 + 2]); + fprintf(f, "vn %f %f %f\n", attribute.normals[vn * 3 + 0], attribute.normals[vn * 3 + 1], attribute.normals[vn * 3 + 2]); } fprintf(f, "\n"); - int numTexCoords = int(shape.mesh.texcoords.size()); + int numTexCoords = int(attribute.texcoords.size()); for (int vt = 0; vt < numTexCoords / 2; vt++) { - fprintf(f, "vt %f %f\n", shape.mesh.texcoords[vt * 2 + 0], shape.mesh.texcoords[vt * 2 + 1]); + fprintf(f, "vt %f %f\n", attribute.texcoords[vt * 2 + 0], attribute.texcoords[vt * 2 + 1]); } fprintf(f, "s off\n"); @@ -375,9 +376,9 @@ int main(int argc, char* argv[]) continue; } fprintf(f, "f %d/%d/%d %d/%d/%d %d/%d/%d\n", - shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1, shape.mesh.indices[face] + 1, - shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1, shape.mesh.indices[face + 1] + 1, - shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1, shape.mesh.indices[face + 2] + 1); + shape.mesh.indices[face].vertex_index + 1, shape.mesh.indices[face].vertex_index + 1, shape.mesh.indices[face].vertex_index + 1, + shape.mesh.indices[face + 1].vertex_index + 1, shape.mesh.indices[face + 1].vertex_index + 1, shape.mesh.indices[face + 1].vertex_index + 1, + shape.mesh.indices[face + 2].vertex_index + 1, shape.mesh.indices[face + 2].vertex_index + 1, shape.mesh.indices[face + 2].vertex_index + 1); } fclose(f); @@ -437,4 +438,4 @@ int main(int argc, char* argv[]) fclose(sdfFile); return 0; -} \ No newline at end of file +} diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index d630f8599..f868bcb13 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -2265,7 +2265,7 @@ int BulletMJCFImporter::getBodyUniqueId() const return m_data->m_activeBodyUniqueId; } -static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale, btScalar collisionMargin) +static btCollisionShape* MjcfCreateConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector& shapes, const btVector3& geomScale, btScalar collisionMargin) { btCompoundShape* compound = new btCompoundShape(); compound->setMargin(collisionMargin); @@ -2278,25 +2278,26 @@ static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vectorsetMargin(collisionMargin); tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); for (int f = 0; f < faceCount; f += 3) { btVector3 pt; - pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); } @@ -2391,10 +2392,11 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, col->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO); //create a convex hull for each shape, and store it in a btCompoundShape - childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin); + childShape = MjcfCreateConvexHullFromShapes(attribute, shapes, col->m_geometry.m_meshScale, m_data->m_globalDefaults.m_defaultCollisionMargin); } break; } diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index b4e02243d..6f13d86b2 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -65,13 +65,14 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& btVector3 shift(0, 0, 0); std::vector shapes; + tinyobj::attrib_t attribute; { B3_PROFILE("tinyobj::LoadObj"); - std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, pathPrefix,fileIO); + std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, pathPrefix, fileIO); //std::string err = tinyobj::LoadObj(shapes, relativeFileName, pathPrefix); } - GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); + GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes); { B3_PROFILE("Load Texture"); //int textureIndex = -1; @@ -84,7 +85,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& meshData.m_rgbaColor[2] = shape.material.diffuse[2]; meshData.m_rgbaColor[3] = shape.material.transparency; meshData.m_flags |= B3_IMPORT_MESH_HAS_RGBA_COLOR; - + meshData.m_specularColor[0] = shape.material.specular[0]; meshData.m_specularColor[1] = shape.material.specular[1]; meshData.m_specularColor[2] = shape.material.specular[2]; diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp index 566682d0a..e5bd6633c 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp @@ -12,6 +12,7 @@ struct CachedObjResult { std::string m_msg; std::vector m_shapes; + tinyobj::attrib_t m_attribute; }; static b3HashMap gCachedObjResults; @@ -31,24 +32,26 @@ void b3EnableFileCaching(int enable) } std::string LoadFromCachedOrFromObj( + tinyobj::attrib_t& attribute, std::vector& shapes, // [output] const char* filename, const char* mtl_basepath, - struct CommonFileIOInterface* fileIO - ) + struct CommonFileIOInterface* fileIO) { CachedObjResult* resultPtr = gCachedObjResults[filename]; if (resultPtr) { const CachedObjResult& result = *resultPtr; shapes = result.m_shapes; + attribute = result.m_attribute; return result.m_msg; } - std::string err = tinyobj::LoadObj(shapes, filename, mtl_basepath,fileIO); + std::string err = tinyobj::LoadObj(attribute, shapes, filename, mtl_basepath, fileIO); CachedObjResult result; result.m_msg = err; result.m_shapes = shapes; + result.m_attribute = attribute; if (gEnableFileCaching) { gCachedObjResults.insert(filename, result); @@ -60,14 +63,15 @@ GLInstanceGraphicsShape* LoadMeshFromObj(const char* relativeFileName, const cha { B3_PROFILE("LoadMeshFromObj"); std::vector shapes; + tinyobj::attrib_t attribute; { B3_PROFILE("tinyobj::LoadObj2"); - std::string err = LoadFromCachedOrFromObj(shapes, relativeFileName, materialPrefixPath,fileIO); + std::string err = LoadFromCachedOrFromObj(attribute, shapes, relativeFileName, materialPrefixPath, fileIO); } { B3_PROFILE("btgCreateGraphicsShapeFromWavefrontObj"); - GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(shapes); + GLInstanceGraphicsShape* gfxShape = btgCreateGraphicsShapeFromWavefrontObj(attribute, shapes); return gfxShape; } } diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h index 9b62074c1..54bc66312 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h @@ -8,8 +8,8 @@ struct GLInstanceGraphicsShape; int b3IsFileCachingEnabled(); void b3EnableFileCaching(int enable); - std::string LoadFromCachedOrFromObj( + tinyobj::attrib_t& attribute, std::vector& shapes, // [output] const char* filename, const char* mtl_basepath, diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp index ead2769a0..08fa521ae 100644 --- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp +++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp @@ -9,7 +9,7 @@ #include "../../OpenGLWindow/GLInstancingRenderer.h" #include "../../OpenGLWindow/GLInstanceGraphicsShape.h" -GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector& shapes, bool flatShading) +GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector& shapes, bool flatShading) { b3AlignedObjectArray* vertices = new b3AlignedObjectArray; { @@ -36,19 +36,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector= 0 && uv1Index >= 0 && (uv0Index < int(shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size()))) + int uv0Index = 2 * v_0.texcoord_index; + int uv1Index = 2 * v_0.texcoord_index + 1; + if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < int(attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size()))) { - vtx0.uv[0] = shape.mesh.texcoords[uv0Index]; - vtx0.uv[1] = shape.mesh.texcoords[uv1Index]; + vtx0.uv[0] = attribute.texcoords[uv0Index]; + vtx0.uv[1] = attribute.texcoords[uv1Index]; } else { @@ -64,19 +65,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())) + int uv0Index = 2 * v_1.texcoord_index; + int uv1Index = 2 * v_1.texcoord_index + 1; + if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size())) { - vtx1.uv[0] = shape.mesh.texcoords[uv0Index]; - vtx1.uv[1] = shape.mesh.texcoords[uv1Index]; + vtx1.uv[0] = attribute.texcoords[uv0Index]; + vtx1.uv[1] = attribute.texcoords[uv1Index]; } else { @@ -92,18 +94,20 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector= 0 && uv1Index >= 0 && (uv0Index < shape.mesh.texcoords.size()) && (uv1Index < shape.mesh.texcoords.size())) + int uv0Index = 2 * v_2.texcoord_index; + int uv1Index = 2 * v_2.texcoord_index + 1; + + if (uv0Index >= 0 && uv1Index >= 0 && (uv0Index < attribute.texcoords.size()) && (uv1Index < attribute.texcoords.size())) { - vtx2.uv[0] = shape.mesh.texcoords[uv0Index]; - vtx2.uv[1] = shape.mesh.texcoords[uv1Index]; + vtx2.uv[0] = attribute.texcoords[uv0Index]; + vtx2.uv[1] = attribute.texcoords[uv1Index]; } else { @@ -123,16 +127,21 @@ GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vectorpush_back(vtx0); vertices->push_back(vtx1); diff --git a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h index 2840a85d7..4054b4dab 100644 --- a/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h +++ b/examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.h @@ -4,6 +4,6 @@ #include "../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" #include -struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(std::vector& shapes, bool flatShading = false); +struct GLInstanceGraphicsShape* btgCreateGraphicsShapeFromWavefrontObj(const tinyobj::attrib_t& attribute, std::vector& shapes, bool flatShading = false); #endif //WAVEFRONT2GRAPHICS_H diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index f295458a8..16c85d0b5 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -509,7 +509,7 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor return true; } -static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale, int flags) +static btCollisionShape* createConvexHullFromShapes(const tinyobj::attrib_t& attribute, std::vector& shapes, const btVector3& geomScale, int flags) { B3_PROFILE("createConvexHullFromShapes"); btCompoundShape* compound = new btCompoundShape(); @@ -528,20 +528,20 @@ static btCollisionShape* createConvexHullFromShapes(std::vectoraddPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * geomScale, false); } @@ -558,8 +558,6 @@ static btCollisionShape* createConvexHullFromShapes(std::vectorm_bulletCollisionShape2UrdfCollision.find(collisionShape); @@ -718,10 +716,10 @@ btCollisionShape* BulletURDFImporter::convertURDFToCollisionShape(const UrdfColl else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, collision->m_geometry.m_meshFileName.c_str(),"",m_data->m_fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, collision->m_geometry.m_meshFileName.c_str(), "", m_data->m_fileIO); //create a convex hull for each shape, and store it in a btCompoundShape - - shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale, m_data->m_flags); + shape = createConvexHullFromShapes(attribute, shapes, collision->m_geometry.m_meshScale, m_data->m_flags); m_data->m_bulletCollisionShape2UrdfCollision.insert(shape, *collision); return shape; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f739bbb1c..43e27b7b6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4724,7 +4724,8 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str else { std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); //shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale); //static btCollisionShape* createConvexHullFromShapes(std::vector& shapes, const btVector3& geomScale) @@ -4745,20 +4746,20 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str for (int f = 0; f < faceCount; f += 3) { btVector3 pt; - pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 0].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 1].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); - pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1], - shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]); + pt.setValue(attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 0], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 1], + attribute.vertices[3 * shape.mesh.indices[f + 2].vertex_index + 2]); convexHull->addPoint(pt * meshScale, false); } @@ -7861,23 +7862,24 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar std::string out_found_filename; int out_type; - bool foundFile = UrdfFindMeshFile(fileIO,pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); + bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); std::vector shapes; - std::string err = tinyobj::LoadObj(shapes, out_found_filename.c_str(),"",fileIO); + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); if (!shapes.empty()) { const tinyobj::shape_t& shape = shapes[0]; btAlignedObjectArray vertices; btAlignedObjectArray indices; - for (int i = 0; i < shape.mesh.positions.size(); i++) + for (int i = 0; i < attribute.vertices.size(); i++) { - vertices.push_back(shape.mesh.positions[i]); + vertices.push_back(attribute.vertices[i]); } for (int i = 0; i < shape.mesh.indices.size(); i++) { - indices.push_back(shape.mesh.indices[i]); + indices.push_back(shape.mesh.indices[i].vertex_index); } - int numTris = indices.size() / 3; + int numTris = shape.mesh.indices.size() / 3; if (numTris > 0) { btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); diff --git a/examples/ThirdPartyLibs/Wavefront/README.md b/examples/ThirdPartyLibs/Wavefront/README.md index 6f911e523..de226acd3 100644 --- a/examples/ThirdPartyLibs/Wavefront/README.md +++ b/examples/ThirdPartyLibs/Wavefront/README.md @@ -38,51 +38,102 @@ Licensed under 2 clause BSD. Usage ----- +Data format +attrib_t contains single and linear array of vertex data(position, normal and texcoord). - std::string inputfile = "cornell_box.obj"; - std::vector shapes; - - std::string err = tinyobj::LoadObj(shapes, inputfile.c_str()); - - if (!err.empty()) { - std::cerr << err << std::endl; - exit(1); - } - - std::cout << "# of shapes : " << shapes.size() << std::endl; - - for (size_t i = 0; i < shapes.size(); i++) { - printf("shape[%ld].name = %s\n", i, shapes[i].name.c_str()); - printf("shape[%ld].indices: %ld\n", i, shapes[i].mesh.indices.size()); - assert((shapes[i].mesh.indices.size() % 3) == 0); - for (size_t f = 0; f < shapes[i].mesh.indices.size(); f++) { - printf(" idx[%ld] = %d\n", f, shapes[i].mesh.indices[f]); - } - - printf("shape[%ld].vertices: %ld\n", i, shapes[i].mesh.positions.size()); - assert((shapes[i].mesh.positions.size() % 3) == 0); - for (size_t v = 0; v < shapes[i].mesh.positions.size() / 3; v++) { - printf(" v[%ld] = (%f, %f, %f)\n", v, - shapes[i].mesh.positions[3*v+0], - shapes[i].mesh.positions[3*v+1], - shapes[i].mesh.positions[3*v+2]); - } - - printf("shape[%ld].material.name = %s\n", i, shapes[i].material.name.c_str()); - printf(" material.Ka = (%f, %f ,%f)\n", shapes[i].material.ambient[0], shapes[i].material.ambient[1], shapes[i].material.ambient[2]); - printf(" material.Kd = (%f, %f ,%f)\n", shapes[i].material.diffuse[0], shapes[i].material.diffuse[1], shapes[i].material.diffuse[2]); - printf(" material.Ks = (%f, %f ,%f)\n", shapes[i].material.specular[0], shapes[i].material.specular[1], shapes[i].material.specular[2]); - printf(" material.Tr = (%f, %f ,%f)\n", shapes[i].material.transmittance[0], shapes[i].material.transmittance[1], shapes[i].material.transmittance[2]); - printf(" material.Ke = (%f, %f ,%f)\n", shapes[i].material.emission[0], shapes[i].material.emission[1], shapes[i].material.emission[2]); - printf(" material.Ns = %f\n", shapes[i].material.shininess); - printf(" material.map_Ka = %s\n", shapes[i].material.ambient_texname.c_str()); - printf(" material.map_Kd = %s\n", shapes[i].material.diffuse_texname.c_str()); - printf(" material.map_Ks = %s\n", shapes[i].material.specular_texname.c_str()); - printf(" material.map_Ns = %s\n", shapes[i].material.normal_texname.c_str()); - std::map::iterator it(shapes[i].material.unknown_parameter.begin()); - std::map::iterator itEnd(shapes[i].material.unknown_parameter.end()); - for (; it != itEnd; it++) { - printf(" material.%s = %s\n", it->first.c_str(), it->second.c_str()); - } - printf("\n"); +attrib_t::vertices => 3 floats per vertex + + v[0] v[1] v[2] v[3] v[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z | + +-----------+-----------+-----------+-----------+ +-----------+ + +attrib_t::normals => 3 floats per vertex + + n[0] n[1] n[2] n[3] n[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z | + +-----------+-----------+-----------+-----------+ +-----------+ + +attrib_t::texcoords => 2 floats per vertex + + t[0] t[1] t[2] t[3] t[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | u | v | u | v | u | v | u | v | .... | u | v | + +-----------+-----------+-----------+-----------+ +-----------+ + +attrib_t::colors => 3 floats per vertex(vertex color. optional) + + c[0] c[1] c[2] c[3] c[n-1] + +-----------+-----------+-----------+-----------+ +-----------+ + | x | y | z | x | y | z | x | y | z | x | y | z | .... | x | y | z | + +-----------+-----------+-----------+-----------+ +-----------+ + +Each shape_t::mesh_t does not contain vertex data but contains array index to attrib_t. See loader_example.cc for more details. + + +mesh_t::indices => array of vertex indices. + + +----+----+----+----+----+----+----+----+----+----+ +--------+ + | i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-1) | + +----+----+----+----+----+----+----+----+----+----+ +--------+ + +Each index has an array index to attrib_t::vertices, attrib_t::normals and attrib_t::texcoords. + +mesh_t::num_face_vertices => array of the number of vertices per face(e.g. 3 = triangle, 4 = quad , 5 or more = N-gons). + + + +---+---+---+ +---+ + | 3 | 4 | 3 | ...... | 3 | + +---+---+---+ +---+ + | | | | + | | | +-----------------------------------------+ + | | | | + | | +------------------------------+ | + | | | | + | +------------------+ | | + | | | | + |/ |/ |/ |/ + + mesh_t::indices + + | face[0] | face[1] | face[2] | | face[n-1] | + +----+----+----+----+----+----+----+----+----+----+ +--------+--------+--------+ + | i0 | i1 | i2 | i3 | i4 | i5 | i6 | i7 | i8 | i9 | ... | i(n-3) | i(n-2) | i(n-1) | + +----+----+----+----+----+----+----+----+----+----+ +--------+--------+--------+ + +```c++ +#define TINYOBJLOADER_IMPLEMENTATION // define this in only *one* .cc +#include "tiny_obj_loader.h" + +std::string inputfile = "cornell_box.obj"; +tinyobj::attrib_t attrib; +std::vector shapes; + +LoadObj(attrib, shapes, inputfile.c_str()); + +// Loop over shapes +for (size_t s = 0; s < shapes.size(); s++) { + // Loop over faces(polygon) + size_t index_offset = 0; + for (size_t f = 0; f < shapes[s].mesh.indices.size(); f++) { + int fv = 3; + // Loop over vertices in the face. + for (size_t v = 0; v < fv; v++) { + // access to vertex + tinyobj::index_t idx = shapes[s].mesh.indices[index_offset + v]; + tinyobj::real_t vx = attrib.vertices[3*idx.vertex_index+0]; + tinyobj::real_t vy = attrib.vertices[3*idx.vertex_index+1]; + tinyobj::real_t vz = attrib.vertices[3*idx.vertex_index+2]; + tinyobj::real_t nx = attrib.normals[3*idx.normal_index+0]; + tinyobj::real_t ny = attrib.normals[3*idx.normal_index+1]; + tinyobj::real_t nz = attrib.normals[3*idx.normal_index+2]; + tinyobj::real_t tx = attrib.texcoords[2*idx.texcoord_index+0]; + tinyobj::real_t ty = attrib.texcoords[2*idx.texcoord_index+1]; } + index_offset += fv; + } +} + +``` + diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index 9cd2f2092..d1b9c6362 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -70,25 +70,6 @@ std::istream& safeGetline(std::istream& is, std::string& t) } } #endif -struct vertex_index -{ - int v_idx, vt_idx, vn_idx, dummy; -}; -struct MyIndices -{ - int m_offset; - int m_numIndices; -}; - -// for std::map -static inline bool operator<(const vertex_index& a, const vertex_index& b) -{ - if (a.v_idx != b.v_idx) return (a.v_idx < b.v_idx); - if (a.vn_idx != b.vn_idx) return (a.vn_idx < b.vn_idx); - if (a.vt_idx != b.vt_idx) return (a.vt_idx < b.vt_idx); - - return false; -} static inline bool isSpace(const char c) { @@ -101,25 +82,33 @@ static inline bool isNewLine(const char c) } // Make index zero-base, and also support relative index. -static inline int fixIndex(int idx, int n) +static inline bool fixIndex(int idx, int n, int* ret) { - int i; + if (!ret) + { + return false; + } if (idx > 0) { - i = idx - 1; + (*ret) = idx - 1; + return true; } - else if (idx == 0) + + if (idx == 0) { - i = 0; + // zero is not allowed according to the spec. + return false; } - else - { // negative value = relative - i = n + idx; + + if (idx < 0) + { + (*ret) = n + idx; // negative value = relative + return true; } - return i; -} + return false; // never reach here. +} static inline std::string parseString(const char*& token) { std::string s; @@ -156,170 +145,184 @@ static inline void parseFloat3( z = parseFloat(token); } -// Parse triples: i, i/j/k, i//k, i/j -static vertex_index parseTriple( - const char*& token, - int vsize, - int vnsize, - int vtsize) +// Parse triples with index offsets: i, i/j/k, i//k, i/j +static bool parseTriple(const char** token, int vsize, int vnsize, int vtsize, + vertex_index_t* ret) { - vertex_index vi; - vi.vn_idx = -1; - vi.vt_idx = -1; - vi.v_idx = -1; - - vi.v_idx = fixIndex(atoi(token), vsize); - token += strcspn(token, "/ \t\r"); - if (token[0] != '/') + if (!ret) { - return vi; + return false; } - token++; - // i//k - if (token[0] == '/') + vertex_index_t vi(-1); + + if (!fixIndex(atoi((*token)), vsize, &(vi.v_idx))) { - token++; - vi.vn_idx = fixIndex(atoi(token), vnsize); - token += strcspn(token, "/ \t\r"); - return vi; + return false; } - // i/j/k or i/j - vi.vt_idx = fixIndex(atoi(token), vtsize); - token += strcspn(token, "/ \t\r"); - if (token[0] != '/') + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { - return vi; + (*ret) = vi; + return true; } + (*token)++; - // i/j/k - token++; // skip '/' - vi.vn_idx = fixIndex(atoi(token), vnsize); - token += strcspn(token, "/ \t\r"); - return vi; -} - -static unsigned int -updateVertex( - std::map& vertexCache, - std::vector& positions, - std::vector& normals, - std::vector& texcoords, - const std::vector& in_positions, - const std::vector& in_normals, - const std::vector& in_texcoords, - const vertex_index& i) -{ - const std::map::iterator it = vertexCache.find(i); - - if (it != vertexCache.end()) + // i//k + if ((*token)[0] == '/') { - // found cache - return it->second; + (*token)++; + if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx))) + { + return false; + } + (*token) += strcspn((*token), "/ \t\r"); + (*ret) = vi; + return true; } - assert(static_cast(in_positions.size()) > (3 * i.v_idx + 2)); - - positions.push_back(in_positions[3 * i.v_idx + 0]); - positions.push_back(in_positions[3 * i.v_idx + 1]); - positions.push_back(in_positions[3 * i.v_idx + 2]); - - if (i.vn_idx >= 0 && ((3 * i.vn_idx + 2) < in_normals.size())) + // i/j/k or i/j + if (!fixIndex(atoi((*token)), vtsize, &(vi.vt_idx))) { - normals.push_back(in_normals[3 * i.vn_idx + 0]); - normals.push_back(in_normals[3 * i.vn_idx + 1]); - normals.push_back(in_normals[3 * i.vn_idx + 2]); + return false; } - if (i.vt_idx >= 0) + (*token) += strcspn((*token), "/ \t\r"); + if ((*token)[0] != '/') { - int numTexCoords = in_texcoords.size(); - int index0 = 2 * i.vt_idx + 0; - int index1 = 2 * i.vt_idx + 1; + (*ret) = vi; + return true; + } - if (index0 >= 0 && (index0) < numTexCoords) - { - texcoords.push_back(in_texcoords[index0]); - } - if (index1 >= 0 && (index1) < numTexCoords) - { - texcoords.push_back(in_texcoords[index1]); - } + // i/j/k + (*token)++; // skip '/' + if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx))) + { + return false; } + (*token) += strcspn((*token), "/ \t\r"); - unsigned int idx = positions.size() / 3 - 1; - vertexCache[i] = idx; + (*ret) = vi; - return idx; + return true; } -static bool -exportFaceGroupToShape( - shape_t& shape, - const std::vector& in_positions, - const std::vector& in_normals, - const std::vector& in_texcoords, - const std::vector& faceGroup, - const material_t material, - const std::string name, - std::vector& allIndices) +static bool exportFaceGroupToShape(shape_t* shape, const std::vector& face_group, + const material_t material, const std::string& name, + const std::vector& v) { - if (faceGroup.empty()) + if (face_group.empty()) { return false; } + shape->name = name; // Flattened version of vertex data - std::vector positions; - std::vector normals; - std::vector texcoords; - std::map vertexCache; - std::vector indices; // Flatten vertices and indices - for (size_t i = 0; i < faceGroup.size(); i++) + for (size_t i = 0; i < face_group.size(); i++) { - const MyIndices& face = faceGroup[i]; + const face_t& face = face_group[i]; + size_t npolys = face.size(); + + if (npolys < 3) + { + // Face must have 3+ vertices. + continue; + } + vertex_index_t i0 = face[0]; + vertex_index_t i1(-1); + vertex_index_t i2 = face[1]; - vertex_index i0 = allIndices[face.m_offset]; - vertex_index i1; - i1.vn_idx = -1; - i1.vt_idx = -1; - i1.v_idx = -1; - vertex_index i2 = allIndices[face.m_offset + 1]; + face_t remainingFace = face; // copy + size_t guess_vert = 0; + vertex_index_t ind[3]; - size_t npolys = face.m_numIndices; //.size(); + // How many iterations can we do without decreasing the remaining + // vertices. + size_t remainingIterations = face.size(); + size_t previousRemainingVertices = remainingFace.size(); + while (remainingFace.size() > 3 && remainingIterations > 0) { - // Polygon -> triangle fan conversion - for (size_t k = 2; k < npolys; k++) + npolys = remainingFace.size(); + if (guess_vert >= npolys) { - i1 = i2; - i2 = allIndices[face.m_offset + k]; - - unsigned int v0 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i0); - unsigned int v1 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i1); - unsigned int v2 = updateVertex(vertexCache, positions, normals, texcoords, in_positions, in_normals, in_texcoords, i2); + guess_vert -= npolys; + } - indices.push_back(v0); - indices.push_back(v1); - indices.push_back(v2); + if (previousRemainingVertices != npolys) + { + // The number of remaining vertices decreased. Reset counters. + previousRemainingVertices = npolys; + remainingIterations = npolys; + } + else + { + // We didn't consume a vertex on previous iteration, reduce the + // available iterations. + remainingIterations--; } - } - } - // - // Construct shape. - // - shape.name = name; - shape.mesh.positions.swap(positions); - shape.mesh.normals.swap(normals); - shape.mesh.texcoords.swap(texcoords); - shape.mesh.indices.swap(indices); + for (size_t k = 0; k < 3; k++) + { + ind[k] = remainingFace[(guess_vert + k) % npolys]; + size_t vi = size_t(ind[k].v_idx); + } + // this triangle is an ear + { + index_t idx0, idx1, idx2; + idx0.vertex_index = ind[0].v_idx; + idx0.normal_index = ind[0].vn_idx; + idx0.texcoord_index = ind[0].vt_idx; + idx1.vertex_index = ind[1].v_idx; + idx1.normal_index = ind[1].vn_idx; + idx1.texcoord_index = ind[1].vt_idx; + idx2.vertex_index = ind[2].v_idx; + idx2.normal_index = ind[2].vn_idx; + idx2.texcoord_index = ind[2].vt_idx; + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + } - shape.material = material; + // remove v1 from the list + size_t removed_vert_index = (guess_vert + 1) % npolys; + while (removed_vert_index + 1 < npolys) + { + remainingFace[removed_vert_index] = + remainingFace[removed_vert_index + 1]; + removed_vert_index += 1; + } + remainingFace.pop_back(); + } + if (remainingFace.size() == 3) + { + i0 = remainingFace[0]; + i1 = remainingFace[1]; + i2 = remainingFace[2]; + { + index_t idx0, idx1, idx2; + idx0.vertex_index = i0.v_idx; + idx0.normal_index = i0.vn_idx; + idx0.texcoord_index = i0.vt_idx; + idx1.vertex_index = i1.v_idx; + idx1.normal_index = i1.vn_idx; + idx1.texcoord_index = i1.vt_idx; + idx2.vertex_index = i2.v_idx; + idx2.normal_index = i2.vn_idx; + idx2.texcoord_index = i2.vt_idx; + + shape->mesh.indices.push_back(idx0); + shape->mesh.indices.push_back(idx1); + shape->mesh.indices.push_back(idx2); + } + } + } + shape->material = material; return true; } @@ -329,7 +332,6 @@ void InitMaterial(material_t& material) material.ambient_texname = ""; material.diffuse_texname = ""; material.specular_texname = ""; - material.normal_texname = ""; for (int i = 0; i < 3; i++) { material.ambient[i] = 0.f; @@ -369,8 +371,8 @@ std::string LoadMtl( return err.str(); } #else - int fileHandle = fileIO->fileOpen(filepath.c_str(),"r"); - if (fileHandle<0) + int fileHandle = fileIO->fileOpen(filepath.c_str(), "r"); + if (fileHandle < 0) { err << "Cannot open file [" << filepath << "]" << std::endl; return err.str(); @@ -396,7 +398,7 @@ std::string LoadMtl( line = fileIO->readLine(fileHandle, tmpBuf, 1024); if (line) { - linebuf=line; + linebuf = line; } #endif @@ -420,7 +422,7 @@ std::string LoadMtl( // Skip leading space. const char* token = linebuf.c_str(); token += strspn(token, " \t"); - + assert(token); if (token[0] == '\0') continue; // empty line @@ -574,12 +576,13 @@ std::string LoadMtl( } } #ifndef USE_STREAM - while (line); + while (line) + ; #endif // flush last material. material_map.insert(std::pair(material.name, material)); - if (fileHandle>=0) + if (fileHandle >= 0) { fileIO->fileClose(fileHandle); } @@ -588,11 +591,16 @@ std::string LoadMtl( std::string LoadObj( + attrib_t& attrib, std::vector& shapes, const char* filename, const char* mtl_basepath, CommonFileIOInterface* fileIO) { + attrib.vertices.clear(); + attrib.normals.clear(); + attrib.texcoords.clear(); + shapes.clear(); std::string tmp = filename; if (!mtl_basepath) { @@ -604,13 +612,6 @@ LoadObj( mtl_basepath = tmp.c_str(); //fprintf(stderr, "MTL PATH '%s' orig '%s'\n", mtl_basepath, filename); } - - shapes.resize(0); - std::vector allIndices; - allIndices.reserve(1024 * 1024); - - MyIndices face; - std::stringstream err; #ifdef USE_STREAM std::ifstream ifs(filename); @@ -620,8 +621,8 @@ LoadObj( return err.str(); } #else - int fileHandle = fileIO->fileOpen(filename,"r"); - if (fileHandle<0) + int fileHandle = fileIO->fileOpen(filename, "r"); + if (fileHandle < 0) { err << "Cannot open file [" << filename << "]" << std::endl; return err.str(); @@ -629,16 +630,15 @@ LoadObj( #endif std::vector v; - v.reserve(1024 * 1024); std::vector vn; - vn.reserve(1024 * 1024); std::vector vt; - vt.reserve(1024 * 1024); - //std::vector > faceGroup; - std::vector faceGroup; - faceGroup.reserve(1024 * 1024); std::string name; + int greatest_v_idx = -1; + int greatest_vn_idx = -1; + int greatest_vt_idx = -1; + + std::vector faceGroup; // material std::map material_map; material_t material; @@ -664,10 +664,9 @@ LoadObj( line = fileIO->readLine(fileHandle, tmpBuf, 1024); if (line) { - linebuf=line; + linebuf = line; } #endif - // Trim newline '\r\n' or '\r' if (linebuf.size() > 0) { @@ -734,23 +733,34 @@ LoadObj( token += 2; token += strspn(token, " \t"); - face.m_offset = allIndices.size(); - face.m_numIndices = 0; + face_t face; + + face.reserve(3); while (!isNewLine(token[0])) { - vertex_index vi = parseTriple(token, v.size() / 3, vn.size() / 3, vt.size() / 2); - allIndices.push_back(vi); - face.m_numIndices++; - int n = strspn(token, " \t\r"); + vertex_index_t vi; + if (!parseTriple(&token, static_cast(v.size() / 3), + static_cast(vn.size() / 3), + static_cast(vt.size() / 2), &vi)) + { + err << "Failed parse `f' line(e.g. zero value for face index."; + return err.str(); + } + + greatest_v_idx = greatest_v_idx > vi.v_idx ? greatest_v_idx : vi.v_idx; + greatest_vn_idx = + greatest_vn_idx > vi.vn_idx ? greatest_vn_idx : vi.vn_idx; + greatest_vt_idx = + greatest_vt_idx > vi.vt_idx ? greatest_vt_idx : vi.vt_idx; + + face.push_back(vi); + size_t n = strspn(token, " \t\r"); token += n; } - faceGroup.push_back(face); - continue; } - // use mtl if ((0 == strncmp(token, "usemtl", 6)) && isSpace((token[6]))) { @@ -777,10 +787,10 @@ LoadObj( token += 7; sscanf(token, "%s", namebuf); - std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath,fileIO); + std::string err_mtl = LoadMtl(material_map, namebuf, mtl_basepath, fileIO); if (!err_mtl.empty()) { - //faceGroup.resize(0); // for safety + //face_group.resize(0); // for safety //return err_mtl; } continue; @@ -791,7 +801,7 @@ LoadObj( { // flush previous face group. shape_t shape; - bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices); + bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v); if (ret) { shapes.push_back(shape); @@ -827,7 +837,7 @@ LoadObj( { // flush previous face group. shape_t shape; - bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices); + bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v); if (ret) { shapes.push_back(shape); @@ -847,18 +857,23 @@ LoadObj( // Ignore unknown command. } #ifndef USE_STREAM - while (line); + while (line) + ; #endif shape_t shape; - bool ret = exportFaceGroupToShape(shape, v, vn, vt, faceGroup, material, name, allIndices); + bool ret = exportFaceGroupToShape(&shape, faceGroup, material, name, v); if (ret) { shapes.push_back(shape); } faceGroup.resize(0); // for safety - if (fileHandle>=0) + attrib.vertices.swap(v); + attrib.normals.swap(vn); + attrib.texcoords.swap(vt); + + if (fileHandle >= 0) { fileIO->fileClose(fileHandle); } diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h index 0319b7c0d..5ce9b00d2 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h @@ -14,6 +14,17 @@ struct CommonFileIOInterface; namespace tinyobj { +struct vertex_index_t +{ + int v_idx, vt_idx, vn_idx; + vertex_index_t() : v_idx(-1), vt_idx(-1), vn_idx(-1) {} + explicit vertex_index_t(int idx) : v_idx(idx), vt_idx(idx), vn_idx(idx) {} + vertex_index_t(int vidx, int vtidx, int vnidx) + : v_idx(vidx), vt_idx(vtidx), vn_idx(vnidx) {} +}; + +using face_t = std::vector; + typedef struct { std::string name; @@ -24,21 +35,27 @@ typedef struct float transmittance[3]; float emission[3]; float shininess; - float transparency; + float transparency; // 1 == opaque; 0 == fully transparent - std::string ambient_texname; - std::string diffuse_texname; - std::string specular_texname; + std::string ambient_texname; // map_Ka + std::string diffuse_texname; // map_Kd + std::string specular_texname; // map_Ks std::string normal_texname; std::map unknown_parameter; } material_t; +// Index struct to support different indices for vtx/normal/texcoord. +// -1 means not used. typedef struct { - std::vector positions; - std::vector normals; - std::vector texcoords; - std::vector indices; + int vertex_index; + int normal_index; + int texcoord_index; +} index_t; + +typedef struct +{ + std::vector indices; } mesh_t; typedef struct @@ -48,6 +65,14 @@ typedef struct mesh_t mesh; } shape_t; +// Vertex attributes +struct attrib_t +{ + std::vector vertices; // 'v'(xyz) + std::vector normals; // 'vn' + std::vector texcoords; // 'vt'(uv) + attrib_t() {} +}; /// Loads .obj from a file. /// 'shapes' will be filled with parsed shape data /// The function returns error string. @@ -55,12 +80,14 @@ typedef struct /// 'mtl_basepath' is optional, and used for base path for .mtl file. #ifdef USE_STREAM std::string LoadObj( + attrib_t& attrib, std::vector& shapes, // [output] const char* filename, const char* mtl_basepath = NULL); #else std::string LoadObj( + attrib_t& attrib, std::vector& shapes, const char* filename, const char* mtl_basepath, -- cgit v1.2.1 From eacebc80d54fdb19152805901f51137b1ae48172 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Wed, 14 Aug 2019 11:01:48 -0700 Subject: fix compile --- examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h index 5ce9b00d2..1fb15b34a 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h @@ -23,7 +23,7 @@ struct vertex_index_t : v_idx(vidx), vt_idx(vtidx), vn_idx(vnidx) {} }; -using face_t = std::vector; +typedef std::vector face_t; typedef struct { -- cgit v1.2.1