Revert "Revert "Reduced Deformable Model""

.gitignore

@@ -36,3 +36,7 @@ CTestTestFile.cmake
 
 # vim temp files
 *.swp
+
+.vscode/
+.idea/
+cmake-build-debug/

data/reduced_beam/beam_mesh.vtk

@@ -0,0 +1,83 @@
+#vtk DataFile Version 2.0
+I don't think this matters
+ASCII
+DATASET UNSTRUCTURED_GRID
+POINTS 28 double
+-0.5000000000000000 -0.2500000000000000 -2.0000000000000000
+-0.5000000000000000 0.2500000000000000 -2.0000000000000000
+0.5000000000000000 -0.2500000000000000 -2.0000000000000000
+0.5000000000000000 0.2500000000000000 -2.0000000000000000
+-0.5000000000000000 -0.2500000000000000 2.0000000000000000
+-0.5000000000000000 0.2500000000000000 2.0000000000000000
+0.5000000000000000 -0.2500000000000000 2.0000000000000000
+0.5000000000000000 0.2500000000000000 2.0000000000000000
+-0.5000000000000000 -0.2500000000000000 1.0000000000000000
+-0.5000000000000000 -0.2500000000000000 0.0000000000000000
+-0.5000000000000000 -0.2500000000000000 -1.0000000000000000
+0.5000000000000000 -0.2500000000000000 -1.0000000000000000
+0.5000000000000000 -0.2500000000000000 0.0000000000000000
+0.5000000000000000 -0.2500000000000000 1.0000000000000000
+-0.5000000000000000 0.2500000000000000 1.0000000000000000
+-0.5000000000000000 0.2500000000000000 0.0000000000000000
+-0.5000000000000000 0.2500000000000000 -1.0000000000000000
+0.5000000000000000 0.2500000000000000 1.0000000000000000
+0.5000000000000000 0.2500000000000000 0.0000000000000000
+0.5000000000000000 0.2500000000000000 -1.0000000000000000
+0.0000000000000000 -0.2500000000000000 -1.5000000000000000
+0.0000000000000000 -0.2500000000000000 -0.5000000000000000
+0.0000000000000000 -0.2500000000000000 0.5000000000000000
+0.0000000000000000 -0.2500000000000000 1.5000000000000000
+0.0000000000000000 0.2500000000000000 1.5000000000000000
+0.0000000000000000 0.2500000000000000 0.5000000000000000
+0.0000000000000000 0.2500000000000000 -0.5000000000000000
+0.0000000000000000 0.2500000000000000 -1.5000000000000000
+CELLS 48 192
+4 22 12 17 13
+4 20 26 21 11
+4 23 8 5 24
+4 11 27 26 19
+4 27 11 26 20
+4 17 22 24 25
+4 7 23 13 6
+4 22 17 24 13
+4 9 25 21 15
+4 24 22 8 14
+4 24 8 22 23
+4 25 9 21 22
+4 23 7 13 24
+4 15 10 16 26
+4 14 25 9 15
+4 25 14 9 22
+4 24 23 22 13
+4 15 10 21 9
+4 15 21 10 26
+4 24 25 22 14
+4 2 11 27 20
+4 7 5 23 6
+4 11 21 12 26
+4 20 1 0 16
+4 18 25 12 17
+4 18 12 25 26
+4 5 7 23 24
+4 26 21 25 15
+4 20 16 27 1
+4 21 25 22 12
+4 20 16 10 26
+4 20 27 16 26
+4 20 10 16 0
+4 11 12 19 26
+4 14 22 8 9
+4 8 23 5 4
+4 24 14 8 5
+4 11 2 27 3
+4 2 27 1 20
+4 12 18 19 26
+4 27 2 1 3
+4 24 13 17 7
+4 20 2 0 1
+4 5 23 6 4
+4 20 10 21 26
+4 12 22 17 25
+4 3 27 11 19
+4 21 26 25 12
+CELL_TYPES 10

data/reduced_beam/beam_mesh_origin.vtk

@@ -0,0 +1,83 @@
+#vtk DataFile Version 2.0
+I don't think this matters
+ASCII
+DATASET UNSTRUCTURED_GRID
+POINTS 28 double
+0.0000000000000000 0.0000000000000000 0.0000000000000000
+0.0000000000000000 0.5000000000000000 0.0000000000000000
+1.0000000000000000 0.0000000000000000 0.0000000000000000
+1.0000000000000000 0.5000000000000000 0.0000000000000000
+0.0000000000000000 0.0000000000000000 4.0000000000000000
+0.0000000000000000 0.5000000000000000 4.0000000000000000
+1.0000000000000000 0.0000000000000000 4.0000000000000000
+1.0000000000000000 0.5000000000000000 4.0000000000000000
+0.0000000000000000 0.0000000000000000 3.0000000000000000
+0.0000000000000000 0.0000000000000000 2.0000000000000000
+0.0000000000000000 0.0000000000000000 1.0000000000000000
+1.0000000000000000 0.0000000000000000 1.0000000000000000
+1.0000000000000000 0.0000000000000000 2.0000000000000000
+1.0000000000000000 0.0000000000000000 3.0000000000000000
+0.0000000000000000 0.5000000000000000 3.0000000000000000
+0.0000000000000000 0.5000000000000000 2.0000000000000000
+0.0000000000000000 0.5000000000000000 1.0000000000000000
+1.0000000000000000 0.5000000000000000 3.0000000000000000
+1.0000000000000000 0.5000000000000000 2.0000000000000000
+1.0000000000000000 0.5000000000000000 1.0000000000000000
+0.5000000000000000 0.0000000000000000 0.5000000000000000
+0.5000000000000000 0.0000000000000000 1.5000000000000000
+0.5000000000000000 0.0000000000000000 2.5000000000000000
+0.5000000000000000 0.0000000000000000 3.5000000000000000
+0.5000000000000000 0.5000000000000000 3.5000000000000000
+0.5000000000000000 0.5000000000000000 2.5000000000000000
+0.5000000000000000 0.5000000000000000 1.5000000000000000
+0.5000000000000000 0.5000000000000000 0.5000000000000000
+CELLS 48 192
+4 22 12 17 13
+4 20 26 21 11
+4 23 8 5 24
+4 11 27 26 19
+4 27 11 26 20
+4 17 22 24 25
+4 7 23 13 6
+4 22 17 24 13
+4 9 25 21 15
+4 24 22 8 14
+4 24 8 22 23
+4 25 9 21 22
+4 23 7 13 24
+4 15 10 16 26
+4 14 25 9 15
+4 25 14 9 22
+4 24 23 22 13
+4 15 10 21 9
+4 15 21 10 26
+4 24 25 22 14
+4 2 11 27 20
+4 7 5 23 6
+4 11 21 12 26
+4 20 1 0 16
+4 18 25 12 17
+4 18 12 25 26
+4 5 7 23 24
+4 26 21 25 15
+4 20 16 27 1
+4 21 25 22 12
+4 20 16 10 26
+4 20 27 16 26
+4 20 10 16 0
+4 11 12 19 26
+4 14 22 8 9
+4 8 23 5 4
+4 24 14 8 5
+4 11 2 27 3
+4 2 27 1 20
+4 12 18 19 26
+4 27 2 1 3
+4 24 13 17 7
+4 20 2 0 1
+4 5 23 6 4
+4 20 10 21 26
+4 12 22 17 25
+4 3 27 11 19
+4 21 26 25 12
+CELL_TYPES 10

data/reduced_beam/reduced_beam.urdf

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot name="reduced_cube">
+  <reduced_deformable name="reduced_beam">
+    <num_modes value="20"/>
+    <mass value="1"/>
+    <stiffness_scale value="100"/>
+    <collision_margin value="0.01"/>
+    <erp value= "0.2"/>
+    <cfm value= "0.2"/>
+    <friction value= "0.5"/>
+    <damping_coefficient value="0.0"/>
+    <visual filename="beam_mesh_origin.vtk"/>
+  </reduced_deformable>
+</robot>

data/reduced_cube/deform_cube.urdf

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot name="cube">
+  <deformable name="cube">
+    <inertial>
+      <mass value="10" />
+      <inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
+    </inertial>
+    <collision_margin value="0.006"/>
+    <repulsion_stiffness value="800.0"/>
+    <friction value= "0.5"/>
+    <neohookean mu= "180" lambda= "600" damping= "0.01" />
+    <visual filename="cube_mesh.vtk"/>
+  </deformable>
+</robot>

data/reduced_cube/reduced_cube.urdf

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<robot name="reduced_cube">
+  <reduced_deformable name="reduced_cube">
+    <num_modes value="20"/>
+    <mass value="10"/>
+    <stiffness_scale value="100"/>
+    <collision_margin value="0.01"/>
+    <erp value= "0.2"/>
+    <cfm value= "0.2"/>
+    <friction value= "0.5"/>
+    <damping_coefficient value="0.0"/>
+    <visual filename="cube_mesh.vtk"/>
+  </reduced_deformable>
+</robot>

data/reduced_torus/reduced_torus.urdf

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<robot name="reduced_cube">
+  <reduced_deformable name="reduced_cube">
+    <num_modes value="20"/>
+    <mass value="10"/>
+    <stiffness_scale value="10"/>
+    <collision_margin value="0.01"/>
+    <erp value= "0.2"/>
+    <cfm value= "0.2"/>
+    <friction value= "0.5"/>
+    <damping_coefficient value="0.00001"/>
+    <visual filename="torus_textured.obj"/>
+    <collision filename="torus_mesh.vtk"/>
+  </reduced_deformable>
+</robot>

examples/ExampleBrowser/CMakeLists.txt

@@ -390,6 +390,24 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp
   ../RigidBody/KinematicRigidBodyExample.cpp
+	../ReducedDeformableDemo/ConservationTest.cpp
+	../ReducedDeformableDemo/ConservationTest.h
+	../ReducedDeformableDemo/Springboard.cpp
+	../ReducedDeformableDemo/Springboard.h
+	../ReducedDeformableDemo/ModeVisualizer.cpp
+	../ReducedDeformableDemo/ModeVisualizer.h
+	../ReducedDeformableDemo/FreeFall.cpp
+	../ReducedDeformableDemo/FreeFall.h
+	../ReducedDeformableDemo/FrictionSlope.cpp
+	../ReducedDeformableDemo/FrictionSlope.h
+	../ReducedDeformableDemo/ReducedCollide.cpp
+	../ReducedDeformableDemo/ReducedCollide.h
+	../ReducedDeformableDemo/ReducedGrasp.cpp
+	../ReducedDeformableDemo/ReducedGrasp.h
+	../ReducedDeformableDemo/ReducedBenchmark.cpp
+	../ReducedDeformableDemo/ReducedBenchmark.h
+	../ReducedDeformableDemo/ReducedMotorGrasp.cpp
+	../ReducedDeformableDemo/ReducedMotorGrasp.h
   ../Constraints/TestHingeTorque.cpp
   ../Constraints/TestHingeTorque.h
   ../Constraints/ConstraintDemo.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -73,6 +73,15 @@
 #include "../RoboticsLearning/R2D2GraspExample.h"
 #include "../RoboticsLearning/KukaGraspExample.h"
 #include "../RoboticsLearning/GripperGraspExample.h"
+#include "../ReducedDeformableDemo/ConservationTest.h"
+#include "../ReducedDeformableDemo/ModeVisualizer.h"
+#include "../ReducedDeformableDemo/Springboard.h"
+#include "../ReducedDeformableDemo/FreeFall.h"
+#include "../ReducedDeformableDemo/FrictionSlope.h"
+#include "../ReducedDeformableDemo/ReducedCollide.h"
+#include "../ReducedDeformableDemo/ReducedGrasp.h"
+#include "../ReducedDeformableDemo/ReducedMotorGrasp.h"
+#include "../ReducedDeformableDemo/ReducedBenchmark.h"
 #include "../InverseKinematics/InverseKinematicsExample.h"
 
 #ifdef B3_ENABLE_TINY_AUDIO
@@ -216,6 +225,18 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
         ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
         // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
+		
+		ExampleEntry(0, "Reduced Deformabe Body"),
+		ExampleEntry(1, "Mode Visualizer", "Visualizer the modes for reduced deformable objects", ReducedModeVisualizerCreateFunc),
+		ExampleEntry(1, "Reduced Conservation Test", "Momentum conservation test for the reduced deformable objects", ReducedConservationTestCreateFunc),
+		ExampleEntry(1, "Reduced Springboard", "Moving rigid object colliding with a fixed reduced deformable objects", ReducedSpringboardCreateFunc),
+		ExampleEntry(1, "Reduced Free Fall", "Free fall ground contact test for the reduced deformable model", ReducedFreeFallCreateFunc),
+		ExampleEntry(1, "Reduced Collision Test", "Collision between a reduced block and the a rigid block", ReducedCollideCreateFunc),
+		ExampleEntry(1, "Reduced Grasp", "Grasp a reduced deformable block", ReducedGraspCreateFunc),
+		ExampleEntry(1, "Reduced Motor Grasp", "Grasp a reduced deformable block with motor", ReducedMotorGraspCreateFunc),
+		ExampleEntry(1, "Reduced Friction Slope", "Grasp a reduced deformable block", FrictionSlopeCreateFunc),
+		ExampleEntry(1, "Reduced Benchmark", "Reduced deformable performance benchmark example", ReducedBenchmarkCreateFunc),
+		// ExampleEntry(1, "Simple Reduced Deformable Test", "Simple dynamics test for the reduced deformable objects", ReducedBasicTestCreateFunc),
         
 #ifdef INCLUDE_CLOTH_DEMOS
 		ExampleEntry(0, "Soft Body"),

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -1541,3 +1541,8 @@ const struct UrdfDeformable& BulletURDFImporter::getDeformableModel() const
 {
 	return m_data->m_urdfParser.getDeformable();
 }
+
+const struct UrdfReducedDeformable& BulletURDFImporter::getReducedDeformableModel() const
+{
+	return m_data->m_urdfParser.getReducedDeformable();
+}

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -93,6 +93,7 @@ public:
 	virtual void setEnableTinyRenderer(bool enable);
 	void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
 	const struct UrdfDeformable& getDeformableModel() const;
+	const struct UrdfReducedDeformable& getReducedDeformableModel() const;
 };
 
 #endif  //BULLET_URDF_IMPORTER_H

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -1282,6 +1282,171 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
 	return true;
 }
 
