root/trunk/glQuaternions.cpp

///////////////////////////////////////////////////////////
//                      MobSurgeon                       //
//          Ðåäàêòîð mob-îâ äëÿ Ïðîêëÿòûõ Çåìåëü         //
//           Copyright (C) 2005-2007 Gipat Group         //
//              Ðàñïðîñòðàíÿåòñÿ íà óñëîâèÿõ             //
//    Gipat Group's opened EI-editor-utility license     //
//                      âåðñèè 1.0                       //
//                                                       //
//                  www.gipatgroup.org                   //
///////////////////////////////////////////////////////////

//Ê ðàáîòå íàä äàííûì ôàéëîì ïðèëîæèëè ðóêè, íîãè.... êîðî÷å àôôòàðû:
// 1) Sagrer (sagrer@yandex.ru)

////////////////////////////////////////////////////////////////////////

//---------------------------------------------------------------------------


#pragma hdrstop

#include "glQuaternions.h"
#include <limits>

//Íó, òèïî, ðåàëèçàöèÿ...
//---------------------------------------------------------------------------

TQuaternion::~TQuaternion()
//äåñòðóêòîð
{
  //Àí íèôèãà, ïóñòî. Íî ïî èäåå ñþäà ìîæíî âïèñàòü òî ÷òî äåëàåòñÿ ïðè óáèâñòâå
  //(delete) îáúåêòà ýòîãî êëàññà.
};
//---------------------------------------------------------------------------

TQuaternion::TQuaternion()
//Äåôîëòíûé êîíñòðóêòîð
{
  //Èíèöèàëèçèðóåì äåôîëòíûå çíà÷åíèÿ ïåðåìåííûõ...
  this->ident();
};
//---------------------------------------------------------------------------

TQuaternion::TQuaternion(float &w, float &x, float &y, float &z)
//Êîíñòðóêòîð íà ïðîèçâîëüíûé êâàòåðíèîí
{
  //Èíèöèàëèçèðóåì óêàçàííûå çíà÷åíèÿ ïåðåìåííûõ...
  QuatW = w;
  QuatX = x;
  QuatY = y;
  QuatZ = z;
};
//---------------------------------------------------------------------------

TQuaternion::TQuaternion(float w, float x, float y, float z)
//Êîíñòðóêòîð íà ïðîèçâîëüíûé êâàòåðíèîí
{
  //Èíèöèàëèçèðóåì óêàçàííûå çíà÷åíèÿ ïåðåìåííûõ...
  QuatW = w;
  QuatX = x;
  QuatY = y;
  QuatZ = z;
};
//---------------------------------------------------------------------------

void TQuaternion::ident()
//Ïîñòàâèòü êâàòåðíèîíó ïóñòîå çíà÷åíèå.
{
  QuatW = 1;
  QuatX = 0;
  QuatY = 0;
  QuatZ = 0;
};
//---------------------------------------------------------------------------

void TQuaternion::scale(float s)
//Òèïî ìàñøòàáèðîâàòü. Â ïðèíöèïå - óìíîæåíèå íà ñêàëÿð.
{
  QuatW *= s;
  QuatX *= s;
  QuatY *= s;
  QuatZ *= s;
};
//---------------------------------------------------------------------------

float TQuaternion::norm()
//Íîðìà.
{
  //Ïåðåìåííûå
  float Result;
  /*double Arr[] = {QuatX,QuatY,QuatZ,QuatW};

  //Ñ÷èòàåì...
  Result = SumOfSquares(&Arr[0],3);*/

  //Ñ÷èòàåì...
  Result = QuatX*QuatX + QuatY*QuatY + QuatZ*QuatZ + QuatW*QuatW;

  //Âåðíóòü ðåçóëüòàò...
  return Result;
};

//---------------------------------------------------------------------------

float TQuaternion::magnitude()
//Ìîäóëü.
{
  //Ïåðåìåííûå
  float Result;

  //Ñ÷èòàåì...
  Result = (float)sqrt(norm());

  //Âåðíóòü ðåçóëüòàò...
  return Result;
};
//---------------------------------------------------------------------------

void TQuaternion::StabilizeLength()
//Òèïî ñòàáèëèçèðîâàòü äëèíó êâàòåðíèîíà - ýòî âûïîëíÿåòñÿ áûñòðåå íîðìàëèçàöèè.
{
  float cs = (float)(fabs(QuatW) + fabs(QuatX) + fabs(QuatY) + fabs(QuatZ));

  if (cs > 0.0f)
  {
    QuatW /= cs;
    QuatX /= cs;
    QuatY /= cs;
    QuatZ /= cs;
  }
  else
  {
    this->ident();
  };
};
//---------------------------------------------------------------------------

void TQuaternion::normalize()
//Íîðìàëèçîâàòü. Ìåäëåííåå ñòàáèëèçàöèè, íî â ðåçóëüòàòå åäèíè÷íûé êâàòåðíèîí.
{
  float m = magnitude();
  if( m < std::numeric_limits<float>::epsilon() )
  {
    StabilizeLength();
    m = magnitude();
  }
  scale( 1.0f/m );
};
//---------------------------------------------------------------------------

