void SolveClamped(ref idVecX x, float[] b)
{
// solve L
SIMDProcessor.MatX_LowerTriangularSolve(clamped, solveCache1.ToArray(), b, numClamped, clampedChangeStart);
// solve D
SIMDProcessor.Mul(solveCache2.ToArray(), solveCache1.ToArray(), diagonal.ToArray(), numClamped);
// solve Lt
SIMDProcessor.MatX_LowerTriangularSolveTranspose(clamped, x.ToArray(), solveCache2.ToArray(), numClamped);
clampedChangeStart = numClamped;
}
void AddClamped(int r, bool useSolveCache)
{
Debug.Assert(r >= numClamped);
if (numClamped < clampedChangeStart)
{
clampedChangeStart = numClamped;
}
// add a row at the bottom and a column at the right of the factored matrix for the clamped variables
Swap(numClamped, r);
// solve for v in L * v = rowPtr[numClamped]
float dot = 0;
if (useSolveCache)
{
// the lower triangular solve was cached in SolveClamped called by CalcForceDelta
Array.Copy(clamped[numClamped], solveCache2.ToArray(), numClamped);
// calculate row dot product
SIMDProcessor.Dot(out dot, solveCache2.ToArray(), solveCache1.ToArray(), numClamped);
}
else
{
float[] v = new float[numClamped];
SIMDProcessor.MatX_LowerTriangularSolve(clamped, v, rowPtrs[numClamped], numClamped);
// add bottom row to L
SIMDProcessor.Mul(clamped[numClamped], v, diagonal.ToArray(), numClamped);
// calculate row dot product
SIMDProcessor.Dot(out dot, clamped[numClamped], v, numClamped);
}
// update diagonal[numClamped]
float d = rowPtrs[numClamped][numClamped] - dot;
if (d == 0.0f)
{
idLib.common.Printf("AddClamped: updating factorization failed\n"); numClamped++; return;
}
clamped[numClamped][numClamped] = d;
diagonal[numClamped] = 1.0f / d;
numClamped++;
}
void RemoveClamped(int r)
{
double diag;
Debug.Assert(r < numClamped);
if (r < clampedChangeStart)
{
clampedChangeStart = r;
}
numClamped--;
// no need to swap and update the factored matrix when the last row and column are removed
if (r == numClamped)
{
return;
}
// swap the to be removed row/column with the last row/column
Swap(r, numClamped);
// update the factored matrix
float[] addSub = new float[numClamped];
if (r == 0)
{
if (numClamped == 1)
{
diag = rowPtrs[0][0];
if (diag == 0.0f)
{
idLib.common.Printf("RemoveClamped: updating factorization failed\n"); return;
}
clamped[0][0] = diag;
diagonal[0] = (float)(1.0f / diag);
return;
}
// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
float[] original = rowPtrs[numClamped];
float[] ptr = rowPtrs[r];
addSub[0] = ptr[0] - original[numClamped];
for (int i = 1; i < numClamped; i++)
{
addSub[i] = ptr[i] - original[i];
}
}
else
{
float[] v = new float[numClamped];
// solve for v in L * v = rowPtr[r]
SIMDProcessor.MatX_LowerTriangularSolve(clamped, v, rowPtrs[r], r);
// update removed row
SIMDProcessor.Mul(clamped[r], v, diagonal.ToArray(), r);
// if the last row/column of the matrix is updated
if (r == numClamped - 1)
{
// only calculate new diagonal
float dot;
SIMDProcessor.Dot(out dot, clamped[r], v, r);
diag = rowPtrs[r][r] - dot;
if (diag == 0.0f)
{
idLib.common.Printf("RemoveClamped: updating factorization failed\n"); return;
}
clamped[r][r] = diag;
diagonal[r] = (float)(1.0f / diag);
return;
}
// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
for (int i = 0; i < r; i++)
{
v[i] = clamped[r][i] * clamped[i][i];
}
for (int i = r; i < numClamped; i++)
{
double sum = (i == r ? clamped[r][r] : clamped[r][r] * clamped[i][r]);
float[] ptr = clamped[i];
for (int j = 0; j < r; j++)
{
sum += ptr[j] * v[j];
}
addSub[i] = (float)(rowPtrs[r][i] - sum);
}
}
// add row/column to the lower right sub matrix starting at (r, r)
float[] v1 = new float[numClamped];
float[] v2 = new float[numClamped];
diag = idMath.SQRT_1OVER2;
v1[r] = (float)((0.5f * addSub[r] + 1.0f) * diag);
v2[r] = (float)((0.5f * addSub[r] - 1.0f) * diag);
for (int i = r + 1; i < numClamped; i++)
{
v1[i] = v2[i] = (float)(addSub[i] * diag);
}
double alpha1 = 1.0f;
double alpha2 = -1.0f;
// simultaneous update/downdate of the sub matrix starting at (r, r)
int n = clamped.GetNumColumns();
for (int i = r; i < numClamped; i++)
{
diag = clamped[i][i];
double p1 = v1[i];
double newDiag = diag + alpha1 * p1 * p1;
if (newDiag == 0.0f)
{
idLib.Common.Printf("RemoveClamped: updating factorization failed\n"); return;
}
alpha1 /= newDiag;
double beta1 = p1 * alpha1;
alpha1 *= diag;
diag = newDiag;
double p2 = v2[i];
newDiag = diag + alpha2 * p2 * p2;
if (newDiag == 0.0f)
{
idLib.common.Printf("RemoveClamped: updating factorization failed\n"); return;
}
clamped[i][i] = newDiag;
double invNewDiag;
diagonal[i] = (float)(invNewDiag = 1.0f / newDiag);
alpha2 *= invNewDiag;
double beta2 = p2 * alpha2;
alpha2 *= diag;
// update column below diagonal (i,i)
float[] ptr = clamped.ToArray() + i;
for (int j = i + 1; j < numClamped - 1; j += 2)
{
float sum0 = ptr[(j + 0) * n];
float sum1 = ptr[(j + 1) * n];
v1[j + 0] -= (float)(p1 * sum0);
v1[j + 1] -= (float)(p1 * sum1);
sum0 += (float)(beta1 * v1[j + 0]);
sum1 += (float)(beta1 * v1[j + 1]);
v2[j + 0] -= (float)(p2 * sum0);
v2[j + 1] -= (float)(p2 * sum1);
sum0 += (float)(beta2 * v2[j + 0]);
sum1 += (float)(beta2 * v2[j + 1]);
ptr[(j + 0) * n] = sum0;
ptr[(j + 1) * n] = sum1;
}
for (int j; j < numClamped; j++)
{
double sum = ptr[j * n];
v1[j] -= (float)(p1 * sum);
sum += beta1 * v1[j];
v2[j] -= (float)(p2 * sum);
sum += beta2 * v2[j];
ptr[j * n] = (float)sum;
}
}
}