Rotation matrix -> quaternion -> matrix problem..

[mauryr]

Hello~
I ve been stuck for a while on this problem, which is probably trivial to most of you, and I can’t quite understand if I am doing something wrong or not. As the topic says, I need to make a routine to convert a rotation matrix into a quaternion and back. I understood this operation is totally reversible and it is possible to recreate the original matrix from the quaternion, provided it was an orthonormal matrix, besides some approximation error. Now… this is the output I get if I try to run some conversions:

Matr1: 
-0.947701, 0.11744351, 0.29676592
-0.07347012, 0.8245853, -0.56094676
0.31058836, 0.5534132, 0.7728316

q: 
0.82485795, -0.55718, 0.006911218, 0.09545682

Matr2: 
0.98168045, -0.16517824, -0.094971724
0.14977506, 0.36087695, 0.9205082
-0.11777481, -0.91786927, 0.37900543

q2: 
0.824858, -0.55718, 0.006911215, 0.09545682

matr3: 
0.98168045, -0.16517824, -0.09497173
0.14977507, 0.36087692, 0.9205082
-0.11777481, -0.9178693, 0.37900537

That output was generated running this code:

Matrix3f matr1=new Matrix3f(-0.947701f, -0.07347012f, 0.31058836f,
                                   0.11744351f, 0.8245853f, 0.5534132f,
                                   0.29676595f, -0.56094676f, 0.7728316f      );

        matr1.orthonormalize();
        matr1.traceMe("Matr1");
        Quaternion q=matr1.getRotationMatrix4f().toQuaternion();
        q.traceMe("q");
        Matrix3f matr2=q.toMatrix3f();        
        matr2.traceMe("Matr2");
        Quaternion q2=matr2.toQuaternion();q2.traceMe("q2");
        Matrix3f matr3=q2.toMatrix3f(); matr3.traceMe("matr3");

And the function I used are the following:

public Matrix3f orthonormalize(){
        Vector3f u1=new Vector3f(m[0], m[1], m[2]);
        Vector3f u2=new Vector3f(m[3], m[4], m[5]);
        Vector3f u3=new Vector3f(m[6], m[7], m[8]);
        
        Vector3f w1 = u1.normalize();
        Vector3f w2 = (u2.subtract(w1.proj(u2))).normalize();
        Vector3f w3 = (u3.subtract(w1.proj(u3)).subtract(w2.proj(u3))).normalize();             
        
        m[0]=w1.x;
        m[1]=w1.y;
        m[2]=w1.z;

        m[3]=w2.x;
        m[4]=w2.y;
        m[5]=w2.z;
        
        m[6]=w3.x;
        m[7]=w3.y;
        m[8]=w3.z;
        
        return this;
    }

and…

public Quaternion toQuaternion(){
        normalize();
        
        double tr, s;
        float[][] mat=new float[3][3];
        float[] q=new float[4];
        
        int i=0;
        for(int x=0; x<3; x++){  
            for(int y=0; y<3; y++){
                mat[x][y]=m[i++];
            }
        }
        
        tr= 0.25*(1.0+mat[0][0]+mat[1][1]+mat[2][2]);
        
        if(tr>0) {
            s= Math.sqrt( tr);
            q[0]= (float)s;
            s*= 4.0;
            q[1]= (float)((mat[1][2]-mat[2][1])/s);
            q[2]= (float)((mat[2][0]-mat[0][2])/s);
            q[3]= (float)((mat[0][1]-mat[1][0])/s);
        } else {
            q[0]= 0.0f;
            s= -0.5*(mat[1][1]+mat[2][2]);
            
            if(s>0) {
                s= Math.sqrt(s);
                q[1]= (float)s;
                q[2]= (float)(mat[0][1]/(2*s));
                q[3]= (float)(mat[0][2]/(2*s));
            } else {
                q[1]= 0.0f;
                s= 0.5*(1.0-mat[2][2]);
                
                if(s>FLT_EPSILON) {
                    s= Math.sqrt(s);
                    q[2]= (float)s;
                    q[3]= (float)(mat[1][2]/(2*s));
                } else {
                    q[2]= 0.0f;
                    q[3]= 1.0f;
                }
            }
        }
        return new Quaternion(q[1], q[2], q[3], q[0]).normalize();
    }

and finally…

public Matrix3f toMatrix3f(){  
/* float x2 = x * x;
        float y2 = y * y;
        float z2 = z * z;
        float xy = x * y;
        float xz = x * z;
        float yz = y * z;
        float wx = w * x;
        float wy = w * y;
        float wz = w * z;
 
        float[] values=new float[9];
 
        values[0]=1.0f - 2.0f * (y2 + z2);
        values[1]=2.0f * (xy + wz);
        values[2]=2.0f * (xz - wy);
 
        values[3]=2.0f * (xy - wz);
        values[4]=1.0f - 2.0f * (x2 + z2);
        values[5]=2.0f * (yz + wx);
 
        values[6]=2.0f * (xz + wy);
        values[7]=2.0f * (yz - wx);
        values[8]=1.0f - 2.0f * (x2 + y2);   */
            
            double q0, q1, q2, q3, qda,qdb,qdc,qaa,qab,qac,qbb,qbc,qcc;
            
            float[] values=new float[9];
            
            q0= M_SQRT2 * w;
            q1= M_SQRT2 * x;
            q2= M_SQRT2 * y;
            q3= M_SQRT2 * z;
            
            qda= q0*q1;
            qdb= q0*q2;
            qdc= q0*q3;
            qaa= q1*q1;
            qab= q1*q2;
            qac= q1*q3;
            qbb= q2*q2;
            qbc= q2*q3;
            qcc= q3*q3;
            Matrix3f tmp= new Matrix3f(
                    (float)(1.0-qbb-qcc),
                    (float)(qdc+qab),
                    (float)(-qdb+qac),
                    
                    (float)(-qdc+qab),
                    (float)(1.0-qaa-qcc),
                    (float)(qda+qbc),
                    
                    (float)(qdb+qac),
                    (float)(-qda+qbc),
                    (float)(1.0-qaa-qbb));
            return tmp.orthonormalize();
        }

