[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[/url]
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

1. ; Quat.bb : v1.0 : 15/11/02
2.  
3. ; A tutorial on how to use this file is at http://www.dscho.co.uk/blitz/tutorials/quaternions.shtml
4.  
5. ; Types
6. Type Rotation
7.         Field pitch#, yaw#, roll#
8. End Type
9.  
10. Type Quat
11.         Field w#, x#, y#, z#
12. End Type
13.  
14. ; Change these constants if you notice slips in accuracy
15. Const QuatToEulerAccuracy# = 0.001
16. Const QuatSlerpAccuracy#   = 0.0001
17.  
18. ; convert a Rotation to a Quat
19. Function EulerToQuat(out.Quat, src.Rotation)
20.         ; NB roll is inverted due to change in handedness of coordinate systems
21.         Local cr# = Cos(-src
22. oll/2)
23.         Local cp# = Cos(srcpitch/2)
24.         Local cy# = Cos(srcyaw/2)
25.  
26.         Local sr# = Sin(-src
27. oll/2)
28.         Local sp# = Sin(srcpitch/2)
29.         Local sy# = Sin(srcyaw/2)
30.  
31.         ; These variables are only here to cut down on the number of multiplications
32.         Local cpcy# = cp * cy
33.         Local spsy# = sp * sy
34.         Local spcy# = sp * cy
35.         Local cpsy# = cp * sy
36.  
37.         ; Generate the output quat
38.         outw = cr * cpcy + sr * spsy
39.         outx = sr * cpcy - cr * spsy
40.         outy = cr * spcy + sr * cpsy
41.         outz = cr * cpsy - sr * spcy
42. End Function
43.  
44. ; convert a Quat to a Rotation
45. Function QuatToEuler(out.Rotation, src.Quat)
46.         Local sint#, cost#, sinv#, cosv#, sinf#, cosf#
47.         Local cost_temp#
48.  
49.         sint = (2 * srcw * srcy) - (2 * srcx * srcz)
50.         cost_temp = 1.0 - (sint * sint)
51.  
52.         If Abs(cost_temp) > QuatToEulerAccuracy
53.                 cost = Sqr(cost_temp)
54.         Else
55.                 cost = 0
56.         EndIf
57.  
58.         If Abs(cost) > QuatToEulerAccuracy
59.                 sinv = ((2 * srcy * srcz) + (2 * srcw * srcx)) / cost
60.                 cosv = (1 - (2 * srcx * srcx) - (2 * srcy * srcy)) / cost
61.                 sinf = ((2 * srcx * srcy) + (2 * srcw * srcz)) / cost
62.                 cosf = (1 - (2 * srcy * srcy) - (2 * srcz * srcz)) / cost
63.         Else
64.                 sinv = (2 * srcw * srcx) - (2 * srcy * srcz)
65.                 cosv = 1 - (2 * srcx * srcx) - (2 * srcz * srcz)
66.                 sinf = 0
67.                 cosf = 1
68.         EndIf
69.  
70.         ; Generate the output rotation
71.         out
72. oll = -ATan2(sinv, cosv) ;  inverted due to change in handedness of coordinate system
73.         outpitch = ATan2(sint, cost)
74.         outyaw = ATan2(sinf, cosf)
75. End Function
76.  
77. ; use this to interpolate between quaternions
78. Function QuatSlerp(res.Quat, start.Quat, fin.Quat, t#)
79.         Local scaler_w#, scaler_x#, scaler_y#, scaler_z#
80.         Local omega#, cosom#, sinom#, scale0#, scale1#
81.  
82.         cosom = startx * finx + starty * finy + startz * finz + startw * finw
83.  
84.         If cosom <= 0.0
85.                 cosom = -cosom
86.                 scaler_w = -finw
87.                 scaler_x = -finx
88.                 scaler_y = -finy
89.                 scaler_z = -finz
90.         Else
91.                 scaler_w = finw
92.                 scaler_x = finx
93.                 scaler_y = finy
94.                 scaler_z = finz
95.         EndIf
96.  
97.         If (1 - cosom) > QuatSlerpAccuracy
98.                 omega = ACos(cosom)
99.                 sinom = Sin(omega)
100.                 scale0 = Sin((1 - t) * omega) / sinom
101.                 scale1 = Sin(t * omega) / sinom
102.         Else
103.                 ; Angle too small: use linear interpolation instead
104.                 scale0 = 1 - t
105.                 scale1 = t
106.         EndIf
107.  
108.         resx = scale0 * startx + scale1 * scaler_x
109.         resy = scale0 * starty + scale1 * scaler_y
110.         resz = scale0 * startz + scale1 * scaler_z
111.         resw = scale0 * startw + scale1 * scaler_w
112. End Function
113.  
114. ; result will be the same rotation as doing q1 then q2 (order matters!)
115. Function MultiplyQuat(result.Quat, q1.Quat, q2.Quat)
116.         Local a#, b#, c#, d#, e#, f#, g#, h#
117.  
118.         a = (q1w + q1x) * (q2w + q2x)
119.         b = (q1z - q1y) * (q2y - q2z)
120.         c = (q1w - q1x) * (q2y + q2z)
121.         d = (q1y + q1z) * (q2w - q2x)
122.         e = (q1x + q1z) * (q2x + q2y)
123.         f = (q1x - q1z) * (q2x - q2y)
124.         g = (q1w + q1y) * (q2w - q2z)
125.         h = (q1w - q1y) * (q2w + q2z)
126.  
127.         resultw = b + (-e - f + g + h) / 2
128.         resultx = a - ( e + f + g + h) / 2
129.         resulty = c + ( e - f + g - h) / 2
130.         resultz = d + ( e - f - g + h) / 2
131. End Function
132.  
133. ; convenience function to fill in a rotation structure
134. Function FillRotation(r.Rotation, pitch#, yaw#, roll#)
135.         rpitch = pitch
136.         ryaw = yaw
137.         r
138. oll = roll
139. End Function

Comments :


Davros(Posted 1+ years ago)

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