summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorChuyuan Kelly Fu <fuchuyuan@google.com>2022-02-18 14:52:03 -0800
committerChuyuan Kelly Fu <fuchuyuan@google.com>2022-02-18 17:49:21 -0800
commit450c95d0b96032814ef3cd4f5c8b67a7fad3e1e4 (patch)
tree68cb24d9fe0af7bd8fd39d9c868b7d0f31a44cfe
parent236d39d2ec20be7ffe68d21b632eb5e845d77a89 (diff)
downloadbullet3-450c95d0b96032814ef3cd4f5c8b67a7fad3e1e4.tar.gz
format files
-rw-r--r--examples/DeformableDemo/LoadDeformed.cpp423
-rw-r--r--src/BulletSoftBody/btSoftBody.cpp27
-rw-r--r--src/BulletSoftBody/btSoftBody.h12
-rw-r--r--src/BulletSoftBody/btSoftBodyHelpers.cpp288
-rw-r--r--src/BulletSoftBody/btSoftBodyHelpers.h8
5 files changed, 377 insertions, 381 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();
-
- 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());
- }
- }
- }
+ 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);
+ 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<btVector3> qs;
- btAlignedObjectArray<btVector3> 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<btVector3> qs;
+ btAlignedObjectArray<btVector3> 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;
-}
+ //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);
+ 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<btVector3>& q, const btAlignedObjectArray<btVector3>& 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];
- }
+void btSoftBody::updateState(const btAlignedObjectArray<btVector3>& q, const btAlignedObjectArray<btVector3>& 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<Cluster*> tClusterArray;
typedef btAlignedObjectArray<Note> tNoteArray;
typedef btAlignedObjectArray<Node> tNodeArray;
- typedef btAlignedObjectArray< RenderNode> tRenderNodeArray;
+ typedef btAlignedObjectArray<RenderNode> tRenderNodeArray;
typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
typedef btAlignedObjectArray<Link> tLinkArray;
typedef btAlignedObjectArray<Face> 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<TetraScratch> m_tetraScratches;
btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
@@ -1051,9 +1051,9 @@ public:
Material* mat = 0);
/* Randomize constraints to reduce solver bias */
void randomizeConstraints();
-
- void updateState(const btAlignedObjectArray<btVector3>& qs, const btAlignedObjectArray<btVector3>& vs);
-
+
+ void updateState(const btAlignedObjectArray<btVector3>& qs, const btAlignedObjectArray<btVector3>& 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,167 +1489,165 @@ 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);
+
+ // 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";
+ }
-void btSoftBodyHelpers:: writeState(const char* file, const btSoftBody* psb){
- std::ofstream fs;
- fs.open(file);
- btAssert(fs);
- fs<< std::scientific<<std::setprecision(16);
-
- // 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<btVector3>& qs, btAlignedObjectArray<btVector3>& 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 btSoftBodyHelpers:: loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& 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<char> 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();
- }
-}
+ int maxchars = 8192; // Alloc enough size.
+ std::vector<char> 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)
{
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<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO);
+
+ static void writeState(const char* file, const btSoftBody* psb);
+
+ static std::string loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& 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);