KeosQuaternion.h

Go to the documentation of this file.

/*
 * This source file is part of KEOS (Free 3D Engine)
 * For the latest info, see http://www.keosengine.org/
 * E-mails : thierry.vouriot@keosengine.org, yeri@keosengine.org
 *
 * 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 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., 59 Temple
 * Place - Suite 330, Boston, MA 02111-1307, USA, or go to
 * http://www.gnu.org/copyleft/lesser.txt.
 *
 */

#ifndef KEOS_QUATERNION_H
#define KEOS_QUATERNION_H

#include "KeosPrerequisites.h"
#include "KeosMatrix4.h"

namespace Keos
{
  //-----------------------------------------------------------------------
  class CQuaternion
  {
  public :

    CQuaternion(float X = 0.0f, float Y = 0.0f, float Z = 0.0f, float W = 1.0f);

    CQuaternion(const CMatrix4& Matrix);

    CQuaternion(const TVector3F& Axis, float Angle);

    void Identity();

    inline void Normalize();

    CQuaternion Conjugate() const;

    CMatrix4 ToMatrix() const;

    void FromMatrix(const CMatrix4& Matrix);

    void FromAxisAngle(const TVector3F& Axis, float Angle);

    void ToAxisAngle(TVector3F& Axis, float& Angle) const;

    void FromEulerAngles(float X, float Y, float Z);

    // Operators
    CQuaternion operator *(const CQuaternion& Quaternion) const;
    const CQuaternion& operator *=(const CQuaternion& Quaternion);

    float x;
    float y;
    float z;
    float w;
  };

  // Globals functions
  std::istream& operator >>(std::istream& Stream, CQuaternion& Quaternion);
  std::ostream& operator <<(std::ostream& Stream, const CQuaternion& Quaternion);

  //=======================================================================
  // CQuaternion inline functions
  //=======================================================================

  //-----------------------------------------------------------------------
  inline CQuaternion::CQuaternion(float X, float Y, float Z, float W) :
      x(X),
      y(Y),
      z(Z),
      w(W)
  {}

  //-----------------------------------------------------------------------
  inline CQuaternion::CQuaternion(const CMatrix4& Matrix)
  {
    FromMatrix(Matrix);
  }

  //-----------------------------------------------------------------------
  inline CQuaternion::CQuaternion(const TVector3F& Axis, float Angle)
  {
    FromAxisAngle(Axis, Angle);
  }

  //-----------------------------------------------------------------------
  inline void CQuaternion::Identity()
  {
    x = y = z = 0.0f;
    w = 1.0f;
  }

  //-----------------------------------------------------------------------
  inline void CQuaternion::Normalize()
  {
    float Norm = x * x + y * y + z * z + w * w;

    if (std::fabs(Norm) > std::numeric_limits<float>::epsilon())
    {
      x /= Norm;
      y /= Norm;
      z /= Norm;
      w /= Norm;
    }
  }

  //-----------------------------------------------------------------------
  inline CQuaternion CQuaternion::Conjugate() const
  {
    return CQuaternion(-x, -y, -z, w);
  }

  //-----------------------------------------------------------------------
  inline CMatrix4 CQuaternion::ToMatrix() const
  {
    float xx = x * x;
    float xy = x * y;
    float xz = x * z;
    float xw = x * w;
    float yy = y * y;
    float yz = y * z;
    float yw = y * w;
    float zz = z * z;
    float zw = z * w;

    return CMatrix4(1 - 2 * (yy + zz),     2 * (xy - zw),     2 * (xy + yw), 0,
                    2 * (xy + zw), 1 - 2 * (xx + zz),     2 * (yz - xw), 0,
                    2 * (xz - yw),     2 * (yz + xw), 1 - 2 * (xx + yy), 0,
                    0,                 0,                 0, 1);
  }

  //-----------------------------------------------------------------------
  inline void CQuaternion::FromMatrix(const CMatrix4& Matrix)
  {
    float Trace = Matrix(0, 0) + Matrix(1, 1) + Matrix(2, 2) + 1;

    if (Trace > 0)
    {
      float s = 0.5f / std::sqrtf(Trace);
      x = (Matrix(2, 1) - Matrix(1, 2)) * s;
      y = (Matrix(0, 2) - Matrix(2, 0)) * s;
      z = (Matrix(1, 0) - Matrix(0, 1)) * s;
      w = 0.25f / s;
    }
    else
    {
      if ((Matrix(0, 0) > Matrix(1, 1) && (Matrix(0, 0) > Matrix(2, 2)))
        {
          float s = std::sqrt(1 + Matrix(0, 0) - Matrix(1, 1) - Matrix(2, 2)) * 2;
            x = 0.5f / s;
            y = (Matrix(0, 1) + Matrix(1, 0)) / s;
            z = (Matrix(0, 2) + Matrix(2, 0)) / s;
            w = (Matrix(1, 2) + Matrix(2, 1)) / s;
          }
          else if (Matrix(1, 1) > Matrix(2, 2))
        {
          float s = std::sqrt(1 - Matrix(0, 0) + Matrix(1, 1) - Matrix(2, 2)) * 2;
            x = (Matrix(0, 1) + Matrix(1, 0)) / s;
            y = 0.5f / s;
            z = (Matrix(1, 2) + Matrix(2, 1)) / s;
            w = (Matrix(0, 2) + Matrix(2, 0)) / s;
          }
          else
          {
            float s = std::sqrt(1 - Matrix(0, 0) - Matrix(1, 1) + Matrix(2, 2)) * 2;
            x = (Matrix(0, 2) + Matrix(2, 0)) / s;
            y = (Matrix(1, 2) + Matrix(2, 1)) / s;
            z = 0.5f / s;
            w = (Matrix(0, 1) + Matrix(1, 0)) / s;
          }
        }
      }

      //-----------------------------------------------------------------------
      inline void CQuaternion::FromAxisAngle(const TVector3F& Axis, float Angle)
      {
        float Cos = std::cos(Angle / 2);
        float Sin = std::sin(Angle / 2);

        x = Axis.x * Sin;
        y = Axis.y * Sin;
        z = Axis.z * Sin;
        w = Cos;

        Normalize();
      }

      //-----------------------------------------------------------------------
      inline void CQuaternion::ToAxisAngle(TVector3F& Axis, float& Angle) const
      {
        // Calcul de l'angle
        Angle = std::acosf(w) * 2;

        // Calcul de l'axe
        float Norm = std::sqrtf(x * x + y * y + z * z);
        if (std::fabs(Norm) > std::numeric_limits<float>::epsilon())
        {
          Axis.x = x / Norm;
          Axis.y = y / Norm;
          Axis.z = z / Norm;
        }
        else
        {
          Axis.x = 0.0f;
          Axis.y = 1.0f;
          Axis.z = 0.0f;
        }
      }

      //-----------------------------------------------------------------------
      inline void CQuaternion::FromEulerAngles(float X, float Y, float Z)
      {
        CQuaternion Qx(TVector3F(1, 0, 0), X);
        CQuaternion Qy(TVector3F(0, 1, 0), Y);
        CQuaternion Qz(TVector3F(0, 0, 1), Z);

        *this = Qx * Qy * Qz;
      }

      //-----------------------------------------------------------------------
      inline CQuaternion CQuaternion::operator *(const CQuaternion& Quaternion) const
      {
        return CQuaternion(w * Quaternion.x + x * Quaternion.w + y * Quaternion.z - z * Quaternion.y,
                           w * Quaternion.y + y * Quaternion.w + z * Quaternion.x - x * Quaternion.z,
                           w * Quaternion.z + z * Quaternion.w + x * Quaternion.y - y * Quaternion.x,
                           w * Quaternion.w - x * Quaternion.x - y * Quaternion.y - z * Quaternion.z);
      }

      //-----------------------------------------------------------------------
      inline const CQuaternion& CQuaternion::operator *=(const CQuaternion& Quaternion)
      {
        *this = *this * Quaternion;

        return *this;
      }

      //-----------------------------------------------------------------------
      std::istream& operator >>(std::istream& Stream, CQuaternion& Quaternion)
      {
        return Stream >> Quaternion.x >> Quaternion.y >> Quaternion.z >> Quaternion.w;
      }

      //-----------------------------------------------------------------------
      std::ostream& operator <<(std::ostream& Stream, const CQuaternion& Quaternion)
      {
        return Stream << Quaternion.x << " " << Quaternion.y << " " << Quaternion.z << " " << Quaternion.w;
      }

    } // namespace Keos


#endif // KEOS_QUATERNION_H

---