Skewed Vertices with Quaternions, Matrices, and Good Ol' Trig

Member
Posts: 227
Joined: 2008.08
Post: #1
I am getting a problem with my dynamic animated models where vertices are skewed when a rotation is performed. Translation works fine but that's no good if I can't also rotate the joints. I tried a matrix, a quaternion, and a normal rotation function, but nothing works.

The Good
Code:
    struct Matrix4x4{
        float m[16];
        void loadIdentity(){
            glPushMatrix();
                glLoadIdentity();
                glGetFloatv(GL_MODELVIEW_MATRIX,m);
            glPopMatrix();
        }
        Matrix4x4(){
            this->loadIdentity();
        }
        inline void set(oeVec3& move,oeVec3& rot){
            glPushMatrix();
                glLoadIdentity();
                glTranslatef(move.x,move.y,move.z);
                glRotatef(rot.x,1,0,0);
                glRotatef(rot.y,0,1,0);
                glRotatef(rot.z,0,0,1);
                glGetFloatv(GL_MODELVIEW_MATRIX,m);
            glPopMatrix();
        }
        inline oeVec3& operator *(oeVec3& v){
            //printf("Doing Dot Product with an oeVec3\n");
            /*v.x=v.x*this->m[0]+v.y*this->m[4]+v.z*this->m[8]+this->m[12];
            v.y=v.x*this->m[1]+v.y*this->m[5]+v.z*this->m[9]+this->m[13];
            v.z=v.x*this->m[2]+v.y*this->m[6]+v.z*this->m[10]+this->m[14];*/
            /*v.x=v.x*this->m[0]+v.y*this->m[1]+v.z*this->m[2]+this->m[4];
            v.y=v.x*this->m[5]+v.y*this->m[6]+v.z*this->m[7]+this->m[8];
            v.z=v.x*this->m[9]+v.y*this->m[10]+v.z*this->m[11]+this->m[12];*/
            return v;
        }
        inline void print(){
        //    for(int i=0;i<16;i++) printf("m[%i]=%f\n",i,m[i]);
            printf("%f\n",this->m[12]);
            printf("%f\n",this->m[13]);
            printf("%f\n",this->m[14]);

        }
    };

The C++ (The Bad)
Code:
namespace Model{
    struct Vertex:oeVec3{
        unsigned int b;
    };
    struct Bone:oeVec3{
        oeVec3 rot;
        int p;
        inline Bone& rotate(float x,float y,float z){
            OERotate((oeVec3*)this,x,y,z);
            this->rot.x=x;
            this->rot.y=y;
            this->rot.z=z;
            return *this;
        }
        inline void move(float x,float y,float z){
            this->x+=x;
            this->y+=y;
            this->z+=z;
        }
        inline void print(){
            printf("%f %f %f\n",x,y,z);
        }
    };
    struct CompiledBone:oeOrientation{
        //Matrix4x4 m;
        OEQuat rotation;
        inline void compile(){
            //OEQuatRotate(rotation,rot.x,1,0,0);
            float sineValue=sinf( rot.y / 1.0f * (M_PI/ 180.0f));
            rotation.x=0*sineValue;
            rotation.y=1*sineValue;
            rotation.z=0*sineValue;
            rotation.w=cosf(rot.y / 2.0f * (M_PI/ 180.));
            float m=rotation.x*rotation.x+rotation.y*rotation.y+rotation.z*rotation.z+rotation.w*r​otation.w;
            rotation.w/=m;
            rotation.x/=m;
            rotation.y/=m;
            rotation.z/=m;
            //OEQuatRotate(this->rotation, this->rot.y , 0, 1, 0);
            //OEQuatRotate(rotation,rot.z,0,0,1);
            //OEQuatRotateX(rotation,rot.x*toDegrees);
            //OEQuatRotateY(rotation,rot.y*toDegrees);
            //OEQuatRotateZ(rotation,rot.z*toDegrees);
        }
        inline void transformVec3(oeVec3&  v){
            //OEQuatRotateVec3(v,rotation,v);
            oeVec3 old=v;
            
            /*OEQuat tempq={-rotation.x,-rotation.y,-rotation.z,rotation.w};
            OEQuat temp={v.x,v.y,v.z,0};
            OEQuatMultiplyByVec3(temp,rotation,temp);
            OEQuatMultiplyByVec3(temp,temp,tempq);
            v.x=x+temp.x+old.x;
            v.y=y+temp.y+old.y;
            v.z=z+temp.z+old.z;*/
        }
    };
    struct DynamicModel{
        struct{unsigned int v,f,b;}count;
        Vertex* v;
        oeVec2* t;
        oeVec3* n;
        oeFace* f;
        Bone* b;
        oeVec3* cmpv;
        CompiledBone* cmpb;//cached for speed
        unsigned int displayList;
        
