diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 4e40d7072..eb7565490 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -388,6 +388,8 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp ../RigidBody/KinematicRigidBodyExample.cpp + ../ReducedDeformableDemo/BasicTest.cpp + ../ReducedDeformableDemo/BasicTest.h ../Constraints/TestHingeTorque.cpp ../Constraints/TestHingeTorque.h ../Constraints/ConstraintDemo.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 9f8ab035c..6bbd7363e 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -72,6 +72,7 @@ #include "../RoboticsLearning/R2D2GraspExample.h" #include "../RoboticsLearning/KukaGraspExample.h" #include "../RoboticsLearning/GripperGraspExample.h" +#include "../ReducedDeformableDemo/BasicTest.h" #include "../InverseKinematics/InverseKinematicsExample.h" #ifdef B3_ENABLE_TINY_AUDIO @@ -214,6 +215,10 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc), ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), + + ExampleEntry(0, "Reduced Deformabe Body"), + ExampleEntry(1, "BasicRD", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc), + // ExampleEntry(1, "Simple Reduced Deformable Test", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc), #ifdef INCLUDE_CLOTH_DEMOS ExampleEntry(0, "Soft Body"), diff --git a/examples/ReducedDeformableDemo/BasicTest.cpp b/examples/ReducedDeformableDemo/BasicTest.cpp new file mode 100644 index 000000000..6d4ffd775 --- /dev/null +++ b/examples/ReducedDeformableDemo/BasicTest.cpp @@ -0,0 +1,286 @@ +/* + Bullet Continuous Collision Detection and Physics Library + 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, + 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 "BasicTest.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonDeformableBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The BasicTest shows the contact between volumetric deformable objects and rigid objects. +static btScalar E = 50; +static btScalar nu = 0.3; +// static btScalar damping_alpha = 0.1; +// static btScalar damping_beta = 0.01; +static btScalar damping_alpha = 0.0; +static btScalar damping_beta = 0.0; +static btScalar COLLIDING_VELOCITY = 0; + +class BasicTest : public CommonDeformableBodyBase +{ + // btDeformableLinearElasticityForce* m_linearElasticity; + btDeformableMassSpringForce* m_massSpring; + + btAlignedObjectArray m_eigenvalues, m_Kr, m_Mr; + btAlignedObjectArray > m_modes; // each inner array is a mode, outer array size = n_modes + unsigned int m_numModes; + bool first_step; + +public: + BasicTest(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper), m_numModes(0) + { + // m_linearElasticity = 0; + m_massSpring = 0; + first_step = true; + } + virtual ~BasicTest() + { + } + void initPhysics(); + + void exitPhysics(); + + // TODO: disable pick force, non-interactive for now. + bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { + return false; + } + + void resetCamera() + { + float dist = 20; + float pitch = 0; + float yaw = 90; + float targetPos[3] = {0, 3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void Ctor_RbUpStack() + { + float mass = 0.5; + btCollisionShape* shape = new btBoxShape(btVector3(2, 2, 2)); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,-2,0)); + btRigidBody* rb = createRigidBody(mass, startTransform, shape); + rb->setLinearVelocity(btVector3(0,+COLLIDING_VELOCITY, 0)); + } + + void stepSimulation(float deltaTime) + { + // TODO: remove this. very hacky way of adding initial deformation + btSoftBody* psb = static_cast(m_dynamicsWorld)->getSoftBodyArray()[0]; + if (first_step && !psb->m_bUpdateRtCst) { + // save rest length + btAlignedObjectArray rest_length; + + // assign initial displacement (perturb each nodes by mode 1) + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + for (int k = 0; k < 3; ++k) + { + psb->m_nodes[i].m_x[k] += m_modes[0][3 * i + k]; + } + } + first_step = false; + } + float internalTimeStep = 1. / 60.f; + m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + } + + virtual void renderScene() + { + CommonDeformableBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* 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 BasicTest::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(); + + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, 0, 0); + m_dynamicsWorld->setGravity(gravity); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + // create volumetric soft body + { + std::string filename("../../../examples/SoftDemo/mesh.vtk"); + btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str()); + + // read in eigenmodes, stiffness and mass matrices + unsigned int buffer_n_modes = 0; + + std::string eigenvalues_file("../../../examples/SoftDemo/eigenvalues.bin"); + btSoftBodyHelpers::readBinary(m_eigenvalues, buffer_n_modes, eigenvalues_file.c_str()); + m_numModes = buffer_n_modes; + + std::string Kr_file("../../../examples/SoftDemo/K_r_diag_mat.bin"); + btSoftBodyHelpers::readBinary(m_Kr, buffer_n_modes, Kr_file.c_str()); + + std::string Mr_file("../../../examples/SoftDemo/M_r_diag_mat.bin"); + btSoftBodyHelpers::readBinary(m_Mr, buffer_n_modes, Mr_file.c_str()); + + std::string modes_file("../../../examples/SoftDemo/modes.bin"); + btSoftBodyHelpers::readBinaryMat(m_modes, 3 * psb->m_nodes.size(), buffer_n_modes, modes_file.c_str()); // default to 3D + + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 7, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(0.5); + 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.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->m_sleepingThreshold = 0; + btSoftBodyHelpers::generateBoundaryFaces(psb); + + psb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); + + // btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); + // m_linearElasticity = linearElasticity; + + m_massSpring = new btDeformableMassSpringForce(10.0, 0.0); //TODO: this stiffness needs to match the modes + + getDeformableDynamicsWorld()->addForce(psb, m_massSpring); + m_forces.push_back(m_massSpring); + } + getDeformableDynamicsWorld()->setReducedModelFlag(true); + getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setLineSearch(false); + getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; + // add a few rigid bodies + // Ctor_RbUpStack(); // TODO: no rigid body for now + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + // { + // SliderParams slider("Young's Modulus", &E); + // slider.m_minVal = 0; + // slider.m_maxVal = 2000; + // if (m_guiHelper->getParameterInterface()) + // m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + // } + // { + // SliderParams slider("Poisson Ratio", &nu); + // slider.m_minVal = 0.05; + // slider.m_maxVal = 0.49; + // if (m_guiHelper->getParameterInterface()) + // m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + // } + // { + // SliderParams slider("Mass Damping", &damping_alpha); + // slider.m_minVal = 0; + // slider.m_maxVal = 1; + // if (m_guiHelper->getParameterInterface()) + // m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + // } + // { + // SliderParams slider("Stiffness Damping", &damping_beta); + // slider.m_minVal = 0; + // slider.m_maxVal = 0.1; + // if (m_guiHelper->getParameterInterface()) + // m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + // } +} + +void BasicTest::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + removePickingConstraint(); + //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 forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + + //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* ReducedBasicTestCreateFunc(struct CommonExampleOptions& options) +{ + return new BasicTest(options.m_guiHelper); +} + + diff --git a/examples/ReducedDeformableDemo/BasicTest.h b/examples/ReducedDeformableDemo/BasicTest.h new file mode 100644 index 000000000..aa15e5843 --- /dev/null +++ b/examples/ReducedDeformableDemo/BasicTest.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + 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, + 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 _REDUCED_BASIC_TEST_H +#define _REDUCED_BASIC_TEST_H + +class CommonExampleInterface* ReducedBasicTestCreateFunc(struct CommonExampleOptions& options); + +#endif //_REDUCED_BASIC_TEST_H diff --git a/examples/SoftDemo/K_r_diag_mat.bin b/examples/SoftDemo/K_r_diag_mat.bin new file mode 100644 index 000000000..af8b7d78e Binary files /dev/null and b/examples/SoftDemo/K_r_diag_mat.bin differ diff --git a/examples/SoftDemo/M_r_diag_mat.bin b/examples/SoftDemo/M_r_diag_mat.bin new file mode 100644 index 000000000..c1c2e634f Binary files /dev/null and b/examples/SoftDemo/M_r_diag_mat.bin differ diff --git a/examples/SoftDemo/eigenvalues.bin b/examples/SoftDemo/eigenvalues.bin new file mode 100644 index 000000000..5b82cc8a5 Binary files /dev/null and b/examples/SoftDemo/eigenvalues.bin differ diff --git a/examples/SoftDemo/mesh.vtk b/examples/SoftDemo/mesh.vtk new file mode 100644 index 000000000..cbab3e1a2 --- /dev/null +++ b/examples/SoftDemo/mesh.vtk @@ -0,0 +1,83 @@ +#vtk DataFile Version 2.0 +I don't think this matters +ASCII +DATASET UNSTRUCTURED_GRID +POINTS 28 double +-0.5000000000000000 -0.2500000000000000 -2.0000000000000000 +-0.5000000000000000 0.2500000000000000 -2.0000000000000000 +0.5000000000000000 -0.2500000000000000 -2.0000000000000000 +0.5000000000000000 0.2500000000000000 -2.0000000000000000 +-0.5000000000000000 -0.2500000000000000 2.0000000000000000 +-0.5000000000000000 0.2500000000000000 2.0000000000000000 +0.5000000000000000 -0.2500000000000000 2.0000000000000000 +0.5000000000000000 0.2500000000000000 2.0000000000000000 +-0.5000000000000000 -0.2500000000000000 1.0000000000000000 +-0.5000000000000000 -0.2500000000000000 0.0000000000000000 +-0.5000000000000000 -0.2500000000000000 -1.0000000000000000 +0.5000000000000000 -0.2500000000000000 -1.0000000000000000 +0.5000000000000000 -0.2500000000000000 0.0000000000000000 +0.5000000000000000 -0.2500000000000000 1.0000000000000000 +-0.5000000000000000 0.2500000000000000 1.0000000000000000 +-0.5000000000000000 0.2500000000000000 0.0000000000000000 +-0.5000000000000000 0.2500000000000000 -1.0000000000000000 +0.5000000000000000 0.2500000000000000 1.0000000000000000 +0.5000000000000000 0.2500000000000000 0.0000000000000000 +0.5000000000000000 0.2500000000000000 -1.0000000000000000 +0.0000000000000000 -0.2500000000000000 -1.5000000000000000 +0.0000000000000000 -0.2500000000000000 -0.5000000000000000 +0.0000000000000000 -0.2500000000000000 0.5000000000000000 +0.0000000000000000 -0.2500000000000000 1.5000000000000000 +0.0000000000000000 0.2500000000000000 1.5000000000000000 +0.0000000000000000 0.2500000000000000 0.5000000000000000 +0.0000000000000000 0.2500000000000000 -0.5000000000000000 +0.0000000000000000 0.2500000000000000 -1.5000000000000000 +CELLS 48 192 +4 22 12 17 13 +4 20 26 21 11 +4 23 8 5 24 +4 11 27 26 19 +4 27 11 26 20 +4 17 22 24 25 +4 7 23 13 6 +4 22 17 24 13 +4 9 25 21 15 +4 24 22 8 14 +4 24 8 22 23 +4 25 9 21 22 +4 23 7 13 24 +4 15 10 16 26 +4 14 25 9 15 +4 25 14 9 22 +4 24 23 22 13 +4 15 10 21 9 +4 15 21 10 26 +4 24 25 22 14 +4 2 11 27 20 +4 7 5 23 6 +4 11 21 12 26 +4 20 1 0 16 +4 18 25 12 17 +4 18 12 25 26 +4 5 7 23 24 +4 26 21 25 15 +4 20 16 27 1 +4 21 25 22 12 +4 20 16 10 26 +4 20 27 16 26 +4 20 10 16 0 +4 11 12 19 26 +4 14 22 8 9 +4 8 23 5 4 +4 24 14 8 5 +4 11 2 27 3 +4 2 27 1 20 +4 12 18 19 26 +4 27 2 1 3 +4 24 13 17 7 +4 20 2 0 1 +4 5 23 6 4 +4 20 10 21 26 +4 12 22 17 25 +4 3 27 11 19 +4 21 26 25 12 +CELL_TYPES 10 diff --git a/examples/SoftDemo/modes.bin b/examples/SoftDemo/modes.bin new file mode 100644 index 000000000..b2d19ef74 Binary files /dev/null and b/examples/SoftDemo/modes.bin differ diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index f63e48f9a..fd6d5920b 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1487,6 +1487,46 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) fs.close(); } +// read in binary files +void btSoftBodyHelpers::readBinary(btAlignedObjectArray& vec, unsigned int& size, const char* file) +{ + std::ifstream f_in(file, std::ios::in | std::ios::binary); + // first get size + f_in.read((char*)&size, sizeof(uint32_t)); + + // read data + vec.resize(size); + double temp; + for (int i = 0; i < size; i++) { + f_in.read((char*)&temp, sizeof(double)); + vec[i] = btScalar(temp); + } + f_in.close(); +} + +void btSoftBodyHelpers::readBinaryMat(btAlignedObjectArray >& mat, const unsigned int n_row, const unsigned int n_col, const char* file) +{ + std::ifstream f_in(file, std::ios::in | std::ios::binary); + // first get size + unsigned int v_size; + f_in.read((char*)&v_size, sizeof(uint32_t)); + btAssert(v_size == n_row * n_col); + + // read data + mat.resize(n_col); + for (int i = 0; i < n_col; ++i) + { + mat[i].resize(n_row); + for (int j = 0; j < n_row; ++j) + { + double temp; + f_in.read((char*)&temp, sizeof(double)); + mat[i][j] = btScalar(temp); + } + } + f_in.close(); +} + void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb) { std::ifstream fs_read; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index 237d29761..c3a4309cf 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -144,6 +144,11 @@ struct btSoftBodyHelpers bool bfacesfromtetras); static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); + // read in a binary vector + static void readBinary(btAlignedObjectArray& vec, unsigned int& size, const char* file); + // read in a binary matrix (must provide matrix size) + static void readBinaryMat(btAlignedObjectArray >& mat, const unsigned int n_row, const unsigned int n_col, const char* file); + static void writeObj(const char* file, const btSoftBody* psb); static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);