import remixlab.proscene.*;
Scene scene;
WorldConstraint pivot, drag;
LocalConstraint planaire, rotative;
InteractiveFrame repere, alidade, alidade1, globeur, dragueur;
PFont arial;
PVector or;




void setup() {
     size(850, 850, P3D);
     scene = new Scene(this);
     scene.setAxisIsDrawn(false);
     scene.setGridIsDrawn(false);
     arial=createFont("Cambria-12.vlw", 20);
     globeur=new InteractiveFrame(scene);
     dragueur=new InteractiveFrame(scene);
     repere=new InteractiveFrame(scene);
     alidade=new InteractiveFrame(scene);
     alidade1=new InteractiveFrame(scene);

     drag=new WorldConstraint();
     pivot=new WorldConstraint();
     planaire=new LocalConstraint();
     rotative=new LocalConstraint();


     drag.setTranslationConstraint(AxisPlaneConstraint.Type.FREE, new PVector(0, 0, 0));
     drag.setRotationConstraint(AxisPlaneConstraint.Type.FREE, new PVector(0, 0, 0));

     rotative.setTranslationConstraint(AxisPlaneConstraint.Type.FORBIDDEN, new PVector(0, 0, 0));
     rotative.setRotationConstraint(AxisPlaneConstraint.Type.FREE, new PVector(0, 0, 0));
     planaire.setRotationConstraint(AxisPlaneConstraint.Type.FORBIDDEN, new PVector(0, 0, 0));
     planaire.setTranslationConstraint(AxisPlaneConstraint.Type.PLANE, new PVector(0, 0, 1));
     pivot.setRotationConstraint(AxisPlaneConstraint.Type.FREE, new PVector(0, 0, 0));
     pivot.setTranslationConstraint(AxisPlaneConstraint.Type.FORBIDDEN, new PVector(0, 0, 0));

     globeur.setConstraint(pivot);
     repere.setConstraint(rotative);
     dragueur.setConstraint(drag);
     alidade.setConstraint(planaire);
     alidade1.setConstraint(planaire);

     repere.setReferenceFrame(globeur);
     alidade1.setReferenceFrame(repere);
     alidade.setReferenceFrame(globeur);



     alidade.setPosition(new PVector(100, 0, 0));
     alidade1.setPosition(new PVector(100*cos(PI/6), 100*sin(PI/6), 0));
     or=new PVector(0, 0, 0);
     dragueur.setPosition(new PVector(-8, -50, -2));
     globeur.setPosition(new PVector(0, 0, 0));
     repere.setTranslation(new PVector(0, 0, 0));
     scene.camera().setPosition(new PVector(0,0,300));
}



void draw() {
     background(210,210,210);
     textFont(arial, 4);
     lights();
     directionalLight(150, 150, 150, 0, 1, 0);
     stroke(0);
     scene.drawAxis(50);
     //dragueur
     ligne(dragueur.position(), or);
     PVector u=dragueur.position().get();
     u.normalize();
     PVector pro=PVector.mult(u, u.dot(dragueur.zAxis())); 
     PVector delt=comb(1, pro, -1, globeur.position());
     globeur.setZAxis(u);
     globeur.setXAxis(delt);
     globeur.setZAxis(u);          


     //globeur    
     pushMatrix();
     dragueur.applyTransformation();
     //scene.drawAxis(40);
     fill(0,255,0);
     noStroke();
     sphere(4);
     stroke(0,255,0);
     text("axe du quaternion", 5, 5);
     text("axe des z du repere mobile", 5, 15);
     popMatrix();

     //globeur
     pushMatrix();
     globeur.applyTransformation();

     repere.setXAxis(alidade.position());
     pushMatrix();
     repere.applyTransformation();
     alidade1.setTranslation(PVector.mult(alidade1.translation(), 100.0f/alidade1.translation().mag()));

     pushMatrix(); 
     PVector ald=alidade1.translation();
     float angleQ=calculAngle(ald);
     strokeWeight(5);
     stroke(255, 255, 0);
     line(ald.x, ald.y, ald.z, 0, ald.y, ald.z);
     line(ald.x, ald.y, ald.z, ald.x, 0, ald.z);
     strokeWeight(20);
     stroke(255, 0, 0);
     line(0, 0, 0, ald.x, 0, ald.z);
     line(0, 0, 0, 0, ald.y, ald.z);
     line(0, 0, 0, ald.x, ald.y, ald.z);
     strokeWeight(1);
     alidade1.applyTransformation();
     fill(70);
     text("Point B", 8, 5);
     text("angle du quaternion = "+angleQ, 8, 15);
     fill(255, 255, 0);
     box(6);
     popMatrix();
     scene.drawAxis(100);
     dessinerArc(angleQ);
     popMatrix(); 
     pushMatrix();
     PVector tran=PVector.mult(alidade.translation(), 100.0f/alidade.translation().mag());
     alidade.setTranslation(tran);
     alidade.applyTransformation();
     //scene.drawAxis(100);
     fill(0);
     text("Point A", 8, 5);
       fill(0,255,0);
     text("Axe des x du repere libre", 8, 10);
     pushMatrix();
     translate(-tran.x, -tran.y, -tran.z);
     rotateZ(HALF_PI);
     translate(tran.x, tran.y, tran.z);
     text("Axe des y du repere libre", 8, 5);
     popMatrix(); 

     popMatrix();

     popMatrix();
     textMode(SCREEN);
     textFont(arial);
     smooth();
     fill(150);
     text("Le quaternion unitaire q est entierement déterminé par son axe et son angle.", 10, 720);
     text("Cet angle est représenté par les vecteurs (OA,OB) dans un repère mobile orthonormé direct dont", 10, 750);
     text(" l' axe des Z est l'axe du quaternion. Donc q est caractérisé par dot(OA,OB) et par cross(OA,OB)",10, 780);
    
     noSmooth();
     textMode(MODEL);
}

