I'm trying to implement a constraint that fixes the relative orientation and position of two rigid bodies. My implementation is based on this document http://danielchappuis.ch/download/ConstraintsDerivationRigidBody3D.pdf

The resulting velocity adjustments do not satisfy the constraint and I don't know that I'm doing wrong.

#include <Eigen/Eigen>

using namespace Eigen;

typedef double Real;
typedef Eigen::Matrix<Real, 3, 3> Matrix3r;
typedef Eigen::Matrix<Real, 3, 1> Vector3r;
typedef Eigen::Quaternion<Real> Quaternionr;
typedef Eigen::AlignedBox<Real, 3> AlignedBox3r;

template <typename Scalar>
inline Eigen::Matrix<Scalar, 3, 3> crossProductMatrix(const Eigen::Matrix<Scalar, 3, 1> &v)
{
	Eigen::Matrix<Scalar, 3, 3> v_hat;
	v_hat << 0, -v(2), v(1),
		v(2), 0, -v(0),
		-v(1), v(0), 0;
	return v_hat;
}

struct RigidBody
{
	Matrix3r R; // world-space rotation of the rigid body
	Vector3r x; // world-space position of the rigid body
	Vector3r v; // linear velocity
	Vector3r w; // angular velocity
	Matrix3r Iinv; // local-space inertia tensor
	Real invmass;
};

Matrix3r compute_inertia_tensor(const AlignedBox3r& box, Real m)
{
	const Real a = box.sizes().x();
	const Real b = box.sizes().y();
	const Real c = box.sizes().z();

	return (Real(1.0 / 3.0) * m * Vector3r {
		b * b + c * c,
		a * a + c * c,
		a * a + b * b
	}).asDiagonal();
}

void computeMatrixK(
	const Vector3r &connector,
	const Real invMass,
	const Vector3r &x,
	const Matrix3r &inertiaInverseW,
	Matrix3r &K)
{
	const Vector3r r = connector - x;
	K.setZero();
	K += invMass * Matrix3r::Identity();
	K += crossProductMatrix(r) * inertiaInverseW * crossProductMatrix(r).transpose();
}

bool init_RigidBodyFixedConstraint(
	const Real invMass0,					// inverse mass is zero if body is static
	const Vector3r &x0,						// center of mass of body 0
	const Vector3r &v0,						// velocity of body 0
	const Matrix3r &inertiaInverseW0,		// inverse inertia tensor (world space) of body 0
	const Vector3r &omega0,					// angular velocity of body 0
	const Real invMass1,					// inverse mass is zero if body is static
	const Vector3r &x1,						// center of mass of body 1
	const Vector3r &v1,						// velocity of body 1
	const Matrix3r &inertiaInverseW1,		// inverse inertia tensor (world space) of body 1
	const Vector3r &omega1,					// angular velocity of body 1
	const Vector3r &connector,              // world-space coordinates of anchor
	Eigen::Matrix<Real, 3, 7> &constraintInfo)
{
	Matrix3r K1, K2;
	computeMatrixK(connector, invMass0, x0, inertiaInverseW0, K1);
	computeMatrixK(connector, invMass1, x1, inertiaInverseW1, K2);
	const Matrix3r inv_K_trans = (K1 + K2).inverse();
	const Matrix3r inv_K_rot = (inertiaInverseW0 + inertiaInverseW1).inverse();

	constraintInfo.block<3, 1>(0, 0) = connector;
	constraintInfo.block<3, 3>(0, 1) = inv_K_trans;
	constraintInfo.block<3, 3>(0, 4) = inv_K_rot;

	return true;
}

