/////////////////////////////////////////////////////////////////////////////////////////////////
//läd die Standartwerte einer Matrix
//@Matrix: Zeiger auf die Matrix die geladen werden soll;
procedure gluLoadMatrixIdentity(const Matrix: PgluMatrix);
begin
  Matrix^[maXAxis] := gluMatrixAxis(1, 0, 0, 0);
  Matrix^[maYAxis] := gluMatrixAxis(0, 1, 0, 0);
  Matrix^[maZAxis] := gluMatrixAxis(0, 0, 1, 0);
  Matrix^[maPos]   := gluMatrixAxis(0, 0, 0, 1);
end;

/////////////////////////////////////////////////////////////////////////////////////////////////
//dreht die Matrix um die angegebene Achse bezüglich ihrer eigenen Achsen
//@Matrix: Matrix die gedreht werden soll;
//@Angle: Winkel um den gedreht werden soll (im Bogenmaß);
//@X: X-Wert der Achse um die gedreht werden soll;
//@X: Y-Wert der Achse um die gedreht werden soll;
//@X: Z-Wert der Achse um die gedreht werden soll;
procedure gluRotateMatrix(const Matrix: PgluMatrix; const Angle: Single; Axis: TglVector3f);
var
  RotMatrix: TgluMatrix;
  X, Y, Z: Single;
  a, c, s: Single;
begin
  gluNormalize(Axis);
  X := Axis[0];
  Y := Axis[1];
  Z := Axis[2];
  a := Angle/180*Pi;
  c := cos(a);
  s := sin(a);
  gluLoadMatrixIdentity(@RotMatrix);
  RotMatrix[maXAxis] := gluMatrixAxis(
    SQR(X) + (1-SQR(X))*c,
    X*Y*(1-c) + Z*s,
    X*Z*(1-c) - Y*s, 0);
  RotMatrix[maYAxis] := gluMatrixAxis(
    X*Y*(1-c) - Z*s,
    SQR(Y) + (1-SQR(Y))*c,
    Y*Z*(1-c) + X*s, 0);
  RotMatrix[maZAxis] := gluMatrixAxis(
    X*Z*(1-c) + Y*s,
    Y*Z*(1-c) - X*s,
    SQR(Z) + (1-SQR(Z))*c, 0);
  gluMatrixMul(Matrix, @RotMatrix);
end;

/////////////////////////////////////////////////////////////////////////////////////////////////
//Mutlipliziert Matrix1 mit Matrix2 und speichert das Ergebniss in Matrix1
//@Matrix1: 1. Multiplikator;
//@Matrix2: 2. Multiplikator;
procedure gluMatrixMul(const Matrix1, Matrix2: PgluMatrix);
type
  TMatrix = array[0..3, 0..3] of glFloat;
  PMatrix = ^TMatrix;
var
  x, y, i: Integer;
  sum : Single;
  M1, M2, MRes: PMatrix;
begin
  new(MRes);
  gluLoadmatrixIdentity(PgluMatrix(MRes));
  M1 := PMatrix(Matrix1);
  M2 := PMatrix(Matrix2);
  for x := 0 to 3 do begin
    for y := 0 to 3 do begin
      sum := 0;
      for i := 0 to 3 do begin
        sum := sum + M1^[i][y] * M2^[x][i];
      end;
      MRes^[x][y] := sum;
    end;
  end;
  for x := 0 to 3 do
    for y := 0 to 3 do
      M1^[x][y] := MRes^[x][y];
  dispose(MRes);
end;

/***************************************************************************
 *   Copyright (C) 2010 by Martin Weusten                                  *
 *   martin dot weusten at rwth minus aachen dot de                         *
 *                                                                         *
 *   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.             *
 ***************************************************************************/
#ifndef INCLUDE_GUARD_CML_FRAME3
#define INCLUDE_GUARD_CML_FRAME3

/** A 'Frame' is a Quaternion4 combined with a Vector3.
 * Using this class you can use quaternions similar as 4x4 matrices. The
 * quaternion does represent the rotation part, the vector is the translation. */
template<class T> class Frame3
{
public:
   Frame3<T>() { } ///< no-initialization constructor
   Frame3<T>(const Vector3<T>& _p, const Quaternion4<T>& _q);
   
   // assignment operators
   Frame3<T>& operator*=(const Frame3<T>& f);    ///< combine two frames and assign result

   // binary operators
   Frame3<T> operator*(const Frame3<T>& f) const; ///< combine two frames
   
   // operations
   Matrix44<T> matrix() const;    ///< convert transformation into matrix
   Frame3<T> inverse() const;     ///< calculate inverse transformation
   Vector3<T> transform(const Vector3<T>& v) const;    ///< transform point
   bool isNaN() const;                 ///< tests if one of the values is NaN = 'Not a Number'
   
   Vector3<T> p;
   Quaternion4<T> q;
};


template<class T> inline Frame3<T>::Frame3(const Vector3<T>& _p, const Quaternion4<T>& _q)
   : p(_p), q(_q)
{
   
}

template<class T> inline Frame3<T>& Frame3<T>::operator*=(const Frame3<T>& f)
{
   p += q.rotate(f.p);
   q *= f.q;
   return *this;
}

