public static Vector[] ToOrthonormal(Vector[] coords, double[] masses, int[] block, Vector[] PBlk)
{
if (HDebug.IsDebuggerAttached)
#region check if elements in non-block are zeros.
{
int leng = coords.Length;
foreach (int i in HEnum.HEnumCount(leng).HEnumExcept(block.HToHashSet()))
{
for (int r = 0; r < PBlk.Length; r++)
{
int c0 = i * 3;
HDebug.Assert(PBlk[r][c0 + 0] == 0);
HDebug.Assert(PBlk[r][c0 + 1] == 0);
HDebug.Assert(PBlk[r][c0 + 2] == 0);
}
}
}
#endregion
Matrix Pmat = new double[block.Length * 3, PBlk.Length];
for (int r = 0; r < PBlk.Length; r++)
{
for (int i = 0; i < block.Length; i++)
{
int i0 = i * 3;
int c0 = block[i] * 3;
Pmat[i0 + 0, r] = PBlk[r][c0 + 0];
Pmat[i0 + 1, r] = PBlk[r][c0 + 1];
Pmat[i0 + 2, r] = PBlk[r][c0 + 2];
}
}
using (new Matlab.NamedLock(""))
{
Matlab.PutValue("n", PBlk.Length);
Matlab.PutMatrix("P", Pmat);
Matlab.Execute("[U,S,V] = svd(P);");
Matlab.Execute("U = U(:,1:n);");
if (HDebug.IsDebuggerAttached)
{
Matlab.Execute("SV = S(1:n,1:n)*V';");
double err = Matlab.GetValue("max(max(abs(P - U*SV)))");
HDebug.Assert(Math.Abs(err) < 0.00000001);
}
Pmat = Matlab.GetMatrix("U");
}
Vector[] PBlkOrth = new Vector[PBlk.Length];
for (int r = 0; r < PBlk.Length; r++)
{
Vector PBlkOrth_r = new double[PBlk[r].Size];
for (int i = 0; i < block.Length; i++)
{
int i0 = i * 3;
int c0 = block[i] * 3;
PBlkOrth_r[c0 + 0] = Pmat[i0 + 0, r];
PBlkOrth_r[c0 + 1] = Pmat[i0 + 1, r];
PBlkOrth_r[c0 + 2] = Pmat[i0 + 2, r];
}
PBlkOrth[r] = PBlkOrth_r;
}
if (HDebug.IsDebuggerAttached)
#region checi the orthonormal condition, and rot/trans condition (using ANM)
{
{ // check if all trans/rot modes are orthonormal
for (int i = 0; i < PBlkOrth.Length; i++)
{
HDebug.Exception(Math.Abs(PBlkOrth[i].Dist - 1) < 0.00000001);
for (int j = i + 1; j < PBlkOrth.Length; j++)
{
double dot = LinAlg.VtV(PBlkOrth[i], PBlkOrth[j]);
HDebug.Exception(Math.Abs(dot) < 0.00000001);
}
}
}
{ // check if this is true rot/trans modes using ANM
Vector[] anmcoords = coords.HClone();
int leng = coords.Length;
foreach (int i in HEnum.HEnumCount(leng).HEnumExcept(block.HToHashSet()))
{
anmcoords[i] = null;
}
HessMatrix H = GetHessAnm(anmcoords, 100);
Matrix PHP;
using (new Matlab.NamedLock(""))
{
Matlab.PutSparseMatrix("H", H.GetMatrixSparse(), 3, 3);
Matlab.PutMatrix("P", PBlkOrth.ToMatrix(true));
PHP = Matlab.GetMatrix("P'*H*P");
}
double maxerr = PHP.HAbsMax();
HDebug.Exception(Math.Abs(maxerr) < 0.00000001);
}
}
#endregion
return(PBlkOrth);
}
