quaternion.H

/*
 * This file is part of the "Archon" framework.
 * (http://files3d.sourceforge.net)
 *
 * Copyright © 2002 by Kristian Spangsege and Brian Kristiansen.
 *
 * Permission to use, copy, modify, and distribute this software and
 * its documentation under the terms of the GNU General Public License is
 * hereby granted. No representations are made about the suitability of
 * this software for any purpose. It is provided "as is" without express
 * or implied warranty. See the GNU General Public License
 * (http://www.gnu.org/copyleft/gpl.html) for more details.
 *
 * The characters in this file are ISO8859-1 encoded.
 *
 * The documentation in this file is in "Doxygen" style
 * (http://www.doxygen.org).
 */

#ifndef ARCHON_MATH_QUATERNION_H
#define ARCHON_MATH_QUATERNION_H

#include <cmath>
#include <algorithm>
#include <iostream>

#include <archon/math/rotation.H>

namespace Archon
{
  namespace Math
  {
    struct Quaternion
    {
      double w;
      Vector3 v;

      static const Quaternion zero()
      {
        static Quaternion q = makeZero();
        return q;
      }

      static const Quaternion one()
      {
        static Quaternion q = makeOne();
        return q;
      }

      Quaternion() {}
      Quaternion(double w, const Vector3 &v): w(w), v(v) {}

      double squareSum() const                        { return w*w + v.squareSum(); }
      double length() const                           { return sqrt(squareSum()); }

      Quaternion operator+(const Quaternion &q) const { Quaternion r(*this); return r += q; }
      Quaternion operator-(const Quaternion &q) const { Quaternion r(*this); return r -= q; }
      Quaternion operator*(const Quaternion &q) const { Quaternion r(*this); return r *= q; }
      Quaternion operator*(double f) const            { Quaternion q(*this); return q *= f; }
      Quaternion operator/(double f) const            { Quaternion q(*this); return q /= f; }
      Quaternion operator-() const                    { Quaternion q(*this); q.negate(); return q; }

      Quaternion &operator+=(const Quaternion &q)     { w += q.w; v += q.v; return *this; }
      Quaternion &operator-=(const Quaternion &q)     { w -= q.w; v -= q.v; return *this; }

      Quaternion &operator*=(const Quaternion &q)
      {
        Vector3 w1 = v;
        w1 *= q.v;
        Vector3 w2 = v;
        w2 *= q.w;
        w1 += w2;
        w2 = q.v;
        w2 *= w;
        w1 += w2;
        w = w*q.w - dot(v,q.v);
        v = w1;
        return *this;
      }

      Quaternion &operator/=(const Quaternion &q)  { Quaternion r(q); r.inverse(); *this *= r; return *this; }

      Quaternion &operator*=(double f)             { w *= f; v *= f; return *this; }
      Quaternion &operator/=(double f)             { w /= f; v /= f; return *this; }

      bool operator==(const Quaternion &q) const   { return w == q.w && v == q.v; }
      bool operator!=(const Quaternion &q) const   { return w != q.w || v != q.v; }

      void set(const Vector3 &v, double w)         { this->w = w; this->v = v; }
      void negate()                                { w = -w; v.negate(); }
      void normalize()                             { *this /= length(); }
      void conjugate()                             { v.negate(); }
      void inverse()                               { *this /= squareSum(); v.negate(); }

      void setRotation(const Rotation3 &r)
      {
        v = r.axis;
        double a = r.angle / 2;
        v *= sin(a);
        w = cos(a);
      }

      void setRotation(const Vector3 &axis, double cosineOfAngle)
      {
        v = axis;
        double k = (cosineOfAngle + 1)/2;
        v *= sqrt(k-cosineOfAngle);
        w = sqrt(k);
      }

      void getRotation(Rotation3 &r) const
      {
        double l = v.squareSum();
        if(l==0)
        {
          r.axis = Vector3(0, 1 ,0);
          r.angle = 0;
          return;
        }
        l = sqrt(l + sq(w));
        double a = acos(w/l);
        l *= sin(a);
        if(l==0)
        {
          r.axis = Vector3(0, 1 ,0);
          r.angle = 0;
          return;
        }
        r.angle = 2*a;
        r.axis = v;
        r.axis /= l;
      }

    private:
      static Quaternion makeZero()
      {
        return Quaternion(0, Vector3::zero());
      }

      static Quaternion makeOne()
      {
        return Quaternion(1, Vector3::zero());
      }
    };

    inline Quaternion operator*(double f, const Quaternion &q)
    {
      Quaternion r = q; return r *= f;
    }

    inline Quaternion operator/(double f, const Quaternion &q)
    {
      Quaternion r = q; r.inverse(); return r *= f;
    }

    inline Quaternion normalize(const Quaternion &q)
    {
      Quaternion r = q; r.normalize(); return r;
    }

    inline Quaternion conjugate(const Quaternion &q)
    {
      Quaternion r = q; r.conjugate(); return r;
    }

    inline Quaternion inverse(const Quaternion &q)
    {
      Quaternion r = q; r.inverse(); return r;
    }

    std::ostream &operator<<(std::ostream &, const Quaternion &);
  }
}

#endif // ARCHON_MATH_QUATERNION_H

---