// 6 Degrees of Freedom Headtracking with wiimote
// 2008 by Manuel Unternaehrer
// Inspired by Johnny Lee: http://www.cs.cmu.edu/~johnny/projects/wii/

wiimote.Leds = 1     //Choose whatever you like :-)

var.DotDist = .12     //Distance left+right dot, in meters
var.DotDepth = 0.045  //Depth of middle dot

//Adjust Smoothing, Amplifying (Factor) and Calibration
FakeTrackIR.x=-smooth(var.pos[1],50)
FakeTrackIR.y=smooth(var.pos[2],50)
FakeTrackIR.z=smooth(var.pos[3],50)
FakeTrackIR.yaw=-smooth(var.yaw,50)*30
FakeTrackIR.roll=smooth(var.roll,50)
FakeTrackIR.pitch=-smooth((var.pitch - 30),50)*10

var.scenter = [1024,768,0]/2/1024 //Sensor's center
var.p1 = [wiimote.dot1x/1024,wiimote.dot1y/1024,0]
var.p2 = [wiimote.dot2x/1024,wiimote.dot2y/1024,0]
var.p3 = [wiimote.dot3x/1024,wiimote.dot3y/1024,0]

//Look for Left, Right and Middle dot
if wiimote.dot1x < wiimote.dot2x then
   if wiimote.dot1x < wiimote.dot3x then
      var.pleft = var.p1
      var.pmid = 0
   else
       var.pleft = var.p3
       var.pmid = var.p1
   endif
   if wiimote.dot2x < wiimote.dot3x then
      var.pright = var.p3
      var.pmid = var.p2
   else
       var.pright = var.p2
       if var.pmid = 0 then var.pmid = var.p3
   endif
else
    if wiimote.dot2x < wiimote.dot3x then
       var.pleft = var.p2
       var.pmid = 0
    else
        var.pleft = var.p3
        var.pmid = var.p2
    endif
    if wiimote.dot1x < wiimote.dot3x then
       var.pright = var.p3
       var.pmid = var.p1
    else
        var.pright = var.p1
        if var.pmid = 0 then var.pmid = var.p3
    endif
endif

//Do the geometric stuff
if wiimote.dot1vis and wiimote.dot2vis and wiimote.dot3vis then

var.posZ = var.DotDist/Tan(Abs(var.pright-var.pleft)*45)
var.pos = ((var.pright+var.pleft)/2-var.scenter)*var.posZ + [0,0,var.posZ]

var.roll = aSin((var.pright-var.pleft)[2]/Abs(var.pright-var.pleft))

var.pmidoff = Abs((var.pright-var.pleft) cross (var.pmid-var.pleft))/Abs(var.pright-var.pleft)^2 //0..1,middle dot off-line
var.pmidcent = ((var.pright-var.pleft) dot (var.pmid-var.pleft))/Abs(var.pright-var.pleft)^2 //0..1, middle dot off-center

var.turn = aTan((0.5-var.pmidcent)/(var.DotDepth*Cos(var.pitch)/var.DotDist))
var.yaw = var.turn - 45*((var.pright+var.pleft)/2-var.scenter)[1]

var.pitch = aSin(var.pmidoff/(var.DotDepth/var.DotDist)) + 45*((var.pright+var.pleft)/2-var.scenter)[2]

debug = smooth(var.pos,50) + " " + smooth(var.yaw,50) + " " + smooth(var.pitch,50)+ " " + smooth(var.roll,50)

endif

//Test 3D Visualisation, comment out if not needed
obj4.colour= 0xFFFF00
obj4.cubesize= 10cm
obj4.x=0.
obj4.y=0.
obj4.z=1.3m
obj5.colour= 0xFFFFFF
obj5.cubesize= 2cm
obj5.x=0.
obj5.y=0.
obj5.z=1m

cam.x=-smooth(var.pos[1],50)  *5
cam.y=smooth(var.pos[2],50)    *5
cam.z=-smooth(var.pos[3],50)  *3 +1
cam.yaw=-smooth(var.yaw,50)
cam.roll=smooth(var.roll,50)
cam.pitch=-smooth((var.pitch-30),50)

// Fishtank with glovepie and wiimote, 2 dots needed
// Watching the fishtank, Close one eye, the effect is much greater!
// 2008 by Manuel Unternaehrer
// Inspired by Johnny Lee: http://www.cs.cmu.edu/~johnny/projects/wii/

pie.framerate=50Hz

wiimote.Leds = 1     //Choose whatever you like :-)

var.DotDist = .12 m    //Distance left+right dot, in meters
var.ScreenHeight = .18 m  //Visualisation Window's Height, measured with ruler

//interesting part
cam.x=-smooth(var.pos[1],5)
cam.y=smooth(var.pos[2],5)
cam.z=-smooth(var.pos[3],5)
cam.yaw=smooth(var.yaw,5)
cam.roll=0
cam.pitch=-smooth((var.pitch),5)
cam.vFov=aTan(var.ScreenHeight/(smooth(var.posZ,5))/2)*2
cam.nearclip=10cm
// cam.Stereoscopic=true

// the rest is my degenerated 6DOF Headtracker (with much shorter code)

var.scenter = [1024,768,0]/2/1024 //Sensor's center
var.p1 = [wiimote.dot1x/1024,wiimote.dot1y/1024,0]
var.p2 = [wiimote.dot2x/1024,wiimote.dot2y/1024,0]

//Look for Left, Right and Middle dot
if wiimote.dot1x < wiimote.dot2x then
  var.pleft = var.p1
  var.pright = var.p2
else
    var.pleft =var.p2
    var.pright = var.p1
endif

//Do the geometric stuff
if wiimote.dot1vis and wiimote.dot2vis  then

var.posZ = var.DotDist/Tan(Abs(var.pright-var.pleft)*45)
var.pos = ((var.pright+var.pleft)/2-var.scenter)*var.posZ*3.14/4 + [0,0,var.posZ]

var.yaw = 45*((var.pright+var.pleft)/2-var.scenter)[1]   //Changed for Fishtank demo

var.pitch =  45*((var.pright+var.pleft)/2-var.scenter)[2]  //Changed for Fishtank demo

endif

debug = smooth(var.pos,0) + " " + smooth(var.yaw,0) + " " + smooth(var.pitch,0)+ " " + smooth(var.roll,0)

//Test 3D Visualisation, comment out if not needed
obj4.colour= 0xFFFF00
obj4.cubesize= var.ScreenHeight
obj4.x=0.
obj4.y=0.
obj4.z=.5m

obj5.colour= 0xFFFFFF
obj5.cubesize= 2cm
obj5.x=0.
obj5.y=0.
obj5.z=0.2m

//Object "in front of the monitor":
obj12.colour= 0x0000FF
obj12.cubesize= 2cm
obj12.x=0.
obj12.y=0.
obj12.z=-0.1m

obj1.colour= 0xFFFF00
obj1.cubesize= 2cm
obj1.pos = [-var.ScreenHeight/2,-var.ScreenHeight/2,.2]

obj2.colour= 0xFFFF00
obj2.cubesize= 2cm
obj2.pos = [var.ScreenHeight/2,-var.ScreenHeight/2,.2]

obj6.colour= 0xFFFF00
obj6.cubesize= 2cm
obj6.pos = [-var.ScreenHeight/2,var.ScreenHeight/2,.2]

obj7.colour= 0xFFFF00
obj7.cubesize= 2cm
obj7.pos = [var.ScreenHeight/2,var.ScreenHeight/2,.2]

obj8.colour= 0xFFFF00
obj8.cubesize= 2cm
obj8.pos = [var.ScreenHeight/2,-var.ScreenHeight/2,.0]

obj9.colour= 0xFFFF00
obj9.cubesize= 2cm
obj9.pos = [-var.ScreenHeight/2,-var.ScreenHeight/2,.0]

obj10.colour= 0xFFFF00
obj10.cubesize= 2cm
obj10.pos = [var.ScreenHeight/2,var.ScreenHeight/2,0.0]

obj11.colour= 0xFFFF00
obj11.cubesize= 2cm
obj11.pos = [-var.ScreenHeight/2,var.ScreenHeight/2,.0]