coordinate.h

/***************************************************************************
                          coordinate.h -  description
                             -------------------
    author               : Armando Navarro-Vazquez
    email                : armando.deus@gmail.com
 ***************************************************************************/

/***************************************************************************
 *                                                                         *
 *   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.                                   *
 *                                                                         *
 ***************************************************************************/


#ifndef MAGNES_COORDINATE_H
#define MAGNES_COORDINATE_H

#include <iostream>
#include <cmath>
#include "mathtools.h"
#include "coreexport.h"

namespace magnes
{

class MAGNES_CORE_API Coordinate
{
public:
  Coordinate();
  Coordinate(float cx,float cy,float cz);
  Coordinate operator^(const Coordinate& b) const;
  float operator*(const Coordinate& b) const;
  void operator-=(const Coordinate& b);
  void operator +=(float f);
  void operator +=(const Coordinate& b);
  void operator *=(float f);
  void operator /=(float f);
  Coordinate operator *(float f) const;
  Coordinate operator /(float f) const;
  Coordinate operator -(const Coordinate& b) const;
  Coordinate operator+(const Coordinate& b) const;
  float& x() { return m_xyz(0); }
  const float& x() const { return m_xyz(0); }
  float& y() { return m_xyz(1); }
  const float& y() const { return m_xyz(1); }
  float& z() { return m_xyz(2); }
  const float& z() const { return m_xyz(2); }
  operator const D1StaticArray<float,3>& () const { return m_xyz; }
  operator D1StaticArray<float,3>& () { return m_xyz; }
  D1Array<double> DoublePrecisionCoords() const;
  ~Coordinate();
  static float Distance(const Coordinate& a, const Coordinate& b);
  float Norm() const;
  void Normalize();
  static  float Angle(const Coordinate& a, const Coordinate& b);
  static float Dihedral(const Coordinate& a, const Coordinate& b, const Coordinate& c, const Coordinate& d);
  static  Coordinate MiddlePoint(const Coordinate& a, const Coordinate& b);
  
  static Coordinate RotAroundAxis(const Coordinate& c, const Coordinate& axisorigin, const Coordinate& axisend,float angle);
private:
  D1StaticArray<float,3> m_xyz;
};

inline Coordinate operator*(float f,Coordinate a)
{
    return a*f;
}


  class MAGNES_CORE_API Quaternion
  {
  public:
    Quaternion() { _w=_v.x()=_v.y()=_v.z()=0; }
    Quaternion(double scalar,const Coordinate& c) { _w=scalar; _v=c; }  
    Coordinate& V() { return _v; }
    const Coordinate& V() const { return _v; } 
    float& W() { return _w; }
    const float& W() const { return _w; }
    float Norm() const 
    {
      return std::sqrt(_w*_w+_v.x()*_v.x()+_v.y()*_v.y()+_v.z()*_v.z());
    }
    Quaternion Invert() const
    {
      Quaternion q1;
      
      q1._w=this->_w;
      q1._v.x()=-(this->_v.x());
      q1._v.y()=-(this->_v.y());
      q1._v.z()=-(this->_v.z());
      return q1;
    }
    Quaternion operator^(const Quaternion& q)
    {
      Quaternion  q1;
      q1._w=  this->_w*q._w -  this->_v.x()*q._v.x() - this->_v.y()*q._v.y() -this->_v.z()*q._v.z();
      q1._v.x()=this->_w*q._v.x()+this->_v.x()*q._w+this->_v.y()*q._v.z()-this->_v.z()*q._v.y();
      q1._v.y()=this->_w*q._v.y()+this->_v.y()*q._w+this->_v.z()*q._v.x()-this->_v.x()*q._v.z();
      q1._v.z()=this->_w*q._v.z()+this->_v.z()*q._w+this->_v.x()*q._v.y()-this->_v.y()*q._v.x();

      return q1;
    }
    
        Quaternion operator^(const Coordinate& c)
        {
          Quaternion c1(0,c);
          return (*this)^c1;
        }
        
    void Normalize()
    {
      float norm=this->Norm();
      _v.x()/=norm;
      _v.y()/=norm;
      _v.z()/=norm;
      _w/=norm;
    }
    static Quaternion FromRotationAxis(float angle,const Coordinate& rv)
    {
        angle *= 0.5f;
        Coordinate nrv=rv;
        nrv.Normalize();
 
        float seno=sin(angle);
        Quaternion q;
        q.V().x() = (nrv.x() * seno);
        q.V().y() = (nrv.y() * seno);
        q.V().z() = (nrv.z() * seno);
        q.W() = cos(angle);
       
        return q;
     
    }
    void GLMatrix(D2Array<float>& glmatrix)
    {

    float xx      = V().x()*V().x();
    float xy      = V().x() * V().y();
    float xz      = V().x()*V().z();
    float xw      = V().x()*W();
    float yy      = V().y()*V().y();
    float yz      = V().y() * V().z();
    float yw      = V().y()*W();
    float zz      = V().z()*V().z();
    float zw      = V().z() * W();
    glmatrix[0]  = 1 - 2 * ( yy + zz );
    glmatrix[1]  =     2 * ( xy - zw );
    glmatrix[2]  =     2 * ( xz + yw );
    glmatrix[4]  =     2 * ( xy + zw );
    glmatrix[5]  = 1 - 2 * ( xx + zz );
    glmatrix[6]  =     2 * ( yz - xw );
    glmatrix[8]  =     2 * ( xz - yw );
    glmatrix[9]  =     2 * ( yz + xw );
    glmatrix[10] = 1 - 2 * ( xx + yy );
    glmatrix[3]  =glmatrix[7] = glmatrix[11] = glmatrix[12] = glmatrix[13] = glmatrix[14] = 0;
    glmatrix[15] = 1;

    }
    void RotationMatrix(D2Array<float>& rotmatrix)
    {

    float xx      = V().x()*V().x();
    float xy      = V().x() * V().y();
    float xz      = V().x()*V().z();
    float xw      = V().x()*W();
    float yy      = V().y()*V().y();
    float yz      = V().y() * V().z();
    float yw      = V().y()*W();
    float zz      = V().z()*V().z();
    float zw      = V().z() * W();
    rotmatrix(0,0)  = 1 - 2 * ( yy + zz );
    rotmatrix(0,1)  =     2 * ( xy - zw );
    rotmatrix(0,2)  =     2 * ( xz + yw );
    rotmatrix(1,0)  =     2 * ( xy + zw );
    rotmatrix(1,1)  = 1 - 2 * ( xx + zz );
    rotmatrix(1,2)  =     2 * ( yz - xw );
    rotmatrix(2,0)  =     2 * ( xz - yw );
    rotmatrix(2,1)  =     2 * ( yz + xw );
    rotmatrix(2,2)  = 1 - 2 * ( xx + yy );

    }

    void RotationMatrix(D2Array<double>& rotmatrix)
    {

    double xx      = V().x()*V().x();
    double xy      = V().x() * V().y();
    double xz      = V().x()*V().z();
    double xw      = V().x()*W();
    double yy      = V().y()*V().y();
    double yz      = V().y() * V().z();
    double yw      = V().y()*W();
    double zz      = V().z()*V().z();
    double zw      = V().z() * W();
    rotmatrix(0,0)  = 1 - 2 * ( yy + zz );
    rotmatrix(0,1)  =     2 * ( xy - zw );
    rotmatrix(0,2)  =     2 * ( xz + yw );
    rotmatrix(1,0)  =     2 * ( xy + zw );
    rotmatrix(1,1)  = 1 - 2 * ( xx + zz );
    rotmatrix(1,2)  =     2 * ( yz - xw );
    rotmatrix(2,0)  =     2 * ( xz - yw );
    rotmatrix(2,1)  =     2 * ( yz + xw );
    rotmatrix(2,2)  = 1 - 2 * ( xx + yy );

    }

private:
    float _w;
    Coordinate _v;

  };
inline MAGNES_CORE_API std::ostream& operator << (std::ostream& s,const Coordinate& c)
{
  s << "(" << c.x() << "," << c.y() << ","  << c.z() << ")";

  return s;
}

inline MAGNES_CORE_API std::ostream& operator << ( std::ostream& s, const Quaternion& c)
{
  s << "[" << ( c.W() ) << "," << ( c.V() ) << "]" <<  std::endl;
  return s;
}
}
#endif // MAGNES_COORDINATE_H