+bool UrdfParser::parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger)
+{
+	UrdfReducedDeformable& reduced_deformable = model.m_reducedDeformable;
+	const char* name = config->Attribute("name");
+	if (!name)
+	{
+		logger->reportError("Reduced deformable with no name");
+		return false;
+	}
+	reduced_deformable.m_name = name;
+
+	{
+		XMLElement* numModes_xml = config->FirstChildElement("num_modes");
+		if (numModes_xml)
+		{
+			if (!numModes_xml->Attribute("value"))
+			{
+				logger->reportError("numModes_xml element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_numModes = urdfLexicalCast<double>(numModes_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* mass_xml = config->FirstChildElement("mass");
+		if (mass_xml)
+		{
+			if (!mass_xml->Attribute("value"))
+			{
+				logger->reportError("mass_xml element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_mass = urdfLexicalCast<double>(mass_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* stiffnessScale_xml = config->FirstChildElement("stiffness_scale");
+		if (stiffnessScale_xml)
+		{
+			if (!stiffnessScale_xml->Attribute("value"))
+			{
+				logger->reportError("stiffnessScale_xml element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_stiffnessScale = urdfLexicalCast<double>(stiffnessScale_xml->Attribute("value"));
+		}
+	}
+
+	{
+		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;
+			}
+			reduced_deformable.m_collisionMargin = urdfLexicalCast<double>(collisionMargin_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* erp_xml = config->FirstChildElement("erp");
+		if (erp_xml)
+		{
+			if (!erp_xml->Attribute("value"))
+			{
+				logger->reportError("friction element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_erp = urdfLexicalCast<double>(erp_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* cfm_xml = config->FirstChildElement("cfm");
+		if (cfm_xml)
+		{
+			if (!cfm_xml->Attribute("value"))
+			{
+				logger->reportError("cfm element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_cfm = urdfLexicalCast<double>(cfm_xml->Attribute("value"));
+		}
+	}
+
+	{
+		XMLElement* damping_xml = config->FirstChildElement("damping_coefficient");
+		if (damping_xml)
+		{
+			if (!damping_xml->Attribute("value"))
+			{
+				logger->reportError("damping_coefficient element must have value attribute");
+				return false;
+			}
+			reduced_deformable.m_damping = urdfLexicalCast<double>(damping_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;
+			}
+			reduced_deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
+		}
+	}
+
+	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");
+	reduced_deformable.m_visualFileName = fn;
+
+	int out_type(0);
+	bool success = UrdfFindMeshFile(m_fileIO,
+									model.m_sourceFile, fn, sourceFileLocation(vis_xml),
+									&reduced_deformable.m_visualFileName, &out_type);
+
+	if (!success)
+	{
+		// warning already printed
+		return false;
+	}
+
+	// collision mesh is optional
+	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 = col_xml->Attribute("filename");
+		success = UrdfFindMeshFile(m_fileIO,
+								   model.m_sourceFile, fn, sourceFileLocation(col_xml),
+								   &reduced_deformable.m_simFileName, &out_type);
+
+		if (!success)
+		{
+			// warning already printed
+			return false;
+		}
+	}
+
+	ParseUserData(config, reduced_deformable.m_userData, logger);
+	return true;
+}
+
 bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
 {
 	joint.m_lowerLimit = 0.f;
@@ -2126,9 +2291,16 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
 	//	logger->printMessage(msg);
 
 
+  
+	XMLElement* reduced_deformable_xml = robot_xml->FirstChildElement("reduced_deformable");
+  if (reduced_deformable_xml) {
+    return parseReducedDeformable(m_urdf2Model, reduced_deformable_xml, logger);
+	}
+	
   XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable");
-  if(deformable_xml)
+  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"))
 	{

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -247,6 +247,37 @@ struct UrdfDeformable
 	}
 };
 
+struct UrdfReducedDeformable
+{
+	std::string m_name;
+	int m_numModes;
+
+	double m_mass;
+	double m_stiffnessScale;
+	double m_erp;
+	double m_cfm;
+	double m_friction;
+	double m_collisionMargin;
+	double m_damping;
+
+	std::string m_visualFileName;
+	std::string m_simFileName;
+	btHashMap<btHashString, std::string> m_userData;
+
+	UrdfReducedDeformable() 
+		:	m_numModes(1),
+			m_mass(1),
+			m_stiffnessScale(100),
+			m_erp(0.2),					// generally, 0.2 is a good value for erp and cfm
+			m_cfm(0.2),
+			m_friction(0),
+			m_collisionMargin(0.02),
+			m_damping(0),
+			m_visualFileName(""),
+			m_simFileName("")
+	{}
+};
+
 struct UrdfModel
 {
 	std::string m_name;
@@ -256,6 +287,7 @@ struct UrdfModel
 	btHashMap<btHashString, UrdfLink*> m_links;
 	btHashMap<btHashString, UrdfJoint*> m_joints;
 	UrdfDeformable m_deformable;
+	UrdfReducedDeformable m_reducedDeformable;
 	// Maps user data keys to user data values.
 	btHashMap<btHashString, std::string> m_userData;
 
@@ -333,7 +365,7 @@ protected:
 	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);
-
+	bool parseReducedDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
 
 public:
 	UrdfParser(struct CommonFileIOInterface* fileIO);
@@ -413,6 +445,11 @@ public:
 		return m_urdf2Model.m_deformable;
 	}
 
+	const UrdfReducedDeformable& getReducedDeformable() const
+	{
+		return m_urdf2Model.m_reducedDeformable;
+	}
+
 	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);

examples/MultiBody/MultiDofDemo.cpp

@@ -165,7 +165,7 @@ void MultiDofDemo::initPhysics()
 	btVector3 linkHalfExtents(0.05, 0.37, 0.1);
 	btVector3 baseHalfExtents(0.05, 0.37, 0.1);
 
-	btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
+	btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
 	//mbC->forceMultiDof();							//if !spherical, you can comment this line to check the 1DoF algorithm
 
 	mbC->setCanSleep(canSleep);

examples/ReducedDeformableDemo/ConservationTest.cpp

@@ -0,0 +1,316 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "ConservationTest.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+#include <random>
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0;
+static int start_mode = 6;
+static int num_modes = 20;
+
+class ConservationTest : public CommonDeformableBodyBase
+{
+    btScalar sim_time;
+    bool first_step;
+
+    // get deformed shape
+    void getDeformedShape(btReducedDeformableBody* rsb, const int mode_n, const btScalar scale = 1)
+    {
+      // for (int i = 0; i < rsb->m_nodes.size(); ++i)
+      //   for (int k = 0; k < 3; ++k)
+      //     rsb->m_nodes[i].m_x[k] += rsb->m_modes[mode_n][3 * i + k] * scale;
+      
+      // rsb->m_reducedDofs[mode_n] = scale;
+      // rsb->m_reducedDofsBuffer[mode_n] = scale;
+
+      srand(1);
+      for (int r = 0; r < rsb->m_nReduced; r++)
+      {
+        rsb->m_reducedDofs[r] = (btScalar(rand()) / btScalar(RAND_MAX) - 0.5);
+        rsb->m_reducedDofsBuffer[r] = rsb->m_reducedDofs[r];
+      }
+
+      rsb->mapToFullPosition(rsb->getRigidTransform());
+      // std::cout << "-----------\n";
+      // std::cout << rsb->m_nodes[0].m_x[0] << '\t' << rsb->m_nodes[0].m_x[1] << '\t' << rsb->m_nodes[0].m_x[2] << '\n';
+      // std::cout << "-----------\n";
+    }
+
+public:
+    ConservationTest(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+        sim_time = 0;
+        first_step = true;
+    }
+    virtual ~ConservationTest()
+    {
+    }
+    void initPhysics();
+
+    void exitPhysics();
+
+    // TODO: disable pick force, non-interactive for now.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        float dist = 10;
+        float pitch = 0;
+        float yaw = 90;
+        float targetPos[3] = {0, 3, 0};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
+
+    void checkMomentum(btReducedDeformableBody* rsb)
+    {
+      btVector3 x_com(0, 0, 0);
+      btVector3 total_linear(0, 0, 0);
+      btVector3 total_angular(0, 0, 0);
+      {
+        std::ofstream myfile("center_of_mass.txt", std::ios_base::app);
+        for (int i = 0; i < rsb->m_nFull; ++i)
+        {
+          x_com += rsb->m_nodalMass[i] * rsb->m_nodes[i].m_x;
+        }
+        x_com /= rsb->getTotalMass();
+        myfile << sim_time << "\t" << x_com[0] << "\t" << x_com[1] << "\t" << x_com[2] << "\n";
+        myfile.close();
+      }
+      {
+        std::ofstream myfile("linear_momentum.txt", std::ios_base::app);
+        for (int i = 0; i < rsb->m_nFull; ++i)
+        {
+          total_linear += rsb->m_nodalMass[i] * rsb->m_nodes[i].m_v;
+        }
+        myfile << sim_time << "\t" << total_linear[0] << "\t" << total_linear[1] << "\t" << total_linear[2] << "\n";
+        myfile.close();
+      }
+      {
+        std::ofstream myfile("angular_momentum.txt", std::ios_base::app);
+        // btVector3 ri(0, 0, 0);
+        // for (int i = 0; i < rsb->m_nFull; ++i)
+        // { 
+        //   ri = rsb->m_nodes[i].m_x - x_com;
+        //   total_angular += rsb->m_nodalMass[i] * ri.cross(rsb->m_nodes[i].m_v);
+        // }
+        total_angular = rsb->computeTotalAngularMomentum();
+        myfile << sim_time << "\t" << total_angular[0] << "\t" << total_angular[1] << "\t" << total_angular[2] << "\n";
+        myfile.close();
+      }
+      {
+        btVector3 angular_rigid(0, 0, 0);
+        std::ofstream myfile("angular_momentum_rigid.txt", std::ios_base::app);
+        btVector3 ri(0, 0, 0);
+        for (int i = 0; i < rsb->m_nFull; ++i)
+        { 
+          ri = rsb->m_nodes[i].m_x - x_com;
+          btMatrix3x3 ri_star = Cross(ri);
+          angular_rigid += rsb->m_nodalMass[i] * (ri_star.transpose() * ri_star * rsb->getAngularVelocity());
+        }
+        myfile << sim_time << "\t" << angular_rigid[0] << "\t" << angular_rigid[1] << "\t" << angular_rigid[2] << "\n";
+        myfile.close();
+      }
+
+      {
+        std::ofstream myfile("reduced_velocity.txt", std::ios_base::app);
+        myfile << sim_time << "\t" << rsb->m_reducedVelocity[0] << "\t" << rsb->m_reducedDofs[0] << "\n";
+        myfile.close();
+      }
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+      // add initial deformation
+      btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
+      if (first_step /* && !rsb->m_bUpdateRtCst*/) 
+      {
+        getDeformedShape(rsb, 0, 1);
+        first_step = false;
+      }
+      
+      float internalTimeStep = 1. / 240.f;
+      m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+
+      sim_time += internalTimeStep;
+      checkMomentum(rsb);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); 
+
+                btVector3 origin = rsb->getRigidTransform().getOrigin();
+                btVector3 line_x = rsb->getRigidTransform().getBasis() * 2 * btVector3(1, 0, 0) + origin;
+                btVector3 line_y = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 1, 0) + origin;
+                btVector3 line_z = rsb->getRigidTransform().getBasis() * 2 * btVector3(0, 0, 1) + origin;
+
+                deformableWorld->getDebugDrawer()->drawLine(origin, line_x, btVector3(1, 0, 0));
+                deformableWorld->getDebugDrawer()->drawLine(origin, line_y, btVector3(0, 1, 0));
+                deformableWorld->getDebugDrawer()->drawLine(origin, line_z, btVector3(0, 0, 1));
+
+                deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
+                deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1));
+                deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1));
+            }
+        }
+    }
+};
+
+void ConservationTest::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+    btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
+    btVector3 gravity = btVector3(0, 0, 0);
+    reducedSoftBodySolver->setGravity(gravity);
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+    m_dynamicsWorld->setGravity(gravity);
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric reduced deformable body
+    {   
+        std::string file_path("../../../data/reduced_beam/");
+        std::string vtk_file("beam_mesh_origin.vtk");
+        btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                            getDeformableDynamicsWorld()->getWorldInfo(),
+                                            file_path,
+                                            vtk_file,
+                                            num_modes,
+                                            false);
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.1);
+        
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(btVector3(0, 4, 0));
+        // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
+        rsb->transformTo(init_transform);
+
+        rsb->setStiffnessScale(100);
+        rsb->setDamping(damping_alpha, damping_beta);
+        
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+        
+        // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
+        // rsb->setRigidVelocity(btVector3(0, 1, 0));
+        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void ConservationTest::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+    //remove the rigidbodies from the dynamics world and delete them
+    int i;
+    for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+    {
+        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btRigidBody* body = btRigidBody::upcast(obj);
+        if (body && body->getMotionState())
+        {
+            delete body->getMotionState();
+        }
+        m_dynamicsWorld->removeCollisionObject(obj);
+        delete obj;
+    }
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+    //delete collision shapes
+    for (int j = 0; j < m_collisionShapes.size(); j++)
+    {
+        btCollisionShape* shape = m_collisionShapes[j];
+        delete shape;
+    }
+    m_collisionShapes.clear();
+
+    delete m_dynamicsWorld;
+
+    delete m_solver;
+
+    delete m_broadphase;
+
+    delete m_dispatcher;
+
+    delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* ReducedConservationTestCreateFunc(struct CommonExampleOptions& options)
+{
+    return new ConservationTest(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/ConservationTest.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_CONSERVATION_TEST_H
+#define _REDUCED_CONSERVATION_TEST_H
+
+class CommonExampleInterface* ReducedConservationTestCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_CONSERVATION_TEST_H

examples/ReducedDeformableDemo/FreeFall.cpp

@@ -0,0 +1,276 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "FreeFall.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
+// static btScalar E = 50;
+// static btScalar nu = 0.3;
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0001;
+static btScalar COLLIDING_VELOCITY = 0;
+static int num_modes = 40;
+
+class FreeFall : public CommonDeformableBodyBase
+{
+public:
+    FreeFall(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+    }
+    virtual ~FreeFall()
+    {
+    }
+    void initPhysics();
+
+    void exitPhysics();
+
+    // TODO: disable pick force, non-interactive for now.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        float dist = 8;
+        float pitch = -10;
+        float yaw = 90;
+        float targetPos[3] = {0, 2, 0};
+        // float dist = 10;
+        // float pitch = -30;
+        // float yaw = 125;
+        // float targetPos[3] = {0, 2, 0};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
+    
+    void Ctor_RbUpStack(const btVector3& origin)
+    {
+        float mass = 10;
+        btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
+        // btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
+        btTransform startTransform;
+        startTransform.setIdentity();
+        // startTransform.setOrigin(btVector3(0, 12, 0));
+        // btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
+        // rb0->setLinearVelocity(btVector3(0, 0, 0));
+
+        startTransform.setOrigin(origin);
+        // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
+        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+        rb1->setActivationState(DISABLE_DEACTIVATION);
+        rb1->setLinearVelocity(btVector3(0, 0, 0));
+    }
+
+    void createReducedDeformableObject(const btVector3& origin, const btQuaternion& rotation)
+    {   
+        std::string file_path("../../../data/reduced_cube/");
+        std::string vtk_file("cube_mesh.vtk");
+        btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                            getDeformableDynamicsWorld()->getWorldInfo(),
+                                            file_path,
+                                            vtk_file,
+                                            num_modes,
+                                            false);
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.01);
+        // rsb->scale(btVector3(1, 1, 0.5));
+
+        rsb->setTotalMass(10);
+
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(origin);
+        init_transform.setRotation(rotation);
+        rsb->transformTo(init_transform);
+
+        rsb->setStiffnessScale(25);
+        rsb->setDamping(damping_alpha, damping_beta);
+        // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+      float internalTimeStep = 1. / 60.f;
+      m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        // int flag = 0;
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                // btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag);
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); 
+
+                // for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
+                // {
+                //     deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
+                // }
+                // for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
+                // {
+                //     deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0));
+                // }
+
+                // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
+                // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 5, 0), 0.1, btVector3(1, 1, 1));
+                // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 10, 0), 0.1, btVector3(1, 1, 1));
+            }
+        }
+    }
+};
+
+void FreeFall::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+    btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+    btVector3 gravity = btVector3(0, -9.8, 0);
+    m_dynamicsWorld->setGravity(gravity);
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    // m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
+    
+    // 2 reduced deformable cubes
+    createReducedDeformableObject(btVector3(0, 4, -2), btQuaternion(0, 0, 0));
+    createReducedDeformableObject(btVector3(0, 4, 2), btQuaternion(0, 0, 0));
+   
+    // 2 rigid cubes
+    Ctor_RbUpStack(btVector3(0, 10, -2));
+    Ctor_RbUpStack(btVector3(0, 10, 2));
+    
+    // create a static rigid box as the ground
+    {
+        // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
+        btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
+        // groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
+        groundTransform.setOrigin(btVector3(0, -2, 0));
+        // groundTransform.setOrigin(btVector3(0, 0, 6));
+        // groundTransform.setOrigin(btVector3(0, -50, 0));
+        {
+            btScalar mass(0.);
+            createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
+        }
+    }
+
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+    m_dynamicsWorld->setGravity(gravity);
+}
+
+void FreeFall::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+    //remove the rigidbodies from the dynamics world and delete them
+    int i;
+    for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+    {
+        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btRigidBody* body = btRigidBody::upcast(obj);
+        if (body && body->getMotionState())
+        {
+            delete body->getMotionState();
+        }
+        m_dynamicsWorld->removeCollisionObject(obj);
+        delete obj;
+    }
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+    //delete collision shapes
+    for (int j = 0; j < m_collisionShapes.size(); j++)
+    {
+        btCollisionShape* shape = m_collisionShapes[j];
+        delete shape;
+    }
+    m_collisionShapes.clear();
+
+    delete m_dynamicsWorld;
+
+    delete m_solver;
+
+    delete m_broadphase;
+
+    delete m_dispatcher;
+
+    delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* ReducedFreeFallCreateFunc(struct CommonExampleOptions& options)
+{
+    return new FreeFall(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/FreeFall.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_FREE_FALL_H
+#define _REDUCED_FREE_FALL_H
+
+class CommonExampleInterface* ReducedFreeFallCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_FREE_FALL_H

examples/ReducedDeformableDemo/FrictionSlope.cpp

@@ -0,0 +1,288 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "FrictionSlope.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
+// static btScalar E = 50;
+// static btScalar nu = 0.3;
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.001;
+static btScalar COLLIDING_VELOCITY = 0;
+static int num_modes = 20;
+
+class FrictionSlope : public CommonDeformableBodyBase
+{
+public:
+    FrictionSlope(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {}
+    virtual ~FrictionSlope()
+    {
+    }
+    void initPhysics();
+
+    void exitPhysics();
+
+    // TODO: disable pick force, non-interactive for now.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        float dist = 20;
+        float pitch = -20;
+        float yaw = 90;
+        float targetPos[3] = {0, 0, 0.5};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
+    
+    void Ctor_RbUpStack()
+    {
+        float mass = 1;
+        btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
+        // btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
+        // btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2));
+        btTransform startTransform;
+        startTransform.setIdentity();
+
+        startTransform.setOrigin(btVector3(0,4,0));
+        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+        rb1->setLinearVelocity(btVector3(0, 0, 0));
+    }
+
+    void createGround()
+    {
+        btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        // groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
+        groundTransform.setOrigin(btVector3(0, 0, 0));
+        btScalar mass(1e6);
+        btRigidBody* ground = createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
+        // ground->setFriction(1);
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+      float internalTimeStep = 1. / 60.f;
+      m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                // btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), flag);
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); 
+
+                // for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
+                // {
+                //     deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
+                // }
+                // for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
+                // {
+                //     deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.2, btVector3(0, 1, 0));
+                // }
+            }
+        }
+    }
+};
+
+namespace FrictionSlopeHelper 
+{
+    void groundMotion(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
+    {
+        btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
+
+        btRigidBody* ground = rbs[0];
+        btAssert(ground->getMass() > 1e5);
+
+        btScalar start_time = 2;
+        btScalar end_time = 8;
+        btScalar start_angle = 0;
+        btScalar end_angle = SIMD_PI / 6;
+        btScalar current_angle = 0;
+        btScalar turn_speed = (end_angle - start_angle) / (end_time - start_time);
+
+        if (time >= start_time)
+        {
+            current_angle = (time - start_time) * turn_speed;
+            if (time > end_time)
+            {
+                current_angle = end_angle;
+                turn_speed = 0;
+            }
+        }
+        else
+        {
+            current_angle = start_angle;
+            turn_speed = 0;
+        }
+        
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
+        groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), current_angle));
+
+        ground->setCenterOfMassTransform(groundTransform);
+        ground->setLinearVelocity(btVector3(0, 0, 0));
+        ground->setAngularVelocity(btVector3(0, 0, 0));
+    }
+};
+
+void FrictionSlope::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+    btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
+    btVector3 gravity = btVector3(0, -10, 0);
+    reducedSoftBodySolver->setGravity(gravity);
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric reduced deformable body
+    {   
+        std::string file_path("../../../data/reduced_beam/");
+        std::string vtk_file("beam_mesh_origin.vtk");
+        btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                            getDeformableDynamicsWorld()->getWorldInfo(),
+                                            file_path,
+                                            vtk_file,
+                                            num_modes,
+                                            false);
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.01);
+
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(btVector3(0, 4, 0));
+        init_transform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 2.0));
+        rsb->transform(init_transform);
+        rsb->setStiffnessScale(50);
+        rsb->setDamping(damping_alpha, damping_beta);
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
+
+    createGround();
+    // add a few rigid bodies
+    // Ctor_RbUpStack();
+
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+    getDeformableDynamicsWorld()->setSolverCallback(FrictionSlopeHelper::groundMotion);
+    m_dynamicsWorld->setGravity(gravity);
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void FrictionSlope::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+    //remove the rigidbodies from the dynamics world and delete them
+    int i;
+    for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+    {
+        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btRigidBody* body = btRigidBody::upcast(obj);
+        if (body && body->getMotionState())
+        {
+            delete body->getMotionState();
+        }
+        m_dynamicsWorld->removeCollisionObject(obj);
+        delete obj;
+    }
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+    //delete collision shapes
+    for (int j = 0; j < m_collisionShapes.size(); j++)
+    {
+        btCollisionShape* shape = m_collisionShapes[j];
+        delete shape;
+    }
+    m_collisionShapes.clear();
+
+    delete m_dynamicsWorld;
+
+    delete m_solver;
+
+    delete m_broadphase;
+
+    delete m_dispatcher;
+
+    delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* FrictionSlopeCreateFunc(struct CommonExampleOptions& options)
+{
+    return new FrictionSlope(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/FrictionSlope.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _FRICTION_SLOPE_H
+#define _FRICTION_SLOPE_H
+
+class CommonExampleInterface* FrictionSlopeCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_FREE_FALL_H

examples/ReducedDeformableDemo/ModeVisualizer.cpp

@@ -0,0 +1,227 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "ModeVisualizer.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+
+static int num_modes = 20;
+static btScalar visualize_mode = 0;
+static btScalar frequency_scale = 1;
+
+class ModeVisualizer : public CommonDeformableBodyBase
+{
+    btScalar sim_time;
+
+    // get deformed shape
+    void getDeformedShape(btReducedDeformableBody* rsb, const int mode_n, const btScalar time_term = 1)
+    {
+      for (int i = 0; i < rsb->m_nodes.size(); ++i)
+        for (int k = 0; k < 3; ++k)
+          rsb->m_nodes[i].m_x[k] = rsb->m_x0[i][k] + rsb->m_modes[mode_n][3 * i + k] * time_term;
+    }
+
+    btVector3 computeMassWeightedColumnSum(btReducedDeformableBody* rsb, const int mode_n)
+    {
+        btVector3 sum(0, 0, 0);
+        for (int i = 0; i < rsb->m_nodes.size(); ++i)
+        {
+            for (int k = 0; k < 3; ++k)
+            {
+                sum[k] += rsb->m_nodalMass[i] * rsb->m_modes[mode_n][3 * i + k];
+            }
+        }
+        return sum;
+    }
+
+public:
+    ModeVisualizer(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+        sim_time = 0;
+    }
+    virtual ~ModeVisualizer()
+    {
+    }
+    void initPhysics();
+
+    void exitPhysics();
+
+    // disable pick force. non-interactive example.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        float dist = 10;
+        float pitch = 0;
+        float yaw = 90;
+        float targetPos[3] = {0, 3, 0};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+      btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(static_cast<btDeformableMultiBodyDynamicsWorld*>(m_dynamicsWorld)->getSoftBodyArray()[0]);
+
+      sim_time += deltaTime;
+      int n_mode = floor(visualize_mode);
+      btScalar scale = sin(sqrt(rsb->m_eigenvalues[n_mode]) * sim_time / frequency_scale);
+      getDeformedShape(rsb, n_mode, scale);
+    //   btVector3 mass_weighted_column_sum = computeMassWeightedColumnSum(rsb, visualize_mode);
+    //   std::cout << "mode=" << int(visualize_mode) << "\t" << mass_weighted_column_sum[0] << "\t"
+    //                                                       << mass_weighted_column_sum[1] << "\t"
+    //                                                       << mass_weighted_column_sum[2] << "\n";
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btSoftBody* rsb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void ModeVisualizer::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+    btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric soft body
+    {
+      std::string file_path("../../../data/reduced_cube/");
+      std::string vtk_file("cube_mesh.vtk");
+      btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                          getDeformableDynamicsWorld()->getWorldInfo(),
+                                          file_path,
+                                          vtk_file,
+                                          num_modes,
+                                          false);
+
+      getDeformableDynamicsWorld()->addSoftBody(rsb);
+      rsb->getCollisionShape()->setMargin(0.1);
+
+      btTransform init_transform;
+      init_transform.setIdentity();
+      init_transform.setOrigin(btVector3(0, 2, 0));
+    //   init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
+      rsb->transform(init_transform);
+      btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+    
+    {
+      SliderParams slider("Visualize Mode", &visualize_mode);
+      slider.m_minVal = 0;
+      slider.m_maxVal = num_modes - 1;
+      if (m_guiHelper->getParameterInterface())
+          m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    {
+      SliderParams slider("Frequency Reduction", &frequency_scale);
+      slider.m_minVal = 1;
+      slider.m_maxVal = 1e3;
+      if (m_guiHelper->getParameterInterface())
+          m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+}
+
+void ModeVisualizer::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+    //remove the rigidbodies from the dynamics world and delete them
+    int i;
+    for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+    {
+        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btRigidBody* body = btRigidBody::upcast(obj);
+        if (body && body->getMotionState())
+        {
+            delete body->getMotionState();
+        }
+        m_dynamicsWorld->removeCollisionObject(obj);
+        delete obj;
+    }
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+    //delete collision shapes
+    for (int j = 0; j < m_collisionShapes.size(); j++)
+    {
+        btCollisionShape* shape = m_collisionShapes[j];
+        delete shape;
+    }
+    m_collisionShapes.clear();
+
+    delete m_dynamicsWorld;
+
+    delete m_solver;
+
+    delete m_broadphase;
+
+    delete m_dispatcher;
+
+    delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* ReducedModeVisualizerCreateFunc(struct CommonExampleOptions& options)
+{
+    return new ModeVisualizer(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/ModeVisualizer.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_MODE_VISUALIZER_H
+#define _REDUCED_MODE_VISUALIZER_H
+
+class CommonExampleInterface* ReducedModeVisualizerCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_MODE_VISUALIZER_H

examples/ReducedDeformableDemo/ReducedBenchmark.cpp

@@ -0,0 +1,349 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "ReducedBenchmark.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0001;
+static btScalar COLLIDING_VELOCITY = 0;
+static int num_modes = 20;
+static bool run_reduced = true;
+
+class ReducedBenchmark : public CommonDeformableBodyBase
+{
+    btVector3 m_gravity;
+public:
+    ReducedBenchmark(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+    }
+    virtual ~ReducedBenchmark()
+    {
+    }
+    void initPhysics();
+
+    void exitPhysics();
+
+    // TODO: disable pick force, non-interactive for now.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        // float dist = 6;
+        // float pitch = -10;
+        // float yaw = 90;
+        // float targetPos[3] = {0, 2, 0};
+        float dist = 10;
+        float pitch = -30;
+        float yaw = 125;
+        float targetPos[3] = {0, 2, 0};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
+    
+    void Ctor_RbUpStack(const btVector3& origin)
+    {
+        float mass = 10;
+        btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
+        // btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
+        btTransform startTransform;
+        startTransform.setIdentity();
+        // startTransform.setOrigin(btVector3(0, 12, 0));
+        // btRigidBody* rb0 = createRigidBody(mass, startTransform, shape);
+        // rb0->setLinearVelocity(btVector3(0, 0, 0));
+
+        startTransform.setOrigin(origin);
+        // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 4.0));
+        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+        rb1->setActivationState(DISABLE_DEACTIVATION);
+        // rb1->setLinearVelocity(btVector3(0, 0, 4));
+    }
+
+    void createDeform(const btVector3& origin, const btQuaternion& rotation)
+    {
+
+        if (run_reduced)
+        {   
+            std::string file_path("../../../data/reduced_torus/");
+            std::string vtk_file("torus_mesh.vtk");
+            btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                                getDeformableDynamicsWorld()->getWorldInfo(),
+                                                file_path,
+                                                vtk_file,
+                                                num_modes,
+                                                false);
+
+            getDeformableDynamicsWorld()->addSoftBody(rsb);
+            rsb->getCollisionShape()->setMargin(0.01);
+            // rsb->scale(btVector3(1, 1, 0.5));
+
+            rsb->setTotalMass(10);
+
+            btTransform init_transform;
+            init_transform.setIdentity();
+            init_transform.setOrigin(origin);
+            init_transform.setRotation(rotation);
+            rsb->transformTo(init_transform);
+
+            rsb->setStiffnessScale(5);
+            rsb->setDamping(damping_alpha, damping_beta);
+            // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+            rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+            rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+            rsb->m_cfg.kDF = 0;
+            rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+            rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+            rsb->m_sleepingThreshold = 0;
+            btSoftBodyHelpers::generateBoundaryFaces(rsb);
+
+            std::cout << "Running reduced deformable\n";
+        }
+        else    // create full deformable cube
+        {
+            std::string filepath("../../../data/reduced_torus/");
+            std::string filename = filepath + "torus_mesh.vtk";
+            btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
+            
+            btTransform init_transform;
+            init_transform.setIdentity();
+            init_transform.setOrigin(origin);
+            init_transform.setRotation(rotation);
+            psb->transform(init_transform);
+            psb->getCollisionShape()->setMargin(0.015);
+            psb->setTotalMass(10);
+            psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+            psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+            psb->m_cfg.kDF = .5;
+            psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+            psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+            getDeformableDynamicsWorld()->addSoftBody(psb);
+            btSoftBodyHelpers::generateBoundaryFaces(psb);
+            
+            btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(m_gravity);
+            getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+            m_forces.push_back(gravity_force);
+            
+            btScalar E = 10000;
+            btScalar nu = 0.3;
+            btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
+            btScalar mu = E / (2 * (1 + nu));
+            btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.01);
+            // neohookean->setPoissonRatio(0.3);
+            // neohookean->setYoungsModulus(25);
+            neohookean->setDamping(0.01);
+            psb->m_cfg.drag = 0.001;
+            getDeformableDynamicsWorld()->addForce(psb, neohookean);
+            m_forces.push_back(neohookean);
+
+            std::cout << "Running full deformable\n";
+        }
+
+        // btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
+
+        // getDeformableDynamicsWorld()->addSoftBody(rsb);
+        // rsb->getCollisionShape()->setMargin(0.01);
+        // // rsb->scale(btVector3(1, 1, 0.5));
+
+        // rsb->setTotalMass(10);
+
+        // btTransform init_transform;
+        // init_transform.setIdentity();
+        // init_transform.setOrigin(origin);
+        // init_transform.setRotation(rotation);
+        // rsb->transformTo(init_transform);
+
+        // rsb->setStiffnessScale(5);
+        // rsb->setDamping(damping_alpha, damping_beta);
+        // // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+        // rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        // rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        // rsb->m_cfg.kDF = 0;
+        // rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        // rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        // rsb->m_sleepingThreshold = 0;
+        // btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+      float internalTimeStep = 1. / 240.f;
+      m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); 
+            }
+        }
+    }
+};
+
+void ReducedBenchmark::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+
+    if (run_reduced)
+    {
+        btReducedDeformableBodySolver* solver = new btReducedDeformableBodySolver();
+
+        btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+        sol->setDeformableSolver(solver);
+        m_solver = sol;
+        m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
+    }
+    else
+    {
+        btDeformableBodySolver* solver = new btDeformableBodySolver();
+
+        btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+        sol->setDeformableSolver(solver);
+        m_solver = sol;
+        m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
+    }
+
+    // m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
+    btVector3 gravity = btVector3(0, -10, 0);
+    m_gravity = gravity;
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    
+    // 3x3 torus
+    createDeform(btVector3(4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(0, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(0, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(0, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(-4, 4, -4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(-4, 4, 0), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    createDeform(btVector3(-4, 4, 4), btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+    
+    // create a static rigid box as the ground
+    {
+        // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
+        btBoxShape* groundShape = createBoxShape(btVector3(btScalar(20), btScalar(2), btScalar(20)));
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        // groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI / 6.0));
+        // groundTransform.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_PI / 6.0));
+        groundTransform.setOrigin(btVector3(0, 0, 0));
+        // groundTransform.setOrigin(btVector3(0, 0, 6));
+        // groundTransform.setOrigin(btVector3(0, -50, 0));
+        {
+            btScalar mass(0.);
+            createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
+        }
+    }
+
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 0.5;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+    m_dynamicsWorld->setGravity(gravity);
+}
+
+void ReducedBenchmark::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+    //remove the rigidbodies from the dynamics world and delete them
+    int i;
+    for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+    {
+        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btRigidBody* body = btRigidBody::upcast(obj);
+        if (body && body->getMotionState())
+        {
+            delete body->getMotionState();
+        }
+        m_dynamicsWorld->removeCollisionObject(obj);
+        delete obj;
+    }
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+    //delete collision shapes
+    for (int j = 0; j < m_collisionShapes.size(); j++)
+    {
+        btCollisionShape* shape = m_collisionShapes[j];
+        delete shape;
+    }
+    m_collisionShapes.clear();
+
+    delete m_dynamicsWorld;
+
+    delete m_solver;
+
+    delete m_broadphase;
+
+    delete m_dispatcher;
+
+    delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* ReducedBenchmarkCreateFunc(struct CommonExampleOptions& options)
+{
+    return new ReducedBenchmark(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/ReducedBenchmark.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_BENCHMARK_H
+#define _REDUCED_BENCHMARK_H
+
+class CommonExampleInterface* ReducedBenchmarkCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_BENCHMARK_H

examples/ReducedDeformableDemo/ReducedCollide.cpp

@@ -0,0 +1,522 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "ReducedCollide.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
+// static btScalar E = 50;
+// static btScalar nu = 0.3;
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0;
+static btScalar COLLIDING_VELOCITY = 4;
+static int num_modes = 20;
+
+class ReducedCollide : public CommonDeformableBodyBase
+{
+public:
+    ReducedCollide(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+    }
+    virtual ~ReducedCollide()
+    {
+    }
+    void initPhysics();
+
+    void exitPhysics();
+
+	btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
+	void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+
+    // TODO: disable pick force, non-interactive for now.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        // float dist = 20;
+        // float pitch = -10;
+        float dist = 10;
+        float pitch = -5;
+        float yaw = 90;
+        float targetPos[3] = {0, 0, 0};
+
+        // float dist = 5;
+		// float pitch = -35;
+		// float yaw = 50;
+		// float targetPos[3] = {-3, 2.8, -2.5};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
+    
+    void Ctor_RbUpStack()
+    {
+        float mass = 10;
+
+        btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.5, 0.5));
+        // btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
+        btVector3 localInertia(0, 0, 0);
+		if (mass != 0.f)
+			shape->calculateLocalInertia(mass, localInertia);
+
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0,-2,0));
+        // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+
+		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
+		btRigidBody* body = new btRigidBody(rbInfo);
+
+		m_dynamicsWorld->addRigidBody(body, 1, 1+2);
+
+        body->setActivationState(DISABLE_DEACTIVATION);
+        body->setLinearVelocity(btVector3(0, COLLIDING_VELOCITY, 0));
+        // body->setFriction(1);
+    }
+
+    void rigidBar()
+    {
+        float mass = 10;
+
+        btCollisionShape* shape = new btBoxShape(btVector3(0.5, 0.25, 2));
+        btVector3 localInertia(0, 0, 0);
+		if (mass != 0.f)
+			shape->calculateLocalInertia(mass, localInertia);
+
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0,10,0));
+        // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+
+		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
+		btRigidBody* body = new btRigidBody(rbInfo);
+
+		m_dynamicsWorld->addRigidBody(body, 1, 1+2);
+
+        body->setActivationState(DISABLE_DEACTIVATION);
+        body->setLinearVelocity(btVector3(0, 0, 0));
+        // body->setFriction(0);
+    }
+
+    void createGround()
+    {
+        // float mass = 55;
+        float mass = 0;
+
+        btCollisionShape* shape = new btBoxShape(btVector3(10, 2, 10));
+        btVector3 localInertia(0, 0, 0);
+		if (mass != 0.f)
+			shape->calculateLocalInertia(mass, localInertia);
+
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0,-2,0));
+        // startTransform.setRotation(btQuaternion(btVector3(1, 0, 1), SIMD_PI / 3.0));
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+
+		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, shape, localInertia);
+		btRigidBody* body = new btRigidBody(rbInfo);
+
+		m_dynamicsWorld->addRigidBody(body, 1, 1+2);
+
+        body->setActivationState(DISABLE_DEACTIVATION);
+        body->setLinearVelocity(btVector3(0, 0, 0));
+        // body->setFriction(1);
+    }
+    
+    void stepSimulation(float deltaTime)
+    {
+      float internalTimeStep = 1. / 60.f;
+      m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); 
+            }
+
+            for (int p = 0; p < rsb->m_contactNodesList.size(); ++p)
+            {
+                int index = rsb->m_contactNodesList[p];
+                deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[index].m_x, 0.2, btVector3(0, 1, 0));
+            }
+        }
+    }
+};
+
+void ReducedCollide::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+    btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
+    btVector3 gravity = btVector3(0, 0, 0);
+    reducedSoftBodySolver->setGravity(gravity);
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+    m_dynamicsWorld->setGravity(gravity);
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = 1e-3;
+    m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric reduced deformable body
+    {   
+        std::string file_path("../../../data/reduced_cube/");
+        std::string vtk_file("cube_mesh.vtk");
+        btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                            getDeformableDynamicsWorld()->getWorldInfo(),
+                                            file_path,
+                                            vtk_file,
+                                            num_modes,
+                                            false);
+                                            
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.1);
+        // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+        rsb->setStiffnessScale(100);
+        rsb->setDamping(damping_alpha, damping_beta);
+
+        rsb->setTotalMass(15);
+
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(btVector3(0, 4, 0));
+        // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
+        rsb->transformTo(init_transform);
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+        
+        rsb->setRigidVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
+        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
+        b3Printf("total mass: %e", rsb->getTotalMass());
+    }
+    // rigidBar();
+
+    // add a few rigid bodies
+    Ctor_RbUpStack();
+    
+    // create ground
+    // createGround();
+
+    // create multibody
+    // {
+    //     bool damping = false;
+    //     bool gyro = true;
+    //     int numLinks = 0;
+    //     bool spherical = true;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+    //     bool multibodyOnly = true;
+    //     bool canSleep = false;
+    //     bool selfCollide = true;
+    //     bool multibodyConstraint = false;
+    //     btVector3 linkHalfExtents(0.05, 0.37, 0.1);
+    //     btVector3 baseHalfExtents(1, 1, 1);
+    //     // btVector3 baseHalfExtents(2.5, 0.5, 2.5);
+    //     // btVector3 baseHalfExtents(0.05, 0.37, 0.1);
+
+    //     bool g_floatingBase = true;
+    //     // btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0, 4, 0), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase);
+    //     btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(m_dynamicsWorld, numLinks, btVector3(0.f, 4.f, 0.f), baseHalfExtents, linkHalfExtents, spherical, g_floatingBase);
+    //     //mbC->forceMultiDof();							//if !spherical, you can comment this line to check the 1DoF algorithm
+
+    //     mbC->setCanSleep(canSleep);
+    //     mbC->setHasSelfCollision(selfCollide);
+    //     mbC->setUseGyroTerm(gyro);
+    //     //
+    //     if (!damping)
+    //     {
+    //         mbC->setLinearDamping(0.f);
+    //         mbC->setAngularDamping(0.f);
+    //     }
+    //     else
+    //     {
+    //         mbC->setLinearDamping(0.1f);
+    //         mbC->setAngularDamping(0.9f);
+    //     }
+    //     //
+    //     //////////////////////////////////////////////
+    //     // if (numLinks > 0)
+    //     // {
+    //     //     btScalar q0 = 45.f * SIMD_PI / 180.f;
+    //     //     if (!spherical)
+    //     //     {
+    //     //         mbC->setJointPosMultiDof(0, &q0);
+    //     //     }
+    //     //     else
+    //     //     {
+    //     //         btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+    //     //         quat0.normalize();
+    //     //         mbC->setJointPosMultiDof(0, quat0);
+    //     //     }
+    //     // }
+    //     ///
+    //     addColliders_testMultiDof(mbC, m_dynamicsWorld, baseHalfExtents, linkHalfExtents);
+    // }
+
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+    
+    // {
+    //     SliderParams slider("Young's Modulus", &E);
+    //     slider.m_minVal = 0;
+    //     slider.m_maxVal = 2000;
+    //     if (m_guiHelper->getParameterInterface())
+    //         m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    // }
+    // {
+    //     SliderParams slider("Poisson Ratio", &nu);
+    //     slider.m_minVal = 0.05;
+    //     slider.m_maxVal = 0.49;
+    //     if (m_guiHelper->getParameterInterface())
+    //         m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    // }
+    // {
+    //     SliderParams slider("Mass Damping", &damping_alpha);
+    //     slider.m_minVal = 0;
+    //     slider.m_maxVal = 1;
+    //     if (m_guiHelper->getParameterInterface())
+    //         m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    // }
+    // {
+    //     SliderParams slider("Stiffness Damping", &damping_beta);
+    //     slider.m_minVal = 0;
+    //     slider.m_maxVal = 0.1;
+    //     if (m_guiHelper->getParameterInterface())
+    //         m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    // }
+}
+
+void ReducedCollide::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+    //remove the rigidbodies from the dynamics world and delete them
+    int i;
+    for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+    {
+        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btRigidBody* body = btRigidBody::upcast(obj);
+        if (body && body->getMotionState())
+        {
+            delete body->getMotionState();
+        }
+        m_dynamicsWorld->removeCollisionObject(obj);
+        delete obj;
+    }
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+    //delete collision shapes
+    for (int j = 0; j < m_collisionShapes.size(); j++)
+    {
+        btCollisionShape* shape = m_collisionShapes[j];
+        delete shape;
+    }
+    m_collisionShapes.clear();
+
+    delete m_dynamicsWorld;
+
+    delete m_solver;
+
+    delete m_broadphase;
+
+    delete m_dispatcher;
+
+    delete m_collisionConfiguration;
+}
+
+btMultiBody* ReducedCollide::createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
+{
+	//init the base
+	btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+	float baseMass = 10;
+
+	if (baseMass)
+	{
+		btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+		pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+		delete pTempBox;
+	}
+
+	bool canSleep = false;
+
+	btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
+
+	btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+	// btQuaternion baseOriQuat(btVector3(0, 0, 1), -SIMD_PI / 6.0);
+	pMultiBody->setBasePos(basePosition);
+	pMultiBody->setWorldToBaseRot(baseOriQuat);
+	btVector3 vel(0, 0, 0);
+
+	//init the links
+	btVector3 hingeJointAxis(1, 0, 0);
+	float linkMass = 1.f;
+	btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+
+	btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
+	pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
+	delete pTempBox;
+
+	//y-axis assumed up
+	btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);                      //par body's COM to cur body's COM offset
+	btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);                         //cur body's COM to cur body's PIV offset
+	btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;  //par body's COM to cur body's PIV offset
+
+	//////
+	btScalar q0 = 0.f * SIMD_PI / 180.f;
+	btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
+	quat0.normalize();
+	/////
+
+	for (int i = 0; i < numLinks; ++i)
+	{
+		if (!spherical)
+			pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
+		else
+			//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
+			pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
+	}
+
+	pMultiBody->finalizeMultiDof();
+    pMultiBody->setBaseVel(vel);
+
+	///
+	pWorld->addMultiBody(pMultiBody);
+	///
+	return pMultiBody;
+}
+
+void ReducedCollide::addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
+{
+	btAlignedObjectArray<btQuaternion> world_to_local;
+	world_to_local.resize(pMultiBody->getNumLinks() + 1);
+
+	btAlignedObjectArray<btVector3> local_origin;
+	local_origin.resize(pMultiBody->getNumLinks() + 1);
+	world_to_local[0] = pMultiBody->getWorldToBaseRot();
+	local_origin[0] = pMultiBody->getBasePos();
+
+	{
+		//	float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+		btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+
+		if (1)
+		{
+			btCollisionShape* box = new btBoxShape(baseHalfExtents);
+			btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
+			col->setCollisionShape(box);
+
+			btTransform tr;
+			tr.setIdentity();
+			tr.setOrigin(local_origin[0]);
+			tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+			col->setWorldTransform(tr);
+
+			pWorld->addCollisionObject(col, 2, 1 + 2);
+
+			col->setFriction(1);
+			pMultiBody->setBaseCollider(col);
+		}
+	}
+
+	for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+	{
+		const int parent = pMultiBody->getParent(i);
+		world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
+		local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
+	}
+
+	for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+	{
+		btVector3 posr = local_origin[i + 1];
+		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+
+		btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
+
+		btCollisionShape* box = new btBoxShape(linkHalfExtents);
+		btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+
+		col->setCollisionShape(box);
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(posr);
+		tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+		col->setWorldTransform(tr);
+		col->setFriction(1);
+		pWorld->addCollisionObject(col, 2, 1 + 2);
+
+		pMultiBody->getLink(i).m_collider = col;
+	}
+}
+
+
+
+class CommonExampleInterface* ReducedCollideCreateFunc(struct CommonExampleOptions& options)
+{
+    return new ReducedCollide(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/ReducedCollide.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_COLLIDE_H
+#define _REDUCED_COLLIDE_H
+
+class CommonExampleInterface* ReducedCollideCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_COLLIDE_H

examples/ReducedDeformableDemo/ReducedGrasp.cpp

@@ -0,0 +1,457 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "ReducedGrasp.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The BasicTest shows the contact between volumetric deformable objects and rigid objects.
+// static btScalar E = 50;
+// static btScalar nu = 0.3;
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0001;
+static int num_modes = 20;
+
+class ReducedGrasp : public CommonDeformableBodyBase
+{
+public:
+    ReducedGrasp(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+    }
+    virtual ~ReducedGrasp()
+    {
+    }
+    void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+        float dist = 10;
+        float pitch = -10;
+        float yaw = 90;
+        
+        // float dist = 25;
+        // float pitch = -30;
+        // float yaw = 100;
+        float targetPos[3] = {0, 0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    void stepSimulation(float deltaTime)
+    {
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+        // float internalTimeStep = 1. / 60.f;
+        // m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+    }
+    
+    void createGrip()
+    {
+        int count = 2;
+        float mass = 1e6;
+        btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 0.25));
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(0,1,0));
+            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+            createRigidBody(mass, startTransform, shape);
+        }
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(0,1,-4));
+            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+            createRigidBody(mass, startTransform, shape);
+        }
+        
+    }
+
+    void Ctor_RbUpStack()
+    {
+        float mass = 8;
+        btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
+        btTransform startTransform;
+        startTransform.setIdentity();
+
+        startTransform.setOrigin(btVector3(0,9.5,0));
+        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+        rb1->setLinearVelocity(btVector3(0, 0, 0));
+        rb1->setFriction(0.7);
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            // btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            // {
+            //     btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+            //     btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            // }
+
+            // for (int p = 0; p < rsb->m_nodeRigidContacts.size(); ++p)
+            // {
+            //     deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_contactNodesList[p]].m_x, 0.1, btVector3(0, 1, 0));
+            // }
+
+            btSoftBody* psb = static_cast<btSoftBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+
+    static void GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
+};
+
+void ReducedGrasp::GripperDynamics(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
+{
+    btAlignedObjectArray<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
+    if (rbs.size()<2)
+        return;
+    btRigidBody* rb0 = rbs[0];
+    // btScalar pressTime = 0.9;
+    // btScalar pressTime = 0.96;
+    btScalar pressTime = 1.26;
+    btScalar liftTime = 2.5;
+    btScalar shiftTime = 6;
+    btScalar holdTime = 7;
+    btScalar dropTime = 10;
+    // btScalar holdTime = 500;
+    // btScalar dropTime = 1000;
+    btTransform rbTransform;
+    rbTransform.setIdentity();
+    btVector3 translation;
+    btVector3 velocity;
+    
+    btVector3 initialTranslationLeft = btVector3(0,1,0);            // inner face has z=2
+    btVector3 initialTranslationRight = btVector3(0,1,-4);          // inner face has z=-2
+    btVector3 pinchVelocityLeft = btVector3(0,0,-1);
+    btVector3 pinchVelocityRight = btVector3(0,0,1);
+    btVector3 liftVelocity = btVector3(0,4,0);
+    btVector3 shiftVelocity = btVector3(0,0,2);
+    btVector3 holdVelocity = btVector3(0,0,0);
+    btVector3 openVelocityLeft = btVector3(0,0,4);
+    btVector3 openVelocityRight = btVector3(0,0,-4);
+    
+    if (time < pressTime)
+    {
+        velocity = pinchVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * time;
+    }
+    // else
+    // {
+    //     velocity = btVector3(0, 0, 0);
+    //     translation = initialTranslationLeft + pinchVelocityLeft * pressTime;
+    // }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (time - pressTime);
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityLeft;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime);
+    }
+    else
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationLeft + pinchVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime);
+    }
+    rbTransform.setOrigin(translation);
+    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+    rb0->setCenterOfMassTransform(rbTransform);
+    rb0->setAngularVelocity(btVector3(0,0,0));
+    rb0->setLinearVelocity(velocity);
+    
+    btRigidBody* rb1 = rbs[1];
+    if (time < pressTime)
+    {
+        velocity = pinchVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * time;
+    }
+    // else
+    // {
+    //     velocity = btVector3(0, 0, 0);
+    //     translation = initialTranslationRight + pinchVelocityRight * pressTime;
+    // }
+    else if (time < liftTime)
+    {
+        velocity = liftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (time - pressTime);
+    }
+    else if (time < shiftTime)
+    {
+        velocity = shiftVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime);
+    }
+    else if (time < holdTime)
+    {
+        velocity = btVector3(0,0,0);
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime);
+    }
+    else if (time < dropTime)
+    {
+        velocity = openVelocityRight;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime);
+    }
+    else
+    {
+        velocity = holdVelocity;
+        translation = initialTranslationRight + pinchVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime);
+    }
+    rbTransform.setOrigin(translation);
+    rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+    rb1->setCenterOfMassTransform(rbTransform);
+    rb1->setAngularVelocity(btVector3(0,0,0));
+    rb1->setLinearVelocity(velocity);
+    
+    rb0->setFriction(20);
+    rb1->setFriction(20);
+}
+
+void ReducedGrasp::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+    // btDeformableBodySolver* solver = new btDeformableBodySolver();
+    btReducedDeformableBodySolver* solver = new btReducedDeformableBodySolver();
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(solver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, solver);
+    btVector3 gravity = btVector3(0, -10, 0);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
+    getDeformableDynamicsWorld()->setSolverCallback(GripperDynamics);
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric reduced deformable body
+    {   
+        std::string file_path("../../../data/reduced_cube/");
+        std::string vtk_file("cube_mesh.vtk");
+        btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                            getDeformableDynamicsWorld()->getWorldInfo(),
+                                            file_path,
+                                            vtk_file,
+                                            num_modes,
+                                            false);
+                                            
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.015);
+        
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(btVector3(0, 1, -2));
+        // init_transform.setRotation(btQuaternion(0, SIMD_PI / 2.0, SIMD_PI / 2.0));
+        // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
+        rsb->transform(init_transform);
+
+        rsb->setStiffnessScale(100);
+        rsb->setDamping(damping_alpha, damping_beta);
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
+
+    // create full deformable cube
+    {
+        // std::string filepath("../../../examples/SoftDemo/cube/");
+        // std::string filename = filepath + "mesh.vtk";
+        // btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), filename.c_str());
+        
+        // // psb->scale(btVector3(2, 2, 2));
+        // psb->translate(btVector3(0, 1, -2));
+        // psb->getCollisionShape()->setMargin(0.05);
+        // psb->setTotalMass(28.6);
+        // psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        // psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        // psb->m_cfg.kDF = .5;
+        // psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        // psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        // getDeformableDynamicsWorld()->addSoftBody(psb);
+        // btSoftBodyHelpers::generateBoundaryFaces(psb);
+        
+        // btDeformableGravityForce* gravity_force =  new btDeformableGravityForce(gravity);
+        // getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+        // m_forces.push_back(gravity_force);
+        
+        // btScalar E = 10000;
+        // btScalar nu = 0.3;
+        // btScalar lambda = E * nu / ((1 + nu) * (1 - 2 * nu));
+        // btScalar mu = E / (2 * (1 + nu));
+        // btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(lambda, mu, 0.02);
+        // // neohookean->setPoissonRatio(0.3);
+        // // neohookean->setYoungsModulus(25);
+        // neohookean->setDamping(0.01);
+        // psb->m_cfg.drag = 0.001;
+        // getDeformableDynamicsWorld()->addForce(psb, neohookean);
+        // m_forces.push_back(neohookean);
+    }
+
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+    
+    // grippers
+    createGrip();
+
+    // rigid block
+    // Ctor_RbUpStack();
+
+    // {
+    //     float mass = 10;
+    //     btCollisionShape* shape = new btBoxShape(btVector3(0.25, 2, 0.5));
+    //     btTransform startTransform;
+    //     startTransform.setIdentity();
+    //     startTransform.setOrigin(btVector3(0,4,0));
+    //     btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+    //     rb1->setLinearVelocity(btVector3(0, 0, 0));
+    // }
+
+    //create a ground
+    {
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
+
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, -25, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+        btScalar mass(0.);
+
+        //rigidbody is dynamic if and only if mass is non zero, otherwise static
+        bool isDynamic = (mass != 0.f);
+
+        btVector3 localInertia(0, 0, 0);
+        if (isDynamic)
+            groundShape->calculateLocalInertia(mass, localInertia);
+
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void ReducedGrasp::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* ReducedGraspCreateFunc(struct CommonExampleOptions& options)
+{
+    return new ReducedGrasp(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/ReducedGrasp.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_GRASP_H
+#define _REDUCED_GRASP_H
+
+class CommonExampleInterface* ReducedGraspCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_GRASP_H

examples/ReducedDeformableDemo/ReducedMotorGrasp.cpp

@@ -0,0 +1,558 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "ReducedMotorGrasp.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../Utils/b3BulletDefaultFileIO.h"
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../CommonInterfaces/CommonFileIOInterface.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+///The ReducedMotorGrasp shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
+static btScalar sGripperVerticalVelocity = 0.f;
+static btScalar sGripperClosingTargetVelocity = 0.f;
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0001;
+static int num_modes = 20;
+static float friction = 1.;
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+struct TetraBunny
+{
+#include "../SoftDemo/bunny.inl"
+};
+
+static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
+{
+    bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute
+                         || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic);
+    return canHaveMotor;
+}
+
+class ReducedMotorGrasp : public CommonDeformableBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+public:
+	ReducedMotorGrasp(struct GUIHelperInterface* helper)
+    :CommonDeformableBodyBase(helper)
+	{
+	}
+	virtual ~ReducedMotorGrasp()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+    void Ctor_RbUpStack()
+    {
+        float mass = 8;
+        btCollisionShape* shape = new btBoxShape(btVector3(2, 0.25, 0.5));
+        btTransform startTransform;
+        startTransform.setIdentity();
+
+        startTransform.setOrigin(btVector3(0,0.25,0));
+        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+        rb1->setLinearVelocity(btVector3(0, 0, 0));
+        rb1->setFriction(0.7);
+    }
+
+	void resetCamera()
+	{
+        // float dist = 0.3;
+        // float pitch = -45;
+        // float yaw = 100;
+        // float targetPos[3] = {0, -0.1, 0};
+        float dist = 0.4;
+        float pitch = -25;
+        float yaw = 90;
+        float targetPos[3] = {0, 0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+    
+    btMultiBody* createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld,const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating);
+    
+    void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+    
+    btMultiBody* createFeatherstoneMultiBody_testMultiDof(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating);
+    
+    void stepSimulation(float deltaTime)
+    {
+        double fingerTargetVelocities[2] = {sGripperVerticalVelocity, sGripperClosingTargetVelocity};
+        int num_multiBody = getDeformableDynamicsWorld()->getNumMultibodies();
+        for (int i = 0; i < num_multiBody; ++i)
+        {
+            btMultiBody* mb = getDeformableDynamicsWorld()->btMultiBodyDynamicsWorld::getMultiBody(i);
+            mb->setBaseVel(btVector3(0,sGripperVerticalVelocity, 0));
+            int dofIndex = 6;  //skip the 3 linear + 3 angular degree of freedom entries of the base
+            for (int link = 0; link < mb->getNumLinks(); link++)
+            {
+                if (supportsJointMotor(mb, link))
+                {
+                    btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
+                    if (motor)
+                    {
+                        if (dofIndex == 6)
+                        {
+                            motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
+                            motor->setMaxAppliedImpulse(10);
+                        }
+                        if (dofIndex == 7)
+                        {
+                            motor->setVelocityTarget(fingerTargetVelocities[1], 1);
+                            motor->setMaxAppliedImpulse(10);
+                        }
+                        motor->setMaxAppliedImpulse(25);
+                    }
+                }
+                dofIndex += mb->getLink(link).m_dofCount;
+            }
+        }
+        
+        //use a smaller internal timestep, there are stability issues
+        float internalTimeStep = 1. / 240.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+        // float internalTimeStep = 1. / 60.f;
+        // m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+    }
+    
+    void createGrip()
+    {
+        int count = 2;
+        float mass = 2;
+        btCollisionShape* shape[] = {
+            new btBoxShape(btVector3(3, 3, 0.5)),
+        };
+        static const int nshapes = sizeof(shape) / sizeof(shape[0]);
+        for (int i = 0; i < count; ++i)
+        {
+            btTransform startTransform;
+            startTransform.setIdentity();
+            startTransform.setOrigin(btVector3(10, 0, 0));
+            startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.));
+            createRigidBody(mass, startTransform, shape[i % nshapes]);
+        }
+    }
+    
+    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
+    {
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
+    {
+        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+				btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), fDrawFlags::Faces);// deformableWorld->getDrawFlags());
+            }
+            // for (int p = 0; p < rsb->m_contactNodesList.size(); ++p)
+            // {
+            //     int index = rsb->m_contactNodesList[p];
+            //     deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[index].m_x, 0.2, btVector3(0, 1, 0));
+            // }
+        }
+    }
+    
+    virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+    {
+        return false;
+    }
+    virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+    {
+        return false;
+    }
+    virtual void removePickingConstraint(){}
+};
+
+
+void ReducedMotorGrasp::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+    
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+    btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
+    // btVector3 gravity = btVector3(0, 0, 0);
+    btVector3 gravity = btVector3(0, -9.81, 0);
+    reducedSoftBodySolver->setGravity(gravity);
+
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+	m_dynamicsWorld->setGravity(gravity);
+    getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+    // getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1;
+    // getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0;
+    // getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_maxPickingForce = 0.001;
+    // build a gripper
+    {
+        bool damping = true;
+        bool gyro = false;
+        bool canSleep = false;
+        bool selfCollide = true;
+        int numLinks = 2;
+        // btVector3 linkHalfExtents(0.02, 0.018, .003);
+        // btVector3 baseHalfExtents(0.02, 0.002, .002);
+        btVector3 linkHalfExtents(0.03, 0.04, 0.006);
+        btVector3 baseHalfExtents(0.02, 0.015, 0.015);
+        btVector3 basePosition(0, 0.3, 0);
+        // btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), btVector3(0.f, 0.05f,0.f), baseHalfExtents, linkHalfExtents, false);
+        btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), basePosition, baseHalfExtents, linkHalfExtents, false);
+        
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+
+        for (int i = 0; i < numLinks; i++)
+        {
+            int mbLinkIndex = i;
+            double maxMotorImpulse = 1;
+
+            if (supportsJointMotor(mbC, mbLinkIndex))
+            {
+                int dof = 0;
+                btScalar desiredVelocity = 0.f;
+                btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mbC, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
+                motor->setPositionTarget(0, 0);
+                motor->setVelocityTarget(0, 1);
+                mbC->getLink(mbLinkIndex).m_userPtr = motor;
+                getDeformableDynamicsWorld()->addMultiBodyConstraint(motor);
+                motor->finalizeMultiDof();
+            }
+        }
+        
+        if (!damping)
+        {
+            mbC->setLinearDamping(0.0f);
+            mbC->setAngularDamping(0.0f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
+        }
+        btScalar q0 = 0.f * SIMD_PI / 180.f;
+        if (numLinks > 0)
+            mbC->setJointPosMultiDof(0, &q0);
+        addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
+    }
+    
+    //create a ground
+    {
+        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
+        groundShape->setMargin(0.001);
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, -5.1, 0));
+        groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+        //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+        btScalar mass(0.);
+
+        //rigidbody is dynamic if and only if mass is non zero, otherwise static
+        bool isDynamic = (mass != 0.f);
+
+        btVector3 localInertia(0, 0, 0);
+        if (isDynamic)
+            groundShape->calculateLocalInertia(mass, localInertia);
+
+        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+        btRigidBody* body = new btRigidBody(rbInfo);
+        body->setFriction(0.5);
+
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body,1,1+2);
+    }
+
+	// create volumetric reduced deformable body
+    {   
+        std::string file_path("../../../data/reduced_cube/");
+        std::string vtk_file("cube_mesh.vtk");
+        btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                            getDeformableDynamicsWorld()->getWorldInfo(),
+                                            file_path,
+                                            vtk_file,
+                                            num_modes,
+                                            false);
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.001);
+
+        rsb->setStiffnessScale(100);
+        rsb->setDamping(damping_alpha, damping_beta);
+
+        rsb->scale(btVector3(0.075, 0.075, 0.075));
+        rsb->setTotalMass(1);
+
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(btVector3(0, 0.1, 0));
+        // init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, 0, SIMD_PI / 2.0));
+        rsb->transform(init_transform);
+        
+        // rsb->setRigidVelocity(btVector3(0, 1, 0));
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+    }
+
+    // Ctor_RbUpStack();
+    
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_friction = 1;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-6;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 200;
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+    
+    {
+        SliderParams slider("Moving velocity", &sGripperVerticalVelocity);
+        // slider.m_minVal = -.02;
+        // slider.m_maxVal = .02;
+        slider.m_minVal = -.2;
+        slider.m_maxVal = .2;
+        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    
+    {
+        SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity);
+        slider.m_minVal = -1;
+        slider.m_maxVal = 1;
+        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+    }
+    
+}
+
+void ReducedMotorGrasp::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+btMultiBody* ReducedMotorGrasp::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool floating)
+{
+    //init the base
+    btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+    float baseMass = 55;
+    float linkMass = 55;
+    int numLinks = 2;
+    
+    if (baseMass)
+    {
+        btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+        pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+        delete pTempBox;
+    }
+    
+    bool canSleep = false;
+    btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
+    
+    btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+    pMultiBody->setBasePos(basePosition);
+    pMultiBody->setWorldToBaseRot(baseOriQuat);
+    
+    //init the links
+    btVector3 hingeJointAxis(1, 0, 0);
+    
+    btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+    
+    btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
+    pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
+    delete pTempBox;
+    
+    //y-axis assumed up
+    btAlignedObjectArray<btVector3> parentComToCurrentCom;
+    parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.f, -baseHalfExtents[2] * 2.f));
+    parentComToCurrentCom.push_back(btVector3(0, -linkHalfExtents[1] * 2.f, +baseHalfExtents[2] * 2.f));//par body's COM to cur body's COM offset
+
+    
+    btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1]*2.f, 0);                         //cur body's COM to cur body's PIV offset
+    
+    btAlignedObjectArray<btVector3> parentComToCurrentPivot;
+    parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[0] - currentPivotToCurrentCom));
+    parentComToCurrentPivot.push_back(btVector3(parentComToCurrentCom[1] - currentPivotToCurrentCom));//par body's COM to cur body's PIV offset
+    
+    //////
+    btScalar q0 = 0.f * SIMD_PI / 180.f;
+    btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
+    quat0.normalize();
+    /////
+    
+    for (int i = 0; i < numLinks; ++i)
+    {
+        pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot[i], currentPivotToCurrentCom, true);
+    }
+    pMultiBody->finalizeMultiDof();
+    ///
+    pWorld->addMultiBody(pMultiBody);
+    ///
+    return pMultiBody;
+}
+
+void ReducedMotorGrasp::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
+{
+    btAlignedObjectArray<btQuaternion> world_to_local;
+    world_to_local.resize(pMultiBody->getNumLinks() + 1);
+    
+    btAlignedObjectArray<btVector3> local_origin;
+    local_origin.resize(pMultiBody->getNumLinks() + 1);
+    world_to_local[0] = pMultiBody->getWorldToBaseRot();
+    local_origin[0] = pMultiBody->getBasePos();
+    
+    {
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+        
+        if (1)
+        {
+            btCollisionShape* box = new btBoxShape(baseHalfExtents);
+            box->setMargin(0.001);
+            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
+            col->setCollisionShape(box);
+            
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(local_origin[0]);
+            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+            col->setWorldTransform(tr);
+            
+            pWorld->addCollisionObject(col, 2, 1 + 2);
+            
+            col->setFriction(friction);
+            pMultiBody->setBaseCollider(col);
+        }
+    }
+    
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        const int parent = pMultiBody->getParent(i);
+        world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
+        local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
+    }
+    
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        btVector3 posr = local_origin[i + 1];
+        
+        btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
+        
+        btCollisionShape* box = new btBoxShape(linkHalfExtents);
+        box->setMargin(0.001);
+        btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+        
+        col->setCollisionShape(box);
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        col->setWorldTransform(tr);
+        col->setFriction(friction);
+        pWorld->addCollisionObject(col, 2, 1 + 2);
+        
+        pMultiBody->getLink(i).m_collider = col;
+    }
+}
+
+class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options)
+{
+	return new ReducedMotorGrasp(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/ReducedMotorGrasp.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_MOTOR_GRASP
+#define _REDUCED_MOTOR_GRASP
+
+class CommonExampleInterface* ReducedMotorGraspCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_MOTOR_GRASP

examples/ReducedDeformableDemo/Springboard.cpp

@@ -0,0 +1,264 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#include "Springboard.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+static btScalar damping_alpha = 0.0;
+static btScalar damping_beta = 0.0001;
+static int num_modes = 20;
+
+class Springboard : public CommonDeformableBodyBase
+{
+    btScalar sim_time;
+    bool first_step;
+
+public:
+    Springboard(struct GUIHelperInterface* helper)
+        : CommonDeformableBodyBase(helper)
+    {
+        sim_time = 0;
+        first_step = true;
+    }
+    virtual ~Springboard()
+    {
+    }
+    void initPhysics();
+
+    void exitPhysics();
+
+    // TODO: disable pick force, non-interactive for now.
+    bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) {
+        return false;
+    } 
+
+    void resetCamera()
+    {
+        float dist = 10;
+        float pitch = 0;
+        float yaw = 90;
+        float targetPos[3] = {0, 3, 0};
+        m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+    }
+    
+    void Ctor_RbUpStack()
+    {
+        float mass = 2;
+        btCollisionShape* shape = new btBoxShape(btVector3(1, 1, 1));
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0,8,1));
+        btRigidBody* rb1 = createRigidBody(mass, startTransform, shape);
+        rb1->setActivationState(DISABLE_DEACTIVATION);
+    }
+    
+    void stepSimulation(float deltaTime)
+    {      
+      float internalTimeStep = 1. / 60.f;
+      m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep);
+
+      {
+        sim_time += internalTimeStep;
+        btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(getDeformableDynamicsWorld()->getSoftBodyArray()[0]);
+        
+        // std::ofstream myfile("fixed_node.txt", std::ios_base::app);
+        // myfile << sim_time << "\t" << rsb->m_nodes[0].m_x[0] - rsb->m_x0[0][0] << "\t" 
+        //                            << rsb->m_nodes[0].m_x[1] - rsb->m_x0[0][1] << "\t" 
+        //                            << rsb->m_nodes[0].m_x[2] - rsb->m_x0[0][2] << "\n";
+        // myfile.close();
+      }
+    }
+    
+    virtual void renderScene()
+    {
+        CommonDeformableBodyBase::renderScene();
+        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+        
+        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+        {
+            btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(deformableWorld->getSoftBodyArray()[i]);
+            {
+                btSoftBodyHelpers::DrawFrame(rsb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(rsb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); 
+
+                for (int p = 0; p < rsb->m_fixedNodes.size(); ++p)
+                {
+                    deformableWorld->getDebugDrawer()->drawSphere(rsb->m_nodes[rsb->m_fixedNodes[p]].m_x, 0.2, btVector3(1, 0, 0));
+                    // std::cout << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[0] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[1] << "\t" << rsb->m_nodes[rsb->m_fixedNodes[p]].m_x[2] << "\n";
+                }
+                // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 0, 0), 0.1, btVector3(1, 1, 1));
+                // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 2, 0), 0.1, btVector3(1, 1, 1));
+                // deformableWorld->getDebugDrawer()->drawSphere(btVector3(0, 4, 0), 0.1, btVector3(1, 1, 1));
+            }
+        }
+    }
+};
+
+void Springboard::initPhysics()
+{
+    m_guiHelper->setUpAxis(1);
+
+    ///collision configuration contains default setup for memory, collision setup
+    m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+    m_broadphase = new btDbvtBroadphase();
+    btReducedDeformableBodySolver* reducedSoftBodySolver = new btReducedDeformableBodySolver();
+    btVector3 gravity = btVector3(0, -10, 0);
+    reducedSoftBodySolver->setGravity(gravity);
+
+    btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+    sol->setDeformableSolver(reducedSoftBodySolver);
+    m_solver = sol;
+
+    m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, reducedSoftBodySolver);
+    m_dynamicsWorld->setGravity(gravity);
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+    // create volumetric reduced deformable body
+    {   
+        std::string file_path("../../../data/reduced_beam/");
+        std::string vtk_file("beam_mesh_origin.vtk");
+        btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createReducedDeformableObject(
+                                            getDeformableDynamicsWorld()->getWorldInfo(),
+                                            file_path,
+                                            vtk_file,
+                                            num_modes,
+                                            false);
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.1);
+        
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(btVector3(0, 4, 0));
+        // init_transform.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_PI / 2.0));
+        rsb->transform(init_transform);
+
+        rsb->setStiffnessScale(200);
+        rsb->setDamping(damping_alpha, damping_beta);
+        
+        // set fixed nodes
+        rsb->setFixedNodes(0);
+        rsb->setFixedNodes(1);
+        rsb->setFixedNodes(2);
+        rsb->setFixedNodes(3);
+        
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+        
+        // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
+        // rsb->setRigidVelocity(btVector3(0, 1, 0));
+        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    getDeformableDynamicsWorld()->setUseProjection(false);
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0.2;
+    getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200);
+    getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3;
+    getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = false;
+    getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100;
+    // add a few rigid bodies
+    Ctor_RbUpStack();
+
+    // create a static rigid box as the ground
+    {
+        // btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50), btScalar(50), btScalar(50)));
+        btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10), btScalar(2), btScalar(10)));
+        m_collisionShapes.push_back(groundShape);
+
+        btTransform groundTransform;
+        groundTransform.setIdentity();
+        groundTransform.setOrigin(btVector3(0, 0, 0));
+        {
+            btScalar mass(0.);
+            createRigidBody(mass, groundTransform, groundShape, btVector4(0,0,0,0));
+        }
+    }
+
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void Springboard::exitPhysics()
+{
+    //cleanup in the reverse order of creation/initialization
+    removePickingConstraint();
+    //remove the rigidbodies from the dynamics world and delete them
+    int i;
+    for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+    {
+        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+        btRigidBody* body = btRigidBody::upcast(obj);
+        if (body && body->getMotionState())
+        {
+            delete body->getMotionState();
+        }
+        m_dynamicsWorld->removeCollisionObject(obj);
+        delete obj;
+    }
+    // delete forces
+    for (int j = 0; j < m_forces.size(); j++)
+    {
+        btDeformableLagrangianForce* force = m_forces[j];
+        delete force;
+    }
+    m_forces.clear();
+    
+    //delete collision shapes
+    for (int j = 0; j < m_collisionShapes.size(); j++)
+    {
+        btCollisionShape* shape = m_collisionShapes[j];
+        delete shape;
+    }
+    m_collisionShapes.clear();
+
+    delete m_dynamicsWorld;
+
+    delete m_solver;
+
+    delete m_broadphase;
+
+    delete m_dispatcher;
+
+    delete m_collisionConfiguration;
+}
+
+
+
+class CommonExampleInterface* ReducedSpringboardCreateFunc(struct CommonExampleOptions& options)
+{
+    return new Springboard(options.m_guiHelper);
+}
+
+

