Thread: Quaternion Rotation

  1. #1
    The Right Honourable psychopath's Avatar
    Join Date
    Mar 2004
    Location
    Where circles begin.
    Posts
    1,071

    Quaternion Rotation

    I've done as much research as I can on this; i've read numerous articles online, read and re-read the sections of my physics book on quaternions, and messed with my physics code for the last week.

    However, after all that, I still can't get quaternion rotation to work properly.

    I'm working on collision checking, starting first with a simple function to check contanct between a box and the ground plane. For this collision check, I set 8 vertices at the bounding corners of the object in question, and loop though the vertices to check for contact, and apply forces as necessary. However, for rotation, I of course, need to rotate these bounding vertices.

    My code for rotating a vector by a quaternion is as follows:
    Code:
    Vec3 Rotate(Vec3 Vec){
    	Quat Q(n, V.x, V.y, V.z); //this quaternion
    	Quat tQ = Q*Vec*(~Q); //new quaternion
    
    	return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
    }
    And so for each bounding vertex:
    Code:
    for(int i=0; i<8; i++){
    	tmp = actor->points[i];
    	tmpR[i] = actor->orientation.Rotate(tmp);
    	tmpR[i] += actor->position;
    }
    Which produces innacurate results; or doesn't rotate at all, i'm not sure. I can drop a cube on a 45 degree angle, and it will not rotate at all, although, if I look very closely, it does appear to be moving veeery slightly.

    When I first set the actor's orientation quaternion, I call this code, to create the orientation from 3 euler angles:
    Code:
    Quat EulerToQuat(Vec3 V){
    	Quat Qx(cos(V.x/2), Vec3(sin(V.x/2),0,0));
    	Quat Qy(cos(V.y/2), Vec3(0,sin(V.y/2),0));
    	Quat Qz(cos(V.z/2), Vec3(0,0,sin(V.z/2)));
    
    	Quat retQuat = Qx*Qy*Qz;
    	return Quat(DegToRad(retQuat.n), retQuat.V.x, retQuat.V.y, retQuat.V.z);
    }
    And then I normalize the quaternion.

    At the end of the integration step, I call
    Code:
    Vec3 QuatToAxisAngle(){
    	float scale = sin(acos(n));
    				
    	float aX = V.x/scale;
    	float aY = V.y/scale;
    	float aZ = V.z/scale;
    
    	return Vec3(RadToDeg(aX), RadToDeg(aY), RadToDeg(aZ));
    }
    Which *should* give me the angles that I then apply to my rotation matrix (my matrix rotation function does work if I use hardcoded values, eliminating the possibility that this is the problem).

    There are other quaternion rotation calls made throughout the code, which are also probably broken, but I figure if I can get this problem solved, I should consequently learn how to fix all my other rotation issues as they arise.

    Anyway, I think i've provided all the necessary information, but if not, let me know.

    Thanks!
    M.Eng Computer Engineering Candidate
    B.Sc Computer Science

    Robotics and graphics enthusiast.

  2. #2
    Registered User
    Join Date
    Apr 2006
    Posts
    43
    Ok, some parts look ok, others are a little weird to me, some of the code looks incomplete, is Rotate a class function?

    Here are some of the thing I thought about when I looked through the Rotate function, perhaps my confusion can be helpful to you =P

    Where is the quaternion that Vec should be rotated by?
    Vec must be normalized.
    n should be zero
    What is the result of Q*Vec ? What do the overloaded multiply operator do?
    Make sure you always use either (w,xyz) or (xyz,w) order in the quaternion code, especially if you have used code from different places.

    // The following code skeleton should work.
    Code:
    Vector3 Quaternion::transform(const Vector3 &v) const
    {
      const Quaternion vq(v[0], v[1], v[2], 0);
      Quaternion result(this->multiply(vq.multiply(this->inverse())));
      return Vector3(result[0], result[1], result[2]);
    }
    Using your types and functions I would guess it should be something like this.
    Code:
    // Rotate the vector Vec by the quaternion rot.
    Vec3 Rotate(Vec3 Vec, Quat rot) {
     Quat Q(0, V.x, V.y, V.z); //this quaternion
     Quat tQ = rot*Q*(~rot); //new quaternion
    
     return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
    }
    Try sending in easy to debug values into your function and print the result, i.e. rotate (1,0,0) 90 degrees and see if you get what you expected, that can be really helpful if you just have missed some sign or other small thing somewhere in the quaternion code.

    (when you rotate those bounding box vertices, make sure you are in the right coordinate system, otherwise the result won't be what you want)

    Good luck!

  3. #3
    The Right Honourable psychopath's Avatar
    Join Date
    Mar 2004
    Location
    Where circles begin.
    Posts
    1,071
    Sorry, I probably should have been a little more clear on my functions.

    Here's the entire class, for completeness.
    Code:
    namespace Clockwork{
    	namespace Quaternion{
    
    		float const	tol = 0.0000000001f;
    
    		public value class Quat{
    		public:
    			float n;
    			Vec3 V;
    
    			float DegToRad(float deg){
    				return deg * PI / 180.0f;
    			}
    			float RadToDeg(float rad){
    				return rad * 180.0f / PI;
    			}
    
    			Quat(float n, float vX, float vY, float vZ){
    				n = n;
    				V.x = vX;
    				V.y = vY;
    				V.z = vZ;
    			}
    			Quat(float n, Vec3 V){
    				n = n;
    				this->V = V;
    			}
    
    			Quat operator~(){
    				return Quat(n, -V.x, -V.y, -V.z);
    			}
    
    			Quat operator+(Quat Q){
    				return Quat(n+Q.n, V+Q.V);
    			}
    			
    			Quat operator-(Quat Q){
    				return Quat(n=Q.n, V-Q.V);
    			}
    
    			Quat operator*(Quat Q){
    				return Quat(n*Q.n - V.x*Q.V.x - V.y*Q.V.y - V.z*Q.V.z,
    							n*Q.V.x + V.x*Q.n + V.y*Q.V.z - V.z*Q.V.y,
    							n*Q.V.y + V.y*Q.n + V.z*Q.V.x - V.x*Q.V.z,
    							n*Q.V.z + V.z*Q.n + V.x*Q.V.y - V.y*Q.V.x);
    			}
    			Quat operator*(Vec3 Vec){
    				return Quat(-(V.x*Vec.x + V.y*Vec.y + V.z*Vec.z),
    							n*Vec.x + V.z*Vec.y - V.y*Vec.z,
    							n*Vec.y + V.x*Vec.z - V.z*Vec.x,
    							n*Vec.z + V.y*Vec.x - V.x*Vec.y);
    			}
    			Quat operator*(float num){
    				return Quat(n*num, V.x*num, V.y*num, V.z*num);
    			}
    
    			Quat operator/(float num){
    				return Quat(n/num, V.x/num, V.y/num, V.z/num);
    			}
    
    			float Length(){
    				return (float)sqrt((n*n) + (V.x*V.x) + (V.y*V.y) + (V.z*V.z));
    			}
    
    			float GetAngle(){
    				return (float)(2*acos(n));
    			}
    
    			Quat Normalize(){
    				Quat res(n, V.x, V.y, V.z);
    				res = res / Length();
    				
    				return res;
    			}
    
    			Vec3 GetAxis(){
    				Vec3 Vec = V;
    				float m = Vec.Length();
    
    				if(m<tol){
    					return Vec3();
    				}
    				else{
    					return Vec/m;
    				}
    			}
    
    			Vec3 QuatToAxisAngle(Quat Q){
    				//float scale = (float)sqrt((Q.V.x*Q.V.x) + (Q.V.y*Q.V.y) + (Q.V.z*Q.V.z));
    				float scale = sin(acos(n));
    				
    				float aX = Q.V.x/scale;
    				float aY = Q.V.y/scale;
    				float aZ = Q.V.z/scale;
    
    				return Vec3(aX, aY, aZ);
    			}
    			Vec3 QuatToAxisAngle(){
    				//float scale = (float)sqrt((V.x*V.x) + (V.y*V.y) + (V.z*V.z));
    				float scale = sin(acos(n));
    				
    				float aX = V.x/scale;
    				float aY = V.y/scale;
    				float aZ = V.z/scale;
    
    				return Vec3(RadToDeg(aX), RadToDeg(aY), RadToDeg(aZ));
    			}
    
    			Quat EulerToQuat(Vec3 V){
    				float vX = DegToRad(V.x);
    				float vY = DegToRad(V.y);
    				float vZ = DegToRad(V.z);
    
    				Quat Qx(cos(vX/2), Vec3(sin(vX/2),0,0));
    				Quat Qy(cos(vY/2), Vec3(0,sin(vY/2),0));
    				Quat Qz(cos(vZ/2), Vec3(0,0,sin(vZ/2)));
    
    				Quat retQuat = Qx*Qy*Qz;
    				return Quat(retQuat.n, retQuat.V.x, retQuat.V.y, retQuat.V.z);
    			}
    
    			Quat Rotate(Quat Q2){
    				Quat Q1(n, V.x, V.y, V.z);
    				return Q1*Q2*(~Q1);
    			}
    			Quat Rotate(Quat Q1, Quat Q2){
    				return Q1*Q2*(~Q1);
    			}
    			Vec3 Rotate(Vec3 Vec){
    				Quat Q(n, V.x, V.y, V.z);
    				Quat tQ = Q*Vec*(~Q);
    				tQ.n = 0;
    				return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
    			}
    			Vec3 RotateToRad(Vec3 Vec){
    				Quat Q(n, V.x, V.y, V.z);
    				Quat tQ = Q*Vec*(~Q);
    				return Vec3(DegToRad(tQ.V.x), DegToRad(tQ.V.y), DegToRad(tQ.V.z));
    			}
    			Vec3 RotateToDeg(Vec3 Vec){
    				Quat Q(n, V.x, V.y, V.z);
    				Quat tQ = Q*Vec*(~Q);
    				return Vec3(RadToDeg(tQ.V.x), RadToDeg(tQ.V.y), RadToDeg(tQ.V.z));
    			}
    			Vec3 Rotate(Quat Q, Vec3 Vec){
    				Quat tQ = Q*Vec*(~Q);
    				return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
    			}
    		};
    
    	}
    }

    I changed the Vec3 rotate function to this:
    Code:
    Vec3 Rotate(Vec3 Vec){
    	Quat Q(n, V.x, V.y, V.z);
    	Quat tQ = Q*Vec*(~Q);
    	tQ.n = 0;
    	return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
    }
    The quaternion is first set like this...
    Code:
    	this->orientation.EulerToQuat(CPhysCtrl::objects[objID]->rotation/10);
    	this->orientation.Normalize();
    where 'this' is the actor object in question, on its creation routine.

    The rotate function you posted above seems a little weird, because it just multiplies 3 quaternions together, and then returns a vector? I thought the quaternion had to be multiplied by the vector you want to rotate?

    As far as the coord systems go, i'm pretty sure that's consistant everywhere. If it matters, i'm using a right-handed coordinate system, like OpenGL (x=right, y=up, z=into the screen [or somthing like that ;p]).

    Thanks!
    M.Eng Computer Engineering Candidate
    B.Sc Computer Science

    Robotics and graphics enthusiast.

  4. #4
    Registered User VirtualAce's Avatar
    Join Date
    Aug 2001
    Posts
    9,607
    I'm not sure about this (you?) psychopath.

    Allow me to do some research and I'll get back to you. I'm not good enough with quats yet to just throw out a solution here.

    Perhaps Bob can help.

  5. #5
    (?<!re)tired Mario F.'s Avatar
    Join Date
    May 2006
    Location
    Ireland
    Posts
    8,446
    Boost provides boost/math/quaternion.hpp.
    That will probably take a lot of work from you.
    Originally Posted by brewbuck:
    Reimplementing a large system in another language to get a 25% performance boost is nonsense. It would be cheaper to just get a computer which is 25% faster.

  6. #6
    Registered User
    Join Date
    Apr 2006
    Posts
    43
    I of course made a small but vital typo (typed V.x instead of Vec.x) when I tried to write the code with your types...the code skeleton was correct.

    Let's see if I can resolve this for you.

    The trick with rotating vectors by using quaternions is to represent the vector as a quaternion with n=0.

    So first build a temporary quaternion from the vector we want to rotate, first line in the code I wrote (but V.x should be Vec.x etc).
    Then transform the new quaternion by the rotation quaternion Q, second line in the code.
    Then we need to extract the vector part of the transformed quaternion and return it, third line.

    Since I didn't know what kind of overloaded multiply operators you had between quaternions and vectors I took the safe path and just used quaternions.

    If you look at the two overloaded multiply-operators you'll notice that quaternion-vector multiplication is identical to quaternion-quaternion mulitplication except that n=0 and therefore some code has been optimized away.

    Code:
    Quat operator*(Quat Q){
      return Quat(n*Q.n - V.x*Q.V.x - V.y*Q.V.y - V.z*Q.V.z,
        n*Q.V.x + V.x*Q.n + V.y*Q.V.z - V.z*Q.V.y,
        n*Q.V.y + V.y*Q.n + V.z*Q.V.x - V.x*Q.V.z,
        n*Q.V.z + V.z*Q.n + V.x*Q.V.y - V.y*Q.V.x);
    }
    Quat operator*(Vec3 Vec){
      return Quat(-(V.x*Vec.x + V.y*Vec.y + V.z*Vec.z),
        n*Vec.x + V.z*Vec.y - V.y*Vec.z,
        n*Vec.y + V.x*Vec.z - V.z*Vec.x,
        n*Vec.z + V.y*Vec.x - V.x*Vec.y);
    }
    If you look at this function that you have in your code
    Code:
    Vec3 Rotate(Quat Q, Vec3 Vec){
      Quat tQ = Q*Vec*(~Q);
      return Vec3(tQ.V.x, tQ.V.y, tQ.V.z);
    }
    It does exactly the thing you want and in the same way as the code I suggested (but using overloaded quaternion-vector multiplication).

    Hope this helps.

    /F

  7. #7
    The Right Honourable psychopath's Avatar
    Join Date
    Mar 2004
    Location
    Where circles begin.
    Posts
    1,071
    Ok, I think all my rotation functions are correct, and I do now understand how they work a little better. However, i'm starting to think the issue lies more in how the orientation quaternion is being dealt with.

    What I mean is, I think I could be missing something in my EulerToQuat() functions, or not normalizing something where I should be.

    First of all, i've changed some of my quaternion code. I set the quaternion like this:
    Code:
    this->orientation = this->orientation.FromEulerAngles(CPhysCtrl::objects[objID]->rotation/10);
    The variable 'rotation', is a Vec3 containing the 3 euler angles for each respective axis in degrees. 'orientation' is the objects orientation quaternion.

    FromEulerAngles()...
    Code:
    Quat FromEulerAngles(Vec3 angle){
    	Quat qX = FromAxisAngle(angle.x, 1,0,0);
    	Quat qY = FromAxisAngle(angle.y, 0,1,0);
    	Quat qZ = FromAxisAngle(angle.z, 0,0,1);
    				
    	Quat res = qX*qY*qZ;
    	return res;
    }
    FromAxisAngle()...
    Code:
    Quat FromAxisAngle(float degA, float x, float y, float z){
    	float angle = DegToRad(degA);
    	float result = sin(angle/2.0f);
    
    	return Quat(cos(angle/2.0f), x*result, y*result, z*result);
    }
    I'm setting the cube to be rotated 40 degrees on the X initially. The quaternion comes back in the debugger as: n=5.0477321e-039, V.x=0, V.y=0, V.z=0
    Shouldn't at least one of the vector parts have a non-zero value?

    Also, all my points are coming back with x and z as zero, and y as 10 (the height the box starts at on initialization) after rotation. All the points are initially set 0.5f away from the boxes center ([0,0,0] in body coords) (before rotation).

    Thanks again!
    Last edited by psychopath; 07-13-2006 at 06:14 PM.
    M.Eng Computer Engineering Candidate
    B.Sc Computer Science

    Robotics and graphics enthusiast.

  8. #8

    Join Date
    May 2005
    Posts
    1,042
    You're on the right track; I implemented all of my math by reading a *lot* and fiddling around.

    Usually, when you have gotten one thing working properly, e.g. getting matrix to euler angle functionality, you can use that to test other things (you will know what i mean if you choose to delve deper into the scary world of implementing 3d math constructs on your own). Using boost isn't necessarily a bad idea.

    I've just gone ahead and uploaded my matrix3x3/matrix4x4, Vector and Quaternion code so you can look at it, and compare it against your own. You've got the right idea, and you're def. on the right track.

    In as far as I have used this stuff, well, I'm pretty confident it works (I use quaternions for representing the orientation of vehicles in my project).

    math

    enjoy and stuff
    Last edited by BobMcGee123; 07-14-2006 at 04:26 PM.
    I'm not immature, I'm refined in the opposite direction.

  9. #9
    The Right Honourable psychopath's Avatar
    Join Date
    Mar 2004
    Location
    Where circles begin.
    Posts
    1,071
    I'd use boost, but i'd really rather learn this stuff on my own (without a helper library, I mean).

    And the link you posted is broken, bob.
    M.Eng Computer Engineering Candidate
    B.Sc Computer Science

    Robotics and graphics enthusiast.

  10. #10

    Join Date
    May 2005
    Posts
    1,042
    What the hell, I can't add anything new to the ftp space.

    I just manually uploaded them instead (just the .cpp though)
    I'm not immature, I'm refined in the opposite direction.

  11. #11
    Registered User
    Join Date
    Apr 2002
    Posts
    1,571
    Take a close look at your Quat constructors. Is n = n really what you want to be doing there? Also look at your operator- , see anything amiss?
    Last edited by MrWizard; 07-14-2006 at 10:45 PM.
    "...the results are undefined, and we all know what "undefined" means: it means it works during development, it works during testing, and it blows up in your most important customers' faces." --Scott Meyers

  12. #12
    The Right Honourable psychopath's Avatar
    Join Date
    Mar 2004
    Location
    Where circles begin.
    Posts
    1,071
    Oh.

    Seriously, how did I miss that!? I must have looked at that 50 times by now! Argh.

    Anyway, it seems to work now (although the box just starts spinning wildly for some reason, the bounding verts are where they're supposed to be, and the orientation reads back OK).

    Thanks to fractality for explaining how this stuff worked, to Bob for taking the time to post the code (which was very helpful. it's nice to have somthing to compare against [I did find a few issues in the math of my own code after looking at yours]), and to MrWizard for ending my late-night, 4 hour math'n'code marathon.

    Ugh..i'm so dense...
    Thanks again all!
    M.Eng Computer Engineering Candidate
    B.Sc Computer Science

    Robotics and graphics enthusiast.

  13. #13

    Join Date
    May 2005
    Posts
    1,042
    to Bob for taking the time to post the code
    Yeah, np man. I can later show you how I've implemented quaternions to integrate orientation using RK4. In fact, I was forced to use an RK4 integrator so that my hovertanks could remain stable. I mostly used 'Game Physics' by David Eberly, my implementation is very similar to that on pgs 238+ of that book (I believe this is just the first edition), but I also had an understanding of the calculus/taylor series before I implemented it, from school, just not how to apply it to quaternions.

    One thing to note here is that if you are implementing an RK4 integrator you need to have some function that determines the force and torque on the object. It's hard to explain, but this 'function' should be different for different objects, and usually has something to do with some sort of AI.

    The reason is that you get intermittent 'positions' and 'orientations' inside the single RK4 step (4 to be exact!). In the case of something simple like a ball in freefall, there is no net torque from gravity, and the only force is simply going to be the weight of the object, easy enough. For a hovertank, however, the force and torque depends on the position and orientation of the object with respect to the ground (the closer a part of the chassis is to the ground, that area gets pushed upward due to an increased region of pressure, and vice versa when it is further away). Thus you get intermittent positions and orientations and need to run the logic to compute the continuous forces and torques on the body, within the RK4 step.

    You can also implement less accurate integrators which use less steps (RK2+3).

    I can help you out and hopefully help you avoid the frustration that I had which made me nearly commit suicide while implementing this...

    and it'll make more sense later on

    g'luck d00d
    Last edited by BobMcGee123; 07-17-2006 at 03:51 PM.
    I'm not immature, I'm refined in the opposite direction.

  14. #14
    Registered User VirtualAce's Avatar
    Join Date
    Aug 2001
    Posts
    9,607
    See I knew before I even responded that Bob would have the answer before my research was done.

    Ain't the board great.

  15. #15

    Join Date
    May 2005
    Posts
    1,042
    Are you hitting on me?!
    I'm not immature, I'm refined in the opposite direction.

Popular pages Recent additions subscribe to a feed

Similar Threads

  1. camera rotation matrix
    By Vick jr in forum Game Programming
    Replies: 5
    Last Post: 05-26-2009, 08:16 AM
  2. Image rotation - doesn't always work
    By ulillillia in forum C Programming
    Replies: 12
    Last Post: 05-03-2007, 12:46 PM
  3. Binary Search Trees Part III
    By Prelude in forum A Brief History of Cprogramming.com
    Replies: 16
    Last Post: 10-02-2004, 03:00 PM
  4. optimizing my quaternion code
    By confuted in forum Game Programming
    Replies: 9
    Last Post: 08-16-2003, 08:50 PM
  5. Camera problem: rolling
    By darksaidin in forum Game Programming
    Replies: 37
    Last Post: 08-15-2003, 08:49 AM