- use XPBD to implement an implicit spring

- added distance joint for rigid bodies

Author: janbender

Changelog.txt

@@ -1,3 +1,5 @@
+	- use XPBD to implement an implicit spring
+	- added distance joint for rigid bodies
 	- small optimizations
 	- fixed single precision build
 	- cleanup code

Demos/Common/DemoBase.cpp

@@ -127,6 +127,7 @@ void DemoBase::cleanup()
 	m_scene.m_targetPositionMotorSliderJointData.clear();
 	m_scene.m_targetVelocityMotorSliderJointData.clear();
 	m_scene.m_rigidBodySpringData.clear();
+	m_scene.m_distanceJointData.clear();
 }
 
 void DemoBase::init(int argc, char **argv, const char *demoName)
@@ -384,6 +385,10 @@ void DemoBase::render()
 		{
 			renderSpring(*(RigidBodySpring*)constraints[i]);
 		}
+		else if (constraints[i]->getTypeId() == DistanceJoint::TYPE_ID)
+		{
+			renderDistanceJoint(*(DistanceJoint*)constraints[i]);
+		}
 	}
 
 
@@ -695,6 +700,13 @@ void DemoBase::renderSpring(RigidBodySpring &s)
 	MiniGL::drawCylinder(s.m_jointInfo.col(2), s.m_jointInfo.col(3), m_jointColor, 0.05f);
 }
 
+void DemoBase::renderDistanceJoint(DistanceJoint &j)
+{
+	MiniGL::drawSphere(j.m_jointInfo.col(2), 0.1f, m_jointColor);
+	MiniGL::drawSphere(j.m_jointInfo.col(3), 0.1f, m_jointColor);
+	MiniGL::drawCylinder(j.m_jointInfo.col(2), j.m_jointInfo.col(3), m_jointColor, 0.05f);
+}
+
 void DemoBase::mouseMove(int x, int y, void *clientData)
 {
 	DemoBase *base = (DemoBase*)clientData;

Demos/Common/DemoBase.h

@@ -66,6 +66,7 @@ namespace PBD
 		void renderRigidBodyContact(RigidBodyContactConstraint &cc);
 		void renderParticleRigidBodyContact(ParticleRigidBodyContactConstraint &cc);
 		void renderSpring(RigidBodySpring &s);
+		void renderDistanceJoint(DistanceJoint &j);
 
 	public:
 		static int PAUSE;

Demos/RigidBodyDemos/JointDemo.cpp

@@ -162,6 +162,9 @@ void render ()
 	MiniGL::drawStrokeText(6.6f, -4.0, 1.0, 0.002f, "target position motor", 21, textColor);
 	MiniGL::drawStrokeText(10.6f, -4.0, 1.0, 0.002f, "target velocity motor", 21, textColor);
 
+	MiniGL::drawStrokeText(-0.25, -9.5, 1.0, 0.002f, "spring", 6, textColor);
+	MiniGL::drawStrokeText(3.3, -9.5, 1.0, 0.002f, "distance joint", 14, textColor);
+
 	MiniGL::drawTime( TimeManager::getCurrent ()->getTime ());
 }
 
@@ -239,12 +242,18 @@ void createBodyModel()
 	loadObj(fileName, vdStatic, meshStatic, Vector3r(0.5, 0.5, 0.5));
 
 	// static body
-	const unsigned int numberOfBodies = 24;
+	const unsigned int numberOfBodies = 30;
 	rb.resize(numberOfBodies);
 	Real startX = 0.0;
-	Real startY = 1.0;
-	for (unsigned int i = 0; i < 8; i++)
+	Real startY = 6.5;
+	for (unsigned int i = 0; i < 10; i++)
 	{
+		if (i % 4 == 0)
+		{
+			startY -= 5.5;
+			startX = 0.0;
+		}
+
 		rb[3*i] = new RigidBody();
 		rb[3*i]->initBody(0.0,
 			Vector3r(startX, startY, 1.0),
@@ -269,12 +278,6 @@ void createBodyModel()
 			vd, mesh);
 
 		startX += 4.0;
-
-		if (i == 3)
-		{
-			startY -= 5.5;
-			startX = 0.0;
-		}
 	}
 
 	Real jointY = 0.75;
@@ -302,6 +305,13 @@ void createBodyModel()
 
 	model->addTargetVelocityMotorSliderJoint(21, 22, Vector3r(12.0, jointY, 1.0), Vector3r(1.0, 0.0, 0.0));
 	model->addBallJoint(22, 23, Vector3r(12.25, jointY, 3.0));