template<class T> inline Frame3<T> Frame3<T>::operator*(const Frame3<T>& f) const
{
   return Frame3<T>(p + q.rotate(f.p), q * f.q);
}

template<class T> inline Matrix44<T> Frame3<T>::matrix() const
{
   return q.getMatrix44().setW(p);
}

template<class T> inline Frame3<T> Frame3<T>::inverse() const
{
   const Quaternion4<T>& c = q.conjugated();
   return Frame3<T>(c.rotate(-p), c);
}

template<class T> inline Vector3<T> Frame3<T>::transform(const Vector3<T>& v) const
{
   return p + q.rotate(v);
}

template<class T> inline bool Frame3<T>::isNaN() const
{
   return p.isNaN() || q.isNaN();
}

#endif

   TQuaternion = Packed Record RealW,ImagX,ImagY,ImagZ: TFloat32; End;
   TVector3F   = Packed Record X,Y,Z: TFloat32; End;

//********************************************************************
//                        Q U A T E R N I O N
//********************************************************************
// Has been tested and found OK
Function Quaternion(Const Angle,AX,AY,AZ: TFloat32): TQuaternion;
Var
   LocalQuat: TQuaternion;
   LocalVector: TVector3F;
   CosHalfAngle,SinHalfAngle: TFloat32;
Begin
   CosHalfAngle:= Cos(Angle/2*PIDIV180);
   SinHalfAngle:= Sin(Angle/2*PIDIV180);

   With Result Do
   Begin
      RealW:= CosHalfAngle;
      ImagX:= AX*SinHalfAngle;
      ImagY:= AY*SinHalfAngle;
      ImagZ:= AZ*SinHalfAngle;
   End;
End;
//********************************************************************
// Has been tested and found OK
Function QuaternionAdd(Const Quat1,Quat2: TQuaternion): TQuaternion;
Begin
   With Result Do
   Begin
      RealW:= Quat1.RealW + Quat2.RealW;
      ImagX:= Quat1.ImagX + Quat2.ImagX;
      ImagY:= Quat1.ImagY + Quat2.ImagY;
      ImagZ:= Quat1.ImagZ + Quat2.ImagZ;
   End;
End;
//********************************************************************
// Linear interpolation between two quaternions (LERP)
// Has been tested and found OK
Function QuaternionBlendLinear(Const Quat1,Quat2: TQuaternion;
               Const AFactor: TFloat32): TQuaternion;
Begin
    Result:= QuaternionNormalize(QuaternionAdd(
         (QuaternionScale(Quat1,(1-AFactor))),
         (QuaternionScale(Quat2, AFactor))));
End;
//********************************************************************
// Spherical linear interpolation between two quaternions (SLERP)

//                      sin(1 - t)Alpha          sin(t)Alpha
// Slerp(q1, q2, t) :=  ---------------  * q1 +  -----------  * q2
//                         sin Alpha              sin Alpha

// where cos Alpha := q1 · q2

// Has been tested and found OK
Function QuaternionBlendSpheric(Quat1,Quat2: TQuaternion;
               Const AFactor: TFloat32): TQuaternion;
Var
   AlphaInRad,CosAlpha,SinAlpha,Scale0,Scale1: TFloat32;
Begin
   // Calculate angle between the two quaternions
   CosAlpha:= QuaternionDotProduct(Quat1,Quat2);
   AlphaInRad:= ArcCos(CosAlpha);
   SinAlpha:= Sin(AlphaInRad);

   // Consider negative cosinus (angles between 90 and 270 degree)
   If CosAlpha < 0
   Then Begin
      CosAlpha:= Abs(CosAlpha);
      Quat2:= QuaternionConjugate(Quat2);
   End;

   // Calculate spherical interpolation factors
   Scale0:= Sin((1-AFactor) * AlphaInRad) / SinAlpha;
   Scale1:= Sin(AFactor * AlphaInRad) / SinAlpha;

   With Result Do
   Begin
      RealW:= Scale0*Quat1.RealW + Scale1*Quat2.RealW;
      ImagX:= Scale0*Quat1.ImagX + Scale1*Quat2.ImagX;
      ImagY:= Scale0*Quat1.ImagY + Scale1*Quat2.ImagY;
      ImagZ:= Scale0*Quat1.ImagZ + Scale1*Quat2.ImagZ;
   End;
End;
//********************************************************************
// Has been tested and found OK
Function QuaternionConjugate(Const AQuat: TQuaternion): TQuaternion;
Begin
   With Result Do
   Begin
      ImagX:=-ImagX; ImagY:=-ImagY; ImagZ:=-ImagZ;
   End;
End;
//********************************************************************
// Has been tested and found OK
Function QuaternionDotProduct
               (Const Quat1,Quat2: TQuaternion): TFloat32;
Begin
   Result:= +Quat1.RealW * Quat2.RealW
            +Quat1.ImagX * Quat2.ImagX
            +Quat1.ImagY * Quat2.ImagY
            +Quat1.ImagZ * Quat2.ImagZ;
