/// <summary>
/// Performs a simulation step using Symplectic integration.
/// </summary>
private void stepSymplectic()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
Minv.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
v += TimeStep * (Minv * f);
x += TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Performs a simulation step using Symplectic integration with constrained dynamics.
/// The constraints are treated as implicit
/// </summary>
private void stepSymplecticConstraints()
{
// TO BE COMPLETED
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
MatrixXD J = new DenseMatrixXD(m_numConstraints, m_numDoFs);
MatrixXD A = new DenseMatrixXD(m_numDoFs);
VectorXD B = new DenseVectorXD(m_numDoFs);
VectorXD c = new DenseVectorXD(m_numConstraints);
VectorXD b = new DenseVectorXD(m_numDoFs);
VectorXD lamda = new DenseVectorXD(m_numConstraints);
MatrixXD I = DenseMatrixXD.CreateIdentity(m_numDoFs);
f.Clear();
Minv.Clear();
J.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetVelocity(v);
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (IConstraint constraint in m_constraints)
{
constraint.GetForce(f);
constraint.GetConstraints(c);
constraint.GetConstraintJacobian(J);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
b = v + TimeStep * Minv * f;
A = J * I * J.Transpose();
B = J * I * b + (1.0f / TimeStep) * c;
lamda = A.Solve(B);
v = I * (b - J.Transpose() * lamda);
VectorXD x = TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.AdvanceIncrementalPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Performs a simulation step using Verlet integration.
/// </summary>
private void stepVerlet()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
Minv.Clear();
VectorXD x0 = x;
//Compute forces at t0
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
if (first)
{
x = x + TimeStep * v + 0.5f * TimeStep * TimeStep * (Minv * f);
first = false;
}
else
{
x = 2 * x - xOld + TimeStep * TimeStep * (Minv * f);
}
v = (1.0f / TimeStep) * (x - x0);
xOld = x0;
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Performs a simulation step using Implicit integration.
/// </summary>
private void stepImplicit()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD M = new DenseMatrixXD(m_numDoFs);
MatrixXD Jk = new DenseMatrixXD(m_numDoFs);
MatrixXD Jd = new DenseMatrixXD(m_numDoFs);
MatrixXD A = new DenseMatrixXD(m_numDoFs);
VectorXD B = new DenseVectorXD(m_numDoFs);
M.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMass(M);
obj.GetForceJacobian(Jk, Jd);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(M);
}
A = M + TimeStep * Jd + TimeStep * TimeStep * -Jk;
B = (M + TimeStep * Jd) * v + TimeStep * f;
v = A.Solve(B);
x += TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.FixVector(v);
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Performs a simulation step using Symplectic integration.
/// </summary>
private void stepSymplectic()
{
// TO BE COMPLETED
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
f.Clear();
Minv.Clear();
foreach (ISimulable obj in m_objs)
{
obj.GetVelocity(v);
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (IConstraint constraint in m_constraints)
{
constraint.GetForce(f);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
v += TimeStep * (Minv * f);
VectorXD x = TimeStep * v;
foreach (ISimulable obj in m_objs)
{
obj.AdvanceIncrementalPosition(x);
obj.SetVelocity(v);
}
}
/// <summary>
/// Performs a simulation step using Midpoint integration.
/// </summary>
private void stepMidpoint()
{
// TO BE COMPLETED
VectorXD x = new DenseVectorXD(m_numDoFs);
VectorXD v = new DenseVectorXD(m_numDoFs);
VectorXD f = new DenseVectorXD(m_numDoFs);
MatrixXD Minv = new DenseMatrixXD(m_numDoFs);
Minv.Clear();
VectorXD x0 = x;
VectorXD v0 = v;
//Compute forces at t0
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
//Integrate to h/2
x += 0.5f * TimeStep * v;
v += 0.5f * TimeStep * (Minv * f);
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
//Compute forces at h/2
foreach (ISimulable obj in m_objs)
{
obj.GetPosition(x);
obj.GetVelocity(v);
obj.ComputeForces();
obj.GetForce(f);
obj.GetMassInverse(Minv);
}
foreach (ISimulable obj in m_objs)
{
obj.FixVector(f);
obj.FixMatrix(Minv);
}
//Integrate to h
x = x0 + TimeStep * v;
v = v0 + TimeStep * (Minv * f);
foreach (ISimulable obj in m_objs)
{
obj.SetPosition(x);
obj.SetVelocity(v);
}
}
