private void HardConstraintsStep()
{
// Vectors and matrices to store data
VectorXD C0 = new DenseVectorXD(m_numcs);
VectorXD f = new DenseVectorXD(m_numdofs);
VectorXD v = new DenseVectorXD(m_numdofs);
MatrixXD M = new DenseMatrixXD(m_numdofs, m_numdofs);
MatrixXD J = new DenseMatrixXD(m_numcs, m_numdofs);
MatrixXD System = new DenseMatrixXD(m_numcs + m_numdofs, m_numcs + m_numdofs);
VectorXD Independent = new DenseVectorXD(m_numdofs + m_numcs);
// Compute forces and retrieve the rigid bodies data
foreach (ISimulable i in m_objs)
{
i.clearForcesAndMatrices();
i.addForcesAndMatrices();
i.getForceVector(f);
i.getVelocityVector(v);
i.getMassMatrix(M);
}
// Compute and retrieve restrictions and Jacobians
foreach (Constraint c in m_constraints)
{
c.getConstraintVector(C0);
c.getConstraintJacobian(J);
}
// Set up left-side system
System.SetSubMatrix(0, m_numdofs, 0, m_numdofs, M);
System.SetSubMatrix(m_numdofs, m_numcs, 0, m_numdofs, J);
System.SetSubMatrix(0, m_numdofs, m_numdofs, m_numcs, J.Transpose());
System.SetSubMatrix(m_numdofs, m_numcs, m_numdofs, m_numcs, DenseMatrixXD.Create(m_numcs, m_numcs, 0.0));
// Set up right-side system
VectorXD b = M * v + TimeStep * f;
VectorXD AtC0 = (-1.0f / TimeStep) * C0;
Independent.SetSubVector(0, m_numdofs, b);
Independent.SetSubVector(m_numdofs, m_numcs, AtC0);
// Solve system
VectorXD newVelocities = System.Solve(Independent);
// Update bodies
foreach (ISimulable i in m_objs)
{
i.setVelocityVector(newVelocities);
i.advancePosition();
}
}