protected override void MatrixTimesVectorImpl(double[] a_Source, double[] a_Destination)
{
for (int i = 0; i < m_TempVector.Length; ++i)
{
m_TempVector[i] = 0;
}
m_J.MatrixTransposeTimesVector(a_Source, m_TempVector); //temp = JTa
for (int i = 0; i < m_W.Length; ++i)
{
m_TempVector[i] = m_TempVector[i] * m_W[i]; //temp = WJTa
}
m_J.MatrixTimesVector(m_TempVector, a_Destination); // a_Destination = JWJTa
}
private void SolveEquation11(float a_SpringConstant, float a_DampingConstant)
{
ParticlesGetVelocities(qdot);
ValidateVector(qdot);
//double[] M = ParticlesMassMatrix();
//ValidateVector(M);
ParticlesInverseMassMatrix(W);
ValidateVector(W);
ParticlesGetForces(Q);
ValidateVector(Q);
ConstraintsGetValues(C);
ValidateVector(C);
//double[] CMass = ConstraintsGetAvgMasses();
//ValidateVector(CMass);
ConstraintsGetDerivativeValues(CDot);
ValidateVector(CDot);
// JDot times qdot.
for (int i = 0; i < JDotqdot.Length; ++i)
{
JDotqdot[i] = 0;
}
m_JDot.MatrixTimesVector(qdot, JDotqdot);
ValidateVector(JDotqdot);
// W times Q.
for (int i = 0; i < WQ.Length; ++i)
{
WQ[i] = W[i] * Q[i];
}
ValidateVector(WQ);
// J times WQ.
for (int i = 0; i < JWQ.Length; ++i)
{
JWQ[i] = 0;
}
m_J.MatrixTimesVector(WQ, JWQ);
ValidateVector(JWQ);
// Compute the RHS of equation 11.
for (int i = 0; i < RHS.Length; ++i)
{
RHS[i] = -JDotqdot[i] - JWQ[i] - a_SpringConstant * C[i] - a_DampingConstant * CDot[i];
if (double.IsNaN(RHS[i]) || double.IsNaN(RHS[i]))
{
throw new System.Exception("NaN or Inf in RHS of eq 11");
}
}
// Set up implicit matrix of LHS and solve.
int stepsPerformed = 0;
m_Solver.Solve(LHS, lambda, RHS, out stepsPerformed);
ValidateVector(lambda);
//Debug.Log("Nr of iterations in conjgrad solver: " + stepsPerformed);
for (int i = 0; i < QHat.Length; ++i)
{
QHat[i] = 0;
}
m_J.MatrixTransposeTimesVector(lambda, QHat);
ValidateVector(QHat);
if (QHat.Length != m_Particles.Count << 1)
{
throw new Exception("QHat does not match particles!");
}
for (int i = 0; i < m_Particles.Count; ++i)
{
m_Particles[i].ForceAccumulator += new Vector2((float)QHat[i * 2], (float)QHat[(i * 2) + 1]);
Vector2 newForce = m_Particles[i].ForceAccumulator;
if (double.IsNaN(newForce.x) || double.IsNaN(newForce.y) || double.IsInfinity(newForce.x) || double.IsInfinity(newForce.y))
{
throw new Exception("NaN or Inf in accumulated force after eq 11");
}
}
}
