Quaternion Rotation

[dcromley]

[ image changed, minor program improvements ] This comes from a long fascination with quaternions.  I wanted to see them work. 

I need to get into OpenGL/QB64.  I have gone through the great tutorials of Ashish ( https://ashishkingdom.github.io/OpenGL-Tutorials/intro/ ) but he hasn't gotten to rotations.  I was surprised that looking at the QB64 Wiki, that there are 7 _gl statements with "matrix" but none with "quaternion".  I assume that means that OpenGL does not use quaternions?

Lord Kelvin: "Although beautiful and of ingenious origin, they have been a curse on anyone who has come into contact with them in any way."
But I had a good time.

I hope the output is somewhat obvious: there are 5 points in the figure which are rotated using the quaternion.  I added the extraction of the Euler Angles at the bottom. 

The program is intended to work, not to be fast.  Still, I was surprised that the framerate is well above 100/sec.  A credit to QB64.

Code: (Select All)

_Title "Quaternion Rotation" ' dcromley
Option _Explicit
DefSng A-Z: DefLng I-N: DefStr S
Const TRUE = -1, FALSE = 0
Dim Shared mx, my, m1Clk, m1Rpt, m1Dn, m1End, m2Clk, m2Dn ' for MouseCk
Dim Shared Img1, Img2
Img1 = _NewImage(1024, 768, 256)
Img2 = _NewImage(1024, 768, 256)
_Dest Img2: Color 0, 15: Cls
_Dest Img1: Color 0, 15: Cls

' == MAIN start ==

Type type4f ' 4 floats for quaternions, points, triangles
  w As Single ' 0 for points; color for triangles
  x As Single ' pt1 for triangles
  y As Single ' pt2 for triangles
  z As Single ' pt3 for triangles
End Type

Const x0 = 384, y0 = 384, kxy = 200, z0 = 200 ' center, scale
Dim Shared As type4f T, aPts(5), aPts0(5), aTris(4), QMain, QSlew, va, vb
Dim Shared nPts, nTris ' # of Points, Triangles
Dim As type4f Qxp, Qxm, Qyp, Qym, Qzp, Qzm ' +- 1 deg Q's
Dim i, x, y, z, s, p1, p2, p3, icolor, nloop
Dim EuAngX, EuAngY, EuAngZ, fcos, fsin ' Euler angles
Dim az(4), ndx(4), time0, iSlew, xa, ya

' -- Points - x,y,z,/ (# ends)
Data 0,1,0,/,-1,-1,-1,/,-1,-1,1,/,1,-1,1,/,1,-1,-1,#
' -- Triangles - p1,p2,p3,color,/ (# ends)
Data 1,2,3,9,/,1,3,4,10,/,1,4,5,12,/,1,5,2,14,#

Do ' -- load aPoints
  nPts = nPts + 1 ' read point x,y,z
  Read aPts0(nPts).x, aPts0(nPts).y, aPts0(nPts).z, s ' s is / or # to end
Loop Until s = "#"
Do ' -- load aTriangles
  nTris = nTris + 1 ' read triangle p1,p2,p3,color
  Read aTris(nTris).x, aTris(nTris).y, aTris(nTris).z, aTris(nTris).w, s
Loop Until s = "#"
' --  load 1 deg quaternions
fcos = Cos(1 * _Pi / 360): fsin = Sin(1 * _Pi / 360) ' half angle
Qxp.w = fcos: Qxp.x = fsin: Qxm.w = fcos: Qxm.x = -fsin
Qyp.w = fcos: Qyp.y = fsin: Qym.w = fcos: Qym.y = -fsin
Qzp.w = fcos: Qzp.z = fsin: Qzm.w = fcos: Qzm.z = -fsin
QMain.w = 1 ' start with null rotation
QSlew = QMain

time0 = Timer - 1 ' prevent div by 0
Do ' ======== MAIN LOOP ========
  nloop = nloop + 1 ' nloop + 1 and print
  If nloop Mod 2 = 1 Then _Dest Img1: screen img1 _
  Else _Dest Img2: Screen Img2 ' swap screens
  Cls ' simplicity, not performance
  Line (768, 0)-(768, 752), _RGB(192, 192, 192) ' vertical
  MouseCk ' get mouse data
  ' -- check controls
  If iBox(110, 12, " Up") Then Qmult Qxm, QMain, QMain ' nudge orientation
  If iBox(106, 13, "Lft") Then Qmult Qym, QMain, QMain
  If iBox(114, 13, "Rht") Then Qmult Qyp, QMain, QMain
  If iBox(110, 14, " Dn") Then Qmult Qxp, QMain, QMain
  If iBox(106, 15, "CCW") Then Qmult Qzp, QMain, QMain
  If iBox(114, 15, " CW") Then Qmult Qzm, QMain, QMain
  ' -- check for mouse dragging (slewing)
  vb.x = mx - x0: vb.y = y0 - my: vb.z = z0 ' new mouse data
  If m1Dn And isIn(mx, 0, 767) And isIn(my, 0, 767) Then ' yes
    QVtoV va, vb, T ' need to smooth out the mouse data
    QSlew.x = QSlew.x * .9 + T.x * .1: QSlew.y = QSlew.y * .9 + T.y * .1: QSlew.z = QSlew.z * .9 + T.z * .1
    Qnorm QSlew ' this is what slews
  Else
    Const k = .99 ' make the slewing decay
    QSlew.x = QSlew.x * k: QSlew.y = QSlew.y * k: QSlew.z = QSlew.z * k
    QSlew.w = Sqr(1 - QSlew.x * QSlew.x - QSlew.y * QSlew.y - QSlew.z * QSlew.z)
  End If
  Qmult QSlew, QMain, QMain ' add slew to QMain
  va = vb ' new becomes old mouse data
  ' -- quaternion to Euler
  EuAngX = _Atan2(2 * QMain.x * QMain.w - 2 * QMain.y * QMain.z, 1 - 2 * QMain.x * QMain.x - 2 * QMain.z * QMain.z)
  EuAngY = _Atan2(2 * QMain.y * QMain.w - 2 * QMain.x * QMain.z, 1 - 2 * QMain.y * QMain.y - 2 * QMain.z * QMain.z)
  EuAngZ = _Asin(2 * QMain.x * QMain.y + 2 * QMain.z * QMain.w)
  ' -- rotate points
  For i = 1 To nPts
    aPts(i) = aPts0(i) ' reset to original
    T = QMain
    T.x = -T.x: T.y = -T.y: T.z = -T.z: ' << Q' >> conjugate
    Qmult aPts(i), T, T '                 << PQ' >>
    Qmult QMain, T, aPts(i) '             << QPQ' >>
  Next i
  For i = 1 To 4 ' get center Z's into a(4)
    T = aTris(i)
    az(i) = aPts(T.x).z + aPts(T.y).z + aPts(T.z).z ' p1.z+p2.z+p3.z
  Next i
  zSortIndexF az(), ndx() ' getting z-order
  For i = 1 To nTris ' this draws the triangles
    drawTri (ndx(i)) ' in z-order
  Next i
  ' -- print stuff
  Locate 2, 101: Print Using "nloops:#,###,###,###"; nloop
  Locate , 101: Print Using "fps:          ####.#"; nloop / (Timer - time0)
  Locate , 104: Print
  Locate , 104: Print "-- To rotate --"
  Locate , 104: Print "1) Click boxes"
  Locate , 104: Print "2) Press boxes"
  Locate , 104: Print "3) Drag mouse"
  Locate , 104: Print "ESC to end"
  Locate 19, 102: Print " -- Quaternion --"
  Locate , 99: Print Using " ##.#####"; QMain.w
  Locate , 99: Print Using " ##.#####"; QMain.x; QMain.y; QMain.z
  '  Locate , 99: Print Using " ##.#####"; QSlew.w
  '  Locate , 99: Print Using " ##.#####"; QSlew.x; QSlew.y; QSlew.z
  Locate , 100: Print ""
  Locate , 102: Print " -- Points --"
  For i = 1 To nPts
    Locate , 99: Print Using " ##.#####"; aPts(i).x; aPts(i).y; aPts(i).z
  Next i
  Locate , 100: Print ""
  Locate , 102: Print " -- Euler Angles --"
  Locate , 100: Print Using "EuAngX: ###"; (EuAngX * 180 / _Pi + 360) Mod 360
  Locate , 100: Print Using "EuAngY: ###"; (EuAngY * 180 / _Pi + 360) Mod 360
  Locate , 100: Print Using "EuAngZ: ###"; (EuAngZ * 180 / _Pi + 360) Mod 360
  _Display
Loop Until InKey$ = Chr$(27)
System

' == ROUTINES start ==

Function iBox (iCol, iRow, s3) ' simple control
  Dim ix, iy
  Locate iRow, iCol: Color 0, 14: Print s3;: Color 0, 15
  ix = iCol * 8 - 11
  iy = iRow * 16 - 1
  Line (ix, iy)-(ix + 3 * 8 + 4, iy - 16), , B ' rectangle
  If m1Rpt And isIn(mx, ix, ix + 28) And isIn(my, iy - 16, iy) Then iBox = TRUE
End Function

Sub Qmult (qa As type4f, qb As type4f, qab As type4f) ' Q multiplication
  Dim w, x, y, z
  w = qa.w * qb.w - qa.x * qb.x - qa.y * qb.y - qa.z * qb.z
  x = qa.w * qb.x + qa.x * qb.w + qa.y * qb.z - qa.z * qb.y
  y = qa.w * qb.y - qa.x * qb.z + qa.y * qb.w + qa.z * qb.x
  z = qa.w * qb.z + qa.x * qb.y - qa.y * qb.x + qa.z * qb.w
  qab.w = w: qab.x = x: qab.y = y: qab.z = z
End Sub

Sub QVtoV (v1 As type4f, v2 As type4f, Q As type4f) ' get Q from v1 to v2
  Dim v1dv2, v1xv2 As type4f ' dot, cross
  v1dv2 = VdotV(v1, v2) ' dot
  VcrossV v1, v2, Q ' cross
  Q.w = v1dv2 + Sqr(v1dv2 * v1dv2 + VdotV(Q, Q)) ' from the book
  Qnorm Q
End Sub

Function VdotV (v1 As type4f, v2 As type4f) ' dot product
  VdotV = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z
End Function

Sub VcrossV (v1 As type4f, v2 As type4f, v As type4f) ' cross product
  v.x = v1.y * v2.z - v1.z * v2.y
  v.y = v1.z * v2.x - v1.x * v2.z
  v.z = v1.x * v2.y - v1.y * v2.x
End Sub

Sub Qnorm (q As type4f) ' normalize
  Dim d
  d = Sqr(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z)
  q.w = q.w / d: q.x = q.x / d: q.y = q.y / d: q.z = q.z / d
End Sub

Sub drawTri (iTri) ' draw Triangle
  Dim ip1, ip2, ip3, icolor
  Dim ixc, iyc, x1, y1, x2, y2, x3, y3
  T = aTris(iTri) ' the triangle
  ip1 = T.x: ip2 = T.y: ip3 = T.z: icolor = T.w ' the points, color
  x1 = 386 + kxy * aPts(ip1).x: y1 = 386 - kxy * aPts(ip1).y
  x2 = 386 + kxy * aPts(ip2).x: y2 = 386 - kxy * aPts(ip2).y
  x3 = 386 + kxy * aPts(ip3).x: y3 = 386 - kxy * aPts(ip3).y
  Line (x1, y1)-(x2, y2), icolor
  Line (x2, y2)-(x3, y3), icolor
  Line (x3, y3)-(x1, y1), icolor
  ' don't paint if points are colinear
  If Abs(x1 * (y2 - y3) + x2 * (y3 - y1) + x3 * (y1 - y2)) < 1000 Then Exit Sub
  ixc = (x1 + x2 + x3) / 3: iyc = (y1 + y2 + y3) / 3 ' center
  Paint (ixc, iyc), icolor ' paint
End Sub

' -- LIBRARY ROUTINES --

' -- need Dim Shared mx,my,m1Clk,m1Rpt,m1Dn,m1End,m2Clk,m2Dn
Sub MouseCk () ' get mouse info
  Static m1Prev, m2Prev, m1Time ' for getting edges (Clk,End) and Repeating
  m1Clk = 0: m1Rpt = 0: m1End = 0: m2Clk = 0
  While _MouseInput: Wend ' bplus
  mx = _MouseX: my = _MouseY: m1Dn = _MouseButton(1): m2Dn = _MouseButton(2)
  If m1Dn Then ' Btn 1 down
    If Not m1Prev Then ' got a Clk (& Rpt), now look for repeats
      m1Clk = TRUE: m1Rpt = TRUE: m1Time = iMsecs + 250 ' delay 1/4 sec for repeats
    Else ' has been down, ck for repeat
      If iMsecs > m1Time Then m1Rpt = TRUE: m1Time = iMsecs + 50 ' repeat 20/sec
    End If
    m1Prev = TRUE
  Else ' Btn 1 up
    If m1Prev Then m1End = TRUE ' end of downtime (upedge)
    m1Prev = FALSE ' for next time
  End If
  If m2Dn Then ' Btn 2 down
    If Not m2Prev Then m2Clk = TRUE ' click (downedge)
    m2Prev = TRUE
  Else
    m2Prev = FALSE
  End If
End Sub

Function isIn (x, a, b) ' ck between
  If x >= a And x <= b Then isIn = TRUE
End Function

Sub zSortIndexF (a(), ndx()) ' make index to a()
  Dim i, j, t
  For i = 1 To UBound(a) ' add one at a time
    t = a(i) ' to be added
    For j = i To 2 Step -1 ' merge in
      If a(ndx(j - 1)) <= t Then Exit For
      ndx(j) = ndx(j - 1)
    Next j
    ndx(j) = i
  Next i
End Sub

Function iMsecs () ' milliseconds since midnight UTC
  iMsecs = Int(Timer(.001) * 1000 + .5)
End Function

___________________________________________________
"I don't understand all I know about quaternions."

[bplus]

Very nice! A _limit of say 60 in main loop will keep the CPU from getting hot and having fan run.

Your screenshot doesn't show all the great controls you have for moving it.

[dcromley]

@bplus, Thanks. You are sure good at commenting on the various doings here. I am impressed with much of it also, but I don't comment.

You got me to thinking -- and wondering if you were joking. Is heat really an issue? I have 4-cores and 8 logical processors (what is a logical processor?). As I start concurrent instances, the CPU% goes 22%, 44%, 70%, 87%, 99%. And the framerates go down.

So I notice heat from a laptop even when nothing is running. One wouldn't notice heat in a desktop. So ..?

[DSMan195276]

The Quaternion question is very interesting to me, but unfortunately I'm not sure I haven't really done any OpenGL stuff using QB64 before and most of what I've done used OpenGL shaders which work a bit differently.

That said, while it has been awhile I'm pretty sure in most (or maybe all) cases quaternions were just used in the program and weren't given to OpenGL, it would just receive the resulting model matrix after I had use the quaternions to produce it. So Ex. I would store the orientation of a model as a quaternion, but then when drawing the object I would convert the orientation into a rotation matrix and use the rotation matrix (along with translation, scaling, or other transformations) to produce the model matrix for OpenGL to use. So looking at it I think `glMultMatrix` is want you want, but you have to compute the matrix yourself.

> Is heat really an issue?

It's notable that most modern processors will start throttling themselves if they get too hot, so running the CPU too hot can actually reduce performance until it cools down.

[bplus]

I work from a laptop and know right away when the CPU is racing around a main loop doing nothing.
_Limit is your friend

Your mouse code is inefficient. Unless you are doing mouse wheel, just poll the Mouse like this:
While _MouseInput: Wend
mx = _MouseX: my = _MouseY: mb (or mb1) = _Mousebutton(1) ' usu. for left side clicking

[euklides]

Nice. You are ready for writing a flight simulator program !