/*
 * Math3d - The 3D Computer Graphics Math Library
 * Copyright (C) 1996-2000 by J.E. Hoffmann <je-h@gmx.net>
 * All rights reserved.
 *
 * This program 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.
 *
 * 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 Lesser General Public  
 * License for more details.
 *
 * You should  have received  a copy of the GNU Lesser General Public License
 * along with  this program;  if not, write to the  Free Software Foundation,
 * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 * $Id: m4d.cpp,v 1.1 2002/11/18 08:23:01 rst Exp $
 */
#define _MATH3D_EXPORT
#include <math3d/m4d.h>
#include <math3d/m3d.h>
#include <cmath>
#include <iostream>


/*!
 * Construction from cartesian coordinates
 * ([X, Y, Z] -> [X, Y, Z, 1]).
 *
 * \param A 3-dimensional vector. 
 */
Math3d::M4d::M4d(const M3d& A)
{
  d_v[0]=A.d_v[0];
  d_v[1]=A.d_v[1];
  d_v[2]=A.d_v[2];
  d_v[3]=1.0;
}


/*!
 * Standard copy constuctor.
 */
Math3d::M4d::M4d(const M4d& A)
{
  d_v[0]=A.d_v[0];
  d_v[1]=A.d_v[1];
  d_v[2]=A.d_v[2];
  d_v[3]=A.d_v[3];
}


/*!
 * Standard assignment operator.
 */
const Math3d::M4d&
Math3d::M4d::operator=(const M4d& A)
{
  d_v[0]=A.d_v[0];
  d_v[1]=A.d_v[1];
  d_v[2]=A.d_v[2];
  d_v[3]=A.d_v[3];
  return(*this);
}


/*!
 *
 */
void
Math3d::M4d::zero()
{
  d_v[0]=d_v[1]=d_v[2]=d_v[3]=0.0;
}


/*!
 *
 */
void
Math3d::M4d::copy(const M4d& A)
{
  d_v[0]=A.d_v[0];
  d_v[1]=A.d_v[1];
  d_v[2]=A.d_v[2];
  d_v[3]=A.d_v[3];
}


/*!
 *
 */
double&
Math3d::M4d::get(int i)
{
  ASSERT(i>=0 && i<4);
  return(d_v[i]);
}


/*!
 *
 */
Math3d::M4d
Math3d::M4d::operator+(const M4d& A)
{
  return(M4d(
    d_v[0]+A.d_v[0],
    d_v[1]+A.d_v[1],
    d_v[2]+A.d_v[2],
    d_v[3]+A.d_v[3]
  ));
}


/*!
 *
 */
Math3d::M4d
Math3d::M4d::operator+()
{
  return(*this);
}


/*!
 *
 */
const Math3d::M4d&
Math3d::M4d::operator+=(const M4d& A)
{
  d_v[0]+=A.d_v[0];
  d_v[1]+=A.d_v[1];
  d_v[2]+=A.d_v[2];
  d_v[3]+=A.d_v[3];
  return(*this);
}


/*!
 *
 */
Math3d::M4d
Math3d::M4d::operator-(const M4d& A)
{
  return(M4d(
    d_v[0]-A.d_v[0],
    d_v[1]-A.d_v[1],
    d_v[2]-A.d_v[2],
    d_v[3]-A.d_v[3]
  ));
}


/*!
 *
 */
Math3d::M4d
Math3d::M4d::operator-()
{
  return(M4d(-d_v[0],-d_v[1],-d_v[2],-d_v[3]));
}


/*!
 *
 */
const Math3d::M4d&
Math3d::M4d::operator-=(const M4d& A)
{
  d_v[0]-=A.d_v[0];
  d_v[1]-=A.d_v[1];
  d_v[2]-=A.d_v[2];
  d_v[3]-=A.d_v[3];
  return(*this);
}


/*!
 *
 */
Math3d::M4d
Math3d::M4d::operator*(double k)
{
  return(M4d(
    d_v[0]*k,
    d_v[1]*k,
    d_v[2]*k,
    d_v[3]*k
  ));
}


/*!
 *
 */
const Math3d::M4d&
Math3d::M4d::operator*=(double k)
{
  d_v[0]*=k;
  d_v[1]*=k;
  d_v[2]*=k;
  d_v[3]*=k;
  return(*this);
}


/*!
 *
 */
