diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index a37e610ed..c898fbb65 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -1174,6 +1174,17 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, } deformable.m_repulsionStiffness = urdfLexicalCast(repulsion_xml->Attribute("value")); } + + XMLElement* grav_xml = config->FirstChildElement("gravity_factor"); + if (grav_xml) + { + if (!grav_xml->Attribute("value")) + { + logger->reportError("gravity_factor element must have value attribute"); + return false; + } + deformable.m_gravFactor = urdfLexicalCast(grav_xml->Attribute("value")); + } XMLElement* spring_xml = config->FirstChildElement("spring"); if (spring_xml) diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 7cd87c49a..b11578113 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -228,6 +228,7 @@ struct UrdfDeformable double m_collisionMargin; double m_friction; double m_repulsionStiffness; + double m_gravFactor; SpringCoeffcients m_springCoefficients; LameCoefficients m_corotatedCoefficients; @@ -237,7 +238,7 @@ struct UrdfDeformable std::string m_simFileName; btHashMap m_userData; - UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_visualFileName(""), m_simFileName("") + UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_repulsionStiffness(0.5), m_gravFactor(1.), m_visualFileName(""), m_simFileName("") { } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d764f26e6..7e6ba5097 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -413,6 +413,15 @@ B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandl return 0; } +B3_SHARED_API int b3LoadSoftBodySetGravityFactor(b3SharedMemoryCommandHandle commandHandle, double gravFactor) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_gravFactor = gravFactor; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_GRAVITY_FACTOR; + return 0; +} + B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 13e6e3ab1..fe6352f2d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1655,7 +1655,9 @@ struct PhysicsServerCommandProcessorInternalData #ifndef SKIP_DEFORMABLE_BODY - + btSoftBody* m_pickedSoftBody; + btDeformableMousePickingForce* m_mouseForce; + btScalar m_maxPickingForce; btDeformableBodySolver* m_deformablebodySolver; btAlignedObjectArray m_lf; #endif @@ -1685,6 +1687,7 @@ struct PhysicsServerCommandProcessorInternalData //data for picking objects class btRigidBody* m_pickedBody; + int m_savedActivationState; class btTypedConstraint* m_pickedConstraint; class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point; @@ -1727,6 +1730,9 @@ struct PhysicsServerCommandProcessorInternalData m_solver(0), m_collisionConfiguration(0), #ifndef SKIP_DEFORMABLE_BODY + m_pickedSoftBody(0), + m_mouseForce(0), + m_maxPickingForce(0.3), m_deformablebodySolver(0), #endif m_dynamicsWorld(0), @@ -8428,7 +8434,10 @@ void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDe { deformable.m_repulsionStiffness = loadSoftBodyArgs.m_repulsionStiffness; } - + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_GRAVITY_FACTOR) + { + deformable.m_gravFactor = loadSoftBodyArgs.m_gravFactor; + } #endif } @@ -8641,14 +8650,17 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo // 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 + // turn on face contact for multibodies psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; + /// turn on face contact for rigid body + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; // 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); psb->setSpringStiffness(deformable.m_repulsionStiffness); + psb->setGravityFactor(deformable.m_gravFactor); psb->initializeFaceTree(); } #endif //SKIP_DEFORMABLE_BODY @@ -10949,7 +10961,7 @@ bool PhysicsServerCommandProcessor::processApplyExternalForceCommand(const struc int link = clientCmd.m_externalForceArguments.m_linkIds[i]; btVector3 forceWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpForce : tmpForce; - btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpPosition : tmpPosition - mb->getBaseWorldTransform().getOrigin(); + btVector3 relPosWorld = isLinkFrame ? mb->getLink(link).m_cachedWorldTransform.getBasis() * tmpPosition : tmpPosition - mb->getLink(link).m_cachedWorldTransform.getOrigin(); mb->addLinkForce(link, forceWorld); mb->addLinkTorque(link, relPosWorld.cross(forceWorld)); //b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]); @@ -13775,10 +13787,15 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) } } + + struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback { + int m_faceId; + MyResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld) - : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld) + : btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld), + m_faceId(-1) { } @@ -13786,8 +13803,37 @@ struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback { return true; } + + virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace) + { + //caller already does the filter on the m_closestHitFraction + btAssert(rayResult.m_hitFraction <= m_closestHitFraction); + + m_closestHitFraction = rayResult.m_hitFraction; + m_collisionObject = rayResult.m_collisionObject; + if (rayResult.m_localShapeInfo) + { + m_faceId = rayResult.m_localShapeInfo->m_triangleIndex; + } + else + { + m_faceId = -1; + } + if (normalInWorldSpace) + { + m_hitNormalWorld = rayResult.m_hitNormalLocal; + } + else + { + ///need to transform normal into worldspace + m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal; + } + m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction); + return rayResult.m_hitFraction; + } }; + bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { if (m_data->m_dynamicsWorld == 0) @@ -13843,6 +13889,31 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons world->addMultiBodyConstraint(p2p); m_data->m_pickingMultiBodyPoint2Point = p2p; } + else + { +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + //deformable/soft body? + btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject); + if (psb) + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + int face_id = rayCallback.m_faceId; + if (face_id >= 0 && face_id < psb->m_faces.size()) + { + m_data->m_pickedSoftBody = psb; + psb->setActivationState(DISABLE_DEACTIVATION); + const btSoftBody::Face& f = psb->m_faces[face_id]; + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0, f, pickPos, m_data->m_maxPickingForce); + m_data->m_mouseForce = mouse_force; + + deformWorld->addForce(psb, mouse_force); + } + } + } +#endif + } } // pickObject(pickPos, rayCallback.m_collisionObject); @@ -13886,6 +13957,21 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); } +#ifndef SKIP_DEFORMABLE_BODY + if (m_data->m_pickedSoftBody) + { + if (m_data->m_pickedSoftBody && m_data->m_mouseForce) + { + btVector3 newPivot; + btVector3 dir = rayToWorld - rayFromWorld; + dir.normalize(); + dir *= m_data->m_oldPickingDist; + newPivot = rayFromWorld + dir; + m_data->m_mouseForce->setMousePos(newPivot); + } + } +#endif + return false; } @@ -13907,6 +13993,20 @@ void PhysicsServerCommandProcessor::removePickingConstraint() delete m_data->m_pickingMultiBodyPoint2Point; m_data->m_pickingMultiBodyPoint2Point = 0; } + + +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + //deformable/soft body? + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld && m_data->m_mouseForce) + { + deformWorld->removeForce(m_data->m_pickedSoftBody, m_data->m_mouseForce); + delete m_data->m_mouseForce; + m_data->m_mouseForce = 0; + m_data->m_pickedSoftBody = 0; + } +#endif + } void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 66a9d4494..4df72e31a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -515,6 +515,7 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_SIM_MESH = 1<<15, LOAD_SOFT_BODY_SET_REPULSION_STIFFNESS = 1<<16, LOAD_SOFT_BODY_SET_DAMPING_SPRING_MODE = 1<<17, + LOAD_SOFT_BODY_SET_GRAVITY_FACTOR = 1<<18, }; enum EnumSimParamInternalSimFlags @@ -549,6 +550,7 @@ struct LoadSoftBodyArgs int m_useFaceContact; char m_simFileName[MAX_FILENAME_LENGTH]; double m_repulsionStiffness; + double m_gravFactor; }; struct b3LoadSoftBodyResultArgs diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index 13ee3eacb..50a6584b5 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -68,7 +68,7 @@ public: btSoftBody::Node& n = psb->m_nodes[j]; size_t id = n.index; btScalar mass = (n.m_im == 0) ? 0 : 1. / n.m_im; - btVector3 scaled_force = scale * m_gravity * mass; + btVector3 scaled_force = scale * m_gravity * mass * m_softBodies[i]->m_gravityFactor; force[id] += scaled_force; } } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 49d1c9fd5..59b5365ca 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -228,6 +228,7 @@ void btSoftBody::initDefaults() m_softSoftCollision = false; m_maxSpeedSquared = 0; m_repulsionStiffness = 0.5; + m_gravityFactor = 1; m_fdbvnt = 0; } @@ -3445,6 +3446,11 @@ void btSoftBody::setSpringStiffness(btScalar k) m_repulsionStiffness = k; } +void btSoftBody::setGravityFactor(btScalar gravFactor) +{ + m_gravityFactor = gravFactor; +} + void btSoftBody::initializeDmInverse() { btScalar unit_simplex_measure = 1./6.; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 1bd8334bc..224867305 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -821,6 +821,7 @@ public: btScalar m_maxSpeedSquared; btAlignedObjectArray m_quads; // quadrature points for collision detection btScalar m_repulsionStiffness; + btScalar m_gravityFactor; btAlignedObjectArray m_X; // initial positions btAlignedObjectArray m_renderNodesInterpolationWeights; @@ -1169,6 +1170,7 @@ public: void applyClusters(bool drift); void dampClusters(); void setSpringStiffness(btScalar k); + void setGravityFactor(btScalar gravFactor); void initializeDmInverse(); void updateDeformation(); void advanceDeformation();