累积矩阵 Accumulating Matrices

Matrix ToMatrix(Transform transform) {
    // First, extract the rotation basis of the transform
    // 也可以直接使用 四元数公式展开法 直接得到旋转矩阵
    Vector x = Vector(1, 0, 0) * transform.rotation; // Vec3 * Quat (right vector)
    Vector y = Vector(0, 1, 0) * transform.rotation; // Vec3 * Quat (up vector)
    Vector z = Vector(0, 0, 1) * transform.rotation; // Vec3 * Quat (forward vector)
    
    // Next, scale the basis vectors
    // 如果上一步使用了 公式展开 此处应该构造缩放矩阵
    x = x * transform.scale.x; // Vector * float
    y = y * transform.scale.y; // Vector * float
    z = z * transform.scale.z; // Vector * float
 
    // Extract the position of the transform
    Vector t = transform.position;
 
    // Create matrix
    return Matrix(
        x.x, x.y, x.z, 0, // X basis (& Scale)
        y.x, y.y, y.z, 0, // Y basis (& scale)
        z.x, z.y, z.z, 0, // Z basis (& scale)
        t.x, t.y, t.z, 1  // Position
    );
}
 
Matrix GetWorldMatrix(Transform t) {
    Matrix local = ToMatrix(t);       // 由 pos/rot/scale 组装
    if(t.parent)                      // 递归到根
        return local * GetWorldMatrix(t.parent);
    return local;
}

typedef struct {
    HVect t;  /* Translation components */
    Quat  q;  /* Essential rotation   */
    Quat  u;  /* Stretch rotation   */
    HVect k;  /* Stretch factors    */
    float f;  /* Sign of determinant    */
} AffineParts;
 
void decomp_affine(HMatrix A, AffineParts *parts);

Matrix GetWorldMatrix(Transform transform) {
    Matrix localMatrix = ToMatrix(transform);
    Matrix worldMatrix = localMatrix;
    
    if (transform.parent != NULL) {
        Matrix parentMatrix = GetWorldMatrix(transform.parent);
        worldMatrix = localMatrix * parentMatrix;
 
        if (ContainsNonUniformScale(transform.parent)) {
            AffineParts decomp = decomp_affine(worldMatrix, &decomp);
 
            Transform temp;
            temp.parent = NULL;
            temp.position = decomp.t;
            temp.scale = decomp.k;
            temp.rotation = decomp.q;
        
            worldMatrix = ToMatrix(temp);  
        }  
    }
 
    return worldMatrix;
}

void decomp_affine(HMatrix A, AffineParts *parts) {
    HMatrix Q, S, U;
    Quat p;
    float det;
    parts->t = Qt_(A[X][W], A[Y][W], A[Z][W], 0);
    det = polar_decomp(A, Q, S);
    if (det<0.f) {
        mat_copy(Q,=,-Q,3);
        parts->f = -1;
    } 
    else {
        parts->f = 1;
    }
    parts->q = Qt_FromMatrix(Q);
    parts->k = spect_decomp(S, U);
    parts->u = Qt_FromMatrix(U);
    p = snuggle(parts->u, &parts->k);
    parts->u = Qt_Mul(parts->u, p);
}

struct FactorTranslationResult {
    Matrix T; // Translation
    Matrix X; // Linear transformation
};
 
FactorTranslationResult FactorTranslation(Matrix M) {
    FactorTranslationResult result;
 
    result.T = Matrix4();
    result.T[12] = M[12];
    result.T[13] = M[13];
    result.T[14] = M[14];
 
    result.X = M;
    result.X[12] = 0;
    result.X[13] = 0;
    result.X[14] = 0;
 
    return result;
}

struct PolarDecompResult {
    Matrix Q; // 旋转/翻转矩阵
    Matrix S; // 对称矩阵（缩放+剪切）
};
 
bool PolarDecompositionEarlyOut(Matrix Q, Matrix Qprev) {
    return (Q - Qprev); // 全部分量足够接近 0
}
 
PolarDecompResult PolarDecomposition(Matrix X) {
    Matrix Q = X;                       // Q0
    for (int i = 0; i < 20; ++i) {      // 最多 20 次
        Matrix Qprev = Q;
        Matrix Qit   = Inverse(Transpose(Q)); // Q_i^{-T}
        Q = 0.5f * (Q + Qit);                // 取平均
        if (PolarDecompositionEarlyOut(Q, Qprev))
            break;                           // 收敛就退出
    }
    result.Q = Q;
    // result.S = Inverse(Q) * X;
    // 可以用正交矩阵的转置来替代其逆 计算更快
    result.S = Transpose(Q) * X;
    // 或者，使用 “正交性” 容差，在未成功收敛到正交时时兜底
    // if (fabs(det(Q) - 1.0f) < 1e-3f && 
    //     norm(Q.transpose()*Q - I) < 1e-3f)
    //     S = Q.transpose() * X;
    // else
    //     S = inverse(Q) * X;   // 退化或未收敛时兜底
 
    return result;
}