bool velocitySolve_RigidBodyFixedConstraint(
	const Real invMass0,					// inverse mass is zero if body is static
	const Vector3r &x0, 					// center of mass of body 0
	const Vector3r &v0,						// velocity of body 0
	const Matrix3r &inertiaInverseW0,		// inverse inertia tensor (world space) of body 0
	const Vector3r &omega0,					// angular velocity of body 0
	const Real invMass1,					// inverse mass is zero if body is static
	const Vector3r &x1, 					// center of mass of body 1
	const Vector3r &v1,						// velocity of body 1
	const Matrix3r &inertiaInverseW1,		// inverse inertia tensor (world space) of body 1
	const Vector3r &omega1,					// angular velocity of body 1
	Eigen::Matrix<Real, 3, 7> &constraintInfo,	// precomputed constraint info
	Vector3r &corr_v0, Vector3r &corr_omega0,
	Vector3r &corr_v1, Vector3r &corr_omega1)
{
	const Vector3r connector = constraintInfo.block<3, 1>(0, 0);
	const Matrix3r invKt = constraintInfo.block<3, 3>(0, 1);
	const Matrix3r invKr = constraintInfo.block<3, 3>(0, 4);

	const Vector3r r0 = connector - x0;
	const Vector3r r1 = connector - x1;

	const Vector3r J_trans_v = (v1 + omega1.cross(r1)) - (v0 + omega0.cross(r0));
	const Vector3r J_rot_v = omega1 - omega0;

	const Vector3r lambda_trans = -invKt * J_trans_v;
	const Vector3r lambda_rot = -invKr * J_rot_v;

	corr_v0.setZero();
	corr_omega0.setZero();

	corr_v1.setZero();
	corr_omega1.setZero();

	if (invMass0 != 0.0f)
	{
		corr_v0 -= invMass0 * lambda_trans;
		corr_omega0 -= inertiaInverseW0 * r0.cross(lambda_trans);
		corr_omega0 -= inertiaInverseW0 * lambda_rot;
	}

	if (invMass1 != 0.0f)
	{
		corr_v1 += invMass1 * lambda_trans;
		corr_omega1 += inertiaInverseW1 * r1.cross(lambda_trans);
		corr_omega1 += inertiaInverseW1 * lambda_rot;
	}

	return true;
}

Real evalError(
	const Vector3r &x0, 					// center of mass of body 0
	const Vector3r &v0,						// velocity of body 0
	const Vector3r &omega0,					// angular velocity of body 0
	const Vector3r &x1, 					// center of mass of body 1
	const Vector3r &v1,						// velocity of body 1
	const Vector3r &omega1,					// angular velocity of body 1
	const Vector3r &connector)
{
	const Vector3r r0 = connector - x0;
	const Vector3r r1 = connector - x1;

	const Vector3r J_trans_v = v1 + omega1.cross(r1) - v0 - omega0.cross(r0);
	const Vector3r J_rot_v = -omega0 + omega1;

	return J_trans_v.norm() + J_rot_v.norm();
}

int main(int argc, char ** argv)
{
	const Real extents = 2.0f; // each cube is 2x2x2

	RigidBody A, B;
	A.x = Vector3r{ 0.0f, 1.0f, 0.0f };
	B.x = A.x + Vector3r::UnitY() * (1.0f * extents);

	const AlignedBox3r box{
		Vector3r{ -1.0f, -1.0f, -1.0f },
		Vector3r{ +1.0f, +1.0f, +1.0f }
	};
	const Real massA = 20.0f;
	const Real massB = 40.0f;
	A.Iinv = compute_inertia_tensor(box, massA).inverse();
	B.Iinv = compute_inertia_tensor(box, massB).inverse();
	A.invmass = massA == 0.0 ? 0.0 : 1.0 / massA;
	B.invmass = massB == 0.0 ? 0.0 : 1.0f / massB;

	B.v = Vector3r::UnitX() * +1.0f;
	A.v = Vector3r::UnitY() * -1.0f;

	//A.w.setZero();
	//B.w.setZero();

	B.w = { 0.1f, 0.2f, 0.3f };
	A.w = { 0.4f, 0.1f, 0.7f };

	A.R.setIdentity();
	B.R.setIdentity();

	// world-space inverse inertia tensors
	const Matrix3r inertiaInverseW1 = A.R * A.Iinv * A.R.transpose();
	const Matrix3r inertiaInverseW2 = B.R * B.Iinv * B.R.transpose();

	const Vector3r connector{ 0.0f, 2.0f, 0.0f };

	Eigen::Matrix<Real, 3, 7> constraintInfo;
	init_RigidBodyFixedConstraint(
		A.invmass, A.x, A.v, inertiaInverseW1, A.w,
		B.invmass, B.x, B.v, inertiaInverseW2, B.w,
		connector, constraintInfo);

	for (int i = 0; i < 1; ++i)
	{
		Vector3r corr_v0, corr_omega0;
		Vector3r corr_v1, corr_omega1;

		velocitySolve_RigidBodyFixedConstraint(
			A.invmass, A.x, A.v, inertiaInverseW1, A.w,
			B.invmass, B.x, B.v, inertiaInverseW2, B.w,
			constraintInfo,
			corr_v0, corr_omega0,
			corr_v1, corr_omega1);

		A.v += corr_v0;
		A.w += corr_omega0;

		B.v += corr_v1;
		B.w += corr_omega1;
	}

	const Real error = evalError(A.x, A.v, A.w, B.x, B.v, B.w, connector);

	return 0;
}

