Anyone still using the Quaternion Based 6DOF Camera Lib?

[FutureCow]

I had posted in that thread, but seeing the post is stickied and old, I thought people would overlook the post so moved it here.

I'm not sure what's going on but I seem to have big issues with this code and GLBasic 10.283. Hopefully someone will have some ideas! (I don't understand the maths too well so I don't know where to start sorry!!!!)

If I use camera_roll,pitch or yaw, every alternate call seems to give me a result 180 degrees out (based on how it shows on the screen) from where it should be. For example, assuming each call to "camera_roll" rotates 1 degree, my first run through the code would be 0 degrees roll, rollowed by 181 degrees, followed by 2 degrees, followed by 183 degrees, followed by 4 degrees etc.

My only change to the original code to show the problem easily is to modify the showscreen section of the main loop to say :

Code (glbasic) Select Expand


SHOWSCREEN
MOUSEWAIT
camera_roll(cam1,2)


but I can change that "2" value to anything I like and still appear to get the same results.

Here's the full combined code from the post about the library (http://www.glbasic.com/forum/index.php?topic=3851.0) with my little change to make it easy for someone to paste straight into a project. Run it, and click the mouse multiple times and you'll see the issue.

Code (glbasic) Select Expand

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

main()

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
DEBUG cam.x+","+cam.ux+","+cam.y+","+cam.uy+","+cam.z+","+cam.uz+"\n"
X_CAMERA cam.x, cam.y, cam.z, cam.lx, cam.ly, cam.lz
ENDFUNCTION



FUNCTION main:


// --------------------------------- //
// 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%
CONSTANT rad = 3.1415926535/180

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
SETPIXEL mx,my,RGB(255,255,255)
SHOWSCREEN
MOUSEWAIT
camera_roll(cam1,0.1)
WEND
ENDFUNCTION


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

I know the entity system is an alternative, but I found it a bit clunky and buggy when I used it in the past so at this point I'd rather stick with the camera library. Also, I might not understand the maths in the camera library, but at least I wouldn't have to rewrite my project if someone can identify how to fix it.

Any help is appreciated!

[FutureCow]

Okay, I think I've figured it out!!!  I have no idea what the maths are doing, but I had a crazy idea (based on a number minus a negative does an addition instead of a subtraction) and swapped some of the operations around. After a few attempts to work out which operations to swap, now pitch, roll and yaw seem to work as they should! 

For yaw, swap the two sides of the minus operation as below

Code (glbasic) Select Expand


//        qLook.x = cam.lx - cam.x
//        qLook.y = cam.ly - cam.y
//        qLook.z = cam.lz - cam.z

// Fixes yaw inversion problem
        qLook.x = cam.x - cam.lx
        qLook.y = cam.y - cam.ly
        qLook.z = cam.z - cam.lz

For roll swap as below

Code (glbasic) Select Expand

//        qUp.x = cam.ux - cam.x
//        qUp.y = cam.uy - cam.y
//        qUp.z = cam.uz - cam.z

// Fixes roll inversion problem
        qUp.x = cam.x - cam.ux
        qUp.y = cam.y - cam.uy
        qUp.z = cam.z - cam.uz


And for pitch you have to swap two sets of operations

Code (glbasic) Select Expand


        qUp.w = 0
//        qUp.x = cam.ux - cam.x
//        qUp.y = cam.uy - cam.y
//        qUp.z = cam.uz - cam.z

// Fixes pitch inversion problem
        qUp.x = cam.x - cam.ux
        qUp.y = cam.y - cam.uy
        qUp.z = cam.z - cam.uz

        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

// Fixes pitch inversion problem
        qLook.x = cam.x - cam.lx
        qLook.y = cam.y - cam.ly
        qLook.z = cam.z - cam.lz


I've got to test the other functions are all working as they should (at some point that's not this late at night), and that it works when you start at some point other than (0,0,0) but it's fixed based on the starting position in the demo code!

If anyone would like me to fix their code by trying vague theories without understanding what their code's doing, let me know.