examples/ReducedDeformableDemo/Springboard.h

@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ This software is provided 'as-is', without any express or implied warranty.
+ In no event will the authors be held liable for any damages arising from the use of this software.
+ Permission is granted to anyone to use this software for any purpose,
+ including commercial applications, and to alter it and redistribute it freely,
+ subject to the following restrictions:
+ 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+ 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+ 3. This notice may not be removed or altered from any source distribution.
+ */
+
+#ifndef _REDUCED_SPRINGBOARD_H
+#define _REDUCED_SPRINGBOARD_H
+
+class CommonExampleInterface* ReducedSpringboardCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_REDUCED_SPRINGBOARD_H

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -120,6 +120,10 @@
 #include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
 #include "BulletSoftBody/btDeformableBodySolver.h"
 #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
+
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h"
+#include "BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
 #endif  //SKIP_DEFORMABLE_BODY
 
 #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
@@ -1665,6 +1669,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btDeformableMousePickingForce* m_mouseForce;
 	btScalar m_maxPickingForce;
 	btDeformableBodySolver* m_deformablebodySolver;
+	btReducedDeformableBodySolver* m_reducedSoftBodySolver;
 	btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
 #endif
 
@@ -2721,16 +2726,26 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
 		m_data->m_broadphase = bv;
 	}
 
