quaternion.java

化学图形处理软件

/* Quaternion.java
 * 
 * Autor: Stephan Michels 
 * EMail: [e-mail]
 * Datum: 6.7.2001
 * 
 * Copyright (C) 1997-2007  The Chemistry Development Kit (CDK) project
 * 
 * Contact: [e-mail]
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2.1
 * of the License, or (at your option) any later version.
 * All we ask is that proper credit is given for our work, which includes
 * - but is not limited to - adding the above copyright notice to the beginning
 * of your source code files, and to any copyright notice that you may distribute
 * with programs based on this work.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *  */

package org.openscience.cdk.math;

/**
 * This class handles quaternions
 * Quaternion are 2*2 complex matrices
 */ 
public class Quaternion
{
  /** The content of the quaternion */
  private double a, b, c, d;

  public Quaternion()
  {
    a = b = c = d = 0d;
  }

  public Quaternion(double a, double b, double c, double d)
  {
    this.a = a;
    this.b = b;
    this.c = c;
    this.d = d;
  }

  /**
   * Generate a quaternion from a rotation axis and an angle
   */
  public Quaternion(Vector axis, double angle)
  {
    double sin_a = Math.sin( angle / 2 );
    double cos_a = Math.cos( angle / 2 );

    if (axis.size>=3)
    {
      a = axis.vector[0] / sin_a;
      b = axis.vector[1] / sin_a;
      c = axis.vector[2] / sin_a;
      d = cos_a;
    }
    else
    {
      a = b = c = 0d;
      d = cos_a;
    }
  }

  /**
   * Generate a quaternion from spherical coordinates and a rotation angle
   */
  public Quaternion(double latitude, double longitude, double angle)
  {
    double sin_a    = Math.sin( angle / 2 );
    double cos_a    = Math.cos( angle / 2 );

    double sin_lat  = Math.sin( latitude );
    double cos_lat  = Math.cos( latitude );

    double sin_long = Math.sin( longitude );
    double cos_long = Math.cos( longitude );

    a = sin_a * cos_lat * sin_long;
    b = sin_a * sin_lat;
    c = sin_a * sin_lat * cos_long;
    d = cos_a;
  }

  public Quaternion add(Quaternion q)
  {
    return new Quaternion(a+q.a, b+q.b, c+q.c, d+q.d);
  }

  public Quaternion sub(Quaternion q)
  {
    return new Quaternion(a-q.a, b-q.b, c-q.c, d-q.d);
  }

  public Quaternion negate()
  {
    return new Quaternion(-a, -b, -c, -d);
  }

  public Quaternion mul(Quaternion q)
  {
    return new Quaternion(a*q.a - b*q.b - c*q.c - d*q.d, 
                          a*q.b + b*q.a + c*q.d - d*q.c,
                          a*q.c + c*q.a + d*q.b - b*q.d,
                          a*q.d + d*q.a + a*q.c - c*q.b);
  }

  public Quaternion mul(double v)
  {
    return new Quaternion(a*v, b*v, c*v, d*v);
  }

  public Quaternion div(Quaternion q)
  {
    Quaternion temp1, temp2;
    temp1 = new Quaternion(q.a, -q.b, -q.c, -q.d);
    temp2 = mul(temp1);
    temp1 = q.mul(temp1);
    return new Quaternion(temp2.a/temp1.a,
                          temp2.b/temp1.a,
                          temp2.c/temp1.a,
                          temp2.d/temp1.a);
  }

  public Quaternion normalize()
  {
    double length = Math.sqrt(a*a + b*b + c*c + d*d);
    return new Quaternion(a/length, b/length, c/length, d/length);
  }

  public Quaternion sqrt()
  {
    double temp = 2*a;
    return new Quaternion(a*a - b*b - c*c - d*d, 
                          temp*b, 
                          temp*c,
                          temp*d);
  }

  public double mag_sq()
  {
    return a*a + b*b;
  }

  public double mag()
  {
    return Math.sqrt(a*a + b*b + c*c + d*d);
  }

  public Matrix toRotationMatrix()
  {
    Matrix result = new Matrix(4,4);

    double xx      = a * a;
    double xy      = a * b;
    double xz      = a * c;
    double xw      = a * d;

    double yy      = b * b;
    double yz      = b * c;
    double yw      = b * d;

    double zz      = c * c;
    double zw      = c * d;

    result.matrix[0][0]  = 1 - 2 * ( yy + zz );
    result.matrix[0][1]  =     2 * ( xy - zw );
    result.matrix[0][2]  =     2 * ( xz + yw );

    result.matrix[1][0]  =     2 * ( xy + zw );
    result.matrix[1][1]  = 1 - 2 * ( xx + zz );
    result.matrix[1][2]  =     2 * ( yz - xw );

    result.matrix[2][0]  =     2 * ( xz - yw );
    result.matrix[2][1]  =     2 * ( yz + xw );
    result.matrix[2][2] = 1 - 2 * ( xx + yy );

    result.matrix[3][0] = result.matrix[3][1] = result.matrix[3][2] = 
    result.matrix[0][3] = result.matrix[1][3] = result.matrix[2][3] = 0d;
    result.matrix[3][3] = 1d;

    return result;
  }

  public static Quaternion fromRotationMatrix(Matrix m)
  {
    if ((m.rows<3) || (m.columns<3))
      return null;

    double trace = m.matrix[0][0] + m.matrix[1][1] + m.matrix[2][2] + 1d;
 
    double S,a,b,c,d;
    if (trace>0)
    {
      S = 0.5 / Math.sqrt(trace);

      a = ( m.matrix[2][1] - m.matrix[1][2] ) * S;
      b = ( m.matrix[0][2] - m.matrix[2][0] ) * S;
      c = ( m.matrix[1][0] - m.matrix[0][1] ) * S;
      d = 0.25 / S;
      
      return new Quaternion(a,b,c,d);
    }
    else if ((m.matrix[0][0]>m.matrix[1][1]) && (m.matrix[0][0]>m.matrix[2][2]))
    {
      S  = Math.sqrt( 1.0 + m.matrix[0][0] - m.matrix[1][1] - m.matrix[2][2] ) * 2;

      a = 0.5 / S;
      b = ( m.matrix[0][1] + m.matrix[1][0] ) / S;
      c = ( m.matrix[0][2] + m.matrix[2][0] ) / S;
      d = ( m.matrix[1][2] + m.matrix[2][1] ) / S;

      return new Quaternion(a,b,c,d);
    }
    else if ((m.matrix[1][1]>m.matrix[0][0]) && (m.matrix[1][1]>m.matrix[2][2]))
    {
      S  = Math.sqrt( 1.0 + m.matrix[1][1] - m.matrix[0][0] - m.matrix[2][2] ) * 2;

      a = ( m.matrix[0][1] + m.matrix[1][0] ) / S;
      b = 0.5 / S;
      c = ( m.matrix[1][2] + m.matrix[2][1] ) / S;
      d = ( m.matrix[0][2] + m.matrix[2][0] ) / S;

      return new Quaternion(a,b,c,d);
    }
    else
    {
      S  = Math.sqrt( 1.0 + m.matrix[2][2] - m.matrix[0][0] - m.matrix[1][1] ) * 2;

      a = ( m.matrix[0][2] + m.matrix[2][0] ) / S;
      b = ( m.matrix[1][2] + m.matrix[2][1] ) / S;
      c = 0.5 / S;
      d = ( m.matrix[0][1] + m.matrix[1][0] ) / S;

      return new Quaternion(a,b,c,d);
    }
  }

  public String toString()
  {
    return "("+a+","+b+","+c+","+d+")";
  }
}