+
+	jointY -= 5.5;
+	model->addRigidBodySpring(24, 25, Vector3r(0.25, jointY, 1.0), Vector3r(0.25, jointY, 1.0), 50.0);
+	model->addBallJoint(25, 26, Vector3r(0.25, jointY, 3.0));
+
+	model->addDistanceJoint(27, 28, Vector3r(4.25, jointY, 1.0), Vector3r(4.25, jointY, 2.0));
+	model->addBallJoint(28, 29, Vector3r(4.25, jointY, 3.0));
 }
 
 

Demos/SceneLoaderDemo/SceneLoaderDemo.cpp

@@ -1075,6 +1075,12 @@ void readScene(const bool readFile)
 		const SceneLoader::RigidBodySpringData &jd = data.m_rigidBodySpringData[i];
 		model->addRigidBodySpring(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position1, jd.m_position2, jd.m_stiffness);
 	}
+
+	for (unsigned int i = 0; i < data.m_distanceJointData.size(); i++)
+	{
+		const SceneLoader::DistanceJointData &jd = data.m_distanceJointData[i];
+		model->addDistanceJoint(id_index[jd.m_bodyID[0]], id_index[jd.m_bodyID[1]], jd.m_position1, jd.m_position2);
+	}
 }
 
 void TW_CALL setContactTolerance(const void *value, void *clientData)

Demos/StiffRodsDemos/StiffRodsSceneLoader.cpp