struct FactorRotationResult {
    Matrix F; // ±I，det<0 时为 -I
    Matrix R; // 纯旋转矩阵
};
 
FactorRotationResult FactorRotation(Matrix Q) {
    float f = 1.0f;
    if (Det(Q) < 0) {   // 含镜像
        f = -1.0f;
        Q = Q * -1;     // 元素逐分量取负，去掉翻转
    }
    result.F = diag(f,f,f,1); // ±I
    result.R = Q;            // 现在 det(R)=+1
    return result;
}

Vector x(A[0], A[1], A[2]);
Vector y(A[4], A[5], A[6]);
Vector z(A[8], A[9], A[10]);
 
y = y - ProjectionV3(x, y);
z = (z - Projection(x, z)) - Projection(y, z);
 
x = Normalize(x);
y = Normalize(y);
z = Normalize(z);
 
// 写回 result.Q 的 0~2、4~6、8~10 元素

result.R = Transpose(result.Q) * A;

struct QRDecompResult {
    Matrix Q; // Q is an orthogonal matrix
    Matrix R; // R os an upper triangular matrix
}
 
QRDecompResult QRDecomposition(Matrix A) {
    QRDecompResult result;
 
    Vector x(A[0], A[1], A[2]);
    Vector y(A[4], A[5], A[6]);
    Vector z(A[8], A[9], A[10]);
 
    y = y -  ProjectionV3(x, y); // y minus projection of y onto x
    z = (z - Projection(x, z)) - Projection(y, z);
 
    x = Normalize(x);
    y = Normalize(y);
    z = Normalize(z);
 
    result.Q[0] = x[0]; resuly.Q[1] = x[1]; result.Q[2] = x[2];
    result.Q[4] = y[0]; result.Q[5] = y[1]; result.Q[6] = y[2];
    result.Q[8] = z[0]; result.Q[9] = z[1]; result.Q[10]= z[2];
 
    result.R = Transpose(result.Q) * A;
 
    return result;
}

struct SpectralDecompositionResult{
    Matrix U;   // 特征向量矩阵
    Matrix K;   // 对角存特征值
    Matrix Ut;  // U 的转置
};
 
bool SpectralDecompositionEarlyOut(Matrix A){
    int idx[]{4,8,9,12,13,14};   // 下三角的 6 个元素
    for(auto i: idx)
        if(!EpsilonCompare(A[i],0)) return false;
    return true;                 // 都接近 0 说明已趋近对角
}
 
SpectralDecompositionResult SpectralDecomposition(Matrix S){
    result.U = Identity();
    Matrix A = S;
 
    for(int i=0;i<20;++i){
        auto decomp = QRDecomposition(A); // 第 1 步
        result.U = result.U * decomp.Q;   // 第 2 步：累积特征向量
        A        = decomp.R * decomp.Q;   // 第 3 步：A ← RQ
 
        if(SpectralDecompositionEarlyOut(A)) break;
    }
 
    result.Ut = Transpose(result.U);      // 保存 U^T
 
    result.K = Identity();
    result.K[0]=A[0]; result.K[1]=A[1]; result.K[2]=A[2]; // 对角=特征值
 
    return result;
}

// ❶ 24 个 3×3 置换-翻符号矩阵预先写在 permutations[24]
Matrix U1 = input.U;        // 上一帧或当前基准
Matrix U2 = input.Ut;       // 本帧原始 U (待调整)
 
// ❷ 遍历全部 24 组合
for(i=0;i<24;++i){
    Matrix U2p = permutations[i] * U2;        // 应用置换/翻符号
    Quaternion q = ToQuat(  U2p * U1ᵀ );       // 相对四元数
    if(q.w > best) { best=q.w; save=i; }      // cos(θ/2) 最大 = 旋转角最小
}
 
// ❸ 选中最佳组合
Matrix U2_best = permutations[save] * U2;
Matrix Ut_best = U2_bestᵀ;
Matrix K_best  = diag( K 原值按同样置换/翻符号重排 );
 
// ❹ 输出
return { U2_best, K_best, Ut_best };

struct SpectralAdjustmentResult {
    Matrix U; // Each basis vector is an eigenvector
    Matrix K; // Contains eigenvalues on main diagonal
    Matrix Ut; // Transpose of U
}
 