void
Math3d::M4d::neg()
{
  d_v[0]=-d_v[0];
  d_v[1]=-d_v[1];
  d_v[2]=-d_v[2];
  d_v[3]=-d_v[3];
}


/*!
 *
 */
void
Math3d::M4d::abs()
{
  d_v[0]=fabs(d_v[0]);
  d_v[1]=fabs(d_v[1]);
  d_v[2]=fabs(d_v[2]);
  d_v[3]=fabs(d_v[3]);
}


/*!
 *
 */
void
Math3d::M4d::add(const M4d& A, const M4d& B)
{
  d_v[0]=A.d_v[0] + B.d_v[0]; 
  d_v[1]=A.d_v[1] + B.d_v[1]; 
  d_v[2]=A.d_v[2] + B.d_v[2]; 
  d_v[3]=A.d_v[3] + B.d_v[3]; 
}


/*!
 *
 */
void
Math3d::M4d::sub(const M4d& A, const M4d& B)
{
  d_v[0]=A.d_v[0] - B.d_v[0]; 
  d_v[1]=A.d_v[1] - B.d_v[1]; 
  d_v[2]=A.d_v[2] - B.d_v[2]; 
  d_v[3]=A.d_v[3] - B.d_v[3]; 
}


/*!
 *
 */
void
Math3d::M4d::scalar(double k)
{
  d_v[0]*=k;
  d_v[1]*=k;
  d_v[2]*=k;
  d_v[3]*=k;
}


/*!
 *
 */
void
Math3d::M4d::normalize()
{
  double l,c;

  l=length();
  if (fabs(l)<EPSILON) {
    d_v[0]=d_v[1]=d_v[2]=d_v[2]=0.0;
  }
  else {
    c=1.0/l;
    d_v[0]*=c;
    d_v[1]*=c;
    d_v[2]*=c;
    d_v[3]*=c;
  }
}


/*!
 * If this point instance is of the form [W*X, W*Y, W*Z, W], for some 
 * scale factor W != 0, then it is reset to be [X, Y, Z, 1]. This will 
 * only work correctly if the point is in homogenous form or cartesian 
 * form. If the point is in rational form, the results are not defined.
 */
void 
Math3d::M4d::cartesianize()
{
  if (fabs(d_v[3])<EPSILON) {
    ASSERT(false);
    d_v[0]=d_v[1]=d_v[2]=d_v[3]=0.0;
  }
  else {
    double c=1/d_v[3];
    d_v[0]*=c;
    d_v[1]*=c;
    d_v[2]*=c;
    d_v[3]=1.0;
  }
}
      

/*!
 * If this point instance is of the form [W*X, W*Y, W*Z, W], for some 
 * scale factor W != 0, then [X, Y, Z] is returned. 
 *
 * \return The cartesianized coordinates.
 */
Math3d::M3d 
Math3d::M4d::cartesianized()
{
  if (fabs(d_v[3])<EPSILON) {
    ASSERT(false);
    return(M3d(0,0,0));
  }

  double c=1/d_v[3];
  M3d A;
  A[0]=d_v[0]*c;
  A[1]=d_v[1]*c;
  A[2]=d_v[2]*c;
  return(A);
}
      

/*!
 * If this point instance is of the form [W*X, W*Y, W*Z, W] (ie. is in 
 * homogenous or (for W==1) cartesian form), for some scale factor W != 0,
 * then it is reset to be [X, Y, Z, W]. This will only work correctly if 
 * the point is in homogenous or cartesian form. If the point is already
 * in rational form, the resultsare not defined.
 */
void 
Math3d::M4d::rationalize()
{
  if (fabs(d_v[3])<EPSILON) {
    ASSERT(false);
    d_v[0]=d_v[1]=d_v[2]=d_v[3]=0.0;
  }
  else {
    double c=1/d_v[3];
    d_v[0]*=c;
    d_v[1]*=c;
    d_v[2]*=c;
    d_v[3]=1.0;
  }
}
     
/*!
 * If this point instance is of the form [X, Y, Z, W] (ie. is in rational
 * or (for W==1) cartesian form), for some scale factor W != 0, then it is
 * reset to be [W*X, W*Y, W*Z, W].
 */
void 
Math3d::M4d::homogenize()
{
  d_v[0]*=d_v[3];
  d_v[1]*=d_v[3];
  d_v[2]*=d_v[3];
}