@@ -609,6 +609,12 @@ void PBD::StiffRodsSceneLoader::readStiffRodsScene(const std::string &fileName,
 		if (m_json.find("RigidBodySprings") != m_json.end())
 			readRigidBodySprings(m_json, "RigidBodySprings", sceneData);
 
+		//////////////////////////////////////////////////////////////////////////
+		// read DistanceJoints
+		//////////////////////////////////////////////////////////////////////////
+		if (m_json.find("DistanceJoints") != m_json.end())
+			readDistanceJoints(m_json, "DistanceJoints", sceneData);
+
 		//////////////////////////////////////////////////////////////////////////
 		// read TargetAngleMotorHingeJoint
 		//////////////////////////////////////////////////////////////////////////

PositionBasedDynamics/PositionBasedRigidBodyDynamics.cpp

@@ -195,7 +195,7 @@ bool PositionBasedRigidBodyDynamics::solve_BallJoint(
 }
 
 // ----------------------------------------------------------------------------------------------
-bool PositionBasedRigidBodyDynamics::init_Spring(
+bool PositionBasedRigidBodyDynamics::init_DistanceJoint(
 	const Vector3r &x0,
 	const Quaternionr &q0,
 	const Vector3r &x1,
@@ -224,7 +224,7 @@ bool PositionBasedRigidBodyDynamics::init_Spring(
 }
 
 // ----------------------------------------------------------------------------------------------
-bool PositionBasedRigidBodyDynamics::update_Spring(
+bool PositionBasedRigidBodyDynamics::update_DistanceJoint(
 	const Vector3r &x0,
 	const Quaternionr &q0,
 	const Vector3r &x1,
@@ -249,7 +249,7 @@ bool PositionBasedRigidBodyDynamics::update_Spring(
 
 
 // ----------------------------------------------------------------------------------------------
-bool PositionBasedRigidBodyDynamics::solve_Spring(
+bool PositionBasedRigidBodyDynamics::solve_DistanceJoint(
 	const Real invMass0,
 	const Vector3r &x0,
 	const Matrix3r &inertiaInverseW0,
@@ -259,8 +259,10 @@ bool PositionBasedRigidBodyDynamics::solve_Spring(
 	const Matrix3r &inertiaInverseW1,
 	const Quaternionr &q1,
 	const Real stiffness,
-	const Real restLength,
+	const Real restLength,	
+	const Real dt,
 	const Eigen::Matrix<Real, 3, 4> &jointInfo,
+	Real &lambda,
 	Vector3r &corr_x0, Quaternionr &corr_q0,
 	Vector3r &corr_x1, Quaternionr &corr_q1)
 {
@@ -270,14 +272,38 @@ bool PositionBasedRigidBodyDynamics::solve_Spring(
 	// 2:	connector in body 0 (global)
 	// 3:	connector in body 1 (global)
 
-	const Vector3r &connector0 = jointInfo.col(2);
-	const Vector3r &connector1 = jointInfo.col(3);
+	// evaluate constraint function
+	const Vector3r &c0 = jointInfo.col(2);
+	const Vector3r &c1 = jointInfo.col(3);
+	const Real length = (c0 - c1).norm();
 
-	Vector3r dir = connector1 - connector0;
-	Real length = dir.norm();
-	dir = (1.0 / length) * dir;
+	const Real C = (length - restLength);
 
-	const Vector3r pt = stiffness * (length- restLength) * dir;
+	// compute K = J M^-1 J^T
+	Matrix3r K1, K2;
+	computeMatrixK(c0, invMass0, x0, inertiaInverseW0, K1);
+	computeMatrixK(c1, invMass1, x1, inertiaInverseW1, K2);
+	Vector3r dir = c0 - c1;
+	if (length > static_cast<Real>(1e-6))
+		dir /= length;
+
+	Real K = (dir.transpose() * (K1 + K2)).dot(dir);
+
+	Real alpha = 0.0;
+	if (stiffness != 0)
+	{
+		alpha = 1.0 / (stiffness * dt * dt);
+		K += alpha;
+	}
+	
+	Real Kinv = 0.0;
+	if (fabs(K) > static_cast<Real>(1e-6))
+		Kinv = 1.0 / K;
+
+	const Real delta_lambda = -Kinv * (C + alpha * lambda);
+	lambda += delta_lambda;
+
+	const Vector3r pt = dir * delta_lambda;
 
 	corr_x0.setZero();
 	corr_q0.coeffs().setZero();
@@ -286,8 +312,8 @@ bool PositionBasedRigidBodyDynamics::solve_Spring(
 
 	if (invMass0 != 0.0)
 	{
-		const Vector3r r0 = connector0 - x0;
-		corr_x0 = invMass0*pt;
+		const Vector3r r0 = c0 - x0;
+		corr_x0 = invMass0 * pt;
 
 		const Vector3r ot = (inertiaInverseW0 * (r0.cross(pt)));
 		const Quaternionr otQ(0.0, ot[0], ot[1], ot[2]);
@@ -296,8 +322,8 @@ bool PositionBasedRigidBodyDynamics::solve_Spring(
 
 	if (invMass1 != 0.0)
 	{
-		const Vector3r r1 = connector1 - x1;
-		corr_x1 = -invMass1*pt;
+		const Vector3r r1 = c1 - x1;
+		corr_x1 = -invMass1 * pt;
 
 		const Vector3r ot = (inertiaInverseW1 * (r1.cross(-pt)));
 		const Quaternionr otQ(0.0, ot[0], ot[1], ot[2]);

PositionBasedDynamics/PositionBasedRigidBodyDynamics.h

@@ -1191,24 +1191,24 @@ namespace PBD
 			Vector3r &corr_v1, Vector3r &corr_omega1);
 
 
-		/** Initialize ball joint and return info which is required by the solver step.
+		/** Initialize distance joint and return info which is required by the solver step.
 		* 
 		* @param x0 center of mass of first body
 		* @param q0 rotation of first body
 		* @param x1 center of mass of second body
 		* @param q1 rotation of second body
-		* @param jointPosition position of ball joint
+		* @param jointPosition position of distance joint
 		* @param jointInfo Stores the local and global positions of the connector points. 
 		* The first two columns store the local connectors in body 0 and 1, respectively, while
 		* the last two columns contain the global connector positions which have to be
-		* updated in each simulation step by calling update_BallJoint().\n
+		* updated in each simulation step by calling update_DistanceJoint().\n
 		* The joint info contains the following columns:\n
 		* 0:	connector in body 0 (local)\n
 		* 1:	connector in body 1 (local)\n
 		* 2:	connector in body 0 (global)\n
 		* 3:	connector in body 1 (global)		
 		*/
-		static bool init_Spring(			
+		static bool init_DistanceJoint(			
 			const Vector3r &x0, 						// center of mass of body 0
 			const Quaternionr &q0,					// rotation of body 0	
 			const Vector3r &x1, 						// center of mass of body 1
@@ -1218,35 +1218,33 @@ namespace PBD
 			Eigen::Matrix<Real, 3, 4> &jointInfo
 			);
 
-		/** Update ball joint info which is required by the solver step.
-		* The ball joint info must be generated in the initialization process of the model
-		* by calling the function init_BallJoint().
+		/** Update distance joint info which is required by the solver step.
+		* The distance joint info must be generated in the initialization process of the model
+		* by calling the function init_DistanceJoint().
 		* This method should be called once per simulation step before executing the solver.\n\n
 		*
 		* @param x0 center of mass of first body
 		* @param q0 rotation of first body
 		* @param x1 center of mass of second body
 		* @param q1 rotation of second body
-		* @param ballJointInfo ball joint information which should be updated
+		* @param jointInfo joint information which should be updated
 		*/
-		static bool update_Spring(
+		static bool update_DistanceJoint(
 			const Vector3r &x0, 						// center of mass of body 0
 			const Quaternionr &q0,					// rotation of body 0	
 			const Vector3r &x1, 						// center of mass of body 1
 			const Quaternionr &q1,					// rotation of body 1
 			Eigen::Matrix<Real, 3, 4> &jointInfo
 			);
 
-		/** Perform a solver step for a ball joint which links two rigid bodies.
-		* A ball joint removes three translational degrees of freedom between the bodies.
-		* The ball joint info must be generated in the initialization process of the model
-		* by calling the function init_BallJoint() and updated each time the bodies 
-		* change their state by update_BallJoint().\n\n
+		/** Perform a solver step for a distance joint which links two rigid bodies.
+		* A distance joint removes one translational degrees of freedom between the bodies.
+		* When setting a stiffness value which is not zero, we get an implicit spring.
+		* The distance joint info must be generated in the initialization process of the model
+		* by calling the function init_DistanceJoint() and updated each time the bodies 
+		* change their state by update_DistanceJoint().\n\n
 		* More information can be found in: \cite Deul2014
 		*
-		* \image html balljoint.jpg "ball joint"
-		* \image latex balljoint.jpg "ball joint" width=0.5\textwidth
-		*
 		* @param invMass0 inverse mass of first body
 		* @param x0 center of mass of first body
 		* @param inertiaInverseW0 inverse inertia tensor in world coordinates of first body
@@ -1255,15 +1253,19 @@ namespace PBD
 		* @param x1 center of mass of second body
 		* @param inertiaInverseW1 inverse inertia tensor in world coordinates of second body
 		* @param q1 rotation of second body
-		* @param ballJointInfo Ball joint information which is required by the solver. This 
-		* information must be generated in the beginning by calling init_BallJoint()
-		* and updated each time the bodies change their state by update_BallJoint().
+		* @param stiffness Stiffness of the constraint. 0 means it is totally stiff and we get a distance joint otherwise we get an implicit spring.
+		* @param restLength Rest length of the joint.
+		* @param dt Time step size (required for XPBD when simulating a spring)
+		* @param jointInfo Joint information which is required by the solver. This 
+		* information must be generated in the beginning by calling init_DistanceJoint()
+		* and updated each time the bodies change their state by update_DistanceJoint().
+		* @param lambda Lagrange multiplier (required for XPBD). Must be 0 in the first iteration.
 		* @param corr_x0 position correction of center of mass of first body
 		* @param corr_q0 rotation correction of first body
 		* @param corr_x1 position correction of center of mass of second body
 		* @param corr_q1 rotation correction of second body
 		*/
-		static bool solve_Spring(
+		static bool solve_DistanceJoint(
 			const Real invMass0,							// inverse mass is zero if body is static
 			const Vector3r &x0, 						// center of mass of body 0
 			const Matrix3r &inertiaInverseW0,		// inverse inertia tensor (world space) of body 0
@@ -1274,7 +1276,9 @@ namespace PBD
 			const Quaternionr &q1,					// rotation of body 1
 			const Real stiffness,
 			const Real restLength,
+			const Real dt,
 			const Eigen::Matrix<Real,3,4> &jointInfo,	// precomputed joint info
+			Real &lambda,
 			Vector3r &corr_x0, Quaternionr &corr_q0,
 			Vector3r &corr_x1, Quaternionr &corr_q1);
 	};

README.md

@@ -7,7 +7,6 @@ The PositionBasedDynamics library allows the position-based handling of many typ
 Furthermore we use our own library:
 - [Discregrid](https://github.com/InteractiveComputerGraphics/Discregrid/) to generate cubic signed distance fields for the collision detection
 
-The library was tested on Windows 10, Ubuntu 16.10 and Mac OS X 10.10.5. 
 
 **Author**: [Jan Bender](http://www.interactive-graphics.de), **License**: MIT
 
@@ -23,7 +22,7 @@ The library was tested on Windows 10, Ubuntu 16.10 and Mac OS X 10.10.5.
 
 This project is based on [CMake](https://cmake.org/). Simply generate project, Makefiles, etc. using [CMake](https://cmake.org/) and compile the project with the compiler of your choice. The code was tested with the following configurations:
 - Windows 10 64-bit, CMake 3.9.5, Visual Studio 2017
-- Ubuntu 16.10 64-bit, CMake 3.5.2, GCC 6.2.0.
+- Debian 9 64-bit, CMake 3.12.3, GCC 6.3.0.
 
 Note: Please use a 64-bit target on a 64-bit operating system. 32-bit builds on a 64-bit OS are not supported.
 
@@ -89,6 +88,8 @@ http://www.interactive-graphics.de/PositionBasedDynamics/doc/html
 		- target position motor slider joint
 		- target velocity motor slider joint
 		- ball joint between rigid body and particle
+		- distance joint
+		- implicit spring
 	- Generic constraints
 
 ## Videos