Quaternion Based 6DOF Camera Lib

[bigsofty]

Hi,

This allows for full Pitch, Yaw, Roll as well was positioning the camera, oh, and most importantly for anything 'flying', no Gimbal lock problems.

This is based on a Freebasic article I read, maths is not my strong point but everything seems to function OK, not heavily tested though.

One line, marked, is for Gernot, had a problem with that line and had to split a calculation up into three lines to avoid it?

Any ways, have fun!


Ian

Actual lib, just add it to your project...

Code: (glbasic) [Select]

// --------------------------------- //
// Project: 6DOFCam_lib
// Start: Thursday, November 19, 2009
// IDE Version: 7.177
//

CONSTANT rad = 3.1415926535/180

TYPE Tquaternion
    w#
    x#
    y#
    z#
ENDTYPE

TYPE Tcamera
    //position
    x#
    y#
    z#

    //look vector
    lx#
    ly#
    lz#

    //up vector
    ux#
    uy#
    uz#

    //right vector
    rx#
    ry#
    rz#

    FOV#
    aspect#
    nearClip#
    farClip#
ENDTYPE


FUNCTION quaternion_normalize AS Tquaternion: tmpQ AS Tquaternion
LOCAL mag# = SQR(tmpQ.w*tmpQ.w+tmpQ.x*tmpQ.x+tmpQ.y*tmpQ.y+tmpQ.z*tmpQ.z)
LOCAL q AS Tquaternion
q.w = tmpQ.w / mag
q.x = tmpQ.x / mag
q.y = tmpQ.y / mag
q.z = tmpQ.z / mag
RETURN q
ENDFUNCTION


FUNCTION quaternion_conj AS Tquaternion: tmpQ AS Tquaternion
LOCAL q AS Tquaternion
q.w = -tmpQ.w
q.x = -tmpQ.x
q.y = -tmpQ.y
q.z = -tmpQ.z
    RETURN q
ENDFUNCTION


FUNCTION quaternion_mult AS Tquaternion: lhs AS Tquaternion, rhs AS Tquaternion
LOCAL q AS Tquaternion
    q.w = lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z
    q.x = lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y
    q.y = lhs.w * rhs.y - lhs.x * rhs.z + lhs.y * rhs.w + lhs.z * rhs.x
    q.z = lhs.w * rhs.z + lhs.x * rhs.y - lhs.y * rhs.x + lhs.z * rhs.w
RETURN q
ENDFUNCTION


FUNCTION camera_advance: cam AS Tcamera, d#
        LOCAL xt#, yt#, zt#
        xt = (cam.lx - cam.x) * d
        yt = (cam.ly - cam.y) * d
        zt = (cam.lz - cam.z) * d
        cam.x = cam.x + xt
        cam.y = cam.y + yt
        cam.z = cam.z + zt
        cam.ux =cam.ux + xt
        cam.uy =cam.uy + yt
        cam.uz =cam.uz + zt
        cam.rx =cam.rx + xt
        cam.ry =cam.ry + yt
        cam.rz =cam.rz + zt
        cam.lx =cam.lx + xt
        cam.ly =cam.ly + yt
        cam.lz =cam.lz + zt
ENDFUNCTION


FUNCTION camera_strafe: cam AS Tcamera, d#
        LOCAL xt#, yt#, zt#
        xt = (cam.rx - cam.x) * d
        yt = (cam.ry - cam.y) * d
        zt = (cam.rz - cam.z) * d
        cam.x = cam.x + xt
        cam.y = cam.y + yt
        cam.z = cam.z + zt
        cam.ux = cam.ux + xt
        cam.uy = cam.uy + yt
        cam.uz = cam.uz + zt
        cam.rx = cam.rx + xt
        cam.ry = cam.ry + yt
        cam.rz = cam.rz + zt
        cam.lx = cam.lx + xt
        cam.ly = cam.ly + yt
        cam.lz = cam.lz + zt
ENDFUNCTION


FUNCTION camera_rise: cam AS Tcamera, d#
        LOCAL xt#, yt#, zt#
        xt = (cam.ux - cam.x) * d
        yt = (cam.uy - cam.y) * d
        zt = (cam.uz - cam.z) * d
        cam.x = cam.x + xt
        cam.y = cam.y + yt
        cam.z = cam.z + zt
        cam.ux = cam.ux + xt
        cam.uy = cam.uy + yt
        cam.uz = cam.uz + zt
        cam.rx = cam.rx + xt
        cam.ry = cam.ry + yt
        cam.rz = cam.rz + zt
        cam.lx = cam.lx + xt
        cam.ly = cam.ly + yt
        cam.lz = cam.lz + zt