Your expectation is wrong. You are currently using an iterative Jacobi block solver on a 6x6  linear system. Using the new angular velocities after you applied the linear impulse would get you a Gauss-Seidel iterative solver which has better convergence. Note that there is no guarantee for convergence. You would need to solve a 6x6 system for the correct analytical solution.

When checking the error I would return two 3d vectors: the relative linear velocity at the pivot and the relative angular velocity. I would also print this each iteration. The error (length of each vector) should become smaller each iteration. The error would not be zero after ONE iteration.  With Gauss-Seidel solving I would expect the error to be reasonably small after 5-10 iterations depending on the configuration (e.g. convergence will most depend on the inertia tensor and relative pivot location to the mass centers).

One other thing. From a quick look I think the rotation constraint derivation in the referenced paper is not correct. The time derivative of the angle triplet is not the angular velocity. This is only true in 2D. Using angles for orientation is always problematic. If you use any triple of three angles small changes in orientation can result in large changes in any of the angles which gives you inconsistent behavior. You are not using a stabilization term yet, but later you need this. One solution is to use orthogonality constraints to fix the relative orientation.

HTH,

-Dirk

I don't know whether the orientation constraint derivation is correct or not, but it looks like the same Jacobian is used by other people as well, e.g http://www.randygaul.net/2013/11/10/orientation-constraint-derivation/

Using the updated velocities when calculating the rotation impulse greatly improves convergence, so I think I can use it for my intended purpose. I don't think that I will need any stabilization since I will be using it for a simplified structural integrity solver where nothing actually moves.

Looks like totally different Jacobians to me. In the first one J = ( 0 -E3 0 E ) which is constant while Randy uses the t vectors which would be cross products e.g. xa and yb. I think his equation (1) is a bit unlucky and makes it look the same though.

I spend quite some time to use weld joints for structural analysis. To put in famous words of a current movie: "This is not going the way you think". Reasons are plentiful and complicated.  For simple structural analysis to detect unstable overhangs (e.g. games) simple geometric approaches work best from my experience. If you need real structural analysis I suggest looking into the "Direct Stiffness Method" This is also a nice and non-mathematical entry into the world of finite elements. 

https://en.wikipedia.org/wiki/Direct_stiffness_method

There are some really good resources I can share if you are interested.

HTH,

-Dirk

I have implemented something similar to http://matthias-mueller-fischer.ch/publications/fractureSG2013.pdf

In that paper they write "Also, in nature, objects do not necessarily fracture at the impact location only. Both problems can be solved by performing a simplified stress analysis on the connectivity graph of a compound". It also seems that Nvidia Blast does something very similar (in Blast\sdk\extensions\stress\source\NvBlastExtStressSolver.cpp).

My first approach was to voxelize the compounds and then compute deformation using FEM, but it takes forever for an iterative solver to converge when dealing with very stiff materials.

Looks nice! Let me know how it goes.

From my experience convergence will be an issue here as well. Even for reasonable structures with only a few bodies. You can try warm-starting and only run a couple of iterations per step. This will amortize your costs over several frames. Be aware that the iterative solver can create large impulses in early phases (after only a few iterations). So you need to run for some time before making a break decision. I applied a low-pass filter, but that made things soft. In the end I stopped and used a connectivity graph since it felt like going circles.

5 hours ago, _swx_ said:

I don't know whether the orientation constraint derivation is correct or not, but it looks like the same Jacobian is used by other people as well, e.g http://www.randygaul.net/2013/11/10/orientation-constraint-derivation/

Using the updated velocities when calculating the rotation impulse greatly improves convergence, so I think I can use it for my intended purpose. I don't think that I will need any stabilization since I will be using it for a simplified structural integrity solver where nothing actually moves.

 

Just want to point out I wrote that before I knew much calculus. Although I did get it working, I didn't test it out much and the results weren't all that great. So probably not a good idea to use my link as a reference