Codesnippets > 3D-snippets
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) ---// --------------------------------- //
// 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
--- End code ---
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
--- End code ---
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