//////////////////////////////////////////////////////////////QUAT //------------------------------------------------------------ // Quat class //------------------------------------------------------------ public static class Quat { float w, x, y, z; Quat() { reset(); } Quat(float w, float x, float y, float z) { this.w = w; this.x = x; this.y = y; this.z = z; } Quat(float w, Vecteur v) { this.w = w; this.x =v. x; this.y = v.y; this.z = v.z; } Quat(Quat q) { set(q); } void reset() { w = 1.0f; x = 0.0f; y = 0.0f; z = 0.0f; } void set(float w, Vecteur v) { this.w = w; x = v.x; y = v.y; z = v.z; } void set(Quat q) { w = q.w; x = q.x; y = q.y; z = q.z; } public float norme(){ return (float)Math.sqrt(x * x + y * y + z * z + w * w); } void 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; } 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; } 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; } 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; } Quat conjugue(){ return new Quat(w,-x,-y,-z); } Quat oppose(){ return new Quat(-w,-x,-y,-z); } Quat copie(){ return new Quat(w,x,y,z); } Quat copieNorme(){ Quat res=new Quat(w,x,y,z); res.normalize(); return res; } Vecteur tourner(Vecteur v){ Quat qv=new Quat(0.0f,v.x,v.y,v.z); Quat qc=this.conjugue(); Quat q1=Quat.mul(qv,qc); Quat q= Quat.mul(this,q1); return new Vecteur(q.x,q.y,q.z); } static Quat slerp(Quat d, Quat a,float t){ d.normalize(); a.normalize(); float angle=(float)Math.acos(d.x*a.x+d.y*a.y+d.z*a.z); float sina=(float)Math.sin(angle); float rd=(float)Math.sin(angle*(1-t))/sina; float ra=(float)Math.sin(angle*t)/sina; Quat res= Quat.comb(d,rd,a,ra); return res; } 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); float sina=(float)Math.sin(angle); float rd=(float)Math.sin(angle*(1-t))/sina; float ra=(float)Math.sin(angle*t)/sina; Quat res= Quat.comb(d,rd,a,ra); return res; } Vecteur axe(){ return new Vecteur(x,y,z); } Arc donneArc(Vecteur vd){ float[] c=getValue(); Vecteur vn=new Vecteur(c[1],c[2],c[3]); vn.normalize(); Arc res=new Arc(vn,c[0],this.norme()); res.calculerPoints(vd); return res; } }