// gglMatrix.cpp

#include "stdafx.h"
#include <gl/gl.h>
#include <float.h> // for DBL_MIN
#include "gglMatrix.h"

void gglMatrix::Set(const GLdouble Mat[16]) {
  memcpy(M,Mat,16*sizeof GLdouble);
  GetInverse(M,I);
}

void gglMatrix::SetFromInverse(const GLdouble Inv[16]) {
  memcpy(I,Inv,16*sizeof GLdouble);
  GetInverse(I,M);
}

void gglMatrix::SetFromGL() { // Called from default constructor
  GLdouble T[16];
  glGetIntegerv(GL_VIEWPORT        ,  V);
  glGetDoublev (GL_MODELVIEW_MATRIX,  T);
  glGetDoublev (GL_PROJECTION_MATRIX, I);
  Multiply(T,I,M); // M=ModelView x Projection
  GetInverse(M,I);
}

namespace {int Round(GLdouble d) {return static_cast<int>(d+0.5);}};

// MUST have SetFromGL(); first!!!
bool gglMatrix::World2Pixel(GLdouble x, GLdouble y, GLdouble z, CPoint& Point) {
  GLdouble B[4];
  GLdouble A[4]={x,y,z,1};
  Transform(A,M,B);
  if(B[3]==0) return false;
  B[0]/=B[3];
  B[1]/=B[3];
  B[2]/=B[3];
  x=V[0]+((B[0]+1)*V[2])/2; // Map from (-1,1)
  y=V[1]+((B[1]+1)*V[3])/2; // Map from (-1,1)
  z=(1+B[2])/2;
  Point.SetPoint(Round(x),Round(V[3]-y));
  return true;
}

// MUST have SetFromGL(); first!!!
bool gglMatrix::Pixel2World(const CPoint& Point, GLdouble& x, GLdouble& y, GLdouble& z) {
  x=Point.x;
  y=V[3]-Point.y;
  float zFloat=0; // If nothing there, z is zero, otherwise find the depth:
  glReadPixels(Round(x), Round(y), 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &zFloat);
  z=zFloat;
  GLdouble in[4]={
    2*(x - V[0])/V[2]-1, // Map between (-1,1)
    2*(y - V[1])/V[3]-1, // Map between (-1,1)
    2* z             -1,
    1
  };
  GLdouble out[4];
  Transform(in, I, out);
  if(out[3]==0) return false;
  x=out[0]/out[3];
  y=out[1]/out[3];
  z=out[2]/out[3];
  return true;
}

void gglMatrix::Transform(const GLdouble P[4], const GLdouble M[16], GLdouble PxM[4]) {
  PxM[0]=P[0]*M[0] + P[1]*M[4] + P[2]*M[ 8] + P[3]*M[12];
  PxM[1]=P[0]*M[1] + P[1]*M[5] + P[2]*M[ 9] + P[3]*M[13];
  PxM[2]=P[0]*M[2] + P[1]*M[6] + P[2]*M[10] + P[3]*M[14];
  PxM[3]=P[0]*M[3] + P[1]*M[7] + P[2]*M[11] + P[3]*M[15];
}

void gglMatrix::Multiply(const GLdouble A[16], const GLdouble B[16], GLdouble AxB[16]) {
  AxB[ 0]=A[ 0]*B[0] + A[ 1]*B[4] + A[ 2]*B[ 8] + A[ 3]*B[12];
  AxB[ 1]=A[ 0]*B[1] + A[ 1]*B[5] + A[ 2]*B[ 9] + A[ 3]*B[13];
  AxB[ 2]=A[ 0]*B[2] + A[ 1]*B[6] + A[ 2]*B[10] + A[ 3]*B[14];
  AxB[ 3]=A[ 0]*B[3] + A[ 1]*B[7] + A[ 2]*B[11] + A[ 3]*B[15];
  AxB[ 4]=A[ 4]*B[0] + A[ 5]*B[4] + A[ 6]*B[ 8] + A[ 7]*B[12];
  AxB[ 5]=A[ 4]*B[1] + A[ 5]*B[5] + A[ 6]*B[ 9] + A[ 7]*B[13];
  AxB[ 6]=A[ 4]*B[2] + A[ 5]*B[6] + A[ 6]*B[10] + A[ 7]*B[14];
  AxB[ 7]=A[ 4]*B[3] + A[ 5]*B[7] + A[ 6]*B[11] + A[ 7]*B[15];
  AxB[ 8]=A[ 8]*B[0] + A[ 9]*B[4] + A[10]*B[ 8] + A[11]*B[12];
  AxB[ 9]=A[ 8]*B[1] + A[ 9]*B[5] + A[10]*B[ 9] + A[11]*B[13];
  AxB[10]=A[ 8]*B[2] + A[ 9]*B[6] + A[10]*B[10] + A[11]*B[14];
  AxB[11]=A[ 8]*B[3] + A[ 9]*B[7] + A[10]*B[11] + A[11]*B[15];
  AxB[12]=A[12]*B[0] + A[13]*B[4] + A[14]*B[ 8] + A[15]*B[12];
  AxB[13]=A[12]*B[1] + A[13]*B[5] + A[14]*B[ 9] + A[15]*B[13];
  AxB[14]=A[12]*B[2] + A[13]*B[6] + A[14]*B[10] + A[15]*B[14];
  AxB[15]=A[12]*B[3] + A[13]*B[7] + A[14]*B[11] + A[15]*B[15];
}