/*void balle(int i) {
 color  c=color(255,255,255);   
 pushStyle();
 switch(i) {
 case 0 :
 case 1:
 case 4: 
 c=color(255,0,0);  
 break;
 case 2 :
 case 3 :
 c=color(255,255,0); 
 break;
 case 5 :
 case 6:
 c=color(255,255,255); 
 break;
 
 }  
 fill(c);
 noStroke();
 sphere(14);  
 popStyle();
 }*/
PVector comb(float t1, PVector v1, float t2, PVector v2) {
     PVector res=PVector.add(PVector.mult(v1, t1), PVector.mult(v2, t2));
     return res;
}
PVector comb(float t1, PVector v1, float t2, PVector v2, float t3, PVector v3) {
     PVector res=PVector.add(PVector.mult(v1, t1), PVector.mult(v2, t2));
     res=PVector.add(res, PVector.mult(v3, t3));
     return res;
}

//a revoir complètement
/*
float angleEntre(PVector u, PVector v) {
     PVector uu=u.get();
      PVector vv=v.get();
     uu.normalize();
     vv.normalize();
    
     float cosinus=uu.dot(vv);

     return acos(cosinus);
}     //
*/

PVector centreGravite(PVector u, PVector v, PVector r) {
     PVector gr= comb(0.5f, u, 0.5f, v);
     gr= comb(1.0f/3.0f, r, 2.0f/3.0f, gr);
     return gr;
} 
PVector barycentre(float lamb, PVector u, PVector v) {
     return comb(lamb, u, 1-lamb, v);
}

float  barycentre(float lamb, float u, float v) {
     return lamb*u+(1-lamb)*v;
}

void ligne(PVector a, PVector b) {
     line(a.x, a.y, a.z, b.x, b.y, b.z);
}
void afficher(PVector u) {
     println("vecteur = "+u.x+"    "+u.y+"   "+u.z);
}
void afficher(Quaternion q) {
     println("quaternion = x  "+q.x+"  y  "+q.y+" z  "+q.z+"... w  "+q.z);
}
/*
PVector projection(PVector M) {
 return  projection( M,true);
 }
 
 PVector projection(PVector M,boolean dess) {
 PVector SM=comb(1, M, -1, soleil.position() );
 PVector P=planProjection.repere.position();
 PVector SP=comb(1, P, -1, soleil.position() );
 planProjection.getNormaleDansWorld();
 float lambda=(SP.dot( planProjection.normale))/(SM.dot( planProjection.normale));
 PVector rep= comb(1, soleil.position(), lambda, SM);
 strokeWeight(1); stroke(0,0,200,150);
 if(dess) ligne(soleil.position(), rep);
 strokeWeight(1);
 return rep;
 }
 PVector intersectionDroite(PVector u1, PVector v1, PVector u2, PVector v2) {
 PVector uv1=comb(1, v1, -1, u1);
 PVector uv2=comb(1, v2, -1, u2);
 PVector nor=uv2.cross(uv2.cross(uv1));
 nor.normalize();
 PVector u1u2=comb(1, u2, -1, u1);
 float lambda=(u1u2.dot(nor))/(uv1.dot(nor));
 return comb(1, u1, lambda, uv1);
 }
 PVector intersectionDroiteDir2(PVector u1, PVector v1, PVector u2, PVector dir2) {
 PVector uv1=comb(1, v1, -1, u1);
 PVector uv2=dir2;
 PVector nor=uv2.cross(uv2.cross(uv1));
 nor.normalize();
 PVector u1u2=comb(1, u2, -1, u1);
 float lambda=(u1u2.dot(nor))/(uv1.dot(nor));
 return comb(1, u1, lambda, uv1);
 }*/
