// Template and inlines implementing quaternion number support for C++
// Written by Boris Burger (flashp@users.sourceforge.net)
// updated 14 Jul 2000

#ifndef __QUATERNION__
#define __QUATERNION__

#ifdef __GNUG__
#pragma interface
#endif

#if !defined (__GNUG__) && !defined (__attribute__)
#define __attribute__(foo) /* Ignore.  */
#endif

#include <iostream>
#include <cmath>

extern "C++" {

template <class T> class quaternion;
template <class T> quaternion<T>&
  __doapl (quaternion<T>* ths, const quaternion<T>& r);
template <class T> quaternion<T>&
  __doami (quaternion<T>* ths, const quaternion<T>& r);
template <class T> quaternion<T>&
  __doaml (quaternion<T>* ths, const quaternion<T>& r);

template <class T> class quaternion
{
private:

  mutable T a0, a1, a2, a3;

public:

  // constructor

  quaternion(T x0=0, T x1=0, T x2=0, T x3=0):
             a0 (x0), a1 (x1), a2 (x2), a3 (x3) { }

  // function prototypes

  quaternion& operator += (const quaternion&);
  quaternion& operator -= (const quaternion&);
  quaternion& operator *= (const quaternion&);

  friend quaternion& __doapl<> (quaternion *, const quaternion&);
  friend quaternion& __doami<> (quaternion *, const quaternion&);
  friend quaternion& __doaml<> (quaternion *, const quaternion&);

  T re () const { return a0; }
  T im1 () const { return a1; }
  T im2 () const { return a2; }
  T im3 () const { return a3; }
};

// specializations

template class quaternion<float>;
template class quaternion<double>;
template class quaternion<long double>;

// aliases

typedef quaternion<float> float_quaternion;
typedef quaternion<double> double_quaternion;
typedef quaternion<long double> long_double_quaternion;

// function definitions

template <class T>
ostream& operator<<(ostream& s, const quaternion<T>& q)
  {
    return(s<<"("<<q.re()<<","<<q.im1()<<","<<q.im2()<<","<<q.im3()<<")");
  }

template <class T>
inline quaternion<T>&
__doapl (quaternion<T>* ths, const quaternion<T>& r)
{
  ths->a0 += r.a0;
  ths->a1 += r.a1;
  ths->a2 += r.a2;
  ths->a3 += r.a3;
  return *ths;
}
template <class T>
inline quaternion<T>&
quaternion<T>::operator += (const quaternion<T>& r)
{
  return __doapl (this, r);
}

template <class T>
inline quaternion<T>&
__doami (quaternion<T>* ths, const quaternion<T>& r)
{
  ths->a0 -= r.a0;
  ths->a1 -= r.a1;
  ths->a2 -= r.a2;
  ths->a3 -= r.a3;
  return *ths;
}
template <class T>
inline quaternion<T>&
quaternion<T>::operator -= (const quaternion<T>& r)
{
  return __doami (this, r);
}

template <class T>
inline quaternion<T>&
__doaml (quaternion<T>* ths, const quaternion<T>& r)
{
  T aa0 = ths->a0*r.a0 - ths->a1*r.a1 - ths->a2*r.a2 - ths->a3*r.a3;
  T aa1 = ths->a0*r.a1 + ths->a1*r.a0 + ths->a2*r.a3 - ths->a3*r.a2;
  T aa2 = ths->a0*r.a2 + ths->a2*r.a0 + ths->a3*r.a1 - ths->a1*r.a3;
  T aa3 = ths->a0*r.a3 + ths->a3*r.a0 + ths->a1*r.a2 - ths->a2*r.a1;
  ths->a0 = aa0;
  ths->a1 = aa1;
  ths->a2 = aa2;
  ths->a3 = aa3;
  return *ths;
}

template <class T>
inline quaternion<T>&
quaternion<T>::operator *= (const quaternion<T>& r)
{
  return __doaml (this, r);
}

template <class T> inline T
re (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline T
re (const quaternion<T>& x)
{
  return x.re ();
}

template <class T> inline T
im1 (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline T
im1 (const quaternion<T>& x)
{
  return x.im1 ();
}

template <class T> inline T
im2 (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline T
im2 (const quaternion<T>& x)
{
  return x.im2 ();
}

template <class T> inline T
im3 (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline T
im3 (const quaternion<T>& x)
{
  return x.im3 ();
}


template <class T> inline quaternion<T>
operator + (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator + (const quaternion<T>& x, const quaternion<T>& y)
{
  return quaternion<T> ( re(x)+re(y), im1(x)+im1(y),
                            im2(x)+im2(y), im3(x)+im3(y));
}

template <class T> inline quaternion<T>
operator + (const quaternion<T>& x, T y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator + (const quaternion<T>& x, T y)
{
  return quaternion<T> (re(x)+y, im1(x), im2(x), im3(x));
}

template <class T> inline quaternion<T>
operator + (T x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator + (T x, const quaternion<T>& y)
{
  return quaternion<T> (x + re(y), im1(y), im2(y), im3(y));
}

template <class T> inline quaternion<T>
operator - (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator - (const quaternion<T>& x, const quaternion<T>& y)
{
  return quaternion<T> ( re(x)-re(y), im1(x)-im1(y),
                            im2(x)-im2(y), im3(x)-im3(y));
}

template <class T> inline quaternion<T>
operator - (const quaternion<T>& x, T y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator - (const quaternion<T>& x, T y)
{
  return quaternion<T> (re(x) - y, im1 (x), im2 (x), im3 (x));
}

template <class T> inline quaternion<T>
operator - (T x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator - (T x, const quaternion<T>& y)
{
  return quaternion<T> (x - re (y), - im1 (y), - im2 (y), - im3 (y));
}

template <class T> inline quaternion<T>
operator * (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator * (const quaternion<T>& x, const quaternion<T>& y)
{
  return quaternion<T>
             (re(x)*re(y) - im1(x)*im1(y) - im2(x)*im2(y) - im3(x)*im3(y), 
              re(x)*im1(y) + im1(x)*re(y) + im2(x)*im3(y) - im3(x)*im2(y), 
              re(x)*im2(y) + im2(x)*re(y) + im3(x)*im1(y) - im1(x)*im3(y), 
              re(x)*im3(y) + im3(x)*re(y) + im1(x)*im2(y) - im2(x)*im1(y) );
}

template <class T> inline quaternion<T>
operator * (const quaternion<T>& x, T y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator * (const quaternion<T>& x, T y)
{
  return quaternion<T> (re (x) * y, im1 (x) * y, im2 (x) * y, im3 (x) * y);
}

template <class T> inline quaternion<T>
operator * (T x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline quaternion<T>
operator * (T x, const quaternion<T>& y)
{
  return quaternion<T> (x * re (y), x * im1 (y), x * im2 (y), x * im3 (y));
}

template <class T> quaternion<T>
operator / (const quaternion<T>& x, T y) __attribute__ ((const));

template <class T> quaternion<T>
operator / (const quaternion<T>& x, T y)
{
  return quaternion<T> (re (x) / y, im1 (x) / y, im2 (x) / y, im3 (x) / y);
}

template <class T> inline quaternion<T>
operator + (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline quaternion<T>
operator + (const quaternion<T>& x)
{
  return x;
}

template <class T> inline quaternion<T>
operator - (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline quaternion<T>
operator - (const quaternion<T>& x)
{
  return quaternion<T> (-re (x), -im1 (x), -im2 (x), -im3 (x));
}


template <class T> inline bool
operator == (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline bool
operator == (const quaternion<T>& x, const quaternion<T>& y)
{
  return re(x)==re(y) && im1(x)==im1(y) && im2(x)==im2(y) && im3(x)==im3(y);
}

template <class T> inline bool
operator == (const quaternion<T>& x, T y) __attribute__ ((const));

template <class T> inline bool
operator == (const quaternion<T>& x, T y)
{
  return re(x)==y && im1(x) == 0 && im2(x) == 0 && im3(x) == 0;
}

template <class T> inline bool
operator == (T x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline bool
operator == (T x, const quaternion<T>& y)
{
  return x==re(y) && im1(y)==0 && im2(y)==0 && im3(y)==0;
}

template <class T> inline bool
operator != (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline bool
operator != (const quaternion<T>& x, const quaternion<T>& y)
{
  return re(x)!=re(y) || im1(x)!=im1(y) || im2(x)!=im2(y) || im3(x)!=im3(y);
}

template <class T> inline bool
operator != (const quaternion<T>& x, T y) __attribute__ ((const));

template <class T> inline bool
operator != (const quaternion<T>& x, T y)
{
  return re(x)!=y || im1(x)!=0 || im2(x)!=0 || im3(x)!=0;
}

template <class T> inline bool
operator != (T x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline bool
operator != (T x, const quaternion<T>& y)
{
  return x!=re(y) || im1(y)!=0 || im2(y)!=0 || im3(y)!=0;
}

template <class T> inline T
abs (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline T
abs (const quaternion<T>& x)
{
  return sqrt(re(x)*re(x) + im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x));
}

template <class T> inline T
arg (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline T
arg (const quaternion<T>& x)
{
  return atan2(sqrt(im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x)), re(x));
}

template <class T> inline quaternion<T>
conj (const quaternion<T>& x)  __attribute__ ((const));

template <class T> inline quaternion<T>
conj (const quaternion<T>& x) 
{
  return quaternion<T> (re(x), -im1(x), -im2(x), -im3(x));
}

template <class T> inline T
norm (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline T
norm (const quaternion<T>& x)
{
  return re(x)*re(x) + im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x);
}

template <class T> inline T
dot (const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline T
dot (const quaternion<T>& x, const quaternion<T>& y)
{
  return re(x)*re(y) + im1(x)*im1(y) + im2(x)*im2(y) + im3(x)*im3(y);
}

template <class T> inline quaternion<T>
slerp (T t, const quaternion<T>& x, const quaternion<T>& y) __attribute__ ((const));

template <class T> inline quaternion<T>
slerp (T t, const quaternion<T>& x, const quaternion<T>& y)
{
  double a = acos(dot(x, y));
  double sa = sin(a);
  if(sa>=(1e-06))
    return x*(T)(sin((1.0-t)*a)/sin(a)) + y*(T)(sin(t*a)/sin(a));
  return x*(T)(1.0-t) + y*(T)(t);
}



template <class T> inline quaternion<T>
exp (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline quaternion<T>
exp (const quaternion<T>& x)
{
  T angle = sqrt(im1(x)*im1(x) + im2(x)*im2(x) + im3(x)*im3(x));
  T cs = cos(angle);
  T sn = sin(angle);

  if ( fabs(sn) >= (1e-06) ) {
    T coeff = sn/angle;
    return quaternion<T> (cs, coeff*im1(x), coeff*im2(x), coeff*im3(x));
    }
  return quaternion<T> (cs, im1(x), im2(x), im3(x));
}

template <class T> inline quaternion<T>
log (const quaternion<T>& x) __attribute__ ((const));

template <class T> inline quaternion<T>
log (const quaternion<T>& x)
{
  if(fabs(re(x))<1)
  {
    T angle = acos(re(x));
    T sn = sin(angle);
    if(fabs(sn)>=(1e-06))
    {
      T coeff = angle/sn;
      return quaternion<T>(0, coeff*im1(x), coeff*im2(x), coeff*im3(x));
    }
  }
  return quaternion<T> (0, im1(x), im2(x), im3(x));
}

} // extern "C++"

#endif

