Merge remote-tracking branch 'bp/master' into master

.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/DeformableDemo/LoadDeformed.cpp

@@ -0,0 +1,261 @@
+/*
+ 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 "LoadDeformed.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.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"
+#include "../Utils/b3BulletDefaultFileIO.h"
+
+class LoadDeformed : public CommonDeformableBodyBase
+{
+	int steps;
+	btSoftBody* psb;
+	char filename;
+	int reset_frame;
+	float sim_time;
+
+public:
+	LoadDeformed(struct GUIHelperInterface* helper)
+		: CommonDeformableBodyBase(helper)
+	{
+		steps = 0;
+		psb = nullptr;
+		reset_frame = 0;
+		sim_time = 0;
+	}
+	virtual ~LoadDeformed()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+		float dist = 2;
+		float pitch = -45;
+		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)
+	{
+		steps++;
+		sim_time += deltaTime;
+		////        int seconds = 1/deltaTime;
+		if (0)
+		{
+			//        if (reset_frame==0 && steps<100){
+			////            printf("steps %d, seconds %d, steps/seconds %d\n", steps,seconds,steps/seconds);
+			char filename[100];
+			sprintf(filename, "%s_%d_%d.txt", "states", reset_frame, steps);
+			btSoftBodyHelpers::writeState(filename, psb);
+		}
+		if (sim_time + reset_frame * 0.05 >= 5) exit(0);
+		float internalTimeStep = 1. / 240.f;
+		//        float internalTimeStep = 0.1f;
+		m_dynamicsWorld->stepSimulation(deltaTime, deltaTime / internalTimeStep, internalTimeStep);
+	}
+
+	void addCloth(const btVector3& origin);
+
+	virtual void renderScene()
+	{
+		CommonDeformableBodyBase::renderScene();
+		btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+
+		for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+		{
+			btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+			{
+				btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+				btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+			}
+		}
+	}
+};
+
+void LoadDeformed::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* deformableBodySolver = new btDeformableBodySolver();
+
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+	sol->setDeformableSolver(deformableBodySolver);
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+	btVector3 gravity = btVector3(0, -9.8, 0);
+	m_dynamicsWorld->setGravity(gravity);
+	getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
+
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	{
+		///create a ground
+		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.)));
+		groundShape->setMargin(0.02);
+		m_collisionShapes.push_back(groundShape);
+
+		btTransform groundTransform;
+		groundTransform.setIdentity();
+		groundTransform.setOrigin(btVector3(0, -3.5, 0));
+		groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+		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(4);
+
+		//add the ground to the dynamics world
+		m_dynamicsWorld->addRigidBody(body);
+	}
+	addCloth(btVector3(0, 1, 0));
+	getDeformableDynamicsWorld()->setImplicit(false);
+	getDeformableDynamicsWorld()->setLineSearch(false);
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void LoadDeformed::addCloth(const btVector3& origin)
+// create a piece of cloth
+{
+	const btScalar s = 0.6;
+	const btScalar h = 0;
+
+	psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2 * s),
+										 btVector3(+s, h, -2 * s),
+										 btVector3(-s, h, +2 * s),
+										 btVector3(+s, h, +2 * s),
+										 15, 30,
+										 0, true, 0.0);
+
+	psb->getCollisionShape()->setMargin(0.02);
+	psb->generateBendingConstraints(2);
+	psb->setTotalMass(.5);
+	psb->m_cfg.kKHR = 1;  // collision hardness with kinematic objects
+	psb->m_cfg.kCHR = 1;  // collision hardness with rigid body
+	psb->m_cfg.kDF = 0.1;
+	psb->rotate(btQuaternion(0, SIMD_PI / 2, 0));
+	btTransform clothTransform;
+	clothTransform.setIdentity();
+	clothTransform.setOrigin(btVector3(0, 0.2, 0) + origin);
+	psb->transform(clothTransform);
+
+	b3BulletDefaultFileIO fileio;
+	char absolute_path[1024];
+	char filename[100];
+	sprintf(filename, "/Users/fuchuyuan/Documents/mybullet/build_cmake/examples/ExampleBrowser/states_0_%d.txt", reset_frame);
+	fileio.findResourcePath(filename, absolute_path, 1024);
+	btAlignedObjectArray<btVector3> qs;
+	btAlignedObjectArray<btVector3> vs;
+	btSoftBodyHelpers::loadDeformableState(qs, vs, absolute_path, &fileio);
+	if (reset_frame > 0)
+		psb->updateState(qs, vs);
+	psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+	psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
+	psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
+	//    psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+	psb->setCollisionFlags(0);
+	psb->setCacheBarycenter(true);
+	getDeformableDynamicsWorld()->addSoftBody(psb);
+	psb->setSelfCollision(false);
+
+	btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.2, true);
+	psb->setSpringStiffness(4);
+	getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+	m_forces.push_back(mass_spring);
+	btVector3 gravity = btVector3(0, -9.8, 0);
+	btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
+	getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+	//    getDeformableDynamicsWorld()->setUseProjection(true);
+	m_forces.push_back(gravity_force);
+}
+
+void LoadDeformed::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* LoadDeformedCreateFunc(struct CommonExampleOptions& options)
+{
+	return new LoadDeformed(options.m_guiHelper);
+}

examples/DeformableDemo/LoadDeformed.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 _LOAD_DEFORMED_H
+#define _LOAD_DEFORMED_H
+
+class CommonExampleInterface* LoadDeformedCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_LOAD_DEFORMED_H

examples/ExampleBrowser/CMakeLists.txt

@@ -379,6 +379,8 @@ SET(BulletExampleBrowser_SRCS
   ../DeformableDemo/Collide.cpp
   ../DeformableDemo/Collide.h
   ../DeformableDemo/LargeDeformation.cpp
+  ../DeformableDemo/LoadDeformed.h
+  ../DeformableDemo/LoadDeformed.cpp
   ../DeformableDemo/LargeDeformation.h
   ../DeformableDemo/DeformableClothAnchor.cpp
   ../DeformableDemo/DeformableClothAnchor.h
@@ -388,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

@@ -55,6 +55,7 @@
 #include "../DeformableDemo/DeformableMultibody.h"
 #include "../DeformableDemo/VolumetricDeformable.h"
 #include "../DeformableDemo/LargeDeformation.h"
+#include "../DeformableDemo/LoadDeformed.h"
 #include "../DeformableDemo/Collide.h"
 #include "../DeformableDemo/GraspDeformable.h"
 #include "../DeformableDemo/DeformableContact.h"
@@ -72,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
@@ -209,11 +219,24 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
         ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
 		ExampleEntry(1, "Extreme Deformation", "Recovery from extreme deformation", LargeDeformationCreateFunc),
+        ExampleEntry(1, "Load Deformed", "Reconstruct a deformed object", LoadDeformedCreateFunc),
 		ExampleEntry(1, "Colliding Test", "Volumetric deformable collide with rigid box", CollideCreateFunc),
         ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc),
         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/OpenGLWindow/X11OpenGLWindow.cpp

@@ -566,7 +566,7 @@ void X11OpenGLWindow::enableOpenGL()
 	//Access pthreads as a workaround for a bug in Linux/Ubuntu
 	//See https://bugs.launchpad.net/ubuntu/+source/nvidia-graphics-drivers-319/+bug/1248642
 
-#if !defined(__NetBSD__)
+#if !defined(__NetBSD__) && !defined(__ANDROID__)
 	int i = pthread_getconcurrency();
 	printf("pthread_getconcurrency()=%d\n", i);
 #endif

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/PhysicsClientC_API.cpp

@@ -1549,6 +1549,22 @@ B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHand
 	}
 }
 
+B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_MESH_DATA);
+	command->m_updateFlags |= B3_MESH_DATA_SIMULATION_MESH;
+}
+
+B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_MESH_DATA || command->m_type == CMD_RESET_MESH_DATA);
+	command->m_updateFlags |= B3_MESH_DATA_SIMULATION_MESH_VELOCITY;
+}
+
 B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -532,6 +532,8 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId);
 
 	B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
+	B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle);
+	B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle);
 	B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex);
 	B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
 	

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"
@@ -1664,6 +1668,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btDeformableMousePickingForce* m_mouseForce;
 	btScalar m_maxPickingForce;
 	btDeformableBodySolver* m_deformablebodySolver;
+	btReducedDeformableBodySolver* m_reducedSoftBodySolver;
 	btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
 #endif
 
@@ -2720,16 +2725,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
 
 	
 
@@ -2776,7 +2791,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;
@@ -3643,6 +3665,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)
 		{
@@ -5524,10 +5551,23 @@ bool PhysicsServerCommandProcessor::processResetMeshDataCommand(const struct Sha
 			int numVertices = psb->m_nodes.size();
 			if (clientCmd.m_resetMeshDataArgs.m_numVertices == numVertices)
 			{
-				for (int i = 0; i < numVertices; ++i)
+				if (clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY)
 				{
-					btSoftBody::Node& n = psb->m_nodes[i];
-					n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]);
+					for (int i = 0; i < numVertices; ++i)
+					{
+						btSoftBody::Node& n = psb->m_nodes[i];
+						n.m_v.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+						n.m_vn.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+					}
+				}
+				else
+				{
+					for (int i = 0; i < numVertices; ++i)
+					{
+						btSoftBody::Node& n = psb->m_nodes[i];
+						n.m_x.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+						n.m_q.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+					}
 				}
 				serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED;
 			}
@@ -5614,10 +5654,12 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 			}
 
 			bool separateRenderMesh = false;
-			if ((flags & B3_MESH_DATA_SIMULATION_MESH) == 0)
+			if ((clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH) == 0)
 			{
 				separateRenderMesh = (psb->m_renderNodes.size() != 0);
 			}
+            bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY;
+            
 			int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
 			int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
 			int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
@@ -5626,14 +5668,22 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 			{
 				if (separateRenderMesh)
 				{
-					
 					const btSoftBody::RenderNode& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
+                    if(requestVelocity){
+                        b3Warning("Request mesh velocity not implemented for Render Mesh.");
+                        return hasStatus;
+                    }
 					verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
 				}
 				else
 				{
 					const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
-					verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
+                    if(!requestVelocity){
+                        verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
+                    }
+                    else{
+                        verticesOut[i].setValue(n.m_v.x(), n.m_v.y(), n.m_v.z());
+                    }
 				}
 			}
 			sizeInBytes = verticesCopied * sizeof(btVector3);
@@ -8077,6 +8127,125 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 	return hasStatus;
 }
 
+bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndexA, int& linkIndexB, int& objectIndexA, int& objectIndexB, bool& swap){
+   
+    if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
+    {
+        if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
+        {
+            swap = false;
+        }
+        else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB)
+        {
+            swap = true;
+        }
+        else
+        {
+            return true;
+        }
+    }
+
+    if (swap)
+    {
+        std::swap(objectIndexA, objectIndexB);
+        std::swap(linkIndexA, linkIndexB);
+    }
+
+    //apply the second object filter, if the user provides it
+    if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
+    {
+        if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)
+        {
+            return true;
+        }
+    }
+
+    if (
+        (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
+        clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
+    {
+        return true;
+    }
+
+    if (
+        (clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
+        clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
+    {
+        return true;
+    }
+    
+    return false;
+}
+
+bool PhysicsServerCommandProcessor::processRequestDeformableDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd){
+#ifndef SKIP_DEFORMABLE_BODY
+    btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+    if (!deformWorld)
+    {
+        return false;
+    }
+	const int max_contacts_per_object = 4;
+	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
+    {
+		int num_contacts_reported = 0;
+		btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
+        for (int c = 0; c < psb->m_faceNodeContacts.size(); c++)
+        {
+            const btSoftBody::DeformableFaceNodeContact* contact = &psb->m_faceNodeContacts[c];
+            //apply the filter, if the user provides it
+            int linkIndexA = -1;
+            int linkIndexB = -1;
+            int objectIndexA = psb->getUserIndex2();
+            int objectIndexB = -1;
+            const btSoftBody* bodyB = btSoftBody::upcast(contact->m_colObj);
+            if (bodyB)
+            {
+                objectIndexB = bodyB->getUserIndex2();
+            }
+            bool swap = false;
+            if(RequestFiltered(clientCmd, linkIndexA, linkIndexB, objectIndexA, objectIndexB, swap)==true){
+                continue;
+            }
+			if (++num_contacts_reported > max_contacts_per_object)
+			{
+				break;
+			}
+			//Convert contact info
+            b3ContactPointData pt;
+			btVector3 l = contact->m_node->m_x - BaryEval(contact->m_face->m_n[0]->m_x, contact->m_face->m_n[1]->m_x, contact->m_face->m_n[2]->m_x, contact->m_normal);
+			pt.m_contactDistance = -contact->m_margin + contact->m_normal.dot(l);
+			pt.m_bodyUniqueIdA = objectIndexA;
+            pt.m_bodyUniqueIdB = objectIndexB;
+            pt.m_contactFlags = 0;
+            pt.m_linkIndexA = linkIndexA;
+            pt.m_linkIndexB = linkIndexB;
+            for (int j = 0; j < 3; j++)
+            {
+                if (swap)
+                {
+                    pt.m_contactNormalOnBInWS[j] = -contact->m_normal[j];
+                }
+                else
+                {
+                    pt.m_contactNormalOnBInWS[j] = contact->m_normal[j];
+                }
+                pt.m_positionOnAInWS[j] = contact->m_node->m_x[j];
+                pt.m_positionOnBInWS[j] = contact->m_node->m_x[j];
+                pt.m_linearFrictionDirection1[j] = 0;
+                pt.m_linearFrictionDirection2[j] = 0;
+            }
+            pt.m_normalForce = 0;
+            pt.m_linearFrictionForce1 = 0;
+            pt.m_linearFrictionForce2 = 0;
+           m_data->m_cachedContactPoints.push_back(pt);
+        }
+    }
+#endif
+    return true;
+}
+
+
+
 bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd)
 {
 #ifndef SKIP_DEFORMABLE_BODY
@@ -8345,6 +8514,7 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand
 
 #ifndef SKIP_DEFORMABLE_BODY
 				processRequestDeformableContactpointHelper(clientCmd);
+ 				processRequestDeformableDeformableContactpointHelper(clientCmd);
 #endif
 				break;
 			}
@@ -9220,6 +9390,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
@@ -9481,6 +9655,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;
@@ -10714,7 +11329,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

@@ -46,6 +46,7 @@ protected:
 	bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd);
+    bool processRequestDeformableDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd);
 	bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -109,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/PosixSharedMemory.cpp

@@ -8,6 +8,11 @@
 #define TEST_SHARED_MEMORY
 #endif  //_WIN32
 
+//Shmem not available on target api < 26
+#if defined(__ANDROID_API__) && (__ANDROID_API__ < 26)
+#undef TEST_SHARED_MEMORY
+#endif  //__ANDROID__
+
 #include <stddef.h>
 
 #ifdef TEST_SHARED_MEMORY

examples/SharedMemory/SharedMemoryPublic.h

@@ -461,12 +461,12 @@ struct b3MeshVertex
 	double x, y, z, w;
 };
 
-
 enum eMeshDataFlags
 {
-	B3_MESH_DATA_SIMULATION_MESH=1,
-	B3_MESH_DATA_SIMULATION_INDICES,
-	B3_MESH_DATA_GRAPHICS_INDICES,
+	B3_MESH_DATA_SIMULATION_MESH = 1,
+	B3_MESH_DATA_SIMULATION_INDICES = 2,
+	B3_MESH_DATA_GRAPHICS_INDICES = 4,
+	B3_MESH_DATA_SIMULATION_MESH_VELOCITY = 8,
 };
 
 enum eMeshDataEnum
@@ -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/gym/pybullet_utils/urdfEditor.py

@@ -64,6 +64,8 @@ class UrdfJoint(object):
     self.joint_type = p.JOINT_REVOLUTE
     self.joint_lower_limit = 0
     self.joint_upper_limit = -1
+    self.joint_max_force = 0
+    self.joint_max_velocity = 0
     self.parent_name = "parentName"
     self.child_name = "childName"
     self.joint_origin_xyz = [1, 2, 3]
@@ -171,6 +173,8 @@ class UrdfEditor(object):
       urdfJoint.joint_type = jointInfo[2]
       urdfJoint.joint_lower_limit = jointInfo[8]
       urdfJoint.joint_upper_limit = jointInfo[9]
+      urdfJoint.joint_max_force = jointInfo[10]
+      urdfJoint.joint_max_velocity = jointInfo[11]
       urdfJoint.joint_axis_xyz = jointInfo[13]
       orgParentIndex = jointInfo[16]
       if (orgParentIndex < 0):
@@ -326,12 +330,14 @@ class UrdfEditor(object):
     str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
     file.write(str)
 
-    if urdfJoint.joint_type == p.JOINT_PRISMATIC:
-      #todo: handle limits
-      lowerLimit = -0.5
-      upperLimit = 0.5
-      str = '<limit effort="1000.0" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="0.5"/>'.format(
-          lowerLimit, upperLimit, prec=precision)
+    if jointTypeStr == "prismatic" or jointTypeStr == "revolute":
+      lowerLimit = urdfJoint.joint_lower_limit
+      upperLimit = urdfJoint.joint_upper_limit
+      maxForce = urdfJoint.joint_max_force
+      maxVelocity = urdfJoint.joint_max_velocity
+      
+      str = '\t\t<limit effort="{:.{prec}f}" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="{:.{prec}f}"/>\n'.format(
+          maxForce, lowerLimit, upperLimit, maxVelocity, prec=precision)
       file.write(str)
 
     file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")

examples/pybullet/pybullet.c

@@ -13176,6 +13176,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);
 

examples/pybullet/tensorflow/humanoid_running.py

@@ -1,4 +1,8 @@
-import tf.compat.v1 as tf
+try:
+  import tf.compat.v1 as tf
+except Exception:
+  import tensorflow.compat.v1 as tf
+  tf.disable_eager_execution()
 import sys
 import numpy as np
 import argparse

examples/pybullet/tensorflow/humanoid_running_vr_follow.py

@@ -1,4 +1,8 @@
-import tf.compat.v1 as tf
+try:
+  import tf.compat.v1 as tf
+except Exception:
+  import tensorflow.compat.v1 as tf
+  tf.disable_eager_execution()
 import sys
 import numpy as np
 

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];
@@ -411,6 +412,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
 			psb->m_nodeRigidContacts.resize(0);
 			psb->m_faceRigidContacts.resize(0);
 			psb->m_faceNodeContacts.resize(0);
+			psb->m_faceNodeContactsCCD.resize(0);
 			// predict motion for collision detection
 			predictDeformableMotion(psb, solverdt);
 		}
@@ -503,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);
@@ -201,7 +201,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 			if (psb->isActive())
 			{
 				// clear contact points in the previous iteration
-				psb->m_faceNodeContacts.clear();
+				psb->m_faceNodeContactsCCD.clear();
 
 				// update m_q and normals for CCD calculation
 				for (int j = 0; j < psb->m_nodes.size(); ++j)
@@ -237,7 +237,8 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 			btSoftBody* psb = m_softBodies[i];
 			if (psb->isActive())
 			{
-				penetration_count += psb->m_faceNodeContacts.size();
+				penetration_count += psb->m_faceNodeContactsCCD.size();
+				;
 			}
 		}
 		if (penetration_count == 0)
@@ -297,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)
@@ -390,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
@@ -518,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;
@@ -532,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);
 	}
 }
 
@@ -681,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)
 	{
@@ -695,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)
 	{
@@ -720,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;
 }
 
 //
@@ -1483,6 +1486,21 @@ void btSoftBody::randomizeConstraints()
 #undef NEXTRAND
 }
 
+void btSoftBody::updateState(const btAlignedObjectArray<btVector3>& q, const btAlignedObjectArray<btVector3>& v)
+{
+	int node_count = m_nodes.size();
+	btAssert(node_count == q.size());
+	btAssert(node_count == v.size());
+	for (int i = 0; i < node_count; i++)
+	{
+		Node& n = m_nodes[i];
+		n.m_x = q[i];
+		n.m_q = q[i];
+		n.m_v = v[i];
+		n.m_vn = v[i];
+	}
+}
+
 //
 void btSoftBody::releaseCluster(int index)
 {
@@ -2821,7 +2839,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	btScalar dst;
 	btGjkEpaSolver2::sResults results;
 
-//	#define USE_QUADRATURE 1
+	//	#define USE_QUADRATURE 1
 
 	// use collision quadrature point
 #ifdef USE_QUADRATURE

src/BulletSoftBody/btSoftBody.h

@@ -298,7 +298,7 @@ public:
 	};
 	struct RenderFace
 	{
-		RenderNode* m_n[3];          // Node pointers
+		RenderNode* m_n[3];  // Node pointers
 	};
 
 	/* Face			*/
