/*
 * Math3d - The 3D Computer Graphics Math Library
 * Copyright (C) 1996-2000 by J.E. Hoffmann <je-h@gmx.net>
 * All rights reserved.
 *
 * This program is  free  software;  you can redistribute it and/or modify it
 * under the terms of the  GNU Lesser General Public License  as published by 
 * the  Free Software Foundation;  either version 2.1 of the License,  or (at 
 * your option) any later version.
 *
 * This  program  is  distributed in  the  hope that it will  be useful,  but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or  FITNESS FOR A  PARTICULAR PURPOSE.  See the  GNU Lesser General Public  
 * License for more details.
 *
 * You should  have received  a copy of the GNU Lesser General Public License
 * along with  this program;  if not, write to the  Free Software Foundation,
 * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 * $Id: mquat.cpp,v 1.1 2002/11/18 08:23:01 rst Exp $
 */
#define _MATH3D_EXPORT
#include "mquat.h"
#include "m2d.h"
#include "m3d.h"
#include "m4x4.h"
#include <cmath>
#include <iostream>


/*!
 *
 */
Math3d::MQuat::MQuat(const MQuat& q)
{
  d_v[0]=q.d_v[0];
  d_v[1]=q.d_v[1];
  d_v[2]=q.d_v[2];
  d_v[3]=q.d_v[3];
}


/*!
 *
 */
Math3d::MQuat::MQuat(double x, double y, double z, double w) 
{
  d_v[0]=x; 
  d_v[1]=y; 
  d_v[2]=z; 
  d_v[3]=w;
}


/*!
 *  
 */
Math3d::MQuat::MQuat(const float q[4])
{
  d_v[0]=q[0]; 
  d_v[1]=q[1]; 
  d_v[2]=q[2]; 
  d_v[3]=q[3];
}


/*!
 *  
 */
Math3d::MQuat::MQuat(const double q[4])
{
  d_v[0]=q[0]; 
  d_v[1]=q[1]; 
  d_v[2]=q[2]; 
  d_v[3]=q[3];
}


/*!
 *
 */
Math3d::MQuat::MQuat(const M3d& axis, double angle)
{
  set(axis,angle);
}


/*!
 *
 */
Math3d::MQuat::MQuat(const M4x4& m)
{
  set(m);
}


/*!
 *
 */
const Math3d::MQuat&
Math3d::MQuat::operator=(const MQuat& q)
{
  d_v[0]=q.d_v[0];
  d_v[1]=q.d_v[1];
  d_v[2]=q.d_v[2];
  d_v[3]=q.d_v[3];
  return(*this);
}


/*!
 *
 */
void
Math3d::MQuat::zero()
{
  d_v[0]=d_v[1]=d_v[2]=d_v[3]=0.0;
}


/*!
 *
 */
void
Math3d::MQuat::identity()
{
  d_v[0]=d_v[1]=d_v[2]=0.0;
  d_v[3]=1.0;
}


/*!
 * Constructuin by standard matrix - quaternion conversion.
 * The matrix is supposed to have its last column and row equal 
 * to (0,0,0,1) otherwise, the result is undefined.
 */
void
Math3d::MQuat::set(double x, double y, double z, double w) 
{
  d_v[0]=x; 
  d_v[1]=y; 
  d_v[2]=z; 
  d_v[3]=w;
}


/*!
 *
 */
void
Math3d::MQuat::set(const M3d& axis, double angle)
{
  double omega,s;
  double l;

  l=sqrt(axis.d_v[0]*axis.d_v[0] + axis.d_v[1]*axis.d_v[1] + axis.d_v[2]*axis.d_v[2]);
  if (l<EPSILON) {
    d_v[0]=d_v[1]=d_v[2]=0.0f;
    d_v[3]=1.0f;
    return;
  }
  omega=-0.5f*angle;
  s=sin(omega)/l;
  d_v[0]=s*axis.d_v[0];
  d_v[1]=s*axis.d_v[1];
  d_v[2]=s*axis.d_v[2];
  d_v[3]=cos(omega);
}


/*!
 * Standard matrix - quaternion conversion.
 * The matrix is supposed to have its last column and row equal 
 * to (0,0,0,1) otherwise, the result is undefined.
 */
