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; } } }