setFromMat33 method

Quat setFromMat33(Mat33 mat)

Set Quant from a 3x3 Matrix

Implementation

Quat setFromMat33(Mat33 mat) {
  List<double> m = mat.elements;
  double trace = m[0] + m[4] + m[8];
  double root;

  if ( trace > 0 ) {
    root = math.sqrt( trace + 1.0 );
    w = 0.5 / root;
    root = 0.5 / root;
    x = ( m[5] - m[7] ) * root;
    y = ( m[6] - m[2] ) * root;
    z = ( m[1] - m[3] ) * root;
  }
  else {
    List<double> out = [];
    int i = 0;
    if ( m[4] > m[0] ) i = 1;
    if ( m[8] > m[i*3+i] ) i = 2;

    int j = (i+1)%3;
    int k = (i+2)%3;

    root = math.sqrt( m[i*3+i] - m[j*3+j] - m[k*3+k] + 1.0 );
    out[i] = 0.5 * root;
    root = 0.5 / root;
    w = ( m[j*3+k] - m[k*3+j] ) * root;
    out[j] = ( m[j*3+i] + m[i*3+j] ) * root;
    out[k] = ( m[k*3+i] + m[i*3+k] ) * root;

    x = out[1];
    y = out[2];
    z = out[3];
  }

  return this;
}