/* Copyright (c) 2002-2012 Croteam Ltd. All rights reserved. */

#include "stdh.h"

#include <Engine/Math/Geometry.h>

#include <Engine/Math/Functions.h>

/////////////////////////////////////////////////////////////////////
//
//      General functions
//
/////////////////////////////////////////////////////////////////////

/*
 * Calculate rotation matrix from angles in 3D.
 */
/*void operator^=(FLOATmatrix3D &t3dRotation, const ANGLE3D &a3dAngles)
{
  MakeRotationMatrix(t3dRotation, a3dAngles);
}
*/

void MakeRotationMatrix(FLOATmatrix3D &t3dRotation, const ANGLE3D &a3dAngles)
{
  FLOAT fSinH = Sin(a3dAngles(1));  // heading
  FLOAT fCosH = Cos(a3dAngles(1));
  FLOAT fSinP = Sin(a3dAngles(2));  // pitch
  FLOAT fCosP = Cos(a3dAngles(2));
  FLOAT fSinB = Sin(a3dAngles(3));  // banking
  FLOAT fCosB = Cos(a3dAngles(3));

  t3dRotation(1,1) = fCosH*fCosB+fSinP*fSinH*fSinB;
  t3dRotation(1,2) = fSinP*fSinH*fCosB-fCosH*fSinB;
  t3dRotation(1,3) = fCosP*fSinH;
  t3dRotation(2,1) = fCosP*fSinB;
  t3dRotation(2,2) = fCosP*fCosB;
  t3dRotation(2,3) = -fSinP;
  t3dRotation(3,1) = fSinP*fCosH*fSinB-fSinH*fCosB;
  t3dRotation(3,2) = fSinP*fCosH*fCosB+fSinH*fSinB;
  t3dRotation(3,3) = fCosP*fCosH;
}
void MakeRotationMatrixFast(FLOATmatrix3D &t3dRotation, const ANGLE3D &a3dAngles)
{
  FLOAT fSinH = SinFast(a3dAngles(1));  // heading
  FLOAT fCosH = CosFast(a3dAngles(1));
  FLOAT fSinP = SinFast(a3dAngles(2));  // pitch
  FLOAT fCosP = CosFast(a3dAngles(2));
  FLOAT fSinB = SinFast(a3dAngles(3));  // banking
  FLOAT fCosB = CosFast(a3dAngles(3));

  t3dRotation(1,1) = fCosH*fCosB+fSinP*fSinH*fSinB;
  t3dRotation(1,2) = fSinP*fSinH*fCosB-fCosH*fSinB;
  t3dRotation(1,3) = fCosP*fSinH;
  t3dRotation(2,1) = fCosP*fSinB;
  t3dRotation(2,2) = fCosP*fCosB;
  t3dRotation(2,3) = -fSinP;
  t3dRotation(3,1) = fSinP*fCosH*fSinB-fSinH*fCosB;
  t3dRotation(3,2) = fSinP*fCosH*fCosB+fSinH*fSinB;
  t3dRotation(3,3) = fCosP*fCosH;
}

/*
 * Calculate inverse rotation matrix from angles in 3D.
 */
/*void operator!=(FLOATmatrix3D &t3dRotation, const ANGLE3D &a3dAngles)
{
  MakeInverseRotationMatrix(t3dRotation, a3dAngles);
}
*/
void MakeInverseRotationMatrix(FLOATmatrix3D &t3dRotation, const ANGLE3D &a3dAngles)
{
  FLOAT fSinH = Sin(a3dAngles(1));  // heading
  FLOAT fCosH = Cos(a3dAngles(1));
  FLOAT fSinP = Sin(a3dAngles(2));  // pitch
  FLOAT fCosP = Cos(a3dAngles(2));
  FLOAT fSinB = Sin(a3dAngles(3));  // banking
  FLOAT fCosB = Cos(a3dAngles(3));

  // to make inverse of rotation matrix, we only need to transpose it
  t3dRotation(1,1) = fCosH*fCosB+fSinP*fSinH*fSinB;
  t3dRotation(2,1) = fSinP*fSinH*fCosB-fCosH*fSinB;
  t3dRotation(3,1) = fCosP*fSinH;
  t3dRotation(1,2) = fCosP*fSinB;
  t3dRotation(2,2) = fCosP*fCosB;
  t3dRotation(3,2) = -fSinP;
  t3dRotation(1,3) = fSinP*fCosH*fSinB-fSinH*fCosB;
  t3dRotation(2,3) = fSinP*fCosH*fCosB+fSinH*fSinB;
  t3dRotation(3,3) = fCosP*fCosH;
}
void MakeInverseRotationMatrixFast(FLOATmatrix3D &t3dRotation, const ANGLE3D &a3dAngles)
{
  FLOAT fSinH = SinFast(a3dAngles(1));  // heading
  FLOAT fCosH = CosFast(a3dAngles(1));
  FLOAT fSinP = SinFast(a3dAngles(2));  // pitch
  FLOAT fCosP = CosFast(a3dAngles(2));
  FLOAT fSinB = SinFast(a3dAngles(3));  // banking
  FLOAT fCosB = CosFast(a3dAngles(3));

  // to make inverse of rotation matrix, we only need to transpose it
  t3dRotation(1,1) = fCosH*fCosB+fSinP*fSinH*fSinB;
  t3dRotation(2,1) = fSinP*fSinH*fCosB-fCosH*fSinB;
  t3dRotation(3,1) = fCosP*fSinH;
  t3dRotation(1,2) = fCosP*fSinB;
  t3dRotation(2,2) = fCosP*fCosB;
  t3dRotation(3,2) = -fSinP;
  t3dRotation(1,3) = fSinP*fCosH*fSinB-fSinH*fCosB;
  t3dRotation(2,3) = fSinP*fCosH*fCosB+fSinH*fSinB;
  t3dRotation(3,3) = fCosP*fCosH;
}

