// ------------------------------------------------------------------------
  // This program is complementary material for the book:
  //
  // Frank Nielsen
  //
  // Visual Computing: Geometry, Graphics, and Vision
  //
  // ISBN: 1-58450-427-7
  //
  // Charles River Media, Inc.
  //
  //
  // All programs are available at www.charlesriver.com/visualcomputing/
  //
  // You may use this program for ACADEMIC and PERSONAL purposes ONLY. 
  //
  //
  // The use of this program in a commercial product requires EXPLICITLY
  // written permission from the author. The author is NOT responsible or 
  // liable for damage or loss that may be caused by the use of this program. 
  //
  // Copyright (c) 2005. Frank Nielsen. All rights reserved.
  // ------------------------------------------------------------------------
   
  // ------------------------------------------------------------------------
  // File: slerp.cpp
  // 
  // Description: Spherical linear interpolation demo program in OpenGL(R)
  // ------------------------------------------------------------------------
  
  include <stdafx.h>
  
  include <windows.h>
  include <math.h>
  include <GL/gl.h>
  include <GL/glut.h>
  
  using namespace std;
  
  define W 800
  define H 800
  
  inline double drand() {return rand()/(double)RAND_MAX;}
  
  define M_PI 3.14159265
  define toRad(x) ((x)*(M_PI/180.0))
  
  class Point3D
  {
  public:
          double x,y,z;
  };
  
  class Quaternion{
  public:
          double w;
          Point3D u;
  
          inline void Multiply(const Quaternion q)
                  {
                          Quaternion tmp;
                          tmp.u.x = ((w * q.u.x) + (u.x * q.w) + (u.y * q.u.z) - (u.z * q.u.y));
                          tmp.u.y = ((w * q.u.y) - (u.x * q.u.z) + (u.y * q.w) + (u.z * q.u.x));
                          tmp.u.z = ((w * q.u.z) + (u.x * q.u.y) - (u.y * q.u.x) + (u.z * q.w));
                          tmp.w = ((w * q.w) - (u.x * q.u.x) - (u.y * q.u.y) - (u.z * q.u.z));
  			*this = tmp;
                  }
  
          inline double Norm()
          {return sqrt(u.x*u.x+u.y*u.y+u.z*u.z+w*w);}
  
          inline void Normalize()
          {
          double norm=Norm();
          u.x/=norm;u.y/=norm;u.z/=norm;
          }
  
          inline void Conjugate()
          {
          u.x=-u.x;
          u.y=-u.y;
          u.z=-u.z;
          }
  
          inline void Inverse()
          {
          double norm=Norm();
          Conjugate();
          u.x/=norm;
          u.y/=norm;
          u.z/=norm;
          w/=norm;
          }
  
          void ExportToMatrix(float matrix[16]) 
  {
          float wx, wy, wz, xx, yy, yz, xy, xz, zz;
          // adapted from Shoemake
          xx = u.x * u.x;
          xy = u.x * u.y;
          xz = u.x * u.z;
          yy = u.y * u.y;
          zz = u.z * u.z;
          yz = u.y * u.z;
  
          wx = w * u.x;
          wy = w * u.y;
          wz = w * u.z;
  
          matrix[0] = 1.0f - 2.0f*(yy + zz);
          matrix[4] = 2.0f*(xy - wz);
          matrix[8] = 2.0f*(xz + wy);
          matrix[12] = 0.0;
   
          matrix[1] = 2.0f*(xy + wz);
          matrix[5] = 1.0f - 2.0f*(xx + zz);
          matrix[9] = 2.0f*(yz - wx);
          matrix[13] = 0.0;
  
          matrix[2] = 2.0f*(xz - wy);
          matrix[6] = 2.0f*(yz + wx);
          matrix[10] = 1.0f - 2.0f*(xx + yy);
          matrix[14] = 0.0;
  
          matrix[3] = 0;
          matrix[7] = 0;
          matrix[11] = 0;
          matrix[15] = 1;
  }
          
  };
  
  Quaternion RotateAboutAxis(Point3D pt, double angle, Point3D axis)
          {
          Quaternion q,p, qinv;
  
          q.w=cos(0.5*angle);
          q.u.x=sin(0.5*angle)*axis.x;
          q.u.y=sin(0.5*angle)*axis.y;
          q.u.z=sin(0.5*angle)*axis.z;
  
          p.w=0;
          p.u=pt;
  
          qinv=q;
          qinv.Inverse();
  
          q.Multiply(p);
          q.Multiply(qinv);
  
          return q;
          }
  
  void Slerp(Quaternion q1, Quaternion q2, Quaternion &qr , double lambda) 
  {
          float dotproduct = q1.u.x * q2.u.x + q1.u.y * q2.u.y + q1.u.z * q2.u.z + q1.w * q2.w;
          float theta, st, sut, sout, coeff1, coeff2;
  
          // algorithm adapted from Shoemake's paper
   lambda=lambda/2.0;
  
          theta = (float) acos(dotproduct);
          if (theta<0.0) theta=-theta;
          
          st = (float) sin(theta);
          sut = (float) sin(lambda*theta);
          sout = (float) sin((1-lambda)*theta);
          coeff1 = sout/st;
          coeff2 = sut/st;
  
          qr.u.x = coeff1*q1.u.x + coeff2*q2.u.x;
          qr.u.y = coeff1*q1.u.y + coeff2*q2.u.y;
          qr.u.z = coeff1*q1.u.z + coeff2*q2.u.z;
          qr.w = coeff1*q1.w + coeff2*q2.w;
  
          qr.Normalize();
  }
  
  Point3D p,q,Rp;
  
  double lambdaanim=0.0;
  
  void MultiplyPointMatrix(float m[16], Point3D p, Point3D& rotp)
  {
  rotp.x=m[0]*p.x+m[1]*p.y+m[2]*p.z;
  rotp.y=m[4]*p.x+m[5]*p.y+m[6]*p.z;
  rotp.z=m[8]*p.x+m[9]*p.y+m[10]*p.z;
  }
  
  void display(void)
  {
    GLfloat m[16];
    char buffer[256];
    double s=1.0;
  
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
  
    glColor3f(0.8,0.8,0.8);
    glutSolidSphere(0.99,32,32);
  
    glColor3f(1,1,0);
    glBegin(GL_LINES);
    glVertex3f(0,0,0);
    glVertex3f(0,2,0);
  
    glVertex3f(0,0,0);
    glVertex3f(2,0,0);
  
    glVertex3f(0,0,0);
    glVertex3f(0,0,2);
    glEnd();
  
    glColor3f(0,0,1);
    glPointSize(10);
    glBegin(GL_POINTS);
    glVertex3f(p.x,p.y,p.z);
    glEnd();
  
    glColor3f(1,0,1);
    glBegin(GL_POINTS);
    glVertex3f(q.x,q.y,q.z);
    glEnd();
  
  Quaternion quatp,quatq, quatr;
  quatp.u=p;quatp.w=0;
  quatq.u=q;quatq.w=0;
  
  glColor3f(0,1,0);
  glPointSize(1.0);
  
  glBegin(GL_LINE_STRIP);
  for(double lambda=0;lambda<=1.01;lambda+=0.01)
  {
          Slerp(quatp, quatq, quatr,lambda);
          quatr.ExportToMatrix(m);
          MultiplyPointMatrix(m,p,Rp);
          glVertex3f(s*Rp.x,s*Rp.y,s*Rp.z);
  }
  glEnd();
  
  lambdaanim+=0.01; 
  if (lambdaanim>1.0) lambdaanim=0.0;
  
  Slerp(quatp, quatq, quatr,lambdaanim);
  quatr.ExportToMatrix(m);
  MultiplyPointMatrix(m,p,Rp);
  
  glPointSize(5);
  glColor3f(1,0,0);
  glBegin(GL_POINTS);
  glVertex3f(s*Rp.x,s*Rp.y,s*Rp.z);
  glEnd();
  
  glMatrixMode(GL_PROJECTION);
  glPushMatrix();
  
    glLoadIdentity();
    glOrtho (0.0, W, 0.0, H, -1.0, 1.0);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
  
    glColor3f (0, 0, 0);
  
  glRasterPos2f(50,30);
  sprintf(buffer,"Spherical Linear Interpolation of Quaternions (L=%.3f)",lambdaanim);
     for(int i=0;buffer[i]!=0;i++)
     glutBitmapCharacter(GLUT_BITMAP_TIMES_ROMAN_24, buffer[i]);
  
    glRasterPos2f(50,H-30);
  sprintf(buffer,"Press any key to initialize another pair of points.");
     for(int i=0;buffer[i]!=0;i++)
     glutBitmapCharacter(GLUT_BITMAP_TIMES_ROMAN_24, buffer[i]);
  
    glMatrixMode(GL_PROJECTION);
  glPopMatrix();
  
    
  
   glFlush();
   glutSwapBuffers();
  }
  
  void reshape(int w, int h)
  {
    GLfloat aspect = (GLfloat) w / (GLfloat) h;
    glViewport(0, 0, w, h);
  
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    
    if (w <= h)
      glOrtho(-1.25, 1.25, -1.25 * aspect, 1.25 * aspect, -2.0, 2.0);
    else
      glOrtho(-1.25 * aspect, 1.25 * aspect, -1.25, 1.25, -2.0, 2.0);
          
  
    glutPostRedisplay();
  }
  
  inline void Spherical2Cartesian(double t,double p,double &X, double &Y, double &Z)
  {
  X=cos(p)*sin(t);
  Y=sin(p);
  Z=cos(p)*cos(t);        
  }
  
  void keyboard(unsigned char key, int x, int y)
  {
    if (key=='q') exit(0);
  
  // theta phi of point p
    Spherical2Cartesian(M_PI*drand()*2.0,-M_PI/2.0+M_PI*drand(),p.x,p.y,p.z);
  
    // theta, phi of point q
    Spherical2Cartesian(M_PI*drand()*2.0,-M_PI/2.0+M_PI*drand(),q.x,q.y,q.z);
  
    glutPostRedisplay();
  }
  
  int _tmain(int argc, _TCHAR* argv[])
  {
  
    srand(1234);
  
    
          cout<<"Visual Computing: Geometry, Graphics, and Vision (ISBN:1-58450-427-7)"<<endl;
          cout<<"Demo program\n\n"<<endl;
  
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE| GLUT_DEPTH); 
    glutInitWindowSize(W,H);
    glutCreateWindow("Quaternion SLERP");
  
    glutReshapeFunc(reshape);
    glutDisplayFunc(display);
    glutKeyboardFunc(keyboard);
  
    glClearColor(1.0, 1.0, 1.0, 0.0);
    glEnable(GL_DEPTH_TEST);
  
    glutIdleFunc(display);
  
    keyboard(0,0,0);
    glutMainLoop();
          return 0;
  }