@@ -407,6 +407,7 @@ public:
 		btScalar m_friction;  // Friction
 		btScalar m_imf;       // inverse mass of the face at contact point
 		btScalar m_c0;        // scale of the impulse matrix;
+		const btCollisionObject* m_colObj;  // Collision object to collide with.
 	};
 
 	/* SContact		*/
@@ -786,7 +787,7 @@ public:
 	typedef btAlignedObjectArray<Cluster*> tClusterArray;
 	typedef btAlignedObjectArray<Note> tNoteArray;
 	typedef btAlignedObjectArray<Node> tNodeArray;
-	typedef btAlignedObjectArray< RenderNode> tRenderNodeArray;
+	typedef btAlignedObjectArray<RenderNode> tRenderNodeArray;
 	typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
 	typedef btAlignedObjectArray<Link> tLinkArray;
 	typedef btAlignedObjectArray<Face> tFaceArray;
@@ -798,6 +799,7 @@ public:
 	typedef btAlignedObjectArray<Material*> tMaterialArray;
 	typedef btAlignedObjectArray<Joint*> tJointArray;
 	typedef btAlignedObjectArray<btSoftBody*> tSoftBodyArray;
+	typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > tDenseMatrix;
 
 	//
 	// Fields
@@ -813,7 +815,7 @@ public:
 	tRenderNodeArray m_renderNodes;    // Render Nodes
 	tLinkArray m_links;                // Links
 	tFaceArray m_faces;                // Faces