+#ifndef SKIP_DEFORMABLE_BODY
 	if (flags & RESET_USE_DEFORMABLE_WORLD)
 	{
-#ifndef SKIP_DEFORMABLE_BODY
+		// deformable
 		m_data->m_deformablebodySolver = new btDeformableBodySolver();
 		btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
 		m_data->m_solver = solver;
 		solver->setDeformableSolver(m_data->m_deformablebodySolver);
 		m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
-#endif
 	}
+	else if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD)
+	{
+		// reduced deformable
+		m_data->m_reducedSoftBodySolver = new btReducedDeformableBodySolver();
+		btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
+		m_data->m_solver = solver;
+		solver->setDeformableSolver(m_data->m_reducedSoftBodySolver);
+		m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_reducedSoftBodySolver);
+	}
+#endif
 
 	
 
@@ -2777,7 +2792,14 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
 	m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;  //need to check if there are artifacts with frictionERP
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
-	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
+	if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD) 
+	{
+		m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128;
+	}
+	else
+	{
+		m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 0;
+	}
 	m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = 0.1;
 	gDbvtMargin = btScalar(0);
 	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
@@ -3651,6 +3673,11 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 				return false;
 			}
 		}
+		if (!(u2b.getReducedDeformableModel().m_visualFileName.empty()))
+		{
+			bool use_self_collision = false;
+			return processReducedDeformable(u2b.getReducedDeformableModel(), pos, orn, bodyUniqueIdPtr, bufferServerToClient, bufferSizeInBytes, globalScaling, use_self_collision);
+		}
 		bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
 		if (ok)
 		{
@@ -9412,6 +9439,10 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 			psb->setGravityFactor(deformable.m_gravFactor);
 			psb->setCacheBarycenter(deformable.m_cache_barycenter);
 			psb->initializeFaceTree();
+
+			deformWorld->setImplicit(true);
+			deformWorld->setLineSearch(false);
+			deformWorld->setUseProjection(true);
 		}
 #endif  //SKIP_DEFORMABLE_BODY
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
@@ -9673,6 +9704,447 @@ bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& defo
 	return true;
 }
 
+bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDeformable& reduced_deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision)
+{
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+	btReducedDeformableBody* rsb = NULL;
+	CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
+	char relativeFileName[1024];
+	char pathPrefix[1024];
+	pathPrefix[0] = 0;
+	if (fileIO->findResourcePath(reduced_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 (!reduced_deformable.m_simFileName.empty())
+	{
+		bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, reduced_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)
+	{
+		printf("Obj file is currently unsupported\n");
+		return false;
+	}
+	else if (out_sim_type == UrdfGeometry::FILE_VTK)
+	{
+#ifndef SKIP_DEFORMABLE_BODY
+		btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+		if (deformWorld)
+		{
+			rsb = btReducedDeformableBodyHelpers::createFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
+			if (!rsb)
+			{
+				printf("Load reduced deformable failed\n");
+				return false;
+			}
+
+			// load modes, reduced stiffness matrix
+			rsb->setReducedModes(reduced_deformable.m_numModes, rsb->m_nodes.size());
+			rsb->setStiffnessScale(reduced_deformable.m_stiffnessScale);
+			rsb->setDamping(0, reduced_deformable.m_damping); // damping alpha is set to 0 by default
+			btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, pathPrefix);
+			// set total mass
+			rsb->setTotalMass(reduced_deformable.m_mass);
+		}
+#endif
+	}
+	b3ImportMeshData meshData;
+
+	if (rsb != NULL)
+	{
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+		// load render mesh
+		if ((out_found_sim_filename != out_found_filename) || ((out_sim_type == UrdfGeometry::FILE_OBJ)))
+		{
+			// load render mesh
+			if (!m_data->m_useAlternativeDeformableIndexing)
+			{
+
+				float rgbaColor[4] = { 1,1,1,1 };
+
+				if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(
+					out_found_filename.c_str(), meshData, fileIO))
+				{
+
+					for (int v = 0; v < meshData.m_gfxShape->m_numvertices; v++)
+					{
+						btSoftBody::RenderNode n;
+						n.m_x.setValue(
+							meshData.m_gfxShape->m_vertices->at(v).xyzw[0],
+							meshData.m_gfxShape->m_vertices->at(v).xyzw[1],
+							meshData.m_gfxShape->m_vertices->at(v).xyzw[2]);
+						n.m_uv1.setValue(meshData.m_gfxShape->m_vertices->at(v).uv[0],
+							meshData.m_gfxShape->m_vertices->at(v).uv[1],
+							0.);
+						n.m_normal.setValue(meshData.m_gfxShape->m_vertices->at(v).normal[0],
+							meshData.m_gfxShape->m_vertices->at(v).normal[1],
+							meshData.m_gfxShape->m_vertices->at(v).normal[2]);
+						rsb->m_renderNodes.push_back(n);
+					}
+					for (int f = 0; f < meshData.m_gfxShape->m_numIndices; f += 3)
+					{
+						btSoftBody::RenderFace ff;
+						ff.m_n[0] = &rsb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 0)];
+						ff.m_n[1] = &rsb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 1)];
+						ff.m_n[2] = &rsb->m_renderNodes[meshData.m_gfxShape->m_indices->at(f + 2)];
+						rsb->m_renderFaces.push_back(ff);
+					}
+				}
+			}
+			else
+			{
+				bt_tinyobj::attrib_t attribute;
+				std::vector<bt_tinyobj::shape_t> shapes;
+
+				std::string err = bt_tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
+
+				for (int s = 0; s < (int)shapes.size(); s++)
+				{
+					bt_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++)
+					{
+						btSoftBody::RenderNode n;
+						n.m_x = btVector3(attribute.vertices[3 * v], attribute.vertices[3 * v + 1], attribute.vertices[3 * v + 2]);
+						rsb->m_renderNodes.push_back(n);
+					}
+					for (int f = 0; f < faceCount; f += 3)
+					{
+						if (f < 0 && f >= int(shape.mesh.indices.size()))
+						{
+							continue;
+						}
+						bt_tinyobj::index_t v_0 = shape.mesh.indices[f];
+						bt_tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
+						bt_tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
+						btSoftBody::RenderFace ff;
+						ff.m_n[0] = &rsb->m_renderNodes[v_0.vertex_index];
+						ff.m_n[1] = &rsb->m_renderNodes[v_1.vertex_index];
+						ff.m_n[2] = &rsb->m_renderNodes[v_2.vertex_index];
+						rsb->m_renderFaces.push_back(ff);
+					}
+				}
+			}
+			if (out_sim_type == UrdfGeometry::FILE_VTK)
+			{
+				btSoftBodyHelpers::interpolateBarycentricWeights(rsb);
+			}
+			else if (out_sim_type == UrdfGeometry::FILE_OBJ)
+			{
+				btSoftBodyHelpers::extrapolateBarycentricWeights(rsb);
+			}
+		}
+		else
+		{
+			rsb->m_renderNodes.resize(0);
+		}
+#endif
+#ifndef SKIP_DEFORMABLE_BODY
+		btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+		if (deformWorld)
+		{
+			btScalar collision_hardness = 1;
+			rsb->m_cfg.kKHR = collision_hardness;
+			rsb->m_cfg.kCHR = collision_hardness;
+
+			rsb->m_cfg.kDF = reduced_deformable.m_friction;
+			btSoftBody::Material* pm = rsb->appendMaterial();
+			pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
+
+			// turn on the collision flag for deformable
+			// collision between deformable and rigid
+			rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+			/// turn on node contact for rigid body
+			rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+			// turn on face contact for multibodies
+			// rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
+			// collion between deformable and deformable and self-collision
+			// rsb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
+			rsb->setCollisionFlags(0);
+			rsb->setSelfCollision(useSelfCollision);
+			rsb->initializeFaceTree();
+		}
+#endif  //SKIP_DEFORMABLE_BODY
+// #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+// 		btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
+// 		if (softWorld)
+// 		{
+// 			btSoftBody::Material* pm = rsb->appendMaterial();
+// 			pm->m_kLST = 0.5;
+// 			pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
+// 			rsb->generateBendingConstraints(2, pm);
+// 			rsb->m_cfg.piterations = 20;
+// 			rsb->m_cfg.kDF = 0.5;
+// 			//turn on softbody vs softbody collision
+// 			rsb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
+// 			rsb->randomizeConstraints();
+// 			rsb->setTotalMass(reduced_deformable.m_mass, true);
+// 		}
+// #endif  //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+		rsb->scale(btVector3(scale, scale, scale));
+		btTransform init_transform;
+		init_transform.setOrigin(pos);
+		init_transform.setRotation(orn);
+		rsb->transform(init_transform);
+
+		rsb->getCollisionShape()->setMargin(reduced_deformable.m_collisionMargin);
+		rsb->getCollisionShape()->setUserPointer(rsb);
+#ifndef SKIP_DEFORMABLE_BODY
+		if (deformWorld)
+		{
+			deformWorld->addSoftBody(rsb);
+			deformWorld->getSolverInfo().m_deformable_erp = reduced_deformable.m_erp;
+			deformWorld->getSolverInfo().m_deformable_cfm = reduced_deformable.m_cfm;
+			deformWorld->getSolverInfo().m_friction = reduced_deformable.m_friction;
+			deformWorld->getSolverInfo().m_splitImpulse = false;
+			deformWorld->setImplicit(false);
+			deformWorld->setLineSearch(false);
+			deformWorld->setUseProjection(false);
+		}
+		else
+#endif  //SKIP_DEFORMABLE_BODY
+		{
+			btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
+			if (softWorld)
+			{
+				softWorld->addSoftBody(rsb);
+			}
+		}
+		
+		*bodyUniqueId = m_data->m_bodyHandles.allocHandle();
+		InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
+		bodyHandle->m_softBody = rsb;
+		rsb->setUserIndex2(*bodyUniqueId);
+
+		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] = 1;
+		visualShape.m_dimensions[1] = 1;
+		visualShape.m_dimensions[2] = 1;
+		//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] = 0;
+		visualShape.m_localVisualFrame[1] = 0;
+		visualShape.m_localVisualFrame[2] = 0;
+		visualShape.m_localVisualFrame[3] = 0;
+		visualShape.m_localVisualFrame[4] = 0;
+		visualShape.m_localVisualFrame[5] = 0;
+		visualShape.m_localVisualFrame[6] = 1;
+		//color and ids to be set by the renderer
+		visualShape.m_rgbaColor[0] = 1;
+		visualShape.m_rgbaColor[1] = 1;
+		visualShape.m_rgbaColor[2] = 1;
+		visualShape.m_rgbaColor[3] = 1;
+		visualShape.m_tinyRendererTextureId = -1;
+		visualShape.m_textureUniqueId = -1;
+		visualShape.m_openglTextureId = -1;
+
+		if (meshData.m_gfxShape)
+		{
+			int texUid1 = -1;
+			if (meshData.m_textureHeight > 0 && meshData.m_textureWidth > 0 && meshData.m_textureImage1)
+			{
+				texUid1 = m_data->m_guiHelper->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
+			}
+			visualShape.m_openglTextureId = texUid1;
+			int shapeUid1 = m_data->m_guiHelper->registerGraphicsShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], meshData.m_gfxShape->m_numvertices, &meshData.m_gfxShape->m_indices->at(0), meshData.m_gfxShape->m_numIndices, B3_GL_TRIANGLES, texUid1);
+			rsb->getCollisionShape()->setUserIndex(shapeUid1);
+			float position[4] = { 0,0,0,1 };
+			float orientation[4] = { 0,0,0,1 };
+			float color[4] = { 1,1,1,1 };
+			float scaling[4] = { 1,1,1,1 };
+ 			int instanceUid = m_data->m_guiHelper->registerGraphicsInstance(shapeUid1, position, orientation, color, scaling);
+			rsb->setUserIndex(instanceUid);
+			
+			if (m_data->m_enableTinyRenderer)
+			{
+				int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
+				visualShape.m_tinyRendererTextureId = texUid2;
+				int linkIndex = -1;
+				int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance(
+					visualShape,
+					&meshData.m_gfxShape->m_vertices->at(0).xyzw[0],
+					meshData.m_gfxShape->m_numvertices,
+					&meshData.m_gfxShape->m_indices->at(0),
+					meshData.m_gfxShape->m_numIndices,
+					B3_GL_TRIANGLES,
+					texUid2,
+					rsb->getBroadphaseHandle()->getUid(),
+					*bodyUniqueId,
+					linkIndex);
+
+				rsb->setUserIndex3(softBodyGraphicsShapeUid);
+			}
+			delete meshData.m_gfxShape;
+			meshData.m_gfxShape = 0;
+		}
+		else
+		{
+			//m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
+
+			btAlignedObjectArray<GLInstanceVertex> gfxVertices;
+			btAlignedObjectArray<int> indices;
+			int strideInBytes = 9 * sizeof(float);
+			gfxVertices.resize(rsb->m_faces.size() * 3);
+			for (int i = 0; i < rsb->m_faces.size(); i++)  // Foreach face
+			{
+				for (int k = 0; k < 3; k++)  // Foreach vertex on a face
+				{
+					int currentIndex = i * 3 + k;
+					for (int j = 0; j < 3; j++)
+					{
+						gfxVertices[currentIndex].xyzw[j] = rsb->m_faces[i].m_n[k]->m_x[j];
+					}
+					for (int j = 0; j < 3; j++)
+					{
+						gfxVertices[currentIndex].normal[j] = rsb->m_faces[i].m_n[k]->m_n[j];
+					}
+					for (int j = 0; j < 2; j++)
+					{
+						gfxVertices[currentIndex].uv[j] = btFabs(btFabs(10. * rsb->m_faces[i].m_n[k]->m_x[j]));
+					}
+					indices.push_back(currentIndex);
+				}
+			}
+			if (gfxVertices.size() && indices.size())
+			{
+				int red = 173;
+				int green = 199;
+				int blue = 255;
+
+				int texWidth = 256;
+				int texHeight = 256;
+				btAlignedObjectArray<unsigned char> texels;
+				texels.resize(texWidth* texHeight * 3);
+				for (int i = 0; i < texWidth * texHeight * 3; i++)
+					texels[i] = 255;
+				for (int i = 0; i < texWidth; i++)
+				{
+					for (int j = 0; j < texHeight; j++)
+					{
+						int a = i < texWidth / 2 ? 1 : 0;
+						int b = j < texWidth / 2 ? 1 : 0;
+
+						if (a == b)
+						{
+							texels[(i + j * texWidth) * 3 + 0] = red;
+							texels[(i + j * texWidth) * 3 + 1] = green;
+							texels[(i + j * texWidth) * 3 + 2] = blue;
+						}
+					}
+				}
+
+				int texId = m_data->m_guiHelper->registerTexture(&texels[0], texWidth, texHeight);
+				visualShape.m_openglTextureId = texId;
+				int shapeId = m_data->m_guiHelper->registerGraphicsShape(&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texId);
+				b3Assert(shapeId >= 0);
+				rsb->getCollisionShape()->setUserIndex(shapeId);
+				if (m_data->m_enableTinyRenderer)
+				{
+
+					int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(&texels[0], texWidth, texHeight);
+					visualShape.m_tinyRendererTextureId = texUid2;
+					int linkIndex = -1;
+					int softBodyGraphicsShapeUid = m_data->m_pluginManager.getRenderInterface()->registerShapeAndInstance(
+						visualShape,
+						&gfxVertices[0].xyzw[0], gfxVertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, texUid2,
+						rsb->getBroadphaseHandle()->getUid(),
+						*bodyUniqueId,
+						linkIndex);
+					rsb->setUserIndex3(softBodyGraphicsShapeUid);
+				}
+			}
+		}
+		
+
+
+		btAlignedObjectArray<btVector3> vertices;
+		btAlignedObjectArray<btVector3> normals;
+		if (rsb->m_renderNodes.size() == 0)
+		{
+			rsb->m_renderNodes.resize(rsb->m_faces.size()*3);
+			vertices.resize(rsb->m_faces.size() * 3);
+			normals.resize(rsb->m_faces.size() * 3);
+
+			for (int i = 0; i < rsb->m_faces.size(); i++)  // Foreach face
+			{
+				
+				for (int k = 0; k < 3; k++)  // Foreach vertex on a face
+				{
+					int currentIndex = i * 3 + k;
+					for (int j = 0; j < 3; j++)
+					{
+						rsb->m_renderNodes[currentIndex].m_x[j] = rsb->m_faces[i].m_n[k]->m_x[j];
+					}
+					for (int j = 0; j < 3; j++)
+					{
+						rsb->m_renderNodes[currentIndex].m_normal[j] = rsb->m_faces[i].m_n[k]->m_n[j];
+					}
+					for (int j = 0; j < 2; j++)
+					{
+						rsb->m_renderNodes[currentIndex].m_uv1[j] = btFabs(10*rsb->m_faces[i].m_n[k]->m_x[j]);
+					}
+					rsb->m_renderNodes[currentIndex].m_uv1[2] = 0;
+					vertices[currentIndex] = rsb->m_faces[i].m_n[k]->m_x;
+					normals[currentIndex] = rsb->m_faces[i].m_n[k]->m_n;
+				}
+			}
+			btSoftBodyHelpers::extrapolateBarycentricWeights(rsb);
+		}
+		else
+		{
+			vertices.resize(rsb->m_renderNodes.size());
+			normals.resize(rsb->m_renderNodes.size());
+			for (int i = 0; i < rsb->m_renderNodes.size(); i++)  // Foreach face
+			{
+				vertices[i] = rsb->m_renderNodes[i].m_x;
+				normals[i] = rsb->m_renderNodes[i].m_normal;
+			}
+		}
+		m_data->m_pluginManager.getRenderInterface()->updateShape(rsb->getUserIndex3(), &vertices[0], vertices.size(), &normals[0], normals.size());
+
+		if (!reduced_deformable.m_name.empty())
+		{
+			bodyHandle->m_bodyName = reduced_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);
+	}
+#endif
+	return true;
+}
+
 bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