void TQuaternion::GetGLMatrix(GLfloat *matrix)
//Ïîëó÷èòü OpenGL-øíóþ ìàòðèöó ïîâîðîòà èç êâàòåðíèîíà.
{
  //Åäèíè÷íàÿ ìàòðèöà...
  //Çàêîììåíòèðîâàíî ãäå - òàì îïòèìèçàöèÿ - âñåðàâíî ýòè çíà÷åíèÿ áóäóò ïåðåçàïèñàíû.
  /*matrix[0]  = 1; matrix[1]  = 0; matrix[2]  = 0; */matrix[3]  = 0;
  /*matrix[4]  = 0; matrix[5]  = 1; matrix[6]  = 0; */matrix[7]  = 0;
  /*matrix[8]  = 0; matrix[9]  = 0; matrix[10] = 1; */matrix[11] = 0;
    matrix[12] = 0; matrix[13] = 0; matrix[14] = 0;   matrix[15] = 1;

  //À òåïåðü ïðèíöèïå òóò âñå ñîäðàíî ñ ó÷åáíèêà...
  float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
  float s  = 2.0f/norm();  // 4 mul 3 add 1 div

  x2 = QuatX * s;    y2 = QuatY * s;    z2 = QuatZ * s;
  xx = QuatX * x2;   xy = QuatX * y2;   xz = QuatX * z2;
  yy = QuatY * y2;   yz = QuatY * z2;   zz = QuatZ * z2;
  wx = QuatW * x2;   wy = QuatW * y2;   wz = QuatW * z2;

  matrix[0] = 1.0f - (yy + zz);
  matrix[4] = xy - wz;
  matrix[8] = xz + wy;

  matrix[1] = xy + wz;
  matrix[5] = 1.0f - (xx + zz);
  matrix[9] = yz - wx;

  matrix[2] = xz - wy;
  matrix[6] = yz + wx;
  matrix[10] = 1.0f - (xx + yy);
};
//---------------------------------------------------------------------------

void TQuaternion::Mult(float W, float X, float Y, float Z)
//Òèïî óìíîæèòü íà äðóãîé êâàòåðíèîí...
{
  this->QuatX = (this->QuatW * X) + (this->QuatX * W) + (this->QuatY * Z) - (this->QuatZ * Y);
  this->QuatY = (this->QuatW * Y) + (this->QuatY * W) + (this->QuatZ * X) - (this->QuatX * Z);
  this->QuatZ = (this->QuatW * Z) + (this->QuatZ * W) + (this->QuatX * Y) - (this->QuatY * X);
  this->QuatW = (this->QuatW * W) - (this->QuatX * X) - (this->QuatY * Y) - (this->QuatZ * Z);
};
//---------------------------------------------------------------------------

void TQuaternion::SetAxisAngle(float VecX, float VecY, float VecZ, float angle)
//Òèïî âûñòàâèòü êâàòåðíèîíó óãîë èç Axis-Angle ïðåäñòàâëåíèÿ.
{
  vec3 AxisVec(VecX, VecY, VecZ);  //Îñü ïîâîðîòà.
  float tmp = AxisVec.normalize();     //Íîðìàëèçóåì âåêòîð.
  float HalfAngle = angle*0.5f;    //Âûñ÷èòûâàåì ïîëîâèííûé óãîë.
  float AngleSin = (float)sin(HalfAngle);    //Âûñ÷èòûâàåì ñèíóñ ïîëóóãëà...

  //Âûñòàâëÿåì íîâûå çíà÷åíèÿ êâàòåðíèîíà...
  this->QuatW = (float)cos(HalfAngle);    //Ñêàëÿð - êîñèíóñ ïîëóóãëà ïîâîðîòà.
  this->QuatX = AxisVec.x * AngleSin;     //Êîîðäèíàòû âåêòîðíîé ÷àñòè çàâèñÿò îò ñèíóñà ïîëóóãëà ïîâîðîòà...
  this->QuatY = AxisVec.y * AngleSin;     //  --""--
  this->QuatZ = AxisVec.z * AngleSin;     //  --""--
};
//---------------------------------------------------------------------------

void TQuaternion::RotateByAxisAngle(float VecX, float VecY, float VecZ, float angle)
//Ñäåëàòü óêàçàííûé ïîâîðîò âîêðóã óêàçàííîé îñè (â äîáàâêó ê èìåþùåéñÿ îðèåíòàöèè).
{
  vec3 AxisVec(VecX, VecY, VecZ);  //Îñü ïîâîðîòà.
  float tmp = AxisVec.normalize();     //Íîðìàëèçóåì âåêòîð.
  float HalfAngle = angle*0.5f;    //Âûñ÷èòûâàåì ïîëîâèííûé óãîë.
  float AngleSin = (float)sin(HalfAngle);    //Âûñ÷èòûâàåì ñèíóñ ïîëóóãëà...

  //óìíîæàåì "íîâûé" ïîëó÷åííûé êâàòåðíèîí íà òî ÷òî åñòü...
  this->Mult((float)cos(HalfAngle),(float)(AxisVec.x * AngleSin),(float)(AxisVec.y * AngleSin),(float)(AxisVec.z * AngleSin));
};
//---------------------------------------------------------------------------

#pragma package(smart_init)