        virtual void oldLoad(char* filename){
            oeMesh* m=oeLoadObjFile(filename);
            printf("Attemping to load %s\n",filename);
            this->count.v=m->count.v;
            this->count.f=m->count.f;
            
            this->v=new Vertex[m->count.v];
            this->f=new oeFace[m->count.f];
            this->t=new oeVec2[m->count.v];
            this->n=new oeVec3[m->count.v];
            this->cmpv=new oeVec3[m->count.v];
            
            FILE* file=fopen(filename,"r");
            this->count.b=0;
            unsigned int cv,cb,boneID;
            int boneIDs[20];
            boneIDs[0]=-1;
            cv=cb=0;
            boneID=0;
            char line[512];
            //unsigned int bcount=0;
            
            if(file==NULL){
                printf("Failed to open %s\n",filename);
                exit(1);
            }
            
            while(!feof(file)){
                fgets(line,255,file);
                if((line[0]=='b')&&(line[1]==' ')&&strlen(line)-1>strlen("b 0\n")) this->count.b++;
            }
            rewind(file);
            
            this->b=new Bone[this->count.b];
            
            if(this->b==NULL){
                printf("Couldn't allocate %i bones for file named %s\n",this->count.b,filename);
                exit(2);
            }
            
            float x,y,z;
            while(!feof(file)){
                fgets(line,255,file);
                if(sscanf(line,"b %f %f %f",&x,&y,&z)==3){
                    this->b[cb].x=x;
                    this->b[cb].y=y;
                    this->b[cb].z=z;
                    this->b[cb].p=boneIDs[boneID];
                    boneID++;
                    boneIDs[boneID]=cb;
                    cb++;
                }
                else if(sscanf(line,"b %f",&x)==1){if(x==0)boneID--;}
                if(3==sscanf(line,"v %f %f %f",&x,&y,&z)){
                    this->v[cv].x=m->v[cv].x;
                    this->v[cv].y=m->v[cv].y;
                    this->v[cv].z=m->v[cv].z;
                    this->v[cv].b=boneIDs[boneID];
                    cv++;
                }
            }
            if(this->count.b!=cb){
                printf("Could only find %i of %i bones in %s\n",cb,this->count.b,filename);
                printf("Found %i of %i verts\n",cv,this->count.v);
                exit(1);
            }
            
            oeMeshDelete(m);
        }
        
