From fb3b306af19aac4d7581702a7676c2452fb9658a Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Wed, 9 Feb 2022 17:00:43 -0800 Subject: [PATCH 01/10] load deformable deterministic setting --- examples/DeformableDemo/LoadDeformed.cpp | 264 +++++++++++++++++++++ examples/DeformableDemo/LoadDeformed.h | 19 ++ examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 2 + src/BulletSoftBody/btSoftBody.cpp | 14 ++ src/BulletSoftBody/btSoftBody.h | 3 + src/BulletSoftBody/btSoftBodyHelpers.cpp | 164 +++++++++++++ src/BulletSoftBody/btSoftBodyHelpers.h | 5 + 8 files changed, 473 insertions(+) create mode 100644 examples/DeformableDemo/LoadDeformed.cpp create mode 100644 examples/DeformableDemo/LoadDeformed.h diff --git a/examples/DeformableDemo/LoadDeformed.cpp b/examples/DeformableDemo/LoadDeformed.cpp new file mode 100644 index 000000000..dc1991f2c --- /dev/null +++ b/examples/DeformableDemo/LoadDeformed.cpp @@ -0,0 +1,264 @@ +/* + 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 "LoadDeformed.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" +#include "../Utils/b3BulletDefaultFileIO.h" + +class LoadDeformed : public CommonDeformableBodyBase +{ + int steps; + btSoftBody* psb; + char filename; + int reset_frame; + float sim_time; + +public: + LoadDeformed(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper) + { + steps = 0; + psb = nullptr; + reset_frame = 0; + sim_time=0; + } + virtual ~LoadDeformed() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 2; + float pitch = -45; + 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) + { + steps ++; + sim_time+=deltaTime; +//// int seconds = 1/deltaTime; + if(0){ +// if (reset_frame==0 && steps<100){ +//// printf("steps %d, seconds %d, steps/seconds %d\n", steps,seconds,steps/seconds); + char filename[100]; + sprintf(filename, "%s_%d_%d.txt", "states", reset_frame, steps); + btSoftBodyHelpers::writeState(filename, psb); + } + if(sim_time+reset_frame*0.05>=5) exit(0); + float internalTimeStep = 1. / 240.f; +// float internalTimeStep = 0.1f; + m_dynamicsWorld->stepSimulation(deltaTime, deltaTime/internalTimeStep, internalTimeStep); + } + + void addCloth(const btVector3& origin); + + 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 LoadDeformed::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, -9.8, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.))); + groundShape->setMargin(0.02); + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -3.5, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + 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(4); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + addCloth(btVector3(0, 1, 0)); + getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void LoadDeformed::addCloth(const btVector3& origin) +// create a piece of cloth +{ + const btScalar s = 0.6; + const btScalar h = 0; + + psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2*s), + btVector3(+s, h, -2*s), + btVector3(-s, h, +2*s), + btVector3(+s, h, +2*s), + 15,30, + 0, true, 0.0); + + psb->getCollisionShape()->setMargin(0.02); + psb->generateBendingConstraints(2); + psb->setTotalMass(.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.1; + psb->rotate(btQuaternion(0, SIMD_PI / 2, 0)); + btTransform clothTransform; + clothTransform.setIdentity(); + clothTransform.setOrigin(btVector3(0,0.2,0)+origin); + psb->transform(clothTransform); + + b3BulletDefaultFileIO fileio; + char absolute_path[1024]; + char filename[100]; + sprintf(filename, "/Users/fuchuyuan/Documents/mybullet/build_cmake/examples/ExampleBrowser/states_0_%d.txt", reset_frame); + fileio.findResourcePath(filename, absolute_path, 1024); + btAlignedObjectArray qs; + btAlignedObjectArray vs; + btSoftBodyHelpers:: loadDeformableState(qs, vs, absolute_path, &fileio); + if(reset_frame>0) + psb->updateState(qs,vs); + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; +// psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->setCollisionFlags(0); + psb->setCacheBarycenter(true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->setSelfCollision(false); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.2, true); + psb->setSpringStiffness(4); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + btVector3 gravity = btVector3(0, -9.8, 0); + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); +// getDeformableDynamicsWorld()->setUseProjection(true); + m_forces.push_back(gravity_force); +} + +void LoadDeformed::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* LoadDeformedCreateFunc(struct CommonExampleOptions& options) +{ + return new LoadDeformed(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/LoadDeformed.h b/examples/DeformableDemo/LoadDeformed.h new file mode 100644 index 000000000..b2c1a1598 --- /dev/null +++ b/examples/DeformableDemo/LoadDeformed.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 _LOAD_DEFORMED_H +#define _LOAD_DEFORMED_H + +class CommonExampleInterface* LoadDeformedCreateFunc(struct CommonExampleOptions& options); + +#endif //_LOAD_DEFORMED_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 4e40d7072..ff4cf51e1 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -379,6 +379,8 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/Collide.cpp ../DeformableDemo/Collide.h ../DeformableDemo/LargeDeformation.cpp + ../DeformableDemo/LoadDeformed.h + ../DeformableDemo/LoadDeformed.cpp ../DeformableDemo/LargeDeformation.h ../DeformableDemo/DeformableClothAnchor.cpp ../DeformableDemo/DeformableClothAnchor.h diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 9f8ab035c..6d147f0d8 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -55,6 +55,7 @@ #include "../DeformableDemo/DeformableMultibody.h" #include "../DeformableDemo/VolumetricDeformable.h" #include "../DeformableDemo/LargeDeformation.h" +#include "../DeformableDemo/LoadDeformed.h" #include "../DeformableDemo/Collide.h" #include "../DeformableDemo/GraspDeformable.h" #include "../DeformableDemo/DeformableContact.h" @@ -209,6 +210,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), ExampleEntry(1, "Extreme Deformation", "Recovery from extreme deformation", LargeDeformationCreateFunc), + ExampleEntry(1, "Load Deformed", "Reconstruct a deformed object", LoadDeformedCreateFunc), ExampleEntry(1, "Colliding Test", "Volumetric deformable collide with rigid box", CollideCreateFunc), ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc), ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc), diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c6226a008..862d5e35e 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1483,6 +1483,20 @@ void btSoftBody::randomizeConstraints() #undef NEXTRAND } + +void btSoftBody::updateState(const btAlignedObjectArray& q, const btAlignedObjectArray& v){ + int node_count =m_nodes.size(); + btAssert(node_count == q.size()); + btAssert(node_count == v.size()); + for (int i = 0; i& qs, const btAlignedObjectArray& vs); + /* Release clusters */ void releaseCluster(int index); void releaseClusters(); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index e464c4668..dd7abb48e 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1487,6 +1487,170 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) fs.close(); } +static inline bool isSpace(const char c) +{ + return (c == ' ') || (c == '\t'); +} +static inline bool isNewLine(const char c) +{ + return (c == '\r') || (c == '\n') || (c == '\0'); +} +static inline float parseFloat(const char*& token) +{ + token += strspn(token, " \t"); + float f = (float)atof(token); + token += strcspn(token, " \t\r"); + return f; +} +static inline void parseFloat3( + float& x, float& y, float& z, + const char*& token) +{ + x = parseFloat(token); + y = parseFloat(token); + z = parseFloat(token); +} + + +void btSoftBodyHelpers:: writeState(const char* file, const btSoftBody* psb){ + std::ofstream fs; + fs.open(file); + btAssert(fs); + fs<< std::scientific<m_nodes.size(); ++i) + { + fs << "q"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_q[d]; + } + fs << "\n"; + } + + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_v[d]; + } + fs << "\n"; + } + fs.close(); +} + + +std::string btSoftBodyHelpers:: loadDeformableState(btAlignedObjectArray& qs, btAlignedObjectArray& vs, const char* filename, CommonFileIOInterface* fileIO){ + { + qs.clear(); + vs.clear(); + std::string tmp = filename; + std::stringstream err; + #ifdef USE_STREAM + std::ifstream ifs(filename); + if (!ifs) + { + err << "Cannot open file [" << filename << "]" << std::endl; + return err.str(); + } + #else + int fileHandle = fileIO->fileOpen(filename, "r"); + if (fileHandle < 0) + { + err << "Cannot open file [" << filename << "]" << std::endl; + return err.str(); + } + #endif + + std::string name; + + + int maxchars = 8192; // Alloc enough size. + std::vector buf(maxchars); // Alloc enough size. + std::string linebuf; + linebuf.reserve(maxchars); + + #ifdef USE_STREAM + while (ifs.peek() != -1) + #else + char* line = 0; + do + #endif + { + linebuf.resize(0); + #ifdef USE_STREAM + safeGetline(ifs, linebuf); + #else + char tmpBuf[1024]; + line = fileIO->readLine(fileHandle, tmpBuf, 1024); + if (line) + { + linebuf = line; + } + #endif + // Trim newline '\r\n' or '\r' + if (linebuf.size() > 0) + { + if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); + } + if (linebuf.size() > 0) + { + if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); + } + + // Skip if empty line. + if (linebuf.empty()) + { + continue; + } + + // Skip leading space. + const char* token = linebuf.c_str(); + token += strspn(token, " \t"); + + assert(token); + if (token[0] == '\0') continue; // empty line + + if (token[0] == '#') continue; // comment line + + // q + if (token[0] == 'q' && isSpace((token[1]))) + { + token += 2; + float x, y, z; + parseFloat3(x, y, z, token); + qs.push_back(btVector3(x,y,z)); + continue; + } + + // v + if (token[0] == 'v' && isSpace((token[1]))) + { + token += 3; + float x, y, z; + parseFloat3(x, y, z, token); + vs.push_back(btVector3(x,y,z)); + continue; + } + + // Ignore unknown command. + } + #ifndef USE_STREAM + while (line) + ; + #endif + + if (fileHandle >= 0) + { + fileIO->fileClose(fileHandle); + } + return err.str(); + } +} + + 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 b292d90d1..7dfd58590 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define BT_SOFT_BODY_HELPERS_H #include "btSoftBody.h" +#include "../../examples/CommonInterfaces/CommonFileIOInterface.h" #include #include // @@ -145,6 +146,10 @@ struct btSoftBodyHelpers static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); static void writeObj(const char* file, const btSoftBody* psb); + + static void writeState(const char* file, const btSoftBody* psb); + + static std::string loadDeformableState(btAlignedObjectArray& qs, btAlignedObjectArray& vs, const char* filename, CommonFileIOInterface* fileIO); static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); From 07de08a38e0f94332017982185be3d526b9fe3c5 Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Sun, 13 Feb 2022 20:49:09 -0800 Subject: [PATCH 02/10] request mesh velocity --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 14 ++++++++++++-- examples/SharedMemory/SharedMemoryPublic.h | 5 +++-- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0d826af41..9f1c7b98a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5606,6 +5606,8 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S { separateRenderMesh = (psb->m_renderNodes.size() != 0); } + bool requestVelocity = flags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY; + int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; @@ -5614,14 +5616,22 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S { if (separateRenderMesh) { - const btSoftBody::RenderNode& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; + if(requestVelocity){ + b3Warning("Request mesh velocity not implemented for Render Mesh."); + return hasStatus; + } verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z()); } else { const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; - verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z()); + if(!requestVelocity){ + verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z()); + } + else{ + verticesOut[i].setValue(n.m_v.x(), n.m_v.y(), n.m_v.z()); + } } } sizeInBytes = verticesCopied * sizeof(btVector3); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2649fc44d..b1a560339 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -465,8 +465,9 @@ struct b3MeshVertex enum eMeshDataFlags { B3_MESH_DATA_SIMULATION_MESH=1, - B3_MESH_DATA_SIMULATION_INDICES, - B3_MESH_DATA_GRAPHICS_INDICES, + B3_MESH_DATA_SIMULATION_INDICES=2, + B3_MESH_DATA_GRAPHICS_INDICES=4, + B3_MESH_DATA_SIMULATION_MESH_VELOCITY=8, }; enum eMeshDataEnum From 236d39d2ec20be7ffe68d21b632eb5e845d77a89 Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Sun, 13 Feb 2022 21:16:24 -0800 Subject: [PATCH 03/10] reset deformable velocity --- .../PhysicsServerCommandProcessor.cpp | 17 ++++++++++++++--- examples/SharedMemory/SharedMemoryPublic.h | 2 +- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9f1c7b98a..2506a767f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5512,13 +5512,24 @@ bool PhysicsServerCommandProcessor::processResetMeshDataCommand(const struct Sha int numVertices = psb->m_nodes.size(); if (clientCmd.m_resetMeshDataArgs.m_numVertices == numVertices) { + if(clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY){ for (int i = 0; i < numVertices; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; - n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); + n.m_v.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); + n.m_vn.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); } - serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED; - } + } + else{ + for (int i = 0; i < numVertices; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); + n.m_q.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); + } + } + serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED; + } } #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b1a560339..90143b55e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -467,7 +467,7 @@ enum eMeshDataFlags B3_MESH_DATA_SIMULATION_MESH=1, B3_MESH_DATA_SIMULATION_INDICES=2, B3_MESH_DATA_GRAPHICS_INDICES=4, - B3_MESH_DATA_SIMULATION_MESH_VELOCITY=8, + B3_MESH_DATA_SIMULATION_MESH_VELOCITY=8, }; enum eMeshDataEnum From 450c95d0b96032814ef3cd4f5c8b67a7fad3e1e4 Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Fri, 18 Feb 2022 14:52:03 -0800 Subject: [PATCH 04/10] format files --- examples/DeformableDemo/LoadDeformed.cpp | 397 +++++++++++------------ src/BulletSoftBody/btSoftBody.cpp | 27 +- src/BulletSoftBody/btSoftBody.h | 12 +- src/BulletSoftBody/btSoftBodyHelpers.cpp | 266 ++++++++------- src/BulletSoftBody/btSoftBodyHelpers.h | 8 +- 5 files changed, 353 insertions(+), 357 deletions(-) diff --git a/examples/DeformableDemo/LoadDeformed.cpp b/examples/DeformableDemo/LoadDeformed.cpp index dc1991f2c..55c322779 100644 --- a/examples/DeformableDemo/LoadDeformed.cpp +++ b/examples/DeformableDemo/LoadDeformed.cpp @@ -29,236 +29,233 @@ class LoadDeformed : public CommonDeformableBodyBase { - int steps; - btSoftBody* psb; - char filename; - int reset_frame; - float sim_time; - + int steps; + btSoftBody* psb; + char filename; + int reset_frame; + float sim_time; + public: - LoadDeformed(struct GUIHelperInterface* helper) - : CommonDeformableBodyBase(helper) - { - steps = 0; - psb = nullptr; - reset_frame = 0; - sim_time=0; - } - virtual ~LoadDeformed() - { - } - void initPhysics(); + LoadDeformed(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper) + { + steps = 0; + psb = nullptr; + reset_frame = 0; + sim_time = 0; + } + virtual ~LoadDeformed() + { + } + void initPhysics(); - void exitPhysics(); + void exitPhysics(); - void resetCamera() - { - float dist = 2; - float pitch = -45; - 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) - { - steps ++; - sim_time+=deltaTime; -//// int seconds = 1/deltaTime; - if(0){ -// if (reset_frame==0 && steps<100){ -//// printf("steps %d, seconds %d, steps/seconds %d\n", steps,seconds,steps/seconds); - char filename[100]; - sprintf(filename, "%s_%d_%d.txt", "states", reset_frame, steps); - btSoftBodyHelpers::writeState(filename, psb); - } - if(sim_time+reset_frame*0.05>=5) exit(0); - float internalTimeStep = 1. / 240.f; -// float internalTimeStep = 0.1f; - m_dynamicsWorld->stepSimulation(deltaTime, deltaTime/internalTimeStep, internalTimeStep); - } - - void addCloth(const btVector3& origin); - - 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 resetCamera() + { + float dist = 2; + float pitch = -45; + 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) + { + steps++; + sim_time += deltaTime; + //// int seconds = 1/deltaTime; + if (0) + { + // if (reset_frame==0 && steps<100){ + //// printf("steps %d, seconds %d, steps/seconds %d\n", steps,seconds,steps/seconds); + char filename[100]; + sprintf(filename, "%s_%d_%d.txt", "states", reset_frame, steps); + btSoftBodyHelpers::writeState(filename, psb); + } + if (sim_time + reset_frame * 0.05 >= 5) exit(0); + float internalTimeStep = 1. / 240.f; + // float internalTimeStep = 0.1f; + m_dynamicsWorld->stepSimulation(deltaTime, deltaTime / internalTimeStep, internalTimeStep); + } + + void addCloth(const btVector3& origin); + + 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 LoadDeformed::initPhysics() { - m_guiHelper->setUpAxis(1); + m_guiHelper->setUpAxis(1); - ///collision configuration contains default setup for memory, collision setup - m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + ///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); + ///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(); + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); - btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); - sol->setDeformableSolver(deformableBodySolver); - m_solver = sol; + 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, -9.8, 0); - m_dynamicsWorld->setGravity(gravity); - getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; - getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, -9.8, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25); - m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - { - ///create a ground - btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.))); - groundShape->setMargin(0.02); - m_collisionShapes.push_back(groundShape); - - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0, -3.5, 0)); - groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); - 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(4); - - //add the ground to the dynamics world - m_dynamicsWorld->addRigidBody(body); - } - addCloth(btVector3(0, 1, 0)); - getDeformableDynamicsWorld()->setImplicit(false); - getDeformableDynamicsWorld()->setLineSearch(false); - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.))); + groundShape->setMargin(0.02); + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -3.5, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + 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(4); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + addCloth(btVector3(0, 1, 0)); + getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } void LoadDeformed::addCloth(const btVector3& origin) // create a piece of cloth { - const btScalar s = 0.6; - const btScalar h = 0; - - psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2*s), - btVector3(+s, h, -2*s), - btVector3(-s, h, +2*s), - btVector3(+s, h, +2*s), - 15,30, - 0, true, 0.0); - - psb->getCollisionShape()->setMargin(0.02); - psb->generateBendingConstraints(2); - psb->setTotalMass(.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.1; - psb->rotate(btQuaternion(0, SIMD_PI / 2, 0)); - btTransform clothTransform; - clothTransform.setIdentity(); - clothTransform.setOrigin(btVector3(0,0.2,0)+origin); - psb->transform(clothTransform); - - b3BulletDefaultFileIO fileio; - char absolute_path[1024]; - char filename[100]; - sprintf(filename, "/Users/fuchuyuan/Documents/mybullet/build_cmake/examples/ExampleBrowser/states_0_%d.txt", reset_frame); - fileio.findResourcePath(filename, absolute_path, 1024); - btAlignedObjectArray qs; - btAlignedObjectArray vs; - btSoftBodyHelpers:: loadDeformableState(qs, vs, absolute_path, &fileio); - if(reset_frame>0) - psb->updateState(qs,vs); - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; -// psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; - psb->setCollisionFlags(0); - psb->setCacheBarycenter(true); - getDeformableDynamicsWorld()->addSoftBody(psb); - psb->setSelfCollision(false); - - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.2, true); - psb->setSpringStiffness(4); - getDeformableDynamicsWorld()->addForce(psb, mass_spring); - m_forces.push_back(mass_spring); - btVector3 gravity = btVector3(0, -9.8, 0); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); - getDeformableDynamicsWorld()->addForce(psb, gravity_force); -// getDeformableDynamicsWorld()->setUseProjection(true); - m_forces.push_back(gravity_force); + const btScalar s = 0.6; + const btScalar h = 0; + + psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2 * s), + btVector3(+s, h, -2 * s), + btVector3(-s, h, +2 * s), + btVector3(+s, h, +2 * s), + 15, 30, + 0, true, 0.0); + + psb->getCollisionShape()->setMargin(0.02); + psb->generateBendingConstraints(2); + psb->setTotalMass(.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.1; + psb->rotate(btQuaternion(0, SIMD_PI / 2, 0)); + btTransform clothTransform; + clothTransform.setIdentity(); + clothTransform.setOrigin(btVector3(0, 0.2, 0) + origin); + psb->transform(clothTransform); + + b3BulletDefaultFileIO fileio; + char absolute_path[1024]; + char filename[100]; + sprintf(filename, "/Users/fuchuyuan/Documents/mybullet/build_cmake/examples/ExampleBrowser/states_0_%d.txt", reset_frame); + fileio.findResourcePath(filename, absolute_path, 1024); + btAlignedObjectArray qs; + btAlignedObjectArray vs; + btSoftBodyHelpers::loadDeformableState(qs, vs, absolute_path, &fileio); + if (reset_frame > 0) + psb->updateState(qs, vs); + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; + // psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->setCollisionFlags(0); + psb->setCacheBarycenter(true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->setSelfCollision(false); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.2, true); + psb->setSpringStiffness(4); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + btVector3 gravity = btVector3(0, -9.8, 0); + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + // getDeformableDynamicsWorld()->setUseProjection(true); + m_forces.push_back(gravity_force); } void LoadDeformed::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(); + //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 m_dynamicsWorld; + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); - delete m_solver; + delete m_dynamicsWorld; - delete m_broadphase; + delete m_solver; - delete m_dispatcher; + delete m_broadphase; - delete m_collisionConfiguration; + delete m_dispatcher; + + delete m_collisionConfiguration; } - - class CommonExampleInterface* LoadDeformedCreateFunc(struct CommonExampleOptions& options) { - return new LoadDeformed(options.m_guiHelper); + return new LoadDeformed(options.m_guiHelper); } - - diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 862d5e35e..eafde0d22 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -1483,18 +1483,19 @@ void btSoftBody::randomizeConstraints() #undef NEXTRAND } - -void btSoftBody::updateState(const btAlignedObjectArray& q, const btAlignedObjectArray& v){ - int node_count =m_nodes.size(); - btAssert(node_count == q.size()); - btAssert(node_count == v.size()); - for (int i = 0; i& q, const btAlignedObjectArray& v) +{ + int node_count = m_nodes.size(); + btAssert(node_count == q.size()); + btAssert(node_count == v.size()); + for (int i = 0; i < node_count; i++) + { + Node& n = m_nodes[i]; + n.m_x = q[i]; + n.m_q = q[i]; + n.m_v = v[i]; + n.m_vn = v[i]; + } } // @@ -2835,7 +2836,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btScalar dst; btGjkEpaSolver2::sResults results; -// #define USE_QUADRATURE 1 + // #define USE_QUADRATURE 1 // use collision quadrature point #ifdef USE_QUADRATURE diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index badf4600d..1ef8e89b0 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -298,7 +298,7 @@ public: }; struct RenderFace { - RenderNode* m_n[3]; // Node pointers + RenderNode* m_n[3]; // Node pointers }; /* Face */ @@ -786,7 +786,7 @@ public: typedef btAlignedObjectArray tClusterArray; typedef btAlignedObjectArray tNoteArray; typedef btAlignedObjectArray tNodeArray; - typedef btAlignedObjectArray< RenderNode> tRenderNodeArray; + typedef btAlignedObjectArray tRenderNodeArray; typedef btAlignedObjectArray tLeafArray; typedef btAlignedObjectArray tLinkArray; typedef btAlignedObjectArray tFaceArray; @@ -813,7 +813,7 @@ public: tRenderNodeArray m_renderNodes; // Render Nodes tLinkArray m_links; // Links tFaceArray m_faces; // Faces - tRenderFaceArray m_renderFaces; // Faces + tRenderFaceArray m_renderFaces; // Faces tTetraArray m_tetras; // Tetras btAlignedObjectArray m_tetraScratches; btAlignedObjectArray m_tetraScratchesTn; @@ -1051,9 +1051,9 @@ public: Material* mat = 0); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); - - void updateState(const btAlignedObjectArray& qs, const btAlignedObjectArray& vs); - + + void updateState(const btAlignedObjectArray& qs, const btAlignedObjectArray& vs); + /* Release clusters */ void releaseCluster(int index); void releaseClusters(); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index dd7abb48e..eac45284c 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1489,168 +1489,166 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) static inline bool isSpace(const char c) { - return (c == ' ') || (c == '\t'); + return (c == ' ') || (c == '\t'); } static inline bool isNewLine(const char c) { - return (c == '\r') || (c == '\n') || (c == '\0'); + return (c == '\r') || (c == '\n') || (c == '\0'); } static inline float parseFloat(const char*& token) { - token += strspn(token, " \t"); - float f = (float)atof(token); - token += strcspn(token, " \t\r"); - return f; + token += strspn(token, " \t"); + float f = (float)atof(token); + token += strcspn(token, " \t\r"); + return f; } static inline void parseFloat3( - float& x, float& y, float& z, - const char*& token) + float& x, float& y, float& z, + const char*& token) { - x = parseFloat(token); - y = parseFloat(token); - z = parseFloat(token); + x = parseFloat(token); + y = parseFloat(token); + z = parseFloat(token); } +void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb) +{ + std::ofstream fs; + fs.open(file); + btAssert(fs); + fs << std::scientific << std::setprecision(16); -void btSoftBodyHelpers:: writeState(const char* file, const btSoftBody* psb){ - std::ofstream fs; - fs.open(file); - btAssert(fs); - fs<< std::scientific<m_nodes.size(); ++i) + { + fs << "q"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_q[d]; + } + fs << "\n"; + } - // Only write out for trimesh, directly write out all the nodes and faces.xs - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - fs << "q"; - for (int d = 0; d < 3; d++) - { - fs << " " << psb->m_nodes[i].m_q[d]; - } - fs << "\n"; - } - - for (int i = 0; i < psb->m_nodes.size(); ++i) - { - fs << "v"; - for (int d = 0; d < 3; d++) - { - fs << " " << psb->m_nodes[i].m_v[d]; - } - fs << "\n"; - } - fs.close(); + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + fs << "v"; + for (int d = 0; d < 3; d++) + { + fs << " " << psb->m_nodes[i].m_v[d]; + } + fs << "\n"; + } + fs.close(); } +std::string btSoftBodyHelpers::loadDeformableState(btAlignedObjectArray& qs, btAlignedObjectArray& vs, const char* filename, CommonFileIOInterface* fileIO) +{ + { + qs.clear(); + vs.clear(); + std::string tmp = filename; + std::stringstream err; +#ifdef USE_STREAM + std::ifstream ifs(filename); + if (!ifs) + { + err << "Cannot open file [" << filename << "]" << std::endl; + return err.str(); + } +#else + int fileHandle = fileIO->fileOpen(filename, "r"); + if (fileHandle < 0) + { + err << "Cannot open file [" << filename << "]" << std::endl; + return err.str(); + } +#endif -std::string btSoftBodyHelpers:: loadDeformableState(btAlignedObjectArray& qs, btAlignedObjectArray& vs, const char* filename, CommonFileIOInterface* fileIO){ - { - qs.clear(); - vs.clear(); - std::string tmp = filename; - std::stringstream err; - #ifdef USE_STREAM - std::ifstream ifs(filename); - if (!ifs) - { - err << "Cannot open file [" << filename << "]" << std::endl; - return err.str(); - } - #else - int fileHandle = fileIO->fileOpen(filename, "r"); - if (fileHandle < 0) - { - err << "Cannot open file [" << filename << "]" << std::endl; - return err.str(); - } - #endif + std::string name; - std::string name; + int maxchars = 8192; // Alloc enough size. + std::vector buf(maxchars); // Alloc enough size. + std::string linebuf; + linebuf.reserve(maxchars); +#ifdef USE_STREAM + while (ifs.peek() != -1) +#else + char* line = 0; + do +#endif + { + linebuf.resize(0); +#ifdef USE_STREAM + safeGetline(ifs, linebuf); +#else + char tmpBuf[1024]; + line = fileIO->readLine(fileHandle, tmpBuf, 1024); + if (line) + { + linebuf = line; + } +#endif + // Trim newline '\r\n' or '\r' + if (linebuf.size() > 0) + { + if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); + } + if (linebuf.size() > 0) + { + if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); + } - int maxchars = 8192; // Alloc enough size. - std::vector buf(maxchars); // Alloc enough size. - std::string linebuf; - linebuf.reserve(maxchars); + // Skip if empty line. + if (linebuf.empty()) + { + continue; + } - #ifdef USE_STREAM - while (ifs.peek() != -1) - #else - char* line = 0; - do - #endif - { - linebuf.resize(0); - #ifdef USE_STREAM - safeGetline(ifs, linebuf); - #else - char tmpBuf[1024]; - line = fileIO->readLine(fileHandle, tmpBuf, 1024); - if (line) - { - linebuf = line; - } - #endif - // Trim newline '\r\n' or '\r' - if (linebuf.size() > 0) - { - if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); - } - if (linebuf.size() > 0) - { - if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1); - } + // Skip leading space. + const char* token = linebuf.c_str(); + token += strspn(token, " \t"); - // Skip if empty line. - if (linebuf.empty()) - { - continue; - } + assert(token); + if (token[0] == '\0') continue; // empty line - // Skip leading space. - const char* token = linebuf.c_str(); - token += strspn(token, " \t"); + if (token[0] == '#') continue; // comment line - assert(token); - if (token[0] == '\0') continue; // empty line + // q + if (token[0] == 'q' && isSpace((token[1]))) + { + token += 2; + float x, y, z; + parseFloat3(x, y, z, token); + qs.push_back(btVector3(x, y, z)); + continue; + } - if (token[0] == '#') continue; // comment line + // v + if (token[0] == 'v' && isSpace((token[1]))) + { + token += 3; + float x, y, z; + parseFloat3(x, y, z, token); + vs.push_back(btVector3(x, y, z)); + continue; + } - // q - if (token[0] == 'q' && isSpace((token[1]))) - { - token += 2; - float x, y, z; - parseFloat3(x, y, z, token); - qs.push_back(btVector3(x,y,z)); - continue; - } + // Ignore unknown command. + } +#ifndef USE_STREAM + while (line) + ; +#endif - // v - if (token[0] == 'v' && isSpace((token[1]))) - { - token += 3; - float x, y, z; - parseFloat3(x, y, z, token); - vs.push_back(btVector3(x,y,z)); - continue; - } - - // Ignore unknown command. - } - #ifndef USE_STREAM - while (line) - ; - #endif - - if (fileHandle >= 0) - { - fileIO->fileClose(fileHandle); - } - return err.str(); - } + if (fileHandle >= 0) + { + fileIO->fileClose(fileHandle); + } + return err.str(); + } } - 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 7dfd58590..d1fdf0450 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -146,10 +146,10 @@ struct btSoftBodyHelpers static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file); static void writeObj(const char* file, const btSoftBody* psb); - - static void writeState(const char* file, const btSoftBody* psb); - - static std::string loadDeformableState(btAlignedObjectArray& qs, btAlignedObjectArray& vs, const char* filename, CommonFileIOInterface* fileIO); + + static void writeState(const char* file, const btSoftBody* psb); + + static std::string loadDeformableState(btAlignedObjectArray& qs, btAlignedObjectArray& vs, const char* filename, CommonFileIOInterface* fileIO); static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); From 4e5b3f34c90f1c5e31eb6fb30f4b0edae2203969 Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Fri, 18 Feb 2022 16:38:59 -0800 Subject: [PATCH 05/10] add API to get and set mesh velocity --- examples/SharedMemory/PhysicsClientC_API.cpp | 16 ++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 4 +++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 75f48e39d..ec697cdbf 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1549,6 +1549,22 @@ B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHand } } +B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_MESH_DATA); + command->m_updateFlags |= B3_MESH_DATA_SIMULATION_MESH; +} + +B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_MESH_DATA || command->m_type == CMD_RESET_MESH_DATA); + command->m_updateFlags |= B3_MESH_DATA_SIMULATION_MESH_VELOCITY; +} + B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index c14feb82f..84dcda64c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -532,7 +532,9 @@ extern "C" B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId); B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); - B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex); + B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle); + B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle); + B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex); B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData); From bdba9fa2e0d6a34fc28aeeb6882575b5ddb9d4f5 Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Fri, 18 Feb 2022 16:51:48 -0800 Subject: [PATCH 06/10] format changes --- examples/SharedMemory/PhysicsClientC_API.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 84dcda64c..fb0ac3d7e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -532,9 +532,9 @@ extern "C" B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId); B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); - B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle); - B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle); - B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex); + B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle); + B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle); + B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex); B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData); From 807cf3d387e03d5b47a7bf49f3d12780f6035210 Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Fri, 18 Feb 2022 16:59:07 -0800 Subject: [PATCH 07/10] format commit --- .../PhysicsServerCommandProcessor.cpp | 34 ++++++++++--------- examples/SharedMemory/SharedMemoryPublic.h | 9 +++-- 2 files changed, 22 insertions(+), 21 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2506a767f..4f65462a5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5512,24 +5512,26 @@ bool PhysicsServerCommandProcessor::processResetMeshDataCommand(const struct Sha int numVertices = psb->m_nodes.size(); if (clientCmd.m_resetMeshDataArgs.m_numVertices == numVertices) { - if(clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY){ - for (int i = 0; i < numVertices; ++i) + if (clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY) { - btSoftBody::Node& n = psb->m_nodes[i]; - n.m_v.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); - n.m_vn.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); + for (int i = 0; i < numVertices; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + n.m_v.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]); + n.m_vn.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]); + } } - } - else{ - for (int i = 0; i < numVertices; ++i) - { - btSoftBody::Node& n = psb->m_nodes[i]; - n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); - n.m_q.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]); - } - } - serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED; - } + else + { + for (int i = 0; i < numVertices; ++i) + { + btSoftBody::Node& n = psb->m_nodes[i]; + n.m_x.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]); + n.m_q.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]); + } + } + serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED; + } } #endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 90143b55e..6085f4a0f 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -461,13 +461,12 @@ struct b3MeshVertex double x, y, z, w; }; - enum eMeshDataFlags { - B3_MESH_DATA_SIMULATION_MESH=1, - B3_MESH_DATA_SIMULATION_INDICES=2, - B3_MESH_DATA_GRAPHICS_INDICES=4, - B3_MESH_DATA_SIMULATION_MESH_VELOCITY=8, + B3_MESH_DATA_SIMULATION_MESH = 1, + B3_MESH_DATA_SIMULATION_INDICES = 2, + B3_MESH_DATA_GRAPHICS_INDICES = 4, + B3_MESH_DATA_SIMULATION_MESH_VELOCITY = 8, }; enum eMeshDataEnum From 548d6af9018e4e565efc8cd7f7dc3053770c63f1 Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Fri, 18 Feb 2022 17:39:03 -0800 Subject: [PATCH 08/10] fix compile --- src/BulletSoftBody/btSoftBodyHelpers.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index eac45284c..1c0d65f42 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -18,6 +18,7 @@ subject to the following restrictions: #include #include #include +#include #include #include #include From 60b6df94d921e94a315ecb9dae213e0e8a14a34a Mon Sep 17 00:00:00 2001 From: Chuyuan Kelly Fu Date: Fri, 18 Feb 2022 18:11:18 -0800 Subject: [PATCH 09/10] fix compile --- src/BulletSoftBody/btSoftBodyHelpers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 1c0d65f42..35eb22ec1 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1610,7 +1610,7 @@ std::string btSoftBodyHelpers::loadDeformableState(btAlignedObjectArray Date: Wed, 2 Mar 2022 21:43:01 -0800 Subject: [PATCH 10/10] bug fix --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4f65462a5..2f5040139 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5615,11 +5615,11 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S } bool separateRenderMesh = false; - if ((flags & B3_MESH_DATA_SIMULATION_MESH) == 0) + if ((clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH) == 0) { separateRenderMesh = (psb->m_renderNodes.size() != 0); } - bool requestVelocity = flags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY; + bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY; int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;