public class SifakisSVD
{
const float Four_Gamma_Squared = 5.82842712474619f; // sqrt(8.) + 3.;
const float Sine_Pi_Over_Eight = 0.3826834323650897f; // .5 * sqrt(2. - sqrt(2.));
const float Cosine_Pi_Over_Eight = 0.9238795325112867f; //.5 * sqrt(2. + sqrt(2.));
const int sweeps = 4;
public static void SVD(float3x3 F, out float3x3 U, out float3x3 V, out float3x3 sigma)
{
float a11 = F[0, 0]; float a12 = F[0, 1]; float a13 = F[0, 2];
float a21 = F[1, 0]; float a22 = F[1, 1]; float a23 = F[1, 2];
float a31 = F[2, 0]; float a32 = F[2, 1]; float a33 = F[2, 2];
float u11, u12, u13, u21, u22, u23, u31, u32, u33;
float s11 = 0, s12 = 0, s13 = 0, s21 = 0, s22 = 0, s23 = 0, s31 = 0, s32 = 0, s33 = 0;
float v11, v12, v13, v21, v22, v23, v31, v32, v33;
SifakisSVD.svd(a11, a12, a13, a21, a22, a23, a31, a32, a33,
out u11, out u12, out u13, out u21, out u22, out u23, out u31, out u32, out u33,
out v11, out v12, out v13, out v21, out v22, out v23, out v31, out v32, out v33,
out s11, out s22, out s33
);
U = new float3x3(u11, u12, u13, u21, u22, u23, u31, u32, u33);
V = new float3x3(v11, v12, v13, v21, v22, v23, v31, v32, v33);
sigma = new float3x3(s11, s12, s13, s21, s22, s23, s31, s32, s33);
U = U.transpose();
V = V.transpose();
}
[StructLayout(LayoutKind.Explicit)]//, Size = 4
public struct un
{
//设置联合体的每一位
[FieldOffset(0)]
public float f;
[FieldOffset(0)]
public uint ui;
public un(uint u)
{
f = -107374176;
ui = 3435973836;
}
}
static float rsqrt(float f)
{
return 1.0f / XMath.Sqrt(f);
}
public static void svd(
float a11,
float a12,
float a13,
float a21,
float a22,
float a23,
float a31,
float a32,
float a33,
out float u11,
out float u12,
out float u13,
out float u21,
out float u22,
out float u23,
out float u31,
out float u32,
out float u33,
out float v11,
out float v12,
out float v13,
out float v21,
out float v22,
out float v23,
out float v31,
out float v32,
out float v33,
out float sigma1,
out float sigma2,
out float sigma3)
{
// var
un Sfour_gamma_squared = new un(0);
un Ssine_pi_over_eight = new un(0);
un Scosine_pi_over_eight = new un(0);
un Sone_half = new un(0);
un Sone = new un(0);
un Stiny_number = new un(0);
un Ssmall_number = new un(0);
un Sa11 = new un(0);
un Sa21 = new un(0);
un Sa31 = new un(0);
un Sa12 = new un(0);
un Sa22 = new un(0);
un Sa32 = new un(0);
un Sa13 = new un(0);
un Sa23 = new un(0);
un Sa33 = new un(0);
un Sv11 = new un(0);
un Sv21 = new un(0);
un Sv31 = new un(0);
un Sv12 = new un(0);
un Sv22 = new un(0);
un Sv32 = new un(0);
un Sv13 = new un(0);
un Sv23 = new un(0);
un Sv33 = new un(0);
un Su11 = new un(0);
un Su21 = new un(0);
un Su31 = new un(0);
un Su12 = new un(0);
un Su22 = new un(0);
un Su32 = new un(0);
un Su13 = new un(0);
un Su23 = new un(0);
un Su33 = new un(0);
un Sc = new un(0);
un Ss = new un(0);
un Sch = new un(0);
un Ssh = new un(0);
un Stmp1 = new un(0);
un Stmp2 = new un(0);
un Stmp3 = new un(0);
un Stmp4 = new un(0);
un Stmp5 = new un(0);
un Sqvs = new un(0);
un Sqvvx = new un(0);
un Sqvvy = new un(0);
un Sqvvz = new un(0);
un Ss11 = new un(0);
un Ss21 = new un(0);
un Ss31 = new un(0);
un Ss22 = new un(0);
un Ss32 = new un(0);
un Ss33 = new un(0);
// compute
Sfour_gamma_squared.f = Four_Gamma_Squared;
Ssine_pi_over_eight.f = Sine_Pi_Over_Eight;
Scosine_pi_over_eight.f = Cosine_Pi_Over_Eight;
Sone_half.f = 0.5f;
Sone.f = 1.0f;
Stiny_number.f = 1.0e-20f;
Ssmall_number.f = 1.0e-12f;
Sa11.f = a11;
Sa21.f = a21;
Sa31.f = a31;
Sa12.f = a12;
Sa22.f = a22;
Sa32.f = a32;
Sa13.f = a13;
Sa23.f = a23;
Sa33.f = a33;
Sqvs.f = 1.0f;
Sqvvx.f = 0.0f;
Sqvvy.f = 0.0f;
Sqvvz.f = 0.0f;
Ss11.f = Sa11.f * Sa11.f;
Stmp1.f = Sa21.f * Sa21.f;
Ss11.f = Stmp1.f + Ss11.f;
Stmp1.f = Sa31.f * Sa31.f;
Ss11.f = Stmp1.f + Ss11.f;
Ss21.f = Sa12.f * Sa11.f;
Stmp1.f = Sa22.f * Sa21.f;
Ss21.f = Stmp1.f + Ss21.f;
Stmp1.f = Sa32.f * Sa31.f;
Ss21.f = Stmp1.f + Ss21.f;
Ss31.f = Sa13.f * Sa11.f;
Stmp1.f = Sa23.f * Sa21.f;
Ss31.f = Stmp1.f + Ss31.f;
Stmp1.f = Sa33.f * Sa31.f;
Ss31.f = Stmp1.f + Ss31.f;
Ss22.f = Sa12.f * Sa12.f;
Stmp1.f = Sa22.f * Sa22.f;
Ss22.f = Stmp1.f + Ss22.f;
Stmp1.f = Sa32.f * Sa32.f;
Ss22.f = Stmp1.f + Ss22.f;
Ss32.f = Sa13.f * Sa12.f;
Stmp1.f = Sa23.f * Sa22.f;
Ss32.f = Stmp1.f + Ss32.f;
Stmp1.f = Sa33.f * Sa32.f;
Ss32.f = Stmp1.f + Ss32.f;
Ss33.f = Sa13.f * Sa13.f;
Stmp1.f = Sa23.f * Sa23.f;
Ss33.f = Stmp1.f + Ss33.f;
Stmp1.f = Sa33.f * Sa33.f;
Ss33.f = Stmp1.f + Ss33.f;
for (int sweep = 0; sweep < sweeps; sweep++)
{
Ssh.f = Ss21.f * Sone_half.f;
Stmp5.f = Ss11.f - Ss22.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp1.ui = (Stmp2.f >= Stiny_number.f) ? 0xffffffff : 0;
Ssh.ui = Stmp1.ui & Ssh.ui;
Sch.ui = Stmp1.ui & Stmp5.ui;
Stmp2.ui = ~Stmp1.ui & Sone.ui;
Sch.ui = Sch.ui | Stmp2.ui;
Stmp1.f = Ssh.f * Ssh.f;
Stmp2.f = Sch.f * Sch.f;
Stmp3.f = Stmp1.f + Stmp2.f;
Stmp4.f = rsqrt(Stmp3.f);
Ssh.f = Stmp4.f * Ssh.f;
Sch.f = Stmp4.f * Sch.f;
Stmp1.f = Sfour_gamma_squared.f * Stmp1.f;
Stmp1.ui = (Stmp2.f <= Stmp1.f) ? 0xffffffff : 0;
Stmp2.ui = Ssine_pi_over_eight.ui & Stmp1.ui;
Ssh.ui = ~Stmp1.ui & Ssh.ui;
Ssh.ui = Ssh.ui | Stmp2.ui;
Stmp2.ui = Scosine_pi_over_eight.ui & Stmp1.ui;
Sch.ui = ~Stmp1.ui & Sch.ui;
Sch.ui = Sch.ui | Stmp2.ui;
Stmp1.f = Ssh.f * Ssh.f;
Stmp2.f = Sch.f * Sch.f;
Sc.f = Stmp2.f - Stmp1.f;
Ss.f = Sch.f * Ssh.f;
Ss.f = Ss.f + Ss.f;
Stmp3.f = Stmp1.f + Stmp2.f;
Ss33.f = Ss33.f * Stmp3.f;
Ss31.f = Ss31.f * Stmp3.f;
Ss32.f = Ss32.f * Stmp3.f;
Ss33.f = Ss33.f * Stmp3.f;
Stmp1.f = Ss.f * Ss31.f;
Stmp2.f = Ss.f * Ss32.f;
Ss31.f = Sc.f * Ss31.f;
Ss32.f = Sc.f * Ss32.f;
Ss31.f = Stmp2.f + Ss31.f;
Ss32.f = Ss32.f - Stmp1.f;
Stmp2.f = Ss.f * Ss.f;
Stmp1.f = Ss22.f * Stmp2.f;
Stmp3.f = Ss11.f * Stmp2.f;
Stmp4.f = Sc.f * Sc.f;
Ss11.f = Ss11.f * Stmp4.f;
Ss22.f = Ss22.f * Stmp4.f;
Ss11.f = Ss11.f + Stmp1.f;
Ss22.f = Ss22.f + Stmp3.f;
Stmp4.f = Stmp4.f - Stmp2.f;
Stmp2.f = Ss21.f + Ss21.f;
Ss21.f = Ss21.f * Stmp4.f;
Stmp4.f = Sc.f * Ss.f;
Stmp2.f = Stmp2.f * Stmp4.f;
Stmp5.f = Stmp5.f * Stmp4.f;
Ss11.f = Ss11.f + Stmp2.f;
Ss21.f = Ss21.f - Stmp5.f;
Ss22.f = Ss22.f - Stmp2.f;
Stmp1.f = Ssh.f * Sqvvx.f;
Stmp2.f = Ssh.f * Sqvvy.f;
Stmp3.f = Ssh.f * Sqvvz.f;
Ssh.f = Ssh.f * Sqvs.f;
Sqvs.f = Sch.f * Sqvs.f;
Sqvvx.f = Sch.f * Sqvvx.f;
Sqvvy.f = Sch.f * Sqvvy.f;
Sqvvz.f = Sch.f * Sqvvz.f;
Sqvvz.f = Sqvvz.f + Ssh.f;
Sqvs.f = Sqvs.f - Stmp3.f;
Sqvvx.f = Sqvvx.f + Stmp2.f;
Sqvvy.f = Sqvvy.f - Stmp1.f;
Ssh.f = Ss32.f * Sone_half.f;
Stmp5.f = Ss22.f - Ss33.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp1.ui = (Stmp2.f >= Stiny_number.f) ? 0xffffffff : 0;
Ssh.ui = Stmp1.ui & Ssh.ui;
Sch.ui = Stmp1.ui & Stmp5.ui;
Stmp2.ui = ~Stmp1.ui & Sone.ui;
Sch.ui = Sch.ui | Stmp2.ui;
Stmp1.f = Ssh.f * Ssh.f;
Stmp2.f = Sch.f * Sch.f;
Stmp3.f = Stmp1.f + Stmp2.f;
Stmp4.f = rsqrt(Stmp3.f);
Ssh.f = Stmp4.f * Ssh.f;
Sch.f = Stmp4.f * Sch.f;
Stmp1.f = Sfour_gamma_squared.f * Stmp1.f;
Stmp1.ui = (Stmp2.f <= Stmp1.f) ? 0xffffffff : 0;
Stmp2.ui = Ssine_pi_over_eight.ui & Stmp1.ui;
Ssh.ui = ~Stmp1.ui & Ssh.ui;
Ssh.ui = Ssh.ui | Stmp2.ui;
Stmp2.ui = Scosine_pi_over_eight.ui & Stmp1.ui;
Sch.ui = ~Stmp1.ui & Sch.ui;
Sch.ui = Sch.ui | Stmp2.ui;
Stmp1.f = Ssh.f * Ssh.f;
Stmp2.f = Sch.f * Sch.f;
Sc.f = Stmp2.f - Stmp1.f;
Ss.f = Sch.f * Ssh.f;
Ss.f = Ss.f + Ss.f;
Stmp3.f = Stmp1.f + Stmp2.f;
Ss11.f = Ss11.f * Stmp3.f;
Ss21.f = Ss21.f * Stmp3.f;
Ss31.f = Ss31.f * Stmp3.f;
Ss11.f = Ss11.f * Stmp3.f;
Stmp1.f = Ss.f * Ss21.f;
Stmp2.f = Ss.f * Ss31.f;
Ss21.f = Sc.f * Ss21.f;
Ss31.f = Sc.f * Ss31.f;
Ss21.f = Stmp2.f + Ss21.f;
Ss31.f = Ss31.f - Stmp1.f;
Stmp2.f = Ss.f * Ss.f;
Stmp1.f = Ss33.f * Stmp2.f;
Stmp3.f = Ss22.f * Stmp2.f;
Stmp4.f = Sc.f * Sc.f;
Ss22.f = Ss22.f * Stmp4.f;
Ss33.f = Ss33.f * Stmp4.f;
Ss22.f = Ss22.f + Stmp1.f;
Ss33.f = Ss33.f + Stmp3.f;
Stmp4.f = Stmp4.f - Stmp2.f;
Stmp2.f = Ss32.f + Ss32.f;
Ss32.f = Ss32.f * Stmp4.f;
Stmp4.f = Sc.f * Ss.f;
Stmp2.f = Stmp2.f * Stmp4.f;
Stmp5.f = Stmp5.f * Stmp4.f;
Ss22.f = Ss22.f + Stmp2.f;
Ss32.f = Ss32.f - Stmp5.f;
Ss33.f = Ss33.f - Stmp2.f;
Stmp1.f = Ssh.f * Sqvvx.f;
Stmp2.f = Ssh.f * Sqvvy.f;
Stmp3.f = Ssh.f * Sqvvz.f;
Ssh.f = Ssh.f * Sqvs.f;
Sqvs.f = Sch.f * Sqvs.f;
Sqvvx.f = Sch.f * Sqvvx.f;
Sqvvy.f = Sch.f * Sqvvy.f;
Sqvvz.f = Sch.f * Sqvvz.f;
Sqvvx.f = Sqvvx.f + Ssh.f;
Sqvs.f = Sqvs.f - Stmp1.f;
Sqvvy.f = Sqvvy.f + Stmp3.f;
Sqvvz.f = Sqvvz.f - Stmp2.f;
Ssh.f = Ss31.f * Sone_half.f;
Stmp5.f = Ss33.f - Ss11.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp1.ui = (Stmp2.f >= Stiny_number.f) ? 0xffffffff : 0;
Ssh.ui = Stmp1.ui & Ssh.ui;
Sch.ui = Stmp1.ui & Stmp5.ui;
Stmp2.ui = ~Stmp1.ui & Sone.ui;
Sch.ui = Sch.ui | Stmp2.ui;
Stmp1.f = Ssh.f * Ssh.f;
Stmp2.f = Sch.f * Sch.f;
Stmp3.f = Stmp1.f + Stmp2.f;
Stmp4.f = rsqrt(Stmp3.f);
Ssh.f = Stmp4.f * Ssh.f;
Sch.f = Stmp4.f * Sch.f;
Stmp1.f = Sfour_gamma_squared.f * Stmp1.f;
Stmp1.ui = (Stmp2.f <= Stmp1.f) ? 0xffffffff : 0;
Stmp2.ui = Ssine_pi_over_eight.ui & Stmp1.ui;
Ssh.ui = ~Stmp1.ui & Ssh.ui;
Ssh.ui = Ssh.ui | Stmp2.ui;
Stmp2.ui = Scosine_pi_over_eight.ui & Stmp1.ui;
Sch.ui = ~Stmp1.ui & Sch.ui;
Sch.ui = Sch.ui | Stmp2.ui;
Stmp1.f = Ssh.f * Ssh.f;
Stmp2.f = Sch.f * Sch.f;
Sc.f = Stmp2.f - Stmp1.f;
Ss.f = Sch.f * Ssh.f;
Ss.f = Ss.f + Ss.f;
Stmp3.f = Stmp1.f + Stmp2.f;
Ss22.f = Ss22.f * Stmp3.f;
Ss32.f = Ss32.f * Stmp3.f;
Ss21.f = Ss21.f * Stmp3.f;
Ss22.f = Ss22.f * Stmp3.f;
Stmp1.f = Ss.f * Ss32.f;
Stmp2.f = Ss.f * Ss21.f;
Ss32.f = Sc.f * Ss32.f;
Ss21.f = Sc.f * Ss21.f;
Ss32.f = Stmp2.f + Ss32.f;
Ss21.f = Ss21.f - Stmp1.f;
Stmp2.f = Ss.f * Ss.f;
Stmp1.f = Ss11.f * Stmp2.f;
Stmp3.f = Ss33.f * Stmp2.f;
Stmp4.f = Sc.f * Sc.f;
Ss33.f = Ss33.f * Stmp4.f;
Ss11.f = Ss11.f * Stmp4.f;
Ss33.f = Ss33.f + Stmp1.f;
Ss11.f = Ss11.f + Stmp3.f;
Stmp4.f = Stmp4.f - Stmp2.f;
Stmp2.f = Ss31.f + Ss31.f;
Ss31.f = Ss31.f * Stmp4.f;
Stmp4.f = Sc.f * Ss.f;
Stmp2.f = Stmp2.f * Stmp4.f;
Stmp5.f = Stmp5.f * Stmp4.f;
Ss33.f = Ss33.f + Stmp2.f;
Ss31.f = Ss31.f - Stmp5.f;
Ss11.f = Ss11.f - Stmp2.f;
Stmp1.f = Ssh.f * Sqvvx.f;
Stmp2.f = Ssh.f * Sqvvy.f;
Stmp3.f = Ssh.f * Sqvvz.f;
Ssh.f = Ssh.f * Sqvs.f;
Sqvs.f = Sch.f * Sqvs.f;
Sqvvx.f = Sch.f * Sqvvx.f;
Sqvvy.f = Sch.f * Sqvvy.f;
Sqvvz.f = Sch.f * Sqvvz.f;
Sqvvy.f = Sqvvy.f + Ssh.f;
Sqvs.f = Sqvs.f - Stmp2.f;
Sqvvz.f = Sqvvz.f + Stmp1.f;
Sqvvx.f = Sqvvx.f - Stmp3.f;
}
Stmp2.f = Sqvs.f * Sqvs.f;
Stmp1.f = Sqvvx.f * Sqvvx.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = Sqvvy.f * Sqvvy.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = Sqvvz.f * Sqvvz.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = rsqrt(Stmp2.f);
Stmp4.f = Stmp1.f * Sone_half.f;
Stmp3.f = Stmp1.f * Stmp4.f;
Stmp3.f = Stmp1.f * Stmp3.f;
Stmp3.f = Stmp2.f * Stmp3.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp1.f = Stmp1.f - Stmp3.f;
Sqvs.f = Sqvs.f * Stmp1.f;
Sqvvx.f = Sqvvx.f * Stmp1.f;
Sqvvy.f = Sqvvy.f * Stmp1.f;
Sqvvz.f = Sqvvz.f * Stmp1.f;
Stmp1.f = Sqvvx.f * Sqvvx.f;
Stmp2.f = Sqvvy.f * Sqvvy.f;
Stmp3.f = Sqvvz.f * Sqvvz.f;
Sv11.f = Sqvs.f * Sqvs.f;
Sv22.f = Sv11.f - Stmp1.f;
Sv33.f = Sv22.f - Stmp2.f;
Sv33.f = Sv33.f + Stmp3.f;
Sv22.f = Sv22.f + Stmp2.f;
Sv22.f = Sv22.f - Stmp3.f;
Sv11.f = Sv11.f + Stmp1.f;
Sv11.f = Sv11.f - Stmp2.f;
Sv11.f = Sv11.f - Stmp3.f;
Stmp1.f = Sqvvx.f + Sqvvx.f;
Stmp2.f = Sqvvy.f + Sqvvy.f;
Stmp3.f = Sqvvz.f + Sqvvz.f;
Sv32.f = Sqvs.f * Stmp1.f;
Sv13.f = Sqvs.f * Stmp2.f;
Sv21.f = Sqvs.f * Stmp3.f;
Stmp1.f = Sqvvy.f * Stmp1.f;
Stmp2.f = Sqvvz.f * Stmp2.f;
Stmp3.f = Sqvvx.f * Stmp3.f;
Sv12.f = Stmp1.f - Sv21.f;
Sv23.f = Stmp2.f - Sv32.f;
Sv31.f = Stmp3.f - Sv13.f;
Sv21.f = Stmp1.f + Sv21.f;
Sv32.f = Stmp2.f + Sv32.f;
Sv13.f = Stmp3.f + Sv13.f;
Stmp2.f = Sa12.f;
Stmp3.f = Sa13.f;
Sa12.f = Sv12.f * Sa11.f;
Sa13.f = Sv13.f * Sa11.f;
Sa11.f = Sv11.f * Sa11.f;
Stmp1.f = Sv21.f * Stmp2.f;
Sa11.f = Sa11.f + Stmp1.f;
Stmp1.f = Sv31.f * Stmp3.f;
Sa11.f = Sa11.f + Stmp1.f;
Stmp1.f = Sv22.f * Stmp2.f;
Sa12.f = Sa12.f + Stmp1.f;
Stmp1.f = Sv32.f * Stmp3.f;
Sa12.f = Sa12.f + Stmp1.f;
Stmp1.f = Sv23.f * Stmp2.f;
Sa13.f = Sa13.f + Stmp1.f;
Stmp1.f = Sv33.f * Stmp3.f;
Sa13.f = Sa13.f + Stmp1.f;
Stmp2.f = Sa22.f;
Stmp3.f = Sa23.f;
Sa22.f = Sv12.f * Sa21.f;
Sa23.f = Sv13.f * Sa21.f;
Sa21.f = Sv11.f * Sa21.f;
Stmp1.f = Sv21.f * Stmp2.f;
Sa21.f = Sa21.f + Stmp1.f;
Stmp1.f = Sv31.f * Stmp3.f;
Sa21.f = Sa21.f + Stmp1.f;
Stmp1.f = Sv22.f * Stmp2.f;
Sa22.f = Sa22.f + Stmp1.f;
Stmp1.f = Sv32.f * Stmp3.f;
Sa22.f = Sa22.f + Stmp1.f;
Stmp1.f = Sv23.f * Stmp2.f;
Sa23.f = Sa23.f + Stmp1.f;
Stmp1.f = Sv33.f * Stmp3.f;
Sa23.f = Sa23.f + Stmp1.f;
Stmp2.f = Sa32.f;
Stmp3.f = Sa33.f;
Sa32.f = Sv12.f * Sa31.f;
Sa33.f = Sv13.f * Sa31.f;
Sa31.f = Sv11.f * Sa31.f;
Stmp1.f = Sv21.f * Stmp2.f;
Sa31.f = Sa31.f + Stmp1.f;
Stmp1.f = Sv31.f * Stmp3.f;
Sa31.f = Sa31.f + Stmp1.f;
Stmp1.f = Sv22.f * Stmp2.f;
Sa32.f = Sa32.f + Stmp1.f;
Stmp1.f = Sv32.f * Stmp3.f;
Sa32.f = Sa32.f + Stmp1.f;
Stmp1.f = Sv23.f * Stmp2.f;
Sa33.f = Sa33.f + Stmp1.f;
Stmp1.f = Sv33.f * Stmp3.f;
Sa33.f = Sa33.f + Stmp1.f;
Stmp1.f = Sa11.f * Sa11.f;
Stmp4.f = Sa21.f * Sa21.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp4.f = Sa31.f * Sa31.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp2.f = Sa12.f * Sa12.f;
Stmp4.f = Sa22.f * Sa22.f;
Stmp2.f = Stmp2.f + Stmp4.f;
Stmp4.f = Sa32.f * Sa32.f;
Stmp2.f = Stmp2.f + Stmp4.f;
Stmp3.f = Sa13.f * Sa13.f;
Stmp4.f = Sa23.f * Sa23.f;
Stmp3.f = Stmp3.f + Stmp4.f;
Stmp4.f = Sa33.f * Sa33.f;
Stmp3.f = Stmp3.f + Stmp4.f;
Stmp4.ui = (Stmp1.f < Stmp2.f) ? 0xffffffff : 0;
Stmp5.ui = Sa11.ui ^ Sa12.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa11.ui = Sa11.ui ^ Stmp5.ui;
Sa12.ui = Sa12.ui ^ Stmp5.ui;
Stmp5.ui = Sa21.ui ^ Sa22.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa21.ui = Sa21.ui ^ Stmp5.ui;
Sa22.ui = Sa22.ui ^ Stmp5.ui;
Stmp5.ui = Sa31.ui ^ Sa32.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa31.ui = Sa31.ui ^ Stmp5.ui;
Sa32.ui = Sa32.ui ^ Stmp5.ui;
Stmp5.ui = Sv11.ui ^ Sv12.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv11.ui = Sv11.ui ^ Stmp5.ui;
Sv12.ui = Sv12.ui ^ Stmp5.ui;
Stmp5.ui = Sv21.ui ^ Sv22.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv21.ui = Sv21.ui ^ Stmp5.ui;
Sv22.ui = Sv22.ui ^ Stmp5.ui;
Stmp5.ui = Sv31.ui ^ Sv32.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv31.ui = Sv31.ui ^ Stmp5.ui;
Sv32.ui = Sv32.ui ^ Stmp5.ui;
Stmp5.ui = Stmp1.ui ^ Stmp2.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Stmp1.ui = Stmp1.ui ^ Stmp5.ui;
Stmp2.ui = Stmp2.ui ^ Stmp5.ui;
Stmp5.f = -2.0f;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Stmp4.f = 1.0f;
Stmp4.f = Stmp4.f + Stmp5.f;
Sa12.f = Sa12.f * Stmp4.f;
Sa22.f = Sa22.f * Stmp4.f;
Sa32.f = Sa32.f * Stmp4.f;
Sv12.f = Sv12.f * Stmp4.f;
Sv22.f = Sv22.f * Stmp4.f;
Sv32.f = Sv32.f * Stmp4.f;
Stmp4.ui = (Stmp1.f < Stmp3.f) ? 0xffffffff : 0;
Stmp5.ui = Sa11.ui ^ Sa13.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa11.ui = Sa11.ui ^ Stmp5.ui;
Sa13.ui = Sa13.ui ^ Stmp5.ui;
Stmp5.ui = Sa21.ui ^ Sa23.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa21.ui = Sa21.ui ^ Stmp5.ui;
Sa23.ui = Sa23.ui ^ Stmp5.ui;
Stmp5.ui = Sa31.ui ^ Sa33.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa31.ui = Sa31.ui ^ Stmp5.ui;
Sa33.ui = Sa33.ui ^ Stmp5.ui;
Stmp5.ui = Sv11.ui ^ Sv13.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv11.ui = Sv11.ui ^ Stmp5.ui;
Sv13.ui = Sv13.ui ^ Stmp5.ui;
Stmp5.ui = Sv21.ui ^ Sv23.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv21.ui = Sv21.ui ^ Stmp5.ui;
Sv23.ui = Sv23.ui ^ Stmp5.ui;
Stmp5.ui = Sv31.ui ^ Sv33.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv31.ui = Sv31.ui ^ Stmp5.ui;
Sv33.ui = Sv33.ui ^ Stmp5.ui;
Stmp5.ui = Stmp1.ui ^ Stmp3.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Stmp1.ui = Stmp1.ui ^ Stmp5.ui;
Stmp3.ui = Stmp3.ui ^ Stmp5.ui;
Stmp5.f = -2.0f;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Stmp4.f = 1.0f;
Stmp4.f = Stmp4.f + Stmp5.f;
Sa11.f = Sa11.f * Stmp4.f;
Sa21.f = Sa21.f * Stmp4.f;
Sa31.f = Sa31.f * Stmp4.f;
Sv11.f = Sv11.f * Stmp4.f;
Sv21.f = Sv21.f * Stmp4.f;
Sv31.f = Sv31.f * Stmp4.f;
Stmp4.ui = (Stmp2.f < Stmp3.f) ? 0xffffffff : 0;
Stmp5.ui = Sa12.ui ^ Sa13.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa12.ui = Sa12.ui ^ Stmp5.ui;
Sa13.ui = Sa13.ui ^ Stmp5.ui;
Stmp5.ui = Sa22.ui ^ Sa23.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa22.ui = Sa22.ui ^ Stmp5.ui;
Sa23.ui = Sa23.ui ^ Stmp5.ui;
Stmp5.ui = Sa32.ui ^ Sa33.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sa32.ui = Sa32.ui ^ Stmp5.ui;
Sa33.ui = Sa33.ui ^ Stmp5.ui;
Stmp5.ui = Sv12.ui ^ Sv13.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv12.ui = Sv12.ui ^ Stmp5.ui;
Sv13.ui = Sv13.ui ^ Stmp5.ui;
Stmp5.ui = Sv22.ui ^ Sv23.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv22.ui = Sv22.ui ^ Stmp5.ui;
Sv23.ui = Sv23.ui ^ Stmp5.ui;
Stmp5.ui = Sv32.ui ^ Sv33.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Sv32.ui = Sv32.ui ^ Stmp5.ui;
Sv33.ui = Sv33.ui ^ Stmp5.ui;
Stmp5.ui = Stmp2.ui ^ Stmp3.ui;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Stmp2.ui = Stmp2.ui ^ Stmp5.ui;
Stmp3.ui = Stmp3.ui ^ Stmp5.ui;
Stmp5.f = -2.0f;
Stmp5.ui = Stmp5.ui & Stmp4.ui;
Stmp4.f = 1.0f;
Stmp4.f = Stmp4.f + Stmp5.f;
Sa13.f = Sa13.f * Stmp4.f;
Sa23.f = Sa23.f * Stmp4.f;
Sa33.f = Sa33.f * Stmp4.f;
Sv13.f = Sv13.f * Stmp4.f;
Sv23.f = Sv23.f * Stmp4.f;
Sv33.f = Sv33.f * Stmp4.f;
Su11.f = 1.0f;
Su21.f = 0.0f;
Su31.f = 0.0f;
Su12.f = 0.0f;
Su22.f = 1.0f;
Su32.f = 0.0f;
Su13.f = 0.0f;
Su23.f = 0.0f;
Su33.f = 1.0f;
Ssh.f = Sa21.f * Sa21.f;
Ssh.ui = (Ssh.f >= Ssmall_number.f) ? 0xffffffff : 0;
Ssh.ui = Ssh.ui & Sa21.ui;
Stmp5.f = 0.0f;
Sch.f = Stmp5.f - Sa11.f;
Sch.f = XMath.Max(Sch.f, Sa11.f);
Sch.f = XMath.Max(Sch.f, Ssmall_number.f);
Stmp5.ui = (Sa11.f >= Stmp5.f) ? 0xffffffff : 0;
Stmp1.f = Sch.f * Sch.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = rsqrt(Stmp2.f);
Stmp4.f = Stmp1.f * Sone_half.f;
Stmp3.f = Stmp1.f * Stmp4.f;
Stmp3.f = Stmp1.f * Stmp3.f;
Stmp3.f = Stmp2.f * Stmp3.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp1.f = Stmp1.f - Stmp3.f;
Stmp1.f = Stmp1.f * Stmp2.f;
Sch.f = Sch.f + Stmp1.f;
Stmp1.ui = ~Stmp5.ui & Ssh.ui;
Stmp2.ui = ~Stmp5.ui & Sch.ui;
Sch.ui = Stmp5.ui & Sch.ui;
Ssh.ui = Stmp5.ui & Ssh.ui;
Sch.ui = Sch.ui | Stmp1.ui;
Ssh.ui = Ssh.ui | Stmp2.ui;
Stmp1.f = Sch.f * Sch.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = rsqrt(Stmp2.f);
Stmp4.f = Stmp1.f * Sone_half.f;
Stmp3.f = Stmp1.f * Stmp4.f;
Stmp3.f = Stmp1.f * Stmp3.f;
Stmp3.f = Stmp2.f * Stmp3.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp1.f = Stmp1.f - Stmp3.f;
Sch.f = Sch.f * Stmp1.f;
Ssh.f = Ssh.f * Stmp1.f;
Sc.f = Sch.f * Sch.f;
Ss.f = Ssh.f * Ssh.f;
Sc.f = Sc.f - Ss.f;
Ss.f = Ssh.f * Sch.f;
Ss.f = Ss.f + Ss.f;
Stmp1.f = Ss.f * Sa11.f;
Stmp2.f = Ss.f * Sa21.f;
Sa11.f = Sc.f * Sa11.f;
Sa21.f = Sc.f * Sa21.f;
Sa11.f = Sa11.f + Stmp2.f;
Sa21.f = Sa21.f - Stmp1.f;
Stmp1.f = Ss.f * Sa12.f;
Stmp2.f = Ss.f * Sa22.f;
Sa12.f = Sc.f * Sa12.f;
Sa22.f = Sc.f * Sa22.f;
Sa12.f = Sa12.f + Stmp2.f;
Sa22.f = Sa22.f - Stmp1.f;
Stmp1.f = Ss.f * Sa13.f;
Stmp2.f = Ss.f * Sa23.f;
Sa13.f = Sc.f * Sa13.f;
Sa23.f = Sc.f * Sa23.f;
Sa13.f = Sa13.f + Stmp2.f;
Sa23.f = Sa23.f - Stmp1.f;
Stmp1.f = Ss.f * Su11.f;
Stmp2.f = Ss.f * Su12.f;
Su11.f = Sc.f * Su11.f;
Su12.f = Sc.f * Su12.f;
Su11.f = Su11.f + Stmp2.f;
Su12.f = Su12.f - Stmp1.f;
Stmp1.f = Ss.f * Su21.f;
Stmp2.f = Ss.f * Su22.f;
Su21.f = Sc.f * Su21.f;
Su22.f = Sc.f * Su22.f;
Su21.f = Su21.f + Stmp2.f;
Su22.f = Su22.f - Stmp1.f;
Stmp1.f = Ss.f * Su31.f;
Stmp2.f = Ss.f * Su32.f;
Su31.f = Sc.f * Su31.f;
Su32.f = Sc.f * Su32.f;
Su31.f = Su31.f + Stmp2.f;
Su32.f = Su32.f - Stmp1.f;
Ssh.f = Sa31.f * Sa31.f;
Ssh.ui = (Ssh.f >= Ssmall_number.f) ? 0xffffffff : 0;
Ssh.ui = Ssh.ui & Sa31.ui;
Stmp5.f = 0.0f;
Sch.f = Stmp5.f - Sa11.f;
Sch.f = XMath.Max(Sch.f, Sa11.f);
Sch.f = XMath.Max(Sch.f, Ssmall_number.f);
Stmp5.ui = (Sa11.f >= Stmp5.f) ? 0xffffffff : 0;
Stmp1.f = Sch.f * Sch.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = rsqrt(Stmp2.f);
Stmp4.f = Stmp1.f * Sone_half.f;
Stmp3.f = Stmp1.f * Stmp4.f;
Stmp3.f = Stmp1.f * Stmp3.f;
Stmp3.f = Stmp2.f * Stmp3.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp1.f = Stmp1.f - Stmp3.f;
Stmp1.f = Stmp1.f * Stmp2.f;
Sch.f = Sch.f + Stmp1.f;
Stmp1.ui = ~Stmp5.ui & Ssh.ui;
Stmp2.ui = ~Stmp5.ui & Sch.ui;
Sch.ui = Stmp5.ui & Sch.ui;
Ssh.ui = Stmp5.ui & Ssh.ui;
Sch.ui = Sch.ui | Stmp1.ui;
Ssh.ui = Ssh.ui | Stmp2.ui;
Stmp1.f = Sch.f * Sch.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = rsqrt(Stmp2.f);
Stmp4.f = Stmp1.f * Sone_half.f;
Stmp3.f = Stmp1.f * Stmp4.f;
Stmp3.f = Stmp1.f * Stmp3.f;
Stmp3.f = Stmp2.f * Stmp3.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp1.f = Stmp1.f - Stmp3.f;
Sch.f = Sch.f * Stmp1.f;
Ssh.f = Ssh.f * Stmp1.f;
Sc.f = Sch.f * Sch.f;
Ss.f = Ssh.f * Ssh.f;
Sc.f = Sc.f - Ss.f;
Ss.f = Ssh.f * Sch.f;
Ss.f = Ss.f + Ss.f;
Stmp1.f = Ss.f * Sa11.f;
Stmp2.f = Ss.f * Sa31.f;
Sa11.f = Sc.f * Sa11.f;
Sa31.f = Sc.f * Sa31.f;
Sa11.f = Sa11.f + Stmp2.f;
Sa31.f = Sa31.f - Stmp1.f;
Stmp1.f = Ss.f * Sa12.f;
Stmp2.f = Ss.f * Sa32.f;
Sa12.f = Sc.f * Sa12.f;
Sa32.f = Sc.f * Sa32.f;
Sa12.f = Sa12.f + Stmp2.f;
Sa32.f = Sa32.f - Stmp1.f;
Stmp1.f = Ss.f * Sa13.f;
Stmp2.f = Ss.f * Sa33.f;
Sa13.f = Sc.f * Sa13.f;
Sa33.f = Sc.f * Sa33.f;
Sa13.f = Sa13.f + Stmp2.f;
Sa33.f = Sa33.f - Stmp1.f;
Stmp1.f = Ss.f * Su11.f;
Stmp2.f = Ss.f * Su13.f;
Su11.f = Sc.f * Su11.f;
Su13.f = Sc.f * Su13.f;
Su11.f = Su11.f + Stmp2.f;
Su13.f = Su13.f - Stmp1.f;
Stmp1.f = Ss.f * Su21.f;
Stmp2.f = Ss.f * Su23.f;
Su21.f = Sc.f * Su21.f;
Su23.f = Sc.f * Su23.f;
Su21.f = Su21.f + Stmp2.f;
Su23.f = Su23.f - Stmp1.f;
Stmp1.f = Ss.f * Su31.f;
Stmp2.f = Ss.f * Su33.f;
Su31.f = Sc.f * Su31.f;
Su33.f = Sc.f * Su33.f;
Su31.f = Su31.f + Stmp2.f;
Su33.f = Su33.f - Stmp1.f;
Ssh.f = Sa32.f * Sa32.f;
Ssh.ui = (Ssh.f >= Ssmall_number.f) ? 0xffffffff : 0;
Ssh.ui = Ssh.ui & Sa32.ui;
Stmp5.f = 0.0f;
Sch.f = Stmp5.f - Sa22.f;
Sch.f = XMath.Max(Sch.f, Sa22.f);
Sch.f = XMath.Max(Sch.f, Ssmall_number.f);
Stmp5.ui = (Sa22.f >= Stmp5.f) ? 0xffffffff : 0;
Stmp1.f = Sch.f * Sch.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = rsqrt(Stmp2.f);
Stmp4.f = Stmp1.f * Sone_half.f;
Stmp3.f = Stmp1.f * Stmp4.f;
Stmp3.f = Stmp1.f * Stmp3.f;
Stmp3.f = Stmp2.f * Stmp3.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp1.f = Stmp1.f - Stmp3.f;
Stmp1.f = Stmp1.f * Stmp2.f;
Sch.f = Sch.f + Stmp1.f;
Stmp1.ui = ~Stmp5.ui & Ssh.ui;
Stmp2.ui = ~Stmp5.ui & Sch.ui;
Sch.ui = Stmp5.ui & Sch.ui;
Ssh.ui = Stmp5.ui & Ssh.ui;
Sch.ui = Sch.ui | Stmp1.ui;
Ssh.ui = Ssh.ui | Stmp2.ui;
Stmp1.f = Sch.f * Sch.f;
Stmp2.f = Ssh.f * Ssh.f;
Stmp2.f = Stmp1.f + Stmp2.f;
Stmp1.f = rsqrt(Stmp2.f);
Stmp4.f = Stmp1.f * Sone_half.f;
Stmp3.f = Stmp1.f * Stmp4.f;
Stmp3.f = Stmp1.f * Stmp3.f;
Stmp3.f = Stmp2.f * Stmp3.f;
Stmp1.f = Stmp1.f + Stmp4.f;
Stmp1.f = Stmp1.f - Stmp3.f;
Sch.f = Sch.f * Stmp1.f;
Ssh.f = Ssh.f * Stmp1.f;
Sc.f = Sch.f * Sch.f;
Ss.f = Ssh.f * Ssh.f;
Sc.f = Sc.f - Ss.f;
Ss.f = Ssh.f * Sch.f;
Ss.f = Ss.f + Ss.f;
Stmp1.f = Ss.f * Sa21.f;
Stmp2.f = Ss.f * Sa31.f;
Sa21.f = Sc.f * Sa21.f;
Sa31.f = Sc.f * Sa31.f;
Sa21.f = Sa21.f + Stmp2.f;
Sa31.f = Sa31.f - Stmp1.f;
Stmp1.f = Ss.f * Sa22.f;
Stmp2.f = Ss.f * Sa32.f;
Sa22.f = Sc.f * Sa22.f;
Sa32.f = Sc.f * Sa32.f;
Sa22.f = Sa22.f + Stmp2.f;
Sa32.f = Sa32.f - Stmp1.f;
Stmp1.f = Ss.f * Sa23.f;
Stmp2.f = Ss.f * Sa33.f;
Sa23.f = Sc.f * Sa23.f;
Sa33.f = Sc.f * Sa33.f;
Sa23.f = Sa23.f + Stmp2.f;
Sa33.f = Sa33.f - Stmp1.f;
Stmp1.f = Ss.f * Su12.f;
Stmp2.f = Ss.f * Su13.f;
Su12.f = Sc.f * Su12.f;
Su13.f = Sc.f * Su13.f;
Su12.f = Su12.f + Stmp2.f;
Su13.f = Su13.f - Stmp1.f;
Stmp1.f = Ss.f * Su22.f;
Stmp2.f = Ss.f * Su23.f;
Su22.f = Sc.f * Su22.f;
Su23.f = Sc.f * Su23.f;
Su22.f = Su22.f + Stmp2.f;
Su23.f = Su23.f - Stmp1.f;
Stmp1.f = Ss.f * Su32.f;
Stmp2.f = Ss.f * Su33.f;
Su32.f = Sc.f * Su32.f;
Su33.f = Sc.f * Su33.f;
Su32.f = Su32.f + Stmp2.f;
Su33.f = Su33.f - Stmp1.f;
// end
u11 = Su11.f;
u21 = Su21.f;
u31 = Su31.f;
u12 = Su12.f;
u22 = Su22.f;
u32 = Su32.f;
u13 = Su13.f;
u23 = Su23.f;
u33 = Su33.f;
v11 = Sv11.f;
v21 = Sv21.f;
v31 = Sv31.f;
v12 = Sv12.f;
v22 = Sv22.f;
v32 = Sv32.f;
v13 = Sv13.f;
v23 = Sv23.f;
v33 = Sv33.f;
sigma1 = Sa11.f;
sigma2 = Sa22.f;
sigma3 = Sa33.f;
// output
}
}
记录一下3x3矩阵的SVD
最新推荐文章于 2024-05-21 00:06:35 发布