/*
 * CQuat.cpp
 * $Id: CQuat.cpp,v 1.2 2001/11/15 16:54:52 guenth Exp $
 *
 * Copyright (C) 1999, 2000 Michael Meissner
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * As a special exception to the GPL, the QGLViewer authors (Markus
 * Janich, Michael Meissner, Richard Guenther, Alexander Buck and Thomas
 * Woerner) give permission to link this program with Qt (non-)commercial
 * edition, and distribute the resulting executable, without including
 * the source code for the Qt (non-)commercial edition in the source
 * distribution.
 *
 */

#include "CQuat.h"
#include "CMat4D.h"
#include "CV3D.h"

#ifdef WIN32
#define M_PI  3.14159265359
#endif

// -------------- Konstruktoren ---------------

CQuat::CQuat()
{
w=x=y=z=0;
}

CQuat::CQuat(double qW, double qX, double qY, double qZ)
{
  w=qW;
  x=qX;
  y=qY;
  z=qZ;
}

  
CQuat::CQuat(CMat4D &mat)
{

/* naiver Ansatz
  w=0.5*sqrt( mat(0,0)+mat(1,1)+mat(2,2) + 1);
  x=(mat(2,1)-mat(1,2))/(4*w);
  y=(mat(0,2)-mat(2,0))/(4*w);
  z=(mat(1,0)-mat(0,1))/(4*w);
*/

/* besser : Watt & Watt S.363 */
/* as a Difference to Watt and Watt all the indizes of
   the matrix have had to be switched, since the obms matrix works
   internally with indexing first the row and second the column*/

 double trace=mat(0,0)+mat(1,1)+mat(2,2);
 if (trace>0){
  double s;
  s=sqrt( trace + 1 );
  w=s*0.5;
  s=0.5/s;
  x=(mat(2,1)-mat(1,2))*s;
  y=(mat(0,2)-mat(2,0))*s;
  z=(mat(1,0)-mat(0,1))*s;
 }
 else
 {
  #define X_c 0
  #define Y_c 1
  #define Z_c 2
  int next[3]={Y_c,Z_c,X_c};
  int i=X_c;
  if (mat(Y_c,Y_c)>mat(X_c,X_c)) i=Y_c;
  if (mat(Z_c,Z_c)>mat(i,i)) i=Z_c;
  int j=next[i];
  int k=next[j];
 
  double s; 
  s=sqrt( (mat(i,i)- 
          (mat(j,j) + mat(k,k)) + 1) );
 
  double q[3];
  q[i]=s*0.5;
  s=0.5/s;
  w=   (mat(k,j) - mat(j,k))*s;
  q[j]=(mat(j,i) + mat(i,j))*s;
  q[k]=(mat(k,i) + mat(i,k))*s;
  #undef X_c 
  #undef Y_c 
  #undef Z_c
  x=q[0];
  y=q[1];
  z=q[2];
 }
}

CQuat::CQuat(double qW, CV3D &v)
{
  w=qW;
  x=v[0];
  y=v[1];
  z=v[2];
}

// Initialisierung
CQuat::CQuat(const CQuat& q)
{
  w=q.w;x=q.x;y=q.y;z=q.z;
}

// Zuweisung
void CQuat::operator=(const CQuat& q)
{
  w=q.w;x=q.x;y=q.y;z=q.z;
}


// ------------  Additions- Operatoren --------------------

CQuat CQuat::operator+(CQuat& b)
{
  CQuat c;
  c.w=w+b.w;
  c.x=x+b.x;
  c.y=y+b.y;
  c.z=z+b.z;
  return c;
} 

CQuat CQuat::operator-(CQuat& b)
{
  CQuat c;
  c.w=w-b.w;
  c.x=x-b.x;
  c.y=y-b.y;
  c.z=z-b.z;
  return c;
}

CQuat CQuat::operator-()
{
  CQuat c;
  c.w= -w;
  c.x= -x;
  c.y= -y;
  c.z= -z;
  return c;
}

// Multiplikationen
CQuat CQuat::operator*(CQuat& b) // Fastmul-Alg. siehe Seidel-Paper p.4 
{
  CQuat c;
  double s[9],t;
  s[0] = (z-y)*(b.y-b.z);
  s[1] = (w+x)*(b.w+b.x);
  s[2] = (w-x)*(b.y+b.z);
  s[3] = (z+y)*(b.w-b.x);
  s[4] = (z-x)*(b.x-b.y);
  s[5] = (z+x)*(b.x+b.y);
  s[6] = (w+y)*(b.w-b.z);
  s[7] = (w-y)*(b.w+b.z);
  s[8] = s[5]+s[6]+s[7];
  
  t    = (s[4] +s[8])/2;
  
  c.w=s[0]+t-s[5];
  c.x=s[1]+t-s[8];
  c.y=s[2]+t-s[7];
  c.z=s[3]+t-s[6];
  
  return c;
}

CQuat CQuat::operator*(double& a)
{
  CQuat r; 
  r.w=a*w; r.x=a*x;r.y=a*y;r.z=a*z; 
  return r; 
}

// Konjugation
CQuat CQuat::conj()
{
  CQuat c;
  c.w=  w;
  c.x= -x;
  c.y= -y;
  c.z= -z;
  return c;
}

// Konjugation
CQuat CQuat::inv()
{
  CQuat c = *this;
  c.normalize();
  return c.conj();
}


// Skalarprodukt
double CQuat::operator|(CQuat& q)
{
  double s;
  s=w*q.w+x*q.x+y*q.y+z*q.z;
  return s;
}

