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

#include "Matrice.h"
#include "Coord3D.h"
#include "Vecte3D.h"

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

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

Matrice::~Matrice(void) {
}

void Matrice::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 Matrice::toRotationX(float a) {
  double aa = a*0.01745329251994329576923690768489;
  toRotationXRadian(aa);
}

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

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

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

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

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

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

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

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

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

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

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

void Matrice::toRotationRadianTransposee(float a,float ax,float ay,float az) {
  toIdentite();
  Vecte3D *v = new Vecte3D(ax,ay,az);
  v->normalise();
  float sn =(float) sin(a);
  float cs =(float) 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 Matrice::toTranslation(float tx,float ty,float tz) {
  toIdentite();
  m[0][3] = tx;
  m[1][3] = ty;
  m[2][3] = tz;
}

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

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

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

void Matrice::produit(Matrice *mt,Matrice *r) {
  Matrice 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];    
}

Matrice *Matrice::produitMatriciel(Matrice *m1,Matrice *m2) {
  Matrice *a = new Matrice();
  { 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 Matrice::produit(Matrice *r) {
  produit(r,this);
}

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

void Matrice::produit(Vecte3D *v,Vecte3D *r) {
  float t[4];
  float *tr =(float *) 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 Matrice::produit(Vecte3D *r) {
  produit(r,r);
}

Vecte3D *Matrice::newProduit(Vecte3D *v) {
  Vecte3D *r = new Vecte3D();
  produit(v,r);
  return(r);
}

void Matrice::produit(Coord3D *v,Coord3D *r) {
  float t[4];
  float *tr =(float *) 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 Matrice::produit(Coord3D *r) {
  produit(r,r);
}

Coord3D *Matrice::newProduit(Coord3D *v) {
  Coord3D *r = new Coord3D();
  produit(v,r);
  return(r);
}

void Matrice::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 Matrice::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;  }
}
