#include <GL/glew.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "matr9d.h"

Matr9d Vect3d::v()const{
    return Matr9d(Vect3d(0,-z,y),
                  Vect3d(z,0,-x),
                  Vect3d(-y,x,0));
}
Vect3d vectorielle(Vect3d const& a,Vect3d const& b){
    return Vect3d(
        a.y*b.z-a.z*b.y,
        b.x*a.z-b.z*a.x,
        a.x*b.y-a.y*b.x);
}
Vect3d operator^(Vect3d const& a,Vect3d const& b){
    return Vect3d(
        a.y*b.z-a.z*b.y,
        b.x*a.z-b.z*a.x,
        a.x*b.y-a.y*b.x);
}
Vect3d doubleproduit(Vect3d const& a,Vect3d const& b,Vect3d const& c){
    return (a*c)*b-(a*b)*c;
}
Matr9d::Matr9d(){
    int k;
    for(k=0;k<9;k++)PTR_MATR[k]=0;
}
Matr9d::Matr9d(double d){
    int k;
    for(k=0;k<9;k++)PTR_MATR[k]=0;
    PTR_MATR[0]=d;
    PTR_MATR[4]=d;
    PTR_MATR[8]=d;
}
Matr9d::Matr9d(Vect3d const& diagj){
    int k;
    for(k=0;k<9;k++)PTR_MATR[k]=0;
    PTR_MATR[0]=diagj.x;
    PTR_MATR[4]=diagj.y;
    PTR_MATR[8]=diagj.z;
}
Matr9d::Matr9d(double a,double b, double c){
    int k;
    for(k=0;k<9;k++)PTR_MATR[k]=0;
    PTR_MATR[0]=a;
    PTR_MATR[4]=b;
    PTR_MATR[8]=c;
}
void Matr9d::transpo(){
    echange(Lx.y,Ly.x);
    echange(Lz.y,Ly.z);
    echange(Lz.x,Lx.z);
}
Matr9d transpo(Matr9d const& m){Matr9d a(m);a.transpo();return a;}

Vect3d operator*(Matr9d const& m, Vect3d const& a){
    return Vect3d(m.Lx*a,m.Ly*a,m.Lz*a);
}
void Matr9d::operator+=(Matr9d const& a){Lx+=a.Lx;Ly+=a.Ly;Lz+=a.Lz;}
void Matr9d::operator-=(Matr9d const& a){Lx-=a.Lx;Ly-=a.Ly;Lz-=a.Lz;}
void Matr9d::operator*=(double a){Lx*=a;Ly*=a;Lz*=a;}

Matr9d operator*(double a, Matr9d const& b){
    Matr9d c(b); c*=a; return c;}

Matr9d operator*(Matr9d const& a, Matr9d const& b){
    return Matr9d(a.Lx.x*b.Lx+a.Lx.y*b.Ly+a.Lx.z*b.Lz,
                  a.Ly.x*b.Lx+a.Ly.y*b.Ly+a.Ly.z*b.Lz,
                  a.Lz.x*b.Lx+a.Lz.y*b.Ly+a.Lz.z*b.Lz);
}

Matr9d operator!(Matr9d const& b){
    Matr9d m;
    m[0]=b[4]*b[8]-b[5]*b[7];
    m[1]=b[2]*b[7]-b[8]*b[1];
    m[2]=b[1]*b[5]-b[2]*b[4];
    m[3]=b[5]*b[6]-b[8]*b[3];
    m[4]=b[0]*b[8]-b[6]*b[2];
    m[5]=b[2]*b[3]-b[0]*b[5];
    m[6]=b[3]*b[7]-b[4]*b[6];
    m[7]=b[1]*b[6]-b[0]*b[7];
    m[8]=b[0]*b[4]-b[1]*b[3];
    return (1./(vectorielle(b.Lx,b.Ly)*b.Lz))*m;
}
//*
Matr9d pivotaxeangle(Vect3d const& axe, double teta){
    Matr9d m(axe.v());
    m = sin(teta)*m+(1-cos(teta))*m*m;
    m[0]+=1;
    m[4]+=1;
    m[8]+=1;
    return m;
}