@@ -10906,7 +11378,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 		btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
 		if (deformWorld)
 		{
-			deformWorld->getWorldInfo().m_gravity = grav;
+			// deformWorld->getWorldInfo().m_gravity = grav;
+			deformWorld->setGravity(grav);
 			for (int i = 0; i < m_data->m_lf.size(); ++i)
 			{
 				btDeformableLagrangianForce* force = m_data->m_lf[i];

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -110,6 +110,7 @@ protected:
 
 	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 processReducedDeformable(const UrdfReducedDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision);
 
 	bool supportsJointMotor(class btMultiBody* body, int linkIndex);
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -617,6 +617,7 @@ enum b3ResetSimulationFlags
 	RESET_USE_DEFORMABLE_WORLD=1,
 	RESET_USE_DISCRETE_DYNAMICS_WORLD=2,
 	RESET_USE_SIMPLE_BROADPHASE=4,
+	RESET_USE_REDUCED_DEFORMABLE_WORLD=8,
 };
 
 struct b3BodyNotificationArgs

examples/pybullet/examples/reduced_deformable_cube.py

@@ -0,0 +1,27 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation(p.RESET_USE_REDUCED_DEFORMABLE_WORLD)
+p.resetDebugVisualizerCamera(4,-40,-30,[0, 0, 0])
+p.setGravity(0, 0, -10)
+
+tex = p.loadTexture("uvmap.png")
+planeId = p.loadURDF("plane.urdf", [0,0,-2])
+
+boxId = p.loadURDF("cube.urdf", [1,1,5],useMaximalCoordinates = True)
+
+#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "reduced_cube.mp4")
+cube = p.loadURDF("reduced_cube/reduced_cube.urdf", [1,1,1])
+p.changeVisualShape(cube, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(0)
+
+while p.isConnected():
+  p.stepSimulation()
+  p.getCameraImage(320,200)
+  p.setGravity(0,0,-10)

examples/pybullet/examples/reduced_deformable_torus.py

@@ -0,0 +1,32 @@
+import pybullet as p
+from time import sleep
+import pybullet_data
+
+physicsClient = p.connect(p.GUI)
+
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+p.resetSimulation(p.RESET_USE_REDUCED_DEFORMABLE_WORLD)
+p.resetDebugVisualizerCamera(4,-40,-30,[0, 0, 0])
+p.setGravity(0, 0, -10)
+
+tex = p.loadTexture("uvmap.png")
+planeId = p.loadURDF("plane.urdf", [0,0,-2])
+
+box1 = p.loadURDF("cube.urdf", [1,1,3],useMaximalCoordinates = True)
+box2 = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
+
+# p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "reduced_torus.mp4")
+reduced_obj1= p.loadURDF("reduced_torus/reduced_torus.urdf", [1,1,1])
+p.changeVisualShape(reduced_obj1, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+
+reduced_obj2 = p.loadURDF("reduced_torus/reduced_torus.urdf", [1,2,1])
+p.changeVisualShape(reduced_obj2, -1, rgbaColor=[1,1,1,1], textureUniqueId=tex, flags=0)
+
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(0)
+
+while p.isConnected():
+  p.stepSimulation()
+  p.getCameraImage(320,200)
+  p.setGravity(0,0,-10)

examples/pybullet/pybullet.c

@@ -13253,6 +13253,7 @@ initpybullet(void)
 	//PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_BLOCK",eConstraintSolverLCP_BLOCK_PGS);
 
 	PyModule_AddIntConstant(m, "RESET_USE_DEFORMABLE_WORLD", RESET_USE_DEFORMABLE_WORLD);
+	PyModule_AddIntConstant(m, "RESET_USE_REDUCED_DEFORMABLE_WORLD", RESET_USE_REDUCED_DEFORMABLE_WORLD);
 	PyModule_AddIntConstant(m, "RESET_USE_DISCRETE_DYNAMICS_WORLD", RESET_USE_DISCRETE_DYNAMICS_WORLD);
 	PyModule_AddIntConstant(m, "RESET_USE_SIMPLE_BROADPHASE", RESET_USE_SIMPLE_BROADPHASE);
 

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.cpp

@@ -0,0 +1,792 @@
+#include "btReducedDeformableBody.h"
+#include "../btSoftBodyInternals.h"
+#include "btReducedDeformableBodyHelpers.h"
+#include "LinearMath/btTransformUtil.h"
+#include <iostream>
+#include <fstream>
+
+btReducedDeformableBody::btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
+ : btSoftBody(worldInfo, node_count, x, m), m_rigidOnly(false)
+{
+  // reduced deformable
+  m_reducedModel = true;
+  m_nReduced = 0;
+  m_nFull = 0;
+  m_nodeIndexOffset = 0;
+
+  m_transform_lock = false;
+  m_ksScale = 1.0;
+  m_rhoScale = 1.0;
+
+  // rigid motion
+  m_linearVelocity.setZero();
+	m_angularVelocity.setZero();
+  m_internalDeltaLinearVelocity.setZero();
+  m_internalDeltaAngularVelocity.setZero();
+  m_angularVelocityFromReduced.setZero();
+  m_internalDeltaAngularVelocityFromReduced.setZero();
+	m_angularFactor.setValue(1, 1, 1);
+	m_linearFactor.setValue(1, 1, 1);
+  // m_invInertiaLocal.setValue(1, 1, 1);
+  m_invInertiaLocal.setIdentity();
+  m_mass = 0.0;
+  m_inverseMass = 0.0;
+
+  m_linearDamping = 0;
+  m_angularDamping = 0;
+
+  // Rayleigh damping
+  m_dampingAlpha = 0;
+  m_dampingBeta = 0;
+
+  m_rigidTransformWorld.setIdentity();
+}
+
+void btReducedDeformableBody::setReducedModes(int num_modes, int full_size)
+{
+  m_nReduced = num_modes;
+  m_nFull = full_size;
+  m_reducedDofs.resize(m_nReduced, 0);
+  m_reducedDofsBuffer.resize(m_nReduced, 0);
+  m_reducedVelocity.resize(m_nReduced, 0);
+  m_reducedVelocityBuffer.resize(m_nReduced, 0);
+  m_reducedForceElastic.resize(m_nReduced, 0);
+  m_reducedForceDamping.resize(m_nReduced, 0);
+  m_reducedForceExternal.resize(m_nReduced, 0);
+  m_internalDeltaReducedVelocity.resize(m_nReduced, 0);
+  m_nodalMass.resize(full_size, 0);
+  m_localMomentArm.resize(m_nFull);
+}
+
+void btReducedDeformableBody::setMassProps(const tDenseArray& mass_array)
+{
+  btScalar total_mass = 0;
+  btVector3 CoM(0, 0, 0);
+	for (int i = 0; i < m_nFull; ++i)
+	{
+		m_nodalMass[i] = m_rhoScale * mass_array[i];
+		m_nodes[i].m_im = mass_array[i] > 0 ? 1.0 / (m_rhoScale * mass_array[i]) : 0;
+		total_mass += m_rhoScale * mass_array[i];
+
+    CoM += m_nodalMass[i] * m_nodes[i].m_x;
+	}
+  // total rigid body mass
+  m_mass = total_mass;
+  m_inverseMass = total_mass > 0 ? 1.0 / total_mass : 0;
+  // original CoM
+  m_initialCoM = CoM / total_mass;
+}
+
+void btReducedDeformableBody::setInertiaProps()
+{
+  // make sure the initial CoM is at the origin (0,0,0)
+  // for (int i = 0; i < m_nFull; ++i)
+  // {
+  //   m_nodes[i].m_x -= m_initialCoM;
+  // }
+  // m_initialCoM.setZero();
+  m_rigidTransformWorld.setOrigin(m_initialCoM);
+  m_interpolationWorldTransform = m_rigidTransformWorld;
+  
+  updateLocalInertiaTensorFromNodes();
+
+  // update world inertia tensor
+  btMatrix3x3 rotation;
+  rotation.setIdentity();
+  updateInitialInertiaTensor(rotation);
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+}
+
+void btReducedDeformableBody::setRigidVelocity(const btVector3& v)
+{
+  m_linearVelocity = v;
+}
+
+void btReducedDeformableBody::setRigidAngularVelocity(const btVector3& omega)
+{
+  m_angularVelocity = omega;
+}
+
+void btReducedDeformableBody::setStiffnessScale(const btScalar ks)
+{
+  m_ksScale = ks;
+}
+
+void btReducedDeformableBody::setMassScale(const btScalar rho)
+{
+  m_rhoScale = rho;
+}
+
+void btReducedDeformableBody::setFixedNodes(const int n_node)
+{
+  m_fixedNodes.push_back(n_node);
+  m_nodes[n_node].m_im = 0;   // set inverse mass to be zero for the constraint solver.
+}
+
+void btReducedDeformableBody::setDamping(const btScalar alpha, const btScalar beta)
+{
+  m_dampingAlpha = alpha;
+  m_dampingBeta = beta;
+}
+
+void btReducedDeformableBody::internalInitialization()
+{
+  // zeroing
+  endOfTimeStepZeroing();
+  // initialize rest position
+  updateRestNodalPositions();
+  // initialize local nodal moment arm form the CoM
+  updateLocalMomentArm();
+  // initialize projection matrix
+  updateExternalForceProjectMatrix(false);
+}
+
+void btReducedDeformableBody::updateLocalMomentArm()
+{
+  TVStack delta_x;
+  delta_x.resize(m_nFull);
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    for (int k = 0; k < 3; ++k)
+    {
+      // compute displacement
+      delta_x[i][k] = 0;
+      for (int j = 0; j < m_nReduced; ++j) 
+      {
+        delta_x[i][k] += m_modes[j][3 * i + k] * m_reducedDofs[j];
+      }
+    }
+    // get new moment arm Sq + x0
+    m_localMomentArm[i] = m_x0[i] - m_initialCoM + delta_x[i];
+  }
+}
+
+void btReducedDeformableBody::updateExternalForceProjectMatrix(bool initialized)
+{
+  // if not initialized, need to compute both P_A and Cq
+  // otherwise, only need to udpate Cq
+  if (!initialized)
+  {
+    // resize
+    m_projPA.resize(m_nReduced);
+    m_projCq.resize(m_nReduced);
+
+    m_STP.resize(m_nReduced);
+    m_MrInvSTP.resize(m_nReduced);
+
+    // P_A
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      m_projPA[r].resize(3 * m_nFull, 0);
+      for (int i = 0; i < m_nFull; ++i)
+      {
+        btMatrix3x3 mass_scaled_i = Diagonal(1) - Diagonal(m_nodalMass[i] / m_mass);
+        btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+        btVector3 prod_i = mass_scaled_i * s_ri;
+
+        for (int k = 0; k < 3; ++k)
+          m_projPA[r][3 * i + k] = prod_i[k];
+
+        // btScalar ratio = m_nodalMass[i] / m_mass;
+        // m_projPA[r] += btVector3(- m_modes[r][3 * i] * ratio,
+        //                          - m_modes[r][3 * i + 1] * ratio,
+        //                          - m_modes[r][3 * i + 2] * ratio);
+      }
+    }
+  }
+
+  // C(q) is updated once per position update
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+  	m_projCq[r].resize(3 * m_nFull, 0);
+    for (int i = 0; i < m_nFull; ++i)
+    {
+      btMatrix3x3 r_star = Cross(m_localMomentArm[i]);
+      btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      btVector3 prod_i = r_star * m_invInertiaTensorWorld * r_star * s_ri;
+
+      for (int k = 0; k < 3; ++k)
+        m_projCq[r][3 * i + k] = m_nodalMass[i] * prod_i[k];
+
+      // btVector3 si(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      // m_projCq[r] += m_nodalMass[i] * si.cross(m_localMomentArm[i]);
+    }
+  }
+}
+
+void btReducedDeformableBody::endOfTimeStepZeroing()
+{
+  for (int i = 0; i < m_nReduced; ++i)
+  {
+    m_reducedForceElastic[i] = 0;
+    m_reducedForceDamping[i] = 0;
+    m_reducedForceExternal[i] = 0;
+    m_internalDeltaReducedVelocity[i] = 0;
+    m_reducedDofsBuffer[i] = m_reducedDofs[i];
+    m_reducedVelocityBuffer[i] = m_reducedVelocity[i];
+  }
+  // std::cout << "zeroed!\n";
+}
+
+void btReducedDeformableBody::applyInternalVelocityChanges()
+{
+  m_linearVelocity += m_internalDeltaLinearVelocity;
+  m_angularVelocity += m_internalDeltaAngularVelocity;
+  m_internalDeltaLinearVelocity.setZero();
+  m_internalDeltaAngularVelocity.setZero();
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    m_reducedVelocity[r] += m_internalDeltaReducedVelocity[r];
+    m_internalDeltaReducedVelocity[r] = 0;
+  }
+}
+
+void btReducedDeformableBody::predictIntegratedTransform(btScalar dt, btTransform& predictedTransform)
+{
+	btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, predictedTransform);
+}
+
+void btReducedDeformableBody::updateReducedDofs(btScalar solverdt)
+{
+  for (int r = 0; r < m_nReduced; ++r)
+  { 
+    m_reducedDofs[r] = m_reducedDofsBuffer[r] + solverdt * m_reducedVelocity[r];
+  }
+}
+
+void btReducedDeformableBody::mapToFullPosition(const btTransform& ref_trans)
+{
+  btVector3 origin = ref_trans.getOrigin();
+  btMatrix3x3 rotation = ref_trans.getBasis();
+  
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodes[i].m_x = rotation * m_localMomentArm[i] + origin;
+    m_nodes[i].m_q = m_nodes[i].m_x;
+  }
+}
+
+void btReducedDeformableBody::updateReducedVelocity(btScalar solverdt)
+{
+  // update reduced velocity
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    // the reduced mass is always identity!
+    btScalar delta_v = 0;
+    delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r]);
+    // delta_v = solverdt * (m_reducedForceElastic[r] + m_reducedForceDamping[r] + m_reducedForceExternal[r]);
+    m_reducedVelocity[r] = m_reducedVelocityBuffer[r] + delta_v;
+  }
+}
+
+void btReducedDeformableBody::mapToFullVelocity(const btTransform& ref_trans)
+{
+  // compute the reduced contribution to the angular velocity
+  // btVector3 sum_linear(0, 0, 0);
+  // btVector3 sum_angular(0, 0, 0);
+  // m_linearVelocityFromReduced.setZero();
+  // m_angularVelocityFromReduced.setZero();
+  // for (int i = 0; i < m_nFull; ++i)
+  // {
+  //   btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[i];
+  //   btMatrix3x3 r_star = Cross(r_com);
+
+  //   btVector3 v_from_reduced(0, 0, 0);
+  //   for (int k = 0; k < 3; ++k)
+  //   {
+  //     for (int r = 0; r < m_nReduced; ++r)
+  //     {
+  //       v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
+  //     }
+  //   }
+
+  //   btVector3 delta_linear = m_nodalMass[i] * v_from_reduced;
+  //   btVector3 delta_angular = m_nodalMass[i] * (r_star * ref_trans.getBasis() * v_from_reduced);
+  //   sum_linear += delta_linear;
+  //   sum_angular += delta_angular;
+  //   // std::cout << "delta_linear: " << delta_linear[0] << "\t" << delta_linear[1] << "\t" << delta_linear[2] << "\n";
+  //   // std::cout << "delta_angular: " << delta_angular[0] << "\t" << delta_angular[1] << "\t" << delta_angular[2] << "\n";
+  //   // std::cout << "sum_linear: " << sum_linear[0] << "\t" << sum_linear[1] << "\t" << sum_linear[2] << "\n";
+  //   // std::cout << "sum_angular: " << sum_angular[0] << "\t" << sum_angular[1] << "\t" << sum_angular[2] << "\n";
+  // }
+  // m_linearVelocityFromReduced = 1.0 / m_mass * (ref_trans.getBasis() * sum_linear);
+  // m_angularVelocityFromReduced = m_interpolateInvInertiaTensorWorld * sum_angular;
+
+  // m_linearVelocity -= m_linearVelocityFromReduced;
+  // m_angularVelocity -= m_angularVelocityFromReduced;
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodes[i].m_v = computeNodeFullVelocity(ref_trans, i);
+  }
+}
+
+const btVector3 btReducedDeformableBody::computeTotalAngularMomentum() const
+{
+  btVector3 L_rigid = m_invInertiaTensorWorld.inverse() * m_angularVelocity;
+  btVector3 L_reduced(0, 0, 0);
+  btMatrix3x3 omega_prime_star = Cross(m_angularVelocityFromReduced);
+
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    btVector3 r_com = m_rigidTransformWorld.getBasis() * m_localMomentArm[i];
+    btMatrix3x3 r_star = Cross(r_com);
+
+    btVector3 v_from_reduced(0, 0, 0);
+    for (int k = 0; k < 3; ++k)
+    {
+      for (int r = 0; r < m_nReduced; ++r)
+      {
+        v_from_reduced[k] += m_modes[r][3 * i + k] * m_reducedVelocity[r];
+      }
+    }
+
+    L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced - omega_prime_star * r_com));
+    // L_reduced += m_nodalMass[i] * (r_star * (m_rigidTransformWorld.getBasis() * v_from_reduced));
+  }
+  return L_rigid + L_reduced;
+}
+
+const btVector3 btReducedDeformableBody::computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const
+{
+  btVector3 v_from_reduced(0, 0, 0);
+  btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
+  // compute velocity contributed by the reduced velocity
+  for (int k = 0; k < 3; ++k)
+  {
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      v_from_reduced[k] += m_modes[r][3 * n_node + k] * m_reducedVelocity[r];
+    }
+  }
+  // get new velocity
+  btVector3 vel = m_angularVelocity.cross(r_com) + 
+                  ref_trans.getBasis() * v_from_reduced +
+                  m_linearVelocity;
+  return vel;
+}
+
+const btVector3 btReducedDeformableBody::internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const
+{
+  btVector3 deltaV_from_reduced(0, 0, 0);
+  btVector3 r_com = ref_trans.getBasis() * m_localMomentArm[n_node];
+
+  // compute velocity contributed by the reduced velocity
+  for (int k = 0; k < 3; ++k)
+  {
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      deltaV_from_reduced[k] += m_modes[r][3 * n_node + k] * m_internalDeltaReducedVelocity[r];
+    }
+  }
+
+  // get delta velocity
+  btVector3 deltaV = m_internalDeltaAngularVelocity.cross(r_com) + 
+                     ref_trans.getBasis() * deltaV_from_reduced +
+                     m_internalDeltaLinearVelocity;
+  return deltaV;
+}
+
+void btReducedDeformableBody::proceedToTransform(btScalar dt, bool end_of_time_step)
+{
+  btTransformUtil::integrateTransform(m_rigidTransformWorld, m_linearVelocity, m_angularVelocity, dt, m_interpolationWorldTransform);
+  updateInertiaTensor();
+  // m_interpolateInvInertiaTensorWorld = m_interpolationWorldTransform.getBasis().scaled(m_invInertiaLocal) * m_interpolationWorldTransform.getBasis().transpose();
+  m_rigidTransformWorld = m_interpolationWorldTransform;
+  m_invInertiaTensorWorld = m_interpolateInvInertiaTensorWorld;
+}
+
+void btReducedDeformableBody::transformTo(const btTransform& trs)
+{
+	btTransform current_transform = getRigidTransform();
+	btTransform new_transform(trs.getBasis() * current_transform.getBasis().transpose(),
+                            trs.getOrigin() - current_transform.getOrigin());
+  transform(new_transform);
+}
+
+void btReducedDeformableBody::transform(const btTransform& trs)
+{
+  m_transform_lock = true;
+
+  // transform mesh
+  {
+    const btScalar margin = getCollisionShape()->getMargin();
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+
+    btVector3 CoM = m_rigidTransformWorld.getOrigin();
+    btVector3 translation = trs.getOrigin();
+    btMatrix3x3 rotation = trs.getBasis();
+
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+      Node& n = m_nodes[i];
+      n.m_x = rotation * (n.m_x - CoM) + CoM + translation;
+      n.m_q = rotation * (n.m_q - CoM) + CoM + translation;
+      n.m_n = rotation * n.m_n;
+      vol = btDbvtVolume::FromCR(n.m_x, margin);
+
+      m_ndbvt.update(n.m_leaf, vol);
+    }
+    updateNormals();
+    updateBounds();
+    updateConstants();
+  }
+
+  // update modes
+  updateModesByRotation(trs.getBasis());
+
+  // update inertia tensor
+  updateInitialInertiaTensor(trs.getBasis());
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+  
+  // update rigid frame (No need to update the rotation. Nodes have already been updated.)
+  m_rigidTransformWorld.setOrigin(m_initialCoM + trs.getOrigin());
+  m_interpolationWorldTransform = m_rigidTransformWorld;
+  m_initialCoM = m_rigidTransformWorld.getOrigin();
+
+  internalInitialization();
+}
+
+void btReducedDeformableBody::scale(const btVector3& scl)
+{
+  // Scaling the mesh after transform is applied is not allowed
+  btAssert(!m_transform_lock);
+
+  // scale the mesh
+  {
+    const btScalar margin = getCollisionShape()->getMargin();
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+
+    btVector3 CoM = m_rigidTransformWorld.getOrigin();
+
+    for (int i = 0; i < m_nodes.size(); ++i)
+    {
+      Node& n = m_nodes[i];
+      n.m_x = (n.m_x - CoM) * scl + CoM;
+      n.m_q = (n.m_q - CoM) * scl + CoM;
+      vol = btDbvtVolume::FromCR(n.m_x, margin);
+      m_ndbvt.update(n.m_leaf, vol);
+    }
+    updateNormals();
+    updateBounds();
+    updateConstants();
+    initializeDmInverse();
+  }
+
+  // update inertia tensor
+  updateLocalInertiaTensorFromNodes();
+
+  btMatrix3x3 id;
+  id.setIdentity();
+  updateInitialInertiaTensor(id);   // there is no rotation, but the local inertia tensor has changed
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+
+  internalInitialization();
+}
+
+void btReducedDeformableBody::setTotalMass(btScalar mass, bool fromfaces)
+{
+  // Changing the total mass after transform is applied is not allowed
+  btAssert(!m_transform_lock);
+
+  btScalar scale_ratio = mass / m_mass;
+
+  // update nodal mass
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_nodalMass[i] *= scale_ratio;
+  }
+  m_mass = mass;
+  m_inverseMass = mass > 0 ? 1.0 / mass : 0;
+
+  // update inertia tensors
+  updateLocalInertiaTensorFromNodes();
+
+  btMatrix3x3 id;
+  id.setIdentity();
+  updateInitialInertiaTensor(id);   // there is no rotation, but the local inertia tensor has changed
+  updateInertiaTensor();
+  m_interpolateInvInertiaTensorWorld = m_invInertiaTensorWorld;
+
+  internalInitialization();
+}
+
+void btReducedDeformableBody::updateRestNodalPositions()
+{
+  // update reset nodal position
+  m_x0.resize(m_nFull);
+  for (int i = 0; i < m_nFull; ++i)
+  {
+    m_x0[i] = m_nodes[i].m_x;
+  }
+}
+
+// reference notes:
+// https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-07-dynamics-fall-2009/lecture-notes/MIT16_07F09_Lec26.pdf
+void btReducedDeformableBody::updateLocalInertiaTensorFromNodes()
+{
+  btMatrix3x3 inertia_tensor;
+  inertia_tensor.setZero();
+
+  for (int p = 0; p < m_nFull; ++p)
+  {
+    btMatrix3x3 particle_inertia;
+    particle_inertia.setZero();
+
+    btVector3 r = m_nodes[p].m_x - m_initialCoM;
+
+    particle_inertia[0][0] = m_nodalMass[p] * (r[1] * r[1] + r[2] * r[2]);
+    particle_inertia[1][1] = m_nodalMass[p] * (r[0] * r[0] + r[2] * r[2]);
+    particle_inertia[2][2] = m_nodalMass[p] * (r[0] * r[0] + r[1] * r[1]);
+
+    particle_inertia[0][1] = - m_nodalMass[p] * (r[0] * r[1]);
+    particle_inertia[0][2] = - m_nodalMass[p] * (r[0] * r[2]);
+    particle_inertia[1][2] = - m_nodalMass[p] * (r[1] * r[2]);
+
+    particle_inertia[1][0] = particle_inertia[0][1];
+    particle_inertia[2][0] = particle_inertia[0][2];
+    particle_inertia[2][1] = particle_inertia[1][2];
+
+    inertia_tensor += particle_inertia;
+  }
+  m_invInertiaLocal = inertia_tensor.inverse();
+}
+
+void btReducedDeformableBody::updateInitialInertiaTensor(const btMatrix3x3& rotation)
+{
+  // m_invInertiaTensorWorldInitial = rotation.scaled(m_invInertiaLocal) * rotation.transpose();
+  m_invInertiaTensorWorldInitial = rotation * m_invInertiaLocal * rotation.transpose();
+}
+
+void btReducedDeformableBody::updateModesByRotation(const btMatrix3x3& rotation)
+{
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    for (int i = 0; i < m_nFull; ++i)
+    {
+      btVector3 nodal_disp(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      nodal_disp = rotation * nodal_disp;
+
+      for (int k = 0; k < 3; ++k)
+      {
+        m_modes[r][3 * i + k] = nodal_disp[k];
+      }
+    }
+  }
+}
+
+void btReducedDeformableBody::updateInertiaTensor()
+{
+	m_invInertiaTensorWorld = m_rigidTransformWorld.getBasis() * m_invInertiaTensorWorldInitial * m_rigidTransformWorld.getBasis().transpose();
+}
+
+void btReducedDeformableBody::applyDamping(btScalar timeStep)
+{
+  m_linearVelocity *= btScalar(1) - m_linearDamping;
+  m_angularDamping *= btScalar(1) - m_angularDamping;
+}
+
+void btReducedDeformableBody::applyCentralImpulse(const btVector3& impulse)
+{
+  m_linearVelocity += impulse * m_linearFactor * m_inverseMass;
+  #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+  clampVelocity(m_linearVelocity);
+  #endif
+}
+
+void btReducedDeformableBody::applyTorqueImpulse(const btVector3& torque)
+{
+  m_angularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
+  #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+  clampVelocity(m_angularVelocity);
+  #endif
+}
+
+void btReducedDeformableBody::internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos)
+{
+  if (m_inverseMass == btScalar(0.))
+  {
+    std::cout << "something went wrong...probably didn't initialize?\n";
+    btAssert(false);
+  }
+  // delta linear velocity
+  m_internalDeltaLinearVelocity += impulse * m_linearFactor * m_inverseMass;
+  // delta angular velocity
+  btVector3 torque = rel_pos.cross(impulse * m_linearFactor);
+  m_internalDeltaAngularVelocity += m_interpolateInvInertiaTensorWorld * torque * m_angularFactor;
+}
+
+btVector3 btReducedDeformableBody::getRelativePos(int n_node)
+{
+  btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
+  btVector3 ri = rotation * m_localMomentArm[n_node];
+  return ri;
+}
+
+btMatrix3x3 btReducedDeformableBody::getImpulseFactor(int n_node)
+{
+  // relative position
+  btMatrix3x3 rotation = m_interpolationWorldTransform.getBasis();
+  btVector3 ri = rotation * m_localMomentArm[n_node];
+  btMatrix3x3 ri_skew = Cross(ri);
+
+  // calculate impulse factor
+  // rigid part
+  btScalar inv_mass = m_nodalMass[n_node] > btScalar(0) ? btScalar(1) / m_mass : btScalar(0);
+  btMatrix3x3 K1 = Diagonal(inv_mass);
+  K1 -= ri_skew * m_interpolateInvInertiaTensorWorld * ri_skew;
+
+  // reduced deformable part
+  btMatrix3x3 SA;
+  SA.setZero();
+  for (int i = 0; i < 3; ++i)
+  {
+    for (int j = 0; j < 3; ++j)
+    {
+      for (int r = 0; r < m_nReduced; ++r)
+      {
+        SA[i][j] += m_modes[r][3 * n_node + i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
+      }
+    }
+  }
+  btMatrix3x3 RSARinv = rotation * SA * rotation.transpose();
+
+
+  TVStack omega_helper; // Sum_i m_i r*_i R S_i
+  omega_helper.resize(m_nReduced);
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    omega_helper[r].setZero();
+    for (int i = 0; i < m_nFull; ++i)
+    {
+      btMatrix3x3 mi_rstar_i = rotation * Cross(m_localMomentArm[i]) * m_nodalMass[i];
+      btVector3 s_ri(m_modes[r][3 * i], m_modes[r][3 * i + 1], m_modes[r][3 * i + 2]);
+      omega_helper[r] += mi_rstar_i * rotation * s_ri;
+    }
+  }
+
+  btMatrix3x3 sum_multiply_A;
+  sum_multiply_A.setZero();
+  for (int i = 0; i < 3; ++i)
+  {
+    for (int j = 0; j < 3; ++j)
+    {
+      for (int r = 0; r < m_nReduced; ++r)
+      {
+        sum_multiply_A[i][j] += omega_helper[r][i] * (m_projPA[r][3 * n_node + j] + m_projCq[r][3 * n_node + j]);
+      }
+    }
+  }
+
+  btMatrix3x3 K2 = RSARinv + ri_skew * m_interpolateInvInertiaTensorWorld * sum_multiply_A * rotation.transpose();
+
+  return m_rigidOnly ? K1 : K1 + K2;
+}
+
+void btReducedDeformableBody::internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt)
+{
+  if (!m_rigidOnly)
+  {
+    // apply impulse force
+    applyFullSpaceNodalForce(impulse / dt, n_node);
+
+    // update delta damping force
+    tDenseArray reduced_vel_tmp;
+    reduced_vel_tmp.resize(m_nReduced);
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      reduced_vel_tmp[r] = m_reducedVelocity[r] + m_internalDeltaReducedVelocity[r];
+    }
+    applyReducedDampingForce(reduced_vel_tmp);
+    // applyReducedDampingForce(m_internalDeltaReducedVelocity);
+
+    // delta reduced velocity
+    for (int r = 0; r < m_nReduced; ++r)
+    {
+      // The reduced mass is always identity!
+      m_internalDeltaReducedVelocity[r] += dt * (m_reducedForceDamping[r] + m_reducedForceExternal[r]);
+    }
+  }
+
+  internalApplyRigidImpulse(impulse, rel_pos);
+}
+
+void btReducedDeformableBody::applyFullSpaceNodalForce(const btVector3& f_ext, int n_node)
+{
+  // f_local = R^-1 * f_ext //TODO: interpoalted transfrom
+  // btVector3 f_local = m_rigidTransformWorld.getBasis().transpose() * f_ext;
+  btVector3 f_local = m_interpolationWorldTransform.getBasis().transpose() * f_ext;
+
+  // f_ext_r = [S^T * P]_{n_node} * f_local
+  tDenseArray f_ext_r;
+  f_ext_r.resize(m_nReduced, 0);
+  for (int r = 0; r < m_nReduced; ++r)
+  {
+    m_reducedForceExternal[r] = 0;
+    for (int k = 0; k < 3; ++k)
+    {
+      f_ext_r[r] += (m_projPA[r][3 * n_node + k] + m_projCq[r][3 * n_node + k]) * f_local[k];
+    }
+
+    m_reducedForceExternal[r] += f_ext_r[r];
+  }
+}
+
+void btReducedDeformableBody::applyRigidGravity(const btVector3& gravity, btScalar dt)
+{
+  // update rigid frame velocity
+  m_linearVelocity += dt * gravity;
+}
+
+void btReducedDeformableBody::applyReducedElasticForce(const tDenseArray& reduce_dofs)
+{
+  for (int r = 0; r < m_nReduced; ++r) 
+  {
+    m_reducedForceElastic[r] = - m_ksScale * m_Kr[r] * reduce_dofs[r];
+  }
+}
+
+void btReducedDeformableBody::applyReducedDampingForce(const tDenseArray& reduce_vel)
+{
+  for (int r = 0; r < m_nReduced; ++r) 
+  {
+    m_reducedForceDamping[r] = - m_dampingBeta * m_ksScale * m_Kr[r] * reduce_vel[r];
+  }
+}
+
+btScalar btReducedDeformableBody::getTotalMass() const
+{
+  return m_mass;
+}
+
+btTransform& btReducedDeformableBody::getRigidTransform()
+{
+  return m_rigidTransformWorld;
+}
+
+const btVector3& btReducedDeformableBody::getLinearVelocity() const
+{
+  return m_linearVelocity;
+}
+
+const btVector3& btReducedDeformableBody::getAngularVelocity() const
+{
+  return m_angularVelocity;
+}
+
+void btReducedDeformableBody::disableReducedModes(const bool rigid_only)
+{
+  m_rigidOnly = rigid_only;
+}
+
+bool btReducedDeformableBody::isReducedModesOFF() const
+{
+  return m_rigidOnly;
+}

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBody.h

