December 04, 2020, 11:37:53 AM

Author Topic: [bmx] Loads of quaternion functions by Warpy [ 1+ years ago ]  (Read 699 times)

Offline BlitzBot

  • Jr. Member
  • **
  • Posts: 1
Title : Loads of quaternion functions
Author : Warpy
Posted : 1+ years ago

Description : <a href="http://en.wikipedia.org/wiki/Quaternion" target="_blank">Quaternions[/url] are a way of representing 3d rotations that don't suffer from gimbal lock.

This post contains code to do loads of things with quaternions: rotations, SLERP interpolation, operations on spheres, and a quadtree for storing things on a sphere efficiently.

The quadtree thing is based on <a href="http://arxiv.org/abs/cs.DB/0701164" target="_blank">this paper[/url], and everything else comes from Wikipedia or Wolfram MathWorld.

Terminology:

<a href="http://en.wikipedia.org/wiki/Euclidean_vector" target="_blank">Vector[/url]: a list of numbers, representing a point in space or a direction and a distance.

<a href="http://en.wikipedia.org/wiki/Quaternion" target="_blank">Quaternion[/url]: a 4d vector, representing a rotation by a certain amount around an axis. The first component is the cosine of the angle of rotation, and the other three components represent the axis.

Halfspace: A circle on the surface of a sphere. It's called a halfspace because it divides the surface into two halves - the points inside the circle and the points outside. They're not necessarily the same size.

<a href="http://en.wikipedia.org/wiki/Slerp" target="_blank">SLERP[/url]: Spherical Linear Interpolation. A way of interpolating (getting halfway points) between two points on a sphere.
A note on usage: for rotations, you should make sure you normalise rotation vectors. Otherwise, rotating also changes the lengths of things.


