#include <stdlib.h>
#include <stdio.h>
#include <math.h>

#include "MatriceD.h"
#include "Coord3DD.h"
#include "Vecte3DD.h"

MatriceD::MatriceD(void) {
  for ( int i = 0 ; i < 4 ; i++ )
    for ( int j = 0 ; j < 4 ; j++ )
      m[i][j] = ( i == j ) ? 1.0F : 0.0F;
}

MatriceD::MatriceD(double *t) {
  for ( int i = 0 ; i < 16 ; i++ )
    m[i/4][i%4] = t[i];
}

MatriceD::~MatriceD(void) {
}

void MatriceD::toIdentite(void) {
  for ( int i = 0 ; i < 4 ; i++ )
    for ( int j = 0 ; j < 4 ; j++ )
      m[i][j] = ( i == j ) ? 1.0F : 0.0F;
}

void MatriceD::toRotationX(double a) {
  double aa = a*0.01745329251994329576923690768489;
  toRotationXRadian(aa);
}

void MatriceD::toRotationY(double a) {
  double aa = a*0.01745329251994329576923690768489;
  toRotationYRadian(aa);
}

void MatriceD::toRotationZ(double a) {
  double aa = a*0.01745329251994329576923690768489;
  toRotationZRadian(aa);
}

void MatriceD::toRotation(double a,double ax,double ay,double az) {
  double aa = a*0.01745329251994329576923690768489;
  toRotationRadian(aa,ax,ay,az);
}

void MatriceD::toRotationTransposee(double a,double ax,double ay,double az) {
  double aa = a*0.01745329251994329576923690768489;
  toRotationRadianTransposee((double) aa,ax,ay,az);
}

void MatriceD::toRotationXRadian(double a) {
  toIdentite();
  m[2][1] =(double) sin(a);
  m[1][2] = -m[2][1];
  m[1][1] = m[2][2] =(double) cos(a);
}

void MatriceD::toRotationYRadian(double a) {
  toIdentite();
  m[0][2] =(double) sin(a);
  m[2][0] = -m[0][2];
  m[0][0] = m[2][2] =(double) cos(a);
}

void MatriceD::toRotationZRadian(double a) {
  toIdentite();
  m[1][0] =(double) sin(a);
  m[0][1] = -m[1][0];
  m[1][1] = m[0][0] =(double) cos(a);
}

void MatriceD::toRotationRadian(double a,double ax,double ay,double az) {
  toIdentite();
  Vecte3DD *v = new Vecte3DD(ax,ay,az);
  v->normalise();
  double sn =(double) sin(a);
  double cs =(double) cos(a);
  m[0][0] = v->x*v->x+cs*(1.0-v->x*v->x);
  m[0][1] = v->x*v->y*(1.0-cs)-sn*v->z;
  m[0][2] = v->x*v->z*(1.0-cs)+sn*v->y;
  m[1][0] = v->x*v->y*(1.0-cs)+sn*v->z;
  m[1][1] = v->y*v->y+cs*(1.0-v->y*v->y);
  m[1][2] = v->y*v->z*(1.0-cs)-sn*v->x;
  m[2][0] = v->x*v->z*(1.0-cs)-sn*v->y;
  m[2][1] = v->y*v->z*(1.0-cs)+sn*v->x;
  m[2][2] = v->z*v->z+cs*(1.0-v->z*v->z);
  delete(v);
}

void MatriceD::toRotationRadianTransposee(double a,double ax,double ay,double az) {
  toIdentite();
  Vecte3DD *v = new Vecte3DD(ax,ay,az);
  v->normalise();
  double sn =(double) sin(a);
  double cs =(double) cos(a);
  m[0][0] = v->x*v->x+cs*(1-v->x*v->x);
  m[1][0] = v->x*v->y*(1-cs)-sn*v->z;
  m[2][0] = v->x*v->z*(1-cs)+sn*v->y;
  m[0][1] = v->x*v->y*(1-cs)+sn*v->z;
  m[1][1] = v->y*v->y+cs*(1-v->y*v->y);
  m[2][1] = v->y*v->z*(1-cs)-sn*v->x;
  m[0][2] = v->x*v->z*(1-cs)-sn*v->y;
  m[1][2] = v->y*v->z*(1-cs)+sn*v->x;
  m[2][2] = v->z*v->z+cs*(1-v->z*v->z);
}

