diff --git a/README.md b/README.md index e84da725f..f6930828f 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,20 @@ This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc. -New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3). +## PyBullet ## +New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and checkout the [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3). + +If you use PyBullet in your research, please cite it like this: + +``` +@MISC{coumans2019, +author = {Erwin Coumans and Yunfei Bai}, +title = {PyBullet, a Python module for physics simulation for games, robotics and machine learning}, +howpublished = {\url{http://pybullet.org}}, +year = {2016--2019} +} +``` + The Bullet 2 API will stay default and up-to-date while slowly moving to a new API. The steps towards a new API is in a nutshell: 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 651457385..ee506c143 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -338,20 +338,20 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle return 0; } -B3_SHARED_API int b3LoadSoftBodyAddRenderMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename) +B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); int len = strlen(filename); if (len < MAX_FILENAME_LENGTH) { - strcpy(command->m_loadSoftBodyArguments.m_renderFileName, filename); + strcpy(command->m_loadSoftBodyArguments.m_simFileName, filename); } else { - command->m_loadSoftBodyArguments.m_renderFileName[0] = 0; + command->m_loadSoftBodyArguments.m_simFileName[0] = 0; } - command->m_updateFlags |= LOAD_SOFT_BODY_RENDER_MESH; + command->m_updateFlags |= LOAD_SOFT_BODY_SIM_MESH; return 0; } @@ -3266,6 +3266,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 ee3f26119..c67f79d9c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -633,7 +633,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin); B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ); B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); - B3_SHARED_API int b3LoadSoftBodyAddRenderMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename); + B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename); B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping); @@ -644,6 +644,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 75d32524f..92a9746e5 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 @@ -1636,18 +1636,14 @@ struct PhysicsServerCommandProcessorInternalData btDefaultCollisionConfiguration* m_collisionConfiguration; -//#ifndef SKIP_DEFORMABLE_BODY +#ifndef SKIP_DEFORMABLE_BODY btDeformableBodySolver* m_deformablebodySolver; btAlignedObjectArray m_lf; -//#else//SKIP_DEFORMABLE_BODY -//#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD +#endif -//#else//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btMultiBodyDynamicsWorld* m_dynamicsWorld; -//#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD -//#endif//SKIP_DEFORMABLE_BODY int m_constraintSolverType; SharedMemoryDebugDrawer* m_remoteDebugDrawer; @@ -1713,6 +1709,9 @@ struct PhysicsServerCommandProcessorInternalData m_dispatcher(0), m_solver(0), m_collisionConfiguration(0), +#ifndef SKIP_DEFORMABLE_BODY + m_deformablebodySolver(0), +#endif m_dynamicsWorld(0), m_deformablebodySolver(0), m_constraintSolverType(-1), @@ -2652,23 +2651,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags) bv->setVelocityPrediction(0); m_data->m_broadphase = bv; } - - if (flags & RESET_USE_DEFORMABLE_WORLD) + + if (flags & RESET_USE_DEFORMABLE_WORLD) { #ifndef SKIP_DEFORMABLE_BODY - m_data->m_deformablebodySolver = new btDeformableBodySolver(); - btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; - m_data->m_solver = solver; - solver->setDeformableSolver(m_data->m_deformablebodySolver); - m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); + m_data->m_deformablebodySolver = new btDeformableBodySolver(); + btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; + m_data->m_solver = solver; + solver->setDeformableSolver(m_data->m_deformablebodySolver); + m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); #endif } - if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD))) + if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD))) { + m_data->m_solver = new btMultiBodyConstraintSolver; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - m_data->m_solver = new btMultiBodyConstraintSolver; - m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); +#else + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); #endif } @@ -5127,24 +5128,24 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S { btSoftBody* psb = bodyHandle->m_softBody; int totalBytesPerVertex = sizeof(btVector3); - bool separateRenderMesh = (psb->m_renderNodes.size() != 0); - int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); + bool separateRenderMesh = (psb->m_renderNodes.size() != 0); + int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size(); int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); btVector3* verticesOut = (btVector3*)bufferServerToClient; for (int i = 0; i < verticesCopied; ++i) { - if (separateRenderMesh) - { - const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; - verticesOut[i] = n.m_x; - } - else - { - const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; - verticesOut[i] = n.m_x; - } + if (separateRenderMesh) + { + const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; + verticesOut[i] = n.m_x; + } + else + { + const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex]; + verticesOut[i] = n.m_x; + } } serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; @@ -8070,7 +8071,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin; } - + { btSoftBody* psb = NULL; @@ -8084,13 +8085,22 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } - const std::string& error_message_prefix = ""; - std::string out_found_filename; - int out_type(0); + std::string out_found_filename, out_found_sim_filename; + int out_type(0), out_sim_type(0); bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - if (out_type == UrdfGeometry::FILE_OBJ) + + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH) + { + bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); + } + else + { + out_sim_type = out_type; + out_found_sim_filename = out_found_filename; + } + if (out_sim_type == UrdfGeometry::FILE_OBJ) { std::vector shapes; tinyobj::attrib_t attribute; @@ -8149,7 +8159,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { - psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_filename.c_str()); + psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str()); btScalar corotated_mu, corotated_lambda; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) { @@ -8186,37 +8196,55 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar if (psb != NULL) { #ifndef SKIP_DEFORMABLE_BODY - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - bool foundRenderMesh = false; + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) { - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_RENDER_MESH) - { - int out_render_type(0); - std::string out_found_render_filename; - char relativeRenderFileName[1024]; - char pathRenderPrefix[1024]; - pathRenderPrefix[0] = 0; - if (fileIO->findResourcePath(loadSoftBodyArgs.m_renderFileName, relativeRenderFileName, 1024)) - { - b3FileUtils::extractPath(relativeRenderFileName, pathRenderPrefix, 1024); - } - foundRenderMesh = UrdfFindMeshFile(fileIO, pathRenderPrefix, relativeRenderFileName, error_message_prefix, &out_found_render_filename, &out_render_type); - // load render mesh - if (foundRenderMesh) - { - btSoftBodyHelpers::readRenderMeshFromObj(out_found_render_filename.c_str(), psb); - btSoftBodyHelpers::interpolateBarycentricWeights(psb); - } - } - + // load render mesh + if (out_found_sim_filename != out_found_filename) + { + // load render mesh + { + 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()); - if (!foundRenderMesh) - { - psb->m_renderNodes.resize(0); - } - // This value should really be a exposed parameter - psb->setCollisionQuadrature(5); + 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 + { + psb->m_renderNodes.resize(0); + } btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); deformWorld->addForce(psb, gravityForce); @@ -8563,10 +8591,21 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD else if (body && body->m_softBody) { + btSoftBody* sb = body->m_softBody; + serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; + btVector3 aabbMin, aabbMax; + sb->getAabb(aabbMin, aabbMax); + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2]; + + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1]; + serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2]; + SharedMemoryStatus& serverCmd = serverStatusOut; serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED; - serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0; - setDefaultRootWorldAABB(serverCmd); } #endif return hasStatus; @@ -9441,6 +9480,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SPARSE_SDF) { +#ifndef SKIP_DEFORMABLE_BODY { btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); if (deformWorld) @@ -9449,6 +9489,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st deformWorld ->getWorldInfo().m_sparsesdf.Reset(); } } +#endif +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD { btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); if (softWorld) @@ -9457,6 +9499,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st softWorld->getWorldInfo().m_sparsesdf.Reset(); } } +#endif } if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) @@ -10647,6 +10690,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 935844f37..ccb6357fe 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -505,7 +505,7 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12, LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13, LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14, - LOAD_SOFT_BODY_RENDER_MESH = 1<<15, + LOAD_SOFT_BODY_SIM_MESH = 1<<15, }; enum EnumSimParamInternalSimFlags @@ -536,7 +536,7 @@ struct LoadSoftBodyArgs double m_NeoHookeanLambda; double m_NeoHookeanDamping; int m_useFaceContact; - char m_renderFileName[MAX_FILENAME_LENGTH]; + char m_simFileName[MAX_FILENAME_LENGTH]; }; struct b3LoadSoftBodyResultArgs @@ -802,6 +802,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..666334628 --- /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) + +gravZ=-10 +p.setGravity(0, 0, gravZ) + +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.setGravity(0,0,gravZ) + sleep(1./240.) + diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py index 8803e2fb2..4ff81b613 100644 --- a/examples/pybullet/examples/deformable_ball.py +++ b/examples/pybullet/examples/deformable_ball.py @@ -24,4 +24,4 @@ while p.isConnected(): sleep(1./240.) #p.resetSimulation() -#p.stopStateLogging(logId) \ No newline at end of file +#p.stopStateLogging(logId) diff --git a/examples/pybullet/examples/test_inertia.py b/examples/pybullet/examples/test_inertia.py new file mode 100644 index 000000000..a290f3bac --- /dev/null +++ b/examples/pybullet/examples/test_inertia.py @@ -0,0 +1,28 @@ +import pybullet as p +from time import sleep + +physicsClient = p.connect(p.GUI) + +useDeformable = True +if useDeformable: + p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) + +gravZ=-10 +p.setGravity(0, 0, gravZ) + +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("bunny.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1, useMassSpring=1, springElasticStiffness=100, springDampingStiffness=.001, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1) + +p.setTimeStep(0.0005) + +p.setRealTimeSimulation(1) + + +while p.isConnected(): + p.setGravity(0,0,gravZ) + sleep(1./240.) + 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 f89784ed4..a0d025480 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2567,7 +2567,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; @@ -3377,7 +3378,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]; + } } } } @@ -3966,7 +3970,7 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ for (int j = 0; j < 4; j++) { m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]); - memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j] ? m_tetras[j].m_n[j] - &m_nodes[0] : -1; + memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1; } memPtr->m_c1 = m_tetras[i].m_c1; memPtr->m_c2 = m_tetras[i].m_c2; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index c9e4af382..649d6f58c 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1500,44 +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 void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb) 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 37e657d62..eb290a1db 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -20,27 +20,38 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -// Modified Paul Hsieh hash -template -unsigned int HsiehHash(const void* pdata) -{ - const unsigned short* data = (const unsigned short*)pdata; - unsigned hash = DWORDLEN << 2, tmp; - for (int i = 0; i < DWORDLEN; ++i) - { - hash += data[0]; - tmp = (data[1] << 11) ^ hash; - hash = (hash << 16) ^ tmp; - data += 2; - hash += hash >> 11; - } - hash ^= hash << 3; - hash += hash >> 5; - hash ^= hash << 4; - hash += hash >> 17; - hash ^= hash << 25; - hash += hash >> 6; - return (hash); +// Fast Hash + +#if !defined (get16bits) +#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\ ++(unsigned int)(((const unsigned char *)(d))[0]) ) +#endif +// +// super hash function by Paul Hsieh +// +inline unsigned int HsiehHash (const char * data, int len) { + unsigned int hash = len, tmp; + len>>=2; + + /* Main loop */ + for (;len > 0; len--) { + hash += get16bits (data); + tmp = (get16bits (data+2) << 11) ^ hash; + hash = (hash << 16) ^ tmp; + data += 2*sizeof (unsigned short); + hash += hash >> 11; + } + + + /* Force "avalanching" of final 127 bits */ + hash ^= hash << 3; + hash += hash >> 5; + hash ^= hash << 4; + hash += hash >> 17; + hash ^= hash << 25; + hash += hash >> 6; + + return hash; } template @@ -209,6 +220,9 @@ struct btSparseSdf } else { + // printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]); + //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b); + c = c->next; } } @@ -260,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 @@ -340,16 +354,16 @@ struct btSparseSdf btS myset; //memset may be needed in case of additional (uninitialized) padding! - //memset(myset, 0, sizeof(btS)); + //memset(&myset, 0, sizeof(btS)); myset.x = x; myset.y = y; myset.z = z; myset.w = 0; myset.p = (void*)shape; - const void* ptr = &myset; + const char* ptr = (const char*)&myset; - unsigned int result = HsiehHash(ptr); + unsigned int result = HsiehHash(ptr, sizeof(btS) ); return result; }