void    rectangle(color c, float dx, float dy, float ax, float ay) {
     stroke(150);
     fill(c);
     beginShape();
     vertex(dx, dy, 0);
     vertex(ax, dy, 0);
     fill(color(red(c)*2, green(c)*2, blue(c)*2));
     vertex(ax, ay, 0);
     vertex(dx, ay, 0);
     endShape(CLOSE);
}

//
void triangle3d(PVector a, PVector b, PVector c) {

     beginShape();
     fill(255, 200, 0, 200);
     vertex( a.x, a.y, a.z);
     fill(255, 255, 0, 200);        
     vertex( b.x, b.y, b.z);
     fill(155, 50, 250, 200);
     vertex( c.x, c.y, c.z);
     endShape();
}     
void triangle3d(PVector a, PVector b, PVector c, float k, float l, float m) {
     stroke(0, 100, 255);
     beginShape();
     fill(k, l, m, 250);
     vertex( a.x, a.y, a.z);
     fill(l, k, m, 250);
     vertex( b.x, b.y, b.z);
     fill(m, k, l, 250);
     vertex( c.x, c.y, c.z);
     endShape();
}     
void triangles3D(PVector a, PVector b, PVector c) {

     triangle3d(a, b, or);
     triangle3d(b, c, or);
     triangle3d(a, c, or);
}

PVector symetriePlan(PVector m, PVector u, PVector v) {
     PVector normale=u.cross(v);
     normale.normalize();
     PVector pm=PVector.mult(normale, m.dot(normale));

     return comb(1, m, -2.0, pm);
}
PVector projectionSurDroite(PVector v, PVector droite) {
     PVector u=droite.get();
     u.normalize();
     return PVector.mult(u, u.dot(v));
}

PVector normaliser(PVector vv, float f) {
     PVector v=vv.get();
     v.normalize();
     return PVector.mult(v.get(), f);
}



float calculAngle(PVector ve){
   float angl=acos(ve.x/ve.mag());
    angl=(ve.y>=0)? angl: -angl;
    return angl;}

void dessinerArc(float aq) {   
 
      float angl=aq;
    float anglArc=2*angl;
   
      if (angl>PI) anglArc=2*(angl-2.0*PI);
      if (angl<-PI) anglArc=2*(angl+2*PI);
          fill(215, 110, 155, 50);
          float r=100;
          ellipse(0, 0, 2.0*r, 2.0*r);
          
          noStroke();
          fill(0,0,250);
          pushMatrix();translate(r,0,0);
          box(5);
popMatrix();
 fill(255);
          beginShape(QUAD_STRIP); 
          
          for (int a=0;a<=100;a++) {
              float aa=anglArc/100*a;
               vertex(r*cos(aa), r*sin(aa), -3);
               vertex(r*cos(aa), r*sin(aa), 3);
          }
          endShape();
          fill(190, 120, 255);
         beginShape(TRIANGLE_FAN); 
         vertex(0, 0, 0);
        ;fill(150,195,250);
          for (int a=0;a<100;a+=4) {
              float aa=anglArc*a/100.0;
               vertex(r*cos(aa), r*sin(aa), 0);
          }
          vertex(r*cos(anglArc), r*sin(anglArc), 0);
          endShape();
         
          pushMatrix();
         
         
          rotateZ(anglArc);
          translate(r,0,0);
        
          fill(70);
              float angleRot=2.0*angl; if(angleRot>TWO_PI) angleRot-=TWO_PI;
            text("Point C ",8,5);   
           text("mesure de l'arc = "+anglArc,5,15);
       
          if(anglArc<0){rotateX(HALF_PI);}else { rotateX(-HALF_PI);}
          fill(255,55,55);
           scene.cone(8,8);
           
          popMatrix();
           
     }