glfMatrix.cpp

/*
 This file is part of the BIAS library (Basic ImageAlgorithmS).

 Copyright (C) 2003-2009    (see file CONTACT for details)
 Multimediale Systeme der Informationsverarbeitung
 Institut fuer Informatik
 Christian-Albrechts-Universitaet Kiel


 BIAS is free software; you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation; either version 2.1 of the License, or
 (at your option) any later version.

 BIAS 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 Lesser General Public License for more details.

 You should have received a copy of the GNU Lesser General Public License
 along with BIAS; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "glfMatrix.hh"
#include "glfException.hh"

using namespace BIAS;

static inline Matrix4x4<float> ToFloat(const Matrix4x4<double>& m)
{
  return Matrix4x4<float>(
    (float)m[0][0], (float)m[0][1], (float)m[0][2], (float)m[0][3],
    (float)m[1][0], (float)m[1][1], (float)m[1][2], (float)m[1][3],
    (float)m[2][0], (float)m[2][1], (float)m[2][2], (float)m[2][3],
    (float)m[3][0], (float)m[3][1], (float)m[3][2], (float)m[3][3]);
}

static inline Matrix3x3<float> ToFloat(const Matrix3x3<double>& m)
{
  return Matrix3x3<float>(
    (float)m[0][0], (float)m[0][1], (float)m[0][2],
    (float)m[1][0], (float)m[1][1], (float)m[1][2],
    (float)m[2][0], (float)m[2][1], (float)m[2][2]);
}

static inline Vector3<float> ToFloat(const Vector3<double>& v)
{
  return Vector3<float>((float)v[0], (float)v[1], (float)v[2]);
}

const glfMatrix glfMatrix::IDENTITY(1.0f, 0.0f, 0.0f, 0.0f,
                                    0.0f, 1.0f, 0.0f, 0.0f,
                                    0.0f, 0.0f, 1.0f, 0.0f,
                                    0.0f, 0.0f, 0.0f, 1.0f);

glfMatrix::glfMatrix()
{
}

glfMatrix::glfMatrix(GLfloat m0, GLfloat m1, GLfloat m2, GLfloat m3,
                     GLfloat m4, GLfloat m5, GLfloat m6, GLfloat m7,
                     GLfloat m8, GLfloat m9, GLfloat m10, GLfloat m11,
                     GLfloat m12, GLfloat m13, GLfloat m14, GLfloat m15)
{
  m[0] = m0; m[4] = m4; m[ 8] = m8;  m[12] = m12;
  m[1] = m1; m[5] = m5; m[ 9] = m9;  m[13] = m13;
  m[2] = m2; m[6] = m6; m[10] = m10; m[14] = m14;
  m[3] = m3; m[7] = m7; m[11] = m11; m[15] = m15;
}

void glfMatrix::MakeIdentity()
{
  *this = IDENTITY;
}

void glfMatrix::MakeRotation(GLfloat angle, GLfloat x, GLfloat y, GLfloat z)
{
  // normalize (x,y,z) if length is not 1.
  GLfloat ls = x*x + y*y + z*z;
  if (fabs(ls - 1.0) > 0.00001) {
    GLfloat l = sqrtf(ls);
    GLfloat invL = 1.0 / l;
    x *= invL;
    y *= invL;
    z *= invL;
  }

  GLfloat c = cosf((angle / 180.0) * M_PI);
  GLfloat s = sinf((angle / 180.0) * M_PI);
  GLfloat ic = 1.0 - c;

  m[0] = x*x*ic+c;   m[4] = y*x*ic-z*s; m[ 8] = x*z*ic+y*s; m[12] = 0.0;
  m[1] = x*y*ic+z*s; m[5] = y*y*ic+c;   m[ 9] = y*z*ic-x*s; m[13] = 0.0;
  m[2] = x*z*ic-y*s; m[6] = y*z*ic+x*s; m[10] = z*z*ic+c;   m[14] = 0.0;
  m[3] = 0.0;        m[7] = 0.0;        m[11] = 0.0;        m[15] = 1.0;
}

void glfMatrix::MakeTranslation(GLfloat x, GLfloat y, GLfloat z)
{
  m[0] = 1.0f; m[4] = 0.0f; m[ 8] = 0.0f; m[12] = x;
  m[1] = 0.0f; m[5] = 1.0f; m[ 9] = 0.0f; m[13] = y;
  m[2] = 0.0f; m[6] = 0.0f; m[10] = 1.0f; m[14] = z;
  m[3] = 0.0f; m[7] = 0.0f; m[11] = 0.0f; m[15] = 1.0f;
}

void glfMatrix::MakeScalation(GLfloat x, GLfloat y, GLfloat z)
{
  m[0] =   x; m[4] = 0.0; m[ 8] = 0.0; m[12] = 0.0;
  m[1] = 0.0; m[5] =   y; m[ 9] = 0.0; m[13] = 0.0;
  m[2] = 0.0; m[6] = 0.0; m[10] =   z; m[14] = 0.0;
  m[3] = 0.0; m[7] = 0.0; m[11] = 0.0; m[15] = 1.0;
}

void glfMatrix::MakeFrustum(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat nearVal, GLfloat farVal)
{
  GLfloat A = (right+left)/(right-left);
  GLfloat B = (top+bottom)/(top-bottom);
  GLfloat C = -(farVal+nearVal)/(farVal-nearVal);
  GLfloat D = -2.0*(farVal*nearVal)/(farVal-nearVal);

  GLfloat E = 2*nearVal/(right - left);
  GLfloat F = 2*nearVal/(top - bottom);

  m[0] =   E; m[4] = 0.0; m[ 8] =    A; m[12] = 0.0;
  m[1] = 0.0; m[5] =   F; m[ 9] =    B; m[13] = 0.0;
  m[2] = 0.0; m[6] = 0.0; m[10] =    C; m[14] =   D;
  m[3] = 0.0; m[7] = 0.0; m[11] = -1.0; m[15] = 0.0;
}

void glfMatrix::MakeOrtho(GLfloat left, GLfloat right, GLfloat bottom, GLfloat top, GLfloat nearVal, GLfloat farVal)
{
  GLfloat tx = -(right+left)/(right-left);
  GLfloat ty = -(top+bottom)/(top-bottom);
  GLfloat tz = -(farVal+nearVal)/(farVal-nearVal);

  GLfloat a = 2.0/(right-left);
  GLfloat b = 2.0/(top-bottom);
  GLfloat c = -2.0/(farVal-nearVal);

  m[0] =    a; m[4] = 0.0f; m[ 8] = 0.0f; m[12] =   tx;
  m[1] = 0.0f; m[5] =    b; m[ 9] = 0.0f; m[13] =   ty;
  m[2] = 0.0f; m[6] = 0.0f; m[10] =    c; m[14] =   tz;
  m[3] = 0.0f; m[7] = 0.0f; m[11] = 0.0f; m[15] = 1.0f;
}

void glfMatrix::MakePerspective(GLfloat fovy, GLfloat aspect, GLfloat zNear, GLfloat zFar)
{
  GLfloat f = 1.0 / tan(0.5 * ((fovy / 180.0) * M_PI));

  GLfloat E = f / aspect;
  GLfloat F = f;
  GLfloat C = (zFar + zNear) / (zNear - zFar);
  GLfloat D = (2.0 * zFar * zNear) / (zNear - zFar);

  m[0] =    E; m[4] = 0.0f; m[ 8] =  0.0f; m[12] = 0.0f;
  m[1] = 0.0f; m[5] =    F; m[ 9] =  0.0f; m[13] = 0.0f;
  m[2] = 0.0f; m[6] = 0.0f; m[10] =     C; m[14] =    D;
  m[3] = 0.0f; m[7] = 0.0f; m[11] = -1.0f; m[15] = 0.0f;
}

void glfMatrix::MakeLookAt(GLfloat eyeX, GLfloat eyeY, GLfloat eyeZ,
                           GLfloat centerX, GLfloat centerY, GLfloat centerZ,
                           GLfloat upX, GLfloat upY, GLfloat upZ)
{
    Vector3<float> f(centerX - eyeX,
                     centerY - eyeY,
                     centerZ - eyeZ);
    f.Normalize();

    Vector3<float> up(upX, upY, upZ);
    up.Normalize();

    Vector3<float> s;
    s = f.CrossProduct(up);
    s.Normalize();

    Vector3<float> u;
    u = s.CrossProduct(f);

    glfMatrix a;
    a.m[0] =   s[0]; a.m[4] =   s[1]; a.m[ 8] =   s[2]; a.m[12] = 0.0f;
    a.m[1] =   u[0]; a.m[5] =   u[1]; a.m[ 9] =   u[2]; a.m[13] = 0.0f;
    a.m[2] =  -f[0]; a.m[6] =  -f[1]; a.m[10] =  -f[2]; a.m[14] = 0.0f;
    a.m[3] =   0.0f; a.m[7] =   0.0f; a.m[11] =   0.0f; a.m[15] = 1.0f;

    glfMatrix b;
    b.MakeTranslation(-eyeX, -eyeY, -eyeZ);
    
    *this = a * b;
}

void glfMatrix::Make(const Matrix4x4<float>& a)
{
  m[0] = a[0][0]; m[4] = a[0][1]; m[ 8] = a[0][2]; m[12] = a[0][3];
  m[1] = a[1][0]; m[5] = a[1][1]; m[ 9] = a[1][2]; m[13] = a[1][3];
  m[2] = a[2][0]; m[6] = a[2][1]; m[10] = a[2][2]; m[14] = a[2][3];
  m[3] = a[3][0]; m[7] = a[3][1]; m[11] = a[3][2]; m[15] = a[3][3];
}

void glfMatrix::Make(const Matrix4x4<double>& a)
{
  Make(ToFloat(a));
}

void glfMatrix::MakeAffine(const Matrix3x3<float>& r, const Vector3<float>& t)
{
  m[0] =  r[0][0]; m[4] =  r[0][1]; m[ 8] =  r[0][2]; m[12] = t[0];
  m[1] =  r[1][0]; m[5] =  r[1][1]; m[ 9] =  r[1][2]; m[13] = t[1];
  m[2] =  r[2][0]; m[6] =  r[2][1]; m[10] =  r[2][2]; m[14] = t[2];
  m[3] =     0.0f; m[7] =     0.0f; m[11] =     0.0f; m[15] = 1.0f;
}

void glfMatrix::MakeAffine(const Matrix3x3<double>& r, const Vector3<double>& t)
{
  MakeAffine(ToFloat(r), ToFloat(t));
}

void glfMatrix::MakeAffineInverse(const glfMatrix& a)
{
  GLfloat x = a.m[12];
  GLfloat y = a.m[13];
  GLfloat z = a.m[14];
    
  m[0] = a.m[0]; m[4] = a.m[1]; m[ 8] = a.m[ 2]; m[12] = -(x*a.m[0]+y*a.m[1]+z*a.m[2]);
  m[1] = a.m[4]; m[5] = a.m[5]; m[ 9] = a.m[ 6]; m[13] = -(x*a.m[4]+y*a.m[5]+z*a.m[6]);
  m[2] = a.m[8]; m[6] = a.m[9]; m[10] = a.m[10]; m[14] = -(x*a.m[8]+y*a.m[9]+z*a.m[10]);
  m[3] =   0.0f; m[7] =   0.0f; m[11] =    0.0f; m[15] = 1.0f;
}

void glfMatrix::MakeTextureMatrix(const ProjectionParametersPerspective& params, bool flip)
{
  unsigned int uiWidth, uiHeight;
  params.GetImageSize(uiWidth, uiHeight);
  double width = static_cast<double>(uiWidth);
  double height = static_cast<double>(uiHeight);

  KMatrix K = params.GetK();
  BIASASSERT(K[0][1]==0.0);
  Matrix4x4<double> embeddedK;
  embeddedK.SetZero();
  for(unsigned int i =0; i<3; i++) {
   for(unsigned int j =0; j<3; j++) {
     embeddedK[i][j] = static_cast<double>(K[i][j]);
   }
  }

  //  embeddedK[2][2] = 0.0; //deletes z
  embeddedK[3][2] = 1.0; //gets homogenouse 2D part also into 4th component

  embeddedK[0][0] /= width;
  embeddedK[0][2] += 0.5;
  embeddedK[0][2] /= width;

  embeddedK[1][1] /= height;
  embeddedK[1][2] += 0.5;
  embeddedK[1][2] /= height;

  if(flip) {
    embeddedK[1][1] *= -1.0;
    embeddedK[1][2] *= -1.0;
    embeddedK[1][2] += 1.0;
  }

  Matrix4x4<double> res;
  embeddedK.Mult(params.GetPose().GetGlobalToLocalTransform(), res);
  Make(res);
}


void glfMatrix::MakeViewMatrix(const ProjectionParametersBase& params)
{
  RMatrix R = params.GetR();
  Vector3<double> C = params.GetC();

  // flip y and z axis
  for(unsigned int i=0; i<3; i++) {
    R[i][1] = -R[i][1];
    R[i][2] = -R[i][2];
  }

  Matrix4x4<double> adaptedExtrinsics(R.Transpose(),
                                      (-1.0)*R.Transpose()*C);
  Make(adaptedExtrinsics);
}

void glfMatrix::MakeViewMatrixNew(const ProjectionParametersBase& params)
{
  Make(params.GetPose().GetGlobalToLocalTransform());
}

void glfMatrix::MakeProjectionMatrix(const ProjectionParametersPerspective& params,
                                     GLfloat nearZZ, GLfloat farZ, bool useIdealK)
{
  unsigned int uiWidth, uiHeight;
  if (useIdealK)
    params.GetIdealImageSize(uiWidth, uiHeight);
  else
    params.GetImageSize(uiWidth, uiHeight);

  MakeProjectionMatrix(params, nearZZ, farZ, 0, 0, 
                       uiWidth, uiHeight, useIdealK);
}

void glfMatrix::MakeProjectionMatrixNew(const ProjectionParametersPerspective& params,
                                        GLfloat nearZ, GLfloat farZ, bool flip) 
{
  unsigned int width, height;
  params.GetImageSize(width, height);

  MakeProjectionMatrixNew(params, nearZ, farZ, 0,0, width, height, false, flip);
}

void glfMatrix::MakeProjectionMatrixNew(const ProjectionParametersPerspective& params,
                                        GLfloat nearZ, GLfloat farZ,
                                        unsigned int x0, unsigned int y0,
                                        unsigned int width, unsigned int height,
                                        bool useIdealK, bool flip)
{
  BIASASSERT(nearZ<farZ);
  BIASASSERT(nearZ>0.0);

  KMatrix K;

  if (useIdealK) {    
    params.GetIdealK(K);    
  } else   {
    K = params.GetK();
  }

  Matrix4x4<double> embededK;
  embededK.SetZero();
  for(unsigned int i=0; i<3; i++) {    
    for(unsigned int j=0; j<3; j++) {
      embededK[i][j] = K[i][j];//
    }
    //embededK[i][3] = K[i][2];//principle point    
  }
  embededK[2][2] = 1;//propagating z to new z
  embededK[3][3] = 1;//propagating w to new w

 // std::cout<<"embededK = "<<embededK<<std::endl;

  Matrix4x4<double> normalizingK;
  normalizingK.SetZero();
  normalizingK[0][0] = 2.0/static_cast<double>(width); //kxx
  normalizingK[0][2] = (1.0 - 2.0*static_cast<double>(x0))/static_cast<double>(width) - 1.0; //kxz
  if(!flip) {
    normalizingK[1][1] = -2.0/static_cast<double>(height); //kyy
    normalizingK[1][2] = (2.0*static_cast<double>(y0)-1.0)/static_cast<double>(height) + 1.0; //kyz
  } else {
    normalizingK[1][1] = 2.0/static_cast<double>(height); //kyy
    normalizingK[1][2] = (1.0-2.0*static_cast<double>(y0))/static_cast<double>(height) - 1.0; //kyz
  }
  normalizingK[2][2] = (farZ + nearZ)/(farZ - nearZ); //kzz
  normalizingK[2][3] = -(1+normalizingK[2][2])*nearZ; //kzw
  
  normalizingK[3][2] = 1.0; 

 // std::cout<<"normalizingK = "<<normalizingK<<std::endl;
  
  Matrix4x4<double> res = normalizingK*embededK;
  Make(res);
}

void 
glfMatrix::MakeTextureMatrixNew(const ProjectionParametersPerspective& params, bool flip)
{
  unsigned int width, height;
  params.GetImageSize(width, height);

  KMatrix K;
  K = params.GetK();

  Matrix4x4<double> embededK;
  embededK.SetZero();
  for(unsigned int i=0; i<3; i++) {    
    for(unsigned int j=0; j<3; j++) {
      embededK[i][j] = K[i][j];//
    }
    //embededK[i][3] = K[i][2];//principle point    
  }
  embededK[2][2] = 1;//propagating z to new z
  embededK[3][3] = 1;//propagating w to new w

  Matrix4x4<double> normalizingK;
  normalizingK.SetZero();
  normalizingK[0][0] = 1.0/static_cast<double>(width); //kxx
  normalizingK[0][2] = 1.0/(2.0*static_cast<double>(width)); //kxz
  if(!flip) {
    normalizingK[1][1] = 1.0/static_cast<double>(height); //kyy
    normalizingK[1][2] = 1.0/(2.0*static_cast<double>(height)); //kyz
  } else {
    normalizingK[1][1] = -1.0/static_cast<double>(height); //kyy
    normalizingK[1][2] = 1.0-1.0/(2.0*static_cast<double>(height)); //kyz
  }
  normalizingK[2][2] = 1.0; //kzz
  normalizingK[2][3] = 0; //kzw
  
  normalizingK[3][2] = 1.0; 

  Matrix4x4<double> classicP = embededK*params.GetPose().GetGlobalToLocalTransform();
  Matrix4x4<double> res = normalizingK*classicP;
  Make(res);
}

void glfMatrix::MakeProjectionMatrix(ProjectionParametersPerspective params,
                                     GLfloat nearZ, GLfloat farZ,
                                     unsigned int x0, unsigned int y0,
                                     unsigned int width, unsigned int height,
                                     bool useIdealK)
{

  BIASASSERT(nearZ<farZ);
  BIASASSERT(nearZ>0.0);

  double originX = x0;
  double originY = y0;

  if (useIdealK) {
    KMatrix K;
    params.GetIdealK(K);
    params.SetK(K);
  }

   

//   Vector2<double> clippingWindowUL, clippingWindowLR;
  // shift by half pixel because in BIAS we assume that
  // samples lie on half pixels
  // need to shift by +0.5 in y, because y axis was flipped by view matrix.
  //   clippingWindowUL[0] = ((originX-0.5 - K[0][2]) / K[0][0]) * nearZ;
//   clippingWindowUL[1] = ((originY+0.5 - K[1][2]) / K[1][1]) * nearZ;
//   clippingWindowLR[0] = ((originX+windowWidth-0.5 - K[0][2]) / K[0][0]) * nearZ;
//   clippingWindowLR[1] = ((originY+windowHeight+0.5 - K[1][2]) / K[1][1]) * nearZ;

  //use unprojection to find image plane
  Vector3<double> clippingWindowUL, clippingWindowLR, p;
  params.UnProjectLocal(HomgPoint2D(originX-0.5, originY-0.5), p, clippingWindowUL);
  params.UnProjectLocal(HomgPoint2D(originX+width-0.5, originY+height-0.5), p, clippingWindowLR);
  clippingWindowUL *= (nearZ/clippingWindowUL[2]);
  clippingWindowLR *= (nearZ/clippingWindowLR[2]);
  
  const double left = clippingWindowUL[0];
  const double right = clippingWindowLR[0];

  //we have to flip here ! because GL is using a projection into a 
  // LEFT HAND coordinate frame!!!! 
  //orginX, originY is the UPPER RIGHT corner of the visible region
  //in the BIAS image coordiante frame, which is motivated by a 
  // system transforming purley RIGHT HAND coordinates. 
  // originX+width, originY+height thus is the lower right corner of the
  // image for bias!
  const double bottom = -clippingWindowLR[1];
  const double top = -clippingWindowUL[1];

  double deltaX = right - left;
  double deltaY = top - bottom;
  double deltaZ = nearZ - farZ;

  //implementation of matrix specified in OpenGL sepcs
  //used in glFrustum
  Matrix4x4<double> res;
  res.SetZero();
  res[0][0] = 2.0*nearZ / deltaX;
//   res[0][2] = (clippingWindowLR[0] + clippingWindowUL[0]) / deltaX;
  res[0][2] = (right+left)/deltaX;

  res[1][1] = 2.0*nearZ / deltaY;
//   res[1][2] = (clippingWindowLR[1] + clippingWindowUL[1]) / deltaY;
  res[1][2] = (bottom+top)/deltaY;
  res[2][2] =  (farZ + nearZ) / (deltaZ);
  res[2][3] =  (2.0*farZ*nearZ) / (deltaZ);

  res[3][2] =  -1.0;

  Make(res);
}

void glfMatrix::TransformPoint(const Vector3<float>& in, Vector3<float>& out) const
{
    out[0] = in[0]*m[ 0] + in[1]*m[ 4] + in[2]*m[ 8] + m[12];
    out[1] = in[0]*m[ 1] + in[1]*m[ 5] + in[2]*m[ 9] + m[13];
    out[2] = in[0]*m[ 2] + in[1]*m[ 6] + in[2]*m[10] + m[14];

    GLfloat invW = 1.0 / (in[0]*m[3] + in[1]*m[7] + in[2]*m[11] + m[15]);
    out[0] *= invW;
    out[1] *= invW;
    out[2] *= invW;
}

void glfMatrix::TransformPoint(const Vector3<double>& in, Vector3<double>& out) const
{
  Vector3<float> temp;
  TransformPoint(ToFloat(in), temp);
  out = Vector3<double>(temp[0], temp[1], temp[2]);
}

void glfMatrix::Multiply(const glfMatrix& a)
{
  *this = *this * a;
}

void glfMatrix::operator*=(const glfMatrix& a)
{
  *this = *this * a;
}

glfMatrix glfMatrix::operator*(const glfMatrix& a)
{
  glfMatrix r;
  for (int i = 0; i < 4; i++) {
    for (int j = 0; j < 4; j++) {
      GLfloat c = 0.0;
      for (int k = 0; k < 4; k++) {
        c += m[i+k*4] * a.m[k+j*4];
      }
      r.m[i+j*4] = c;
    }
  }
  return r;
}

unsigned int glfMatrix::Matrix2DIndexToRep(unsigned int row, unsigned int column) 
{
  return column*4 + row;
}

void glfMatrix::Multiply(BIAS::Vector4<double> vIn, BIAS::Vector4<double>& vOut)
{
  for(unsigned int r=0; r<4; r++ ) {
    vOut[r] = 0;
    for(unsigned int c=0; c<4; c++ ) {
      unsigned int i = Matrix2DIndexToRep(r,c);
      vOut[r] += m[i]*vIn[c];
    }
  }
}

void glfMatrix::Load() const
{
  glLoadMatrixf(m);
}