/**
*  QuaternionOperator, SFRotation utility.
*  <P>
*  This class based on rotations.c by Gavin Bell, gavin@acm.org
*  <P>
*  Freeware, no liabilities assumed or implied.
* <P>
*  Not for use in mission critical environment.
* <P>
*  Copyright Finlayson Consulting 1997.  All rights reserved.
*  @version 0.3
*  @author Ross A. Finlayson
*  @author <A HREF="http://www.tomco.net/~raf/vrmlgroup">FC VRML Group</A>
*/


public class QuaternionOperator extends Object
   {
   public boolean normalize=false;
   public QuaternionOperator()
      {
      }
   
   /**
   * @param normalize if true normalize axis vector
   */
   
   public QuaternionOperator(boolean normalize)
      {
      normalize=normalize;
      }
   
   /**
   * Converts SFRotation as axis and angle arrays of floats to quaternion.
   * @param axisin array of floats
   * @param anglein float value of angle in radians
   * @return quatout quaternion represenation of rotation
   */
   
   public float[] to_quaternion(float[] axisin, float anglein)
      {
      
      if (normalize==true)
         {
         
         float rotaxislength=(float) Math.sqrt( (double)(axisin[0] * axisin[0] + axisin[1] * axisin[1] + axisin[2] * axisin[2]) );
         if (rotaxislength !=1)
            {
            axisin[0]=axisin[0]/rotaxislength;
            axisin[1]=axisin[1]/rotaxislength;
            axisin[2]=axisin[2]/rotaxislength;
            }
         
         }
      
      
      float[] quatout=new float[4];
      quatout[0] = axisin[0] * (float)Math.sin((double)(anglein / 2.0F));
      quatout[1] = axisin[1] * (float)Math.sin((double)(anglein / 2.0F));
      quatout[2] = axisin[2] * (float)Math.sin((double)(anglein / 2.0F));
      quatout[3] = (float)Math.cos((double)(anglein / 2.0F));
      return quatout;
      }
   
   /**
   * Converts SFRotation as array of floats to quaternion.
   * @param rotfloatsin array of floats
   * @return quatout quaternion represenation of rotation
   */
   
   
   public float[] to_quaternion(float[] rotfloatsin)
      {
      
      if (normalize==true)
         {
         
         float rotaxislength=(float) Math.sqrt( (double)(rotfloatsin[0] * rotfloatsin[0] + rotfloatsin[1] * rotfloatsin[1] + rotfloatsin[2] * rotfloatsin[2]) );
         if (rotaxislength !=1)
            {
            rotfloatsin[0]=rotfloatsin[0]/rotaxislength;
            rotfloatsin[1]=rotfloatsin[1]/rotaxislength;
            rotfloatsin[2]=rotfloatsin[2]/rotaxislength;
            }
         
         }
      
      float[] quatout=new float[4];
      quatout[0] = rotfloatsin[0] * (float)Math.sin((double)(rotfloatsin[3] / 2.0F));
      quatout[1] = rotfloatsin[1] * (float)Math.sin((double)(rotfloatsin[3] / 2.0F));
      quatout[2] = rotfloatsin[2] * (float)Math.sin((double)(rotfloatsin[3] / 2.0F));
      quatout[3] = (float)Math.cos((double)(rotfloatsin[3] / 2.0F));
      return quatout;
      }
   
   /**
   * Get angle as float from quaternion.
   * @param quatin quaternion value
   * @return angleout float value of rotation angle
   */
   
   public float from_quaternionAngle(float[] quatin)
      {
      float angleout = (float)Math.acos((double)quatin[3]) * 2.0F;
      return angleout;
      }
   
   /**
   * Get axis as array of floats from quaternion.
   * @param quatin quaternion value
   * @return axisout float array value of rotation angle
   */
   
   public float[] from_quaternionAxis(float[] quatin)
      {
      float[] axisout=new float[3];
      float f = (float)Math.acos((double)quatin[3]) * 2.0F;
      axisout[0] = quatin[0] / (float)Math.sin((double)(f / 2.0F));
      axisout[1] = quatin[1] / (float)Math.sin((double)(f / 2.0F));
      axisout[2] = quatin[2] / (float)Math.sin((double)(f / 2.0F));
      return axisout;
      }
   
   /**
   * Multiply quaternion.
   * @param quatin1 first quaternion value
   * @param quatin2 second quaternion value
   * @return quatout quaternion represenation of rotation
   */
   
   public float[] mult_quaternion(float[] quatin1, float[] quatin2)
      {
      float[] quatout=new float[4];
      quatout[0] = quatin2[3] * quatin1[0] + quatin2[0] * quatin1[3] + quatin2[1] * quatin1[2] - quatin2[2] * quatin1[1];
      quatout[1] = quatin2[3] * quatin1[1] + quatin2[1] * quatin1[3] + quatin2[2] * quatin1[0] - quatin2[0] * quatin1[2];
      quatout[2] = quatin2[3] * quatin1[2] + quatin2[2] * quatin1[3] + quatin2[0] * quatin1[1] - quatin2[1] * quatin1[0];
      quatout[3] = quatin2[3] * quatin1[3] - quatin2[0] * quatin1[0] - quatin2[1] * quatin1[1] - quatin2[2] * quatin1[2];
      return quatout;
      }
   
   
   /**
   *  Get SFRotation as array of flaots from quaternion.
   * @param quatin  quaternion value
   * @return rotout rotation as array of floats
   */
   
   
   
   public float[] to_rot(float[] quatin)
      {
      float[] rotout=new float[4];
      float[] tempaxis=new float[3];
      
      tempaxis=this.from_quaternionAxis(quatin);
      float tempangle=this.from_quaternionAngle(quatin);
      rotout[0]=tempaxis[0];
      rotout[1]=tempaxis[1];
      rotout[2]=tempaxis[2];
      rotout[3]=tempangle;
      return rotout;
      }
   
   /**
   * Add two rotations
   * @param rotin1 first rotation value
   * @param rotin2 second rotation value
   * @return rotout rotation as array of floats
   */
   
   
   public float[] addRot(float[] rotin1, float[] rotin2)
      {
      float[] rotout=new float[4];
      float[] quatout=new float[4];
      float[] rotin1quat=new float[4];
      float[] rotin2quat=new float[4];
      rotin1quat=to_quaternion(rotin1);
      rotin2quat=to_quaternion(rotin2);
      quatout=mult_quaternion(rotin1quat, rotin2quat);
      float[] rotaxisout=new float[4];
      rotaxisout=from_quaternionAxis(quatout);
      float rotangleout=from_quaternionAngle(quatout);
      rotout[0]=rotaxisout[0];
      rotout[1]=rotaxisout[1];
      rotout[2]=rotaxisout[2];
      rotout[3]=rotangleout;
      return rotout;
      }
   }