/*
 * Decompose rotation matrix into angles in 3D.
 */
// NOTE: for derivation of the algorithm, see mathlib.doc
void DecomposeRotationMatrixNoSnap(ANGLE3D &a3dAngles, const FLOATmatrix3D &t3dRotation)
{
  ANGLE &h=a3dAngles(1);  // heading
  ANGLE &p=a3dAngles(2);  // pitch
  ANGLE &b=a3dAngles(3);  // banking
  FLOAT a;            // temporary

  // calculate pitch
  FLOAT f23 = t3dRotation(2,3);
  p = ASin(-f23);
  a = Sqrt(1.0f-f23*f23);

  // if pitch makes banking beeing the same as heading
  if (a<0.001) {
    // we choose to have banking of 0
    b = 0;
    // and calculate heading for that
    ASSERT(Abs(t3dRotation(2,3))>0.5); // must be around 1, what is far from 0
    h = ATan2(t3dRotation(1,2)/(-t3dRotation(2,3)), t3dRotation(1,1));  // no division by 0
  // otherwise
  } else {
    // calculate banking and heading normally
    b = ATan2(t3dRotation(2,1), t3dRotation(2,2));
    h = ATan2(t3dRotation(1,3), t3dRotation(3,3));
  }
}

void DecomposeRotationMatrix(ANGLE3D &a3dAngles, const FLOATmatrix3D &t3dRotation)
{
  // decompose the matrix without snapping
  DecomposeRotationMatrixNoSnap(a3dAngles, t3dRotation);
  // snap angles to compensate for errors when converting to and from matrix notation
  Snap(a3dAngles(1), ANGLE_SNAP);
  Snap(a3dAngles(2), ANGLE_SNAP);
  Snap(a3dAngles(3), ANGLE_SNAP);
}

/*void operator^=(ANGLE3D &a3dAngles, const FLOATmatrix3D &t3dRotation) {
  DecomposeRotationMatrix(a3dAngles, t3dRotation);
}
*/

/*
 * Create direction vector from angles in 3D (ignoring banking).
 */
void AnglesToDirectionVector(const ANGLE3D &a3dAngles, FLOAT3D &vDirection)
{
  // find the rotation matrix from the angles
  FLOATmatrix3D mDirection;
  MakeRotationMatrix(mDirection, a3dAngles);
  // rotate a front oriented vector by the matrix
  vDirection = FLOAT3D(0.0f, 0.0f, -1.0f)*mDirection;
}

/*
 * Create angles in 3D from direction vector(ignoring banking).
 */
void DirectionVectorToAnglesNoSnap(const FLOAT3D &vDirection, ANGLE3D &a3dAngles)
{
  // now calculate the angles
  ANGLE &h = a3dAngles(1);
  ANGLE &p = a3dAngles(2);
  ANGLE &b = a3dAngles(3);

  const FLOAT &x = vDirection(1);
  const FLOAT &y = vDirection(2);
  const FLOAT &z = vDirection(3);

  // banking is always irrelevant
  b = 0;
  // calculate pitch
  p = ASin(y);

  // if y is near +1 or -1
  if (y>0.99 || y<-0.99) {
    // heading is irrelevant
    h = 0;
  // otherwise
  } else {
    // calculate heading
    h = ATan2(-x, -z);
  }
}
void DirectionVectorToAngles(const FLOAT3D &vDirection, ANGLE3D &a3dAngles)
{
  DirectionVectorToAnglesNoSnap(vDirection, a3dAngles);

  // snap angles to compensate for errors when converting to and from vector notation
  Snap(a3dAngles(1), ANGLE_SNAP);
  Snap(a3dAngles(2), ANGLE_SNAP);
  Snap(a3dAngles(3), ANGLE_SNAP);
}

/* Create angles in 3D from up vector (ignoring objects relative heading).
   (up vector must be normalized!)*/
