/*
 * 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: m3d.cpp,v 1.1 2002/11/18 08:23:01 rst Exp $
 */
#define _MATH3D_EXPORT
#include "m2d.h"
#include "m3d.h"
#include "m4d.h"
#include <cmath>
#include <iostream>


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


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


/*!
 * Construction from homogenous coordinates
 * ([W*X, W*Y, W*Z, W] with scale factor W !=0 -> [X, Y, Z]).
 *
 * \param A 4-dimensional vector. 
 */
Math3d::M3d::M3d(const M4d& A)
{
  if (fabs(A.d_v[3])<EPSILON) {
    ASSERT(false);
    d_v[0]=d_v[1]=d_v[2]=0.0;
  }

  double c=1/A.d_v[3];
  d_v[0]=A.d_v[0]*c;
  d_v[1]=A.d_v[1]*c;
  d_v[2]=A.d_v[2]*c;
}


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


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


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


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


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


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


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


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


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


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


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


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


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


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


/*!
 *
 */
void
Math3d::M3d::add(const M3d& A, const M3d& 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]; 
}


/*!
 *
 */
void
Math3d::M3d::sub(const M3d& A, const M3d& 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]; 
}


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


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

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


/*!
 * If this point instance is of the form [W*X, W*Y, W], for some 
 * scale factor W != 0, then it is reset to be [X, Y, 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::M3d::cartesianize()
{
  if (fabs(d_v[2])<EPSILON) {
    ASSERT(false);
    d_v[0]=d_v[1]=d_v[2]=0.0;
  }
  else {
    double c=1/d_v[3];
    d_v[0]*=c;
    d_v[1]*=c;
    d_v[2]=1.0;
  }
}
      

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

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

/*!
 * If this point instance is of the form [W*X, W*Y, 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, 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::M3d::rationalize()
{
  if (fabs(d_v[2])<EPSILON) {
    ASSERT(false);
    d_v[0]=d_v[1]=d_v[2]=0.0;
  }
  else {
    double c=1/d_v[2];
    d_v[0]*=c;
    d_v[1]*=c;
    d_v[2]=1.0;
  }
}
     
/*!
 * If this point instance is of the form [X, Y, 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].
 */
void 
Math3d::M3d::homogenize()
{
  d_v[0]*=d_v[2];
  d_v[1]*=d_v[2];
}


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


/*!
 *
 */
void
Math3d::M3d::lerp(const M3d& A, const M3d& 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]);
}


/*!
 *
 */
void
Math3d::M3d::normal(const M3d& A, const M3d& B, const M3d& C)
{
  M3d P,Q;

  P.sub(C,B);
  Q.sub(A,B);
  cross(P,Q);
  normalize();
}


/*!
 *
 */
void
Math3d::M3d::min(const M3d& 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];
}


/*!
 *
 */
void
Math3d::M3d::max(const M3d& 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];
}


/*!
 *
 */
void
Math3d::M3d::cubic(const M3d& A, const M3d& TA, const M3d& TB, 
  const M3d& 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];
}


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


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

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


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


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


/*!
 *
 */
bool
Math3d::M3d::cmp(const M3d& 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)
  );
}



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


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


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