@@ -0,0 +1,257 @@
+#ifndef BT_REDUCED_SOFT_BODY_H
+#define BT_REDUCED_SOFT_BODY_H
+
+#include "../btSoftBody.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btMatrix3x3.h"
+#include "LinearMath/btTransform.h"
+
+// Reduced deformable body is a simplified deformable object embedded in a rigid frame.
+class btReducedDeformableBody : public btSoftBody
+{
+ public:
+  //
+  //  Typedefs
+  //
+  typedef btAlignedObjectArray<btVector3> TVStack;
+  // typedef btAlignedObjectArray<btMatrix3x3> tBlockDiagMatrix;
+  typedef btAlignedObjectArray<btScalar> tDenseArray;
+  typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
+
+ private:
+  // flag to turn off the reduced modes
+  bool m_rigidOnly;
+
+  // Flags for transform. Once transform is applied, users cannot scale the mesh or change its total mass.
+  bool m_transform_lock;
+
+  // scaling factors
+  btScalar m_rhoScale;         // mass density scale
+  btScalar m_ksScale;          // stiffness scale
+
+  // projection matrix
+  tDenseMatrix m_projPA;        // Eqn. 4.11 from Rahul Sheth's thesis
+  tDenseMatrix m_projCq;
+  tDenseArray m_STP;
+  tDenseArray m_MrInvSTP;
+
+  TVStack m_localMomentArm; // Sq + x0
+
+  btVector3 m_internalDeltaLinearVelocity;
+  btVector3 m_internalDeltaAngularVelocity;
+  tDenseArray m_internalDeltaReducedVelocity;
+  
+  btVector3 m_linearVelocityFromReduced;  // contribution to the linear velocity from reduced velocity
+  btVector3 m_angularVelocityFromReduced; // contribution to the angular velocity from reduced velocity
+  btVector3 m_internalDeltaAngularVelocityFromReduced;
+
+ protected:
+  // rigid frame
+  btScalar m_mass;          // total mass of the rigid frame
+  btScalar m_inverseMass;   // inverse of the total mass of the rigid frame
+  btVector3 m_linearVelocity;
+  btVector3 m_angularVelocity;
+  btScalar m_linearDamping;    // linear damping coefficient
+  btScalar m_angularDamping;    // angular damping coefficient
+  btVector3 m_linearFactor;
+  btVector3 m_angularFactor;
+  // btVector3 m_invInertiaLocal;
+  btMatrix3x3 m_invInertiaLocal;
+  btTransform m_rigidTransformWorld;
+  btMatrix3x3 m_invInertiaTensorWorldInitial;
+  btMatrix3x3 m_invInertiaTensorWorld;
+  btMatrix3x3 m_interpolateInvInertiaTensorWorld;
+  btVector3 m_initialCoM;  // initial center of mass (original of the m_rigidTransformWorld)
+
+  // damping
+  btScalar m_dampingAlpha;
+  btScalar m_dampingBeta;
+
+ public:
+  //
+  //  Fields
+  //
+
+  // reduced space
+  int m_nReduced;
+  int m_nFull;
+  tDenseMatrix m_modes;														// modes of the reduced deformable model. Each inner array is a mode, outer array size = n_modes
+  tDenseArray m_reducedDofs;				   // Reduced degree of freedom
+  tDenseArray m_reducedDofsBuffer;     // Reduced degree of freedom at t^n
+  tDenseArray m_reducedVelocity;		   // Reduced velocity array
+  tDenseArray m_reducedVelocityBuffer; // Reduced velocity array at t^n
+  tDenseArray m_reducedForceExternal;          // reduced external force
+  tDenseArray m_reducedForceElastic;           // reduced internal elastic force
+  tDenseArray m_reducedForceDamping;           // reduced internal damping force
+  tDenseArray m_eigenvalues;		// eigenvalues of the reduce deformable model
+  tDenseArray m_Kr;	// reduced stiffness matrix
+  
+  // full space
+  TVStack m_x0;					     				 // Rest position
+  tDenseArray m_nodalMass;           // Mass on each node
+  btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes
+  int m_nodeIndexOffset;             // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world.
+
+  // contacts
+  btAlignedObjectArray<int> m_contactNodesList;
+
+  //
+  // Api
+  //
+  btReducedDeformableBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m);
+
+  ~btReducedDeformableBody() {}
+
+  //
+  // initializing helpers
+  //
+  void internalInitialization();
+
+  void setReducedModes(int num_modes, int full_size);
+
+  void setMassProps(const tDenseArray& mass_array);
+
+  void setInertiaProps();
+
+  void setRigidVelocity(const btVector3& v);
+
+  void setRigidAngularVelocity(const btVector3& omega);
+
+  void setStiffnessScale(const btScalar ks);
+
+  void setMassScale(const btScalar rho);
+
+  void setFixedNodes(const int n_node);
+
+  void setDamping(const btScalar alpha, const btScalar beta);
+
+  void disableReducedModes(const bool rigid_only);
+
+  virtual void setTotalMass(btScalar mass, bool fromfaces = false);
+
+  //
+  // various internal updates
+  //
+  virtual void transformTo(const btTransform& trs);
+  virtual void transform(const btTransform& trs);
+  // caution: 
+  // need to use scale before using transform, because the scale is performed in the local frame 
+  // (i.e., may have some rotation already, but the m_rigidTransformWorld doesn't have this info)
+  virtual void scale(const btVector3& scl);
+
+ private:
+  void updateRestNodalPositions();
+
+  void updateInitialInertiaTensor(const btMatrix3x3& rotation);
+
+  void updateLocalInertiaTensorFromNodes();
+
+  void updateInertiaTensor();
+
+  void updateModesByRotation(const btMatrix3x3& rotation);
+ 
+ public:
+  void updateLocalMomentArm();
+
+  void predictIntegratedTransform(btScalar dt, btTransform& predictedTransform);
+
+  // update the external force projection matrix 
+  void updateExternalForceProjectMatrix(bool initialized);
+
+  void endOfTimeStepZeroing();
+
+  void applyInternalVelocityChanges();
+
+  //
+  // position and velocity update related
+  //
+
+  // compute reduced degree of freedoms
+  void updateReducedDofs(btScalar solverdt);
+
+  // compute reduced velocity update (for explicit time stepping)
+  void updateReducedVelocity(btScalar solverdt);
+
+  // map to full degree of freedoms
+  void mapToFullPosition(const btTransform& ref_trans);
+
+  // compute full space velocity from the reduced velocity
+  void mapToFullVelocity(const btTransform& ref_trans);
+
+  // compute total angular momentum
+  const btVector3 computeTotalAngularMomentum() const;
+
+  // get a single node's full space velocity from the reduced velocity
+  const btVector3 computeNodeFullVelocity(const btTransform& ref_trans, int n_node) const;
+
+  // get a single node's all delta velocity
+  const btVector3 internalComputeNodeDeltaVelocity(const btTransform& ref_trans, int n_node) const;
+
+  //
+  // rigid motion related
+  //
+  void applyDamping(btScalar timeStep);
+
+  void applyCentralImpulse(const btVector3& impulse);
+
+  void applyTorqueImpulse(const btVector3& torque);
+
+  void proceedToTransform(btScalar dt, bool end_of_time_step);
+
+  //
+  // force related
+  //
+
+  // apply impulse to the rigid frame
+  void internalApplyRigidImpulse(const btVector3& impulse, const btVector3& rel_pos);
+
+  // apply impulse to nodes in the full space
+  void internalApplyFullSpaceImpulse(const btVector3& impulse, const btVector3& rel_pos, int n_node, btScalar dt);
+
+  // apply nodal external force in the full space
+  void applyFullSpaceNodalForce(const btVector3& f_ext, int n_node);
+
+  // apply gravity to the rigid frame
+  void applyRigidGravity(const btVector3& gravity, btScalar dt);
+
+  // apply reduced elastic force
+  void applyReducedElasticForce(const tDenseArray& reduce_dofs);
+
+  // apply reduced damping force
+  void applyReducedDampingForce(const tDenseArray& reduce_vel);
+
+  // calculate the impulse factor
+  virtual btMatrix3x3 getImpulseFactor(int n_node);
+
+  // get relative position from a node to the CoM of the rigid frame
+  btVector3 getRelativePos(int n_node);
+
+  //
+  // accessors
+  //
+  bool isReducedModesOFF() const;
+  btScalar getTotalMass() const;
+  btTransform& getRigidTransform();
+  const btVector3& getLinearVelocity() const;
+	const btVector3& getAngularVelocity() const;
+
+  #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
+  void clampVelocity(btVector3& v) const {
+      v.setX(
+          fmax(-BT_CLAMP_VELOCITY_TO,
+                fmin(BT_CLAMP_VELOCITY_TO, v.getX()))
+      );
+      v.setY(
+          fmax(-BT_CLAMP_VELOCITY_TO,
+                fmin(BT_CLAMP_VELOCITY_TO, v.getY()))
+      );
+      v.setZ(
+          fmax(-BT_CLAMP_VELOCITY_TO,
+                fmin(BT_CLAMP_VELOCITY_TO, v.getZ()))
+      );
+  }
+  #endif
+};
+
+#endif // BT_REDUCED_SOFT_BODY_H

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp

@@ -0,0 +1,215 @@
+#include "btReducedDeformableBodyHelpers.h"
+#include "../btSoftBodyHelpers.h"
+#include <iostream>
+#include <string>
+#include <sstream>
+
+btReducedDeformableBody* btReducedDeformableBodyHelpers::createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only) {
+	std::string filename = file_path + vtk_file;
+	btReducedDeformableBody* rsb = btReducedDeformableBodyHelpers::createFromVtkFile(worldInfo, filename.c_str());
+	
+	rsb->setReducedModes(num_modes, rsb->m_nodes.size());
+	btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(rsb, file_path.c_str());
+	
+	rsb->disableReducedModes(rigid_only);
+
+	return rsb;
+}
+
+btReducedDeformableBody* btReducedDeformableBodyHelpers::createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file)
+{
+	std::ifstream fs;
+	fs.open(vtk_file);
+	btAssert(fs);
+
+	typedef btAlignedObjectArray<int> Index;
+	std::string line;
+	btAlignedObjectArray<btVector3> X;
+	btVector3 position;
+	btAlignedObjectArray<Index> indices;
+	bool reading_points = false;
+	bool reading_tets = false;
+	size_t n_points = 0;
+	size_t n_tets = 0;
+	size_t x_count = 0;
+	size_t indices_count = 0;
+	while (std::getline(fs, line))
+	{
+		std::stringstream ss(line);
+		if (line.size() == (size_t)(0))
+		{
+		}
+		else if (line.substr(0, 6) == "POINTS")
+		{
+			reading_points = true;
+			reading_tets = false;
+			ss.ignore(128, ' ');  // ignore "POINTS"
+			ss >> n_points;
+			X.resize(n_points);
+		}
+		else if (line.substr(0, 5) == "CELLS")
+		{
+			reading_points = false;
+			reading_tets = true;
+			ss.ignore(128, ' ');  // ignore "CELLS"
+			ss >> n_tets;
+			indices.resize(n_tets);
+		}
+		else if (line.substr(0, 10) == "CELL_TYPES")
+		{
+			reading_points = false;
+			reading_tets = false;
+		}
+		else if (reading_points)
+		{
+			btScalar p;
+			ss >> p;
+			position.setX(p);
+			ss >> p;
+			position.setY(p);
+			ss >> p;
+			position.setZ(p);
+			//printf("v %f %f %f\n", position.getX(), position.getY(), position.getZ());
+			X[x_count++] = position;
+		}
+		else if (reading_tets)
+		{
+			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++)
+			{
+				ss >> tet[i];
+				//printf("%d ", tet[i]);
+			}
+			//printf("\n");
+			indices[indices_count++] = tet;
+		}
+	}
+	btReducedDeformableBody* rsb = new btReducedDeformableBody(&worldInfo, n_points, &X[0], 0);
+
+	for (int i = 0; i < n_tets; ++i)
+	{
+		const Index& ni = indices[i];
+		rsb->appendTetra(ni[0], ni[1], ni[2], ni[3]);
+		{
+			rsb->appendLink(ni[0], ni[1], 0, true);
+			rsb->appendLink(ni[1], ni[2], 0, true);
+			rsb->appendLink(ni[2], ni[0], 0, true);
+			rsb->appendLink(ni[0], ni[3], 0, true);
+			rsb->appendLink(ni[1], ni[3], 0, true);
+			rsb->appendLink(ni[2], ni[3], 0, true);
+		}
+	}
+
+	btSoftBodyHelpers::generateBoundaryFaces(rsb);
+	rsb->initializeDmInverse();
+	rsb->m_tetraScratches.resize(rsb->m_tetras.size());
+	rsb->m_tetraScratchesTn.resize(rsb->m_tetras.size());
+	printf("Nodes:  %u\r\n", rsb->m_nodes.size());
+	printf("Links:  %u\r\n", rsb->m_links.size());
+	printf("Faces:  %u\r\n", rsb->m_faces.size());
+	printf("Tetras: %u\r\n", rsb->m_tetras.size());
+
+	fs.close();
+
+	return rsb;
+}
+
+void btReducedDeformableBodyHelpers::readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path)
+{
+	// read in eigenmodes, stiffness and mass matrices
+	std::string eigenvalues_file = std::string(file_path) + "eigenvalues.bin";
+	btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_eigenvalues, rsb->m_nReduced, eigenvalues_file.c_str());
+
+	std::string Kr_file = std::string(file_path) + "K_r_diag_mat.bin";
+	btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Kr,  rsb->m_nReduced, Kr_file.c_str());
+
+	// std::string Mr_file = std::string(file_path) + "M_r_diag_mat.bin";
+	// btReducedDeformableBodyHelpers::readBinaryVec(rsb->m_Mr, rsb->m_nReduced, Mr_file.c_str());
+
+	std::string modes_file = std::string(file_path) + "modes.bin";
+	btReducedDeformableBodyHelpers::readBinaryMat(rsb->m_modes, rsb->m_nReduced, 3 * rsb->m_nFull, modes_file.c_str());
+	
+	// read in full nodal mass
+	std::string M_file = std::string(file_path) + "M_diag_mat.bin";
+	btAlignedObjectArray<btScalar> mass_array;
+	btReducedDeformableBodyHelpers::readBinaryVec(mass_array, rsb->m_nFull, M_file.c_str());
+	rsb->setMassProps(mass_array);
+	
+	// calculate the inertia tensor in the local frame 
+ 	rsb->setInertiaProps();
+
+	// other internal initialization
+	rsb->internalInitialization();
+}
+
+// read in a vector from the binary file
+void btReducedDeformableBodyHelpers::readBinaryVec(btReducedDeformableBody::tDenseArray& vec, 
+																				  	 const unsigned int n_size, 				// #entries read
+																						 const char* file)
+{
+	std::ifstream f_in(file, std::ios::in | std::ios::binary);
+	// first get size
+	unsigned int size;
+	f_in.read((char*)&size, sizeof(uint32_t));
+	btAssert(size >= n_size); 	// make sure the #requested mode is smaller than the #available modes
+
+	// read data
+	vec.resize(n_size);
+	double temp;
+	for (unsigned int i = 0; i < n_size; ++i)
+	{
+		f_in.read((char*)&temp, sizeof(double));
+		vec[i] = btScalar(temp);
+	}
+  f_in.close();
+}
+
+// read in a matrix from the binary file
+void btReducedDeformableBodyHelpers::readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, 
+																						 const unsigned int n_modes, 		// #modes, outer array size
+																						 const unsigned int n_full, 		// inner array size
+																						 const char* file)
+{
+	std::ifstream f_in(file, std::ios::in | std::ios::binary);
+	// first get size
+	unsigned int v_size;
+	f_in.read((char*)&v_size, sizeof(uint32_t));
+	btAssert(v_size >= n_modes * n_full); 	// make sure the #requested mode is smaller than the #available modes
+
+	// read data
+	mat.resize(n_modes);
+	for (int i = 0; i < n_modes; ++i) 
+	{
+		for (int j = 0; j < n_full; ++j)
+		{
+			double temp;
+			f_in.read((char*)&temp, sizeof(double));
+
+			if (mat[i].size() != n_modes)
+				mat[i].resize(n_full);
+			mat[i][j] = btScalar(temp);
+		}
+	}
+  f_in.close();
+}
+
+void btReducedDeformableBodyHelpers::calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin)
+{
+	btScalar lx = btScalar(2.) * (half_extents[0] + margin[0]);
+	btScalar ly = btScalar(2.) * (half_extents[1] + margin[1]);
+	btScalar lz = btScalar(2.) * (half_extents[2] + margin[2]);
+
+	inertia.setValue(mass / (btScalar(12.0)) * (ly * ly + lz * lz),
+								   mass / (btScalar(12.0)) * (lx * lx + lz * lz),
+								   mass / (btScalar(12.0)) * (lx * lx + ly * ly));
+}

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h

@@ -0,0 +1,25 @@
+#ifndef BT_REDUCED_SOFT_BODY_HELPERS_H
+#define BT_REDUCED_SOFT_BODY_HELPERS_H
+
+#include "btReducedDeformableBody.h"
+#include <string>
+
+struct btReducedDeformableBodyHelpers
+{
+	// create a reduced deformable object
+	static btReducedDeformableBody* createReducedDeformableObject(btSoftBodyWorldInfo& worldInfo, const std::string& file_path, const std::string& vtk_file, const int num_modes, bool rigid_only);
+	// read in geometry info from Vtk file
+  static btReducedDeformableBody* createFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
+	// read in all reduced files
+	static void readReducedDeformableInfoFromFiles(btReducedDeformableBody* rsb, const char* file_path);
+	// read in a binary vector
+	static void readBinaryVec(btReducedDeformableBody::tDenseArray& vec, const unsigned int n_size, const char* file);
+	// read in a binary matrix
+	static void readBinaryMat(btReducedDeformableBody::tDenseMatrix& mat, const unsigned int n_modes, const unsigned int n_full, const char* file);
+	
+	// calculate the local inertia tensor for a box shape reduced deformable object
+	static void calculateLocalInertia(btVector3& inertia, const btScalar mass, const btVector3& half_extents, const btVector3& margin);
+};
+
+
+#endif // BT_REDUCED_SOFT_BODY_HELPERS_H

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp

@@ -0,0 +1,325 @@
+#include "btReducedDeformableBodySolver.h"
+#include "btReducedDeformableBody.h"
+
+btReducedDeformableBodySolver::btReducedDeformableBodySolver()
+{
+  m_ascendOrder = true;
+  m_reducedSolver = true;
+  m_dampingAlpha = 0;
+  m_dampingBeta = 0;
+  m_gravity = btVector3(0, 0, 0);
+}
+
+void btReducedDeformableBodySolver::setGravity(const btVector3& gravity)
+{
+  m_gravity = gravity;
+}
+
+void btReducedDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt)
+{
+  m_softBodies.copyFromArray(bodies);
+	bool nodeUpdated = updateNodes();
+
+	if (nodeUpdated)
+	{
+		m_dv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_ddv.resize(m_numNodes, btVector3(0, 0, 0));
+		m_residual.resize(m_numNodes, btVector3(0, 0, 0));
+		m_backupVelocity.resize(m_numNodes, btVector3(0, 0, 0));
+	}
+
+	// need to setZero here as resize only set value for newly allocated items
+	for (int i = 0; i < m_numNodes; ++i)
+	{
+		m_dv[i].setZero();
+		m_ddv[i].setZero();
+		m_residual[i].setZero();
+	}
+
+	if (dt > 0)
+	{
+		m_dt = dt;
+	}
+	m_objective->reinitialize(nodeUpdated, dt);
+
+  int N = bodies.size();
+	if (nodeUpdated)
+	{
+		m_staticConstraints.resize(N);
+		m_nodeRigidConstraints.resize(N);
+		// m_faceRigidConstraints.resize(N);
+	}
+	for (int i = 0; i < N; ++i)
+	{
+		m_staticConstraints[i].clear();
+		m_nodeRigidConstraints[i].clear();
+		// m_faceRigidConstraints[i].clear();
+	}
+
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    rsb->m_contactNodesList.clear();
+  }
+
+  // set node index offsets
+  int sum = 0;
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    rsb->m_nodeIndexOffset = sum;
+    sum += rsb->m_nodes.size();
+  }
+
+	btDeformableBodySolver::updateSoftBodies();
+}
+
+void btReducedDeformableBodySolver::predictMotion(btScalar solverdt)
+{
+  applyExplicitForce(solverdt);
+
+  // predict new mesh location
+  predictReduceDeformableMotion(solverdt);
+
+  //TODO: check if there is anything missed from btDeformableBodySolver::predictDeformableMotion
+}
+
+void btReducedDeformableBodySolver::predictReduceDeformableMotion(btScalar solverdt)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    if (!rsb->isActive())
+    {
+      continue;
+    }
+
+    // clear contacts variables
+		rsb->m_nodeRigidContacts.resize(0);
+		rsb->m_faceRigidContacts.resize(0);
+		rsb->m_faceNodeContacts.resize(0);
+    
+    // calculate inverse mass matrix for all nodes
+    for (int j = 0; j < rsb->m_nodes.size(); ++j)
+    {
+      if (rsb->m_nodes[j].m_im > 0)
+      {
+        rsb->m_nodes[j].m_effectiveMass_inv = rsb->m_nodes[j].m_effectiveMass.inverse();
+      }
+    }
+
+    // rigid motion: t, R at time^*
+    rsb->predictIntegratedTransform(solverdt, rsb->getInterpolationWorldTransform());
+
+    // update reduced dofs at time^*
+    // rsb->updateReducedDofs(solverdt);
+
+    // update local moment arm at time^*
+    // rsb->updateLocalMomentArm();
+    // rsb->updateExternalForceProjectMatrix(true);
+
+    // predict full space velocity at time^* (needed for constraints)
+    rsb->mapToFullVelocity(rsb->getInterpolationWorldTransform());
+
+    // update full space nodal position at time^*
+    rsb->mapToFullPosition(rsb->getInterpolationWorldTransform());
+
+    // update bounding box
+    rsb->updateBounds();
+
+    // update tree
+    rsb->updateNodeTree(true, true);
+    if (!rsb->m_fdbvt.empty())
+    {
+      rsb->updateFaceTree(true, true);
+    }
+  }
+}
+
+void btReducedDeformableBodySolver::applyExplicitForce(btScalar solverdt)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+
+    // apply gravity to the rigid frame, get m_linearVelocity at time^*
+    rsb->applyRigidGravity(m_gravity, solverdt);
+
+    if (!rsb->isReducedModesOFF())
+    {
+      // add internal force (elastic force & damping force)
+      rsb->applyReducedElasticForce(rsb->m_reducedDofsBuffer);
+      rsb->applyReducedDampingForce(rsb->m_reducedVelocityBuffer);
+
+      // get reduced velocity at time^* 
+      rsb->updateReducedVelocity(solverdt);
+    }
+
+    // apply damping (no need at this point)
+    // rsb->applyDamping(solverdt);
+  }
+}
+
+void btReducedDeformableBodySolver::applyTransforms(btScalar timeStep)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+
+    // rigid motion
+    rsb->proceedToTransform(timeStep, true);
+
+    if (!rsb->isReducedModesOFF())
+    {
+      // update reduced dofs for time^n+1
+      rsb->updateReducedDofs(timeStep);
+
+      // update local moment arm for time^n+1
+      rsb->updateLocalMomentArm();
+      rsb->updateExternalForceProjectMatrix(true);
+    }
+
+    // update mesh nodal positions for time^n+1
+    rsb->mapToFullPosition(rsb->getRigidTransform());
+
+    // update mesh nodal velocity
+    rsb->mapToFullVelocity(rsb->getRigidTransform());
+
+    // end of time step clean up and update
+    rsb->endOfTimeStepZeroing();
+
+    // update the rendering mesh
+    rsb->interpolateRenderMesh();
+  }
+}
+
+void btReducedDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
+{
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    if (!rsb->isActive())
+		{
+			continue;
+		}
+
+    // set fixed constraints
+    for (int j = 0; j < rsb->m_fixedNodes.size(); ++j)
+		{
+      int i_node = rsb->m_fixedNodes[j];
+			if (rsb->m_nodes[i_node].m_im == 0)
+			{
+        for (int k = 0; k < 3; ++k)
+        {
+          btVector3 dir(0, 0, 0);
+          dir[k] = 1;
+          btReducedDeformableStaticConstraint static_constraint(rsb, &rsb->m_nodes[i_node], rsb->getRelativePos(i_node), rsb->m_x0[i_node], dir, infoGlobal, m_dt);
+          m_staticConstraints[i].push_back(static_constraint);
+        }
+			}
+		}
+    btAssert(rsb->m_fixedNodes.size() * 3 == m_staticConstraints[i].size());
+
+    // set Deformable Node vs. Rigid constraint
+		for (int j = 0; j < rsb->m_nodeRigidContacts.size(); ++j)
+		{
+			const btSoftBody::DeformableNodeRigidContact& contact = rsb->m_nodeRigidContacts[j];
+			// skip fixed points
+			if (contact.m_node->m_im == 0)
+			{
+				continue;
+			}
+			btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
+			m_nodeRigidConstraints[i].push_back(constraint);
+      rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
+		}
+    // std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
+    // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
+
+  }
+}
+
+btScalar btReducedDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+  btScalar residualSquare = 0;
+
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btAlignedObjectArray<int> m_orderNonContactConstraintPool;
+    btAlignedObjectArray<int> m_orderContactConstraintPool;
+
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+
+    // shuffle the order of applying constraint
+    m_orderNonContactConstraintPool.resize(m_staticConstraints[i].size());
+    m_orderContactConstraintPool.resize(m_nodeRigidConstraints[i].size());
+    if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
+    {
+      // fixed constraint order
+      for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+      {
+        m_orderNonContactConstraintPool[j] = m_ascendOrder ? j : m_staticConstraints[i].size() - 1 - j;
+      }
+      // contact constraint order
+      for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+      {
+        m_orderContactConstraintPool[j] = m_ascendOrder ? j : m_nodeRigidConstraints[i].size() - 1 - j;
+      }
+
+      m_ascendOrder = m_ascendOrder ? false : true;
+    }
+    else
+    {
+      for (int j = 0; j < m_staticConstraints[i].size(); ++j)
+      {
+        m_orderNonContactConstraintPool[j] = j;
+      }
+      // contact constraint order
+      for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
+      {
+        m_orderContactConstraintPool[j] = j;
+      }
+    }
+
+    // handle fixed constraint
+    for (int k = 0; k < m_staticConstraints[i].size(); ++k)
+    {
+      btReducedDeformableStaticConstraint& constraint = m_staticConstraints[i][m_orderNonContactConstraintPool[k]];
+      btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+      residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+
+    // handle contact constraint
+
+    // node vs rigid contact
+    // std::cout << "!!#contact_nodes: " << m_nodeRigidConstraints[i].size() << '\n';
+    for (int k = 0; k < m_nodeRigidConstraints[i].size(); ++k)
+    {
+      btReducedDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[i][m_orderContactConstraintPool[k]];
+      btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+      residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+
+    // face vs rigid contact
+    // for (int k = 0; k < m_faceRigidConstraints[i].size(); ++k)
+    // {
+    // 	btReducedDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[i][k];
+    // 	btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
+    // 	residualSquare = btMax(residualSquare, localResidualSquare);
+    // }
+  }
+
+  
+	return residualSquare;
+}
+
+void btReducedDeformableBodySolver::deformableBodyInternalWriteBack()
+{
+  // reduced deformable update
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedDeformableBody* rsb = static_cast<btReducedDeformableBody*>(m_softBodies[i]);
+    rsb->applyInternalVelocityChanges();
+  }
+  m_ascendOrder = true;
+}

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableBodySolver.h