/*!
 *
 */
void
Math3d::M4d::lerp(const M4d& A, const M4d& B, double t)
{
  d_v[0]=A.d_v[0]+t*(B.d_v[0]-A.d_v[0]);
  d_v[1]=A.d_v[1]+t*(B.d_v[1]-A.d_v[1]);
  d_v[2]=A.d_v[2]+t*(B.d_v[2]-A.d_v[2]);
  d_v[3]=A.d_v[3]+t*(B.d_v[3]-A.d_v[3]);
}


/*!
 *
 */
void
Math3d::M4d::min(const M4d& m)
{
  if (m.d_v[0]<d_v[0]) d_v[0]=m.d_v[0];
  if (m.d_v[1]<d_v[1]) d_v[1]=m.d_v[1];
  if (m.d_v[2]<d_v[2]) d_v[2]=m.d_v[2];
  if (m.d_v[3]<d_v[3]) d_v[3]=m.d_v[3];
}


/*!
 *
 */
void
Math3d::M4d::max(const M4d& m)
{
  if (m.d_v[0]>d_v[0]) d_v[0]=m.d_v[0];
  if (m.d_v[1]>d_v[1]) d_v[1]=m.d_v[1];
  if (m.d_v[2]>d_v[2]) d_v[2]=m.d_v[2];
  if (m.d_v[3]>d_v[3]) d_v[3]=m.d_v[3];
}


/*!
 *
 */
void
Math3d::M4d::cubic(const M4d& A, const M4d& TA, const M4d& TB, const M4d& B, double t)
{
  double a,b,c,d;   

  a=2*t*t*t - 3*t*t + 1;
  b=-2*t*t*t + 3*t*t;
  c=t*t*t - 2*t*t + t;
  d=t*t*t - t*t;
  d_v[0]=a*A.d_v[0] + b*B.d_v[0] + c*TA.d_v[0] + d*TB.d_v[0];
  d_v[1]=a*A.d_v[1] + b*B.d_v[1] + c*TA.d_v[1] + d*TB.d_v[1];
  d_v[2]=a*A.d_v[2] + b*B.d_v[2] + c*TA.d_v[2] + d*TB.d_v[2];
  d_v[3]=a*A.d_v[3] + b*B.d_v[3] + c*TA.d_v[3] + d*TB.d_v[3];
}


/*!
 *
 */
double
Math3d::M4d::get(int i) const
{
  ASSERT(i>=0 && i<4);
  return(d_v[i]);
}


/*!
 *
 */
double
Math3d::M4d::operator*(const M4d& A) const
{
  return(d_v[0]*A.d_v[0] + d_v[1]*A.d_v[1] + d_v[2]*A.d_v[2] + d_v[3]*A.d_v[3]);
}


/*!
 *
 */
bool
Math3d::M4d::operator==(const M4d& A) const
{
  return(cmp(A));
}


/*!
 *
 */
bool
Math3d::M4d::operator!=(const M4d& A) const
{
  return(!cmp(A));
}


/*!
 *
 */
double
Math3d::M4d::dot(const M4d& A) const
{
  return(d_v[0]*A.d_v[0] + d_v[1]*A.d_v[1] + d_v[2]*A.d_v[2] + d_v[3]*A.d_v[3]);
}


/*!
 *
 */
bool
Math3d::M4d::cmp(const M4d& A, double epsilon) const
{
  return(
    (fabs(d_v[0]-A.d_v[0])<epsilon) &&
    (fabs(d_v[1]-A.d_v[1])<epsilon) &&
    (fabs(d_v[2]-A.d_v[2])<epsilon) &&
    (fabs(d_v[3]-A.d_v[3])<epsilon)
  );
}


/*!
 *
 */
double
Math3d::M4d::squared() const
{
  return(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2] + d_v[3]*d_v[3]);
}


/*!
 *
 */
double
Math3d::M4d::length() const
{
  return(sqrt(d_v[0]*d_v[0] + d_v[1]*d_v[1] + d_v[2]*d_v[2] + d_v[3]*d_v[3]));
}


/*!
 *
 */
ostream& 
Math3d::operator << (ostream& co, const M4d& v)
{
  co << "(" << v[0] << ", " << v[1] << ", " << v[2] << "," << v[3] << ")";
  return co;
}