ENDFUNCTION


FUNCTION camera_roll: cam AS Tcamera, a#
        LOCAL qUp AS Tquaternion
        qUp.w = 0
        qUp.x = cam.ux - cam.x
        qUp.y = cam.uy - cam.y
        qUp.z = cam.uz - cam.z

        LOCAL qRight AS Tquaternion
        qRight.w = 0
        qRight.x = cam.rx - cam.x
        qRight.y = cam.ry - cam.y
        qRight.z = cam.rz - cam.z

        LOCAL qRot AS Tquaternion
        qRot.w = COS(a * rad/2)
        qRot.x = (cam.lx - cam.x) * SIN(a * rad/2)
        qRot.y = (cam.ly - cam.y) * SIN(a * rad/2)
        qRot.z = (cam.lz - cam.z) * SIN(a * rad/2)

        LOCAL W AS Tquaternion,W1 AS Tquaternion,W2 AS Tquaternion
        W1 = quaternion_mult(qRot,qUp)
        W2 = quaternion_conj(qRot)
        W = quaternion_mult(W2,W1)
        W = quaternion_normalize(W)
            cam.ux = W.x + cam.x
            cam.uy = W.y + cam.y
            cam.uz = W.z + cam.z
//        W = quaternion_mult(quaternion_mult(qRot,qRight), quaternion_conj(qRot)) <<< This does not work?!?!
        W1 = quaternion_mult(qRot,qRight)
        W2 = quaternion_conj(qRot)
        W = quaternion_mult(W2,W1)
        W = quaternion_normalize(W)
            cam.rx = W.x + cam.x
            cam.ry = W.y + cam.y
            cam.rz = W.z + cam.z
ENDFUNCTION

FUNCTION camera_pitch: cam AS Tcamera, a#
        LOCAL qUp AS Tquaternion
        qUp.w = 0
        qUp.x = cam.ux - cam.x
        qUp.y = cam.uy - cam.y
        qUp.z = cam.uz - cam.z

        LOCAL qLook AS Tquaternion
        qLook.w = 0
        qLook.x = cam.lx - cam.x
        qLook.y = cam.ly - cam.y
        qLook.z = cam.lz - cam.z

        LOCAL qRot AS Tquaternion
        qRot.w = COS(a * rad/2)
        qRot.x = (cam.rx - cam.x) * SIN(a * rad/2)
        qRot.y = (cam.ry - cam.y) * SIN(a * rad/2)
        qRot.z = (cam.rz - cam.z) * SIN(a * rad/2)

        LOCAL W AS Tquaternion,W1 AS Tquaternion,W2 AS Tquaternion
        W1 = quaternion_mult(qRot,qUp)
        W2 = quaternion_conj(qRot)
        W = quaternion_mult(W2,W1)
        W = quaternion_normalize(W)
            cam.ux = W.x + cam.x
            cam.uy = W.y + cam.y
            cam.uz = W.z + cam.z
        W1 = quaternion_mult(qRot,qLook)
        W2 = quaternion_conj(qRot)
        W = quaternion_mult(W2,W1)
        W = quaternion_normalize(W)
            cam.lx = W.x + cam.x
            cam.ly = W.y + cam.y
            cam.lz = W.z + cam.z
ENDFUNCTION


FUNCTION camera_yaw: cam AS Tcamera, a#
        LOCAL qRight AS Tquaternion
        qRight.w = 0
        qRight.x = cam.rx - cam.x
        qRight.y = cam.ry - cam.y
        qRight.z = cam.rz - cam.z

        LOCAL qLook AS Tquaternion
        qLook.w = 0
        qLook.x = cam.lx - cam.x
        qLook.y = cam.ly - cam.y
        qLook.z = cam.lz - cam.z

        LOCAL qRot AS Tquaternion
        qRot.w = COS(a * rad/2)
        qRot.x = (cam.ux - cam.x) * SIN(a * rad/2)
        qRot.y = (cam.uy - cam.y) * SIN(a * rad/2)
        qRot.z = (cam.uz - cam.z) * SIN(a * rad/2)

        LOCAL W AS Tquaternion,W1 AS Tquaternion,W2 AS Tquaternion
        W1 = quaternion_mult(qRot,qRight)
        W2 = quaternion_conj(qRot)
        W = quaternion_mult(W2,W1)
        W = quaternion_normalize(W)
            cam.rx = W.x + cam.x
            cam.ry = W.y + cam.y
            cam.rz = W.z + cam.z
        W1 = quaternion_mult(qRot,qLook)
        W2 = quaternion_conj(qRot)
        W = quaternion_mult(W2,W1)
        W = quaternion_normalize(W)
            cam.lx = W.x + cam.x
            cam.ly = W.y + cam.y
            cam.lz = W.z + cam.z
