//------------------------------------------------------------
// Quat class
//------------------------------------------------------------
 public static class Quat
{
  float w, x, y, z;

 public  Quat()
  {
    reset();
  }

 public  Quat(float w, float x, float y, float z)
  {
    this.w = w;
    this.x = x;
    this.y = y;
    this.z = z;
  }
   public Quat(float w, Vecteur v)
  {
    this.w = w;
    this.x =v. x;
    this.y = v.y;
    this.z = v.z;
  }
public  Quat(Quat q)
  {
    set(q);
  }

 public  void reset()
  {
    w = 1.0f;
    x = 0.0f;
    y = 0.0f;
    z = 0.0f;
  }

  public void set(float w, Vecteur v)
  {
    this.w = w;
    x = v.x;
    y = v.y;
    z = v.z;
  }

  public void set(Quat q)
  {
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;
  }
  public Quat(Vecteur v,float angle,float rapport){
	   v.normalize(rapport*(float)Math.sin(angle/2));
 	  this.w = rapport*(float)Math.cos(angle/2);
   	  this.x =v.x;
   	  this.y = v.y;
   	  this.z = v.z;
   }
   public Quat(Vecteur v,Vecteur w){
     float k=w.length()/v.length();
     Vecteur vv=v.normerCopie(1);
      Vecteur ww=w.normerCopie(1);
     float ang=(float)Math.acos(Vecteur.dot(vv,ww));
     Vecteur axe=vv.cross(ww).normerCopie(k*(float)Math.sin(ang/2));    
     this.w = k*(float)Math.cos(ang/2);
     this.x =axe.x;
     this.y = axe.y;
     this.z = axe.z;
     
  }
  public float getx(){return x;}
  public float gety(){return y;}
  public float getz(){return z;}
  public float getw(){return w;}
  
public float norme(){
	return (float)Math.sqrt(x * x + y * y + z * z + w * w);
}

 public  Quat normalize()
  {
    float square = x * x + y * y + z * z + w * w;
    float dist = (square > 0.0f) ? (1.0f / (float) Math.sqrt(square)) : 1.0f;

    x *= dist;
    y *= dist;
    z *= dist;
    w *= dist;
    return this;
  }
  public void modifieAngle(float th){
  	float k=norme();
  	Vecteur v=axe().normalize(k*(float)Math.sin(th/2));
  	w=k*(float)Math.cos(th/2);
  	x=v.x;
  	y=v.y;
  	z=v.z;
  }
  public  Quat normalize(float c)
   {
     float square = x * x + y * y + z * z + w * w;
     float dist = (square > 0.0f) ? (c / (float) Math.sqrt(square)) : c;
	
     x *= dist;
     y *= dist;
     z *= dist;
     w *= dist;
     return this;
   }
 public static Quat mul(Quat q1, Quat q2)
  {
    Quat res = new Quat();
    res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
    res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
    res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
    res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
    return res;
  }
 public static Quat comb(Quat q1,float r1, Quat q2,float r2)
   {
     Quat res = new Quat();
     res.w = q1.w *r1 +  q2.w*r2;
     res.x = q1.x * r1+  q2.x * r2;
     res.y = q1.y * r1+  q2.y * r2;
     res.z = q1.z * r1+  q2.z * r2;
     return res;
   }
   public Quat ajouter(Quat q1,float r1)
     {
       Quat res = new Quat();
       res.w = q1.w *r1;
       res.x = q1.x * r1;
       res.y = q1.y * r1;
       res.z = q1.z * r1;
       return res;
     } 
      
public   float Angle()
  	{	
   
   	 float nq=norme();
	return (float) Math.acos(w/nq)* 2.0f;
 }  
public   float[] getValue()
 	{	
   	float[] res = new float[4];
   	float nq=norme();
	 res[0] = (float) Math.acos(w/nq)* 2.0f;
	 float nV= (float) Math.sqrt(x*x+y*y+z*z);
 	 if ( nV< 0.00001f*nq)
  		 {
		res[1] = x ;
		res[2] = y;
		res[3] = z ;   
  	 }else{
		 res[1] = x /nV;
	 	res[2] = y/nV ;
		 res[3] = z/nV;  }
   return res;
 }
 