void
Math3d::MQuat::set(const M4x4& M)
{
  double tr,s;
  int i,j,k;
  static int nxt[3] = {1,2,0};

  tr = M.d_m[0][0] + M.d_m[1][1] + M.d_m[2][2];
  if (tr > 0.0) {
    s = sqrt(tr + 1.0);
    d_v[3] = s * 0.5;
    s = 0.5 / s;
    d_v[0] = (M.d_m[1][2] - M.d_m[2][1]) * s;
    d_v[1] = (M.d_m[2][0] - M.d_m[0][2]) * s;
    d_v[2] = (M.d_m[0][1] - M.d_m[1][0]) * s;
  }
  else {
    i = 0;
    if (M.d_m[1][1] > M.d_m[0][0]) i = 1;
    if (M.d_m[2][2] > M.d_m[i][i]) i = 2;
    j = nxt[i];
    k = nxt[j];
    s = sqrt((M.d_m[i][i]-(M.d_m[j][j]+M.d_m[k][k]))+1.0);
    d_v[i] = s * 0.5f;
    if (fabs(s)<EPSILON) {
      identity();
    }
    else {
      s = 0.5f /s;
      d_v[3] = (M.d_m[j][k] - M.d_m[k][j]) * s;
      d_v[j] = (M.d_m[i][j] + M.d_m[j][i]) * s;
      d_v[k] = (M.d_m[i][k] + M.d_m[k][i]) * s;
    }
  }
}


/*!
 *
 */
void 
Math3d::MQuat::copy(const MQuat& q)
{
  d_v[0]=q.d_v[0]; 
  d_v[1]=q.d_v[1]; 
  d_v[2]=q.d_v[2]; 
  d_v[3]=q.d_v[3];
}


/*!
 *
 */
double&
Math3d::MQuat::get(int i)
{
  ASSERT(i>=0 && i<4);
  return(d_v[i]);
}


/*!
 *
 */
Math3d::MQuat
Math3d::MQuat::operator*(const MQuat& q)  const
{
  return(MQuat(
    d_v[3]*q.d_v[0] + d_v[0]*q.d_v[3] + d_v[1]*q.d_v[2] - d_v[2]*q.d_v[1],
    d_v[3]*q.d_v[1] + d_v[1]*q.d_v[3] + d_v[2]*q.d_v[0] - d_v[0]*q.d_v[2],
    d_v[3]*q.d_v[2] + d_v[2]*q.d_v[3] + d_v[0]*q.d_v[1] - d_v[1]*q.d_v[0],
    d_v[3]*q.d_v[3] - d_v[0]*q.d_v[0] - d_v[1]*q.d_v[1] - d_v[2]*q.d_v[2]
  ));
}


/*!
 *
 */
const Math3d::MQuat&
Math3d::MQuat::operator*=(const MQuat& q)
{
  double px,py,pz,pw;

  px=d_v[0];
  py=d_v[1];
  pz=d_v[2];
  pw=d_v[3];
  d_v[0]=pw*q.d_v[0] + px*q.d_v[3] + py*q.d_v[2] - pz*q.d_v[1];
  d_v[1]=pw*q.d_v[1] + py*q.d_v[3] + pz*q.d_v[0] - px*q.d_v[2];
  d_v[2]=pw*q.d_v[2] + pz*q.d_v[3] + px*q.d_v[1] - py*q.d_v[0];
  d_v[3]=pw*q.d_v[3] - px*q.d_v[0] - py*q.d_v[1] - pz*q.d_v[2];
  return(*this);
}


/*!
 *
 */
Math3d::MQuat
Math3d::MQuat::operator*(double k)  const
{
  return(MQuat(
    d_v[0]*k,
    d_v[1]*k,
    d_v[2]*k,
    d_v[3]*k
  ));
}


/*!
 *
 */
const Math3d::MQuat&
Math3d::MQuat::operator*=(double k)
{
  d_v[0]*=k;
  d_v[1]*=k;
  d_v[2]*=k;
  d_v[3]*=k;
  return(*this);
}


/*!
 *
 */
void
Math3d::MQuat::neg()
{
  d_v[0]=-d_v[0];
  d_v[1]=-d_v[1];
  d_v[2]=-d_v[2];
  d_v[3]=-d_v[3];
}


/*!
 *
 */
void
Math3d::MQuat::abs()
{
  d_v[0]=fabs(d_v[0]);
  d_v[1]=fabs(d_v[1]);
  d_v[2]=fabs(d_v[2]);
  d_v[3]=fabs(d_v[3]);
}


/*!
 *
 */
void
Math3d::MQuat::cnj()
{
  d_v[0]=-d_v[0];
  d_v[1]=-d_v[1];
  d_v[2]=-d_v[2];
}


/*!
 *
 */
