Merge pull request bulletphysics#2826 from xhan0619/grav-factor

Deformable gravity factor

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -1174,6 +1174,17 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
 		}
 		deformable.m_repulsionStiffness = urdfLexicalCast<double>(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<double>(grav_xml->Attribute("value"));
+	}
 
 	XMLElement* spring_xml = config->FirstChildElement("spring");
 	if (spring_xml)

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<btHashString, std::string> 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("")
 	{
 	}
 };

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;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -8428,7 +8428,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 +8644,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

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

src/BulletSoftBody/btDeformableGravityForce.h

@@ -68,7 +68,7 @@ class btDeformableGravityForce : public btDeformableLagrangianForce
                 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;
             }
         }

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.;

src/BulletSoftBody/btSoftBody.h

@@ -821,6 +821,7 @@ class btSoftBody : public btCollisionObject
 	btScalar m_maxSpeedSquared;
 	btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
 	btScalar m_repulsionStiffness;
+	btScalar m_gravityFactor;
     btAlignedObjectArray<btVector3> m_X;   // initial positions
 
 	btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
@@ -1169,6 +1170,7 @@ class btSoftBody : public btCollisionObject
 	void applyClusters(bool drift);
 	void dampClusters();
     void setSpringStiffness(btScalar k);
+	void setGravityFactor(btScalar gravFactor);
     void initializeDmInverse();
     void updateDeformation();
     void advanceDeformation();