[bmx] Loads of quaternion functions by Warpy [ 1+ years ago ]

Started by BlitzBot, June 29, 2017, 00:28:40

Previous topic - Next topic
Print
Go Down Pages1
User actions

BlitzBot

June 29, 2017, 00:28:40
Title : Loads of quaternion functions
Author : Warpy
Posted : 1+ years ago

Description : <a href="http://en.wikipedia.org/wiki/Quaternion" target="_blank">Quaternions</a> 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</a>, and everything else comes from Wikipedia or Wolfram MathWorld.

Terminology:

<a href="http://en.wikipedia.org/wiki/Euclidean_vector" target="_blank">Vector</a>: 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</a>: 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</a>: 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) Select
'*********** BASICS

'multiply two quaternions together
Function quatmult#[](q1#[],q2#[])
Local a# = q1[0], b# = q1[1], c# = q1[2], d# = q1[3]
Local w# = q2[0], x# = q2[1], y# = q2[2], z# = q2[3]

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 ]
End Function

'subtract one quaternion from another
Function quatsub#[](q1#[],q2#[])
Local q#[4]
For i=0 To 3
q[i]=q1[i]-q2[i]
Next
Return q
End Function

'normalise a quaternion - make it have size 1
Function normalise(p#[])
d#=Sqr(p[1]*p[1]+p[2]*p[2]+p[3]*p[3])
p[1]:/d
p[2]:/d
p[3]:/d
End Function

'get the conjugate of a quaternion
Function conj#[](q#[])
Local c#[4]
c[0]=q[0]
c[1]=-q[1]
c[2]=-q[2]
c[3]=-q[3]
Return c
End Function

'get the multiplicative inverse of a quaternion
Function inverse#[](q#[])
Local i#[4]
n#=q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]
i[0]=q[0]/n
i[1]=-q[1]/n
i[2]=-q[2]/n
i[3]=-q[3]/n
Return i
End Function

'calculate the dot product of two quaternions
Function dotprod#(q1#[],q2#[])
Return q1[1]*q2[1]+q1[2]*q2[2]+q1[3]*q2[3]
End Function

'calculate the angle between two quaternions
Function anglebetween#(q1#[],q2#[])
dp#=dotprod(q1,q2)
Return ACos(dp)
End Function

'get a quaternion perpendicular to two given quaternions
Function crossprod#[](q1#[],q2#[])
Local q#[4]
q[0]=0
q[1]=q1[2]*q2[3]-q1[3]*q2[2]
q[2]=q1[3]*q2[1]-q1[1]*q2[3]
q[3]=q1[1]*q2[2]-q1[2]*q2[1]
Return q
End Function

'calculate size of a quaternion
Function modulus#(q#[])
Return Sqr(q[1]*q[1]+q[2]*q[2]+q[3]*q[3])
End Function


'******************* ROTATION

'rotate a vector 'v' by a rotation quaternion 'r'
Function rotate#[](v#[],r#[])
Return quatmult(r,quatmult(v,conj(r)))
End Function

'get a quaternion representing a rotation of 'an' degrees around a vector 'p'
Function rotaround#[](p#[],an#,inplace=False)
an:/2
Local q#[]
If inplace q=p Else q=New Float[4]
q[0]=Cos(an)
q[1]=p[1]*Sin(an)
q[2]=p[2]*Sin(an)
q[3]=p[3]*Sin(an)
Return q
End Function

'******** COMPLICATED ALGORITHMS

'interpolate linearly between two quaternions. t=0 gives q1, t=1 gives q2.
Function slerp#[](q1#[],q2#[],t#)
Local q#[4]
dp#=dotprod(q1,q2)
an#=ACos(dp)
If Sin(an)=0
q[0]=q1[0]
q[1]=q1[1]
q[2]=q1[2]
q[3]=q1[3]
Return q
EndIf
s1#=Sin((1-t)*an)/Sin(an)
s2#=Sin(t*an)/Sin(an)
q[0]=s1*q1[0]+s2*q2[0]
q[1]=s1*q1[1]+s2*q2[1]
q[2]=s1*q1[2]+s2*q2[2]
q[3]=s1*q1[3]+s2*q2[3]
Return q
End Function

'pick a random point on the unit sphere
Function sphererandom#[]()
x1#=1
x2#=1
While x1*x1+x2*x2>1
x1=Rnd(-1,1)
x2=Rnd(-1,1)
Wend
t#=Sqr(1-x1*x1-x2*x2)
x#=2*x1*t
y#=2*x2*t
z#=1-2*(x1*x1+x2*x2)
Return [0.0,x,y,z]
End Function




'*********** HALFSPACES

'is the given point on a sphere inside the given halfspace?
Function inhalfspace(p#[],s#[],an#)
dp#=dotprod(p,s)
If dp>Cos(an) Return True
End Function

'pick a random point in the given halfspace
Function halfspacerandom#[](pos#[],an#)
s#=Sin(Rnd(90))
fan#=Sqr(s)*an
Local v#[]
v=rotaround(sphererandom(),fan,True)
v=rotate(pos,v)
normalise v
Return v
End Function

'is any part of the edge connecting p1 to p2 in the given halfspace?
Function edgeinhalfspace(p1#[],p2#[],p#[],an#)
g1#=dotprod(p,p1)
g2#=dotprod(p,p2)
an=Cos(an)
theta#=ACos(dotprod(p1,p2))
u#=Tan(theta/2)
a#=-u*u*(g1+an)
b#=g1*(u*u-1)+g2*(u*u+1)
c#=g1-an
If b*b<4*a*c Return False
s#=Sqr(b*b-4*a*c)
s1#=(-b+s)/(2*a)
s2#=(-b-s)/(2*a)
If (s1>0 And s1<1) Or (s2>0 And s1<1)
Return True
EndIf
EndFunction


'do the two given halfspaces intersect?
Function halfspacesintersect(p1#[],an1#,p2#[],an2#)
dp#=dotprod(p1,p2)
an#=ACos(dp)
Return an<an1+an2
End Function

'************* TRIANGLES

'is the given point on a sphere inside the given triangle?
Function intriangle(p#[],p1#[],p2#[],p3#[])
Local v#[4]
Local diff#[4]
v=crossprod(p1,p2)
diff[1]=p[1]-p1[1]
diff[2]=p[2]-p1[2]
diff[3]=p[3]-p1[3]
dp1#=dotprod(v,diff)

v=crossprod(p2,p3)
normalise(v)
diff[1]=p[1]-p2[1]
diff[2]=p[2]-p2[2]
diff[3]=p[3]-p2[3]
dp2#=dotprod(v,diff)

v=crossprod(p3,p1)
normalise(v)
diff[1]=p[1]-p3[1]
diff[2]=p[2]-p3[2]
diff[3]=p[3]-p3[3]
dp3#=dotprod(v,diff)

Return dotprod(p,p1)>0 And ((Sgn(dp1)=Sgn(dp2) And Sgn(dp2)=Sgn(dp3)) Or dp1*dp2*dp3=0)
End Function

'************* DRAWING

'width and height of display
Const gwidth,gheight

'project a 3d point onto the screen
Function projx#(pos#[])
Return projsize*pos[1]+gwidth/2
End Function

Function projy#(pos#[])
Return projsize*pos[2]+gheight/2
End Function

'is a point on the front of the sphere?
'(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)
Function inclip(pos#[])
Return pos[3]<0
End Function

'draw a line between two points using SLERP
Function slerpline(p1#[],p2#[],s#=1)
If Not (inclip(p1) Or inclip(p2)) Return
Local p#[],op#[]

an#=anglebetween(p1,p2)
s:/an

If s=0 Return

op=p1
ox#=projx(p1)
oy#=projy(p1)
t#=0
While t<1
p=slerp(p1,p2,t)
If inclip(p)
x#=projx(p)
y#=projy(p)

If inclip(op)
DrawLine ox,oy,x,y
EndIf

ox=x
oy=y
EndIf
op=p
t:+s
Wend

If inclip(p2) And inclip(op)
x=projx(p2)
y=projy(p2)
ox=projx(op)
oy=projy(op)
DrawLine ox,oy,x,y
EndIf
End Function

'draw a filled halfspace
'(not clever, doesn't clip round visible edge of sphere)
Function drawhalfspace(p#[],an#,bits=30)
If Not inclip(p) Return
Local v#[],ov#[]
v=[p[0],p[2],p[3],p[1]]
v=crossprod(v,p)
normalise(v)
v=rotate(p,rotaround(v,an))
Local rr#[]
anstep#=360.0/bits
rr=rotaround(p,anstep)
px#=projx(p)
py#=projy(p)
Local poly#[]
ov=v
For c=0 To bits
poly=[px,py,projx(v),projy(v),projx(ov),projy(ov)]
DrawPoly poly
ov=v
v=rotate(v,rr)
Next
End Function


'************ TRIANGULATION OF THE SURFACE OF A SPHERE

'the surface of the sphere is divided into triangles, beginning with a regular icosahedron.

'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.


Global trixels:TList=New TList
Type trixel
Field p1#[],p2#[],p3#[] 'corners of triangle
Field centre#[4] 'centre of triangle

Field children:trixel[],parent:trixel 'this trixel's children, and a pointer to the trixel which subdivided into this one
Field contents:TList,numcontents 'list of things in this trixel, and number of things in this trixel, for convenience
Field name$ 'name,

Function Initialise() 'call this exactly once, to initialise the grid
d#=Sqr((10+2*Sqr(5))/4)
a#=1/d
b#=(1+Sqr(5))/(2*d)
n=0
Local ico#[][]
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]]
Local tris[]
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]
For i=0 To 59 Step 3
trixels.addlast trixel.Create((i/3)+"T",ico[tris[i]],ico[tris[i+1]],ico[tris[i+2]])
Next
End Function

Method New()
contents=New TList
End Method

Function Create:trixel(name$,p1#[],p2#[],p3#[],parent:trixel=Null)
t:trixel=New trixel
t.name=name
t.p1=p1
t.p2=p2
t.p3=p3
t.centre[1]=(p1[1]+p2[1]+p3[1])/3
t.centre[2]=(p1[2]+p2[2]+p3[2])/3
t.centre[3]=(p1[3]+p2[3]+p3[3])/3
normalise(t.centre)
t.parent=parent
Return t
End Function

Method contains(p#[]) 'is given point in this trixel?
Return intriangle(p,p1,p2,p3)
End Method

Function findcontainer:trixel(p#[]) 'find a trixel containing given point, anywhere on sphere
For t:trixel=EachIn trixels
If t.contains(p) Return t.container(p)
Next
End Function

Method container:trixel(p#[]) 'find child trixel containing given point, given that it lies inside this parent trixel
If Not contains(p) Return Null
If children
For i=0 To 3
t:trixel=children[i].container(p)
If t Return t
Next
Else
Return Self
EndIf
End Method

Method insert(th:thing) 'add a thing to this trixel's contents - might cause subdivision
t:trixel=Self
While t
t.numcontents:+1
t=t.parent
Wend
contents.addlast th
th.t=Self
If contents.count()>10
subdivide()
t:trixel=Self
n=numcontents
While t
t.numcontents:-n
t=t.parent
Wend
nc:TList=New TList
For th:thing=EachIn contents
t2:trixel=container(th.pos)
If t2
t2.insert th
Else
nc.addlast th
EndIf
Next
contents=nc
EndIf
End Method

Method remove(th:thing) 'remove a thing from this trixel - might cause a merge
If Not contents.contains(th) Return
numcontents:-1
contents.remove th
t:trixel=parent
While t
t.numcontents:-1
t=t.parent
Wend
If parent And parent.numcontents<=10
parent.merge
EndIf
End Method


Method subdivide() 'divide this trixel into four smaller trixels
children=New trixel[4]
Local p4#[4],p5#[4],p6#[4]
p4[1]=(p1[1]+p2[1])/2
p4[2]=(p1[2]+p2[2])/2
p4[3]=(p1[3]+p2[3])/2
p5[1]=(p3[1]+p2[1])/2
p5[2]=(p3[2]+p2[2])/2
p5[3]=(p3[3]+p2[3])/2
p6[1]=(p1[1]+p3[1])/2
p6[2]=(p1[2]+p3[2])/2
p6[3]=(p1[3]+p3[3])/2
normalise(p4)
normalise(p5)
normalise(p6)

children[0]=trixel.Create(name+"0",p1,p4,p6,Self)
children[1]=trixel.Create(name+"1",p4,p2,p5,Self)
children[2]=trixel.Create(name+"2",p5,p3,p6,Self)
children[3]=trixel.Create(name+"3",p4,p5,p6,Self)
End Method

Method merge() 'merge this subdivided trixel back together again
'contents=New TList
For i=0 To 3
For th:thing=EachIn children[i].contents
contents.addlast th
th.t=Self
Next
Next
children=Null
End Method

Method intersectshalfspace(p#[],an#) 'does this trixel intersect given halfspace?
'find if any corners inside halfspace
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

'check if bounding circle intersects halfspace
Local v1#[4],v2#[4],v#[]
v1=quatsub(p2,p1)
v2=quatsub(p3,p1)
v=crossprod(v1,v2)
normalise v
db#=ACos(dotprod(p1,v))
dp#=dotprod(p,v)
anb#=ACos(dp)

If anb>90 anb=180-anb
If anb>an+db Return False 'bounding circle doesn't intersect means no

If edgeinhalfspace(p1,p2,p,an) Or edgeinhalfspace(p2,p3,p,an) Or edgeinhalfspace(p1,p3,p,an) Return True

If contains(p) Return True 'if centre of halfspace is inside triangle then yes
End Method

Function findinhalfspace:trixel[](p#[],an#) 'find all trixels intersecting given halfspace
Local ins:trixel[0]
For t:trixel=EachIn trixels
ins:+t.kidsinhalfspace(p,an)
Next
Return ins
End Function

Function thingsinhalfspace:TList(p#[],an#) 'find all things in given halfspace
Local ins:trixel[]
ins=trixel.findinhalfspace(p,an)
ts:TList=New TList
For t:trixel=EachIn ins
For th:thing=EachIn t.contents
If inhalfspace(th.pos,p,an)
ts.addlast th
EndIf
Next
Next
Return ts
End Function

Method kidsinhalfspace:trixel[](p#[],an#) 'find child trixels intersecting given halfspace
If Not intersectshalfspace(p,an) Return
If children
Local ins:trixel[]
For i=0 To 3
ins:+children[i].kidsinhalfspace(p,an)
Next
Return ins
Else
Return [Self]
EndIf
End Method
End Type

'things which can be placed in a trixel should extend this type
Global things:TList=New TList
Type thing
Field pos#[]
Field t:trixel

Method New()
things.addlast Self
End Method

Method place()
t=trixel.findcontainer(pos)
t.insert Self
End Method


Method die()
things.remove Self
If t
t.remove Self
EndIf
End Method
End Type

Comments :


Ian Thompson(Posted 1+ years ago)

 I like! :)


slenkar(Posted 1+ years ago)

 very good, thanks
Print
Go Up Pages1
User actions