---

quaternion.cc

Go to the documentation of this file.

/*
  svas_server -- virtual World Server of Svas
  Copyright (c) 2001, 2002 David Moreno Montero
 
 
  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.
 
  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
  General Public License for more details.
 
  You should have received a copy of the GNU 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.  

*/

#include "quaternion.h"
#include <stdio.h>
#include <math.h>
#include "log.h"

inline Quaternion::Quaternion(){
  u=x=y=z=0;
}

inline Quaternion::Quaternion(double _u,double _x,double _y,double _z){
  u=_u;  
  x=_x; 
  y=_y;
  z=_z;
}

inline Quaternion::Quaternion(const Vector3d &_V, double _a){
  u=cos(_a/2);
  Vector3d V=_V;
  V=V.normalized()*sin(_a/2);
  x=V.X();
  y=V.Y();
  z=V.Z();
}

inline string Quaternion::toString(){
  char s[256];
  sprintf(s,"[%f,(%f,%f,%f)]",u,x,y,z);
  return string(s);
}

inline double Quaternion::module(){
  return sqrt(u*u+x*x+y*y+z*z);
}

inline Quaternion operator+(const Quaternion &a,const Quaternion &b){
  return Quaternion(a.u+b.u, a.x+b.x, a.y+b.y, a.z+b.z);
}

inline Quaternion Quaternion::operator+=(const Quaternion &a){
  u+=a.u;
  x+=a.x;
  y+=a.y;
  z+=a.z;
  return *this;
}

inline Quaternion operator-(const Quaternion &a,const Quaternion &b){
  return Quaternion(a.u-b.u, a.x-b.x, a.y-b.y, a.z-b.z);
}

inline Quaternion Quaternion::operator-=(const Quaternion &a){
  u-=a.u;
  x-=a.x;
  y-=a.y;
  z-=a.z;
  return *this;
}

inline Quaternion operator*(const Quaternion &a, double s){
  return Quaternion(a.u*s, a.x*s, a.y*s, a.z*s);
}

inline Quaternion Quaternion::operator*=(double s){
  u*=s;
  x*=s;
  y*=s;
  z*=s;
  return *this;
}

inline Quaternion operator/(const Quaternion &a, double s){
  return Quaternion(a.u/s, a.x/s, a.y/s, a.z/s);
}

inline Quaternion Quaternion::operator/=(double s){
  u/=s;
  x/=s;
  y/=s;
  z/=s;
  return *this;
}

inline Quaternion operator~(const Quaternion &a){
  return Quaternion(a.u, -a.x, -a.y, -a.z);
}

inline Quaternion operator*(const Quaternion &a, const Quaternion &b){
  return Quaternion(a.u*b.u - a.x*b.x - a.y*b.y - a.z*b.z,
            a.u*b.x + a.x*b.u + a.y*b.z - a.z*b.y,
            a.u*b.y + a.y*b.u + a.z*b.x - a.x*b.z,
            a.u*b.z + a.z*b.u + a.x*b.y - a.y*b.x);
}

inline Quaternion operator*(const Quaternion &q,const Vector3d &v){
  return Quaternion( -(q.x*v.x + q.y*v.y + q.z*v.z),
             q.u*v.x + q.y*v.z - q.z*v.y,
             q.u*v.y + q.z*v.x - q.x*v.z,
             q.u*v.z + q.x*v.y - q.y*v.x);
}

inline Quaternion operator*(const Vector3d &v,const Quaternion &q){
  return Quaternion( -(q.x*v.x + q.y*v.y + q.z*v.z),
             q.u*v.x + q.z*v.y - q.y*v.z,
             q.u*v.y + q.x*v.z - q.z*v.x,
             q.u*v.z + q.y*v.x - q.x*v.y);
}

inline Quaternion Quaternion::operator=(const Vector3d &_v){
  Vector3d v(_v);
  u=0; 
  x=v.X(); y=v.Y(); z=v.Z();
  return *this;
}

inline Vector3d Quaternion::getAxis(void){
  return Vector3d(x,y,z);
}

inline Quaternion Quaternion::rotate(const Vector3d &v){
  Quaternion q=(*this)*v*(~(*this));
  return q;
}

/**
 * This method rotates the given quaternion over ME.
 */
inline Quaternion Quaternion::rotate(const Quaternion &q){
  Quaternion rq=(*this)*q*(~(*this));
  return rq;
}

inline Quaternion Quaternion::rotate(const Vector3d &axis, double angle){
  log <<"Check this!"<<endl; // FIXME
  return Quaternion(axis,angle).rotate(*this);
}

/**
 * Just a rotation
 */
inline Vector3d Quaternion::toWorld(const Vector3d &v){
  return ((*this)*v*(~(*this))).getAxis();
}

/**
 * Just a rotation vs the conjugate
 */
inline Vector3d Quaternion::toBody(const Vector3d &v){
  return ((~(*this))*v*(*this)).getAxis();
}

---