Codesnippets > 3D-snippets

Quaternion Based 6DOF Camera Lib

(1/5) > >>

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 ---// --------------------------------- //
// 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 ---// --------------------------------- //
// 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:
 :nw:
 
STICKY !  :whip:

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.

Navigation

[0] Message Index

[#] Next page

Go to full version