Spitfire II Release v1.4 -Using quaternions to orient a plane.

1aTabOrder

Spitfire II Tab Order Version: 1.4
------------------------------
This file should not be included in the Codea project.
#Main
#Andrew
#Quaternion
#Q
#Plane

Andrew.lua

--Andrew

--the RotateAndDraw function needs code
VERSION = 1.4
function setup()
    if AutoGist then
        autogist = AutoGist("Spitfire II","Using quaternions to orient a plane.",VERSION)
        autogist:backup(true)
    end
    --** Plane settings **
    plane=Plane() --plane mesh
    planeQ=vec4(1,0,0,0) --plane quaternion
    planeQEXZ = vec4(1,0,0,0) --initialise quaternion to (1,0,0,0)
    planeQEZX = vec4(1,0,0,0)
    planeQT = vec4(1,0,0,0)
    planePos=vec3(0,0,0) --starting position (plane doesn't change position)
    
    --** Joystick sttings **
    joyPos=vec2(5,5) --
    joy=vec2(0,0) --rotation vector derived from joystick position, both values between -1 and +1
    oldJoy = vec2(0,0)
    
    --**Flight controls **
    --for each of pitch and yaw, a true value below means that the plane moves directly in line 
    --with the joystick, with a maximum of 90 degrees, which I've called "slaved"
    --false means that existing rotation is incremented by the joystick movement
    --(this means the plane can rotate indefinitely, and loop the loop and do barrel rolls)
    --yaw is assumed to always be incremented, based on the current roll setting, so there is no parameter,
    --but I have coded for yaw to be allowed the option to be fixed, just in case

    parameter.boolean("Pitch_Slaved",true,Reset)
    parameter.boolean("Roll_Slaved",true,Reset)
    parameter.boolean("Yaw_Slaved",false,Reset)

    yaw = 1
    modes = {"Absolute", "Increment", "Delta", "Parameter"}
    parameter.boolean("Apply_Yaw",false,function() yaw = 1-yaw end)
    parameter.integer("mode",1,#modes,1,function(m) Mode = modes[m] end)
    parameter.watch("Mode")
    --Yaw_Slaved=false --hard code this for now

    --** rotation settings and limits **
    --x, y, z map to pitch, yaw, roll
    --limits for slave rotation
    limit=vec3(90,90,90)*math.pi/180
    --sensitivity of rotation for incremental rotation
    sensitivity=vec3(1,1,1)

    parameter.watch("joy")
    parameter.watch("deltaJoy")
    

    atRest = true
    print(Q:EulerToQuat(1,1,0))
    print(qEuler(0,1,1,{"Z","Y","X"})) -- YX,XZ
    print(qRotation(1,0,1,0)*qRotation(1,1,0,0))
end

function Reset() --reset quaternion if settings changed
    planeQEXZ = vec4(1,0,0,0) --initialise quaternion to (1,0,0,0)
    planeQEZX = vec4(1,0,0,0)
    planeQT = vec4(1,0,0,0)
end

function draw()
    background(179, 205, 220, 255)
    fill(255)
    perspective()
    camera(0,0,100,0,0,0)   
    RotateAndDraw()    
    DrawJoystick()
end

--this function adjusts the rotation based on joystick position, and draws the plane
function RotateAndDraw()
    --joy.x, joy.y hold joystick values ranging from -1 to +1
    --plane position is in the (unchanging) vec3 planePos
    
    --your task, should you choose to accept it, is to rotate and draw the plane
    
    --the pitch and roll rotations may either be slaved to absolute joystick values 
    --each has a parameter which the user can set individually  
    
    --if slaved, the rotation is in line with the joystick position
    --pitch = -joy.y * limit.x  (negative because pulling the stick back lifts the nose)
    --yaw is not slaved, so not applicable
    --roll = joy.x * limit.z
    
    --if not slaved, the rotation is increased from its previous value, by
    --pitch = -joy.y * sensitivity.x
    --yaw = joy.x * sensitivity.y
    --roll = joy.x * sensitivity.z
    deltaJoy = joy - oldJoy
    oldJoy = joy
    if inTouch then
        atRest = false
        local v
        if mode == 1 then
            v = vec3(-joy.y*limit.x,yaw*joy.x*limit.y,joy.x*limit.z)
            planeQEXZ = qEuler(v.y,v.x,v.z,{"Y","X","Z"})
            planeQEZX = qEuler(v.y,v.z,v.x,{"Y","Z","X"})
            planeQT = qTangent(v)
        elseif mode == 2 then
            v = vec3(-joy.y*sensitivity.x*DeltaTime,
                    yaw*joy.x*sensitivity.y*DeltaTime,
                    joy.x*sensitivity.z*DeltaTime)
            planeQEXZ = planeQEXZ * qEuler(v.y,v.x,v.z,{"Y","X","Z"})
            planeQEZX = planeQEZX * qEuler(v.y,v.z,v.x,{"Y","Z","X"})
            planeQT = planeQT * qTangent(v)
        elseif mode == 3 then
            v = vec3(-deltaJoy.y*limit.x,
                    yaw*deltaJoy.x*limit.y,
                    deltaJoy.x*limit.z)
            planeQEXZ = planeQEXZ * qEuler(v.y,v.x,v.z,{"Y","X","Z"})
            planeQEZX = planeQEZX * qEuler(v.y,v.z,v.x,{"Y","Z","X"})
            planeQT = planeQT * qTangent(v)
        elseif mode == 4 then
            v = vec3()
            if Pitch_Slaved then
                v.x = -deltaJoy.y*limit.x
            else
                v.x = -joy.y*sensitivity.x*DeltaTime
            end
            if Yaw_Slaved then
                v.y = yaw*deltaJoy.x*limit.y
            else
                v.y = yaw*joy.x*sensitivity.y*DeltaTime
            end
            if Roll_Slaved then
                v.z = deltaJoy.x*limit.z
            else
                v.z = joy.x*sensitivity.z*DeltaTime
            end
            planeQEXZ = planeQEXZ * qEuler(v.y,v.x,v.z,{"Y","X","Z"})
            planeQEZX = planeQEZX * qEuler(v.y,v.z,v.x,{"Y","Z","X"})
            planeQT = planeQT * qTangent(v)
        end
    else
        if not atRest then
            if not inSlerp then
                planeSlerpEXZ = planeQEXZ:make_slerp()
                planeSlerpEZX = planeQEZX:make_slerp()
                planeSlerpT = planeQT:make_slerp()
                inSlerp = true
                stime = ElapsedTime + 5
            end
            if stime - ElapsedTime > 0 then
                planeQEXZ = planeSlerpEXZ(
                            smootherstep((stime - ElapsedTime)/5))
                planeQEZX = planeSlerpEZX(
                            smootherstep((stime - ElapsedTime)/5))
                planeQT = planeSlerpT(
                            smootherstep((stime - ElapsedTime)/5))
            else
                planeQEXZ = vec4(1,0,0,0)
                planeQEZX = vec4(1,0,0,0)
                planeQT = vec4(1,0,0,0)
                inSlerp = false
                atRest = true
            end
        end
    end

    pushMatrix()
    translate(0,15,0)
    translate(planePos)
    rotate(planeQEXZ)
    plane:draw()
    popMatrix()
    
    pushMatrix()
    translate(planePos)
    rotate(planeQEZX)
    plane:draw()
    popMatrix()
    
    pushMatrix()
    translate(0,-15,0)
    translate(planePos)
    rotate(planeQT)
    plane:draw()
    popMatrix()
    
    ortho()
    viewMatrix(matrix())
    resetMatrix()
    fontSize(40)
    textMode(CORNER)
    text("Euler X,Z",20,HEIGHT/2+250)
    text("Euler Z,X",20,HEIGHT/2+50)
    text("Tangential",20,HEIGHT/2-150)
end

function smoothstep(t)
    return t*t*(3 - 2*t)
end

function smootherstep(t)
    return t*t*t*(t*(t*6 - 15) + 10)
end

--** Utility functions - joystick and touch ***

--manage joystick
function touched(t)
    if t.state~=ENDED then
        local v = vec2(t.x,t.y)
        if v:dist(centre)>radius-joystick then
            v = (v - centre):normalize()*(radius - joystick) + centre
        end  --set x,y values for joy based on touch
        joyPos=v - centre
        joy=joyPos/(radius-joystick)
        inTouch = true
    else --reset joystick to centre when touch ends
        joyPos=vec2(0,0) 
        joy=vec2(0,0)
        inTouch = false
    end
end

function DrawJoystick()
    ortho()
    viewMatrix(matrix())
    radius,joystick=100,30
    location=1 --1 for bottom left, 2 for bottom right
    if location==1 then centre=vec2(radius+5,radius+5) else centre=vec2(WIDTH-radius-5,radius+5) end
    pushStyle()
    fill(160, 182, 191, 50)
    stroke(118, 154, 195, 100)
    strokeWidth(1) 
    ellipse(centre.x,centre.y,2*radius)
    fill(78, 131, 153, 50)
    ellipse(centre.x+joyPos.x,centre.y+joyPos.y,joystick*2)
    popStyle()
end

Main.lua

--------------------------------------------------------------
--Project: Flying Library
--Version: alpha 1.0
--Author: Andrew Stacey, ignatz


--The MIT License (MIT)
--
--Copyright (c) 2014 Andrew Stacey, ignatz
--
--Permission is hereby granted, free of charge, to any person obtaining a copy
--of this software and associated documentation files (the "Software"), to deal
--in the Software without restriction, including without limitation the rights
--to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
--copies of the Software, and to permit persons to whom the Software is
--furnished to do so, subject to the following conditions:
--
--The above copyright notice and this permission notice shall be included in all
--copies or substantial portions of the Software.
--
--THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
--IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
--FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
--AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
--LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
--OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
--SOFTWARE.

-- Flying

function setup()
    --** Plane settings **
    plane=Plane() --plane mesh
    planeQ=Q(plane) --plane quaternion
    planePos=vec3(0,0,0) --starting position (plane doesn't change position)
    
    --** Joystick sttings **
    joyPos=vec2(5,5) --
    joy=vec2(0,0) --rotation vector derived from joystick position, both values between -1 and +1
    
    --**Flight controls **
    --for each of pitch and yaw, a true value below means that the plane moves directly in line 
    --with the joystick, with a maximum of 90 degrees, which I've called "slaved"
    --false means that existing rotation is incremented by the joystick movement
    --(this means the plane can rotate indefinitely, and loop the loop and do barrel rolls)
    --yaw is assumed to always be incremented, based on the current roll setting, so there is no parameter,
    --but I have coded for yaw to be allowed the option to be fixed, just in case
    parameter.boolean("Pitch_Slaved",true,Reset)
    parameter.boolean("Roll_Slaved",true,Reset)
    Yaw_Slaved=false --hard code this for now
    
    --** rotation settings and limits **
    --x, y, z map to pitch, yaw, roll
    --limits for slave rotation
    limit=vec3(90,90,90)
    --sensitivity of rotation for incremental rotation
    sensitivity=vec3(1,.5,1)
end

function Reset() --reset quaternion if settings changed
    planeQ:Reset() --initialise quaternion to (1,0,0,0)
end

function draw()
    background(179, 205, 220, 255)
    fill(255)
    perspective()
    camera(0,0,100,0,0,0)   
    RotateAndDraw()    
    DrawJoystick()
end

function RotateAndDraw()
       --** manage rotation changes here **
    --joy.x, joy.y hold joystick values ranging from -1 to +1
    --plane position is in the (unchanging) vec3 planePos
    
    --my approach is to have two quaternions, one for slaved rotations and one for incremental
    --the slaved one is created freshly each time, the incremental one is stored in the quaternion class
    local slave,increment=vec3(0,0,0),vec3(0,0,0)
    if Pitch_Slaved then
        slave.x=-joy.y*limit.x --pitch up if joystick pulled back (down) and vice versa
    else
        increment.x=-joy.y*sensitivity.x 
    end
    if Yaw_Slaved then
        slave.y=joy.x*limit.y --yaw based on roll
    else
        --increment.y=joy.x*sensitivity.y
    end
    if Roll_Slaved then
        slave.z=joy.x*limit.z 
    else
        increment.z=joy.x*sensitivity.z 
    end
    planeQ:AdjustRotation(slave,increment) 
    planeQ:Draw(plane,planePos)
end


--** Utility functions - joystick and touch ***

--manage joystick
function touched(t)
    if t.state~=ENDED then
        if vec2(t.x,t.y):dist(centre)>radius-joystick then return
        else  --set x,y values for joy based on touch
            joyPos=vec2(t.x-centre.x,t.y-centre.y)
            joy=vec2(joyPos.x,joyPos.y)/(radius-joystick)
        end
    else --reset joystick to centre when touch ends
        joyPos=vec2(0,0) 
        joy=vec2(0,0)
    end
end

function DrawJoystick()
    ortho()
    viewMatrix(matrix())
    radius,joystick=100,30
    location=1 --1 for bottom left, 2 for bottom right
    if location==1 then centre=vec2(radius+5,radius+5) else centre=vec2(WIDTH-radius-5,radius+5) end
    pushStyle()
    fill(160, 182, 191, 50)
    stroke(118, 154, 195, 100)
    strokeWidth(1) 
    ellipse(centre.x,centre.y,2*radius)
    fill(78, 131, 153, 50)
    ellipse(centre.x+joyPos.x,centre.y+joyPos.y,joystick*2)
    popStyle()
end

Plane.lua

--plane

Plane=class()

function Plane:init()
    a={}
    c1=color(107, 139, 184, 255)
    c2=color(72, 194, 81, 255)
    c3=color(176, 159, 101, 255)
    c4=color(255,0,0)
    c5=color(187, 53, 53, 255)
    a[1]=Block(3,3,25,c1,vec3(0,0,0))   
    a[2]=Block(15,1,5,c2,vec3(-9,1,-6))
    a[3]=Block(15,1,5,c5,vec3(9,1,-6))
    a[4]=Block(1,4,3,c2,vec3(0,3.5,11))
    a[5]=Block(4,1,2,c2,vec3(-3.5,1,11))
    a[6]=Block(4,1,2,c5,vec3(3.5,1,11))
    a[7]=Block(3,2,5,c3,vec3(0,2.5,-6))
    a[8]=Block(3,3,1,c4,vec3(0,0,-13))
    self.blocks=a 
end

function Plane:draw()
    for i=1,#self.blocks do
        self.blocks[i]:draw()
    end 
end

Block = class() --taken from 3D lab project

function Block:init(w,h,d,c,p,t,r) --width,height,depth,colour,position,texture, (optional) texture range (see above)
    self.width=w
    self.height=h
    self.depth=d  
    self.color=c  
    self.pos=p
    self.tex=t 
    --if no limits specified on which part of image to draw, set to 0,1
    if r~=nil then self.texR=r else self.texR={0,0,1,1} end
    self.blk=self:createBlock()
end

function Block:createBlock()
    -- all the unique vertices that make up a block
    --There are only 8 corners in a cube - we define them as vertices
    --all measurements are taken from the centre of the block
    --so bottom left front has x of -1/2 width, y of -1/2 height, and z of 1/2 depth
    local w,h,d=self.width,self.height,self.depth
    local x,y,z=self.pos.x,self.pos.y,self.pos.z
    local v = {
      vec3(x-0.5*w, y-0.5*h, z+0.5*d), -- Left  bottom front
      vec3(x+0.5*w, y-0.5*h, z+0.5*d), -- Right bottom front
      vec3(x+0.5*w, y+0.5*h, z+0.5*d), -- Right top    front
      vec3(x-0.5*w, y+0.5*h, z+0.5*d), -- Left  top    front
      vec3(x-0.5*w, y-0.5*h, z-0.5*d), -- Left  bottom back
      vec3(x+0.5*w, y-0.5*h, z-0.5*d), -- Right bottom back
      vec3(x+0.5*w, y+0.5*h, z-0.5*d), -- Right top    back
      vec3(x-0.5*w, y+0.5*h, z-0.5*d), -- Left  top    back
    }

    local cubeverts = {
      -- Front, Right, Back, Left, Top, Bottom
      v[1], v[2], v[3], v[1], v[3], v[4],
      v[2], v[6], v[7], v[2], v[7], v[3],
      v[6], v[5], v[8], v[6], v[8], v[7],
      v[5], v[1], v[4], v[5], v[4], v[8],
      v[4], v[3], v[7], v[4], v[7], v[8],
      v[5], v[6], v[2], v[5], v[2], v[1],
    }
    local cc={}
     --assign colors
        local c=self.color
        local x=0.5
        local BL=color(c.r,c.g,c.b) --bottom left
        local BR=color(c.r,c.g,c.b) --bottom right  
        local TR=color(c.r*x,c.g*x,c.b*x) --top right
        local TL=color(c.r*x,c.g*x,c.b*x) --top left
        for i=1,6 do
            cc[#cc+1]=BL
            cc[#cc+1]=BR
            cc[#cc+1]=TR
            cc[#cc+1]=BL
            cc[#cc+1]=TR
            cc[#cc+1]=TL
        end
  
    --put it all together
    local ms = mesh()
    ms.vertices = cubeverts
    if self.tex then
        ms.texture = self.tex
        ms.texCoords = cubetexCoords
    else
        ms.colors=cc
    end
    --ms:setColors(self.color)
    return ms
end  
    
function Block:draw()
    self.blk:draw() 
end

Q.lua

--Quaternions - dermot

--has two quaternions, one for incremental changes, and the other for total rotation
--the incremental one is updated and stored each draw
--the total one is based on the incremental one plus any slaved rotations, and is temporary
Q=class()

function Q:init()
    self:Reset()
end

function Q:AdjustRotation(a,b) --a,b are vec3 angle rotations, a is slave, b is increment
    local qa=Q:EulerToQuat(math.rad(a.x),math.rad(a.y),math.rad(a.z))
    local qb=Q:EulerToQuat(math.rad(b.x),math.rad(b.y),math.rad(b.z))
    self.incrementQ=Q:Multiply(qb,self.incrementQ):normalize() --add incremental rotation
    self.totalQ=Q:Multiply(qa,self.incrementQ):normalize() --calculate total rotation
    return self.totalQ --return in case it's needed
end


function Q:Rotate()
    modelMatrix(self:QToMatrix(0,0,0,self.totalQ))
end

function Q:Draw(m,p)
    pushMatrix()
    p=p or vec3(0,0,0)
    modelMatrix(self:QToMatrix(p.x,p.y,p.z,self.totalQ))
    m:draw()
    popMatrix()
end

function Q:EulerToQuat(p, y, r)
    -- compute all trigonometric values used to compute the quaternion
    local cp = math.cos(p/2)
    local cy = math.cos(y/2)
    local cr = math.cos(r/2)
    
    local sp = math.sin(p/2)
    local sy = math.sin(y/2)
    local sr = math.sin(r/2)
    
    local cycr = cy * cr
    local sysr = sy * sr
    
    -- combine values to generate the vector and scalar for the quaternion
    --[[
    ijk = -1
    ij = k, jk = i, ki = j
    
    kji = - kij = ikj = - ijk
    
    (cp + sp i)
    (cy + sy j)
    (cr + sr k)
    =
    cp cy cr +
    cp cy sr k +
    cp sy cr j +
    cp sy sr i + (jk)
    sp cy cr i +
  - sp cy sr j + (ik)
    sp sy cr k + (ij)
  - sp sy sr     (ijk)
    --]]
    local w = cp * cycr + sp * sysr
    local x = sp * cycr - cp * sysr
    local y = cp * sy * cr + sp * cy * sr
    local z = cp * cy * sr - sp * sy * cr
    return vec4(w,x,y,z):normalize()
end

function Q:Multiply(q1,q2)
    local w1,x1,y1,z1=q1.x,q1.y,q1.z,q1.w
    local w2,x2,y2,z2=q2.x,q2.y,q2.z,q2.w
    local w = w1*w2 - x1*x2 - y1*y2 - z1*z2
    local x = w1*x2 + x1*w2 + y1*z2 - z1*y2
    local y = w1*y2 - x1*z2 + y1*w2 + z1*x2
    local z = w1*z2 + x1*y2 - y1*x2 + z1*w2
    return vec4(w,x,y,z):normalize()
end

function Q:QToMatrix(px,py,pz)
    local q=self.totalQ
    -- calculate coefficients used for building the matrix
    local w,x,y,z=q.x,q.y,q.z,q.w
    local x2,y2,z2 = x + x,y + y,z + z
    local xx,xy,xz = x * x2,x * y2,x * z2
    local yy,yz,zz = y * y2,y * z2,z * z2
    local wx,wy,wz = w * x2,w * y2,w * z2
    -- fill in matrix positions with them
    return matrix(1-(yy+zz),xy-wz,xz+wy,0,xy+wz,1-(xx+zz),yz-wx,0,xz-wy,yz+wx,1-(xx+yy),0,px,py,pz,1)
end

function Q:Reset()
    self.incrementQ=vec4(1,0,0,0)
    self.totalQ=vec4(1,0,0,0)
end

Quaternion.lua

-- Quaternion library wherein vec4s are promoted to quaternions
-- Author: Andrew Stacey
-- Website: http://www.math.ntnu.no/~stacey/HowDidIDoThat/iPad/Codea.html
-- Licence: CC0 http://wiki.creativecommons.org/CC0

--[[
This provides code for handling rotations.  Internally, the rotations
are implemented as quaternions which are in turn implemented by
extending the methods available for vec4 userdata.

Due to how vec4s are implemented, we cannot have methods beginning with
letters a, b, g, r, w, x, y, z
--]]

-- Simplistic error handling
local error = error or print

-- Localise the maths functions/constants that we use for faster lookup.
local abs = math.abs
local pow = math.pow
local sqrt = math.sqrt
local sin = math.sin
local cos = math.cos
local acos = math.acos
local pi = math.pi
local floor = math.floor

--[[
The function "is_a" extends the capabilities of the method "is_a" which is automatically defined by Codea for classes.

Parameters:
a: object to be tested
b: test

The tests work as follows.

1. If the type of b is a string, it is taken as the name of a type to test a against.
2. If the type of b is a table, it is assumed to be a class to test if a is an instance thereof.
3. If the type of b is a userdata, the test is to see if a is the same type of object.
4. If b is a function, then it is replaced by the value of that function.
--]]

local function is_a(a,b)
    if type(b) == "function" then
        b = b()
    end
    if type(b) == "string" then
        return type(a) == b
    end
    if type(b) == "table" then
        if type(a) == "table"
            and a.is_a
            and a:is_a(b) then
                return true
        else
            return false
        end
    end
    if type(b) == "userdata" then
        if type(a) == "userdata" then
            a = getmetatable(a)
            b = getmetatable(b)
            return a == b
        else
            return false
        end
    end
    return false
end

-- Import the metatable of vec4s for extending
local mtq = getmetatable(vec4())

-- We run into difficulties if our vec4s contain NaNs or infs.  This
-- tests for those.
mtq["is_finite"] = function(q)
    if q.x < math.huge and q.x > -math.huge 
        and q.y < math.huge and q.y > -math.huge 
        and q.z < math.huge and q.z > -math.huge 
        and q.w < math.huge and q.w > -math.huge 
    then
        return true
    end
    return false
end

--[[
Test if we are real.
--]]

mtq["is_real"] = function (self)
    if self.y ~= 0 or self.z ~= 0 or self.w ~= 0 then
        return false
    end
    return true
end

--[[
Test if the real part is zero.
--]]

mtq["is_imaginary"] = function (self)
    if self.x ~= 0 then
        return false
    end
    return true
end

--[[
Normalise a quaternion to have length 1, safely.  Here "safely" means
that we ensure that we do not have NaNs or infs.  If we do, the
returned quaternion is the unit.
--]]

mtq["normalise"] = function (q)
    q = q:normalize()
    if q:is_finite() then
        return q
    else
        return vec4(1,0,0,0)
    end
end

--[[
Add two quaternions inline, including promotion of a number.

q + p
--]]

local __add = mtq["__add"]

mtq["__add"] = function (a,b)
    if is_a(a,"number") then
        a = vec4(a,0,0,0)
    end
    if is_a(b,"number") then
        b = vec4(b,0,0,0)
    end
    return __add(a,b)
end

--[[
Same for inline subtraction.

q - p
--]]

local __sub = mtq["__sub"]

mtq["__sub"] = function (a,b)
    if is_a(a,"number") then
        a = vec4(a,0,0,0)
    end
    if is_a(b,"number") then
        b = vec4(b,0,0,0)
    end
    return __sub(a,b)
end

--[[
For inline multiplication, we also allow multiplication by a matrix.
In this case, we use the fact that we're viewing quaternions as
rotations and matrices as affine transformations, so the best result
of multiplication is as the matrix representing the appropriate
composition.

q * p
--]]

local __mul = mtq["__mul"]

mtq["__mul"] = function (a,b)
    if is_a(a,"number") then
        return __mul(a,b)
    end
    if is_a(b,"number") then
        return __mul(a,b)
    end
    if is_a(a,matrix) then
        return a:__mul(b:tomatrixleft())
    end
    if is_a(b,matrix) then
        a = a:tomatrixleft()
        return a:__mul(b)
    end
    local x,y,z,w
    x = a.x * b.x - a.y * b.y - a.z * b.z - a.w * b.w
    y = a.x * b.y + a.y * b.x + a.z * b.w - a.w * b.z
    z = a.x * b.z - a.y * b.w + a.z * b.x + a.w * b.y
    w = a.x * b.w + a.y * b.z - a.z * b.y + a.w * b.x
    return vec4(x,y,z,w)
end

--[[
Conjugation (corresponds to inverting a rotation).
--]]

mtq["conjugate"] = function (q)
    return vec4(q.x, - q.y, - q.z, - q.w)
end

mtq["co"] = mtq["conjugate"]

--[[
Inline division, including division of and by a number.
--]]

local __div = mtq["__div"]

mtq["__div"] = function (a,b)
    if is_a(b,"number") then
        return __div(a,b)
    end
    local l = b:lenSqr()
    b = vec4(b.x/l,-b.y/l,-b.z/l,-b.w/l)
    if is_a(a,"number") then
        return vec4(a*b.x,a*b.y,a*b.z,a*b.w)
    end
    local x,y,z,w
    x = a.x * b.x - a.y * b.y - a.z * b.z - a.w * b.w
    y = a.x * b.y + a.y * b.x + a.z * b.w - a.w * b.z
    z = a.x * b.z - a.y * b.w + a.z * b.x + a.w * b.y
    w = a.x * b.w + a.y * b.z - a.z * b.y + a.w * b.x
    return vec4(x,y,z,w)
end

--[[
Powers.  Integer powers are defined iteratively.  Non-integer powers
use the slerp function.

TODO: test whether the slerp is faster than the iterative method for
integer powers as well.
--]]

local function intpower(q,n)
    if n ~= floor(n) then
        error("Only able to do integer powers")
        return false
    end
    if n == 0 then
        return vec4(1,0,0,0)
    elseif n > 0 then
       return q:__mul(power(q,n-1))
    elseif n < 0 then
       local l = q:lenSqr()
       q = vec4(q.x/l,-q.y/l,-q.z/l,-q.w/l)
       return q:power(-n)
    end
end

local function power(q,n)
   if n == floor(n) then
      return intpower(q,n)
   end
   local l = q:len()
   q = q:normalise()
   return l^n * q:slerp(n)
end

--[[
Inline exponentiation.

q^n

The behaviour depends on the type of the exponent:

* number: compute the power.
* vec4: conjugate by the exponent.
* other: return the conjugate.
--]]

mtq["__pow"] = function (q,n)
    if is_a(n,"number") then
        return power(q,n)
    elseif is_a(n,vec4) then
        return n:__mul(q):__div(n)
    else
        return q:conjugate()
    end
end

--[[
Interpolation functions, we assume the input to be already normalised
for speed.  If you cannot guarantee this, renormalise the input first.
The constructor functions do do the renormalisation.

Parameters:
q initial rotation
qq final rotation
t parameter (only for the direct functions)

If the input does not provide enough parameters, it is assumed that
the missing one is the initial rotation and that this should be taken
to be the identity rotation (represented by vec4(1,0,0,0)).
--]]


--[[
Linear interpolation, renormalised.
--]]

mtq["lerp"] = function (q,qq,t)
    if not t then
        return vec4(1,0,0,0):lerp(q,qq)
    end
    local v
    if (q + qq):len() == 0 then
        -- antipodal points, need a midpoint
        v = vec4(q.y,-q.x,q.w,-q.z)
        v = (1 - 2*t)*q + (1-abs(2*t-1))*v
    else
        v = (1-t)*q + t*qq
    end
    return v:normalise()
end

--[[
Spherical interpolation.

We have to be quite careful here not to emit anything with NaNs or
infs as there are several opportunities to create them due to the
limits of finite precision mathematics.
--]]

mtq["slerp"] = function (q,qq,t)
    if not t then
        return vec4(1,0,0,0):slerp(q,qq)
    end
    local v
    if (q + qq):len() == 0 then
        -- antipodal points, need a midpoint
        v = vec4(q.y,-q.x,q.w,-q.z)
        t = 2*t
    elseif (q - qq):len() == 0 then
        return q
    else
        v = qq
    end
    local ca = q:dot(v)
    local sa = sqrt(1 - pow(ca,2))
    if sa == 0 or sa ~= sa then
        return q
    end
    local a = acos(ca)
    sa = sin(a*t)/sa
    v = (cos(a*t) - ca*sa)*q+ sa*v
    return v
end

--[[
Constructor for normalised linear interpolation.
--]]

mtq["make_lerp"] = function (q,qq)
    if not qq then
        return vec4(1,0,0,0):make_lerp(q)
    end
    local v,w
    w = q:normalise()
    if (q + qq):len() == 0 then
        -- antipodal points, need a midpoint
        v = vec4(w.y,-w.x,w.w,-w.z)
        return function(t)
            local u = (1 - 2*t)*w + (1-abs(2*t-1))*v
            return u:normalise()
        end
    else
        v = qq:normalise()
        return function(t)
            local u = (1-t)*w + t*v
            return u:normalise()
        end
    end
end

--[[
Spherical interpolation
--]]

mtq["make_slerp"] = function (q,qq)
    if not qq then
        q,qq = vec4(1,0,0,0),q
    end
    local v,f,u
    if (q + qq):len() == 0 then
        -- antipodal points, need a midpoint
        v = vec4(q.y,-q.x,q.w,-q.z)
        f = 2
    elseif (q - qq):len() == 0 then
        return function(t)
            return q
        end
    else
        v = qq
        f = 1
    end
    v = v:normalise()
    u = q:normalise()
    local ca = u:dot(v)
    local sa = sqrt(1 - pow(ca,2))
    if sa == 0 or sa ~= sa then
        return function(t)
            return q
        end
    end
    local a = acos(ca)
    v = (v - ca*q)/sa
    return function(t)
        return cos(a*f*t)*q + sin(a*f*t)*v
    end
end

--[[
Returns the real part.
--]]

mtq["toreal"] = function (q)
    return q.x
end

--[[
Returns the vector (imaginary) part as a vec3 object.
--]]

mtq["vector"] = function (q)
    return vec3(q.y, q.z, q.w)
end

mtq["tovector"] = mtq["vector"]

--[[
Represents a quaternion as a string.
--]]

mtq["tostring"] = function (q)
    local s
    local im = {{q.y,"i"},{q.z,"j"},{q.w,"k"}}
    if q.x ~= 0 then
        s = string.format("%.3f",q.x)
    end
    for k,v in pairs(im) do
    if v[1] ~= 0 then
        if s then 
                if v[1] > 0 then
                    if v[1] == 1 then
                        s = s .. " + " .. v[2]
                    else
                        s = s .. " + " .. 
                        string.format("%.3f",v[1]) .. v[2]
                    end
                else
                    if v[1] == -1 then
                        s = s .. " - " .. v[2]
                    else
                        s = s .. " - " .. 
                        string.format("%.3f",-v[1]) .. v[2]
                    end
                end
        else
                if v[1] == 1 then
                    s = v[2]
                elseif v[1] == - 1 then
                    s = "-" .. v[2]
                else
                    s = string.format("%.3f",v[1]) .. v[2]
                end
        end
    end
    end
    if s then
        return s
    else
        return "0"
    end
end


mtq["__concat"] = function (q,s)
    if is_a(s,"string") then
        return q:tostring() .. s
    else
        return q .. s:tostring()
    end
end

--[[
Converts the quaternion to a matrix.

We distinguish between left and right actions.  A rotation matrix
acting on the right is the transpose of the matrix on the left.
--]]

mtq["tomatrixleft"] = function (q)
    q = q:normalise()
    local a,b,c,d = q.x,q.y,q.z,q.w
    local ab = 2*a*b
    local ac = 2*a*c
    local ad = 2*a*d
    local bb = 2*b*b
    local bc = 2*b*c
    local bd = 2*b*d
    local cc = 2*c*c
    local cd = 2*c*d
    local dd = 2*d*d
    return matrix(
        1 - cc - dd,
        bc - ad,
        ac + bd,
        0,
        bc + ad,
        1 - bb - dd,
        cd - ab,
        0,
        bd - ac,
        cd + ab,
        1 - bb - cc,
        0,
        0,0,0,1
    )
end

mtq["tomatrixright"] = function (q)
    q = q:normalise()
    local a,b,c,d = q.x,-q.y,-q.z,-q.w
    local ab = 2*a*b
    local ac = 2*a*c
    local ad = 2*a*d
    local bb = 2*b*b
    local bc = 2*b*c
    local bd = 2*b*d
    local cc = 2*c*c
    local cd = 2*c*d
    local dd = 2*d*d
    return matrix(
        1 - cc - dd,
        bc - ad,
        ac + bd,
        0,
        bc + ad,
        1 - bb - dd,
        cd - ab,
        0,
        bd - ac,
        cd + ab,
        1 - bb - cc,
        0,
        0,0,0,1
    )
end

mtq["tomatrix"] = mtq["tomatrixright"]

--[[
Converts the quaternion to an "angle-axis" representation.
--]]

mtq["toangleaxis"] = function (q)
    q = q:normalise()
    local a = 2*acos(q.x)
    local v = vec3(q.y,q.z,q.w)
    if v == vec3(0,0,0) then
        return 0,vec3(0,0,1)
    end
    return a,v:normalise()
end

--[[
The following code modifies various of Codea's functions to enable
them to take a quaternion as input.  We have to be careful as for some
passing nil is distinct to passing an empty input.

The other issue is as to the distinction between left and right
actions of matrices.  In OpenGL, matrices act on the right.  However,
in standard linear algebra, the convention is for matrices to act on
the left.
--]]

local __modelMatrix = modelMatrix

function modelMatrix(m)
    if m then
        if is_a(m,vec4) then
            m = m:tomatrixright()
        end
        return __modelMatrix(m)
    else
        return __modelMatrix()
    end
end

local __applyMatrix = applyMatrix

function applyMatrix(m)
    if m then
        if is_a(m,vec4) then
            m = m:tomatrixright()
        end
        return __applyMatrix(m)
    else
        return __applyMatrix()
    end
end

local __viewMatrix = viewMatrix

function viewMatrix(m)
    if m then
        if is_a(m,vec4) then
            m = m:tomatrixright()
        end
        return __viewMatrix(m)
    else
        return __viewMatrix()
    end
end

local __projectionMatrix = projectionMatrix

function projectionMatrix(m)
    if m then
        if is_a(m,vec4) then
            m = m:tomatrixright()
        end
        return __projectionMatrix(m)
    else
        return __projectionMatrix()
    end
end

local __rotate = rotate

function rotate(a,x,y,z)
    if is_a(a,vec4) then
        local v
        a,v = a:toangleaxis()
        x,y,z = v.x,v.y,v.z
        a = a*180/pi
    end
    __rotate(a,x,y,z)
end

local __translate = translate

function translate(x,y,z)
    if not y then
        x,y,z = x.x,x.y,x.z
    end
    __translate(x,y,z)
end

--[[
We also define some extensions to the vec3 type.
--]]

local mt = getmetatable(vec3())

-- Same as for vec4.
mt["is_finite"] = function(v)
    if v.x < math.huge and v.x > -math.huge 
        and v.y < math.huge and v.y > -math.huge 
        and v.z < math.huge and v.z > -math.huge 
    then
        return true
    end
    return false
end

--[[
Promote to a quaternion with 0 real part.
--]]

mt["toQuaternion"] = function (v)
    return vec4(0,v.x,v.y,v.z)
end

--[[
Apply a quaternion as a rotation (assumes unit quaternion for speed)
using conjugation.
--]]

mt["applyQuaternion"] = function (v,q)
   v = v:toQuaternion()
    v = q:__mul(v)
    v = v:__mul(q:conjugate())
    return v:vector()
end

-- There is no native rotate method for a vec3.
mt["rotate"] = mt["applyQuaternion"]

-- We use the exponential notation for writing the action of a
-- quaternion on a vector (this is consistent with group theory
-- notation).
mt["__pow"] = function (v,q)
    if is_a(q,vec4) then
        return v:applyQuaternion(q)
    end
    return false
end

mt["__concat"] = function (u,s)
    if is_a(s,"string") then
        return u:__tostring() .. s
    else
        return u .. s:__tostring()
    end
end

--[[
The rotateTo method produces a rotation that rotates the first vector
to the second about an axis that is perpendicular to both.  So long as
the two are not collinear, the axis is unique.  The angle is taken to
be the smallest angle.  If the vectors point in opposite directions,
an orthogonal axis is chosen in such a way as to minimise precision
error.

TODO: Allow for a third parameter to specify the axis in case of
ambiguity.
--]]
mt["rotateTo"] = function (u,v)
    if v:lenSqr() == 0 or u:lenSqr() == 0 then
        return vec4(1,0,0,0)
    end
    u = u:normalise()
    v = u + v:normalise()
    if v:lenSqr() == 0 then
        -- Opposite vectors, no canonical direction
        local a,b,c = abs(u.x), abs(u.y), abs(u.z)
        if a < b and a < c then
            v = vec3(0,-u.z,u.y)
        elseif b < c then
            v = vec3(u.z,0,-u.x)
        else
            v = vec3(u.y,-u.x,0)
        end
    end
    v = v:normalise()
    local d = u:dot(v)
    u = u:cross(v)
    return vec4(d,u.x,u.y,u.z)
end

--[[
Safe renormalisation, as for quaternions.  Except that there is no
"cannonical" unit vec3, so we return the unit vector in the z
direction.  The rationale for that is that if this is an axis, this
vector represents the axis out of the screen.
--]]
mt["normalise"] = function (v)
    v = v:normalize()
    if v:is_finite() then
        return v
    else
        return vec3(0,0,1)
    end
end

--[[
Inline multiplication extended to allow for multiplication by a matrix.

We interpret the vec3 as vertical or horizontal depending on whether
the matrix is on the right or left.  Multiplication by a matrix is
viewed as applying an affine transformation.
--]]
local __mulv = mt["__mul"]

mt["__mul"] = function(m,v)
    if is_a(m,vec3) and is_a(v,"number") then
        return __mulv(m,v)
    end
    if is_a(m,"number") and is_a(v,vec3) then
        return __mulv(m,v)
    end
    if is_a(m,matrix) and is_a(v,vec3) then
        local l = m[13]*v.x + m[14]*v.y + m[15]*v.z + m[16]
        return vec3(
            (m[1]*v.x + m[2]*v.y + m[3]*v.z + m[4])/l,
            (m[5]*v.x + m[6]*v.y + m[7]*v.z + m[8])/l,
            (m[9]*v.x + m[10]*v.y + m[11]*v.z + m[12])/l
        )
    end
    if is_a(m,vec3) and is_a(v,matrix) then
       m,v = v,m
        local l = m[4]*v.x + m[8]*v.y + m[12]*v.z + m[16]
        return vec3(
            (m[1]*v.x + m[5]*v.y + m[9]*v.z + m[13])/l,
            (m[2]*v.x + m[6]*v.y + m[10]*v.z + m[14])/l,
            (m[3]*v.x + m[7]*v.y + m[11]*v.z + m[15])/l
        )
    end
end

--[[
Extensions to the matrix class.
--]]
local mtm = getmetatable(matrix())

--[[
Inline multiplication by either quaternion or vector.
--]]
local __mulm = mtm["__mul"]

mtm["__mul"] = function (m,mm)
    if is_a(m,matrix) and is_a(mm,matrix) then
        return __mulm(m,mm)
    end
    if is_a(m,matrix) and is_a(mm,vec4) then
        return __mulm(m,mm:tomatrix())
    end
    if is_a(m,vec4) and is_a(mm,matrix) then
        return __mulm(m:tomatrix(),mm)
    end
    if is_a(m,matrix) and is_a(mm,vec3) then
        local l = m[13]*mm.x + m[14]*mm.y + m[15]*mm.z + m[16]
        return vec3(
            (m[1]*mm.x + m[2]*mm.y + m[3]*mm.z + m[4])/l,
            (m[5]*mm.x + m[6]*mm.y + m[7]*mm.z + m[8])/l,
            (m[9]*mm.x + m[10]*mm.y + m[11]*mm.z + m[12])/l
        )
    end
    if is_a(m,vec3) and is_a(mm,matrix) then
        local l = mm[4]*m.x + mm[8]*m.y + mm[12]*m.z + mm[16]
        return vec3(
            (mm[1]*m.x + mm[5]*m.y + mm[9]*m.z + mm[13])/l,
            (mm[2]*m.x + mm[6]*m.y + mm[10]*m.z + mm[14])/l,
            (mm[3]*m.x + mm[7]*m.y + mm[11]*m.z + mm[15])/l
        )
    end
end

-- Extending the rotate method to take a quaternion.
local __mrotate = mtm["rotate"]

mtm["rotate"] = function(m,a,x,y,z)
    if is_a(a,vec4) then
        a,x = a:toangleaxis()
        x,y,z = x.x,x.y,x.z
    end
    m:__mrotate(a,x,y,z)
end


--[[
The following functions are intended to be more "user friendly" and
provide solutions to common problems.

The primary functions construct a rotation dependent on some initial
data.  The simplest are to define a rotation from some other method of
specifying rotations: angle-axis or Euler angles.  More complicated
are methods to define a rotation by giving two (orthogonal) frames and
computing the rotation to get from the first to the last.  In
particular, there are certain common frames that might be expected to
be used frequently:

1. The initial orientation of the iPad.

2. A gravitational frame wherein the Gravity vector is "straight
down".  This is not completely defined as there is currently no way to
choose a corresponding horizontal direction (access to the compass
would provide this).  Two possibilities are: to choose the x-axis so
that it is always in the plane of the iPad, and to use RotationRate to
try to keep track of the initial x-axis.

3. A "DeltaOrientation" in which the change in orientation from one
frame to the next is used (this uses RotationRate).

There are obvious limits in accuracy in using these.
--]]

--[[
Returns a quaternion corresponding to the current gravitational vector
so that after applying the corresponding rotation, the y-axis points
in the gravitational direction and the x-axis is in the plane of the
iPad screen.

When we have access to the compass, the x-axis behaviour might change.
--]]

local function qGravity()
    local gxy, gy, gygxy, a, b, c, d
    if Gravity.x == 0 and Gravity.y == 0 then
        return vec4(1,0,0,0)
    else
        gy = - Gravity.y
        gxy = sqrt(pow(Gravity.x,2) + pow(Gravity.y,2))
        gygxy = gy/gxy
        a = sqrt(1 + gxy - gygxy - gy)/2
        b = sqrt(1 - gxy - gygxy + gy)/2
        c = sqrt(1 - gxy + gygxy - gy)/2
        d = sqrt(1 + gxy + gygxy + gy)/2
        if Gravity.y > 0 then
                a = a
                b = b
        end
        if Gravity.z < 0 then
                b = - b
                c = - c
        end
        if Gravity.x > 0 then
                c = - c
                d = - d
        end
        return vec4(a,b,c,d)
    end
end

function qrGravity(q)
    local qg,qx
    if not q then
        qx = ReferenceFrame()
    else
        qx = q
    end
    local y = vec3(0,-1,0)^qx
    qg = y:rotateTo(Gravity)
    return qg*qx
end

mtq["gravity"] = qrGravity

local frame = {
        vec3(1,0,0),
        vec3(0,1,0),
        vec3(0,0,1)
    }
local qzyx = vec4(1,0,0,0)

--[[
Needs to be run once every frame!
--]]

function updateReferenceFrame(q)
    local x,y,z
    --[[
    if CurrentOrientation == PORTRAIT then
        x,y,z = -frame[1],-frame[2],-frame[3]
    elseif CurrentOrientation == PORTRAIT_UPSIDE_DOWN then
        x,y,z = frame[1],frame[2],-frame[3]
    elseif CurrentOrientation == LANDSCAPE_LEFT then
        x,y,z = frame[2],-frame[1],-frame[3]
    elseif CurrentOrientation == LANDSCAPE_RIGHT then
        x,y,z = -frame[2],frame[1],-frame[3]
    end
    --]]
    x,y,z = unpack(frame)
    local qz = qRotation(RotationRate.z*DeltaTime,z.x,z.y,z.z)
    local qy = qRotation(RotationRate.y*DeltaTime,y.x,y.y,y.z)
    local qx = qRotation(RotationRate.x*DeltaTime,x.x,x.y,x.z)
    if q then
        local qq = qz * qy * qx * q
        q.x,q.y,q.z,q.w = qq.x,qq.y,qq.z,qq.w
    else
        qzyx = qz * qy * qx * qzyx
        return qzyx
    end
end

function ReferenceFrame(q)
    return q or qzyx
end

mtq["updateReferenceFrame"] = updateReferenceFrame
mtq["ReferenceFrame"] = ReferenceFrame

--[[
Converts a rotation to a quaternion.  The first argument is the angle
to rotate, the rest must specify an axis, either as a Vec3 object or
as three numbers.
--]]

function qRotation(a,x,y,z)
    local q,c,s
    if not y then
        x,y,z = x.x,x.y,x.z
    end
    q = vec4(0,x,y,z)
    q = q:normalise()
    if q == vec4(1,0,0,0) then
        return q
    end
    c = cos(a/2)
    s = sin(a/2)
    q = q:__mul(s)
    q = q:__add(c)
    return q
end

--[[
The qEuler function handles conversion from Euler angles.  As there
are many ways to specify Euler angles, we have to allow for quite a
variety.  The table __euler contains the code necessary for allowing
all of this flexibility.

The last parameter is the specification, which is a table taken from
the alphabet "xXyYzZ".  This ought to correspond to the axes of
rotation, using lowercase to mean the absolute frame and uppercase to
mean the transformed frame.

TODO: Test this.

If only two parameters are passed, the first is taken to be a vec3 or
a table containing the Euler angles.
--]]

local __euler = {}
__euler.x = function(q)
    return vec3(1,0,0)^q
end
__euler.X = function(q)
    return vec3(1,0,0)
end
__euler.y = function(q)
    return vec3(0,1,0)^q
end
__euler.Y = function(q)
    return vec3(0,1,0)
end
__euler.z = function(q)
    return vec3(0,0,1)^q
end
__euler.Z = function(q)
    return vec3(0,0,1)
end

function qEuler(a,b,c,v)
    if c then
        a = {a,b,c}
    else
        if is_a(a,vec3) then
            a = {a.x,a.y,a.z}
        end
        v = b
    end
    v = v or {"x","y","z"}
    local q = vec4(1,0,0,0)
    local w
    for k,u in ipairs(v) do
        w = __euler[u](q)
        q = q * qRotation(a[k],w)
    end
    return q
end

-- v is a tangent vector at the identity
function qTangent(x,y,z)
    local q
    if y then
        q = vec4(0,x,y,z)
    else
        q = vec4(0,x.x,x.y,x.z)
    end
    local qn = q:normalise()
    if qn == vec4(1,0,0,0) then
        return qn
    end
    local a = q:len()/2
    return cos(a)*vec4(1,0,0,0) + sin(a)*qn
end

function qRotationRate()
    return qTangent(DeltaTime * RotationRate)
end

local iGravity = vec3(0,-1,0)
Frame = {
    Roll = vec4(1,0,0,0),
    Pitch = vec4(1,0,0,0),
    Yaw = vec4(1,0,0,0),
    Gravity = vec4(1,0,0,0),
    Rotation = vec4(1,0,0,0),
    AdjustedRotation = vec4(1,0,0,0),
    RotationRate = vec4(1,0,0,0)
}

local update_fn 
update_fn = function()
    local q = vec3(iGravity.x,iGravity.y,0):rotateTo(
            vec3(Gravity.x,Gravity.y,0))
    Frame.Roll.x = q.x
    Frame.Roll.y = q.y
    Frame.Roll.z = q.z
    Frame.Roll.w = q.w
    q = vec3(0,iGravity.y,iGravity.z):rotateTo(
            vec3(0,Gravity.y,Gravity.z))
    Frame.Pitch.x = q.x
    Frame.Pitch.y = q.y
    Frame.Pitch.z = q.z
    Frame.Pitch.w = q.w
    q = vec3(iGravity.x,0,iGravity.z):rotateTo(
            vec3(Gravity.x,0,Gravity.z))
    Frame.Yaw.x = q.x
    Frame.Yaw.y = q.y
    Frame.Yaw.z = q.z
    Frame.Yaw.w = q.w
    q = iGravity:rotateTo(Gravity)
    Frame.Gravity.x = q.x
    Frame.Gravity.y = q.y
    Frame.Gravity.z = q.z
    Frame.Gravity.w = q.w
    q = vec4(0,RotationRate.x,RotationRate.y,RotationRate.z)
    local qn = q:normalise()
    if qn ~= vec4(1,0,0,0) then
        local a = q:len()*DeltaTime/2
        qn = cos(a)*vec4(1,0,0,0) + sin(a)*qn
    end
    Frame.RotationRate.x = qn.x
    Frame.RotationRate.y = qn.y
    Frame.RotationRate.z = qn.z
    Frame.RotationRate.w = qn.w
    q = qn*Frame.Rotation
    Frame.Rotation.x = q.x
    Frame.Rotation.y = q.y
    Frame.Rotation.z = q.z
    Frame.Rotation.w = q.w
    q = qn*Frame.AdjustedRotation
    local g = iGravity^(q)
    qn = g:rotateTo(iGravity)
    q = q*qn
    Frame.AdjustedRotation.x = q.x
    Frame.AdjustedRotation.y = q.y
    Frame.AdjustedRotation.z = q.z
    Frame.AdjustedRotation.w = q.w
    tween.delay(0,update_fn)
end
    

tween.delay(.5,function()
     iGravity = vec3(Gravity.x,Gravity.y,Gravity.z)
     tween.delay(0,update_fn)
     end)

--[[
A suite of test functions.
--]]
function testRotations()
    print("Rotation tests:")
    local tolerance = 10^(-6)
    local vars = [[local q1,q2,i,j,k
    q1 = vec4(1,2,3,4)
    q2 = vec4(.5,.5,-.5,-.5)
    i = vec4(0,1,0,0)
    j = vec4(0,0,1,0)
    k = vec4(0,0,0,1)
    q3 = vec4(1/math.sqrt(2),0,0,1/math.sqrt(2))
    v1 = vec3(1,0,0)
    v2 = vec3(0,1,0)
    v3 = vec3(1,-1,2)
    m = matrix(
        10,10,5,1,
        4,6,4,1,
        1,3,3,1,
        0,1,2,1
    )
    return ]]
    local tests = {
        {"q1 * q2", vec4(3,2,4,-1)},
        {"q1 + q2", vec4(1.5,2.5,2.5,3.5)},
        {"q1 - q2", vec4(.5,1.5,3.5,4.5)},
        {"q1 / q2", vec4(-2,0,-1,5)},
        {"q1^q2", vec4(1,-4,-2,3)},
        {"q1^\"\"", vec4(1,-2,-3,-4)},
        {"i * j", vec4(0,0,0,1)},
        {"j * k", vec4(0,1,0,0)},
        {"k * i", vec4(0,0,1,0)},
        {"v1^q3", vec3(0,1,0)},
        {"q3:tomatrixleft()*v1", vec3(0,1,0)},
        {"v1:rotate(q3)", vec3(0,1,0)},
        {"v2^q3", vec3(-1,0,0)},
        {"q3:tomatrixleft()*v2", vec3(-1,0,0)},
        {"v2:rotate(q3)", vec3(-1,0,0)},
        {"qRotation(math.pi/2,0,0,1)", vec4(1/math.sqrt(2),0,0,1/math.sqrt(2))},
        {"v1:rotateTo(v2)", vec4(1/math.sqrt(2),0,0,1/math.sqrt(2))},
        {"v3 * m", vec3(8,11,9)/3},
        {"m * v3", vec3(11,7,5)/4}
    }
    local t,a,m,n,tn
    tn,n = 0,0
    for k,u in ipairs(tests) do
        tn = tn + 1
        t = loadstring(vars .. u[1])
        a = t()
        if a == u[2] then
            m = "OK"
            n = n + 1
        elseif a.dist and a:dist(u[2]) < tolerance then
            m = "OK (" .. a:dist(u[2]) .. ")"
            n = n + 1
        else
            m = "not OK (expected " .. u[2] .. ")"
        end
        print(u[1] .. " = " .. a .. " : " .. m)
    end
    print(n .. " tests passed out of " .. tn)
end