void gglMatrix::GetInverse(const GLdouble M[16], GLdouble I[16]) {
  double Determinant=GetDeterminant(M);
  if(Determinant==0) Determinant=2*DBL_MIN; // Avoid divide by zero
  I[ 0]=(M[5]*(M[10]*M[15]-M[11]*M[14]) + M[9]*(M[7]*M[14]-M[6]*M[15]) + M[13]*(M[6]*M[11]-M[7]*M[10]))/Determinant;
  I[ 1]=(M[1]*(M[11]*M[14]-M[10]*M[15]) + M[9]*(M[2]*M[15]-M[3]*M[14]) + M[13]*(M[3]*M[10]-M[2]*M[11]))/Determinant;
  I[ 2]=(M[1]*(M[ 6]*M[15]-M[ 7]*M[14]) + M[5]*(M[3]*M[14]-M[2]*M[15]) + M[13]*(M[2]*M[ 7]-M[3]*M[ 6]))/Determinant;
  I[ 3]=(M[1]*(M[ 7]*M[10]-M[ 6]*M[11]) + M[5]*(M[2]*M[11]-M[3]*M[10]) + M[ 9]*(M[3]*M[ 6]-M[2]*M[ 7]))/Determinant;
  I[ 4]=(M[4]*(M[11]*M[14]-M[10]*M[15]) + M[8]*(M[6]*M[15]-M[7]*M[14]) + M[12]*(M[7]*M[10]-M[6]*M[11]))/Determinant;
  I[ 5]=(M[0]*(M[10]*M[15]-M[11]*M[14]) + M[8]*(M[3]*M[14]-M[2]*M[15]) + M[12]*(M[2]*M[11]-M[3]*M[10]))/Determinant;
  I[ 6]=(M[0]*(M[ 7]*M[14]-M[ 6]*M[15]) + M[4]*(M[2]*M[15]-M[3]*M[14]) + M[12]*(M[3]*M[ 6]-M[2]*M[ 7]))/Determinant;
  I[ 7]=(M[0]*(M[ 6]*M[11]-M[ 7]*M[10]) + M[4]*(M[3]*M[10]-M[2]*M[11]) + M[ 8]*(M[2]*M[ 7]-M[3]*M[ 6]))/Determinant;
  I[ 8]=(M[4]*(M[ 9]*M[15]-M[11]*M[13]) + M[8]*(M[7]*M[13]-M[5]*M[15]) + M[12]*(M[5]*M[11]-M[7]*M[ 9]))/Determinant;
  I[ 9]=(M[0]*(M[11]*M[13]-M[ 9]*M[15]) + M[8]*(M[1]*M[15]-M[3]*M[13]) + M[12]*(M[3]*M[ 9]-M[1]*M[11]))/Determinant;
  I[10]=(M[0]*(M[ 5]*M[15]-M[ 7]*M[13]) + M[4]*(M[3]*M[13]-M[1]*M[15]) + M[12]*(M[1]*M[ 7]-M[3]*M[ 5]))/Determinant;
  I[11]=(M[0]*(M[ 7]*M[ 9]-M[ 5]*M[11]) + M[4]*(M[1]*M[11]-M[3]*M[ 9]) + M[ 8]*(M[3]*M[ 5]-M[1]*M[ 7]))/Determinant;
  I[12]=(M[4]*(M[10]*M[13]-M[ 9]*M[14]) + M[8]*(M[5]*M[14]-M[6]*M[13]) + M[12]*(M[6]*M[ 9]-M[5]*M[10]))/Determinant;
  I[13]=(M[0]*(M[ 9]*M[14]-M[10]*M[13]) + M[8]*(M[2]*M[13]-M[1]*M[14]) + M[12]*(M[1]*M[10]-M[2]*M[ 9]))/Determinant;
  I[14]=(M[0]*(M[ 6]*M[13]-M[ 5]*M[14]) + M[4]*(M[1]*M[14]-M[2]*M[13]) + M[12]*(M[2]*M[ 5]-M[1]*M[ 6]))/Determinant;
  I[15]=(M[0]*(M[ 5]*M[10]-M[ 6]*M[ 9]) + M[4]*(M[2]*M[ 9]-M[1]*M[10]) + M[ 8]*(M[1]*M[ 6]-M[2]*M[ 5]))/Determinant;
}

double gglMatrix::GetDeterminant(const GLdouble M[16]) {
  return M[ 0]*(M[5]*(M[10]*M[15]-M[14]*M[11]) + M[9]*(M[14]*M[ 7]-M[ 6]*M[15]) + M[13]*(M[ 6]*M[11]-M[10]*M[ 7]))
       + M[ 4]*(M[1]*(M[14]*M[11]-M[10]*M[15]) + M[9]*(M[ 2]*M[15]-M[14]*M[ 3]) + M[13]*(M[10]*M[ 3]-M[ 2]*M[11]))
       + M[ 8]*(M[1]*(M[ 6]*M[15]-M[14]*M[ 7]) + M[5]*(M[14]*M[ 3]-M[ 2]*M[15]) + M[13]*(M[ 2]*M[ 7]-M[ 6]*M[ 3]))
       + M[12]*(M[1]*(M[10]*M[ 7]-M[ 6]*M[11]) + M[5]*(M[ 2]*M[11]-M[10]*M[ 3]) + M[ 9]*(M[ 6]*M[ 3]-M[ 2]*M[ 7]));
}