Code :
Code: BlitzMax
  1. '*********** BASICS
  2.  
  3. 'multiply two quaternions together
  4. Function quatmult#[](q1#[],q2#[])
  5.         Local a# = q1[0], b# = q1[1], c# = q1[2], d# = q1[3]
  6.         Local w# = q2[0], x# = q2[1], y# = q2[2], z# = q2[3]
  7.        
  8.         Return [ a*w - b*x - c*y - d*z,         a*x + w*b + c*z - d*y,          a*y + w*c + d*x - b*z,          a*z + w*d + b*y - c*x ]
  9. End Function
  10.        
  11. 'subtract one quaternion from another
  12. Function quatsub#[](q1#[],q2#[])
  13.         Local q#[4]
  14.         For i=0 To 3
  15.                 q[i]=q1[i]-q2[i]
  16.         Next
  17.         Return q
  18. End Function
  19.  
  20. 'normalise a quaternion - make it have size 1
  21. Function normalise(p#[])
  22.         d#=Sqr(p[1]*p[1]+p[2]*p[2]+p[3]*p[3])
  23.         p[1]:/d
  24.         p[2]:/d
  25.         p[3]:/d
  26. End Function
  27.  
  28. 'get the conjugate of a quaternion
  29. Function conj#[](q#[])
  30.         Local c#[4]
  31.         c[0]=q[0]
  32.         c[1]=-q[1]
  33.         c[2]=-q[2]
  34.         c[3]=-q[3]
  35.         Return c
  36. End Function
  37.  
  38. 'get the multiplicative inverse of a quaternion
  39. Function inverse#[](q#[])
  40.         Local i#[4]
  41.         n#=q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]
  42.         i[0]=q[0]/n
  43.         i[1]=-q[1]/n
  44.         i[2]=-q[2]/n
  45.         i[3]=-q[3]/n
  46.         Return i
  47. End Function
  48.  
  49. 'calculate the dot product of two quaternions
  50. Function dotprod#(q1#[],q2#[])
  51.         Return q1[1]*q2[1]+q1[2]*q2[2]+q1[3]*q2[3]
  52. End Function
  53.  
  54. 'calculate the angle between two quaternions
  55. Function anglebetween#(q1#[],q2#[])
  56.         dp#=dotprod(q1,q2)
  57.         Return ACos(dp)
  58. End Function
  59.  
  60. 'get a quaternion perpendicular to two given quaternions
  61. Function crossprod#[](q1#[],q2#[])
  62.         Local q#[4]
  63.         q[0]=0
  64.         q[1]=q1[2]*q2[3]-q1[3]*q2[2]
  65.         q[2]=q1[3]*q2[1]-q1[1]*q2[3]
  66.         q[3]=q1[1]*q2[2]-q1[2]*q2[1]
  67.         Return q
  68. End Function
  69.  
  70. 'calculate size of a quaternion
  71. Function modulus#(q#[])
  72.         Return Sqr(q[1]*q[1]+q[2]*q[2]+q[3]*q[3])
  73. End Function
  74.  
  75.  
  76. '******************* ROTATION
  77.  
  78. 'rotate a vector 'v' by a rotation quaternion 'r'
  79. Function rotate#[](v#[],r#[])
  80.         Return quatmult(r,quatmult(v,conj(r)))
  81. End Function
  82.  
  83. 'get a quaternion representing a rotation of 'an' degrees around a vector 'p'
  84. Function rotaround#[](p#[],an#,inplace=False)
  85.         an:/2
  86.         Local q#[]
  87.         If inplace q=p Else q=New Float[4]
  88.         q[0]=Cos(an)
  89.         q[1]=p[1]*Sin(an)
  90.         q[2]=p[2]*Sin(an)
  91.         q[3]=p[3]*Sin(an)
  92.         Return q
  93. End Function
  94.  
  95. '******** COMPLICATED ALGORITHMS
  96.  
  97. 'interpolate linearly between two quaternions. t=0 gives q1, t=1 gives q2.
  98. Function slerp#[](q1#[],q2#[],t#)
  99.         Local q#[4]
  100.         dp#=dotprod(q1,q2)
  101.         an#=ACos(dp)
  102.         If Sin(an)=0
  103.                 q[0]=q1[0]
  104.                 q[1]=q1[1]
  105.                 q[2]=q1[2]
  106.                 q[3]=q1[3]
  107.                 Return q
  108.         EndIf
  109.         s1#=Sin((1-t)*an)/Sin(an)
  110.         s2#=Sin(t*an)/Sin(an)
  111.         q[0]=s1*q1[0]+s2*q2[0]
  112.         q[1]=s1*q1[1]+s2*q2[1]
  113.         q[2]=s1*q1[2]+s2*q2[2]
  114.         q[3]=s1*q1[3]+s2*q2[3]
  115.         Return q
  116. End Function
  117.  
  118. 'pick a random point on the unit sphere
  119. Function sphererandom#[]()
  120.         x1#=1
  121.         x2#=1
  122.         While x1*x1+x2*x2>1
  123.                 x1=Rnd(-1,1)
  124.                 x2=Rnd(-1,1)
  125.         Wend
  126.         t#=Sqr(1-x1*x1-x2*x2)
  127.         x#=2*x1*t
  128.         y#=2*x2*t
  129.         z#=1-2*(x1*x1+x2*x2)
  130.         Return [0.0,x,y,z]
  131. End Function
  132.  
  133.  
  134.  
  135.  
  136. '*********** HALFSPACES
  137.  
  138. 'is the given point on a sphere inside the given halfspace?
  139. Function inhalfspace(p#[],s#[],an#)
  140.         dp#=dotprod(p,s)
  141.         If dp>Cos(an) Return True
  142. End Function
  143.  
  144. 'pick a random point in the given halfspace
  145. Function halfspacerandom#[](pos#[],an#)
  146.         s#=Sin(Rnd(90))
  147.         fan#=Sqr(s)*an
  148.         Local v#[]
  149.         v=rotaround(sphererandom(),fan,True)
  150.         v=rotate(pos,v)
  151.         normalise v
  152.         Return v
  153. End Function
  154.  
  155. 'is any part of the edge connecting p1 to p2 in the given halfspace?
  156. Function edgeinhalfspace(p1#[],p2#[],p#[],an#)
  157.         g1#=dotprod(p,p1)
  158.         g2#=dotprod(p,p2)
  159.         an=Cos(an)
  160.         theta#=ACos(dotprod(p1,p2))
  161.         u#=Tan(theta/2)
  162.         a#=-u*u*(g1+an)
  163.         b#=g1*(u*u-1)+g2*(u*u+1)
  164.         c#=g1-an
  165.         If b*b<4*a*c Return False
  166.         s#=Sqr(b*b-4*a*c)
  167.         s1#=(-b+s)/(2*a)
  168.         s2#=(-b-s)/(2*a)
  169.         If (s1>0 And s1<1) Or (s2>0 And s1<1)
  170.                 Return True
  171.         EndIf
  172. EndFunction
  173.  
  174.  
  175. 'do the two given halfspaces intersect?
  176. Function halfspacesintersect(p1#[],an1#,p2#[],an2#)
  177.         dp#=dotprod(p1,p2)
  178.         an#=ACos(dp)
  179.         Return an<an1+an2
  180. End Function
  181.  
  182. '************* TRIANGLES
  183.  
  184. 'is the given point on a sphere inside the given triangle?
  185. Function intriangle(p#[],p1#[],p2#[],p3#[])
  186.         Local v#[4]
  187.         Local diff#[4]
  188.         v=crossprod(p1,p2)
  189.         diff[1]=p[1]-p1[1]
  190.         diff[2]=p[2]-p1[2]
  191.         diff[3]=p[3]-p1[3]
  192.         dp1#=dotprod(v,diff)
  193.        
  194.         v=crossprod(p2,p3)
  195.         normalise(v)
  196.         diff[1]=p[1]-p2[1]
  197.         diff[2]=p[2]-p2[2]
  198.         diff[3]=p[3]-p2[3]
  199.         dp2#=dotprod(v,diff)
  200.        
  201.         v=crossprod(p3,p1)
  202.         normalise(v)
  203.         diff[1]=p[1]-p3[1]
  204.         diff[2]=p[2]-p3[2]
  205.         diff[3]=p[3]-p3[3]
  206.         dp3#=dotprod(v,diff)
  207.        
  208.         Return dotprod(p,p1)>0 And ((Sgn(dp1)=Sgn(dp2) And Sgn(dp2)=Sgn(dp3)) Or dp1*dp2*dp3=0)
  209. End Function
  210.  
  211. '************* DRAWING
  212.  
  213. 'width and height of display
  214. Const gwidth,gheight
  215.  
  216. 'project a 3d point onto the screen
  217. Function projx#(pos#[])
  218.         Return projsize*pos[1]+gwidth/2
  219. End Function
  220.  
  221. Function projy#(pos#[])
  222.         Return projsize*pos[2]+gheight/2
  223. End Function
  224.  
  225. 'is a point on the front of the sphere?
  226. '(I wrote all this code to display points on a globe. This function tells if a point is on the half of the sphere that the user can see)
  227. Function inclip(pos#[])
  228.         Return pos[3]<0
  229. End Function
  230.  
  231. 'draw a line between two points using SLERP
  232. Function slerpline(p1#[],p2#[],s#=1)
  233.         If Not (inclip(p1) Or inclip(p2)) Return
  234.         Local p#[],op#[]
  235.        
  236.         an#=anglebetween(p1,p2)
  237.         s:/an
  238.        
  239.         If s=0 Return
  240.        
  241.         op=p1
  242.         ox#=projx(p1)
  243.         oy#=projy(p1)
  244.         t#=0
  245.         While t<1
  246.                 p=slerp(p1,p2,t)
  247.                 If inclip(p)
  248.                         x#=projx(p)
  249.                         y#=projy(p)
  250.                        
  251.                         If inclip(op)
  252.                                 DrawLine ox,oy,x,y
  253.                         EndIf
  254.                        
  255.                         ox=x
  256.                         oy=y
  257.                 EndIf
  258.                 op=p
  259.                 t:+s
  260.         Wend
  261.  
  262.         If inclip(p2) And inclip(op)
  263.                 x=projx(p2)
  264.                 y=projy(p2)
  265.                 ox=projx(op)
  266.                 oy=projy(op)
  267.                 DrawLine ox,oy,x,y
  268.         EndIf
  269. End Function
  270.  
  271. 'draw a filled halfspace
  272. '(not clever, doesn't clip round visible edge of sphere)
  273. Function drawhalfspace(p#[],an#,bits=30)
  274.         If Not inclip(p) Return
  275.         Local v#[],ov#[]
  276.         v=[p[0],p[2],p[3],p[1]]
  277.         v=crossprod(v,p)
  278.         normalise(v)
  279.         v=rotate(p,rotaround(v,an))
  280.         Local rr#[]
  281.         anstep#=360.0/bits
  282.         rr=rotaround(p,anstep)
  283.         px#=projx(p)
  284.         py#=projy(p)
  285.         Local poly#[]
  286.         ov=v
  287.         For c=0 To bits
  288.                 poly=[px,py,projx(v),projy(v),projx(ov),projy(ov)]
  289.                 DrawPoly poly
  290.                 ov=v
  291.                 v=rotate(v,rr)
  292.         Next
  293. End Function
  294.  
  295.  
  296. '************ TRIANGULATION OF THE SURFACE OF A SPHERE
  297.  
  298. 'the surface of the sphere is divided into triangles, beginning with a regular icosahedron.
  299.  
  300. 'each triangle, or trixel, can contain things. When a trixel has too many things in it, it subdivides into several smaller trixels, so each one has only a few things in.
  301.  
  302.  
  303. Global trixels:TList=New TList
  304. Type trixel
  305.         Field p1#[],p2#[],p3#[] 'corners of triangle
  306.         Field centre#[4]                        'centre of triangle
  307.        
  308.         Field children:trixel[],parent:trixel   'this trixel's children, and a pointer to the trixel which subdivided into this one
  309.         Field contents:TList,numcontents                'list of things in this trixel, and number of things in this trixel, for convenience
  310.         Field name$                                                     'name,
  311.        
  312.         Function Initialise()   'call this exactly once, to initialise the grid
  313.                 d#=Sqr((10+2*Sqr(5))/4)
  314.                 a#=1/d
  315.                 b#=(1+Sqr(5))/(2*d)
  316.                 n=0
  317.                 Local ico#[][]
  318.                 ico=[ [0.0,0.0,a,b],[0.0,0.0,-a,b],[0.0,0.0,a,-b],[0.0,0.0,-a,-b],[0.0,a,b,0.0],[0.0,-a,b,0.0],[0.0,a,-b,0.0],[0.0,-a,-b,0.0],[0.0,b,0.0,a],[0.0,-b,0.0,a],[0.0,b,0.0,-a],[0.0,-b,0.0,-a]]
  319.                 Local tris[]
  320.                 tris=[0,1,8,0,4,8,4,8,10,6,8,10,1,6,8,1,6,7,3,6,7,3,7,11,2,3,11,2,3,10,2,4,10,2,4,5,0,4,5,0,5,9,0,1,9,1,7,9,7,9,11,5,9,11,2,5,11,3,6,10]
  321.                 For i=0 To 59 Step 3
  322.                         trixels.addlast trixel.Create((i/3)+"T",ico[tris[i]],ico[tris[i+1]],ico[tris[i+2]])
  323.                 Next
  324.         End Function
  325.        
  326.         Method New()
  327.                 contents=New TList
  328.         End Method
  329.        
  330.         Function Create:trixel(name$,p1#[],p2#[],p3#[],parent:trixel=Null)
  331.                 t:trixel=New trixel
  332.                 t.name=name
  333.                 t.p1=p1
  334.                 t.p2=p2
  335.                 t.p3=p3
  336.                 t.centre[1]=(p1[1]+p2[1]+p3[1])/3
  337.                 t.centre[2]=(p1[2]+p2[2]+p3[2])/3
  338.                 t.centre[3]=(p1[3]+p2[3]+p3[3])/3
  339.                 normalise(t.centre)
  340.                 t.parent=parent
  341.                 Return t
  342.         End Function
  343.        
  344.         Method contains(p#[])   'is given point in this trixel?
  345.                 Return intriangle(p,p1,p2,p3)
  346.         End Method
  347.        
  348.         Function findcontainer:trixel(p#[])                     'find a trixel containing given point, anywhere on sphere
  349.                 For t:trixel=EachIn trixels
  350.                         If t.contains(p) Return t.container(p)
  351.                 Next
  352.         End Function
  353.        
  354.         Method container:trixel(p#[])                           'find child trixel containing given point, given that it lies inside this parent trixel
  355.                 If Not contains(p) Return Null
  356.                 If children
  357.                         For i=0 To 3
  358.                                 t:trixel=children[i].container(p)
  359.                                 If t Return t
  360.                         Next
  361.                 Else
  362.                         Return Self
  363.                 EndIf
  364.         End Method
  365.        
  366.         Method insert(th:thing)                                 'add a thing to this trixel's contents - might cause subdivision
  367.                 t:trixel=Self
  368.                 While t
  369.                         t.numcontents:+1
  370.                         t=t.parent
  371.                 Wend
  372.                 contents.addlast th
  373.                 th.t=Self
  374.                 If contents.count()>10
  375.                         subdivide()
  376.                         t:trixel=Self
  377.                         n=numcontents
  378.                         While t
  379.                                 t.numcontents:-n
  380.                                 t=t.parent
  381.                         Wend
  382.                         nc:TList=New TList
  383.                         For th:thing=EachIn contents
  384.                                 t2:trixel=container(th.pos)
  385.                                 If t2
  386.                                         t2.insert th
  387.                                 Else
  388.                                         nc.addlast th
  389.                                 EndIf
  390.                         Next
  391.                         contents=nc
  392.                 EndIf
  393.         End Method
  394.        
  395.         Method remove(th:thing)                                 'remove a thing from this trixel - might cause a merge
  396.                 If Not contents.contains(th) Return
  397.                 numcontents:-1
  398.                 contents.remove th
  399.                 t:trixel=parent
  400.                 While t
  401.                         t.numcontents:-1
  402.                         t=t.parent
  403.                 Wend
  404.                 If parent And parent.numcontents<=10
  405.                         parent.merge
  406.                 EndIf
  407.         End Method
  408.        
  409.        
  410.         Method subdivide()                              'divide this trixel into four smaller trixels
  411.                 children=New trixel[4]
  412.                 Local p4#[4],p5#[4],p6#[4]
  413.                 p4[1]=(p1[1]+p2[1])/2
  414.                 p4[2]=(p1[2]+p2[2])/2
  415.                 p4[3]=(p1[3]+p2[3])/2
  416.                 p5[1]=(p3[1]+p2[1])/2
  417.                 p5[2]=(p3[2]+p2[2])/2
  418.                 p5[3]=(p3[3]+p2[3])/2
  419.                 p6[1]=(p1[1]+p3[1])/2
  420.                 p6[2]=(p1[2]+p3[2])/2
  421.                 p6[3]=(p1[3]+p3[3])/2
  422.                 normalise(p4)
  423.                 normalise(p5)
  424.                 normalise(p6)
  425.                
  426.                 children[0]=trixel.Create(name+"0",p1,p4,p6,Self)
  427.                 children[1]=trixel.Create(name+"1",p4,p2,p5,Self)
  428.                 children[2]=trixel.Create(name+"2",p5,p3,p6,Self)
  429.                 children[3]=trixel.Create(name+"3",p4,p5,p6,Self)
  430.         End Method
  431.        
  432.         Method merge()                                  'merge this subdivided trixel back together again
  433.                 'contents=New TList
  434.                 For i=0 To 3
  435.                         For th:thing=EachIn children[i].contents
  436.                                 contents.addlast th
  437.                                 th.t=Self
  438.                         Next
  439.                 Next
  440.                 children=Null
  441.         End Method
  442.                
  443.         Method intersectshalfspace(p#[],an#)    'does this trixel intersect given halfspace?
  444.                 'find if any corners inside halfspace
  445.                 If inhalfspace(p1,p,an) Or inhalfspace(p2,p,an) Or inhalfspace(p3,p,an) Return True     'all or some points in halfspace means yes
  446.                
  447.                 'check if bounding circle intersects halfspace
  448.                 Local v1#[4],v2#[4],v#[]
  449.                 v1=quatsub(p2,p1)
  450.                 v2=quatsub(p3,p1)
  451.                 v=crossprod(v1,v2)
  452.                 normalise v
  453.                 db#=ACos(dotprod(p1,v))
  454.                 dp#=dotprod(p,v)
  455.                 anb#=ACos(dp)
  456.                
  457.                 If anb>90 anb=180-anb
  458.                 If anb>an+db Return False       'bounding circle doesn't intersect means no
  459.                                
  460.                 If edgeinhalfspace(p1,p2,p,an) Or edgeinhalfspace(p2,p3,p,an) Or edgeinhalfspace(p1,p3,p,an) Return True
  461.                
  462.                 If contains(p) Return True      'if centre of halfspace is inside triangle then yes
  463.         End Method
  464.        
  465.         Function findinhalfspace:trixel[](p#[],an#)     'find all trixels intersecting given halfspace
  466.                 Local ins:trixel[0]
  467.                 For t:trixel=EachIn trixels
  468.                         ins:+t.kidsinhalfspace(p,an)
  469.                 Next
  470.                 Return ins
  471.         End Function
  472.        
  473.         Function thingsinhalfspace:TList(p#[],an#)      'find all things in given halfspace
  474.                 Local ins:trixel[]
  475.                 ins=trixel.findinhalfspace(p,an)
  476.                 ts:TList=New TList
  477.                 For t:trixel=EachIn ins
  478.                         For th:thing=EachIn t.contents
  479.                                 If inhalfspace(th.pos,p,an)
  480.                                         ts.addlast th
  481.                                 EndIf
  482.                         Next
  483.                 Next
  484.                 Return ts
  485.         End Function
  486.        
  487.         Method kidsinhalfspace:trixel[](p#[],an#)       'find child trixels intersecting given halfspace
  488.                 If Not intersectshalfspace(p,an) Return
  489.                 If children
  490.                         Local ins:trixel[]
  491.                         For i=0 To 3
  492.                                 ins:+children[i].kidsinhalfspace(p,an)
  493.                         Next
  494.                         Return ins
  495.                 Else
  496.                         Return [Self]
  497.                 EndIf
  498.         End Method
  499. End Type
  500.  
  501. 'things which can be placed in a trixel should extend this type
  502. Global things:TList=New TList
  503. Type thing
  504.         Field pos#[]
  505.         Field t:trixel
  506.        
  507.         Method New()
  508.                 things.addlast Self
  509.         End Method
  510.  
  511.         Method place()
  512.                 t=trixel.findcontainer(pos)
  513.                 t.insert Self
  514.         End Method
  515.        
  516.        
  517.         Method die()
  518.                 things.remove Self
  519.                 If t
  520.                         t.remove Self
  521.                 EndIf
  522.         End Method
  523. End Type


Comments :


Ian Thompson(Posted 1+ years ago)

 I like! :)


slenkar(Posted 1+ years ago)

 very good, thanks


 

SimplePortal 2.3.6 © 2008-2014, SimplePortal