    virtual void load(char* filename){
        int fcount,vcount,ncount,tcount;
        unsigned int cv,cf,ct,cn,v[3],n[3],t[3];
        char buffer[256];
        float x,y,z;
        char line[1000];
    
        cv=cf=ct=cn=fcount=vcount=ncount=tcount=0;
    
        FILE* file=fopen(filename,"r");
        
        this->count.b=0;
        unsigned int cb,boneID;
        int boneIDs[20];
        boneIDs[0]=-1;
        cb=0;
        boneID=0;

        if(file==NULL){
            printf("Failed to open %s\n",filename);
        //    return NULL;
            exit(1);
        }
        while(!feof(file)){
            fgets(buffer,255,file);
            if((buffer[0]=='v')&&(buffer[1]==' ')){
                vcount++;
            
            }
            else if((buffer[0]=='f')&&(buffer[1]==' ')){
                fcount++;
            
            }
            else if((buffer[0]=='v')&&(buffer[1]=='n')){
                ncount++;
                
            }
            else if((buffer[0]=='v')&&(buffer[1]=='t')){
                tcount++;
                
            }
            if((buffer[0]=='b')&&(buffer[1]==' ')&&strlen(buffer)-1>strlen("b 0\n")) this->count.b++;
        }
        rewind(file);

        
        this->v=new Vertex[vcount];
        this->f=new oeFace[fcount];
        this->t=new oeVec2[tcount];
        this->n=new oeVec3[ncount];
        this->cmpv=new oeVec3[vcount];
        this->b=new Bone[this->count.b];
            
        if(this->b==NULL){
            printf("Couldn't allocate %i bones for file named %s\n",this->count.b,filename);
            exit(2);
        }
    
        if(this->f==NULL){
            printf("Couldn't allocate %i faces for file named %s\n",fcount,filename);
            exit(2);
        }

        if(this->v==NULL){
            printf("Couldn't allocate %i vertices for file named %s\n",vcount,filename);
            exit(2);
        }
        if(this->n==NULL){
            printf("Couldn't allocate %i normals for file named %s\n",ncount,filename);
            exit(2);
        }

        if(this->t==NULL){
            printf("Couldn't allocate %i uv's for file named %s\n",tcount,filename);
            exit(2);
        }
        while(!feof(file)){
            fgets(buffer,255,file);
            if(3==sscanf(buffer,"b %f %f %f",&x,&y,&z)){
                this->b[cb].x=x;
                this->b[cb].y=y;
                this->b[cb].z=z;
                //printf("b boneIDs[boneID]:%i  position: %f %f %f ",boneIDs[boneID],x,y,z);
                this->b[cb].p=boneIDs[boneID];
                //printf("this->b[cb].p: %i ",this->b[cb].p);
                //printf("boneID:%i ",boneID);
                boneID++;
                //printf("boneID:%i ",boneID);
                boneIDs[boneID]=cb;
                //printf("boneIDs[boneID]:%i \n",boneIDs[boneID]);
                cb++;
                fflush(stdout);
            }
            else if(sscanf(buffer,"b %f",&x)==1){
                //if(x==0) boneID--;
            }
            else if(3==sscanf(buffer,"v %f %f %f",&x,&y,&z)){
                this->v[cv].x=this->cmpv[cv].x=x;
                this->v[cv].y=this->cmpv[cv].y=y;
                this->v[cv].z=this->cmpv[cv].z=z;
                
                this->v[cv].b=boneIDs[boneID];
                cv++;
            }
            else if(2==sscanf(buffer,"vt %f %f",&x,&y)){
                this->t[ct].x=x;
                this->t[ct].y=y;
                ct++;
            }
            else if(3==sscanf(buffer,"vn %f %f %f",&x,&y,&z)){
                this->n[cn].x=x;
                this->n[cn].y=y;
                this->n[cn].z=z;
                cn++;
            }
            else if(9==sscanf(buffer,"f %i/%i/%i %i/%i/%i %i/%i/%i",&v[0],&t[0],&n[0],&v[1],&t[1],&n[1],&v[2],&t[2],&n[2])){
        
                if(v[0]<1||v[1]<1||v[2]<1){
                    printf("Invalid vertex id in file %s\n",filename);
                    exit(1);
                }
                this->f[cf].v[0]=v[0]-1;
                this->f[cf].v[1]=v[1]-1;
                this->f[cf].v[2]=v[2]-1;
            
                this->f[cf].t[0]=t[0]-1;
                this->f[cf].t[1]=t[1]-1;
                this->f[cf].t[2]=t[2]-1;
            
                this->f[cf].n[0]=n[0]-1;
                this->f[cf].n[1]=n[1]-1;
                this->f[cf].n[2]=n[2]-1;
                cf++;
            }
            else if(6==sscanf(buffer,"f %d//%d %d//%d %d//%d",&v[0],&n[0],&v[1],&n[1],&v[2],&n[2])){
        
                if(v[0]<1||v[1]<1||v[2]<1){
                    printf("Invalid vertex id in file %s\n",filename);
                    exit(1);
                }
            
                this->f[cf].v[0]=v[0]-1;
                this->f[cf].v[1]=v[1]-1;
                this->f[cf].v[2]=v[2]-1;
            
                this->f[cf].n[0]=n[0]-1;
                this->f[cf].n[1]=n[1]-1;
                this->f[cf].n[2]=n[2]-1;
                cf++;
            }
            else if(3==sscanf(buffer,"f %d %d %d",&v[0],&v[1],&v[2])){
        
                if(v[0]<1||v[1]<1||v[2]<1){
                    printf("Invalid vertex id in file %s\n",filename);
                    exit(1);
                }
                this->f[cf].v[0]=v[0]-1;
                this->f[cf].v[1]=v[1]-1;
                this->f[cf].v[2]=v[2]-1;
                cf++;
            }
        }
        if(cf==fcount&&cv==vcount&&ct==tcount&&cn==vcount){
            this->count.f=fcount;
            this->count.v=vcount;
        }else{
            printf("\nError loading %s\nFound|Stated to be\n",filename);
            printf("Vertex:%d|%d\n",cv,vcount);
            printf("Face:%d|%d\n",cf,fcount);
            printf("TexCoord:%d|%d\n",ct,tcount);
            printf("Normals:%d|%d\n",cn,ncount);
            exit(1);
        }
        if(this->count.b!=cb){
            printf("Could only find %i of %i bones in %s\n",cb,this->count.b,filename);
            printf("Found %i of %i verts\n",cv,this->count.v);
            exit(1);
        }
        fclose(file);
        this->cmpb=new CompiledBone[this->count.b];
    }

