import org.lwjgl.util.vector.Matrix4f;
import org.lwjgl.util.vector.Quaternion;

public class Utils {

   public static Matrix4f convertQuatToMatrix(Quaternion quat) {
      float xSq = quat.getX() * quat.getX();
      float ySq = quat.getY() * quat.getY();
      float zSq = quat.getZ() * quat.getZ();
      float xy = quat.getX() * quat.getY();
      float xz = quat.getX() * quat.getZ();
      float yz = quat.getY() * quat.getZ();
      float wx = quat.getW() * quat.getX();
      float wy = quat.getW() * quat.getY();
      float wz = quat.getW() * quat.getZ();
    
      // Only applicable for unit quaternions
      // Column major order
      Matrix4f mat = new Matrix4f();
      mat.m00 = 1.0f - 2.0f * (ySq + zSq);   mat.m10 = 2.0f * (xy + wz);            mat.m20 = 2.0f * (xz - wy);            mat.m30 = 0.0f;
      mat.m01 = 2.0f * (xy - wz);            mat.m11 = 1.0f - 2.0f * (xSq + zSq);   mat.m21 = 2.0f * (yz + wx);            mat.m31 = 0.0f;
      mat.m02 = 2.0f * (xz + wy);            mat.m12 = 2.0f * (yz - wx);            mat.m22 = 1.0f - 2.0f * (xSq + ySq);   mat.m32 = 0.0f;
      mat.m03 = 0.0f;                     mat.m13 = 0.0f;                     mat.m23 = 0.0f;                     mat.m33 = 1.0f;
      
      return  mat;
   }
   
   public static void systemPrintMatrix(Matrix4f mat){
       System.out.print(Float.valueOf(mat.m00) + "  " + Float.valueOf(mat.m10) + "  " + Float.valueOf(mat.m20) + "  " + Float.valueOf(mat.m30) + "  " + "\n" );
       System.out.print(Float.valueOf(mat.m01) + "  " + Float.valueOf(mat.m11) + "  " + Float.valueOf(mat.m21) + "  " + Float.valueOf(mat.m31) + "  " + "\n" );
       System.out.print(Float.valueOf(mat.m02) + "  " + Float.valueOf(mat.m12) + "  " + Float.valueOf(mat.m22) + "  " + Float.valueOf(mat.m32) + "  " + "\n" );
       System.out.print(Float.valueOf(mat.m03) + "  " + Float.valueOf(mat.m13) + "  " + Float.valueOf(mat.m23) + "  " + Float.valueOf(mat.m33) + "  " + "\n **************** \n" );

    }
}