public static float normF(FMatrix6x6 M)
{
float scale = CommonOps_FDF6.elementMaxAbs(M);
if (scale == 0.0f)
{
return(0.0f);
}
float a11 = M.a11 / scale,
a12 = M.a12 / scale,
a13 = M.a13 / scale,
a14 = M.a14 / scale,
a15 = M.a15 / scale,
a16 = M.a16 / scale;
float a21 = M.a21 / scale,
a22 = M.a22 / scale,
a23 = M.a23 / scale,
a24 = M.a24 / scale,
a25 = M.a25 / scale,
a26 = M.a26 / scale;
float a31 = M.a31 / scale,
a32 = M.a32 / scale,
a33 = M.a33 / scale,
a34 = M.a34 / scale,
a35 = M.a35 / scale,
a36 = M.a36 / scale;
float a41 = M.a41 / scale,
a42 = M.a42 / scale,
a43 = M.a43 / scale,
a44 = M.a44 / scale,
a45 = M.a45 / scale,
a46 = M.a46 / scale;
float a51 = M.a51 / scale,
a52 = M.a52 / scale,
a53 = M.a53 / scale,
a54 = M.a54 / scale,
a55 = M.a55 / scale,
a56 = M.a56 / scale;
float a61 = M.a61 / scale,
a62 = M.a62 / scale,
a63 = M.a63 / scale,
a64 = M.a64 / scale,
a65 = M.a65 / scale,
a66 = M.a66 / scale;
float sum = 0;
sum += a11 * a11 + a12 * a12 + a13 * a13 + a14 * a14 + a15 * a15 + a16 * a16;
sum += a21 * a21 + a22 * a22 + a23 * a23 + a24 * a24 + a25 * a25 + a26 * a26;
sum += a31 * a31 + a32 * a32 + a33 * a33 + a34 * a34 + a35 * a35 + a36 * a36;
sum += a41 * a41 + a42 * a42 + a43 * a43 + a44 * a44 + a45 * a45 + a46 * a46;
sum += a51 * a51 + a52 * a52 + a53 * a53 + a54 * a54 + a55 * a55 + a56 * a56;
sum += a61 * a61 + a62 * a62 + a63 * a63 + a64 * a64 + a65 * a65 + a66 * a66;
return(scale * (float)Math.Sqrt(sum));
}
public static float normF(FMatrix6 M)
{
float scale = CommonOps_FDF6.elementMaxAbs(M);
if (scale == 0.0f)
{
return(0.0f);
}
float a1 = M.a1 / scale,
a2 = M.a2 / scale,
a3 = M.a3 / scale,
a4 = M.a4 / scale,
a5 = M.a5 / scale,
a6 = M.a6 / scale;
float sum = a1 * a1 + a2 * a2 + a3 * a3 + a4 * a4 + a5 * a5 + a6 * a6;
return(scale * (float)Math.Sqrt(sum));
}
public static void normalizeF(FMatrix6 M)
{
float val = normF(M);
CommonOps_FDF6.divide(M, val);
}