ENDFUNCTION


FUNCTION X_6DOFCAMERA: cam AS Tcamera
X_MAKE3D cam.nearClip, cam.farClip, cam.FOV
X_CAMERAUP cam.x - cam.ux, cam.y - cam.uy, cam.z - cam.uz
X_CAMERA cam.x, cam.y, cam.z, cam.lx, cam.ly, cam.lz
ENDFUNCTION


Here is an example of how to use the camera lib (see remarks for controls)...

Code: (glbasic) [Select]

// --------------------------------- //
// Project: 6DOFCam_test
// Start: Thursday, November 19, 2009
// IDE Version: 7.177

LOCAL cam1 AS Tcamera

LOCAL speed# = .01
LOCAL xv#, yv#, zv#, p#=0, r#=0, y#=0
LOCAL mx%, my%, mw%, mba%, mbb%


cam1.x# = 0
cam1.y = 0
cam1.z = 0

cam1.lx = 0
cam1.ly = 0
cam1.lz = -1

cam1.ux# = 0
cam1.uy# = 1
cam1.uz# = 0

cam1.rx = 1
cam1.ry = 0
cam1.rz = 0

cam1.FOV = 45
cam1.aspect = 1024/768
cam1.nearClip = .01
cam1.farClip = 5000

WHILE TRUE


MOUSESTATE mx, my, mba, mbb
mx = MOUSEAXIS(0)/10
my = -MOUSEAXIS(1)/10

X_6DOFCAMERA(cam1)

    //Linear Velocity Dampening (SPACE)
    IF KEY(57)
        xv = xv * .95
        yv = yv * .95
        zv = zv * .95
    ENDIF

    //Angular Velocity Dampening (CTRL)
    IF KEY(29)
        r = r * .95
        p = p * .95
        y = y * .95
    ENDIF

    //Accelerate Forward (W)
    IF KEY(17)
        xv = xv + (cam1.lx - cam1.x) * speed
        yv = yv + (cam1.ly - cam1.y) * speed
        zv = zv + (cam1.lz - cam1.z) * speed
    ENDIF
    //Accelerate Backward (S)
    IF KEY(31)
        xv = xv - (cam1.lx - cam1.x) * speed
        yv = yv - (cam1.ly - cam1.y) * speed
        zv = zv - (cam1.lz - cam1.z) * speed
    ENDIF
    //Accelerate Left (A)
    IF KEY(30)
        xv = xv + (cam1.rx - cam1.x) * speed
        yv = yv + (cam1.ry - cam1.y) * speed
        zv = zv + (cam1.rz - cam1.z) * speed
    ENDIF
    //Accelerate Right (D)
    IF KEY(32)
        xv = xv - (cam1.rx - cam1.x) * speed
        yv = yv - (cam1.ry - cam1.y) * speed
        zv = zv - (cam1.rz - cam1.z) * speed
    ENDIF
    //Accelerate Down (F)
    IF KEY(33)
        xv = xv + (cam1.ux - cam1.x) * speed
        yv = yv + (cam1.uy - cam1.y) * speed
        zv = zv + (cam1.uz - cam1.z) * speed
    ENDIF
    //Accelerate Up (R)
    IF KEY(19)
        xv = xv - (cam1.ux - cam1.x) * speed
        yv = yv - (cam1.uy - cam1.y) * speed
        zv = zv - (cam1.uz - cam1.z) * speed
    ENDIF

    // change IF the left mouse button is pressed
    IF mba = 1
        //Pitch mouse up AND
        p = p + my
        //Yaw
        y = y + mx
    ELSEIF mbb = 1
        //Roll with right mouse button
        r = r + mx
    ENDIF

    cam1.x = cam1.x + xv
    cam1.y = cam1.y + yv
    cam1.z = cam1.z + zv
    cam1.lx = cam1.lx + xv
    cam1.ly = cam1.ly + yv
    cam1.lz = cam1.lz + zv
    cam1.rx = cam1.rx + xv
    cam1.ry = cam1.ry + yv
    cam1.rz = cam1.rz + zv
    cam1.ux = cam1.ux + xv
    cam1.uy = cam1.uy + yv
    cam1.uz = cam1.uz + zv

    debug_view(0,0,0,100,RGB(255,255,0),RGB(255,0,255),RGB(0,255,255))

  camera_roll(cam1,r)
  camera_pitch(cam1,p)
  camera_yaw(cam1,y)

