November 25, 2020, 06:05:37 AM

Author Topic: [bb] Quaternions by Wavey [ 1+ years ago ]  (Read 843 times)

Offline BlitzBot

  • Jr. Member
  • **
  • Posts: 1
[bb] Quaternions by Wavey [ 1+ years ago ]
« on: June 29, 2017, 12:28:41 AM »
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 '


 

SimplePortal 2.3.6 © 2008-2014, SimplePortal