[bb] Quaternions by Wavey [ 1+ years ago ]

[BlitzBot]

Title : Quaternions
Author : Wavey
Posted : 1+ years ago

Description : Quaternions are useful for getting over some problems with Euler angles (i.e. pitch, yaw and roll). Briefly: you can interpolate between them easily, and they can help prevent gimbal lock.

Here's a little library of quaternion functions I have gathered from various sources and languages. Have a look at
<a href="http://www.dscho.co.uk/blitz/tutorials/quaternions.shtml" target="_blank">http://www.dscho.co.uk/blitz/tutorials/quaternions.shtml</a>
for information on how to use the functions here, example source code, and a description of what quaternions are useful for. (The original places I found the algorithms are also listed on the page)

Good luck!

Code :

Code (blitzbasic) Select Expand

; Quat.bb : v1.0 : 15/11/02

; A tutorial on how to use this file is at http://www.dscho.co.uk/blitz/tutorials/quaternions.shtml

; Types
Type Rotation
Field pitch#, yaw#, roll#
End Type

Type Quat
Field w#, x#, y#, z#
End Type

; Change these constants if you notice slips in accuracy
Const QuatToEulerAccuracy# = 0.001
Const QuatSlerpAccuracy#   = 0.0001

; convert a Rotation to a Quat
Function EulerToQuat(out.Quat, src.Rotation)
; NB roll is inverted due to change in handedness of coordinate systems
Local cr# = Cos(-srcoll/2)
Local cp# = Cos(srcpitch/2)
Local cy# = Cos(srcyaw/2)

Local sr# = Sin(-srcoll/2)
Local sp# = Sin(srcpitch/2)
Local sy# = Sin(srcyaw/2)

; These variables are only here to cut down on the number of multiplications
Local cpcy# = cp * cy
Local spsy# = sp * sy
Local spcy# = sp * cy
Local cpsy# = cp * sy

; Generate the output quat
outw = cr * cpcy + sr * spsy
outx = sr * cpcy - cr * spsy
outy = cr * spcy + sr * cpsy
outz = cr * cpsy - sr * spcy
End Function

; convert a Quat to a Rotation
Function QuatToEuler(out.Rotation, src.Quat)
Local sint#, cost#, sinv#, cosv#, sinf#, cosf#
Local cost_temp#

sint = (2 * srcw * srcy) - (2 * srcx * srcz)
cost_temp = 1.0 - (sint * sint)

If Abs(cost_temp) > QuatToEulerAccuracy
cost = Sqr(cost_temp)
Else
cost = 0
EndIf

If Abs(cost) > QuatToEulerAccuracy
sinv = ((2 * srcy * srcz) + (2 * srcw * srcx)) / cost
cosv = (1 - (2 * srcx * srcx) - (2 * srcy * srcy)) / cost
sinf = ((2 * srcx * srcy) + (2 * srcw * srcz)) / cost
cosf = (1 - (2 * srcy * srcy) - (2 * srcz * srcz)) / cost
Else
sinv = (2 * srcw * srcx) - (2 * srcy * srcz)
cosv = 1 - (2 * srcx * srcx) - (2 * srcz * srcz)
sinf = 0
cosf = 1
EndIf

; Generate the output rotation
outoll = -ATan2(sinv, cosv) ;  inverted due to change in handedness of coordinate system
outpitch = ATan2(sint, cost)
outyaw = ATan2(sinf, cosf)
End Function

; use this to interpolate between quaternions
Function QuatSlerp(res.Quat, start.Quat, fin.Quat, t#)
Local scaler_w#, scaler_x#, scaler_y#, scaler_z#
Local omega#, cosom#, sinom#, scale0#, scale1#

cosom = startx * finx + starty * finy + startz * finz + startw * finw

If cosom <= 0.0
cosom = -cosom
scaler_w = -finw
scaler_x = -finx
scaler_y = -finy
scaler_z = -finz
Else
scaler_w = finw
scaler_x = finx
scaler_y = finy
scaler_z = finz
EndIf

If (1 - cosom) > QuatSlerpAccuracy
omega = ACos(cosom)
sinom = Sin(omega)
scale0 = Sin((1 - t) * omega) / sinom
scale1 = Sin(t * omega) / sinom
Else
; Angle too small: use linear interpolation instead
scale0 = 1 - t
scale1 = t
EndIf

resx = scale0 * startx + scale1 * scaler_x
resy = scale0 * starty + scale1 * scaler_y
resz = scale0 * startz + scale1 * scaler_z
resw = scale0 * startw + scale1 * scaler_w
End Function

; result will be the same rotation as doing q1 then q2 (order matters!)
Function MultiplyQuat(result.Quat, q1.Quat, q2.Quat)
Local a#, b#, c#, d#, e#, f#, g#, h#

a = (q1w + q1x) * (q2w + q2x)
b = (q1z - q1y) * (q2y - q2z)
c = (q1w - q1x) * (q2y + q2z)
d = (q1y + q1z) * (q2w - q2x)
e = (q1x + q1z) * (q2x + q2y)
f = (q1x - q1z) * (q2x - q2y)
g = (q1w + q1y) * (q2w - q2z)
h = (q1w - q1y) * (q2w + q2z)

resultw = b + (-e - f + g + h) / 2
resultx = a - ( e + f + g + h) / 2
resulty = c + ( e - f + g - h) / 2
resultz = d + ( e - f - g + h) / 2
End Function

; convenience function to fill in a rotation structure
Function FillRotation(r.Rotation, pitch#, yaw#, roll#)
rpitch = pitch
ryaw = yaw
roll = roll
End Function

Comments :


Davros(Posted 1+ years ago)

 you can further optimize this by changing al the ' /2 ' to ' *0.5 '