void MatriceD::toTranslation(double tx,double ty,double tz) {
  toIdentite();
  m[0][3] = tx;
  m[1][3] = ty;
  m[2][3] = tz;
}

void MatriceD::toTranslation(Coord3DD *p) {
  toIdentite();
  m[0][3] = p->x;
  m[1][3] = p->y;
  m[2][3] = p->z;
}

void MatriceD::toTranslation(Vecte3DD *d) {
  toIdentite();
  m[0][3] = d->x;
  m[1][3] = d->y;
  m[2][3] = d->z;
}

void MatriceD::toScale(double rx,double ry,double rz) {
  toIdentite();
  m[0][0] = rx;
  m[1][1] = ry;
  m[2][2] = rz;
}

void MatriceD::produit(MatriceD *mt,MatriceD *r) {
  MatriceD a;
  { for ( int i = 0 ; i < 4 ; i++ )
      for ( int j = 0 ; j < 4 ; j++ ) {
        a.m[i][j] = 0.0F;
        for ( int k = 0 ; k < 4 ; k++ )
          a.m[i][j] += this->m[i][k]*mt->m[k][j]; } }
  for ( int i = 0 ; i < 4 ; i++ )
    for ( int j = 0 ; j < 4 ; j++ )
      r->m[i][j] = a.m[i][j];    
}

MatriceD *MatriceD::produitMatriciel(MatriceD *m1,MatriceD *m2) {
  MatriceD *a = new MatriceD();
  { for ( int i = 0 ; i < 4 ; i++ )
      for ( int j = 0 ; j < 4 ; j++ ) {
        a->m[i][j] = 0.0F;
        for ( int k = 0 ; k < 4 ; k++ )
          a->m[i][j] += m1->m[i][k]*m2->m[k][j]; } }
  return(a);    
}

void MatriceD::produit(MatriceD *r) {
  produit(r,this);
}

MatriceD *MatriceD::newProduit(MatriceD *mt) {
  MatriceD *r = new MatriceD();
  produit(mt,r);
  return(r);
}

void MatriceD::produit(Vecte3DD *v,Vecte3DD *r) {
  double t[4];
  double *tr =(double *) v;
  { for ( int i = 0 ; i < 4 ; i++ ) {
      t[i] = 0.0F;
      for ( int k = 0 ; k < 4 ; k++ )
        t[i] += m[i][k]*tr[k]; } }
  r->x = t[0];
  r->y = t[1];
  r->z = t[2];
  r->t = t[3];
}

void MatriceD::produit(Vecte3DD *r) {
  produit(r,r);
}

Vecte3DD *MatriceD::newProduit(Vecte3DD *v) {
  Vecte3DD *r = new Vecte3DD();
  produit(v,r);
  return(r);
}

void MatriceD::produit(Coord3DD *v,Coord3DD *r) {
  double t[4];
  double *tr =(double *) v;
  { for ( int i = 0 ; i < 4 ; i++ ) {
      t[i] = 0.0F;
      for ( int k = 0 ; k < 4 ; k++ )
        t[i] += m[i][k]*tr[k]; } }
  r->x = t[0];
  r->y = t[1];
  r->z = t[2];
}

void MatriceD::produit(Coord3DD *r) {
  produit(r,r);
}

Coord3DD *MatriceD::newProduit(Coord3DD *v) {
  Coord3DD *r = new Coord3DD();
  produit(v,r);
  return(r);
}

void MatriceD::affichage(void) {
  for ( int i = 0 ; i < 4 ; i++ ) {
    for ( int j = 0 ; j < 4 ; j++ )
       printf("%15.10f ",(double) m[i][j]);
    printf("\n"); }
}

void MatriceD::round(void) {
  for ( int i = 0 ; i < 4 ; i++ )
    for ( int j = 0 ; j < 4 ; j++ ) {
      if ( fabs(m[i][j]) < 0.00001F )
        m[i][j] = 0.0F;
      if ( m[i][j] > 0.99999F )
        m[i][j] = 1.0F; 
      if ( m[i][j] < -0.99999F )
        m[i][j] = -1.0F;  }
}