@@ -0,0 +1,61 @@
+#ifndef BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
+#define BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H
+
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "btReducedDeformableContactConstraint.h"
+
+class btReducedDeformableBody;
+
+class btReducedDeformableBodySolver : public btDeformableBodySolver
+{
+ protected:
+  bool m_ascendOrder;
+  btScalar m_dampingAlpha;
+  btScalar m_dampingBeta;
+
+  btVector3 m_gravity;
+
+  void predictReduceDeformableMotion(btScalar solverdt);
+
+  void applyExplicitForce(btScalar solverdt);
+
+ public:
+  btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableStaticConstraint> > m_staticConstraints;
+  btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
+  btAlignedObjectArray<btAlignedObjectArray<btReducedDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
+  
+  btReducedDeformableBodySolver();
+  ~btReducedDeformableBodySolver() {}
+
+  virtual void setGravity(const btVector3& gravity);
+
+  virtual SolverTypes getSolverType() const
+  {
+    return REDUCED_DEFORMABLE_SOLVER;
+  }
+
+  // resize/clear data structures
+	virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& bodies, btScalar dt);
+
+  virtual void predictMotion(btScalar solverdt);
+
+  virtual void applyTransforms(btScalar timeStep);
+
+  // set up contact constraints
+	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
+
+  // solve all constraints (fixed and contact)
+  virtual btScalar solveContactConstraints(btCollisionObject** deformableBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
+  // apply all the delta velocities
+  virtual void deformableBodyInternalWriteBack();
+
+  // virtual void setProjection() {}
+
+  // virtual void setLagrangeMultiplier() {}
+
+  // virtual void setupDeformableSolve(bool implicit);
+
+};
+
+#endif // BT_REDUCED_DEFORMABLE_BODY_DYNAMICS_WORLD_H

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp

@@ -0,0 +1,579 @@
+#include "btReducedDeformableContactConstraint.h"
+#include <iostream>
+
+// ================= static constraints ===================
+btReducedDeformableStaticConstraint::btReducedDeformableStaticConstraint(
+  btReducedDeformableBody* rsb, 
+  btSoftBody::Node* node,
+	const btVector3& ri,
+	const btVector3& x0,
+	const btVector3& dir,
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt)
+  : m_rsb(rsb), m_ri(ri), m_targetPos(x0), m_impulseDirection(dir), m_dt(dt), btDeformableStaticConstraint(node, infoGlobal)
+{
+	m_erp = 0.2;
+	m_appliedImpulse = 0;
+
+	// get impulse factor
+  m_impulseFactorMatrix = rsb->getImpulseFactor(m_node->index);
+	m_impulseFactor = (m_impulseFactorMatrix * m_impulseDirection).dot(m_impulseDirection);
+
+	btScalar vel_error = btDot(-m_node->m_v, m_impulseDirection);
+	btScalar pos_error = btDot(m_targetPos - m_node->m_x, m_impulseDirection);
+
+	m_rhs = (vel_error + m_erp * pos_error / m_dt) / m_impulseFactor;
+}
+
+btScalar btReducedDeformableStaticConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
+{
+	// target velocity of fixed constraint is 0
+	btVector3 deltaVa = getDeltaVa();
+	btScalar deltaV_rel = btDot(deltaVa, m_impulseDirection);
+  btScalar deltaImpulse = m_rhs - deltaV_rel / m_impulseFactor;
+	m_appliedImpulse = m_appliedImpulse + deltaImpulse;
+
+	btVector3 impulse = deltaImpulse * m_impulseDirection;
+	applyImpulse(impulse);
+
+	// calculate residual
+	btScalar residualSquare = m_impulseFactor * deltaImpulse;
+	residualSquare *= residualSquare;
+
+	return residualSquare;
+}
+  
+// this calls reduced deformable body's internalApplyFullSpaceImpulse
+void btReducedDeformableStaticConstraint::applyImpulse(const btVector3& impulse)
+{
+	// apply full space impulse
+	m_rsb->internalApplyFullSpaceImpulse(impulse, m_ri, m_node->index, m_dt);
+}
+
+btVector3 btReducedDeformableStaticConstraint::getDeltaVa() const
+{
+	return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
+}
+
+// ================= base contact constraints ===================
+btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstraint(
+  btReducedDeformableBody* rsb, 
+  const btSoftBody::DeformableRigidContact& c, 
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt)
+  : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
+{
+	m_nodeQueryIndex = 0;
+	m_appliedNormalImpulse = 0;
+  m_appliedTangentImpulse = 0;
+	m_rhs = 0;
+	m_rhs_tangent = 0;
+	m_cfm = infoGlobal.m_deformable_cfm;
+	m_cfm_friction = 0;
+	m_erp = infoGlobal.m_deformable_erp;
+	m_erp_friction = infoGlobal.m_deformable_erp;
+	m_friction = infoGlobal.m_friction;
+
+	m_collideStatic = m_contact->m_cti.m_colObj->isStaticObject();
+	m_collideMultibody = (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK);
+}
+
+void btReducedDeformableRigidContactConstraint::setSolverBody(const int bodyId, btSolverBody& solver_body)
+{
+	if (!m_collideMultibody)
+	{
+		m_solverBodyId = bodyId;
+		m_solverBody = &solver_body;
+		m_linearComponentNormal = -m_contactNormalA * m_solverBody->internalGetInvMass();
+		btVector3	torqueAxis = -m_relPosA.cross(m_contactNormalA);
+		m_angularComponentNormal = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxis;
+		
+		m_linearComponentTangent = m_contactTangent * m_solverBody->internalGetInvMass();
+		btVector3 torqueAxisTangent = m_relPosA.cross(m_contactTangent);
+		m_angularComponentTangent = m_solverBody->m_originalBody->getInvInertiaTensorWorld() * torqueAxisTangent;
+	}
+}
+
+btVector3 btReducedDeformableRigidContactConstraint::getVa() const
+{
+	btVector3 Va(0, 0, 0);
+	if (!m_collideStatic)
+	{
+		Va = btDeformableRigidContactConstraint::getVa();
+	}
+	return Va;
+}
+
+btScalar btReducedDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
+{
+	// btVector3 Va = getVa();
+	// btVector3 deltaVa = Va - m_bufferVelocityA;
+	// if (!m_collideStatic)
+	// {
+		// std::cout << "moving collision!!!\n";
+		// std::cout << "relPosA: " << m_relPosA[0] << "\t" << m_relPosA[1] << "\t" << m_relPosA[2] << "\n";
+		// std::cout << "moving rigid linear_vel: " << m_solverBody->m_originalBody->getLinearVelocity()[0] << '\t'
+		//  << m_solverBody->m_originalBody->getLinearVelocity()[1] << '\t'
+		//   << m_solverBody->m_originalBody->getLinearVelocity()[2] << '\n';
+	// }
+	btVector3 deltaVa = getDeltaVa();
+	btVector3 deltaVb = getDeltaVb();
+
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "deltaVa: " << deltaVa[0] << '\t' << deltaVa[1] << '\t' << deltaVa[2] << '\n';
+	// 	std::cout << "deltaVb: " << deltaVb[0] << '\t' << deltaVb[1] << '\t' << deltaVb[2] << '\n';
+	// }
+
+	// get delta relative velocity and magnitude (i.e., how much impulse has been applied?)
+	btVector3 deltaV_rel = deltaVa - deltaVb;
+	btScalar deltaV_rel_normal = -btDot(deltaV_rel, m_contactNormalA);
+
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "deltaV_rel: " << deltaV_rel[0] << '\t' << deltaV_rel[1] << '\t' << deltaV_rel[2] << "\n";
+	// 	std::cout << "deltaV_rel_normal: " << deltaV_rel_normal << "\n";
+	// 	std::cout << "normal_A: " << m_contactNormalA[0] << '\t' << m_contactNormalA[1] << '\t' << m_contactNormalA[2] << '\n';
+	// }
+	
+	// get the normal impulse to be applied
+	btScalar deltaImpulse = m_rhs - m_appliedNormalImpulse * m_cfm - deltaV_rel_normal / m_normalImpulseFactor;
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "m_rhs: " << m_rhs << '\t' << "m_appliedNormalImpulse: "  << m_appliedNormalImpulse << "\n";
+	// 	std::cout << "m_normalImpulseFactor: " << m_normalImpulseFactor << '\n';
+	// }
+
+	{
+		// cumulative impulse that has been applied
+		btScalar sum = m_appliedNormalImpulse + deltaImpulse;
+		// if the cumulative impulse is pushing the object into the rigid body, set it zero
+		if (sum < 0)
+		{
+			deltaImpulse = -m_appliedNormalImpulse;
+			m_appliedNormalImpulse = 0;
+		}
+		else
+		{
+			m_appliedNormalImpulse = sum;
+		}	
+	}
+
+	// if (!m_collideStatic)
+	// {
+	// 	std::cout << "m_appliedNormalImpulse: " << m_appliedNormalImpulse << '\n';
+	// 	std::cout << "deltaImpulse: " << deltaImpulse << '\n';
+	// }
+
+	// residual is the nodal normal velocity change in current iteration
+	btScalar residualSquare = deltaImpulse * m_normalImpulseFactor;	// get residual
+	residualSquare *= residualSquare;
+
+	
+	// apply Coulomb friction (based on delta velocity, |dv_t| = |dv_n * friction|)
+	btScalar deltaImpulse_tangent = 0;
+	btScalar deltaImpulse_tangent2 = 0;
+	{
+		// calculate how much impulse is needed
+		// btScalar deltaV_rel_tangent = btDot(deltaV_rel, m_contactTangent);
+		// btScalar impulse_changed = deltaV_rel_tangent * m_tangentImpulseFactorInv;
+		// deltaImpulse_tangent = m_rhs_tangent - impulse_changed;
+
+		// btScalar sum = m_appliedTangentImpulse + deltaImpulse_tangent;
+		btScalar lower_limit = - m_appliedNormalImpulse * m_friction;
+		btScalar upper_limit = m_appliedNormalImpulse * m_friction;
+		// if (sum > upper_limit)
+		// {
+		// 	deltaImpulse_tangent = upper_limit - m_appliedTangentImpulse;
+		// 	m_appliedTangentImpulse = upper_limit;
+		// }
+		// else if (sum < lower_limit)
+		// {
+		// 	deltaImpulse_tangent = lower_limit - m_appliedTangentImpulse;
+		// 	m_appliedTangentImpulse = lower_limit;
+		// }
+		// else
+		// {
+		// 	m_appliedTangentImpulse = sum;
+		// }
+		// 
+		calculateTangentialImpulse(deltaImpulse_tangent, m_appliedTangentImpulse, m_rhs_tangent,
+															 m_tangentImpulseFactorInv, m_contactTangent, lower_limit, upper_limit, deltaV_rel);
+		
+		if (m_collideMultibody)
+		{
+			calculateTangentialImpulse(deltaImpulse_tangent2, m_appliedTangentImpulse2, m_rhs_tangent2,
+															   m_tangentImpulseFactorInv2, m_contactTangent2, lower_limit, upper_limit, deltaV_rel);
+		}
+															 
+
+		if (!m_collideStatic)
+		{
+			// std::cout << "m_contactTangent: " << m_contactTangent[0] << "\t"  << m_contactTangent[1] << "\t"  << m_contactTangent[2] << "\n";
+			// std::cout << "deltaV_rel_tangent: " << deltaV_rel_tangent << '\n';
+			// std::cout << "deltaImpulseTangent: " << deltaImpulse_tangent << '\n';
+			// std::cout << "m_appliedTangentImpulse: " << m_appliedTangentImpulse << '\n';
+		}
+	}
+
+	// get the total impulse vector
+	btVector3 impulse_normal = deltaImpulse * m_contactNormalA;
+	btVector3 impulse_tangent = deltaImpulse_tangent * (-m_contactTangent);
+	btVector3 impulse_tangent2 = deltaImpulse_tangent2 * (-m_contactTangent2);
+	btVector3 impulse = impulse_normal + impulse_tangent + impulse_tangent2;
+
+	applyImpulse(impulse);
+	
+	// apply impulse to the rigid/multibodies involved and change their velocities
+	if (!m_collideStatic)
+	{
+		// std::cout << "linear_component: " << m_linearComponentNormal[0] << '\t'
+		// 																	<< m_linearComponentNormal[1] << '\t'
+		// 																	<< m_linearComponentNormal[2] << '\n';
+		// std::cout << "angular_component: " << m_angularComponentNormal[0] << '\t'
+		// 																	<< m_angularComponentNormal[1] << '\t'
+		// 																	<< m_angularComponentNormal[2] << '\n';
+
+		if (!m_collideMultibody)		// collision with rigid body
+		{
+			// std::cout << "rigid impulse applied!!\n";
+			// std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
+			// std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
+
+			m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, deltaImpulse);
+			m_solverBody->internalApplyImpulse(m_linearComponentTangent, m_angularComponentTangent, deltaImpulse_tangent);
+
+			// std::cout << "after\n";
+			// std::cout << "rigid impulse applied!!\n";
+			// std::cout << "delta Linear: " << m_solverBody->getDeltaLinearVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaLinearVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaLinearVelocity()[2] << '\n';
+			// std::cout << "delta Angular: " << m_solverBody->getDeltaAngularVelocity()[0] << '\t'
+			// << m_solverBody->getDeltaAngularVelocity()[1] << '\t'
+			// 	<< m_solverBody->getDeltaAngularVelocity()[2] << '\n';
+		}
+		else		// collision with multibody
+		{
+			btMultiBodyLinkCollider* multibodyLinkCol = 0;
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+				// apply normal component of the impulse
+				multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, -deltaImpulse);
+				
+				// const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				// std::cout << "deltaV_normal: ";
+				// for (int i = 0; i < ndof; ++i)
+				// {
+				// 	std::cout << i << "\t" << deltaV_normal[i] << '\n';
+				// }
+
+				if (impulse_tangent.norm() > SIMD_EPSILON)
+				{
+					// apply tangential component of the impulse
+					const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+					multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, deltaImpulse_tangent);
+					const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+					multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, deltaImpulse_tangent2);
+				}
+			}
+		}
+	}
+	return residualSquare;
+}
+
+void btReducedDeformableRigidContactConstraint::calculateTangentialImpulse(btScalar& deltaImpulse_tangent, 
+																		 																			 btScalar& appliedImpulse, 
+																																					 const btScalar rhs_tangent,
+																																					 const btScalar tangentImpulseFactorInv,
+																																					 const btVector3& tangent,
+																		 																			 const btScalar lower_limit,
+																																					 const btScalar upper_limit,
+																																					 const btVector3& deltaV_rel)
+{
+	btScalar deltaV_rel_tangent = btDot(deltaV_rel, tangent);
+	btScalar impulse_changed = deltaV_rel_tangent * tangentImpulseFactorInv;
+	deltaImpulse_tangent = rhs_tangent - m_cfm_friction * appliedImpulse - impulse_changed;
+
+	btScalar sum = appliedImpulse + deltaImpulse_tangent;
+	if (sum > upper_limit)
+	{
+		deltaImpulse_tangent = upper_limit - appliedImpulse;
+		appliedImpulse = upper_limit;
+	}
+	else if (sum < lower_limit)
+	{
+		deltaImpulse_tangent = lower_limit - appliedImpulse;
+		appliedImpulse = lower_limit;
+	}
+	else
+	{
+		appliedImpulse = sum;
+	}
+}
+
+// ================= node vs rigid constraints ===================
+btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidContactConstraint(
+  btReducedDeformableBody* rsb, 
+  const btSoftBody::DeformableNodeRigidContact& contact, 
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt)
+  : m_node(contact.m_node), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
+{
+	m_contactNormalA = contact.m_cti.m_normal;
+  m_contactNormalB = -contact.m_cti.m_normal;
+
+	if (contact.m_node->index < rsb->m_nodes.size())
+	{
+		m_nodeQueryIndex = contact.m_node->index;
+	}
+	else
+	{
+		m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset;
+	}
+
+	if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	{
+		m_relPosA = contact.m_c1;
+	}
+	else
+	{
+		m_relPosA = btVector3(0,0,0);
+	}
+	m_relPosB = m_node->m_x - m_rsb->getRigidTransform().getOrigin();
+
+	if (m_collideStatic)		// colliding with static object, only consider reduced deformable body's impulse factor
+	{
+		m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex);
+	}
+	else		// colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors
+	{
+		m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0;
+	}
+
+	m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
+	m_tangentImpulseFactor = 0;
+
+	warmStarting();
+}
+
+void btReducedDeformableNodeRigidContactConstraint::warmStarting()
+{
+	btVector3 va = getVa();
+	btVector3 vb = getVb();
+	m_bufferVelocityA = va;
+	m_bufferVelocityB = vb;
+
+	// we define the (+) direction of errors to be the outward surface normal of the rigid object
+	btVector3 v_rel = vb - va;
+	// get tangent direction of the relative velocity
+	btVector3 v_tangent = v_rel - v_rel.dot(m_contactNormalA) * m_contactNormalA;
+	if (v_tangent.norm() < SIMD_EPSILON)
+	{
+		m_contactTangent = btVector3(0, 0, 0);
+		// tangent impulse factor
+		m_tangentImpulseFactor = 0;
+		m_tangentImpulseFactorInv = 0;
+	}
+	else
+	{
+		if (!m_collideMultibody)
+		{
+			m_contactTangent = v_tangent.normalized();
+			m_contactTangent2.setZero();
+			// tangent impulse factor 1
+			m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
+			m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
+			// tangent impulse factor 2
+			m_tangentImpulseFactor2 = 0;
+			m_tangentImpulseFactorInv2 = 0;
+		}
+		else	// multibody requires 2 contact directions
+		{
+			m_contactTangent = m_contact->t1;
+			m_contactTangent2 = m_contact->t2;
+
+			// tangent impulse factor 1
+			m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
+			m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
+			// tangent impulse factor 2
+			m_tangentImpulseFactor2 = (m_impulseFactor * m_contactTangent2).dot(m_contactTangent2);
+			m_tangentImpulseFactorInv2 = btScalar(1) / m_tangentImpulseFactor2;
+		}
+	}
+
+
+	// initial guess for normal impulse
+	{
+		btScalar velocity_error = btDot(v_rel, m_contactNormalA);	// magnitude of relative velocity
+		btScalar position_error = 0;
+		if (m_penetration > 0)
+		{
+			velocity_error += m_penetration / m_dt;
+		}
+		else
+		{
+			// add penetration correction vel
+			position_error = m_penetration * m_erp / m_dt;
+		}
+		// get the initial estimate of impulse magnitude to be applied
+		m_rhs = -(velocity_error + position_error) / m_normalImpulseFactor;
+	}
+
+	// initial guess for tangential impulse
+	{
+		btScalar velocity_error = btDot(v_rel, m_contactTangent);
+		m_rhs_tangent = velocity_error * m_tangentImpulseFactorInv;
+
+		if (m_collideMultibody)
+		{
+			btScalar velocity_error2 = btDot(v_rel, m_contactTangent2);
+			m_rhs_tangent2 = velocity_error2 * m_tangentImpulseFactorInv2;
+		}
+	}
+
+	// warm starting
+	// applyImpulse(m_rhs * m_contactNormalA);
+	// if (!m_collideStatic)
+	// {
+	// 	const btSoftBody::sCti& cti = m_contact->m_cti;
+	// 	if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+	// 	{
+	// 		m_solverBody->internalApplyImpulse(m_linearComponentNormal, m_angularComponentNormal, -m_rhs);
+	// 	}
+	// }
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getVb() const
+{
+	return m_node->m_v;
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
+{
+	btVector3 deltaVa(0, 0, 0);
+	if (!m_collideStatic)
+	{
+		if (!m_collideMultibody)		// for rigid body
+		{
+			deltaVa = m_solverBody->internalGetDeltaLinearVelocity() + m_solverBody->internalGetDeltaAngularVelocity().cross(m_relPosA);
+		}
+		else		// for multibody
+		{
+			btMultiBodyLinkCollider* multibodyLinkCol = 0;
+			multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(m_contact->m_cti.m_colObj);
+			if (multibodyLinkCol)
+			{
+				const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+				const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
+				const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+				const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+				const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+				// add in the normal component of the va
+				btScalar vel = 0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_dv[k] * J_n[k];
+				}
+				deltaVa = m_contact->m_cti.m_normal * vel;
+				
+				// add in the tangential components of the va
+				vel = 0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_dv[k] * J_t1[k];
+				}
+				deltaVa += m_contact->t1 * vel;
+
+				vel = 0;
+				for (int k = 0; k < ndof; ++k)
+				{
+					vel += local_dv[k] * J_t2[k];
+				}
+				deltaVa += m_contact->t2 * vel;
+			}
+		}
+	}
+	return deltaVa;
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
+{	
+	// std::cout << "node: " << m_node->index << '\n';
+	return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex);
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
+{
+	return m_node->m_splitv;
+}
+
+btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+	return m_total_normal_dv + m_total_tangent_dv;
+}
+
+void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+  m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt);
+	// m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
+	// m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
+	// if (!m_collideStatic)
+	// {
+	// 	// std::cout << "impulse applied: " << impulse[0] << '\t' << impulse[1] << '\t' << impulse[2] << '\n';
+	// 	// std::cout << "node: " << m_node->index << " vel: " << m_node->m_v[0] << '\t' << m_node->m_v[1] << '\t' << m_node->m_v[2] << '\n';
+	// 	btVector3 v_after = getDeltaVb() + m_node->m_v;
+	// 	// std::cout << "vel after: " << v_after[0] << '\t' << v_after[1] << '\t' << v_after[2] << '\n';
+	// }
+	// std::cout << "node: " << m_node->index << " pos: " << m_node->m_x[0] << '\t' << m_node->m_x[1] << '\t' << m_node->m_x[2] << '\n';
+}
+
+// ================= face vs rigid constraints ===================
+btReducedDeformableFaceRigidContactConstraint::btReducedDeformableFaceRigidContactConstraint(
+  btReducedDeformableBody* rsb, 
+  const btSoftBody::DeformableFaceRigidContact& contact, 
+  const btContactSolverInfo& infoGlobal,
+	btScalar dt, 
+  bool useStrainLimiting)
+  : m_face(contact.m_face), m_useStrainLimiting(useStrainLimiting), btReducedDeformableRigidContactConstraint(rsb, contact, infoGlobal, dt)
+{}
+
+btVector3 btReducedDeformableFaceRigidContactConstraint::getVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+	return vb;
+}
+
+btVector3 btReducedDeformableFaceRigidContactConstraint::getSplitVb() const
+{
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	btVector3 vb = (m_face->m_n[0]->m_splitv) * contact->m_bary[0] + (m_face->m_n[1]->m_splitv) * contact->m_bary[1] + (m_face->m_n[2]->m_splitv) * contact->m_bary[2];
+	return vb;
+}
+
+btVector3 btReducedDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+	btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
+	const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+	if (m_face->m_n[0] == node)
+	{
+		return face_dv * contact->m_weights[0];
+	}
+	if (m_face->m_n[1] == node)
+	{
+		return face_dv * contact->m_weights[1];
+	}
+	btAssert(node == m_face->m_n[2]);
+	return face_dv * contact->m_weights[2];
+}
+
+void btReducedDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+  //
+}

src/BulletSoftBody/BulletReducedDeformableBody/btReducedDeformableContactConstraint.h

