void UpdateForSolve()
{
if (need_solve_update == false)
{
return;
}
// construct constraints matrix and RHS
WeightsM.Clear();
Array.Clear(Cx, 0, N);
Array.Clear(Cy, 0, N);
Array.Clear(Cz, 0, N);
foreach (var constraint in SoftConstraints)
{
int vid = constraint.Key;
int i = ToIndex[vid];
double w = constraint.Value.Weight;
if (UseSoftConstraintNormalEquations)
{
w = w * w;
}
WeightsM.Set(i, i, w);
Vector3D pos = constraint.Value.Position;
Cx[i] = w * pos.x;
Cy[i] = w * pos.y;
Cz[i] = w * pos.z;
}
// add RHS vectors
for (int i = 0; i < N; ++i)
{
Bx[i] = MLx[i] + Cx[i];
By[i] = MLy[i] + Cy[i];
Bz[i] = MLz[i] + Cz[i];
}
// update basic preconditioner
// [RMS] currently not using this...it actually seems to make things
// worse!!
for (int i = 0; i < N; i++)
{
//double diag_value = M[i, i] + WeightsM[i, i];
double diag_value = M[i, i];
//Preconditioner.Set(i, i, 1.0 / diag_value);
Preconditioner.Set(i, i, diag_value);
}
need_solve_update = false;
}