void
Math3d::MQuat::mul(const MQuat& p, const MQuat& q)
{
  d_v[0]=p.d_v[3]*q.d_v[0] + p.d_v[0]*q.d_v[3] + p.d_v[1]*q.d_v[2] - p.d_v[2]*q.d_v[1];
  d_v[1]=p.d_v[3]*q.d_v[1] + p.d_v[1]*q.d_v[3] + p.d_v[2]*q.d_v[0] - p.d_v[0]*q.d_v[2];
  d_v[2]=p.d_v[3]*q.d_v[2] + p.d_v[2]*q.d_v[3] + p.d_v[0]*q.d_v[1] - p.d_v[1]*q.d_v[0];
  d_v[3]=p.d_v[3]*q.d_v[3] - p.d_v[0]*q.d_v[0] - p.d_v[1]*q.d_v[1] - p.d_v[2]*q.d_v[2];
}


/*!
 *
 */
void
Math3d::MQuat::scalar(double k)
{
  d_v[0]*=k;
  d_v[1]*=k;
  d_v[2]*=k;
  d_v[3]*=k;
}


/*!
 *
 */
void
Math3d::MQuat::normalize()
{
  double l,c;

  l=sqrt(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2] + d_v[3]*d_v[3]);
  if (fabs(l)<EPSILON) {
    d_v[3]=1.0f; 
    d_v[0]=d_v[1]=d_v[2]=0.0f;
  }
  else {  
    c=1.0f/l;
    d_v[0]*=c;
    d_v[1]*=c;
    d_v[2]*=c;
    d_v[3]*=c;
  }
}


/*!
 *
 */
void
Math3d::MQuat::inv()
{
  double l,c;

  l=sqrt(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2] + d_v[3]*d_v[3]);
  if (fabs(l)<EPSILON) {
    d_v[0]=d_v[1]=d_v[2]=0.0f;
    d_v[3]=1.0f;
  }
  else {
    c=1.0f/l;
    d_v[0]*=-c;
    d_v[1]*=-c;
    d_v[2]*=-c;
    d_v[3]*=c;
  }
}


/*!
 *
 */
void
Math3d::MQuat::ln()
{
  double om,s,c;

  s=sqrt(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2]);
  om=atan2(s,d_v[3]);
  if (fabs(s)<EPSILON) {
    c=0.0f;
  }
  else {
    c=om/s;
  }
  d_v[0]=d_v[0]*c;
  d_v[1]=d_v[1]*c;
  d_v[2]=d_v[2]*c;
  d_v[3]=0.0f;
}


/*!
 *
 */
void
Math3d::MQuat::lnDif(const MQuat& p, const MQuat& q)
{
  MQuat invp;

  invp=p;
  invp.inv();
  mul(invp,q);
  ln();
}


/*!
 *
 */
void
Math3d::MQuat::exp()
{
  double om,sinom;

  om=sqrt(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2]);
  if (fabs(om)<EPSILON) {
    sinom=1.0f;
  }
  else {
    sinom=sin(om)/om;
  }
  d_v[0]*=sinom;
  d_v[1]*=sinom;
  d_v[2]*=sinom;
  d_v[3]=cos(om);
}


/*!
 *
 */
void
Math3d::MQuat::slerp(const MQuat& p, const MQuat& q, double t)
{
  double l;
  double om,sinom;
  double sp,sq;
  MQuat qq;

  l=p.d_v[0]*q.d_v[0] + p.d_v[1]*q.d_v[1] + p.d_v[2]*q.d_v[2] + p.d_v[3]*q.d_v[3];
  if ((1.0+l)>EPSILON) {
    if (fabs(l)>1.0) l/=fabs(l);
    om=acos(l);
    sinom=sin(om);
    if (fabs(sinom)>EPSILON) {
      sp=sin((1.0f-t)*om)/sinom;
      sq=sin(t*om)/sinom;
    }
    else {
      sp=1.0f-t;
      sq=t;
    }
    d_v[0]=sp*p.d_v[0] + sq*q.d_v[0];
    d_v[1]=sp*p.d_v[1] + sq*q.d_v[1];
    d_v[2]=sp*p.d_v[2] + sq*q.d_v[2];
    d_v[3]=sp*p.d_v[3] + sq*q.d_v[3];
  }
  else {
    qq.d_v[0]=-p.d_v[1];
    qq.d_v[1]=p.d_v[0];
    qq.d_v[2]=-p.d_v[3];
    qq.d_v[3]=p.d_v[2];
    sp=sin((1.0-t)*HALFPI);
    sq=sin(t*HALFPI);
    d_v[0]=sp*p.d_v[0] + sq*qq.d_v[0];
    d_v[1]=sp*p.d_v[1] + sq*qq.d_v[1];
    d_v[2]=sp*p.d_v[2] + sq*qq.d_v[2];
    d_v[3]=sp*p.d_v[3] + sq*qq.d_v[3];
  }
}


