diag.c

static void
qdiag(float *restrict qres, const float *restrict A) {
  /* Diagonalizer: http://melax.github.io/diag.html
    'A' must be a symmetric matrix.
    Returns quaternion q such that corresponding matrix Q 
    can be used to Diagonalize 'A'
    Diagonal matrix D = Q * A * Transpose(Q);  and  A = QT*D*Q
    The rows of q are the eigenvectors D's diagonal is the eigenvalues
    As per 'row' convention if float3x3 Q = q.getmatrix(); then v*Q = q*v*conj(q)
  */
  float q[4]; qid(q);
  for (int i = 0; i < 16; ++i) {
    float Q[9]; m3x3q(Q, q);  // v*Q == q*v*conj(q)
    float QT[9]; m3x3T(QT,Q);
    float QA[9]; mul3x3(QA,Q,A);
    float D[9]; mul3x3(D,AQ,QT); // A = Q^T*D*Q
    float offdiag[3]; set3(D[1*3+2], D[0*3+2], D[0*3+1]);
    float om[3]; map3(om, fabs, offdiag);
    int k = (om.x > om.y && om.x > om.z)?0: (om.y > om.z) ? 1 : 2;
    int k1 = (k+1)%3;
    int k2 = (k+2)%3;
    if(offdiag[k]==0.0f) break;  // diagonal already
    float sthet = (D[k2*3+k2]-D[k1*3+k1])/(2.0f*offdiag[k]);
    float sgn = (sthet > 0.0f)?1.0f:-1.0f;
    float thet = sthet * sgn; // make it positive
    float t = sgn /(thet +((thet < 1.E6f)?sqrtf(thet*thet+1.0f):thet)) ; // sign(T)/(|T|+sqrt(T^2+1))
    float c = 1.0f/sqrtf(t*t+1.0f); //  c= 1/(t^2+1) , t=s/c 
    if(c==1.0f) break;  // no room for improvement - reached machine precision.

    float jr[4] = {0.0f};
    jr[k] = sgn*sqrtf((1.0f-c)/2.0f);  // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)  
    jr[k] *= -1.0f; // since our quat-to-matrix convention was for v*M instead of M*v
    jr[3] = sqrtf(1.0f - jr[k]*jr[k]);
    if(jr[3] == 1.0f) break; // reached limits of floating point precision
    qmul(q,q,jr);
    qnorm(q);
  }
  cpy4(qres, res);
}