From 96deb42aa55ad2b91faaaa3f90fc87873ba2aa8b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 19 Nov 2019 19:20:08 -0800 Subject: [PATCH] pybullet.createSoftBodyAnchor --- data/cloth.obj | 64 +++++++++ data/cloth_z_up.obj | 64 +++++++++ examples/SharedMemory/PhysicsClientC_API.cpp | 24 ++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 124 +++++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 1 + .../pybullet/examples/deformable_anchor.py | 29 ++++ examples/pybullet/pybullet.c | 58 ++++++++ src/BulletSoftBody/btSoftBody.cpp | 8 +- src/BulletSoftBody/btSoftBodyHelpers.cpp | 37 ------ src/BulletSoftBody/btSoftBodyHelpers.h | 2 - src/BulletSoftBody/btSparseSDF.h | 2 +- 12 files changed, 370 insertions(+), 45 deletions(-) create mode 100644 data/cloth.obj create mode 100644 data/cloth_z_up.obj create mode 100644 examples/pybullet/examples/deformable_anchor.py diff --git a/data/cloth.obj b/data/cloth.obj new file mode 100644 index 000000000..e5d7a3128 --- /dev/null +++ b/data/cloth.obj @@ -0,0 +1,64 @@ +# Blender v2.79 (sub 0) OBJ File: '' +# www.blender.org +mtllib cloth.mtl +o Plane_Plane.001 +v -1.000000 0.000000 1.000000 +v 1.000000 0.000000 1.000000 +v -1.000000 0.000000 -1.000000 +v 1.000000 0.000000 -1.000000 +v -1.000000 0.000000 0.000000 +v 0.000000 0.000000 1.000000 +v 1.000000 0.000000 0.000000 +v 0.000000 0.000000 -1.000000 +v 0.000000 0.000000 0.000000 +v -1.000000 0.000000 0.500000 +v 0.500000 0.000000 1.000000 +v 1.000000 0.000000 -0.500000 +v -0.500000 0.000000 -1.000000 +v -1.000000 0.000000 -0.500000 +v -0.500000 0.000000 1.000000 +v 1.000000 0.000000 0.500000 +v 0.500000 0.000000 -1.000000 +v 0.000000 0.000000 -0.500000 +v 0.000000 0.000000 0.500000 +v -0.500000 0.000000 0.000000 +v 0.500000 0.000000 0.000000 +v 0.500000 0.000000 0.500000 +v -0.500000 0.000000 0.500000 +v -0.500000 0.000000 -0.500000 +v 0.500000 0.000000 -0.500000 +vn 0.0000 1.0000 0.0000 +usemtl None +s off +f 12//1 17//1 25//1 +f 18//1 13//1 24//1 +f 19//1 20//1 23//1 +f 16//1 21//1 22//1 +f 22//1 9//1 19//1 +f 11//1 19//1 6//1 +f 2//1 22//1 11//1 +f 23//1 5//1 10//1 +f 15//1 10//1 1//1 +f 6//1 23//1 15//1 +f 24//1 3//1 14//1 +f 20//1 14//1 5//1 +f 9//1 24//1 20//1 +f 25//1 8//1 18//1 +f 21//1 18//1 9//1 +f 7//1 25//1 21//1 +f 12//1 4//1 17//1 +f 18//1 8//1 13//1 +f 19//1 9//1 20//1 +f 16//1 7//1 21//1 +f 22//1 21//1 9//1 +f 11//1 22//1 19//1 +f 2//1 16//1 22//1 +f 23//1 20//1 5//1 +f 15//1 23//1 10//1 +f 6//1 19//1 23//1 +f 24//1 13//1 3//1 +f 20//1 24//1 14//1 +f 9//1 18//1 24//1 +f 25//1 17//1 8//1 +f 21//1 25//1 18//1 +f 7//1 12//1 25//1 diff --git a/data/cloth_z_up.obj b/data/cloth_z_up.obj new file mode 100644 index 000000000..e2667a984 --- /dev/null +++ b/data/cloth_z_up.obj @@ -0,0 +1,64 @@ +# Blender v2.79 (sub 0) OBJ File: '' +# www.blender.org +mtllib cloth_z_up.mtl +o Plane_Plane.001 +v -1.000000 -1.000000 0.000000 +v 1.000000 -1.000000 0.000000 +v -1.000000 1.000000 -0.000000 +v 1.000000 1.000000 -0.000000 +v -1.000000 0.000000 0.000000 +v -0.000000 -1.000000 0.000000 +v 1.000000 -0.000000 -0.000000 +v 0.000000 1.000000 -0.000000 +v 0.000000 0.000000 0.000000 +v -1.000000 -0.500000 0.000000 +v 0.500000 -1.000000 0.000000 +v 1.000000 0.500000 -0.000000 +v -0.500000 1.000000 -0.000000 +v -1.000000 0.500000 -0.000000 +v -0.500000 -1.000000 0.000000 +v 1.000000 -0.500000 0.000000 +v 0.500000 1.000000 -0.000000 +v 0.000000 0.500000 -0.000000 +v -0.000000 -0.500000 0.000000 +v -0.500000 0.000000 0.000000 +v 0.500000 -0.000000 -0.000000 +v 0.500000 -0.500000 0.000000 +v -0.500000 -0.500000 0.000000 +v -0.500000 0.500000 -0.000000 +v 0.500000 0.500000 -0.000000 +vn 0.0000 0.0000 1.0000 +usemtl None +s off +f 12//1 17//1 25//1 +f 18//1 13//1 24//1 +f 19//1 20//1 23//1 +f 16//1 21//1 22//1 +f 22//1 9//1 19//1 +f 11//1 19//1 6//1 +f 2//1 22//1 11//1 +f 23//1 5//1 10//1 +f 15//1 10//1 1//1 +f 6//1 23//1 15//1 +f 24//1 3//1 14//1 +f 20//1 14//1 5//1 +f 9//1 24//1 20//1 +f 25//1 8//1 18//1 +f 21//1 18//1 9//1 +f 7//1 25//1 21//1 +f 12//1 4//1 17//1 +f 18//1 8//1 13//1 +f 19//1 9//1 20//1 +f 16//1 7//1 21//1 +f 22//1 21//1 9//1 +f 11//1 22//1 19//1 +f 2//1 16//1 22//1 +f 23//1 20//1 5//1 +f 15//1 23//1 10//1 +f 6//1 19//1 23//1 +f 24//1 13//1 3//1 +f 20//1 24//1 14//1 +f 9//1 18//1 24//1 +f 25//1 17//1 8//1 +f 21//1 25//1 18//1 +f 7//1 12//1 25//1 diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0284d1d1b..611a75655 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3248,6 +3248,30 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHa return 0; } +B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_USER_CONSTRAINT; + command->m_updateFlags = USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR; + command->m_userConstraintArguments.m_parentBodyIndex = softBodyUniqueId; + command->m_userConstraintArguments.m_parentJointIndex = nodeIndex; + command->m_userConstraintArguments.m_childBodyIndex = bodyUniqueId; + command->m_userConstraintArguments.m_childJointIndex = linkIndex; + command->m_userConstraintArguments.m_childFrame[0] = bodyFramePosition[0]; + command->m_userConstraintArguments.m_childFrame[1] = bodyFramePosition[1]; + command->m_userConstraintArguments.m_childFrame[2] = bodyFramePosition[2]; + command->m_userConstraintArguments.m_childFrame[3] = 0.; + command->m_userConstraintArguments.m_childFrame[4] = 0.; + command->m_userConstraintArguments.m_childFrame[5] = 0.; + command->m_userConstraintArguments.m_childFrame[6] = 1.; + + return (b3SharedMemoryCommandHandle)command; +} + B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index ff59cef46..220e5582f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -643,6 +643,8 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings); + + B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]); B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient); B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6d83611bf..74c49a408 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -53,7 +53,7 @@ #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "LinearMath/TaskScheduler/btThreadSupportInterface.h" - +#include "Wavefront/tiny_obj_loader.h" #ifndef SKIP_COLLISION_FILTER_PLUGIN #include "plugins/collisionFilterPlugin/collisionFilterPlugin.h" #endif @@ -1709,10 +1709,11 @@ struct PhysicsServerCommandProcessorInternalData m_dispatcher(0), m_solver(0), m_collisionConfiguration(0), - m_dynamicsWorld(0), + #ifndef SKIP_DEFORMABLE_BODY m_deformablebodySolver(0), #endif + m_dynamicsWorld(0), m_constraintSolverType(-1), m_remoteDebugDrawer(0), m_stateLoggersUniqueId(0), @@ -8204,7 +8205,46 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar if (!render_mesh_is_sim_mesh) { // load render mesh - btSoftBodyHelpers::readRenderMeshFromObj(out_found_filename.c_str(), psb); + + + { + + tinyobj::attrib_t attribute; + std::vector shapes; + + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface()); + + for (int s = 0; s < (int)shapes.size(); s++) + { + tinyobj::shape_t& shape = shapes[s]; + int faceCount = shape.mesh.indices.size(); + int vertexCount = attribute.vertices.size()/3; + for (int v=0;vm_renderNodes.push_back(n); + } + + for (int f = 0; f < faceCount; f += 3) + { + if (f < 0 && f >= int(shape.mesh.indices.size())) + { + continue; + } + tinyobj::index_t v_0 = shape.mesh.indices[f]; + tinyobj::index_t v_1 = shape.mesh.indices[f + 1]; + tinyobj::index_t v_2 = shape.mesh.indices[f + 2]; + btSoftBody::Face ff; + ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index]; + ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index]; + ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index]; + psb->m_renderFaces.push_back(ff); + } + } + + } + btSoftBodyHelpers::interpolateBarycentricWeights(psb); } else @@ -10645,6 +10685,84 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; hasStatus = true; + if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR) + { +#ifndef SKIP_DEFORMABLE_BODY + InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); + if (sbodyHandle) + { + if (sbodyHandle->m_softBody) + { + int nodeIndex = clientCmd.m_userConstraintArguments.m_parentJointIndex; + if (nodeIndex>=0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size()) + { + int bodyUniqueId = clientCmd.m_userConstraintArguments.m_childBodyIndex; + if (bodyUniqueId<=0) + { + //fixed anchor (mass = 0) + sbodyHandle->m_softBody->setMass(nodeIndex,0.0); + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } else + { + InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (mbodyHandle && mbodyHandle->m_multiBody) + { + + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex; + if (linkIndex<0) + { + sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider()); + } else + { + if (linkIndex < mbodyHandle->m_multiBody->getNumLinks()) + { + sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getLinkCollider(linkIndex)); + } + } + } + } + if (mbodyHandle && mbodyHandle->m_rigidBody) + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + //todo: expose those values + bool disableCollisionBetweenLinkedBodies = true; + //btVector3 localPivot(0,0,0); + sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_rigidBody); + } + +#if 1 + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + bool disableCollisionBetweenLinkedBodies = true; + btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0], + clientCmd.m_userConstraintArguments.m_childFrame[1], + clientCmd.m_userConstraintArguments.m_childFrame[2]); + + sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies); + } +#endif + } + int uid = m_data->m_userConstraintUIDGenerator++; + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + + } + + } + + + } + } +#endif + } if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE) { int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ec79d77c5..ab39daaa0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -800,6 +800,7 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512, USER_CONSTRAINT_CHANGE_ERP = 1024, USER_CONSTRAINT_REQUEST_STATE = 2048, + USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096, }; enum EnumBodyChangeFlags diff --git a/examples/pybullet/examples/deformable_anchor.py b/examples/pybullet/examples/deformable_anchor.py new file mode 100644 index 000000000..d55a1bc79 --- /dev/null +++ b/examples/pybullet/examples/deformable_anchor.py @@ -0,0 +1,29 @@ +import pybullet as p +from time import sleep + +physicsClient = p.connect(p.GUI) + +p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) + +p.setGravity(0, 0, -10) + +planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0]) +planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) + +boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True) + +clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1) + + +p.createSoftBodyAnchor(clothId ,0,-1,-1) +p.createSoftBodyAnchor(clothId ,1,-1,-1) +p.createSoftBodyAnchor(clothId ,3,boxId,-1, [0.5,-0.5,0]) +p.createSoftBodyAnchor(clothId ,2,boxId,-1, [-0.5,-0.5,0]) +p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) +p.setRealTimeSimulation(1) + + +while p.isConnected(): + p.getNumBodies() + sleep(1./240.) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 260f6db68..0857ea0da 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1963,6 +1963,10 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + + + + // Load a softbody from an obj file static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* keywds) { @@ -2072,6 +2076,56 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* } return PyLong_FromLong(bodyUniqueId); } + +static PyObject* pybullet_createSoftBodyAnchor(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3SharedMemoryCommandHandle commandHandle; + int softBodyUniqueId = -1; + int nodeIndex = -1; + int bodyUniqueId = -1; + int linkIndex = -1; + PyObject* bodyFramePositionObj = 0; + double bodyFramePosition[3] = {0, 0, 0}; + struct b3JointInfo jointInfo; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"softBodyBodyUniqueId", "nodeIndex", + "bodyUniqueId", "linkIndex", "bodyFramePosition", + "physicsClientId", + NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|iiOi", kwlist, &softBodyUniqueId, &nodeIndex, + &bodyUniqueId, &linkIndex,&bodyFramePositionObj,&physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + pybullet_internalSetVectord(bodyFramePositionObj, bodyFramePosition); + + commandHandle = b3InitCreateSoftBodyAnchorConstraintCommand(sm, softBodyUniqueId, nodeIndex, bodyUniqueId, linkIndex, bodyFramePosition); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_CONSTRAINT_COMPLETED) + { + int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(userConstraintUid); + return ob; + } + + PyErr_SetString(SpamError, "createSoftBodyAnchor failed."); + return NULL; +} + + #endif // Reset the simulation to remove all loaded objects @@ -11798,6 +11852,10 @@ static PyMethodDef SpamMethods[] = { #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD {"loadSoftBody", (PyCFunction)pybullet_loadSoftBody, METH_VARARGS | METH_KEYWORDS, "Load a softbody from an obj file."}, + + {"createSoftBodyAnchor", (PyCFunction)pybullet_createSoftBodyAnchor, METH_VARARGS | METH_KEYWORDS, + "Create an anchor (attachment) between a soft body and a rigid or multi body."}, + #endif {"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS, "Load a world from a .bullet file."}, diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 85bd2b142..e4219b959 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2496,7 +2496,8 @@ void btSoftBody::updateNormals() btSoftBody::Face& f = m_faces[i]; const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x, f.m_n[2]->m_x - f.m_n[0]->m_x); - f.m_normal = n.normalized(); + f.m_normal = n; + f.m_normal.safeNormalize(); f.m_n[0]->m_n += n; f.m_n[1]->m_n += n; f.m_n[2]->m_n += n; @@ -3306,7 +3307,10 @@ void btSoftBody::interpolateRenderMesh() n.m_x.setZero(); for (int j = 0; j < 4; ++j) { - n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + if (m_renderNodesParents[i].size()) + { + n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j]; + } } } } diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index c9e4af382..27221b574 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1500,43 +1500,6 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6); } -void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb) -{ - std::ifstream fs; - fs.open(file); - std::string line; - btVector3 pos; - while (std::getline(fs, line)) - { - std::stringstream ss(line); - if (line.length()>1) - { - if (line[0] == 'v' && line[1] != 't' && line[1] != 'n') - { - ss.ignore(); - for (size_t i = 0; i < 3; i++) - ss >> pos[i]; - btSoftBody::Node n; - n.m_x = pos; - psb->m_renderNodes.push_back(n); - } - else if (line[0] == 'f') - { - ss.ignore(); - int id0, id1, id2; - ss >> id0; - ss >> id1; - ss >> id2; - btSoftBody::Face f; - f.m_n[0] = &psb->m_renderNodes[id0-1]; - f.m_n[1] = &psb->m_renderNodes[id1-1]; - f.m_n[2] = &psb->m_renderNodes[id2-1]; - psb->m_renderFaces.push_back(f); - } - } - } - fs.close(); -} // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index d5d7c3648..b20f2f6d6 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -148,8 +148,6 @@ struct btSoftBodyHelpers static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary); - static void readRenderMeshFromObj(const char* file, btSoftBody* psb); - static void interpolateBarycentricWeights(btSoftBody* psb); static void generateBoundaryFaces(btSoftBody* psb); diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index c6342c285..97b068caa 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -274,7 +274,7 @@ struct btSparseSdf Lerp(gy[2], gy[3], ix.f), iz.f)); normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f), Lerp(gz[2], gz[3], ix.f), iy.f)); - normal = normal.normalized(); + normal.safeNormalize(); #else normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized(); #endif