@@ -0,0 +1,194 @@
+#include "../btDeformableContactConstraint.h"
+#include "btReducedDeformableBody.h"
+
+// ================= static constraints ===================
+class btReducedDeformableStaticConstraint : public btDeformableStaticConstraint
+{
+ public:
+  btReducedDeformableBody* m_rsb;
+  btScalar m_dt;
+  btVector3 m_ri;
+  btVector3 m_targetPos;
+  btVector3 m_impulseDirection;
+  btMatrix3x3 m_impulseFactorMatrix;
+  btScalar m_impulseFactor;
+  btScalar m_rhs;
+  btScalar m_appliedImpulse;
+  btScalar m_erp;
+
+  btReducedDeformableStaticConstraint(btReducedDeformableBody* rsb, 
+                                      btSoftBody::Node* node,
+                                      const btVector3& ri,
+                                      const btVector3& x0,
+                                      const btVector3& dir,
+                                      const btContactSolverInfo& infoGlobal,
+                                      btScalar dt);
+	// btReducedDeformableStaticConstraint(const btReducedDeformableStaticConstraint& other);
+  btReducedDeformableStaticConstraint() {}
+  virtual ~btReducedDeformableStaticConstraint() {}
+
+  virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+  
+  // this calls reduced deformable body's applyFullSpaceImpulse
+  virtual void applyImpulse(const btVector3& impulse);
+
+  btVector3 getDeltaVa() const;
+
+  // virtual void applySplitImpulse(const btVector3& impulse) {}
+};
+
+// ================= base contact constraints ===================
+class btReducedDeformableRigidContactConstraint : public btDeformableRigidContactConstraint
+{
+ public:
+  bool m_collideStatic;     // flag for collision with static object
+  bool m_collideMultibody;  // flag for collision with multibody
+
+  int m_nodeQueryIndex;
+  int m_solverBodyId;       // for debugging
+
+  btReducedDeformableBody* m_rsb;
+  btSolverBody* m_solverBody;
+  btScalar m_dt;
+
+  btScalar m_appliedNormalImpulse;
+  btScalar m_appliedTangentImpulse;
+  btScalar m_appliedTangentImpulse2;
+  btScalar m_normalImpulseFactor;
+  btScalar m_tangentImpulseFactor;
+  btScalar m_tangentImpulseFactor2;
+  btScalar m_tangentImpulseFactorInv;
+  btScalar m_tangentImpulseFactorInv2;
+  btScalar m_rhs;
+  btScalar m_rhs_tangent;
+  btScalar m_rhs_tangent2;
+  
+  btScalar m_cfm;
+  btScalar m_cfm_friction;
+  btScalar m_erp;
+  btScalar m_erp_friction;
+  btScalar m_friction;
+
+  btVector3 m_contactNormalA;     // surface normal for rigid body (opposite direction as impulse)
+  btVector3 m_contactNormalB;     // surface normal for reduced deformable body (opposite direction as impulse)
+  btVector3 m_contactTangent;     // tangential direction of the relative velocity
+  btVector3 m_contactTangent2;    // 2nd tangential direction of the relative velocity
+  btVector3 m_relPosA;            // relative position of the contact point for A (rigid)
+  btVector3 m_relPosB;            // relative position of the contact point for B
+  btMatrix3x3 m_impulseFactor;    // total impulse matrix
+
+  btVector3 m_bufferVelocityA;    // velocity at the beginning of the iteration
+  btVector3 m_bufferVelocityB;
+  btVector3 m_linearComponentNormal;    // linear components for the solver body
+  btVector3 m_angularComponentNormal;   // angular components for the solver body
+  // since 2nd contact direction only applies to multibody, these components will never be used
+  btVector3 m_linearComponentTangent;
+  btVector3 m_angularComponentTangent;
+
+  btReducedDeformableRigidContactConstraint(btReducedDeformableBody* rsb, 
+                                            const btSoftBody::DeformableRigidContact& c, 
+                                            const btContactSolverInfo& infoGlobal,
+                                            btScalar dt);
+	// btReducedDeformableRigidContactConstraint(const btReducedDeformableRigidContactConstraint& other);
+  btReducedDeformableRigidContactConstraint() {}
+  virtual ~btReducedDeformableRigidContactConstraint() {}
+
+  void setSolverBody(const int bodyId, btSolverBody& solver_body);
+
+  virtual void warmStarting() {}
+
+  virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
+
+  void calculateTangentialImpulse(btScalar& deltaImpulse_tangent, 
+                                  btScalar& appliedImpulse, 
+                                  const btScalar rhs_tangent,
+                                  const btScalar tangentImpulseFactorInv,
+                                  const btVector3& tangent,
+                                  const btScalar lower_limit,
+                                  const btScalar upper_limit,
+                                  const btVector3& deltaV_rel);
+
+  virtual void applyImpulse(const btVector3& impulse) {}
+
+  virtual void applySplitImpulse(const btVector3& impulse) {} // TODO: may need later
+
+  virtual btVector3 getVa() const;
+  virtual btVector3 getDeltaVa() const = 0;
+  virtual btVector3 getDeltaVb() const = 0;
+};
+
+// ================= node vs rigid constraints ===================
+class btReducedDeformableNodeRigidContactConstraint : public btReducedDeformableRigidContactConstraint
+{
+ public:
+  btSoftBody::Node* m_node;
+
+  btReducedDeformableNodeRigidContactConstraint(btReducedDeformableBody* rsb, 
+                                                const btSoftBody::DeformableNodeRigidContact& contact, 
+                                                const btContactSolverInfo& infoGlobal,
+                                                btScalar dt);
+	// btReducedDeformableNodeRigidContactConstraint(const btReducedDeformableNodeRigidContactConstraint& other);
+  btReducedDeformableNodeRigidContactConstraint() {}
+  virtual ~btReducedDeformableNodeRigidContactConstraint() {}
+
+  virtual void warmStarting();
+
+  // get the velocity of the deformable node in contact
+	virtual btVector3 getVb() const;
+
+  // get the velocity change of the rigid body
+  virtual btVector3 getDeltaVa() const;
+
+  // get velocity change of the node in contat
+  virtual btVector3 getDeltaVb() const;
+
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableNodeRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
+	}
+  
+  // this calls reduced deformable body's applyFullSpaceImpulse
+  virtual void applyImpulse(const btVector3& impulse);
+};
+
+// ================= face vs rigid constraints ===================
+class btReducedDeformableFaceRigidContactConstraint : public btReducedDeformableRigidContactConstraint
+{
+ public:
+  btSoftBody::Face* m_face;
+	bool m_useStrainLimiting;
+
+  btReducedDeformableFaceRigidContactConstraint(btReducedDeformableBody* rsb, 
+                                                const btSoftBody::DeformableFaceRigidContact& contact, 
+                                                const btContactSolverInfo& infoGlobal,
+                                                btScalar dt, 
+                                                bool useStrainLimiting);
+	// btReducedDeformableFaceRigidContactConstraint(const btReducedDeformableFaceRigidContactConstraint& other);
+  btReducedDeformableFaceRigidContactConstraint() {}
+  virtual ~btReducedDeformableFaceRigidContactConstraint() {}
+
+  // get the velocity of the deformable face at the contact point
+	virtual btVector3 getVb() const;
+
+	// get the split impulse velocity of the deformable face at the contact point
+	virtual btVector3 getSplitVb() const;
+
+	// get the velocity change of the input soft body node in the constraint
+	virtual btVector3 getDv(const btSoftBody::Node*) const;
+
+	// cast the contact to the desired type
+	const btSoftBody::DeformableFaceRigidContact* getContact() const
+	{
+		return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
+	}
+
+  // this calls reduced deformable body's applyFullSpaceImpulse
+  virtual void applyImpulse(const btVector3& impulse);
+};

src/BulletSoftBody/CMakeLists.txt

@@ -24,6 +24,11 @@ SET(BulletSoftBody_SRCS
 	btDeformableMultiBodyDynamicsWorld.cpp
 	btDeformableContactConstraint.cpp
 	poly34.cpp
+
+	BulletReducedDeformableBody/btReducedDeformableBody.cpp
+	BulletReducedDeformableBody/btReducedDeformableBodyHelpers.cpp
+	BulletReducedDeformableBody/btReducedDeformableBodySolver.cpp
+	BulletReducedDeformableBody/btReducedDeformableContactConstraint.cpp
 )
 
 
@@ -63,6 +68,11 @@ SET(BulletSoftBody_HDRS
 	poly34.h
 
 	btSoftBodySolverVertexBuffer.h
+
+	BulletReducedDeformableBody/btReducedDeformableBody.h
+	BulletReducedDeformableBody/btReducedDeformableBodyHelpers.h
+	BulletReducedDeformableBody/btReducedDeformableBodySolver.h
+	BulletReducedDeformableBody/btReducedDeformableContactConstraint.h
 )
 
 

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -25,12 +25,18 @@
 #include "btDeformableNeoHookeanForce.h"
 #include "btDeformableContactProjection.h"
 #include "btPreconditioner.h"
-#include "btDeformableMultiBodyDynamicsWorld.h"
+// #include "btDeformableMultiBodyDynamicsWorld.h"
 #include "LinearMath/btQuickprof.h"
 
 class btDeformableBackwardEulerObjective
 {
 public:
+	enum _
+	{
+		Mass_preconditioner,
+		KKT_preconditioner
+	};
+
 	typedef btAlignedObjectArray<btVector3> TVStack;
 	btScalar m_dt;
 	btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -23,6 +23,7 @@ btDeformableBodySolver::btDeformableBodySolver()
 	: m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false)
 {
 	m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
+	m_reducedSolver = false;
 }
 
 btDeformableBodySolver::~btDeformableBodySolver()
@@ -401,7 +402,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
 			}
 		}
 	}
-	m_objective->applyExplicitForce(m_residual);
+	applyExplicitForce();
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		btSoftBody* psb = m_softBodies[i];
@@ -504,3 +505,89 @@ void btDeformableBodySolver::setLineSearch(bool lineSearch)
 {
 	m_lineSearch = lineSearch;
 }
+
+void btDeformableBodySolver::applyExplicitForce()
+{
+	m_objective->applyExplicitForce(m_residual);
+}
+
+void btDeformableBodySolver::applyTransforms(btScalar timeStep)
+{
+	for (int i = 0; i < m_softBodies.size(); ++i)
+	{
+		btSoftBody* psb = m_softBodies[i];
+		for (int j = 0; j < psb->m_nodes.size(); ++j)
+		{
+			btSoftBody::Node& node = psb->m_nodes[j];
+			btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
+			btScalar clampDeltaV = maxDisplacement / timeStep;
+			for (int c = 0; c < 3; c++)
+			{
+				if (node.m_v[c] > clampDeltaV)
+				{
+					node.m_v[c] = clampDeltaV;
+				}
+				if (node.m_v[c] < -clampDeltaV)
+				{
+					node.m_v[c] = -clampDeltaV;
+				}
+			}
+			node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
+			node.m_q = node.m_x;
+			node.m_vn = node.m_v;
+		}
+		// enforce anchor constraints
+		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
+		{
+			btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
+			btSoftBody::Node* n = a.m_node;
+			n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
+
+			// update multibody anchor info
+			if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+			{
+				btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
+				if (multibodyLinkCol)
+				{
+					btVector3 nrm;
+					const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
+					const btTransform& wtr = multibodyLinkCol->getWorldTransform();
+					psb->m_worldInfo->m_sparsesdf.Evaluate(
+						wtr.invXform(n->m_x),
+						shp,
+						nrm,
+						0);
+					a.m_cti.m_normal = wtr.getBasis() * nrm;
+					btVector3 normal = a.m_cti.m_normal;
+					btVector3 t1 = generateUnitOrthogonalVector(normal);
+					btVector3 t2 = btCross(normal, t1);
+					btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+					findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
+					findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
+					findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
+
+					btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+					btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+					btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+
+					btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+					btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+					btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+
+					btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+									t1.getX(), t1.getY(), t1.getZ(),
+									t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
+					const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+					btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+					a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
+					a.jacobianData_normal = jacobianData_normal;
+					a.jacobianData_t1 = jacobianData_t1;
+					a.jacobianData_t2 = jacobianData_t2;
+					a.t1 = t1;
+					a.t2 = t2;
+				}
+			}
+		}
+		psb->interpolateRenderMesh();
+	}
+}

src/BulletSoftBody/btDeformableBodySolver.h

@@ -24,8 +24,8 @@
 #include "btConjugateResidual.h"
 #include "btConjugateGradient.h"
 struct btCollisionObjectWrapper;
-class btDeformableBackwardEulerObjective;
-class btDeformableMultiBodyDynamicsWorld;
+// class btDeformableBackwardEulerObjective;
+// class btDeformableMultiBodyDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
 {
@@ -46,6 +46,7 @@ protected:
 	int m_maxNewtonIterations;                                     // max number of newton iterations
 	btScalar m_newtonTolerance;                                    // stop newton iterations if f(x) < m_newtonTolerance
 	bool m_lineSearch;                                             // If true, use newton's method with line search under implicit scheme
+	bool m_reducedSolver;																					 // flag for reduced soft body solver
 public:
 	// handles data related to objective function
 	btDeformableBackwardEulerObjective* m_objective;
@@ -68,11 +69,18 @@ public:
 	// solve the momentum equation
 	virtual void solveDeformableConstraints(btScalar solverdt);
 
+	// set gravity (get from deformable world)
+	virtual void setGravity(const btVector3& gravity)
+	{
+		// for full deformable object, we don't store gravity in the solver
+		// this function is overriden in the reduced deformable object
+	}
+
 	// resize/clear data structures
-	void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
+	virtual void reinitialize(const btAlignedObjectArray<btSoftBody*>& softBodies, btScalar dt);
 
 	// set up contact constraints
-	void setConstraints(const btContactSolverInfo& infoGlobal);
+	virtual void setConstraints(const btContactSolverInfo& infoGlobal);
 
 	// add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
 	virtual void predictMotion(btScalar solverdt);
@@ -85,7 +93,7 @@ public:
 	void backupVelocity();
 
 	// set m_dv and m_backupVelocity to desired value to prepare for momentum solve
-	void setupDeformableSolve(bool implicit);
+	virtual void setupDeformableSolve(bool implicit);
 
 	// set the current velocity to that backed up in m_backupVelocity
 	void revertVelocity();
@@ -150,6 +158,62 @@ public:
 	// used in line search
 	btScalar kineticEnergy();
 
+	// add explicit force to the velocity in the objective class
+	virtual void applyExplicitForce();
+
+	// execute position/velocity update and apply anchor constraints in the integrateTransforms from the Dynamics world
+	virtual void applyTransforms(btScalar timeStep);
+
+	virtual void setStrainLimiting(bool opt)
+	{
+		m_objective->m_projection.m_useStrainLimiting = opt;
+	}
+
+	virtual void setPreconditioner(int opt)
+	{
+		switch (opt)
+		{
+			case btDeformableBackwardEulerObjective::Mass_preconditioner:
+				m_objective->m_preconditioner = m_objective->m_massPreconditioner;
+				break;
+
+			case btDeformableBackwardEulerObjective::KKT_preconditioner:
+				m_objective->m_preconditioner = m_objective->m_KKTPreconditioner;
+				break;
+			
+			default:
+				btAssert(false);
+				break;
+		}
+	}
+
+	virtual btAlignedObjectArray<btDeformableLagrangianForce*>* getLagrangianForceArray()
+	{
+		return &(m_objective->m_lf);
+	}
+
+	virtual const btAlignedObjectArray<btSoftBody::Node*>* getIndices()
+	{
+		return m_objective->getIndices();
+	}
+
+	virtual void setProjection()
+	{
+		m_objective->m_projection.setProjection();
+	}
+
+	virtual void setLagrangeMultiplier()
+	{
+		m_objective->m_projection.setLagrangeMultiplier();
+	}
+
+	virtual bool isReducedSolver()
+	{
+		return m_reducedSolver;
+	}
+	
+	virtual void deformableBodyInternalWriteBack() {}
+
 	// unused functions
 	virtual void optimize(btAlignedObjectArray<btSoftBody*>& softBodies, bool forceUpdate = false) {}
 	virtual void solveConstraints(btScalar dt) {}

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -14,11 +14,16 @@
  */
 
 #include "btDeformableMultiBodyConstraintSolver.h"
+#include "BulletReducedDeformableBody/btReducedDeformableBodySolver.h"
 #include <iostream>
+#include <thread>
 // override the iterations method to include deformable/multibody contact
 btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	{
+		// pair deformable body with solver body
+		pairDeformableAndSolverBody(bodies, numBodies, numDeformableBodies, infoGlobal);
+
 		///this is a special step to resolve penetrations (just for contacts)
 		solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 
@@ -37,6 +42,10 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 			// solver body velocity <- rigid body velocity
 			writeToSolverBody(bodies, numBodies, infoGlobal);
 
+
+			// std::cout << "------------Iteration " << iteration << "------------\n";
+			// std::cout << "m_leastSquaresResidual: " << m_leastSquaresResidual << "\n";
+
 			if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
 			{
 #ifdef VERBOSE_RESIDUAL_PRINTF
@@ -51,6 +60,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
 				m_analyticsData.m_numBodies = numBodies;
 				m_analyticsData.m_numContactManifolds = numManifolds;
 				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
+				
+				m_deformableSolver->deformableBodyInternalWriteBack();
+				// std::cout << "[===================Next Step===================]\n";
 				break;
 			}
 		}
@@ -78,6 +90,12 @@ void btDeformableMultiBodyConstraintSolver::solveDeformableBodyGroup(btCollision
 
 void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 {
+	// reduced soft body solver directly modifies the solver body
+	if (m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
 	for (int i = 0; i < numBodies; i++)
 	{
 		int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
@@ -94,6 +112,12 @@ void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject*
 
 void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
 {
+	// reduced soft body solver directly modifies the solver body
+	if (m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
 	for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
 	{
 		btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
@@ -105,6 +129,53 @@ void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactS
 	}
 }
 
+
+void btDeformableMultiBodyConstraintSolver::pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal)
+{
+	if (!m_deformableSolver->isReducedSolver())
+	{
+		return;
+	}
+
+	btReducedDeformableBodySolver* solver = static_cast<btReducedDeformableBodySolver*>(m_deformableSolver);
+	
+	for (int i = 0; i < numDeformableBodies; ++i)
+	{
+		for (int k = 0; k < solver->m_nodeRigidConstraints[i].size(); ++k)
+    {
+      btReducedDeformableNodeRigidContactConstraint& constraint = solver->m_nodeRigidConstraints[i][k];
+
+			if (!constraint.m_contact->m_cti.m_colObj->isStaticObject())
+			{
+				btCollisionObject& col_obj = const_cast<btCollisionObject&>(*constraint.m_contact->m_cti.m_colObj);
+
+				// object index in the solver body pool
+				int bodyId = getOrInitSolverBody(col_obj, infoGlobal.m_timeStep);
+				
+				const btRigidBody* body = btRigidBody::upcast(bodies[bodyId]);
+				if (body && body->getInvMass())
+				{
+						// std::cout << "Node: " << constraint.m_node->index << ", body: " << bodyId << "\n";
+					btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+					constraint.setSolverBody(bodyId, solverBody);
+				}
+			}
+    }
+
+		// for (int j = 0; j < numBodies; j++)
+		// {
+		// 	int bodyId = getOrInitSolverBody(*bodies[j], infoGlobal.m_timeStep);
+
+		// 	btRigidBody* body = btRigidBody::upcast(bodies[j]);
+		// 	if (body && body->getInvMass())
+		// 	{
+		// 		btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+		// 		m_deformableSolver->pairConstraintWithSolverBody(i, bodyId, solverBody);
+		// 	}
+		// }	
+	}
+}
+
 void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -43,6 +43,9 @@ protected:
 	// write the velocity of the underlying rigid body to the the the solver body
 	void writeToSolverBody(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
 
+	// let each deformable body knows which solver body is in constact
+	void pairDeformableAndSolverBody(btCollisionObject** bodies, int numBodies, int numDeformableBodies, const btContactSolverInfo& infoGlobal);
+
 	virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 
 	virtual btScalar solveDeformableGroupIterations(btCollisionObject * *bodies, int numBodies, btCollisionObject** deformableBodies, int numDeformableBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -94,7 +94,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
 
 	beforeSolverCallbacks(timeStep);
 
-	///solve contact constraints and then deformable bodies momemtum equation
+	// ///solve contact constraints and then deformable bodies momemtum equation
 	solveConstraints(timeStep);
 
 	afterSolverCallbacks(timeStep);
@@ -298,83 +298,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 	BT_PROFILE("integrateTransforms");
 	positionCorrection(timeStep);
 	btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
-	for (int i = 0; i < m_softBodies.size(); ++i)
-	{
-		btSoftBody* psb = m_softBodies[i];
-		for (int j = 0; j < psb->m_nodes.size(); ++j)
-		{
-			btSoftBody::Node& node = psb->m_nodes[j];
-			btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
-			btScalar clampDeltaV = maxDisplacement / timeStep;
-			for (int c = 0; c < 3; c++)
-			{
-				if (node.m_v[c] > clampDeltaV)
-				{
-					node.m_v[c] = clampDeltaV;
-				}
-				if (node.m_v[c] < -clampDeltaV)
-				{
-					node.m_v[c] = -clampDeltaV;
-				}
-			}
-			node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
-			node.m_q = node.m_x;
-			node.m_vn = node.m_v;
-		}
-		// enforce anchor constraints
-		for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
-		{
-			btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
-			btSoftBody::Node* n = a.m_node;
-			n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
-
-			// update multibody anchor info
-			if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-			{
-				btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
-				if (multibodyLinkCol)
-				{
-					btVector3 nrm;
-					const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
-					const btTransform& wtr = multibodyLinkCol->getWorldTransform();
-					psb->m_worldInfo->m_sparsesdf.Evaluate(
-						wtr.invXform(n->m_x),
-						shp,
-						nrm,
-						0);
-					a.m_cti.m_normal = wtr.getBasis() * nrm;
-					btVector3 normal = a.m_cti.m_normal;
-					btVector3 t1 = generateUnitOrthogonalVector(normal);
-					btVector3 t2 = btCross(normal, t1);
-					btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
-					findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
-					findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
-					findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
-
-					btScalar* J_n = &jacobianData_normal.m_jacobians[0];
-					btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
-					btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
-
-					btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
-					btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-					btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-
-					btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
-									t1.getX(), t1.getY(), t1.getZ(),
-									t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
-					const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-					btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
-					a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
-					a.jacobianData_normal = jacobianData_normal;
-					a.jacobianData_t1 = jacobianData_t1;
-					a.jacobianData_t2 = jacobianData_t2;
-					a.t1 = t1;
-					a.t2 = t2;
-				}
-			}
-		}
-		psb->interpolateRenderMesh();
-	}
+	m_deformableBodySolver->applyTransforms(timeStep);
 }
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
@@ -391,9 +315,9 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 
 	// set up the directions in which the velocity does not change in the momentum solve
 	if (m_useProjection)
-		m_deformableBodySolver->m_objective->m_projection.setProjection();
+		m_deformableBodySolver->setProjection();
 	else
-		m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
+		m_deformableBodySolver->setLagrangeMultiplier();
 
 	// for explicit scheme, m_backupVelocity = v_{n+1}^*
 	// for implicit scheme, m_backupVelocity = v_n
@@ -519,6 +443,12 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time
 	m_deformableBodySolver->predictMotion(timeStep);
 }
 
+void btDeformableMultiBodyDynamicsWorld::setGravity(const btVector3& gravity)
+{
+	btDiscreteDynamicsWorld::setGravity(gravity);
+	m_deformableBodySolver->setGravity(gravity);
+}
+
 void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 {
 	m_internalTime += timeStep;
@@ -533,14 +463,14 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 	if (m_useProjection)
 	{
 		m_deformableBodySolver->m_useProjection = true;
-		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
-		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
+		m_deformableBodySolver->setStrainLimiting(true);
+		m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner);
 	}
 	else
 	{
 		m_deformableBodySolver->m_useProjection = false;
-		m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
-		m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
+		m_deformableBodySolver->setStrainLimiting(false);
+		m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner);
 	}
 }
 
@@ -682,7 +612,7 @@ void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
 
 void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 {
-	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 	bool added = false;
 	for (int i = 0; i < forces.size(); ++i)
 	{
@@ -696,14 +626,14 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL
 	if (!added)
 	{
 		force->addSoftBody(psb);
-		force->setIndices(m_deformableBodySolver->m_objective->getIndices());
+		force->setIndices(m_deformableBodySolver->getIndices());
 		forces.push_back(force);
 	}
 }
 
 void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 {
-	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 	int removed_index = -1;
 	for (int i = 0; i < forces.size(); ++i)
 	{
@@ -721,7 +651,7 @@ void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformab
 
 void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
 {
-	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+	btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 	for (int i = 0; i < forces.size(); ++i)
 	{
 		forces[i]->removeSoftBody(psb);

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -19,7 +19,7 @@
 #include "btSoftMultiBodyDynamicsWorld.h"
 #include "btDeformableLagrangianForce.h"
 #include "btDeformableMassSpringForce.h"
-#include "btDeformableBodySolver.h"
+// #include "btDeformableBodySolver.h"
 #include "btDeformableMultiBodyConstraintSolver.h"
 #include "btSoftBodyHelpers.h"
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
@@ -121,6 +121,8 @@ public:
 		return m_sbi;
 	}
 
+	virtual void setGravity(const btVector3& gravity);
+
 	void reinitialize(btScalar timeStep);
 
 	void applyRigidBodyGravity(btScalar timeStep);

src/BulletSoftBody/btSoftBody.cpp

@@ -231,6 +231,9 @@ void btSoftBody::initDefaults()
 	m_gravityFactor = 1;
 	m_cacheBarycenter = false;
 	m_fdbvnt = 0;
+
+	// reduced flag
+	m_reducedModel = false;
 }
 
 //

src/BulletSoftBody/btSoftBody.h

@@ -799,6 +799,7 @@ public:
 	typedef btAlignedObjectArray<Material*> tMaterialArray;
 	typedef btAlignedObjectArray<Joint*> tJointArray;
 	typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
+	typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
 
 	//
 	// Fields
@@ -857,6 +858,8 @@ public:
 
 	btScalar m_restLengthScale;
 
+	bool m_reducedModel;	// Reduced deformable model flag
+	
 	//
 	// Api
 	//
@@ -1003,15 +1006,15 @@ public:
 	/* Get best fit rigid transform                                         */
 	btTransform getRigidTransform();
 	/* Transform to given pose                                              */
-	void transformTo(const btTransform& trs);
+	virtual void transformTo(const btTransform& trs);
 	/* Transform															*/
-	void transform(const btTransform& trs);
+	virtual void transform(const btTransform& trs);
 	/* Translate															*/
-	void translate(const btVector3& trs);
+	virtual void translate(const btVector3& trs);
 	/* Rotate															*/
-	void rotate(const btQuaternion& rot);
+	virtual void rotate(const btQuaternion& rot);
 	/* Scale																*/
-	void scale(const btVector3& scl);
+	virtual void scale(const btVector3& scl);
 	/* Get link resting lengths scale										*/
 	btScalar getRestLengthScale();
 	/* Scale resting length of all springs									*/
@@ -1101,6 +1104,13 @@ public:
 	void setZeroVelocity();
 	bool wantsSleeping();
 
+	virtual btMatrix3x3 getImpulseFactor(int n_node)
+	{
+		btMatrix3x3 tmp;
+		tmp.setIdentity();
+		return tmp;
+	}
+
 	//
 	// Functionality to deal with new accelerated solvers.
 	//

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1691,12 +1691,19 @@ struct btSoftColliders
 						if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 						{
 							const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
-							static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
-							const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
 							const btVector3 ra = n.m_x - wtr.getOrigin();
 
-							c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
-							//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+							static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+							const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+							if (psb->m_reducedModel)
+							{
+								c.m_c0 = MassMatrix(imb, iwi, ra); //impulse factor K of the rigid body only (not the inverse)
+							}
+							else
+							{
+								c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
+								//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+							}
 							c.m_c1 = ra;
 						}
 						else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
@@ -1724,7 +1731,16 @@ struct btSoftColliders
 												t1.getX(), t1.getY(), t1.getZ(),
 												t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
 								const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-								btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+								
+								btMatrix3x3 local_impulse_matrix;
+								if (psb->m_reducedModel)
+								{
+									local_impulse_matrix = OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof);
+								}
+								else
+								{
+									local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+								}
 								c.m_c0 = rot.transpose() * local_impulse_matrix * rot;
 								c.jacobianData_normal = jacobianData_normal;
 								c.jacobianData_t1 = jacobianData_t1;

src/BulletSoftBody/btSoftBodySolvers.h

@@ -36,7 +36,8 @@ public:
 		CL_SIMD_SOLVER,
 		DX_SOLVER,
 		DX_SIMD_SOLVER,
-		DEFORMABLE_SOLVER
+		DEFORMABLE_SOLVER,
+		REDUCED_DEFORMABLE_SOLVER
 	};
 
 protected: