quatern.h

class for work with Quaternion

//============================================================================
// file Quatern.h
//
//	20.01.00
//============================================================================

#ifndef __QUATERN_H__
#define __QUATERN_H__

#include <stdio.h>
#include <conio.h>
#include <math.h>
#include <stdlib.h>
#include "Vector.h"
#include "Matrix.h"

#ifndef FLOAT
#define FLOAT   double
#endif

typedef class Quaternion
{
public:
 FLOAT x3;
 Vector x;
 Quaternion () {};
 Quaternion (FLOAT v) {x=v; x3=v;};
 Quaternion(const Quaternion& v) {x = v.x; x3 = v.x3;};
 Quaternion(const FLOAT d, const Vector& v) { x = v; x3 = d;};
 Quaternion(FLOAT d, FLOAT vx, FLOAT vy, FLOAT vz)
                    {x.x = vx; x.y = vy; x.z = vz; x3 = d;};
 Quaternion& operator = ( const Quaternion& v )
                    {x = v.x; x3 = v.x3; return *this; };
 Quaternion& operator = ( FLOAT f )
                    { x.x = x.y = x.z = x3 = f; return *this; };
 Quaternion operator - () const;
 Quaternion& operator += ( const Quaternion& );
 Quaternion& operator -= ( const Quaternion& );
 Quaternion& operator *= ( const Quaternion& );
 Quaternion& operator *= ( const Vector& );
 Quaternion& operator *= ( const FLOAT );
 Quaternion& operator /= ( const FLOAT );
 friend Quaternion operator + ( const Quaternion&, const Quaternion& );
 friend Quaternion operator - ( const Quaternion&, const Quaternion& );
 friend Quaternion operator ^ ( const Quaternion&, const Quaternion& );
 friend Quaternion operator * ( const Quaternion&, const Quaternion& );
 friend Quaternion operator * ( const Quaternion&, const Vector& );
 friend Quaternion operator * ( const FLOAT, const Quaternion& );
 friend Quaternion operator * ( const Quaternion&, const FLOAT );
 friend Quaternion operator / ( const Quaternion&, const FLOAT );
 friend Quaternion operator / ( const Quaternion&, const Quaternion& );
 FLOAT operator !() { return sqrt(x.x*x.x+x.y*x.y+x.z*x.z+x3*x3); };
 int operator < ( FLOAT v ) { return x.x<v && x.y<v && x.z<v && x3<v; };
 int operator > ( FLOAT v ) { return x.x>v && x.y>v && x.z>v  && x3>v; };
 friend Quaternion Notrmalize(Quaternion& q) {return Quaternion(q/(!q)); };
 void Normalize() { (*this)/=!(*this); };
 void print(char*);
 Matrix ConvertToMatrix(void);
 FLOAT Norm();
} *QuaternionPtr, *Quaternion_ptr;

////////////////////////////////////////////////////////////////////////////
inline Quaternion Quaternion::operator - () const
{
 return Quaternion(-x3, -x);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion operator + (const Quaternion& u, const Quaternion& v )
{
  return Quaternion(u.x3 + v.x3, u.x + v.x);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion operator - (const Quaternion& u, const Quaternion& v)
{
  return Quaternion(u.x3 - v.x3, u.x - v.x);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion operator * (const Quaternion& a, const Quaternion& b)
{
  return Quaternion(a.x3*b.x3-(a.x&b.x), a.x*b.x+a.x3*b.x+b.x3*a.x);
}


////////////////////////////////////////////////////////////////////////////
inline Quaternion operator * (const Quaternion& q, const Vector& v)
{
  return Quaternion(-(q.x&v), q.x*v+q.x3*v);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion operator * (const Quaternion& u, const FLOAT f)
{
  return Quaternion(u.x3*f, u.x*f);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion operator * (const FLOAT f, const Quaternion& v)
{
  return Quaternion(v.x3*f, v.x*f);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion operator / ( const Quaternion& v, const FLOAT f )
{
  return Quaternion(v.x3/f, v.x/f);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion operator / (const Quaternion& u, const Quaternion& v)
{
  return Quaternion(u.x3/v.x3, u.x/v.x);
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion& Quaternion::operator += (const Quaternion& v)
{
  x += v.x;
  x3 += v.x3;
  return *this;
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion& Quaternion::operator -= (const Quaternion& v)
{
  x -= v.x;
  x3 -= v.x3;
  return *this;
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion& Quaternion::operator *= ( const FLOAT f )
{
  x *= f;
  x3 *= f;
  return *this;
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion& Quaternion::operator *= (const Quaternion& v)
{
 *this=Quaternion(x3*v.x3-(x&v.x), x*v.x+x3*v.x+v.x3*x);
 return *this;
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion& Quaternion::operator *= (const Vector& v)
{
 *this=Quaternion(-(x&v.x), x*v+x3*v);
 return *this;
}

////////////////////////////////////////////////////////////////////////////
inline Quaternion& Quaternion::operator /= (const FLOAT v)
{
  x /= v;
  x3 /= v;
  return *this;
}

///////////////////////////functions///////////////////////////////
//inline Quaternion Normalize(Quaternion& v) { return v / !v; };
//Quaternion RndQuaternion();
Quaternion & Clip( Quaternion& );

///////////////////////////////////////////////////////////////////
// file vector.cpp
/*Quaternion RndQuaternion()
{
 Quaternion q(rand()-0.5*RAND_MAX, rand()-0.5*RAND_MAX,
              rand()-0.5*RAND_MAX, rand()-0.5*RAND_MAX);
 return Normalize(q);
} */

////////////////////////////////////////////////////////////////////////////
Quaternion& Clip(Quaternion& v)
{
 if(v.x3 < 0.0) v.x3 = 0.0;
  else if(v.x3 > 1.0) v.x3 = 1.0;
 v.x=Clip(v.x);
return v;
}

////////////////////////////////////////////////////////////////////////////
Quaternion Sign(const Quaternion& u)
{
 Quaternion v(u);
 v.x = Sign(v.x);
 if(v.x3<0.0) v.x3 = -1.0;
  else if(v.x3>0.0) v.x3 = 1.0; else v.x = 0.0;
 return v;
}

////////////////////////////////////////////////////////////////////////////
void Quaternion::print(char* s)
{
  printf(s,x3);
  printf(s,x.x);
  printf(s,x.y);
  printf(s,x.z);
}

////////////////////////////////////////////////////////////////////////////
Matrix Quaternion::ConvertToMatrix(void) 
{
	Matrix s;
	FLOAT lambda00 = x3 * x3;
	FLOAT lambda11 = x.x * x.x;
	FLOAT lambda22 = x.y * x.y;
	FLOAT lambda33 = x.z * x.z;
	FLOAT lambda01 = x3 * x.x;
	FLOAT lambda02 = x3 * x.y;
	FLOAT lambda03 = x3 * x.z;
	FLOAT lambda12 = x.x * x.y;
	FLOAT lambda13 = x.x * x.z;
	FLOAT lambda23 = x.y * x.z;

	s.x[0][0]=lambda00+lambda11-lambda22-lambda33;
    s.x[0][1]=2.0*(lambda12-lambda03);
	s.x[0][2]=2.0*(lambda13+lambda02);
	s.x[1][0]=2.0*(lambda12+lambda03);
	s.x[1][1]=lambda00+lambda22-lambda11-lambda33;
	s.x[1][2]=2.0*(lambda23-lambda01);
	s.x[1][2]=2.0*(lambda13-lambda02);
	s.x[1][2]=2.0*(lambda23+lambda01);
	s.x[2][2]=lambda00+lambda33-lambda11-lambda22;

	return s;
}

////////////////////////////////////////////////////////////////////////////
double Quaternion::Norm(void)
{
	return sqrt(x.x*x.x+x.y*x.y+x.z*x.z+x3*x3);
}

#endif