Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

modify loadSoftBody to enable separate render mesh from simulation mesh

Browse Source
This commit is contained in:
Xuchen Han
2019-09-16 17:40:17 -07:00
committed by Xuchen Han
parent 1bc75cc833
commit f813cb1c88
1 changed files with 176 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

329
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7989,6 +7989,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
} }
{ {
btSoftBody* psb = NULL;
btScalar spring_elastic_stiffness, spring_damping_stiffness;
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface()); CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
char relativeFileName[1024]; char relativeFileName[1024];
char pathPrefix[1024]; char pathPrefix[1024];
@@ -7997,39 +8000,57 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
{ {
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
} }
// add _sim.vtk postfix
char relativeSimFileName[1024];
char sim_file_ending[9] = "_sim.vtk";
strncpy(relativeSimFileName, relativeFileName, strlen(relativeFileName));
strncpy(relativeSimFileName + strlen(relativeFileName)-4, sim_file_ending, sizeof(sim_file_ending));
const std::string& error_message_prefix = ""; const std::string& error_message_prefix = "";
std::string out_found_filename; std::string out_found_filename;
int out_type=0; std::string out_found_sim_filename;
int out_type, out_sim_type;
bool render_mesh_is_sim_mesh = true;
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
btSoftBody* psb = NULL; if (out_type == UrdfGeometry::FILE_OBJ)
if (foundFile) {
{ bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeSimFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
btScalar spring_elastic_stiffness, spring_damping_stiffness; render_mesh_is_sim_mesh = !foundFile;
if (out_type == UrdfGeometry::FILE_OBJ) }
{
std::vector<tinyobj::shape_t> shapes; if (render_mesh_is_sim_mesh)
tinyobj::attrib_t attribute; {
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); out_sim_type = out_type;
if (!shapes.empty()) out_found_sim_filename = out_found_filename;
{ }
const tinyobj::shape_t& shape = shapes[0];
btAlignedObjectArray<btScalar> vertices; if (out_sim_type == UrdfGeometry::FILE_OBJ)
btAlignedObjectArray<int> indices; {
for (int i = 0; i < attribute.vertices.size(); i++) std::vector<tinyobj::shape_t> shapes;
{ tinyobj::attrib_t attribute;
vertices.push_back(attribute.vertices[i]); std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
} if (!shapes.empty())
for (int i = 0; i < shape.mesh.indices.size(); i++) {
{ const tinyobj::shape_t& shape = shapes[0];
indices.push_back(shape.mesh.indices[i].vertex_index); btAlignedObjectArray<btScalar> vertices;
} btAlignedObjectArray<int> indices;
int numTris = shape.mesh.indices.size() / 3; for (int i = 0; i < attribute.vertices.size(); i++)
if (numTris > 0) {
{ vertices.push_back(attribute.vertices[i]);
psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); }
} 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)
{
psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
}
}
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{ {
@@ -8038,151 +8059,153 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false)); m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false));
} }
#endif #endif
} }
else if (out_type == UrdfGeometry::FILE_VTK) else if (out_sim_type == UrdfGeometry::FILE_VTK)
{ {
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_filename.c_str()); psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_sim_filename.c_str());
btScalar corotated_mu, corotated_lambda; btScalar corotated_mu, corotated_lambda;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
{ {
corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda)); m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda));
} }
btScalar neohookean_mu, neohookean_lambda; btScalar neohookean_mu, neohookean_lambda;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
{ {
neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda)); m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda));
} }
btScalar spring_elastic_stiffness, spring_damping_stiffness; btScalar spring_elastic_stiffness, spring_damping_stiffness;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{ {
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true)); m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true));
} }
#endif #endif
} }
} }
if (psb != NULL) if (psb != NULL)
{ {
#ifndef SKIP_DEFORMABLE_BODY #ifndef SKIP_DEFORMABLE_BODY
btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); if (!render_mesh_is_sim_mesh)
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE) {
{ // load render mesh
m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity)); btSoftBodyHelpers::readRenderMeshFromObj(out_found_filename.c_str(), psb);
} btSoftBodyHelpers::interpolateBarycentricWeights(psb);
btScalar collision_hardness = 1; }
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_COLLISION_HARDNESS) btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
{ if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE)
collision_hardness = loadSoftBodyArgs.m_collisionHardness; {
} m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity));
psb->m_cfg.kKHR = collision_hardness; }
psb->m_cfg.kCHR = collision_hardness; btScalar collision_hardness = 1;
psb->m_cfg.kKHR = collision_hardness;
psb->m_cfg.kCHR = collision_hardness;
btScalar friction_coeff = 0; btScalar friction_coeff = 0;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
{ {
friction_coeff = loadSoftBodyArgs.m_frictionCoeff; friction_coeff = loadSoftBodyArgs.m_frictionCoeff;
} }
psb->m_cfg.kDF = friction_coeff; psb->m_cfg.kDF = friction_coeff;
bool use_bending_spring = true;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
{
use_bending_spring = loadSoftBodyArgs.m_useBendingSprings;
}
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
if (use_bending_spring)
{
psb->generateBendingConstraints(2,pm);
}
bool use_bending_spring = true; // turn on the collision flag for deformable
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) // collision
{ psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; psb->setCollisionFlags(0);
} psb->setTotalMass(mass);
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
if (use_bending_spring)
{
psb->generateBendingConstraints(2, pm);
}
// turn on the collision flag for deformable
// collision
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
psb->setCollisionFlags(0);
psb->setTotalMass(mass);
#else #else
btSoftBody::Material* pm = psb->appendMaterial(); btSoftBody::Material* pm = psb->appendMaterial();
pm->m_kLST = 0.5; pm->m_kLST = 0.5;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw; pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2, pm); psb->generateBendingConstraints(2, pm);
psb->m_cfg.piterations = 20; psb->m_cfg.piterations = 20;
psb->m_cfg.kDF = 0.5; psb->m_cfg.kDF = 0.5;
//turn on softbody vs softbody collision //turn on softbody vs softbody collision
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
psb->randomizeConstraints(); psb->randomizeConstraints();
psb->setTotalMass(mass, true); psb->setTotalMass(mass, true);
#endif #endif
psb->scale(btVector3(scale, scale, scale)); psb->scale(btVector3(scale, scale, scale));
psb->rotate(initialOrn); psb->rotate(initialOrn);
psb->translate(initialPos); psb->translate(initialPos);
psb->getCollisionShape()->setMargin(collisionMargin); psb->getCollisionShape()->setMargin(collisionMargin);
psb->getCollisionShape()->setUserPointer(psb); psb->getCollisionShape()->setUserPointer(psb);
m_data->m_dynamicsWorld->addSoftBody(psb); m_data->m_dynamicsWorld->addSoftBody(psb);
m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
bodyHandle->m_softBody = psb; bodyHandle->m_softBody = psb;
b3VisualShapeData visualShape; b3VisualShapeData visualShape;
visualShape.m_objectUniqueId = bodyUniqueId; visualShape.m_objectUniqueId = bodyUniqueId;
visualShape.m_linkIndex = -1; visualShape.m_linkIndex = -1;
visualShape.m_visualGeometryType = URDF_GEOM_MESH; visualShape.m_visualGeometryType = URDF_GEOM_MESH;
//dimensions just contains the scale //dimensions just contains the scale
visualShape.m_dimensions[0] = scale; visualShape.m_dimensions[0] = scale;
visualShape.m_dimensions[1] = scale; visualShape.m_dimensions[1] = scale;
visualShape.m_dimensions[2] = scale; visualShape.m_dimensions[2] = scale;
//filename //filename
strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN); strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0; visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
//position and orientation //position and orientation
visualShape.m_localVisualFrame[0] = initialPos[0]; visualShape.m_localVisualFrame[0] = initialPos[0];
visualShape.m_localVisualFrame[1] = initialPos[1]; visualShape.m_localVisualFrame[1] = initialPos[1];
visualShape.m_localVisualFrame[2] = initialPos[2]; visualShape.m_localVisualFrame[2] = initialPos[2];
visualShape.m_localVisualFrame[3] = initialOrn[0]; visualShape.m_localVisualFrame[3] = initialOrn[0];
visualShape.m_localVisualFrame[4] = initialOrn[1]; visualShape.m_localVisualFrame[4] = initialOrn[1];
visualShape.m_localVisualFrame[5] = initialOrn[2]; visualShape.m_localVisualFrame[5] = initialOrn[2];
visualShape.m_localVisualFrame[6] = initialOrn[3]; visualShape.m_localVisualFrame[6] = initialOrn[3];
//color and ids to be set by the renderer //color and ids to be set by the renderer
visualShape.m_rgbaColor[0] = 0; visualShape.m_rgbaColor[0] = 0;
visualShape.m_rgbaColor[1] = 0; visualShape.m_rgbaColor[1] = 0;
visualShape.m_rgbaColor[2] = 0; visualShape.m_rgbaColor[2] = 0;
visualShape.m_rgbaColor[3] = 1; visualShape.m_rgbaColor[3] = 1;
visualShape.m_tinyRendererTextureId = -1; visualShape.m_tinyRendererTextureId = -1;
visualShape.m_textureUniqueId = -1; visualShape.m_textureUniqueId =-1;
visualShape.m_openglTextureId = -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_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED; serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
#ifdef ENABLE_LINK_MAPPER #ifdef ENABLE_LINK_MAPPER
if (m_data->m_urdfLinkNameMapper.size()) if (m_data->m_urdfLinkNameMapper.size())
{ {
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize(); serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
} }
#endif #endif
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
b3Notification notification; b3Notification notification;
notification.m_notificationType = BODY_ADDED; notification.m_notificationType = BODY_ADDED;
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
m_data->m_pluginManager.addNotification(notification); m_data->m_pluginManager.addNotification(notification);
} }
} }
#endif #endif