コード例 #1
0
void Solver::GaussSeidelLCP(const DMatrix& J, const DMatrix& W_Jt, const DMatrix& b, DMatrix& V, DMatrix& x, const DMatrix* lo, const DMatrix* hi)
{
	int maxIterations = 20;
	x.SetToZero();
	const int n = x.GetNumRows();

	DMatrix aDiag = J.diagonalProduct(W_Jt);
	float xi_prime;
	float xi;
	float delta_xi;

	while (maxIterations--)
	{
		for (int i = 0; i < n; i++)
		{
			// xi' = xi + 1/Aii (bi - Ai*x)
			// xi' = xi + 1/Aii (bi - J*Vi)
			xi = x.Get(i);
			xi_prime = xi;
			assert(aDiag.Get(i) != 0.0f);
			xi_prime += 1 / aDiag.Get(i) * (b.Get(i) - J.rowProduct(V, i).Get(0));// A.rowProduct(x, i).Get(0));// 

			if (lo && (xi_prime < lo->Get(i)))
			{
				xi_prime = lo->Get(i);
			}
			if (hi && xi_prime > hi->Get(i))
			{
				xi_prime = hi->Get(i);
			}
			// Update X-component with new result
			x.Set(i) = xi_prime;

			// Fix V component with latest X result by using delta between current
			// and previous result for the X component.
			// V' = V + W*Jt * delta_x
			float delta_xi = xi_prime - xi;
			const DMatrix& V_adjust = W_Jt.colProduct(delta_xi, i);
			V.AddSubMatrix(0, 0, V_adjust);
		}
	}
}
コード例 #2
0
void Solver::ComputeJointConstraints(float dt)
{
	// Magic Formula
	//
	// J * M^-1 * J^t * lamba = -1.0 * J * (1/dt*V + M^-1 * Fext)
	//
	// A x = b
	//
	// where
	//
	// A = J * M^-1 * J^t
	// x = lambda
	// b = -J * (1/dt*V + M^-1 * Fext)
	//
	const int numBodies			= m_rigidBodies.size();
	const int numConstraints	= m_constraints.size();
	if (numBodies == 0 || numConstraints == 0) return;
	//-------------------------------------------------------------------------
	// 1st - build our matrices - very bad to build them each frame, but
	//-------------------------------------------------------------------------
	// simpler to explain and implement this way
	DMatrix s(numBodies * 7, 1);							// pos & qrot
	DMatrix u(numBodies * 6, 1);							// vel & rotvel
	DMatrix s_next(numBodies * 7, 1);						// pos & qrot after timestep
	DMatrix u_next(numBodies * 6, 1);						// vel & rotvel after timestep
	DMatrix S(numBodies * 7, numBodies * 6);
	DMatrix& MInverse = m_MInverseBuffer;
	MInverse.Resize(numBodies * 6, numBodies * 6, true);	// ToDo - This may be mostly static!
	DMatrix Fext(numBodies * 6, 1);
	setUpBodyMatricies(s, u, s_next, u_next, S, MInverse, Fext);
	//-------------------------------------------------------------------------
	// 2nd - apply constraints
	//-------------------------------------------------------------------------
	// Determine the size of our jacobian matrix
	int numRows = 0;
	for (int i = 0; i<numConstraints; i++)
	{
		const Constraint_c* constraint = m_constraints[i];
		assert(constraint);
		numRows += constraint->GetDimension();
	}
	// Allocate it, and fill it
	DMatrix minForces(numRows, 1);
	DMatrix maxForces(numRows, 1);
	DMatrix& J = m_jacobianBuffer;
	J.Resize(numRows, 6 * numBodies, /*memReset*/true);
	DMatrix rst(numRows, numRows);			// Restitution
	DMatrix& s_err = m_cBuffer;
	s_err.Resize(numRows, 1, true);
	int constraintRow = 0;
	for (int c = 0; c<numConstraints; c++)
	{
		Constraint_c* constraint = m_constraints[c];
		assert(constraint);
		DMatrix minLimit(constraint->GetDimension(), 1);
		DMatrix maxLimit(constraint->GetDimension(), 1);

		for (int r = 0; r<numBodies; r++)
		{
			const RigidBody_c* rigidBody = m_rigidBodies[r];
			assert(rigidBody);
			DMatrix JMat = constraint->GetJacobian(rigidBody);
			if (JMat.GetNumCols() == 0 && JMat.GetNumRows() == 0)
				continue;
			assert(JMat.GetNumCols() != 0);
			assert(JMat.GetNumRows() != 0);

			J.SetSubMatrix(constraintRow, r * 6, JMat);
			minLimit.AddSubMatrix(0, 0, constraint->GetLowerLimits(rigidBody));
			maxLimit.AddSubMatrix(0, 0, constraint->GetUpperLimits(rigidBody));

			// Delta position needed to depenetrate bodies 
			// (Unfortunately doesn't use constraints yet) (MAY NEED FIX)
		}
		minForces.AddSubMatrix(constraintRow, 0, minLimit);
		maxForces.AddSubMatrix(constraintRow, 0, maxLimit);
		rst.AddSubMatrix(constraintRow, constraintRow, constraint->GetRestitution());		// elasticity
		s_err.AddSubMatrix(constraintRow, 0, constraint->GetPenalty());						// positional correction

		constraintRow += constraint->GetDimension();
	}

	if (UseJVSolverOpt)
	{
		DMatrix& W_Jt = m_MInverseJtBuffer;
		DMatrix& V = m_virtualDisplacementBuffer;
		DMatrix& b = m_bBuffer;
		DMatrix& x = m_resultImpulseBuffer;
		W_Jt = MInverse * DMatrix::Transpose(J);
		b = rst*J*(u)+J*(dt*MInverse*Fext);
		// TODO - To implement Caching/Warmstarting we have to pre-set x and V with previous Data!
		x.Resize(J.GetNumRows(), b.GetNumCols(), /*memReset*/ true);
		V.Resize(W_Jt.GetNumRows(), x.GetNumCols(), /*memReset*/ true);

		// Solve for x [Velocity Stage]
		GaussSeidelLCP(J, W_Jt, b, V, x, &minForces, &maxForces);

		DMatrix& y = m_resultPositionCorrectionBuffer;
		DMatrix& Z = m_virtualCorrectionBuffer;
		y.Resize(J.GetNumRows(), b.GetNumCols(), /*memReset*/ true);
		Z.Resize(W_Jt.GetNumRows(), y.GetNumCols(), /*memReset*/ true);

		// Solve for y [Position Stage]
		GaussSeidelLCP(J, W_Jt, s_err, Z, y, nullptr, nullptr);

		u_next = u - W_Jt*x + dt*MInverse*Fext;
		s_next = s + dt*S*u_next - S*W_Jt*y * Config::positionalCorrectionFactor;
	}
	else
	{
		// Velocity Gauss Seidel
		DMatrix Jt = DMatrix::Transpose(J);
		DMatrix A = J*MInverse*Jt;
		DMatrix b = rst*J*(u)+J*(dt*MInverse*Fext);          
		DMatrix x(A.GetNumRows(), b.GetNumCols());

		DMatrix* lo = NULL; // Don’t set any min/max boundaries for this demo/sample
		DMatrix* hi = NULL;

		// Solve for x
		//GaussSeidelLCP(A, b, &x, lo, hi);
		GaussSeidelLCP(A, b, &x, &minForces, &maxForces);

		// Positional Correction Gauss Seidel
		DMatrix c = s_err;
		DMatrix y(A.GetNumRows(), b.GetNumCols());
		GaussSeidelLCP(A, c, &y, lo, hi);

		u_next = u - MInverse*Jt*x + dt*MInverse*Fext;
		s_next = s + dt*S*u_next - S*MInverse*Jt*y * Config::positionalCorrectionFactor;
	}

	// Basic integration without - euler integration standalone
	// u_next = u + dt*MInverse*Fext;
	// s_next = s + dt*S*u_next;
	//-------------------------------------------------------------------------
	// 3rd – re-inject solved values back into the simulator
	//-------------------------------------------------------------------------
	applyIntegrationOnRigidBodies(s_next, u_next);
}