 public   Quat conjugue(){
	return new Quat(w,-x,-y,-z);
}
public  Quat oppose(){
	return new Quat(-w,-x,-y,-z);
}
public Quat copie(){
	return new Quat(w,x,y,z);
	}
	public Quat copieNorme(){
		Quat res=new Quat(w,x,y,z);
		res.normalize();
		return res;
		}	

public static Quat slerp(Quat d, Quat a,float t){
	Quat res;
	d.normalize();
	a.normalize();
	float costheta=d.x*a.x+d.y*a.y+d.z*a.z;
	
	if(costheta<0.99f){
		float theta=(float)Math.acos(costheta);
		float sintheta=(float)Math.sin(theta);
		float rd=(float)Math.sin(theta*(1-t))/sintheta;
		float ra=(float)Math.sin(theta*t)/sintheta;
		 res= Quat.comb(d,rd,a,ra);}
	else{
		res=Quat.comb(d,1-t,a,t);
	}
	return res;

}
public static Quat slerp1(Quat d, Quat a,float tt){
	
	d.normalize();
	a.normalize();
	float t=(tt-1)*(tt-1)*(tt-1)+1;
	float angle=(float)Math.acos(d.x*a.x+d.y*a.y+d.z*a.z);
	if(angle>0.05f){	
	float sina=(float)Math.sin(angle);
	float rd=(float)Math.sin(angle*(1-t))/sina;
	float ra=(float)Math.sin(angle*t)/sina;
	return Quat.comb(d,rd,a,ra);}
	else{return Quat.comb(d,1-t,a,t);
	}
}
public Vecteur axe(){
return new Vecteur(x,y,z);	
}
public static Quat slerp2(Quat d, Quat a,float tt){
	Quat res;
	d.normalize();
	a.normalize();
	float costheta=d.x*a.x+d.y*a.y+d.z*a.z;
	float t=(tt-1)*(tt-1)+1;
	if(costheta<0.9999f){
		float theta=(float)Math.acos(costheta);
		float sintheta=(float)Math.sin(theta);
		float rd=(float)Math.sin(theta*(1-t))/sintheta;
		float ra=(float)Math.sin(theta*t)/sintheta;
		 res= Quat.comb(d,rd,a,ra);
		}
	else{
		res=Quat.comb(d,1-t,a,t);
	}
	return  Quat.mul(res,new Quat((float)Math.cos(t*6.2832f),(float)Math.sin(t*6.2832f),(float)Math.sin(t*6.2832f),0));

}
public Quat exposant(float t){
    //le quaternion doit être normé, il est de nouveau normalisé
    
    normalize();
    float[] res=getValue();
    float c=(float)Math.sin(res[0]*t/2);
    Quat q=new Quat((float)Math.cos(res[0]*t/2), c*res[1],c*res[2],c*res[3]);
    return q;
}


public Quat puissance(float t){
    float noq=this.norme();
    float[] res=getValue();
 
    float npq=(float)Math.exp(t*Math.log(noq));
    float c=(float)Math.sin(res[0]*t/2)*npq;
    Quat q=new Quat((float)Math.cos(res[0]*t/2)*npq, c*res[1],c*res[2],c*res[3]);
    return q;
}

public static Quat euler2quat(float psi,float theta,float phi){
	/* psi      :angle de la rotation autour de 0z    :precession
	 * theta   :angle de la rotation autour de 0x1  :nutation
	 * phi      :angle de la rotation autoue de 0z2  : rotation propre
	 */
		float w,x,y,z,th,psiMphi,psiPphi;
		th=theta/2;
		psiMphi=(psi-phi)/2;
		psiPphi=(psi+phi)/2;		
		w=(float)Math.cos(th)*(float)Math.cos(psiPphi);
		x=(float)Math.sin(th)*(float)Math.cos(psiMphi);
		y=(float)Math.sin(th)*(float)Math.sin(psiMphi);
		z=(float)Math.cos(th)*(float)Math.sin(psiPphi);		
	return new Quat(w,x,y,z);
}
public  Vecteur tourner(Vecteur v){
	    Quat qv=new Quat(0.0f,v.x,v.y,v.z);
	    Quat qc=conjugue();
	    Quat q1=Quat.mul(qv,qc);	
	    Quat q= Quat.mul(this,q1);
	    return new Vecteur(q.x,q.y,q.z);	
	
    }

}