// Norm
double CQuat::norm()
{
  double n;
  n=sqrt(w*w+x*x+y*y+z*z);
  return n;
}

// Normalisieren
CQuat CQuat::normalize() 
{
  CQuat q;
  double n= sqrt(w*w+x*x+y*y+z*z);
  double f;
  
  if(n != 0){
    f=1/n;
    q.w=f*w;
    q.x=f*x;
    q.y=f*y;
    q.z=f*z;
  }
  return q;
} 

// Zugriff auf Real- bzw. Imaginaerteil
double CQuat::re()
{
  return w;
}

CV3D CQuat::im()
{
  CV3D v(x,y,z);
  return v;
}

// Ausgabe
void CQuat::print()
{
  cout<<"("<<w<<","<<x<<","<<y<<","<<z<<")\n";
}

// Beschleunigte Berechnung von Rotationen
CQuat CQuat::QVQ(CQuat& Q)
{
  double w,x,y,z,Qx,Qy,Qz,Qw,Vx,Vy,Vz,Vw;
  CV3D vec;

  Qw=Q.re();
  vec=Q.im();
  Qx=vec[0];
  Qy=vec[1];
  Qz=vec[2];
  
  Vw=re();
  vec=im();
  Vx=vec[0];
  Vy=vec[1];
  Vz=vec[2];
  
  w= (Qw*Vw-Qx*Vx-Qy*Vy-Qz*Vz)*Qw+(Qx*Vw+Qw*Vx-Qz*Vy+Qy*Vz)*Qx+
    (Qy*Vw+Qz*Vx+Qw*Vy-Qx*Vz)*Qy+(Qz*Vw-Qy*Vx+Qx*Vy+Qw*Vz)*Qz;
  x= (Qx*Vw+Qw*Vx-Qz*Vy+Qy*Vz)*Qw-(Qw*Vw-Qx*Vx-Qy*Vy-Qz*Vz)*Qx+
    (Qz*Vw-Qy*Vx+Qx*Vy+Qw*Vz)*Qy-(Qy*Vw+Qz*Vx+Qw*Vy-Qx*Vz)*Qz;
  y= (Qy*Vw+Qz*Vx+Qw*Vy-Qx*Vz)*Qw-(Qz*Vw-Qy*Vx+Qx*Vy+Qw*Vz)*Qx-
    (Qw*Vw-Qx*Vx-Qy*Vy-Qz*Vz)*Qy+(Qx*Vw+Qw*Vx-Qz*Vy+Qy*Vz)*Qz;
  z= (Qz*Vw-Qy*Vx+Qx*Vy+Qw*Vz)*Qw+(Qy*Vw+Qz*Vx+Qw*Vy-Qx*Vz)*Qx-
    (Qx*Vw+Qw*Vx-Qz*Vy+Qy*Vz)*Qy-(Qw*Vw-Qx*Vx-Qy*Vy-Qz*Vz)*Qz;
  
  return CQuat(w,x,y,z);
}

CV3D CQuat::rotate(CV3D& vec)
{
  CQuat V=CQuat(0,vec[0],vec[1],vec[2]);
  CQuat erg=V.QVQ(*this);
  return CV3D(erg.x,erg.y,erg.z);
}


// ------------   Friend-Funktionen -----------------------

// Erzeugen einer Rotationsmatrix aus einem Einheits-CQuaternion
/*
Mat  rotmat(const V3D& v, double phi)
{
  Mat mat(3,3);
  V3D v2;
  
  v2= v.getNormalized()*sin(phi/2.0);
 
  double c=  cos(phi/2.0);
  CQuat Q(c,v2.x(),v2.y(),v2.z());
  mat.setCoeff(0, 0, 1-2*Q.y*Q.y-2*Q.z*Q.z);
  mat.setCoeff(0, 1, 2*Q.x*Q.y-2*Q.w*Q.z);
  mat.setCoeff(0, 2, 2*Q.x*Q.z+2*Q.w*Q.y);
  mat.setCoeff(1, 0, 2*Q.x*Q.y+2*Q.w*Q.z);
  mat.setCoeff(1, 1, 1-2*Q.x*Q.x-2*Q.z*Q.z);
  mat.setCoeff(1, 2, 2*Q.y*Q.z-2*Q.w*Q.x);
  mat.setCoeff(2, 0, 2*Q.x*Q.z-2*Q.w*Q.y);
  mat.setCoeff(2, 1, 2*Q.y*Q.z+2*Q.w*Q.x);
  mat.setCoeff(2, 2, 1-2*Q.x*Q.x-2*Q.y*Q.y);
  return mat;
}

Mat  CQuat::getRotMat()
{
  Mat mat(3,3);
  CQuat Q=this->normalize();
  mat.setCoeff(0, 0, 1-2*Q.y*Q.y-2*Q.z*Q.z);
  mat.setCoeff(0, 1, 2*Q.x*Q.y-2*Q.w*Q.z);
  mat.setCoeff(0, 2, 2*Q.x*Q.z+2*Q.w*Q.y);
  mat.setCoeff(1, 0, 2*Q.x*Q.y+2*Q.w*Q.z);
  mat.setCoeff(1, 1, 1-2*Q.x*Q.x-2*Q.z*Q.z);
  mat.setCoeff(1, 2, 2*Q.y*Q.z-2*Q.w*Q.x);
  mat.setCoeff(2, 0, 2*Q.x*Q.z-2*Q.w*Q.y);
  mat.setCoeff(2, 1, 2*Q.y*Q.z+2*Q.w*Q.x);
  mat.setCoeff(2, 2, 1-2*Q.x*Q.x-2*Q.y*Q.y);
  return mat;
}
*/