        inline void draw(){
            oeVec3* v;oeFace* f;oeVec3* n;oeVec2* t;
            glBegin(GL_TRIANGLES);
                for(int cf=0; cf<this->count.f; cf++){
                    f=&this->f[cf];
                    for (int cv = 0; cv <3; cv++) {
                        v=&this->cmpv[this->f[cf].v[cv]];
                        t=&this->t[this->f[cf].t[cv]];
                        n=&this->n[this->f[cf].n[cv]];
                
                        glNormal3f(n->x,n->y,n->z);
                        glTexCoord2f(t->x,t->y);
                        glVertex3f(v->x,v->y,v->z);
                        
                    }
                }
            
            glEnd();
            
        //    OEDebugOnlyLine(printf("In %s, line #%i:%s\n",__FILE__,__LINE__,gluErrorString(glGetError())));
        }
        inline void drawWithNonCompiledVs(){
            oeVec3* v;oeFace* f;oeVec3* n;oeVec2* t;
            glBegin(GL_TRIANGLES);
                for(int cf=0; cf<this->count.f; cf++){
                    f=&this->f[cf];
                    for (int cv = 0; cv <3; cv++) {
                        v=&this->v[this->f[cf].v[cv]];
                        t=&this->t[this->f[cf].t[cv]];
                        n=&this->n[this->f[cf].n[cv]];
                
                        glNormal3f(n->x,n->y,n->z);
                        glTexCoord2f(t->x,t->y);
                        glVertex3f(v->x,v->y,v->z);
                        
                        //printf("v = %f %f %f\n",v->x,v->y,v->z);
                    }
                }
            
            glEnd();
        //    OEDebugOnlyLine(printf("In %s, line #%i:%s\n",__FILE__,__LINE__,gluErrorString(glGetError())));
        }
        inline Bone& bone(int ID){
            return b[ID];
        }
        virtual void flush(){
            memset((void*)cmpb,0,sizeof(CompiledBone)*this->count.b);
            for(int i=0;i<this->count.b;i++){
                if(i==0){//for that special case where 0 gets skipped
                    cmpb[i].x=b->x;
                    cmpb[i].y=b->y;
                    cmpb[i].z=b->z;
                    cmpb[i].rot.x=b->rot.x;
                    cmpb[i].rot.y=b->rot.y;
                    cmpb[i].rot.z=b->rot.z;
                }
                else{
                    Bone* b=&this->b[i];
                    while(b->p!=-1){
                        cmpb[i].x+=b->x;
                        cmpb[i].y+=b->y;
                        cmpb[i].z+=b->z;
                        cmpb[i].rot.x+=b->rot.x;
                        cmpb[i].rot.y+=b->rot.y;
                        cmpb[i].rot.z+=b->rot.z;
                        b=&this->b[b->p];
                    }
                }
                cmpb[i].compile();
            }
            for(int j=0;j<this->count.v;j++){
                this->cmpv[j]=this->v[j];
                cmpb[this->v[j].b].transformVec3(this->cmpv[j]);
                /*cv.x=this->b[v.b].x+v.x;
                cv.y=this->b[v.b].y+v.y;
                cv.z=this->b[v.b].z+v.z;*/
            }
            
        }
        virtual void release(){
            delete[] this->v;
            delete[] this->cmpv;
            delete[] this->f;
            delete[] this->t;
            delete[] this->n;
            delete[] this->b;
            delete[] this->cmpb;
        }
    };
    
}

and The C (and The Ugly)
Code:
#define OEAdd(a,b) {a.x+b.x,a.y+b.y,a.z+b.z}
#define OESub(a,b) {a.x-b.x,a.y-b.y,a.z-b.z}
#define OEMul(a,b) {a.x*b.x,a.y*b.y,a.z*b.z}
#define OEDiv(a,b) {a.x/b.x,a.y/b.y,a.z/b.z}
#define OEDot(a,b) {a.x*b.x+a.y*b.y+a.z*b.z}

