Graphics3D 800,600,16,2
SetBuffer BackBuffer()

Global cam=CreateCamera()
PositionEntity cam,0,0,-100

Global cube=CreateCube()
ScaleMesh cube,10,10,10
Global c1=CreateCube(cube)
Global c2=CreateCube(cube)
ScaleMesh c1,3,3,3
ScaleMesh c2,3,3,3
PositionEntity c1,-20,0,0
PositionEntity c2, 20,0,0

Local q.Quat = New Quat
Local qr1.Quat = New Quat
Local qr2.Quat = New Quat
Local qr3.Quat = New Quat

EulerToQuat q, 0,0,0

While Not KeyHit(1)

 If KeyDown(200) Then EulerToQuat qr1, 1,0,0 :MultiplyQuat(q,q,qr1)
 If KeyDown(208) Then EulerToQuat qr1,-1,0,0 :MultiplyQuat(q,q,qr1)
 If KeyDown(203) Then EulerToQuat qr2,0, 1,0 :MultiplyQuat(q,q,qr2)
 If KeyDown(205) Then EulerToQuat qr2,0,-1,0 :MultiplyQuat(q,q,qr2)

 If KeyDown(211) Then EulerToQuat qr3,0,0, 1 :MultiplyQuat(q,q,qr3)
 If KeyDown(209) Then EulerToQuat qr3,0,0,-1 :MultiplyQuat(q,q,qr3)

 QuatToEntity q,cube,0

 MoveEntity cube,0,0,1

 If KeyHit(57) Then
  PointEntity cam,cube,0
 EndIf

 RenderWorld

 Delay 10

 Flip
Wend
End

;----------------------------------------------------------------------------

Type Quat
   Field w#, x#, y#, z#
End Type

; Change these constants if you notice slips in accuracy
Const QuatToEulerAccuracy# = 0.001

;----------------------------------------------------------------------------

Function EulerToQuat(out.Quat, pitch#,yaw#,roll#)
  ; convert a Rotation to a Quat
   ; NB roll is inverted due to change in handedness of coordinate systems
   Local cr# = Cos(-roll/2)
   Local cp# = Cos(pitch/2)
   Local cy# = Cos(yaw/2)

   Local sr# = Sin(-roll/2)
   Local sp# = Sin(pitch/2)
   Local sy# = Sin(yaw/2)

   ; These variables are only here to cut down on the number of multiplications
   Local cpcy# = cp * cy
   Local spsy# = sp * sy
   Local spcy# = sp * cy
   Local cpsy# = cp * sy

   ; Generate the output quat
   out\w = cr * cpcy + sr * spsy
   out\x = sr * cpcy - cr * spsy
   out\y = cr * spcy + sr * cpsy
   out\z = cr * cpsy - sr * spcy
End Function

Function QuatToEntity(src.Quat,Entity,RotGlobal=0)

 ; convert a Quat to a Rotation

   Local sint#, cost#, sinv#, cosv#, sinf#, cosf#
   Local cost_temp#

   sint = (2 * src\w * src\y) - (2 * src\x * src\z)
   cost_temp = 1.0 - (sint * sint)

   If Abs(cost_temp) > QuatToEulerAccuracy
      cost = Sqr(cost_temp)
   Else
      cost = 0
   EndIf

   If Abs(cost) > QuatToEulerAccuracy
      sinv = ((2 * src\y * src\z) + (2 * src\w * src\x)) / cost
      cosv = (1 - (2 * src\x * src\x) - (2 * src\y * src\y)) / cost
      sinf = ((2 * src\x * src\y) + (2 * src\w * src\z)) / cost
      cosf = (1 - (2 * src\y * src\y) - (2 * src\z * src\z)) / cost
   Else
      sinv = (2 * src\w * src\x) - (2 * src\y * src\z)
      cosv = 1 - (2 * src\x * src\x) - (2 * src\z * src\z)
      sinf = 0
      cosf = 1
   EndIf

   ; Generate the output rotation
   RotateEntity Entity,ATan2(sint, cost),ATan2(sinf, cosf),-ATan2(sinv, cosv),RotGlobal ;  inverted due to change in handedness of coordinate system

End Function

Function MultiplyQuat(result.Quat, q1.Quat, q2.Quat)

 ; result will be the same rotation as doing q1 then q2 (order matters!)

   Local a#, b#, c#, d#, e#, f#, g#, h#

   a = (q1\w + q1\x) * (q2\w + q2\x)
   b = (q1\z - q1\y) * (q2\y - q2\z)
   c = (q1\w - q1\x) * (q2\y + q2\z)
   d = (q1\y + q1\z) * (q2\w - q2\x)
   e = (q1\x + q1\z) * (q2\x + q2\y)
   f = (q1\x - q1\z) * (q2\x - q2\y)
   g = (q1\w + q1\y) * (q2\w - q2\z)
   h = (q1\w - q1\y) * (q2\w + q2\z)

   result\w = b + (-e - f + g + h) / 2.0
   result\x = a - ( e + f + g + h) / 2.0
   result\y = c + ( e - f + g - h) / 2.0
   result\z = d + ( e - f - g + h) / 2.0

End Function