The test case output shows the first quaternion derived matrix is different from the original one, but it yeilds the same quaternion, and the system is stable for further loops. So I ve been wondering if these 2 matrices would by chance give the same rotation, and I ve tested this in my graphics engine, but of course, they do not. I don’t see what I did wrong, after having tried to implement and re-implement this a couple times, using different sources, books, tutorials, websites etc, the one I posted is a translation of blender3d’s conversion functions.

Thanks to whoever reads this ~

[eteq]

I think your problem is that the rotation matrix you’re using does something inappropriate (not too helpful, I know, but…) . Maybe you do some sort of shear or other transform along with the rotation? Maybe try drawing some points or a shape with the matrix and see how it transforms space. At any rate, using the javax.vecmath, I tested your matrix, and got the same results you did… So its in the math, not your algorithm.

As an example of one that does work, try the following matrix as your initial:
0.87758255, -0.47942555, 0.0
0.47942555, 0.87758255, 0.0
0.0, 0.0, 1.0

When I tested it with the vecmath package, this one was stable through even the first cycle (its a simple z-axis rotation by half a radian).

Hope this helps.

[mauryr]

It’s the first thought I had too, and I noticed there are indeed some matrices generated by the code which work properly, some others dont. That matrix comes off the code I use to rotate the world as camera moves, and it seems to be perfectly fine, I ve been using it for a few months now and at least there is no VISIBLE shearing or anything else. Is it wrong of me to think an orthonormalized matrix is always a representation of a rotation? Thanks for replying~

[eteq]

One other thing to bear in mind… an orthonormal matrix represents rotation, but it can represent a composition of any number of rotations (or reflections) around different axis. A quaternion, meanwhile, represents only a single rotation around a single axis. So any set of rotations or reflections that can’t be represented as rotations around a single axis do something strange when you convert them to quaternions, but when you transform that quat back into a matrix, from then on it represents a valid single-axis rotation. But did you mean that the you used this code to convert to quat and back again in your program with no distortion? If so, I can’t explain it…

[operator]

some code for you…
there is no method for converting a matrix into an quaternion, because the rotations are stored as quaternions. the other direction should be working fine.

    public Quaternion() {
        m_x = m_y = m_z = 0.0f;
        m_w = 1.0f;
    }
    
    public void createFromAxisAngle(Vector3 vec, float rad)
    { 
        float result = (float)Math.sin(rad / 2.0f);
        m_w = (float)Math.cos( rad / 2.0f );
        m_x = vec.x * result;
        m_y = vec.y * result;
        m_z = vec.z * result;
    }
    
    public Matrix44 getRotMatrix()
    {
        Matrix44 ret = new Matrix44();
        
        float n,s;
        float xs, ys, zs;
        float wx, wy, wz;
        float xx, xy, xz;
        float yy, yz, zz;
        
        n = (m_x * m_x) + (m_y * m_y) + (m_z * m_z) + (m_w * m_w);
        s = (n > 0.0f) ? (2.0f / n) : 0.0f;

        xs = m_x * s;  ys = m_y * s;  zs = m_z * s;
        wx = m_w * xs; wy = m_w * ys; wz = m_w * zs;
        xx = m_x * xs; xy = m_x * ys; xz = m_x * zs;
        yy = m_y * ys; yz = m_y * zs; zz = m_z * zs;

        ret._11 = 1.0f - (yy + zz); ret._12 =         xy - wz;  ret._13 =         xz + wy;  ret._14 = 0.0f;
        ret._21 =         xy + wz;  ret._22 = 1.0f - (xx + zz); ret._23 =         yz - wx;  ret._24 = 0.0f;
        ret._31 =         xz - wy;  ret._32 =         yz + wx;  ret._33 = 1.0f - (xx + yy); ret._34 = 0.0f;
        ret._41 = 0.0f;             ret._42 = 0.0f;             ret._43 = 0.0f;             ret._44 = 1.0f;
       
        return ret;
    }
    
    public Quaternion multiply(Quaternion q)
    {
        Quaternion r = new Quaternion();
        
        r.m_w = m_w*q.m_w - m_x*q.m_x - m_y*q.m_y - m_z*q.m_z;
	r.m_x = m_w*q.m_x + m_x*q.m_w + m_y*q.m_z - m_z*q.m_y;
	r.m_y = m_w*q.m_y + m_y*q.m_w + m_z*q.m_x - m_x*q.m_z;
	r.m_z = m_w*q.m_z + m_z*q.m_w + m_x*q.m_y - m_y*q.m_x;
        
        return r;
    }
    
    public String toString()
    {
        return "Quaternion: x: " + m_x + " y: " + m_y + " z: " + m_z + " w: " + m_w;
    }

[mauryr]

Guess I’ll have to convert all the rotation generation code to store them as quaternions, that was on the to-do list anyways…
Thanks for all the help.