Vous n’avez pas trouvé la réponse que vous recherchez ?
Posez votre questionpackage com.gmail.bernabe.laurent.jogl.cube_anti_gimbal_lock; /** * Represente un quaternion * @author laurent BERNABE * */ public class CopyOfQuaternion { /** * Construit un quaternion identite (pour multiplications) * => (w=1, x=0, y=0, z=0). */ private CopyOfQuaternion(){ w = 1; x y z = 0; } /** * Construit un quaternion a partir des 4 composantes * @param w - float - le composant scalaire * @param vector - float ... ou float[3] - la composante vecteur */ private CopyOfQuaternion(float w, float ...vector){ this.w = w; x = vector[0]; y = vector[1]; z = vector[2]; } /** * Construit un quaternion identite (pour multiplications) * > (w1, x=0, y=0, z=0). * @return Quaternion - le quaternion */ public static CopyOfQuaternion buildIdentityQuaternion(){ return new CopyOfQuaternion(); } /** * Construit un quaternion a partir des 4 composantes * @param w - float - le composant scalaire * @param vector - float ... ou float[3] - composante vecteur * @return Quaternion - le quaternion */ public static CopyOfQuaternion buildFromFourComponents(float w, float ...vector){ return new CopyOfQuaternion(w, vector[0], vector[1], vector[2]); } /** * Construit un quaternion a partir d'un vecteur * > w 0 * > et x vecX, y = vecY, z = vecZ * @param vector - float ... ou float[3] - vecteur * @return Quaternion - le quaternion */ public static CopyOfQuaternion buildFromVector(float ...vector){ return new CopyOfQuaternion(0, vector[0], vector[1], vector[2]); } /** * Construit un quaternion a partir d'un angle et d'un axe arbitraire. * @param angle - float - angle en degres * @param axis - float ... ou float[3] - axe de rotation * @return Quaternion - le quaternion */ public static CopyOfQuaternion buildFromAxisAngle(float angle, float ... axis){ float halfAngleRad = (float) Math.toRadians(angle)/2; float axisNorm = (float) Math.sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); if (axisNorm < 0.001) // si la norme est nulle return CopyOfQuaternion.buildIdentityQuaternion(); axis[0] /= axisNorm; axis[1] /= axisNorm; axis[2] /= axisNorm; return CopyOfQuaternion.buildFromFourComponents( (float) Math.cos(halfAngleRad), (float) (axis[0] * Math.sin(halfAngleRad)), (float) (axis[1] * Math.sin(halfAngleRad)), (float) (axis[2] * Math.sin(halfAngleRad)) ); } /** * Construit un quaternion a partir d'angles d'Euler (en Degres) * @param angleX - float - angle autour de l'axe x * @param angleY - float - angle autour de l'axe y * @param angleZ - float - angle autour de l'axe z */ public static CopyOfQuaternion buildFromEulerAngles(float angleX, float angleY, float angleZ){ float angleXRad = (float) Math.toRadians(angleX); float angleYRad = (float) Math.toRadians(angleY); float angleZRad = (float) Math.toRadians(angleZ); CopyOfQuaternion qX = CopyOfQuaternion.buildFromAxisAngle(angleXRad, 1.0f, 0.0f, 0.0f); CopyOfQuaternion qYUnity = CopyOfQuaternion.buildFromFourComponents(0.0f, 0.0f, 1.0f, 0.0f); qYUnity = qX.mulInLeftOf(qYUnity.mulInLeftOf(qX.conjugate())); CopyOfQuaternion qY = CopyOfQuaternion.buildFromAxisAngle(angleYRad, qYUnity.getX(), qYUnity.getY(), qYUnity.getZ()); CopyOfQuaternion qZUnity = CopyOfQuaternion.buildFromFourComponents(0.0f, 0.0f, 0.0f, 1.0f); CopyOfQuaternion qTmp = qY.mulInLeftOf(qX); qZUnity = qTmp.mulInLeftOf(qZUnity.mulInLeftOf(qTmp.conjugate())); CopyOfQuaternion qZ = CopyOfQuaternion.buildFromAxisAngle(angleZRad, qZUnity.getX(), qZUnity.getY(), qZUnity.getZ()); return qZ.mulInLeftOf(qY.mulInLeftOf(qX)); } /** * Retourne un nouveau quaternion comme le produit de q1 * q2 * Attention !!! Aucun quaternion ne sera modifie. * @param q1 - Quaternion - quaternion gauche * @param q2 - Quaternion - quaternion droite * @return Quaternion : le produit q1 * q2. */ public static CopyOfQuaternion mul(CopyOfQuaternion q1, CopyOfQuaternion q2){ return q1.mulInLeftOf(q2); } /** * Calcule autre * this. * Aucun quaternion n'est changé. * @param autre - l'autre quaternion. * @return Quaternion - autre * this */ public CopyOfQuaternion mulInLeftOf(CopyOfQuaternion autre){ return new CopyOfQuaternion( this.w * autre.w - this.x * autre.x - this.y * autre.y - this.z * autre.z, this.w * autre.x + this.x * autre.w + this.y * autre.z - this.z * autre.y, this.w * autre.y - this.x * autre.z + this.y * autre.w + this.z * autre.x, this.w * autre.z + this.x * autre.y - this.y * autre.x + this.z * autre.w ); } /** * Retourne le quaternion normalisé : de sorte a ce que sa * longueur vaille 1. * Attention : le quaternion this n'est pas change. * @return Quaternion - quaternion normalise */ public CopyOfQuaternion normalize(){ float norm = (float) Math.sqrt(w*w + x*x + y*y + z*z); if (norm <= 0.001) // Identity quaternion for multiplication return new CopyOfQuaternion(1, 0,0,0); return new CopyOfQuaternion(w/norm, x/norm, y/norm, z/norm); } /** * Retourne l'equivalent dans le modele "axis angle" * * <li>axisAngle[0] contiendra l'angle en degres * <li>axisAngle[1 to 2] contiendra le vecteur (x,y,z) * * @return float [4] - l'"axis angle" equivalent */ public float [] toAxisAngle(){ float axisAngle [] = new float[16]; float scale = (float) Math.sqrt(x*x + y*y + z*z); float thetaRadians = (float) Math.toDegrees(2*Math.acos(w)); if (scale <= 0.001){ // if scale is 0, axis direction is not important axisAngle[0] = thetaRadians; axisAngle[1] = x; axisAngle[2] = y; axisAngle[3] = z; } else { axisAngle[0] = thetaRadians; axisAngle[1] = (x/scale); axisAngle[2] = (y/scale); axisAngle[3] = (z/scale); } return axisAngle; } /** * Convertit le quaternion en matrice * @return float[16] - la matrice equivalente. */ public float[] toMatrix(){ float matrix[] = new float[16]; CopyOfQuaternion q = normalize(); matrix[0] = (float) (1.0 - 2.0 * (q.y*q.y + q.z*q.z)); matrix[1] = (float) (2.0 * (q.x*q.y + q.w*q.z)); matrix[2] = (float) (2.0 * (q.x*q.z - q.w*q.y)); matrix[3] = 0; matrix[4] = (float) (2.0 * (q.x*q.y - q.w*q.z)); matrix[5] = (float) (1.0 - 2.0 * (q.x*q.x + q.z*q.z)); matrix[6] = (float) (2.0 * (q.y*q.z + q.w*q.x)); matrix[7] = 0; matrix[8] = (float) (2.0 * (q.x*q.z + q.w*q.y)); matrix[9] = (float) (2.0 * (q.y*q.z - q.w*q.x)); matrix[10] = (float) (1.0 - 2.0 * (q.x*q.x + q.y*q.y)); matrix[11] = 0; matrix[12] = matrix[13] = matrix[14] = 0; matrix[15] = 1; return matrix; } /** * Retourne le conjugue du quaternion * >x-x, y=-y, z=-z * Attention, le quaternion this reste inchange !!! * @return Quaternion - le conjugue */ public CopyOfQuaternion conjugate(){ return new CopyOfQuaternion(w, -x, -y, -z); } /** * Reinitialise this en quaternion identite (pour multiplications) * => w = 1 * => x = y = z = 0 */ public void setIdentity() { w = 1; x y z = 0; } /** * Fixe les valeurs a partir d'un "axis angle" * @param angleDegrees - float - angle en degres * @param vector - float... ou float[3] - vector (x,y,z) */ public void setFromAxisAngle(float angleDegrees, float ...vector){ float norm = (float) Math.sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]); if (norm < 0.01){ setIdentity(); } else { vector[0] /= norm; vector[1] /= norm; vector[2] /= norm; double cosHalfTheta = Math.cos(Math.toRadians(angleDegrees)/2); double sinHalfTheta = Math.sin(Math.toRadians(angleDegrees)/2); w = (float) cosHalfTheta; x = (float) (vector[0] * sinHalfTheta); y = (float) (vector[1] * sinHalfTheta); z = (float) (vector[2] * sinHalfTheta); } } public float getW() { return w; } public float getX() { return x; } public float getY() { return y; } public float getZ() { return z; } private float w, x, y ,z; }