/*!
 *
 */
void
Math3d::MQuat::squad(const MQuat& p, const MQuat& Tp, const MQuat& Tq, const MQuat& q, double t)
{
  MQuat pq,ab;

  pq.slerp(p,q,t);
  ab.slerp(Tp,Tq,t);
  slerp(pq,ab,2*t*(1-t));
}


/*!
 *
 */
void
Math3d::MQuat::tangent(const MQuat& qp, const MQuat& q, const MQuat& qn)
{
  MQuat invq, dn,dp,x;
  int i;

  invq=q;
  invq.inv();

  dn.lnDif(q,qn);
  dp.lnDif(q,qp);
  for (i=0; i<4; i++) {
    x.d_v[i]=-1.0/4.0*(dn[i]+dp[i]);
  }
  x.exp();
  mul(q,x);
}


static double
project_to_sphere(double r, double x, double y)
{
  double d, t, z;

  d = sqrt(x*x + y*y);
  if (d < r * 0.70710678118654752440) {    
    // Inside sphere 
    z = sqrt(r*r - d*d);
  } else { 
    // On hyperbola
    t = r / 1.41421356237309504880;
    z = t*t / d;
  }
  return(z);
}


/*!
 * Implementation of a virtual trackball.
 * Implemented by Gavin Bell, lots of ideas from Thant Tessman and
 *   the August '88 issue of Siggraph's "Computer Graphics," pp. 121-129.
 *
 * Simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic sheet of rotation away from the
 * center.  This particular function was chosen after trying out
 * several variations.
 *
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
void
Math3d::MQuat::trackball(const M2d& p, const M2d q)
{
  M3d a;           // Axis of rotation
  double phi;      // how much to rotate about axis
  M3d p1, p2, d;
  double t;

  if ((fabs(p[0]-q[0])<EPSILON) && (fabs(p[1]-q[1])<EPSILON)) {
    //Zero rotation
    identity();
    return;
  }

  // First, figure out z-coordinates for projection of P1 and P2 to
  // deformed sphere
  p1.set(p[0], project_to_sphere(0.8,p[0],p[1]), p[1]);
  p2.set(q[0], project_to_sphere(0.8,q[0],q[1]), q[1]);
  
  // Now, we want the cross product of P1 and P2
  a.cross(p2,p1);
  
  // Figure out how much to rotate around that axis.
  d.sub(p1,p2);
  t = d.length() / (2.0*0.8);
  
  // Avoid problems with out-of-control values...
  if (t > 1.0) t = 1.0;
  if (t < -1.0) t = -1.0;
  phi = 2.0 * asin(t);

  set(a,phi);
}


/*!
 *
 */
double
Math3d::MQuat::get(int i) const
{
  ASSERT(i>=0 && i<4);
  return(d_v[i]);
}


/*!
 *
 */
bool
Math3d::MQuat::operator==(const MQuat& q) const
{
  return(cmp(q));
}


/*!
 *
 */
bool
Math3d::MQuat::operator!=(const MQuat& q) const
{
  return(!cmp(q));
}


/*!
 *
 */
double
Math3d::MQuat::dot(const MQuat& q) const
{
  return(d_v[0]*q.d_v[0] + d_v[1]*q.d_v[1] + d_v[2]*q.d_v[2] + d_v[3]*q.d_v[3]);
}


/*!
 *
 */
bool
Math3d::MQuat::cmp(const MQuat& q, double epsilon) const
{
  return(
    (fabs(d_v[0]-q.d_v[0])<epsilon) &&
    (fabs(d_v[1]-q.d_v[1])<epsilon) &&
    (fabs(d_v[2]-q.d_v[2])<epsilon) &&
    (fabs(d_v[3]-q.d_v[3])<epsilon)
  );
}


/*!
 *
 */
double
Math3d::MQuat::squared() const
{
  return(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2] + d_v[3]*d_v[3]);
}


/*!
 *
 */
double
Math3d::MQuat::length() const
{
  return(sqrt(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2] + d_v[3]*d_v[3]));
}


/*!
 *  
 */
ostream& 
Math3d::operator << (ostream& co, const MQuat& q)
{
  co << "(" << q[0] << ", " << q[1] << ", " << q[2] << "," << q[3] << ")";
  return co;
}