diff --git a/Extras/ConvexDecomposition/cd_wavefront.cpp b/Extras/ConvexDecomposition/cd_wavefront.cpp index b33080964..53b5784df 100644 --- a/Extras/ConvexDecomposition/cd_wavefront.cpp +++ b/Extras/ConvexDecomposition/cd_wavefront.cpp @@ -437,7 +437,7 @@ const char **InPlaceParser::GetArglist(char *line, int &count) // convert sourc { const char **ret = 0; - const char *argv[MAXARGS]; + static const char *argv[MAXARGS]; int argc = 0; char *foo = line; diff --git a/data/torus_deform.urdf b/data/torus_deform.urdf new file mode 100644 index 000000000..c5d661993 --- /dev/null +++ b/data/torus_deform.urdf @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp index 93ec0fd7a..b3b6923f0 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp @@ -51,7 +51,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler m_showValue(true) { - memcpy(m_variableName, varName, strlen(varName) + 1); + strncpy(m_variableName, varName, sizeof(m_variableName)); } void SliderMoved(Gwen::Controls::Base* pControl) @@ -85,7 +85,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler if (m_showValue) { char txt[1024]; - sprintf(txt, "%s : %.3f", m_variableName, val); + snprintf(txt, sizeof(txt), "%s : %.3f", m_variableName, val); m_label->SetText(txt); } } @@ -231,7 +231,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params) } pSlider->SetValue(*params.m_paramValuePointer); //dimensions[i] ); char labelName[1024]; - sprintf(labelName, "%s", params.m_name); //axisNames[0]); + snprintf(labelName, sizeof(labelName), "%s", params.m_name); //axisNames[0]); MySliderEventHandler* handler = new MySliderEventHandler(labelName, label, pSlider, params.m_paramValuePointer, params.m_callback, params.m_userPointer); handler->m_showValue = params.m_showValues; m_paramInternalData->m_sliderEventHandlers.push_back(handler); diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp index f868bcb13..fe48dc8a6 100644 --- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp +++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp @@ -796,7 +796,7 @@ struct BulletMJCFImporterInternalData bool lastThree = false; parseVector3(size, sizeStr, logger, lastThree); } - geom.m_boxSize = size; + geom.m_boxSize = 2*size; handledGeomType = true; } if (geomType == "box") @@ -809,7 +809,7 @@ struct BulletMJCFImporterInternalData parseVector3(size, sizeStr, logger, lastThree); } geom.m_type = URDF_GEOM_BOX; - geom.m_boxSize = size; + geom.m_boxSize = 2*size; handledGeomType = true; } @@ -1012,7 +1012,7 @@ struct BulletMJCFImporterInternalData } case URDF_GEOM_BOX: { - totalVolume += 8. * col->m_geometry.m_boxSize[0] * + totalVolume += col->m_geometry.m_boxSize[0] * col->m_geometry.m_boxSize[1] * col->m_geometry.m_boxSize[2]; break; @@ -1890,7 +1890,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu case URDF_GEOM_BOX: { - btVector3 extents = visual->m_geometry.m_boxSize; + btVector3 extents = 0.5*visual->m_geometry.m_boxSize; btBoxShape* boxShape = new btBoxShape(extents * 0.5f); //btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5); convexColShape = boxShape; @@ -2336,7 +2336,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn } case URDF_GEOM_BOX: { - childShape = new btBoxShape(col->m_geometry.m_boxSize); + childShape = new btBoxShape(0.5*col->m_geometry.m_boxSize); break; } case URDF_GEOM_CYLINDER: diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 72113b88c..ec0cba28f 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -1533,3 +1533,8 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn const struct UrdfModel* BulletURDFImporter::getUrdfModel() const { return &m_data->m_urdfParser.getModel(); }; + +const struct UrdfDeformable& BulletURDFImporter::getDeformableModel() const +{ + return m_data->m_urdfParser.getDeformable(); +} diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 8c1906d2b..eb890c016 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -2,8 +2,8 @@ #define BULLET_URDF_IMPORTER_H #include "URDFImporterInterface.h" - #include "UrdfRenderingInterface.h" +#include "UrdfParser.h" struct BulletURDFTexture { @@ -91,6 +91,7 @@ public: virtual void setEnableTinyRenderer(bool enable); void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, struct b3ImportMeshData& meshData) const; + const struct UrdfDeformable& getDeformableModel() const; }; #endif //BULLET_URDF_IMPORTER_H diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 23a29383a..f656e4fde 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1098,6 +1098,155 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement* config, return true; } +bool UrdfParser::parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger) +{ + const char* mu = config->Attribute("mu"); + const char* lambda = config->Attribute("lambda"); + const char* damping = config->Attribute("damping"); + if (!mu || !lambda) + { + logger->reportError("expected mu lambda for LameCoefficients."); + return false; + } + lameCoefficients.mu = urdfLexicalCast(mu); + lameCoefficients.lambda = urdfLexicalCast(lambda); + if (damping) + lameCoefficients.damping = urdfLexicalCast(damping); + else + lameCoefficients.damping = 0; + return true; +} + +bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger) +{ + UrdfDeformable& deformable = model.m_deformable; + const char* deformableName = config->Attribute("name"); + if (!deformableName) + { + logger->reportError("Deformable with no name"); + return false; + } + deformable.m_name = deformableName; + + XMLElement* i = config->FirstChildElement("inertial"); + if (!i) + { + logger->reportError("expected an inertial element"); + } + UrdfInertia inertia; + if (!parseInertia(inertia, i, logger)) + { + logger->reportError("Could not parse inertial element for deformable:"); + logger->reportError(deformable.m_name.c_str()); + return false; + } + deformable.m_mass = inertia.m_mass; + + XMLElement* collisionMargin_xml = config->FirstChildElement("collision_margin"); + if (collisionMargin_xml) + { + if (!collisionMargin_xml->Attribute("value")) + { + logger->reportError("collision_margin element must have value attribute"); + return false; + } + deformable.m_collisionMargin = urdfLexicalCast(collisionMargin_xml->Attribute("value")); + } + + XMLElement* friction_xml = config->FirstChildElement("friction"); + if (friction_xml) + { + if (!friction_xml->Attribute("value")) + { + logger->reportError("friction element must have value attribute"); + return false; + } + deformable.m_friction = urdfLexicalCast(friction_xml->Attribute("value")); + } + + XMLElement* spring_xml = config->FirstChildElement("spring"); + if (spring_xml) + { + if (!spring_xml->Attribute("elastic_stiffness") || !spring_xml->Attribute("damping_stiffness")) + { + logger->reportError("spring element expect elastic and damping stiffness"); + return false; + } + + deformable.m_springCoefficients.elastic_stiffness = urdfLexicalCast(spring_xml->Attribute("elastic_stiffness")); + deformable.m_springCoefficients.damping_stiffness = urdfLexicalCast(spring_xml->Attribute("damping_stiffness")); + + if (spring_xml->Attribute("bending_stiffness")) + deformable.m_springCoefficients.bending_stiffness = urdfLexicalCast(spring_xml->Attribute("bending_stiffness")); + } + + XMLElement* corotated_xml = config->FirstChildElement("corotated"); + if (corotated_xml) + { + if (!parseLameCoefficients(deformable.m_corotatedCoefficients, corotated_xml, logger)) + { + return false; + } + } + + XMLElement* neohookean_xml = config->FirstChildElement("neohookean"); + if (neohookean_xml) + { + if (!parseLameCoefficients(deformable.m_neohookeanCoefficients, neohookean_xml, logger)) + { + return false; + } + } + + XMLElement* vis_xml = config->FirstChildElement("visual"); + if (!vis_xml) + { + logger->reportError("expected an visual element"); + return false; + } + if (!vis_xml->Attribute("filename")) + { + logger->reportError("expected a filename for visual geometry"); + return false; + } + std::string fn = vis_xml->Attribute("filename"); + deformable.m_visualFileName = fn; + + int out_type(0); + bool success = UrdfFindMeshFile(m_fileIO, + model.m_sourceFile, fn, sourceFileLocation(vis_xml), + &deformable.m_visualFileName, &out_type); + + if (!success) + { + // warning already printed + return false; + } + + XMLElement* col_xml = config->FirstChildElement("collision"); + if (col_xml) + { + if (!col_xml->Attribute("filename")) + { + logger->reportError("expected a filename for collision geoemtry"); + return false; + } + fn = vis_xml->Attribute("filename"); + success = UrdfFindMeshFile(m_fileIO, + model.m_sourceFile, fn, sourceFileLocation(vis_xml), + &deformable.m_simFileName, &out_type); + + if (!success) + { + // warning already printed + return false; + } + } + + ParseUserData(config, deformable.m_userData, logger); + return true; +} + bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger) { joint.m_lowerLimit = 0.f; @@ -1898,6 +2047,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF } m_urdf2Model.m_name = name; + ParseUserData(robot_xml, m_urdf2Model.m_userData, logger); + // Get all Material elements for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material")) { @@ -1921,6 +2072,11 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF // sprintf(msg,"Num materials=%d", m_model.m_materials.size()); // logger->printMessage(msg); + + XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable"); + if(deformable_xml) + return parseDeformable(m_urdf2Model, deformable_xml, logger); + for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link")) { UrdfLink* link = new UrdfLink; @@ -2040,8 +2196,6 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF } } - ParseUserData(robot_xml, m_urdf2Model.m_userData, logger); - if (m_urdf2Model.m_links.size() == 0) { logger->reportWarning("No links found in URDF file"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index dff15c472..9b99bc36f 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -200,6 +200,45 @@ struct UrdfJoint } }; + +struct SpringCoeffcients{ + double elastic_stiffness; + double damping_stiffness; + double bending_stiffness; + SpringCoeffcients(): + elastic_stiffness(0.), + damping_stiffness(0.), + bending_stiffness(0.){} +}; + +struct LameCoefficients +{ + double mu; + double lambda; + double damping; + LameCoefficients() : mu(0.), lambda(0.), damping(0.) {} +}; + +struct UrdfDeformable +{ + std::string m_name; + double m_mass; + double m_collisionMargin; + double m_friction; + + SpringCoeffcients m_springCoefficients; + LameCoefficients m_corotatedCoefficients; + LameCoefficients m_neohookeanCoefficients; + + std::string m_visualFileName; + std::string m_simFileName; + btHashMap m_userData; + + UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_visualFileName(""), m_simFileName("") + { + } +}; + struct UrdfModel { std::string m_name; @@ -208,6 +247,7 @@ struct UrdfModel btHashMap m_materials; btHashMap m_links; btHashMap m_joints; + UrdfDeformable m_deformable; // Maps user data keys to user data values. btHashMap m_userData; @@ -283,6 +323,9 @@ protected: bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger); bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger); bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger); + bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger); + bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger); + public: UrdfParser(struct CommonFileIOInterface* fileIO); @@ -357,11 +400,14 @@ public: return m_urdf2Model; } + const UrdfDeformable& getDeformable() const + { + return m_urdf2Model.m_deformable; + } bool mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level); bool printTree(UrdfLink* link, ErrorLogger* logger, int level); bool recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger); - std::string sourceFileLocation(tinyxml2::XMLElement* e); diff --git a/examples/OpenGLWindow/EGLOpenGLWindow.cpp b/examples/OpenGLWindow/EGLOpenGLWindow.cpp index 6fb9797ae..f8918f318 100644 --- a/examples/OpenGLWindow/EGLOpenGLWindow.cpp +++ b/examples/OpenGLWindow/EGLOpenGLWindow.cpp @@ -141,8 +141,13 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) if (eglGetError() == EGL_SUCCESS && initialized == EGL_TRUE) { m_data->egl_display = display; + break; } } + else + { + fprintf(stderr, "GetDisplay %d failed with error: %x\n", i, eglGetError()); + } } } else @@ -166,6 +171,10 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) m_data->egl_display = display; } } + else + { + fprintf(stderr, "GetDisplay %d failed with error: %x\n", m_data->m_renderDevice, eglGetError()); + } } if (!eglInitialize(m_data->egl_display, NULL, NULL)) diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index f1f46f951..a55729eaf 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -654,8 +654,8 @@ void PhysicsClientExample::createButtons() { if (m_numMotors < MAX_NUM_MOTORS) { - char motorName[1024]; - sprintf(motorName, "%s q", info.m_jointName); + char motorName[1026]; + snprintf(motorName, sizeof(motorName), "%s q", info.m_jointName); // MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors]; MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors]; motorInfo->m_velTarget = 0.f; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0c611326f..e1807e354 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3489,6 +3489,12 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto rootTrans.setOrigin(pos); rootTrans.setRotation(orn); u2b.setRootTransformInWorld(rootTrans); + if (!(u2b.getDeformableModel().m_visualFileName.empty())) + { + bool use_self_collision = false; + use_self_collision = (flags & CUF_USE_SELF_COLLISION); + return processDeformable(u2b.getDeformableModel(), pos, orn, bodyUniqueIdPtr, bufferServerToClient, bufferSizeInBytes, globalScaling, use_self_collision); + } bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); if (ok) { @@ -8169,274 +8175,276 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe return hasStatus; } -bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDeformable& deformable, bool verbose) { - serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; - bool hasStatus = true; -#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - double scale = 1; - double mass = 1; - double collisionMargin = 0.02; const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments; - if (m_data->m_verboseOutput) + if (verbose) { b3Printf("Processed CMD_LOAD_SOFT_BODY:%s", loadSoftBodyArgs.m_fileName); } btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0); btAssert(loadSoftBodyArgs.m_fileName); - btVector3 initialPos(0, 0, 0); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION) - { - initialPos[0] = loadSoftBodyArgs.m_initialPosition[0]; - initialPos[1] = loadSoftBodyArgs.m_initialPosition[1]; - initialPos[2] = loadSoftBodyArgs.m_initialPosition[2]; - } - btQuaternion initialOrn(0, 0, 0, 1); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION) - { - initialOrn[0] = loadSoftBodyArgs.m_initialOrientation[0]; - initialOrn[1] = loadSoftBodyArgs.m_initialOrientation[1]; - initialOrn[2] = loadSoftBodyArgs.m_initialOrientation[2]; - initialOrn[3] = loadSoftBodyArgs.m_initialOrientation[3]; - } - - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE) - { - scale = clientCmd.m_loadSoftBodyArguments.m_scale; - } if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS) { - mass = clientCmd.m_loadSoftBodyArguments.m_mass; + deformable.m_mass = loadSoftBodyArgs.m_mass; } if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN) { - collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin; + deformable.m_collisionMargin = loadSoftBodyArgs.m_collisionMargin; } - btScalar spring_bending_stiffness = 0; + deformable.m_visualFileName = loadSoftBodyArgs.m_fileName; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH) { - btSoftBody* psb = NULL; - CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); - char relativeFileName[1024]; - char pathPrefix[1024]; - pathPrefix[0] = 0; - if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024)) - { - b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); - } - const std::string& error_message_prefix = ""; - std::string out_found_filename, out_found_sim_filename; - int out_type(0), out_sim_type(0); + deformable.m_simFileName = loadSoftBodyArgs.m_simFileName; + } + else + { + deformable.m_simFileName = ""; + } +#ifndef SKIP_DEFORMABLE_BODY + deformable.m_springCoefficients.elastic_stiffness = loadSoftBodyArgs.m_springElasticStiffness; + deformable.m_springCoefficients.damping_stiffness = loadSoftBodyArgs.m_springDampingStiffness; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) + { + deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness; + } - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH) + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) + { + deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu; + deformable.m_corotatedCoefficients.lambda = loadSoftBodyArgs.m_corotatedLambda; + } + + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) + { + deformable.m_neohookeanCoefficients.mu = loadSoftBodyArgs.m_NeoHookeanMu; + deformable.m_neohookeanCoefficients.lambda = loadSoftBodyArgs.m_NeoHookeanLambda; + deformable.m_neohookeanCoefficients.damping = loadSoftBodyArgs.m_NeoHookeanDamping; + } + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) + { + deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff; + } + +#endif +} + +bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision) +{ + btSoftBody* psb = NULL; + CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); + char relativeFileName[1024]; + char pathPrefix[1024]; + pathPrefix[0] = 0; + if (fileIO->findResourcePath(deformable.m_visualFileName.c_str(), relativeFileName, 1024)) + { + b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); + } + const std::string& error_message_prefix = ""; + 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 (!deformable.m_simFileName.empty()) + { + bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, deformable.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; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO); + if (!shapes.empty()) { - 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; - std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO); - if (!shapes.empty()) + const tinyobj::shape_t& shape = shapes[0]; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + for (int i = 0; i < attribute.vertices.size(); i++) + { + vertices.push_back(attribute.vertices[i]); + } + for (int i = 0; i < shape.mesh.indices.size(); i++) + { + indices.push_back(shape.mesh.indices[i].vertex_index); + } + int numTris = shape.mesh.indices.size() / 3; + if (numTris > 0) { - const tinyobj::shape_t& shape = shapes[0]; - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - for (int i = 0; i < attribute.vertices.size(); i++) - { - vertices.push_back(attribute.vertices[i]); - } - for (int i = 0; i < shape.mesh.indices.size(); i++) - { - indices.push_back(shape.mesh.indices[i].vertex_index); - } - int numTris = shape.mesh.indices.size() / 3; - if (numTris > 0) { + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) { - btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); - if (softWorld) + psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + if (!psb) { - psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + printf("Load deformable failed\n"); + return false; } } + } + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) + psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + if (!psb) { - psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + printf("Load deformable failed\n"); + return false; } } } } -#ifndef SKIP_DEFORMABLE_BODY - btScalar spring_elastic_stiffness, spring_damping_stiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) - { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) - { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) - { - spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness; - } - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); - deformWorld->addForce(psb, springForce); - m_data->m_lf.push_back(springForce); - } - } -#endif } - else if (out_sim_type == UrdfGeometry::FILE_VTK) - { #ifndef SKIP_DEFORMABLE_BODY - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) - { - 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) - { - corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; - corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; - btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); - deformWorld->addForce(psb, corotatedForce); - m_data->m_lf.push_back(corotatedForce); - } - btScalar neohookean_mu, neohookean_lambda, neohookean_damping; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) - { - neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; - neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; - neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping; - btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); - deformWorld->addForce(psb, neohookeanForce); - m_data->m_lf.push_back(neohookeanForce); - } - btScalar spring_elastic_stiffness, spring_damping_stiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) - { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) - { - spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness; - } - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); - deformWorld->addForce(psb, springForce); - m_data->m_lf.push_back(springForce); - } - } -#endif + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld && deformable.m_springCoefficients.elastic_stiffness > 0.) + { + btDeformableLagrangianForce* springForce = + new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness, + deformable.m_springCoefficients.damping_stiffness, + true, deformable.m_springCoefficients.bending_stiffness); + deformWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); } - if (psb != NULL) - { +#endif + } + else if (out_sim_type == UrdfGeometry::FILE_VTK) + { #ifndef SKIP_DEFORMABLE_BODY - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str()); + if (!psb) + { + printf("Load deformable failed\n"); + return false; + } + btScalar corotated_mu(0.), corotated_lambda(0.); + corotated_mu = deformable.m_corotatedCoefficients.mu; + corotated_lambda = deformable.m_corotatedCoefficients.lambda; + if (corotated_mu > 0 || corotated_lambda > 0) + { + btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); + deformWorld->addForce(psb, corotatedForce); + m_data->m_lf.push_back(corotatedForce); + } + btScalar neohookean_mu, neohookean_lambda, neohookean_damping; + neohookean_mu = deformable.m_neohookeanCoefficients.mu; + neohookean_lambda = deformable.m_neohookeanCoefficients.lambda; + neohookean_damping = deformable.m_neohookeanCoefficients.damping; + if (neohookean_mu > 0 || neohookean_lambda > 0) + { + btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); + deformWorld->addForce(psb, neohookeanForce); + m_data->m_lf.push_back(neohookeanForce); + } + + btScalar spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness; + spring_elastic_stiffness = deformable.m_springCoefficients.elastic_stiffness; + spring_damping_stiffness = deformable.m_springCoefficients.damping_stiffness; + spring_bending_stiffness = deformable.m_springCoefficients.bending_stiffness; + if (spring_elastic_stiffness > 0.) + { + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness); + deformWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); + } + } +#endif + } + if (psb != NULL) + { +#ifndef SKIP_DEFORMABLE_BODY + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + // load render mesh + if (out_found_sim_filename != out_found_filename) { // 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()); + + for (int s = 0; s < (int)shapes.size(); s++) { - 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; v < vertexCount; v++) { - 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())) { - btSoftBody::Node n; - n.m_x = btVector3(attribute.vertices[3*v],attribute.vertices[3*v+1],attribute.vertices[3*v+2]); - psb->m_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); + 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); } } - if (out_sim_type == UrdfGeometry::FILE_VTK) - { - btSoftBodyHelpers::interpolateBarycentricWeights(psb); - } - else if (out_sim_type == UrdfGeometry::FILE_OBJ) - { - btSoftBodyHelpers::extrapolateBarycentricWeights(psb); - } } - else + if (out_sim_type == UrdfGeometry::FILE_VTK) { - psb->m_renderNodes.resize(0); + btSoftBodyHelpers::interpolateBarycentricWeights(psb); } - btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); - btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); - deformWorld->addForce(psb, gravityForce); - m_data->m_lf.push_back(gravityForce); - btScalar collision_hardness = 1; - psb->m_cfg.kKHR = collision_hardness; - psb->m_cfg.kCHR = collision_hardness; - - btScalar friction_coeff = 0; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) + else if (out_sim_type == UrdfGeometry::FILE_OBJ) { - friction_coeff = loadSoftBodyArgs.m_frictionCoeff; + btSoftBodyHelpers::extrapolateBarycentricWeights(psb); } - psb->m_cfg.kDF = friction_coeff; - bool use_bending_spring = false; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) - { - use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; - if (use_bending_spring) - { - psb->generateBendingConstraints(2); - } - } - btSoftBody::Material* pm = psb->appendMaterial(); - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - - // turn on the collision flag for deformable - // collision between deformable and rigid - psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - // turn on face contact only for multibodies - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; - // collion between deformable and deformable and self-collision - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; - psb->setCollisionFlags(0); - psb->setTotalMass(mass); - bool use_self_collision = false; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION) - { - use_self_collision = loadSoftBodyArgs.m_useSelfCollision; - } - psb->setSelfCollision(use_self_collision); } -#endif//SKIP_DEFORMABLE_BODY + else + { + psb->m_renderNodes.resize(0); + } + btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); + btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); + deformWorld->addForce(psb, gravityForce); + m_data->m_lf.push_back(gravityForce); + btScalar collision_hardness = 1; + psb->m_cfg.kKHR = collision_hardness; + psb->m_cfg.kCHR = collision_hardness; + + psb->m_cfg.kDF = deformable.m_friction; + if (deformable.m_springCoefficients.bending_stiffness) + { + psb->generateBendingConstraints(2); + } + btSoftBody::Material* pm = psb->appendMaterial(); + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + + // turn on the collision flag for deformable + // collision between deformable and rigid + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + // turn on face contact only for multibodies + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + // collion between deformable and deformable and self-collision + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->setCollisionFlags(0); + psb->setTotalMass(deformable.m_mass); + psb->setSelfCollision(useSelfCollision); + } +#endif //SKIP_DEFORMABLE_BODY #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); if (softWorld) @@ -8450,21 +8458,22 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar //turn on softbody vs softbody collision psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->randomizeConstraints(); - psb->setTotalMass(mass, true); + psb->setTotalMass(deformable.m_mass, true); } -#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - psb->scale(btVector3(scale, scale, scale)); - psb->rotate(initialOrn); - psb->translate(initialPos); +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + psb->scale(btVector3(scale, scale, scale)); + psb->rotate(orn); + psb->translate(pos); - psb->getCollisionShape()->setMargin(collisionMargin); - psb->getCollisionShape()->setUserPointer(psb); + psb->getCollisionShape()->setMargin(deformable.m_collisionMargin); + psb->getCollisionShape()->setUserPointer(psb); #ifndef SKIP_DEFORMABLE_BODY if (deformWorld) { deformWorld->addSoftBody(psb); - } else -#endif//SKIP_DEFORMABLE_BODY + } + else +#endif //SKIP_DEFORMABLE_BODY { btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); if (softWorld) @@ -8472,71 +8481,123 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar softWorld->addSoftBody(psb); } } - m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); - InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); - bodyHandle->m_softBody = psb; + m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); + *bodyUniqueId = m_data->m_bodyHandles.allocHandle(); + InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId); + bodyHandle->m_softBody = psb; - b3VisualShapeData visualShape; + b3VisualShapeData visualShape; - visualShape.m_objectUniqueId = bodyUniqueId; - visualShape.m_linkIndex = -1; - visualShape.m_visualGeometryType = URDF_GEOM_MESH; - //dimensions just contains the scale - visualShape.m_dimensions[0] = scale; - visualShape.m_dimensions[1] = scale; - visualShape.m_dimensions[2] = scale; - //filename - strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); - visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; - //position and orientation - visualShape.m_localVisualFrame[0] = initialPos[0]; - visualShape.m_localVisualFrame[1] = initialPos[1]; - visualShape.m_localVisualFrame[2] = initialPos[2]; - visualShape.m_localVisualFrame[3] = initialOrn[0]; - visualShape.m_localVisualFrame[4] = initialOrn[1]; - visualShape.m_localVisualFrame[5] = initialOrn[2]; - visualShape.m_localVisualFrame[6] = initialOrn[3]; - //color and ids to be set by the renderer - visualShape.m_rgbaColor[0] = 0; - visualShape.m_rgbaColor[1] = 0; - visualShape.m_rgbaColor[2] = 0; - visualShape.m_rgbaColor[3] = 1; - visualShape.m_tinyRendererTextureId = -1; - visualShape.m_textureUniqueId =-1; - visualShape.m_openglTextureId = -1; + visualShape.m_objectUniqueId = *bodyUniqueId; + visualShape.m_linkIndex = -1; + visualShape.m_visualGeometryType = URDF_GEOM_MESH; + //dimensions just contains the scale + visualShape.m_dimensions[0] = scale; + visualShape.m_dimensions[1] = scale; + visualShape.m_dimensions[2] = scale; + //filename + strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); + visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; + //position and orientation + visualShape.m_localVisualFrame[0] = pos[0]; + visualShape.m_localVisualFrame[1] = pos[1]; + visualShape.m_localVisualFrame[2] = pos[2]; + visualShape.m_localVisualFrame[3] = orn[0]; + visualShape.m_localVisualFrame[4] = orn[1]; + visualShape.m_localVisualFrame[5] = orn[2]; + visualShape.m_localVisualFrame[6] = orn[3]; + //color and ids to be set by the renderer + visualShape.m_rgbaColor[0] = 0; + visualShape.m_rgbaColor[1] = 0; + visualShape.m_rgbaColor[2] = 0; + visualShape.m_rgbaColor[3] = 1; + visualShape.m_tinyRendererTextureId = -1; + visualShape.m_textureUniqueId = -1; + visualShape.m_openglTextureId = -1; - m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO); + m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO); - serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; - serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; - int pos = strlen(relativeFileName)-1; - while(pos>=0 && relativeFileName[pos]!='/') { pos--;} - btAssert(strlen(relativeFileName)-pos-5>0); - std::string object_name (std::string(relativeFileName).substr(pos+1, strlen(relativeFileName)- 5 - pos)); + if (!deformable.m_name.empty()) + { + bodyHandle->m_bodyName = deformable.m_name; + } + else + { + int pos = strlen(relativeFileName) - 1; + while (pos >= 0 && relativeFileName[pos] != '/') + { + pos--; + } + btAssert(strlen(relativeFileName) - pos - 5 > 0); + std::string object_name(std::string(relativeFileName).substr(pos + 1, strlen(relativeFileName) - 5 - pos)); bodyHandle->m_bodyName = object_name; + } + b3Notification notification; + notification.m_notificationType = BODY_ADDED; + notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId; + m_data->m_pluginManager.addNotification(notification); + } + return true; +} - int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; +bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED; + bool hasStatus = true; +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + UrdfDeformable deformable; + + constructUrdfDeformable(clientCmd, deformable, m_data->m_verboseOutput); + // const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments; + btVector3 initialPos(0, 0, 0); + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION) + { + initialPos[0] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[0]; + initialPos[1] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[1]; + initialPos[2] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[2]; + } + btQuaternion initialOrn(0, 0, 0, 1); + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION) + { + initialOrn[0] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[0]; + initialOrn[1] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[1]; + initialOrn[2] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[2]; + initialOrn[3] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[3]; + } + + double scale = 1; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE) + { + scale = clientCmd.m_loadSoftBodyArguments.m_scale; + } + bool use_self_collision = false; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION) + { + use_self_collision = clientCmd.m_loadSoftBodyArguments.m_useSelfCollision; + } + + int bodyUniqueId = -1; + bool completedOk = processDeformable(deformable, initialPos, initialOrn, &bodyUniqueId, bufferServerToClient, bufferSizeInBytes, scale, use_self_collision); + if (completedOk && bodyUniqueId >= 0) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld); + serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; #ifdef ENABLE_LINK_MAPPER - if (m_data->m_urdfLinkNameMapper.size()) - { - serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize(); - } -#endif - serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; - InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); - strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); - - b3Notification notification; - notification.m_notificationType = BODY_ADDED; - notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; - m_data->m_pluginManager.addNotification(notification); + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize(); } - } #endif + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId; +#endif + } return hasStatus; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index ef972a400..18db2cdd7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -5,6 +5,7 @@ #include "LinearMath/btVector3.h" #include "PhysicsCommandProcessorInterface.h" +#include "../Importers/ImportURDFDemo/UrdfParser.h" struct SharedMemLines { @@ -104,6 +105,7 @@ protected: bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags); bool processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, class URDFImporterInterface& u2b); + bool processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision); bool supportsJointMotor(class btMultiBody* body, int linkIndex); diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index 6110b45b1..672b7311d 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -1350,9 +1350,6 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL( m_data->m_instancingRenderer->updateCamera(m_data->m_upAxis); m_data->m_instancingRenderer->renderScene(); - m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(1, 0, 0), b3MakeVector3(1, 0, 0), 3); - m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 1, 0), b3MakeVector3(0, 1, 0), 3); - m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 0, 1), b3MakeVector3(0, 0, 1), 3); int numBytesPerPixel = 4; //RGBA diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index 6acf1f004..808a1161e 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -72,7 +72,6 @@ struct InMemoryFileIO : public CommonFileIOInterface { for (int i=0;i + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d3b40dcee..d6beded74 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -30,9 +30,18 @@ #if defined(__APPLE__) && (!defined(B3_NO_PYTHON_FRAMEWORK)) #include #else +#ifdef _WIN32 + #ifdef _DEBUG + #define BT_REMOVED_DEBUG + //always use the release build of Python + #undef _DEBUG + #endif //_DEBUG +#endif #include #endif - +#ifdef BT_REMOVED_DEBUG + #define _DEBUG +#endif #include "../Importers/ImportURDFDemo/urdfStringSplit.h" #ifdef B3_DUMP_PYTHON_VERSION diff --git a/setup.py b/setup.py index 5dd87e615..2d97481b1 100644 --- a/setup.py +++ b/setup.py @@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name='pybullet', - version='2.7.1', + version='2.7.3', description= 'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description= diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 6b9f5e23a..04309670c 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -46,8 +46,6 @@ protected: btAlignedObjectArray m_manifoldsPtr; - btManifoldResult m_defaultManifoldResult; - btNearCallback m_nearCallback; btPoolAllocator* m_collisionAlgorithmPoolAllocator; @@ -95,11 +93,15 @@ public: btPersistentManifold* getManifoldByIndexInternal(int index) { + btAssert(index>=0); + btAssert(index=0); + btAssert(indexm_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY); + if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR) + info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY); + else + info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1); btScalar minf = f < fd ? f : fd; btScalar maxf = f < fd ? fd : f; diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index 00e24364e..c86dc373d 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -265,6 +265,7 @@ enum bt6DofFlags2 BT_6DOF_FLAGS_ERP_STOP2 = 2, BT_6DOF_FLAGS_CFM_MOTO2 = 4, BT_6DOF_FLAGS_ERP_MOTO2 = 8, + BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16) }; #define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e4da46829..d2641c582 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -14,7 +14,9 @@ subject to the following restrictions: */ //#define COMPUTE_IMPULSE_DENOM 1 -//#define BT_ADDITIONAL_DEBUG +#ifdef BT_DEBUG +# define BT_ADDITIONAL_DEBUG +#endif //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. @@ -690,8 +692,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& { #if BT_THREADSAFE int solverBodyId = -1; - bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; - if (isRigidBodyType && !body.isStaticOrKinematicObject()) + const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; + const bool isStaticOrKinematic = body.isStaticOrKinematicObject(); + const bool isKinematic = body.isKinematicObject(); + if (isRigidBodyType && !isStaticOrKinematic) { // dynamic body // Dynamic bodies can only be in one island, so it's safe to write to the companionId @@ -704,7 +708,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body.setCompanionId(solverBodyId); } } - else if (isRigidBodyType && body.isKinematicObject()) + else if (isRigidBodyType && isKinematic) { // // NOTE: must test for kinematic before static because some kinematic objects also diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index 9e8705b00..f1b50b39c 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -136,8 +136,13 @@ void btRigidBody::setGravity(const btVector3& acceleration) void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) { - m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); +#ifdef BT_USE_OLD_DAMPING_METHOD + m_linearDamping = btMax(lin_damping, btScalar(0.0)); + m_angularDamping = btMax(ang_damping, btScalar(0.0)); +#else + m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0)); + m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0)); +#endif } ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping @@ -146,10 +151,9 @@ void btRigidBody::applyDamping(btScalar timeStep) //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway -//#define USE_OLD_DAMPING_METHOD 1 -#ifdef USE_OLD_DAMPING_METHOD - m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); +#ifdef BT_USE_OLD_DAMPING_METHOD + m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0)); + m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0)); #else m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep); m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep); diff --git a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp index 5353fe009..772b77420 100644 --- a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp +++ b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -171,6 +171,8 @@ void btSimulationIslandManagerMt::initIslandPools() btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id) { + btAssert(id >= 0); + btAssert(id < m_lookupIslandFromId.size()); Island* island = m_lookupIslandFromId[id]; if (island == NULL) { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index bdaa47347..a1d5bb9ca 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -583,52 +583,6 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const } } -btScalar btMultiBody::getKineticEnergy() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray omega; - omega.resize(num_links + 1); - btAlignedObjectArray vel; - vel.resize(num_links + 1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - // we will do the factor of 0.5 at the end - btScalar result = m_baseMass * vel[0].dot(vel[0]); - result += omega[0].dot(m_baseInertia * omega[0]); - - for (int i = 0; i < num_links; ++i) - { - result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]); - result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]); - } - - return 0.5f * result; -} - -btVector3 btMultiBody::getAngularMomentum() const -{ - int num_links = getNumLinks(); - // TODO: would be better not to allocate memory here - btAlignedObjectArray omega; - omega.resize(num_links + 1); - btAlignedObjectArray vel; - vel.resize(num_links + 1); - btAlignedObjectArray rot_from_world; - rot_from_world.resize(num_links + 1); - compTreeLinkVelocities(&omega[0], &vel[0]); - - rot_from_world[0] = m_baseQuat; - btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0])); - - for (int i = 0; i < num_links; ++i) - { - rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1]; - result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1]))); - } - - return result; -} void btMultiBody::clearConstraintForces() { diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index afed669a7..be795633f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -307,13 +307,6 @@ public: // btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const; - // - // calculate kinetic energy and angular momentum - // useful for debugging. - // - - btScalar getKineticEnergy() const; - btVector3 getAngularMomentum() const; // // set external forces and torques. Note all external forces/torques are given in the WORLD frame. diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 877e327c4..278836743 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -30,10 +30,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); //solve featherstone non-contact constraints - + btScalar nonContactResidual = 0; //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i) { + // reset the nonContactResdual to 0 at start of each inner iteration + nonContactResidual = 0; for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) { int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; @@ -41,7 +43,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; btScalar residual = resolveSingleConstraintRowGeneric(constraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + nonContactResidual = btMax(nonContactResidual, residual * residual); if (constraint.m_multiBodyA) constraint.m_multiBodyA->setPosUpdated(false); @@ -49,6 +51,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl constraint.m_multiBodyB->setPosUpdated(false); } } + leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual); //solve featherstone normal contact for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c3cb53806..3df9320e9 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -222,6 +222,9 @@ void btSoftBody::initDefaults() m_useSelfCollision = false; m_collisionFlags = 0; m_softSoftCollision = false; + m_maxSpeedSquared = 0; + m_repulsionStiffness = 0.5; + m_fdbvnt = 0; } // @@ -2602,7 +2605,7 @@ void btSoftBody::initializeFaceTree() for (int i = 0; i < m_faces.size(); ++i) { Face& f = m_faces[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, m_sst.radmrg); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &f; @@ -2638,10 +2641,10 @@ void btSoftBody::initializeFaceTree() } } m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj); - updateFaceTree(false, true); if (m_fdbvnt) delete m_fdbvnt; m_fdbvnt = copyToDbvnt(m_fdbvt.m_root); + updateFaceTree(false, false); rebuildNodeTree(); } @@ -2654,7 +2657,7 @@ void btSoftBody::rebuildNodeTree() for (int i = 0; i < m_nodes.size(); ++i) { Node& n = m_nodes[i]; - ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg); + ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0); btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode(); node->parent = NULL; node->data = &n; @@ -3451,7 +3454,7 @@ void btSoftBody::setSpringStiffness(btScalar k) { m_links[i].Feature::m_material->m_kLST = k; } - repulsionStiffness = k; + m_repulsionStiffness = k; } void btSoftBody::initializeDmInverse() diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index b03af0736..6a55eccbd 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -724,6 +724,15 @@ public: /* SolverState */ struct SolverState { + //if you add new variables, always initialize them! + SolverState() + :sdt(0), + isdt(0), + velmrg(0), + radmrg(0), + updmrg(0) + { + } btScalar sdt; // dt*timescale btScalar isdt; // 1/sdt btScalar velmrg; // velocity margin @@ -810,7 +819,7 @@ public: btScalar m_sleepingThreshold; btScalar m_maxSpeedSquared; btAlignedObjectArray m_quads; // quadrature points for collision detection - btScalar repulsionStiffness; + btScalar m_repulsionStiffness; btAlignedObjectArray m_X; // initial positions btAlignedObjectArray m_renderNodesInterpolationWeights; @@ -1300,7 +1309,7 @@ public: btScalar I = 0; btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im; if (applySpringForce) - I = -btMin(repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); + I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn)); if (vn < 0) I += 0.5 * mass * vn; btScalar face_penetration = 0, node_penetration = node->m_penetration; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 8f98b3bea..c1a87c7d5 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1300,13 +1300,15 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, } else if (reading_tets) { - int d; - ss >> d; - if (d != 4) - { - printf("Only Tetrahedra are supported in vtk file\n"); - return 0; - } + int d; + ss >> d; + if (d != 4) + { + printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n"); + fs.close(); + return 0; + } + ss.ignore(128, ' '); // ignore "4" Index tet; tet.resize(4); for (size_t i = 0; i < 4; i++) @@ -1597,7 +1599,13 @@ void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) { new_min_bary_weight = btMin(new_min_bary_weight, bary[k]); } - if (new_min_bary_weight > min_bary_weight) + + // p is out of the current best triangle, we found a traingle that's better + bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.); + // p is inside of the current best triangle, we found a triangle that's better + bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist) parents; parents.push_back(f.m_n[0]); @@ -1607,11 +1615,6 @@ void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb) optimal_bary = bary; optimal_dist = dist; min_bary_weight = new_min_bary_weight; - // stop searching if the projected p is inside the triangle at hand - if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0.) - { - break; - } } } psb->m_renderNodesInterpolationWeights[i] = optimal_bary; diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h index 961c94dc6..bb0f0dd25 100644 --- a/src/LinearMath/btMatrixX.h +++ b/src/LinearMath/btMatrixX.h @@ -346,10 +346,9 @@ struct btMatrixX T dotProd = 0; { { - int r = rows(); int c = cols(); - for (int k = 0; k < cols(); k++) + for (int k = 0; k < c; k++) { T w = (*this)(i, k); if (other(k, j) != 0.f)