X_MAKE2D
PRINT "Cam Pitch = "+p,10,10
PRINT "Cam Yaw = "+y,10,20
PRINT "Cam Roll = "+r,10,30
SHOWSCREEN
WEND


FUNCTION debug_view: x, y, z, crad, rgb1, rgb2, rgb3
LOCAL rad, x1, y1, j, x2, y2
y1=SIN(0)*crad
x1=COS(0)*crad
FOR j=4 TO 360 STEP 4
    y2=SIN(j)*crad
    x2=COS(j)*crad
    X_LINE x+x1,y+y1,z  , x+x2,y+y2,z,0.1,rgb1
    X_LINE x+x1,y+0,y1+z, x+x2,y+0,y2+z,1,rgb2
    X_LINE x+0,y+x1,y1+z, x+0,y+x2,y2+z,1,rgb3
    x1=x2
    y1=y2
NEXT
X_DOT x,y,z,10,rgb1
X_DRAWAXES x+crad,y,z
    X_DRAWAXES x-crad,y,z
    X_DRAWAXES x,y+crad,z
    X_DRAWAXES x,y-crad,z
    X_DRAWAXES x,y,crad+z
    X_DRAWAXES x,y,-crad+z
    X_PRINT "RIGHT X+",x+crad,y,z,0
    X_PRINT "LEFT X-",x-crad,y,z,0
    X_PRINT "UP Y+",x,y+crad,z,0
    X_PRINT "DOWN Y-",x,y-crad,z,0
    X_PRINT "OUT Z+",x,y,crad+z,0
    X_PRINT "IN Z-",x,y,-crad+z,0
    X_SETTEXTURE -1, -1
ENDFUNCTION


[Schranz0r]

 
 
STICKY ! 

[Hemlos]

Very Awesome work my friend!

[Hemlos]

I found a glitch...i hope you can figgure out what is wrong..

Try this...
1. pitch up 45 degrees, left mouse.
2. move mouse left/right, left mouse.
As the camera turns on the mouse Y axes, you can see the camera is rocking left and right, as if a roll is being added through a sin wave, similiar to a gimbal lock.

Why is this extra rolling occuring?

[bigsofty]

I don't think that its 'rolling' as such, its just that its almost impossible to stop at an exact pitch, so when you rotate the camera, you see above the horizon on one side of the sphere but when you turn 180 degrees, you see just below the horizon. Pitch and Yaw combined in the demo with the mouse is not a good combination when trying for an exact axis. I'll change this to use the middle button tomorrow.

[codegit]

 

[Hemlos]

Love it man...love it. 

Curious, how do we attach an object to the camera rotated orientations?
ie a cockpit for a plane.

Is there a way to convert the quats to glbasic rotations?
Correct me if im wrong here, but i think glbasic is eulers.
A shot in the dark, i think we need some kind of conversion i imagine.

edit:

Really i appreciate you sharing quaternion math with us.
I think i have the hang of it now.

 

[bigsofty]

No problem buddy my pleasure.

Hurt my back yesterday lifting a new washing machine... real silly... so I'm not on my computer much today

There is still room to make this a more fully fledged quaternion library. No SLERP implementation yet, matrix<->quat routines etc... but its a start. I'm kinda busy with a 2D game for now though.

 

[Hemlos]

AHHHHH
Quaternions have an issue, the math fails.
It is a well known fact, as ive researched this math issue on the web.
So many people have issues with it.

I figgured out why it fails, on my own, through testing(damn im good):

It fails because the magnitude of the identity matrix changing....this is why speeds change, and ultimately crash a program.
The internal direction vectors, forward,right,up (quaternion object identity matrix) are always being altered, due to the imperfection of computers number limits!
You see, numbers can only be so precise(right hand precision), and this is where the problem starts.

And i have been working on a real solution..........and if i get this right, im going to publish a book on 3d quaternion math LOL


So the solution would seem to be to limit the "smallness" of the numbers.
And so, the numbers need to be rounded, which i have done.
Solution, maybe, well this is true, but only partially.
Because of the nature of quaternion math, the math must be precise, or we get an explosion in the identity magnitude.

So, rounding the small numbers will change the magnitude of the identity matrix, to spread out still!

WOW, this is why the math will fail........ again.

Ive seen a "solution" on the web, they said to normalize the quat every now and then.
But if you designate the object to have forward right and up magnitudes spread out, it increases the magnitude of motion, which is cool because you can regulate speed.