-	tRenderFaceArray m_renderFaces;          // Faces
+	tRenderFaceArray m_renderFaces;    // Faces
 	tTetraArray m_tetras;              // Tetras
 	btAlignedObjectArray<TetraScratch> m_tetraScratches;
 	btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
@@ -823,6 +825,7 @@ public:
 	btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
 	btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
 	btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
+	btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContactsCCD;
 	tSContactArray m_scontacts;     // Soft contacts
 	tJointArray m_joints;           // Joints
 	tMaterialArray m_materials;     // Materials
@@ -855,6 +858,8 @@ public:
 
 	btScalar m_restLengthScale;
 
+	bool m_reducedModel;	// Reduced deformable model flag
+	
 	//
 	// Api
 	//
@@ -1001,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									*/
@@ -1051,6 +1056,9 @@ public:
 								   Material* mat = 0);
 	/* Randomize constraints to reduce solver bias							*/
 	void randomizeConstraints();
+
+	void updateState(const btAlignedObjectArray<btVector3>& qs, const btAlignedObjectArray<btVector3>& vs);
+
 	/* Release clusters														*/
 	void releaseCluster(int index);
 	void releaseClusters();
@@ -1096,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/btSoftBodyHelpers.cpp

@@ -18,6 +18,7 @@ subject to the following restrictions:
 #include <stdio.h>
 #include <string>
 #include <iostream>
