Code archives/3D Graphics - Maths/Loads of quaternion functions
This code has been declared by its author to be Public Domain code.
Download source code
| |||||
Quaternions 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 this paper, and everything else comes from Wikipedia or Wolfram MathWorld. Terminology: Vector: a list of numbers, representing a point in space or a direction and a distance. Quaternion: 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. SLERP: 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. | |||||
'*********** 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
| ||
I like! :) |
| ||
very good, thanks |
Code Archives Forum