If you do this, then normalizing is not the solution, because it crunches your internal vectors, messing up your original identity.

I have been studying the math....and testing.
And the real solution is going to be pretty complex....
What needs to be done, is the quaternion math functions internally, needs to be given a tolerance, of changes in magnitude of the quaternions.
The functions must be able to regulate the magnitude spread, keeping it very close to the original identity.
This will be complicated...im not even sure if i can do this type of algorithm.

I will be back this week to let you know if i was successful in creating this "regulator".

[Hemlos]

Im sSO sorry buddy.......i didnt mean to delete your post.
I will place your response at the bottom of this message........

I never noticed any cumulative errors but a quaternion normalization function already exists, try this(another alternative is just to normalise the camera vectors in TCamera)...

The culmative error occur when you scale the identity up.
ie. cam.l, cam.u, cam.r, each are 1.0, but this naturally changes as time goes on because of rounding.
Thats bad because, this is your speed scalar.
for instance, if you set these 3 values to 2.0, then you will move twice as fast.
If you move around in 3d at a scale of 2.0, as a max/min speed scalar, then it should never go above or under that, and normalizing doesnt solve it.

Code: [Select]
        W = quaternion_mult(W2,W1)
W = quaternion_normalize(W)
If this solves your problem, I'll add it to the above lib.

The error causes unproportional scalars as a result, this is the problem with quaternions.
No matter what scalar values you set in your identity, not even 1.0, you can not regulate the original scalar by normalizing.
Here is why, if you have the real number quaternion vector with these errored values,
 (Where, all 3 identity original speed scalars were all equal to one):
.l=1.0
.u=1.0
.r=4.0 //this is the culmulating scalar in this case, for whatever reason, not important here.

If those were your output values, and you try to normalize all 3 at once, you end up with something like this:
.l=0.25
.u=0.25
.r=0.888

Obviously this is an error, these 3 scalars MUST all go back to 1.0 speed scale, or something close to it.

You can not normalize all 3 scalars back to the original identity scalar if they are not proportional, can you?

This is what people all over the world dont understand, is the solution is in regulating the the current vector scalar, to match the original identity scalar size.
Normalizing the current output, causes unproportioinal output....and this causes unproportional vector velocity speed outputs.

Im no mathematician, however, i do now have a solid understanding of quaternion math because i spent a few hours analyzing the inputs versus the output values of every single value within the algorithms.





I deleted the last post from softy by accident, here is the original message:

I never noticed any cumulative errors but a quaternion normalization function already exists, try this(another alternative is just to normalise the camera vectors in TCamera)...

Code: [Select]
        W = quaternion_normalize(W)
each time the W (notice its a Capital) is calculated.

so...

Code: [Select]
        W = quaternion_mult(W2,W1)
becomes...

Code: [Select]
        W = quaternion_mult(W2,W1)
        W = quaternion_normalize(W)
If this solves your problem, I'll add it to the above lib.

[bigsofty]

Still a little unsure where your error is. 

Each of the Tcamera vectors is not proportionally normalized to each other one, they don't have to be, only to within their own .x .y .z components to avoid cumulative errors in each vector, normalizing the quaternion result is enough, as they are derived from that.

[Hemlos]

Each of the Tcamera vectors is not proportionally normalized to each other one, they don't have to be

Yes they do, because youll end up with different speed scalars for each dimension.

If you set your vectors to 1,1,1 in loadcam()
The magnitude of the movement vectorXYZ is 1.732050, SQR(3), SQR(1*1-1*1-1*1)
As time progresses, this number fluxuates just a little ...normally.
This is due to the vector differences in each new XYZ

So as each dimension becomes unproportional, the vector scale for each dimension is different, and thus resulting in unproportional movement.
It takes alot of movement and rotating to get these to become so unproportional that the camera will slow down in one dimension or another and increase speed in another.
This is when the crash occurs....when they become extremely unproportional.

These vectors get reused to perform new movements, and these are the scale in which they move in each dimension, separetly.

[bigsofty]

Please excuse my ignorance Hemlos but could you provide a small demo, showing the cumulative error? 

There are 3 directional vectors, all being normalized (with the additional code) and one positional vector, which one are you referring to?

Also, what is "loadcam()"?

I think a small demo is the way to go if you could stick one together?

[Hemlos]

The problem was happengin because of my rounding function.
Right hand precision of 6 was the issue.
Sorry for bothering you...well now i know what not to do 

[bigsofty]

No problem bud. I personally learn more when things go wrong and then doing further investigation, than when things go immediately right, which requires none!