+#include <iomanip> 
 #include <sstream>
 #include <string.h>
 #include <algorithm>
@@ -1487,6 +1488,168 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 	fs.close();
 }
 
+static inline bool isSpace(const char c)
+{
+	return (c == ' ') || (c == '\t');
+}
+static inline bool isNewLine(const char c)
+{
+	return (c == '\r') || (c == '\n') || (c == '\0');
+}
+static inline float parseFloat(const char*& token)
+{
+	token += strspn(token, " \t");
+	float f = (float)atof(token);
+	token += strcspn(token, " \t\r");
+	return f;
+}
+static inline void parseFloat3(
+	float& x, float& y, float& z,
+	const char*& token)
+{
+	x = parseFloat(token);
+	y = parseFloat(token);
+	z = parseFloat(token);
+}
+
+void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb)
+{
+	std::ofstream fs;
+	fs.open(file);
+	btAssert(fs);
+	fs << std::scientific << std::setprecision(16);
+
+	// Only write out for trimesh, directly write out all the nodes and faces.xs
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		fs << "q";
+		for (int d = 0; d < 3; d++)
+		{
+			fs << " " << psb->m_nodes[i].m_q[d];
+		}
+		fs << "\n";
+	}
+
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		fs << "v";
+		for (int d = 0; d < 3; d++)
+		{
+			fs << " " << psb->m_nodes[i].m_v[d];
+		}
+		fs << "\n";
+	}
+	fs.close();
+}
+
+std::string btSoftBodyHelpers::loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO)
+{
+	{
+		qs.clear();
+		vs.clear();
+		std::string tmp = filename;
+		std::stringstream err;
+#ifdef USE_STREAM
+		std::ifstream ifs(filename);
+		if (!ifs)
+		{
+			err << "Cannot open file [" << filename << "]" << std::endl;
+			return err.str();
+		}
+#else
+		int fileHandle = fileIO->fileOpen(filename, "r");
+		if (fileHandle < 0)
+		{
+			err << "Cannot open file [" << filename << "]" << std::endl;
+			return err.str();
+		}
+#endif
+
+		std::string name;
+
+		int maxchars = 8192;              // Alloc enough size.
+		std::vector<char> buf(maxchars);  // Alloc enough size.
+		std::string linebuf;
+		linebuf.reserve(maxchars);
+
+#ifdef USE_STREAM
+		while (ifs.peek() != -1)
+#else
+		char* line = 0;
+		do
+#endif
+		{
+			linebuf.resize(0);
+#ifdef USE_STREAM
+			safeGetline(ifs, linebuf);
+#else
+			char tmpBuf[1024];
+			line = fileIO->readLine(fileHandle, tmpBuf, 1024);
+			if (line)
+			{
+				linebuf = line;
+			}
+#endif
+			// Trim newline '\r\n' or '\r'
+			if (linebuf.size() > 0)
+			{
+				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
+			}
+			if (linebuf.size() > 0)
+			{
+				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
+			}
+
+			// Skip if empty line.
+			if (linebuf.empty())
+			{
+				continue;
+			}
+
+			// Skip leading space.
+			const char* token = linebuf.c_str();
+			token += strspn(token, " \t");
+
+			btAssert(token);
+			if (token[0] == '\0') continue;  // empty line
+
+			if (token[0] == '#') continue;  // comment line
+
+			// q
+			if (token[0] == 'q' && isSpace((token[1])))
+			{
+				token += 2;
+				float x, y, z;
+				parseFloat3(x, y, z, token);
+				qs.push_back(btVector3(x, y, z));
+				continue;
+			}
+
+			// v
+			if (token[0] == 'v' && isSpace((token[1])))
+			{
+				token += 3;
+				float x, y, z;
+				parseFloat3(x, y, z, token);
+				vs.push_back(btVector3(x, y, z));
+				continue;
+			}
+
+			// Ignore unknown command.
+		}
+#ifndef USE_STREAM
+		while (line)
+			;
+#endif
+
+		if (fileHandle >= 0)
+		{
+			fileIO->fileClose(fileHandle);
+		}
+		return err.str();
+	}
+}
+
 void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
 {
 	std::ifstream fs_read;

src/BulletSoftBody/btSoftBodyHelpers.h

@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_SOFT_BODY_HELPERS_H
 
 #include "btSoftBody.h"
+#include "../../examples/CommonInterfaces/CommonFileIOInterface.h"
 #include <fstream>
 #include <string>
 //
@@ -146,6 +147,10 @@ struct btSoftBodyHelpers
 
 	static void writeObj(const char* file, const btSoftBody* psb);
 
+	static void writeState(const char* file, const btSoftBody* psb);
+
+	static std::string loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO);
+
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
 
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);

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;
@@ -1944,6 +1960,7 @@ struct btSoftColliders
 					c.m_weights = btVector3(0, 0, 0);
 					c.m_imf = 0;
 					c.m_c0 = 0;
+					c.m_colObj = psb[1];
 					psb[0]->m_faceNodeContacts.push_back(c);
 				}
 			}
@@ -2019,6 +2036,7 @@ struct btSoftColliders
 				c.m_weights = btVector3(0, 0, 0);
 				c.m_imf = 0;
 				c.m_c0 = 0;
+				c.m_colObj = psb[1];
 				psb[0]->m_faceNodeContacts.push_back(c);
 			}
 		}
@@ -2050,7 +2068,8 @@ struct btSoftColliders
 				c.m_margin = mrg;
 				c.m_imf = 0;
 				c.m_c0 = 0;
-				psb[0]->m_faceNodeContacts.push_back(c);
+				c.m_colObj = psb[1];
+				psb[0]->m_faceNodeContactsCCD.push_back(c);
 			}
 		}
 		void Process(const btDbvntNode* lface1,
@@ -2114,7 +2133,8 @@ struct btSoftColliders
 					c.m_margin = mrg;
 					c.m_imf = 0;
 					c.m_c0 = 0;
-					psb[0]->m_faceNodeContacts.push_back(c);
+					c.m_colObj = psb[1];
+					psb[0]->m_faceNodeContactsCCD.push_back(c);
 				}
 			}
 		}

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: