//Content-type: application/x-javascript

//CONSTANTS
TOL=1e-14;
D2R=Math.PI/180;

// MATH LIBRARIES //
N=function(x) {return parseFloat(x)}; //make strings numbers
nan2zero=function(x) {if (isNaN(x)) {return N(0)} else {return x}}; //make NaNs zeros

function norm(a) {
  //calculates the 2-norm of linear array a

  //first find the largest-magnitude element
  var index=-1;var si=0; var dum;
  for(i=0;i<a.length;i++) {
    if((dum=Math.abs(a[i]))>si) {
      si=dum; index=i;
    }
  }
  //si is now the magnitude of the largest-magnitude element
  if(si==0) return 0;
  else {
   var norm2 = 0;
   for(i=0;i<a.length;i++) {norm2 +=(a[i]/si)*(a[i]/si)};
   return si*Math.sqrt(norm2);
  }
}
function flatten(v) {
 //returns a linear array that corresponds to column vector v
 var l=v.length;
 a=new Array(l)
 for(i=0;i<l;i++) a[i]=v[i][0];
 return a;
}
function hat(v) {
 //returns a unit vector in the same direction as v, or zero if v=0
 var a=flatten(v);
 var n=norm(a);
 if(n>0) return sxm(1/n,v);
 else    return v;
}

// ORIENTATION OBJECT BELOW

//Define Orientation Object Type
function Orientation(qin) {
  this.q=[1,0,0,0];
  this.dcm=[[1,0,0],[0,1,0],[0,0,1]];
  this.valid=false;
  //set validity if passed 1
  if(qin==1) this.valid=true;
  //set quaternion if passed qin
  if(typeof(qin)=="object" && qin.length==4) {
    this.q=qin;
    this.q2dcm();
    this.valid=true;
  }
}
Orientation.prototype.toString = function() {
  return "Orientation: q="+this.q; 
}
Orientation.prototype.badquaterntion = function () {
  alert('Illegal quaternion--ignored.');
}
Orientation.prototype.zeroquaternion = function () {
  alert('The quaternion contains all zeros--ignored');
  if(this.valid) {
    this.dcm2q();
  }
}
Orientation.prototype.smallquaternion = function () {
  alert('Quaternion does not have unit norm--normalizing');
}
Orientation.prototype.bade = function () {
  alert('Bad Euler angles--ignored.');
}
Orientation.prototype.badaa = function () {
  alert('Bad angle-axis--ignored.');
}
Orientation.prototype.zeroaxis = function () {
  alert('zero axis with nonzero angle--ignored.');
}
Orientation.prototype.smallaxis = function () {
  alert('Axis does not have unit norm--normalizing');
}
Orientation.prototype.q2dcm = function () {
  // Converts quaternion q to DCM t

  //first, get the quaternion
  var q=this.q

  //check for unit norm

  var qnorm = norm(q);
  if(qnorm==0) this.zeroquaternion();
  else {
    if(Math.abs(qnorm-1)>TOL) this.smallquaternion();
    //normalize no matter what
    for(i=0;i<4;i++) this.q[i]=this.q[i]/qnorm;

    //now convert the quaternion to a rotation matrix and put it in the table
    //var z=2/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
    var z=2;

    this.dcm[0][0]=(this.q[1]*this.q[1]+this.q[0]*this.q[0])*z - 1;
    this.dcm[0][1]=(this.q[1]*this.q[2]-this.q[3]*this.q[0])*z;
    this.dcm[0][2]=(this.q[1]*this.q[3]+this.q[2]*this.q[0])*z;

    this.dcm[1][0]=(this.q[1]*this.q[2]+this.q[3]*this.q[0])*z;
    this.dcm[1][1]=(this.q[2]*this.q[2]+this.q[0]*this.q[0])*z - 1;
    this.dcm[1][2]=(this.q[2]*this.q[3]-this.q[1]*this.q[0])*z;

    this.dcm[2][0]=(this.q[1]*this.q[3]-this.q[2]*this.q[0])*z;
    this.dcm[2][1]=(this.q[2]*this.q[3]+this.q[1]*this.q[0])*z;
    this.dcm[2][2]=(this.q[3]*this.q[3]+this.q[0]*this.q[0])*z - 1;  

    this.valid=true;
  }
}

Orientation.prototype.dcm2q = function() {
// Converts DCM t to quaternion q
  // set t to be the DCM matrix 
  var t=this.dcm
  //set any undefined values to zero
  for(i=0;i<3;i++) {
    for(j=0;j<3;j++) {
      if (isNaN(t[i][j])) 
         t[i][j]=N(0);
    }
  }
  // now set q
  var dx=1.0+t[0][0]-t[1][1]-t[2][2]; //4 q(1)-squared
  var dy=1.0-t[0][0]+t[1][1]-t[2][2]; //4 q(2)-squared
  var dz=1.0-t[0][0]-t[1][1]+t[2][2]; //4 q(3)-squared
  var ds=1.0+t[0][0]+t[1][1]+t[2][2]; //4 q(4)-squared
  // At least one of the above is nonzero.

  if (dx>=dy&&dx>=dz&&dx>=ds) {
    this.q[1]=Math.sqrt(dx)/2; this.q[2]=(t[1][0]+t[0][1])/(4 * this.q[1]);
    this.q[3]=(t[2][0]+t[0][2])/(4 * this.q[1]); this.q[0]=(t[2][1]-t[1][2])/(4 * this.q[1]);
  }
  else if (dy>dx&&dy>=dz&&dy>=ds) {
    this.q[2]=Math.sqrt(dy)/2; this.q[1]=(t[1][0]+t[0][1])/(4 * this.q[2]);
    this.q[3]=(t[2][1]+t[1][2])/(4 * this.q[2]); this.q[0]=(t[0][2]-t[2][0])/(4 * this.q[2]);
  }
  else if (dz>dx&&dz>dy&&dz>=ds) {
    this.q[3]=Math.sqrt(dz)/2; this.q[1]=(t[2][0]+t[0][2])/(4 * this.q[3]);
    this.q[2]=(t[2][1]+t[1][2])/(4 * this.q[3]); this.q[0]=(t[1][0]-t[0][1])/(4 * this.q[3]);
  }
  else if (ds>dx&&ds>dy&&ds>dz) {
    this.q[0]=Math.sqrt(ds)/2; this.q[1]=(t[2][1]-t[1][2])/(4 * this.q[0]);
    this.q[2]=(t[0][2]-t[2][0])/(4 * this.q[0]); this.q[3]=(t[1][0]-t[0][1])/(4 * this.q[0]);
  }
  this.valid=true;
}

Orientation.prototype.set_dcm = function(t) {
// Sets orientation based on rotation matrix t
  this.dcm=t;
  this.dcm2q();
}
Orientation.prototype.set_q = function(q) {
// Sets orientation based on quaternion q
  this.q=q;
  this.q2dcm();
}
Orientation.prototype.set_e = function(e) {
// Sets orientation based on 3-2-1 Euler angles e

  //check for length 3
  if(e.length!=3) {
    this.bade();
    e=[0,0,0];
  } 

  //set any undefined values to zero
  if (isNaN(e[0])) {document.e.psi.value=e[0]=N(0);}
  if (isNaN(e[1])) {document.e.theta.value=e[1]=N(0);}
  if (isNaN(e[2])) {document.e.phi.value=e[2]=N(0);}

  //set sines and cosines
  var cpsi=   Math.cos(e[0]);
  var spsi=   Math.sin(e[0]);
  var ctheta= Math.cos(e[1]);
  var stheta= Math.sin(e[1]);
  var cphi=   Math.cos(e[2]);
  var sphi=   Math.sin(e[2]);

  //set the dcm
  this.dcm[0][0]=cpsi*ctheta;
  this.dcm[0][1]=cpsi*stheta*sphi-spsi*cphi;
  this.dcm[0][2]=cpsi*stheta*cphi+spsi*sphi;

  this.dcm[1][0]=spsi*ctheta;
  this.dcm[1][1]=spsi*stheta*sphi+cpsi*cphi;
  this.dcm[1][2]=spsi*stheta*cphi-cpsi*sphi;

  this.dcm[2][0]=-stheta;
  this.dcm[2][1]=ctheta*sphi;
  this.dcm[2][2]=ctheta*cphi;

  //set the quaternion
  this.dcm2q();
}
Orientation.prototype.set_aa = function(angle,kin) {
  //finds the orientation corresponding to an angle-axis pair

  if(kin.length!=3) {
    this.badaa(); //bad input-tragic
  }
  else {
    norma=norm(kin);
    if(!norma && angle!=0) {
      this.zeroaxis(); //zero axis with nonzero angle--tragic
    }
    else {
      if(!norma) {
        norma=1;  //zero angle, so axis doesn't matter
      }
      if(Math.abs(norma-1)>TOL) {
        this.smallaxis();
      }
      //normalize no matter what
      var k=new Array(3);
      k[0]=kin[0]/norma;
      k[1]=kin[1]/norma;
      k[2]=kin[2]/norma;

      //now set the Rotation matrix
      s=Math.sin(angle);
      c=Math.cos(angle);
      v=1-c; //versine
      this.dcm[0][0]=k[0]*k[0]*v+c;
      this.dcm[0][1]=k[0]*k[1]*v-k[2]*s;
      this.dcm[0][2]=k[0]*k[2]*v+k[1]*s;

      this.dcm[1][0]=k[0]*k[1]*v+k[2]*s;
      this.dcm[1][1]=k[1]*k[1]*v+c;
      this.dcm[1][2]=k[1]*k[2]*v-k[0]*s;

      this.dcm[2][0]=k[0]*k[2]*v-k[1]*s;
      this.dcm[2][1]=k[1]*k[2]*v+k[0]*s;
      this.dcm[2][2]=k[2]*k[2]*v+c;

      //set the quaternion
      this.dcm2q();
    }
  }
}
Orientation.prototype.find_dcm = function() {
  return this.dcm;
}
Orientation.prototype.find_q = function() {
  return this.q;
}
Orientation.prototype.find_e = function() {
  // Converts DCM t to Z-Y-X Euler angles

  // initialize Euler angles and intermediates
  var e=new Array(3);

  var ct=Math.sqrt(this.dcm[0][0]*this.dcm[0][0]+this.dcm[1][0]*this.dcm[1][0]);
  e[1]=Math.atan2(-this.dcm[2][0],ct);

  if(ct!=0.0) {
      s=this.dcm[2][1]/ct;
      c=this.dcm[2][2]/ct; if(s!=0||c!=0) {e[2]=Math.atan2(s,c);}
      s=this.dcm[1][0]/ct;
      c=this.dcm[0][0]/ct; if(s!=0|c!=0) {e[0]=Math.atan2(s,c);}
  }
  else {  //can't distinguish yaw from roll--set psi to 0
      s=-this.dcm[2][1];
      if(s!=0||this.dcm[1][1]!=0) {e[2]=Math.atan2(s,this.dcm[1][1]);}
      e[0]=0;
  }
  return e;
}
Orientation.prototype.find_e2 = function() {
  // Converts DCM t to Z-Y-X Euler angles

  // initialize Euler angles and intermediates
  var e=new Array(3);

  var ct=-Math.sqrt(this.dcm[0][0]*this.dcm[0][0]+this.dcm[1][0]*this.dcm[1][0]);
  e[1]=Math.atan2(-this.dcm[2][0],ct);

  if(ct!=0.0) {
      s=this.dcm[2][1]/ct;
      c=this.dcm[2][2]/ct; if(s!=0||c!=0) {e[2]=Math.atan2(s,c);}
      s=this.dcm[1][0]/ct;
      c=this.dcm[0][0]/ct; if(s!=0|c!=0) {e[0]=Math.atan2(s,c);}
  }
  else {  //can't distinguish yaw from roll--set psi to 0
      s=-this.dcm[2][1];
      if(s!=0||this.dcm[1][1]!=0) {e[2]=Math.atan2(s,this.dcm[1][1]);}
      e[0]=0;
  }
  return e;
}
Orientation.prototype.find_aa = function() {
  //finds the angle and axis corresponding to quaternion q

  //set q to the quaternion, flipped in sign if necessary 
  var sign;
  N(this.q[0])<0.0 ? sign=-1 : sign=1;
  var q=[sign*N(this.q[0]),
         sign*N(this.q[1]),
         sign*N(this.q[2]),
         sign*N(this.q[3])];

  var k=[q[1],q[2],q[3]];
  var normk=norm(k);
  if(normk!=0) {
    var angle=2*Math.atan2(normk,q[0]);
    k[0]/=normk; k[1]/=normk; k[2]/=normk;
  }
  else {
    if(q[0]!=0) {
      var angle=0.0;
      var k=[1,0,0]; //completely arbitrary
    }
    else { //quaternion is all zeros
      var angle=Number.NaN;
      var k=[Number.NaN,Number.NaN,Number.NaN];
    }
  }
  return [angle,k];
}