///////////////////////////////////////////////////////////
//                      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)