Matr9d dpivotaxe(Vect3d const& axe){
    Matr9d m;
    m.Lx = Vect3d(1,-axe.z,axe.y);
    m.Ly = Vect3d(axe.z,1,-axe.x);
    m.Lz = Vect3d(-axe.y,axe.x,1);
    return m;
}
Matr9d dpivotaxe2(Vect3d const& axe){
    Matr9d m(Vect3d(0,-axe.z,axe.y),
              Vect3d(axe.z,0,-axe.x),
              Vect3d(-axe.y,axe.x,0));
    m += 0.5*m*m;
    m.Lx.x+=1;
    m.Ly.y+=1;
    m.Lz.z+=1;
    return m;
}
/*
void multglmatrix(Matr9d const& m){
    float m2[16] = //codage en colonne obligatoire...
    {
        m[0],m[3],m[6],0,
        m[1],m[4],m[7],0,
        m[2],m[5],m[8],0,
        0,0,0,1
    };
    glMultMatrixf(m2); //appel de la multiplication GL
}
void multgltmatrix(Matr9d const& m){
    float m2[16] = //codage en ligne car on transpose...
    {
        m[0],m[1],m[2],0,
        m[3],m[4],m[5],0,
        m[6],m[7],m[8],0,
        0,0,0,1
    };
    glMultMatrixf(m2); //appel de la multiplication GL
}//*/
void Vect3d::print (const char* nom) const
    {printf("%s : %f %f %f \n",nom,x,y,z);}
void Matr9d::print (const char* nom) const
    {printf("%s\n",nom);Lx.print("");Ly.print("");Lz.print("");}

void Quaternion::renorme(){
    double d=(s*s+v*v)*(-0.5)+1.5;
    s*=d;
    v*=d;
}
void Quaternion::renorme2(){
    double d0 = (s*s+v*v)-1;
    double d = 1+d0*(-0.5+d0*(0.375-d0*0.3125));//developpement au 3eme ordre 1-1/2*x+3/8*x²-5/16*x³
    s*=d;
    v*=d;
}//*
Matr9d Quaternion::matr()const{
    double ab = 2.*QUAT[0]*QUAT[1],
        ac= 2.*QUAT[0]*QUAT[2],ad= 2.*QUAT[0]*QUAT[3],
        bc= 2.*QUAT[1]*QUAT[2],bd= 2.*QUAT[1]*QUAT[3],
        cd= 2.*QUAT[2]*QUAT[3],bb= 2.*QUAT[1]*QUAT[1],
        cc= 2.*QUAT[2]*QUAT[2],dd= 2.*QUAT[3]*QUAT[3];
    return Matr9d(Vect3d(1-cc-dd,bc-ad,ac+bd),
                   Vect3d(ad+bc,1-bb-dd,cd-ab),
                   Vect3d(bd-ac,ab+cd,1-bb-cc));
}//*/
Quaternion dquat2(Vect3d const&a){
    return Quaternion(1-0.125*a*a,0.5*a);
}
Quaternion operator*(Quaternion const&a, Quaternion const&b){
    return Quaternion(a.s*b.s-a.v*b.v, a.s*b.v+b.s*a.v+vectorielle(a.v,b.v));
}
Quaternion operator*(Vect3d const&a, Quaternion const&b){
    return Quaternion(-a*b.v, b.s*a+vectorielle(a,b.v));
}
Quaternion operator/(Quaternion const&a, Quaternion const&b){
    return Quaternion(a.s*b.s+a.v*b.v, b.s*a.v-a.s*b.v-vectorielle(a.v,b.v));
}//même chose que operator* mais avec -b.v
Matr9d getprojection(Matr9d const&base, const bool *tab3){
    return base*Matr9d((double)tab3[0],(double)tab3[1],(double)tab3[2])*transpo(base);
}