#define OEQuatMultiply(q,a,b) \
        ({q.x=a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y;\
        q.y=a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x;\
        q.z=a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w;\
        q.w=a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z;})
#define OEQuatMultiplyByVec3(q,a,b) \
        ({q.x=a.w*b.x  + a.y*b.z - a.z*b.y;\
        q.y=a.w*b.y - a.x*b.z  + a.z*b.x;\
        q.z=a.w*b.z + a.x*b.y - a.y*b.x;})
#define OEQuatConjungate(q) ({q.x=-q.x; q.y=-q.y; q.z=-q.z;})
#define OEQuatRotateVec3(r,q,v) \
        ({\
            OEQuat tempq={-q.x,-q.y,-q.z,q.w};\
            OEQuat temp={v.x,v.y,v.z,0};\
            OEQuatMultiplyByVec3(temp,q,temp);\
            OEQuatMultiplyByVec3(temp,temp,tempq);\
            /*return*/ r=temp;r;\
        })
#define OEQuatRotate(q,angle,x,y,z)\
    ({\
        float sineValue=sinf(angle / 2.0f * (M_PI/ 180.0f));\
        q.x=x*sineValue;\
        q.y=y*sineValue;\
        q.z=z*sineValue;\
        q.w=cosf(angle / 2.0f * (M_PI/ 180.));\
    })
#define OEQuatRotateX(q,angle) q.x=sinf(angle/2.0);
#define OEQuatRotateY(q,angle) q.y=sinf(angle/2.0);
#define OEQuatRotateZ(q,angle) q.z=sinf(angle/2.0);

inline void OERotate(oeVec3* p,float x,float y,float z){
//Z
    p->x= p->x*cos( toRadians*z) - p->y*sin( toRadians*z);
    p->y= p->x*sin( toRadians*z) + p->y*cos( toRadians*z) ;
//Y
    p->x=p-> x*cos( toRadians*y) - p->z*sin( toRadians*y);
    p->z= p->x*sin( toRadians*y) + p->z*cos( toRadians*y) ;
//X
    p->z= p->y*cos( toRadians*x) - p->z*sin( toRadians*x);
    p->y= p->y*sin( toRadians*x) + p->z*cos( toRadians*x) ;    
}
Quote this message in a reply
Member
Posts: 227
Joined: 2008.08
Post: #2
Come on, no one can correct my math? ::BUMP::
Quote this message in a reply
Moderator
Posts: 1,562
Joined: 2003.10
Post: #3
Can't spend the time to look at it in detail at the moment, but you can compare against the reference implementations I use for matrix, vector, and quaternion math here:

http://sacredsoftware.net/tutorials/Vectors/Vector.c
http://sacredsoftware.net/tutorials/Quat...aternion.c
http://sacredsoftware.net/tutorials/Matrices/Matrix.c
Quote this message in a reply
Sage
Posts: 1,199
Joined: 2004.10
Post: #4
That was a lot of code, and while I skimmed it, I didn't fuly parse it. But off the top of my head I have a guess:

When performing your matrix transforms on your model, are you (semantically) doing something like the following?

Code:
for each vertex:
    vertices[i] = matrix * vertices[i]

Because if you are, what you're seeing is a progressive loss of precision.
Quote this message in a reply
Member
Posts: 227
Joined: 2008.08
Post: #5
No, no progressive loss of presicion, as the model makes 2 sets of vertecies, one that is write-once read-only and another that is write-only so any loss of precision is kept constant.

@ThemsAllTook: Great sample, its all in C and easy to follow, thanks.
Quote this message in a reply
Post Reply 

Possibly Related Threads...
Thread: Author Replies: Views: Last Post
  Game rectangles & OpenGL vertices mk12 2 2,856 Sep 7, 2010 07:45 PM
Last Post: mk12
  squares[] and sorting vertices into arrays... mikey 3 3,075 Sep 11, 2009 11:21 AM
Last Post: mikey
  Angles and Matrices and Whatnot merrill541 5 3,516 Jun 2, 2009 04:13 AM
Last Post: unknown
  Quaternions: Pointing one vector at another wonza 4 3,632 Oct 1, 2008 12:34 PM
Last Post: wonza
  Problems with Rotation using Quaternions wyrmmage 2 3,847 Apr 6, 2008 05:57 PM
Last Post: wyrmmage