End;
//********************************************************************
Function QuaternionInverse(Const AQuat: TQuaternion): TQuaternion;
Begin
   Result:= QuaternionNormalize(QuaternionConjugate(AQuat));
End;
//********************************************************************
// Has been tested and found OK
Function QuaternionMultiply
               (Const ANormQuat1,ANormQuat2: TQuaternion): TQuaternion;
Begin
  With Result Do
  Begin
     RealW:= + ANormQuat1.RealW * ANormQuat2.RealW
             - ANormQuat1.ImagX * ANormQuat2.ImagX
             - ANormQuat1.ImagY * ANormQuat2.ImagY
             - ANormQuat1.ImagZ * ANormQuat2.ImagZ;

     ImagX:= + ANormQuat1.RealW * ANormQuat2.ImagX
             + ANormQuat1.ImagX * ANormQuat2.RealW
             + ANormQuat1.ImagY * ANormQuat2.ImagZ
             - ANormQuat1.ImagZ * ANormQuat2.ImagY;

     ImagY:= + ANormQuat1.RealW * ANormQuat2.ImagY
             - ANormQuat1.ImagX * ANormQuat2.ImagZ
             + ANormQuat1.ImagY * ANormQuat2.RealW
             + ANormQuat1.ImagZ * ANormQuat2.ImagX;

     ImagZ:= + ANormQuat1.RealW * ANormQuat2.ImagZ
             + ANormQuat1.ImagX * ANormQuat2.ImagY
             - ANormQuat1.ImagY * ANormQuat2.ImagX
             + ANormQuat1.ImagZ * ANormQuat2.RealW;
  End;
End;
//********************************************************************
Function QuaternionNormalize(Const AQuat: TQuaternion): TQuaternion;
Var QuatValue: TFloat32;
Begin
   QuatValue:= QuaternionValue(AQuat);
   If QuatValue <> 0
      Then With Result Do
      Begin
         RealW:= AQuat.RealW / QuatValue;
         ImagX:= AQuat.ImagX / QuatValue;
         ImagY:= AQuat.ImagY / QuatValue;
         ImagZ:= AQuat.ImagZ / QuatValue;
      End
      Else Result:= QUATIDENTITY;
End;
//********************************************************************
Function QuaternionScale(Const AQuat: TQuaternion;
               Const AFactor: TFloat32): TQuaternion;
Begin
   With Result Do
   Begin
      RealW:= AQuat.RealW * AFactor;
      ImagX:= AQuat.ImagX * AFactor;
      ImagY:= AQuat.ImagY * AFactor;
      ImagZ:= AQuat.ImagZ * AFactor;
   End;
End;
//********************************************************************
Function QuaternionValue(Const AQuat: TQuaternion): TFloat32;
Begin
   With AQuat Do
      Result:= RealW*RealW + ImagX*ImagX + ImagY*ImagY + ImagZ*ImagZ;
End;
//********************************************************************
//             Q U A T E R N I O N - Conversion
//********************************************************************
// Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm
Function AngleAxisToQuaternion
               (Const ARotation: TAngleAxis): TQuaternion;
Begin
   With ARotation,Axis Do Result:= Quaternion(Angle,X,Y,Z);
End;
//********************************************************************
// Source:http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
Function QuaternionToAngleAxis(Const AQuat: TQuaternion): TAngleAxis;
Var Scale: TFloat32;
Begin
   With AQuat Do Scale:= Sqrt(ImagX*ImagX+ImagY*ImagY+ImagZ*ImagZ);

   With Result Do If Scale <> 0
      Then Begin
         Angle:= 2*ArcCos(AQuat.RealW)/PIDIV180;
         Axis.X:= AQuat.ImagX/Scale;
         Axis.Y:= AQuat.ImagY/Scale;
         Axis.Z:= AQuat.ImagZ/Scale;
      End
      Else Begin
         Angle:= 0;
         Axis:= ZPLUSVECTOR;
      End;
End;
//********************************************************************
// Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
// Has been tested and found OK
Function QuaternionToMatrix(AQuat: TQuaternion): TMatrix4D;
Begin
   Result:= MATIDENTITY;

   AQuat:= QuaternionNormalize(AQuat);
   With AQuat Do Begin
      Result[0].X:= 1 - 2*ImagY*ImagY - 2*ImagZ*ImagZ;
      Result[0].Y:= 2*ImagX*ImagY + 2*RealW*ImagZ;
      Result[0].Z:= 2*ImagX*ImagZ - 2*RealW*ImagY;
      Result[0].W:= 0;

      Result[1].X:= 2*ImagX*ImagY - 2*RealW*ImagZ;
      Result[1].Y:= 1 - 2*ImagX*ImagX - 2*ImagZ*ImagZ;
      Result[1].Z:= 2*ImagY*ImagZ + 2*RealW*ImagX;
      Result[1].W:= 0;

      Result[2].X:= 2*ImagX*ImagZ + 2*RealW*ImagY;
      Result[2].Y:= 2*ImagY*ImagZ - 2*RealW*ImagX;
      Result[2].Z:= 1 - 2*ImagX*ImagX - 2*ImagY*ImagY;
      Result[2].W:= 0;
   End;
End;