void UpVectorToAngles(const FLOAT3D &vY, ANGLE3D &a3dAngles)
{
  // create any front vector
  FLOAT3D vZ;
  if (Abs(vY(2))>0.5f) {
    vZ = FLOAT3D(1,0,0)*vY;
  } else {
    vZ = FLOAT3D(0,1,0)*vY;
  }
  vZ.Normalize();
  // side vector is cross product
  FLOAT3D vX = vY*vZ;
  vX.Normalize();
  // create the rotation matrix
  FLOATmatrix3D m;
  m(1,1) = vX(1); m(1,2) = vY(1); m(1,3) = vZ(1);
  m(2,1) = vX(2); m(2,2) = vY(2); m(2,3) = vZ(2);
  m(3,1) = vX(3); m(3,2) = vY(3); m(3,3) = vZ(3);

  // decompose the matrix without snapping
  DecomposeRotationMatrixNoSnap(a3dAngles, m);
}

/*
 * Calculate rotation matrix from angles in 3D.
 */
void operator^=(DOUBLEmatrix3D &t3dRotation, const ANGLE3D &a3dAngles) {
  const ANGLE &h=a3dAngles(1);  // heading
  const ANGLE &p=a3dAngles(2);  // pitch
  const ANGLE &b=a3dAngles(3);  // banking

  t3dRotation(1,1) = Cos(h)*Cos(b)+Sin(p)*Sin(h)*Sin(b);
  t3dRotation(1,2) = Sin(p)*Sin(h)*Cos(b)-Cos(h)*Sin(b);
  t3dRotation(1,3) = Cos(p)*Sin(h);
  t3dRotation(2,1) = Cos(p)*Sin(b);
  t3dRotation(2,2) = Cos(p)*Cos(b);
  t3dRotation(2,3) = -Sin(p);
  t3dRotation(3,1) = Sin(p)*Cos(h)*Sin(b)-Sin(h)*Cos(b);
  t3dRotation(3,2) = Sin(p)*Cos(h)*Cos(b)+Sin(h)*Sin(b);
  t3dRotation(3,3) = Cos(p)*Cos(h);
}

/*
 * Calculate inverse rotation matrix from angles in 3D.
 */
void operator!=(DOUBLEmatrix3D &t3dRotation, const ANGLE3D &a3dAngles) {
  const ANGLE &h=a3dAngles(1);  // heading
  const ANGLE &p=a3dAngles(2);  // pitch
  const ANGLE &b=a3dAngles(3);  // banking

  // to make inverse of rotation matrix, we only need to transpose it
  t3dRotation(1,1) = Cos(h)*Cos(b)+Sin(p)*Sin(h)*Sin(b);
  t3dRotation(2,1) = Sin(p)*Sin(h)*Cos(b)-Cos(h)*Sin(b);
  t3dRotation(3,1) = Cos(p)*Sin(h);
  t3dRotation(1,2) = Cos(p)*Sin(b);
  t3dRotation(2,2) = Cos(p)*Cos(b);
  t3dRotation(3,2) = -Sin(p);
  t3dRotation(1,3) = Sin(p)*Cos(h)*Sin(b)-Sin(h)*Cos(b);
  t3dRotation(2,3) = Sin(p)*Cos(h)*Cos(b)+Sin(h)*Sin(b);
  t3dRotation(3,3) = Cos(p)*Cos(h);
}

/*
 * Decompose rotation matrix into angles in 3D.
 */
// NOTE: for derivation of the algorithm, see mathlib.doc
void operator^=(ANGLE3D &a3dAngles, const DOUBLEmatrix3D &t3dRotation) {
  ANGLE &h=a3dAngles(1);  // heading
  ANGLE &p=a3dAngles(2);  // pitch
  ANGLE &b=a3dAngles(3);  // banking
  DOUBLE a;            // temporary

  // calculate pitch
  p = ASin(-t3dRotation(2,3));
  a = sqrt(1-t3dRotation(2,3)*t3dRotation(2,3));

  // if pitch makes banking beeing the same as heading
  if (a<0.0001) {
    // we choose to have banking of 0
    b = 0;
    // and calculate heading for that
    ASSERT(Abs(t3dRotation(2,3))>0.5); // must be around 1, what is far from 0
    h = ATan2(t3dRotation(1,2)/(-t3dRotation(2,3)), t3dRotation(1,1));  // no division by 0
  // otherwise
  } else {
    // calculate banking and heading normally
    b = ATan2(t3dRotation(2,1)/a, t3dRotation(2,2)/a);
    h = ATan2(t3dRotation(1,3)/a, t3dRotation(3,3)/a);
  }
  // snap angles to compensate for errors when converting to and from matrix notation
  Snap(h, ANGLE_SNAP);
  Snap(p, ANGLE_SNAP);
  Snap(b, ANGLE_SNAP);
}