SpectralAdjustmentResult SpectralDecompositonAdjustment(SpectralDecompositionResult input) {
  Matrix U1 = input.U;
  Matrix U1t = input.Ut;
 
  Matrix[24][9] m_permutations = [
    // Permutation 0: x, y, z
    [ 1, 0, 0,  0, 1, 0,  0, 0, 1 ],
    [-1,-0,-0, -0,-1,-0,  0, 0, 1 ],
    [ 1, 0, 0, -0,-1,-0, -0,-0,-1 ],
    [-1,-0,-0,  0, 1, 0, -0,-0,-1 ],
    // Permutation 1: x, z, y
    [-1,-0,-0,  0, 0, 1,  0, 1, 0 ],
    [-1,-0,-0, -0,-0,-1, -0,-1,-0 ],
    [ 1, 0, 0,  0, 0, 1, -0,-1,-0 ],
    [ 1, 0, 0, -0,-0,-1,  0, 1, 0 ],
    // Permutation 2: y, x, z
    [-0,-1,-0,  1, 0, 0,  0, 0, 1 ],
    [-0,-1,-0, -1,-0,-0, -0,-0,-1 ],
    [ 0, 1, 0,  1, 0, 0, -0,-0,-1 ],
    [ 0, 1, 0, -1,-0,-0,  0, 0, 1 ],
    // Permutation 3: y, z, x
    [ 0, 1, 0,  0, 0, 1,  1, 0, 0 ],
    [-0,-1,-0, -0,-0,-1,  1, 0, 0 ],
    [ 0, 1, 0, -0,-0,-1, -1,-0,-0 ],
    [-0,-1,-0,  0, 0, 1, -1,-0,-0 ],
     // Permutation 4: z, x, y
    [ 0, 0, 1,  1, 0, 0,  0, 1, 0 ],
    [-0,-0,-1, -1,-0,-0,  0, 1, 0 ],
    [ 0, 0, 1, -1,-0,-0, -0,-1,-0 ],
    [-0,-0,-1,  1, 0, 0, -0,-1,-0 ],
    // Permutation 5: z, y, x
    [-0,-0,-1,  0, 1, 0,  1, 0, 0 ],
    [-0,-0,-1, -0,-1,-0, -1,-0,-0 ],
    [ 0, 0, 1,  0, 1, 0, -1,-0,-0 ],
    [ 0, 0, 1, -0,-1,-0,  1, 0, 0],
  ];
 
  float x = input.K[0];
  float y = input.K[5];
  float z = input.K[10];
 
  Vector[6][3] eigen_value_permutations = [
    [ x,  y,  z], // Permutation 0
    [ x,  z,  y], // Permutation 1
    [ y,  x,  z], // Permutation 2
    [ y,  z,  x], // Permutation 3
    [ z,  x,  y], // Permutation 4
    [ z,  y,  x], // Permutation 5
  ];
 
  int saved_index = -1
  int saved_value = -1
 
  // The rotation taking U1 into U2 is U1t * U2
  for (int i = 0; i < 24; ++i) {
    Matrix U2 = m_permutations[i];
    Matrix U12 = U1t * U2;
 
    Quaternion QU12 = ToQuaternion(U12);
    
    // Optimize for largest w, which is smallest angle of rotation
    if (saved_index == -1 || QU12.w > saved_value) {
      saved_value = QU12.w
      saved_index = i
    }
  }
 
  Matrix U2t = Transpose(m_permutations[saved_index])
  var index = saved_index/4; // Integer division (floor this)
 
  SpectralAdjustmentResult result;
  result.U = Mul3(U1, U2t);
  result.K = Matrix(
    eigen_value_permutations[index][0], 0, 0, 0,
    0, eigen_value_permutations[index][1], 0, 0,
    0, 0, eigen_value_permutations[index][2], 0,
    0, 0, 0, 1
  );
  result.Ut = Transpose(result.U)
 
  return result;
}

AffineDecompositionResult AffineDecomposition(Matrix M) {
    // ① 平移
    auto trans  = FactorTranslation(M);          // 得 T, X
    // ② 极分解
    auto polar  = PolarDecomposition(trans.X);   // 得 Q, S
    // ③ 抽翻转 & 纯旋转
    auto rot    = FactorRotation(polar.Q);       // 得 F, R
    // ④ 特征分解
    auto spec   = SpectralDecomposition(polar.S);// 得 U, K, Ut
    // ⑤ snuggle 调整
    auto snug   = SpectralDecompositionAdjustment(spec);
 
    AffineDecompositionResult res;
    res.T  = trans.T;        // 平移
    res.F  = rot.F;          // ±1 翻转标记
    res.R  = rot.R;          // 纯旋转矩阵
    res.U  = snug.U;         // stretch-rotation
    res.K  = snug.K;         // 缩放对角
    res.Ut = snug.Ut;        // Uᵀ
    return res;
}

ShoemakeResult ConvertToShoemake(AffineDecompositionResult a){
    ShoemakeResult s;
    s.T = Vector3(a.T[12], a.T[13], a.T[14]);
    s.F = a.F[0];                      // ±1
    s.q = ToQuaternion(a.R);           // R → 四元数
    s.u = ToQuaternion(a.U);           // U → 四元数
    s.k = Vector3(a.K[0], a.K[5], a.K[10]);
    return s;
}