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...
// --------------------------------- //
// 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)...
// --------------------------------- //
// 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