December 04, 2020, 11:37:53 AM

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

#### BlitzBot

• Jr. Member
• Posts: 1
##### [bmx] Loads of quaternion functions by Warpy [ 1+ years ago ]
« on: June 29, 2017, 12:28:40 AM »
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
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
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
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
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)
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()
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

Ian Thompson(Posted 1+ years ago)

I like!

slenkar(Posted 1+ years ago)

very good, thanks