src/math/SquareMatrix3.hpp

/*
 * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
 *
 * The University of Notre Dame grants you ("Licensee") a
 * non-exclusive, royalty free, license to use, modify and
 * redistribute this software in source and binary code form, provided
 * that the following conditions are met:
 *
 * 1. Acknowledgement of the program authors must be made in any
 *    publication of scientific results based in part on use of the
 *    program.  An acceptable form of acknowledgement is citation of
 *    the article in which the program was described (Matthew
 *    A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
 *    J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
 *    Parallel Simulation Engine for Molecular Dynamics,"
 *    J. Comput. Chem. 26, pp. 252-271 (2005))
 *
 * 2. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 3. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the
 *    distribution.
 *
 * This software is provided "AS IS," without a warranty of any
 * kind. All express or implied conditions, representations and
 * warranties, including any implied warranty of merchantability,
 * fitness for a particular purpose or non-infringement, are hereby
 * excluded.  The University of Notre Dame and its licensors shall not
 * be liable for any damages suffered by licensee as a result of
 * using, modifying or distributing the software or its
 * derivatives. In no event will the University of Notre Dame or its
 * licensors be liable for any lost revenue, profit or data, or for
 * direct, indirect, special, consequential, incidental or punitive
 * damages, however caused and regardless of the theory of liability,
 * arising out of the use of or inability to use software, even if the
 * University of Notre Dame has been advised of the possibility of
 * such damages.
 */
 
/**
 * @file SquareMatrix3.hpp
 * @author Teng Lin
 * @date 10/11/2004
 * @version 1.0
 */
#ifndef MATH_SQUAREMATRIX3_HPP
#define  MATH_SQUAREMATRIX3_HPP
#include <vector>
#include "Quaternion.hpp"
#include "SquareMatrix.hpp"
#include "Vector3.hpp"
#include "utils/NumericConstant.hpp"
namespace oopse {

  template<typename Real>
  class SquareMatrix3 : public SquareMatrix<Real, 3> {
  public:

    typedef Real ElemType;
    typedef Real* ElemPoinerType;
            
    /** default constructor */
    SquareMatrix3() : SquareMatrix<Real, 3>() {
    }

    /** Constructs and initializes every element of this matrix to a scalar */ 
    SquareMatrix3(Real s) : SquareMatrix<Real,3>(s){
    }

    /** Constructs and initializes from an array */ 
    SquareMatrix3(Real* array) : SquareMatrix<Real,3>(array){
    }


    /** copy  constructor */
    SquareMatrix3(const SquareMatrix<Real, 3>& m)  : SquareMatrix<Real, 3>(m) {
    }
            
    SquareMatrix3( const Vector3<Real>& eulerAngles) {
      setupRotMat(eulerAngles);
    }
            
    SquareMatrix3(Real phi, Real theta, Real psi) {
      setupRotMat(phi, theta, psi);
    }

    SquareMatrix3(const Quaternion<Real>& q) {
      setupRotMat(q);

    }

    SquareMatrix3(Real w, Real x, Real y, Real z) {
      setupRotMat(w, x, y, z);
    }
            
    /** copy assignment operator */
    SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) {
      if (this == &m)
        return *this;
      SquareMatrix<Real, 3>::operator=(m);
      return *this;
    }


    SquareMatrix3<Real>& operator =(const Quaternion<Real>& q) {
      this->setupRotMat(q);
      return *this;
    }

    /**
     * Sets this matrix to a rotation matrix by three euler angles
     * @ param euler
     */
    void setupRotMat(const Vector3<Real>& eulerAngles) {
      setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
    }

    /**
     * Sets this matrix to a rotation matrix by three euler angles
     * @param phi
     * @param theta
     * @psi theta
     */
    void setupRotMat(Real phi, Real theta, Real psi) {
      Real sphi, stheta, spsi;
      Real cphi, ctheta, cpsi;

      sphi = sin(phi);
      stheta = sin(theta);
      spsi = sin(psi);
      cphi = cos(phi);
      ctheta = cos(theta);
      cpsi = cos(psi);

      this->data_[0][0] = cpsi * cphi - ctheta * sphi * spsi;
      this->data_[0][1] = cpsi * sphi + ctheta * cphi * spsi;
      this->data_[0][2] = spsi * stheta;
                
      this->data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi;
      this->data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi;
      this->data_[1][2] = cpsi * stheta;

      this->data_[2][0] = stheta * sphi;
      this->data_[2][1] = -stheta * cphi;
      this->data_[2][2] = ctheta;
    }


    /**
     * Sets this matrix to a rotation matrix by quaternion
     * @param quat
     */
    void setupRotMat(const Quaternion<Real>& quat) {
      setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
    }

    /**
     * Sets this matrix to a rotation matrix by quaternion
     * @param w the first element 
     * @param x the second element
     * @param y the third element
     * @param z the fourth element
     */
    void setupRotMat(Real w, Real x, Real y, Real z) {
      Quaternion<Real> q(w, x, y, z);
      *this = q.toRotationMatrix3();
    }

    void setupSkewMat(Vector3<Real> v) {
        setupSkewMat(v[0], v[1], v[2]);
    }

    void setupSkewMat(Real v1, Real v2, Real v3) {
        this->data_[0][0] = 0;
        this->data_[0][1] = -v3;
        this->data_[0][2] = v2;
        this->data_[1][0] = v3;
        this->data_[1][1] = 0;
        this->data_[1][2] = -v1;
        this->data_[2][0] = -v2;
        this->data_[2][1] = v1;
        this->data_[2][2] = 0;
        
        
    }


    /**
     * Returns the quaternion from this rotation matrix
     * @return the quaternion from this rotation matrix
     * @exception invalid rotation matrix
     */            
    Quaternion<Real> toQuaternion() {
      Quaternion<Real> q;
      Real t, s;
      Real ad1, ad2, ad3;    
      t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0;

      if( t > NumericConstant::epsilon ){

        s = 0.5 / sqrt( t );
        q[0] = 0.25 / s;
        q[1] = (this->data_[1][2] - this->data_[2][1]) * s;
        q[2] = (this->data_[2][0] - this->data_[0][2]) * s;
        q[3] = (this->data_[0][1] - this->data_[1][0]) * s;
      } else {

        ad1 = this->data_[0][0];
        ad2 = this->data_[1][1];
        ad3 = this->data_[2][2];

        if( ad1 >= ad2 && ad1 >= ad3 ){

          s = 0.5 / sqrt( 1.0 + this->data_[0][0] - this->data_[1][1] - this->data_[2][2] );
          q[0] = (this->data_[1][2] - this->data_[2][1]) * s;
          q[1] = 0.25 / s;
          q[2] = (this->data_[0][1] + this->data_[1][0]) * s;
          q[3] = (this->data_[0][2] + this->data_[2][0]) * s;
        } else if ( ad2 >= ad1 && ad2 >= ad3 ) {
          s = 0.5 / sqrt( 1.0 + this->data_[1][1] - this->data_[0][0] - this->data_[2][2] );
          q[0] = (this->data_[2][0] - this->data_[0][2] ) * s;
          q[1] = (this->data_[0][1] + this->data_[1][0]) * s;
          q[2] = 0.25 / s;
          q[3] = (this->data_[1][2] + this->data_[2][1]) * s;
        } else {

          s = 0.5 / sqrt( 1.0 + this->data_[2][2] - this->data_[0][0] - this->data_[1][1] );
          q[0] = (this->data_[0][1] - this->data_[1][0]) * s;
          q[1] = (this->data_[0][2] + this->data_[2][0]) * s;
          q[2] = (this->data_[1][2] + this->data_[2][1]) * s;
          q[3] = 0.25 / s;
        }
      }             

      return q;
                
    }

    /**
     * Returns the euler angles from this rotation matrix
     * @return the euler angles in a vector 
     * @exception invalid rotation matrix
     * We use so-called "x-convention", which is the most common definition. 
     * In this convention, the rotation given by Euler angles (phi, theta, 
     * psi), where the first rotation is by an angle phi about the z-axis, 
     * the second is by an angle theta (0 <= theta <= 180) about the x-axis, 
     * and the third is by an angle psi about the z-axis (again). 
     */            
    Vector3<Real> toEulerAngles() {
      Vector3<Real> myEuler;
      Real phi;
      Real theta;
      Real psi;
      Real ctheta;
      Real stheta;
                
      // set the tolerance for Euler angles and rotation elements

      theta = acos(std::min((RealType)1.0, std::max((RealType)-1.0,this->data_[2][2])));
      ctheta = this->data_[2][2]; 
      stheta = sqrt(1.0 - ctheta * ctheta);

      // when sin(theta) is close to 0, we need to consider
      // singularity In this case, we can assign an arbitary value to
      // phi (or psi), and then determine the psi (or phi) or
      // vice-versa. We'll assume that phi always gets the rotation,
      // and psi is 0 in cases of singularity.
      // we use atan2 instead of atan, since atan2 will give us -Pi to Pi. 
      // Since 0 <= theta <= 180, sin(theta) will be always
      // non-negative. Therefore, it will never change the sign of both of
      // the parameters passed to atan2.

      if (fabs(stheta) < 1e-6){
        psi = 0.0;
        phi = atan2(-this->data_[1][0], this->data_[0][0]);  
      }
      // we only have one unique solution
      else{    
        phi = atan2(this->data_[2][0], -this->data_[2][1]);
        psi = atan2(this->data_[0][2], this->data_[1][2]);
      }

      //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
      if (phi < 0)
        phi += 2.0 * M_PI;

      if (psi < 0)
        psi += 2.0 * M_PI;

      myEuler[0] = phi;
      myEuler[1] = theta;
      myEuler[2] = psi;

      return myEuler;
    }
            
    /** Returns the determinant of this matrix. */
    Real determinant() const {
      Real x,y,z;

      x = this->data_[0][0] * (this->data_[1][1] * this->data_[2][2] - this->data_[1][2] * this->data_[2][1]);
      y = this->data_[0][1] * (this->data_[1][2] * this->data_[2][0] - this->data_[1][0] * this->data_[2][2]);
      z = this->data_[0][2] * (this->data_[1][0] * this->data_[2][1] - this->data_[1][1] * this->data_[2][0]);

      return(x + y + z);
    }            

    /** Returns the trace of this matrix. */
    Real trace() const {
      return this->data_[0][0] + this->data_[1][1] + this->data_[2][2];
    }
            
    /**
     * Sets the value of this matrix to  the inversion of itself. 
     * @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the 
     * implementation of inverse in SquareMatrix class
     */
    SquareMatrix3<Real>  inverse() const {
      SquareMatrix3<Real> m;
      RealType det = determinant();
      if (fabs(det) <= oopse::epsilon) {
        //"The method was called on a matrix with |determinant| <= 1e-6.",
        //"This is a runtime or a programming error in your application.");
        std::vector<int> zeroDiagElementIndex;
        for (int i =0; i < 3; ++i) {
            if (fabs(this->data_[i][i]) <= oopse::epsilon) {
                zeroDiagElementIndex.push_back(i);
            }
        }

        if (zeroDiagElementIndex.size() == 2) {
            int index = zeroDiagElementIndex[0];
            m(index, index) = 1.0 / this->data_[index][index];
        }else if (zeroDiagElementIndex.size() == 1) {

            int a = (zeroDiagElementIndex[0] + 1) % 3;
            int b = (zeroDiagElementIndex[0] + 2) %3;
            RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b];
            m(a, a) = this->data_[b][b] /denom;
            m(b, a) = -this->data_[b][a]/denom;

            m(a,b) = -this->data_[a][b]/denom;
            m(b, b) = this->data_[a][a]/denom;
                
        }
      
/*
        for(std::vector<int>::iterator iter = zeroDiagElementIndex.begin(); iter != zeroDiagElementIndex.end() ++iter) {
            if (this->data_[*iter][0] > oopse::epsilon || this->data_[*iter][1] ||this->data_[*iter][2] ||
                this->data_[0][*iter] > oopse::epsilon || this->data_[1][*iter] ||this->data_[2][*iter] ) {
                std::cout << "can not inverse matrix" << std::endl;
            }
        }
*/
      } else {

          m(0, 0) = this->data_[1][1]*this->data_[2][2] - this->data_[1][2]*this->data_[2][1];
          m(1, 0) = this->data_[1][2]*this->data_[2][0] - this->data_[1][0]*this->data_[2][2];
          m(2, 0) = this->data_[1][0]*this->data_[2][1] - this->data_[1][1]*this->data_[2][0];
          m(0, 1) = this->data_[2][1]*this->data_[0][2] - this->data_[2][2]*this->data_[0][1];
          m(1, 1) = this->data_[2][2]*this->data_[0][0] - this->data_[2][0]*this->data_[0][2];
          m(2, 1) = this->data_[2][0]*this->data_[0][1] - this->data_[2][1]*this->data_[0][0];
          m(0, 2) = this->data_[0][1]*this->data_[1][2] - this->data_[0][2]*this->data_[1][1];
          m(1, 2) = this->data_[0][2]*this->data_[1][0] - this->data_[0][0]*this->data_[1][2];
          m(2, 2) = this->data_[0][0]*this->data_[1][1] - this->data_[0][1]*this->data_[1][0];

          m /= det;
        }
      return m;
    }

    SquareMatrix3<Real> transpose() const{
      SquareMatrix3<Real> result;
                
      for (unsigned int i = 0; i < 3; i++)
        for (unsigned int j = 0; j < 3; j++)              
          result(j, i) = this->data_[i][j];

      return result;
    }
    /**
     * Extract the eigenvalues and eigenvectors from a 3x3 matrix.
     * The eigenvectors (the columns of V) will be normalized. 
     * The eigenvectors are aligned optimally with the x, y, and z
     * axes respectively.
     * @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is
     *     overwritten             
     * @param w will contain the eigenvalues of the matrix On return of this function
     * @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are 
     *    normalized and mutually orthogonal.              
     * @warning a will be overwritten
     */
    static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v); 
  };
  /*=========================================================================

  Program:   Visualization Toolkit
  Module:    $RCSfile: SquareMatrix3.hpp,v $

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.  See the above copyright notice for more information.

  =========================================================================*/
  template<typename Real>
  void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, 
                                        SquareMatrix3<Real>& v) {
    int i,j,k,maxI;
    Real tmp, maxVal;
    Vector3<Real> v_maxI, v_k, v_j;

    // diagonalize using Jacobi
    jacobi(a, w, v);
    // if all the eigenvalues are the same, return identity matrix
    if (w[0] == w[1] && w[0] == w[2] ) {
      v = SquareMatrix3<Real>::identity();
      return;
    }

    // transpose temporarily, it makes it easier to sort the eigenvectors
    v = v.transpose(); 
        
    // if two eigenvalues are the same, re-orthogonalize to optimally line
    // up the eigenvectors with the x, y, and z axes
    for (i = 0; i < 3; i++) {
      if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
        // find maximum element of the independant eigenvector
        maxVal = fabs(v(i, 0));
        maxI = 0;
        for (j = 1; j < 3; j++) {
          if (maxVal < (tmp = fabs(v(i, j)))){
            maxVal = tmp;
            maxI = j;
          }
        }
            
        // swap the eigenvector into its proper position
        if (maxI != i) {
          tmp = w(maxI);
          w(maxI) = w(i);
          w(i) = tmp;

          v.swapRow(i, maxI);
        }
        // maximum element of eigenvector should be positive
        if (v(maxI, maxI) < 0) {
          v(maxI, 0) = -v(maxI, 0);
          v(maxI, 1) = -v(maxI, 1);
          v(maxI, 2) = -v(maxI, 2);
        }

        // re-orthogonalize the other two eigenvectors
        j = (maxI+1)%3;
        k = (maxI+2)%3;

        v(j, 0) = 0.0; 
        v(j, 1) = 0.0; 
        v(j, 2) = 0.0;
        v(j, j) = 1.0;

        /** @todo */
        v_maxI = v.getRow(maxI);
        v_j = v.getRow(j);
        v_k = cross(v_maxI, v_j);
        v_k.normalize();
        v_j = cross(v_k, v_maxI);
        v.setRow(j, v_j);
        v.setRow(k, v_k);


        // transpose vectors back to columns
        v = v.transpose();
        return;
      }
    }

    // the three eigenvalues are different, just sort the eigenvectors
    // to align them with the x, y, and z axes

    // find the vector with the largest x element, make that vector
    // the first vector
    maxVal = fabs(v(0, 0));
    maxI = 0;
    for (i = 1; i < 3; i++) {
      if (maxVal < (tmp = fabs(v(i, 0)))) {
        maxVal = tmp;
        maxI = i;
      }
    }

    // swap eigenvalue and eigenvector
    if (maxI != 0) {
      tmp = w(maxI);
      w(maxI) = w(0);
      w(0) = tmp;
      v.swapRow(maxI, 0);
    }
    // do the same for the y element
    if (fabs(v(1, 1)) < fabs(v(2, 1))) {
      tmp = w(2);
      w(2) = w(1);
      w(1) = tmp;
      v.swapRow(2, 1);
    }

    // ensure that the sign of the eigenvectors is correct
    for (i = 0; i < 2; i++) {
      if (v(i, i) < 0) {
        v(i, 0) = -v(i, 0);
        v(i, 1) = -v(i, 1);
        v(i, 2) = -v(i, 2);
      }
    }

    // set sign of final eigenvector to ensure that determinant is positive
    if (v.determinant() < 0) {
      v(2, 0) = -v(2, 0);
      v(2, 1) = -v(2, 1);
      v(2, 2) = -v(2, 2);
    }

    // transpose the eigenvectors back again
    v = v.transpose();
    return ;
  }

  /**
   * Return the multiplication of two matrixes  (m1 * m2). 
   * @return the multiplication of two matrixes
   * @param m1 the first matrix
   * @param m2 the second matrix
   */
  template<typename Real> 
  inline SquareMatrix3<Real> operator *(const SquareMatrix3<Real>& m1, const SquareMatrix3<Real>& m2) {
    SquareMatrix3<Real> result;

    for (unsigned int i = 0; i < 3; i++)
      for (unsigned int j = 0; j < 3; j++)
        for (unsigned int k = 0; k < 3; k++)
          result(i, j)  += m1(i, k) * m2(k, j);                

    return result;
  }

  template<typename Real> 
  inline SquareMatrix3<Real> outProduct(const Vector3<Real>& v1, const Vector3<Real>& v2) {
    SquareMatrix3<Real> result;

    for (unsigned int i = 0; i < 3; i++) {
      for (unsigned int j = 0; j < 3; j++) {
        result(i, j)  = v1[i] * v2[j];                
      }
    }
            
    return result;        
  }

    
  typedef SquareMatrix3<RealType> Mat3x3d;
  typedef SquareMatrix3<RealType> RotMat3x3d;

} //namespace oopse
#endif // MATH_SQUAREMATRIX_HPP

Revision:	3520
Committed:	Mon Sep 7 16:31:51 2009 UTC (15 years, 10 months ago) by cli2
File size:	18514 byte(s)
Log Message:	Added new restraint infrastructure Added MolecularRestraints Added ObjectRestraints Added RestraintStamp Updated thermodynamic integration to use ObjectRestraints Added Quaternion mathematics for twist swing decompositions Significantly updated RestWriter and RestReader to use dump-like files Added selections for x, y, and z coordinates of atoms Removed monolithic Restraints class Fixed a few bugs in gradients of Euler angles in DirectionalAtom and RigidBody Added some rotational capabilities to prinicpalAxisCalculator
#	User	Rev	Content
1	gezelter	2204	/*
2	gezelter	1930	* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3	tim	1563	*
4	gezelter	1930	* The University of Notre Dame grants you ("Licensee") a
5			* non-exclusive, royalty free, license to use, modify and
6			* redistribute this software in source and binary code form, provided
7			* that the following conditions are met:
8			*
9			* 1. Acknowledgement of the program authors must be made in any
10			* publication of scientific results based in part on use of the
11			* program. An acceptable form of acknowledgement is citation of
12			* the article in which the program was described (Matthew
13			* A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher
14			* J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented
15			* Parallel Simulation Engine for Molecular Dynamics,"
16			* J. Comput. Chem. 26, pp. 252-271 (2005))
17			*
18			* 2. Redistributions of source code must retain the above copyright
19			* notice, this list of conditions and the following disclaimer.
20			*
21			* 3. Redistributions in binary form must reproduce the above copyright
22			* notice, this list of conditions and the following disclaimer in the
23			* documentation and/or other materials provided with the
24			* distribution.
25			*
26			* This software is provided "AS IS," without a warranty of any
27			* kind. All express or implied conditions, representations and
28			* warranties, including any implied warranty of merchantability,
29			* fitness for a particular purpose or non-infringement, are hereby
30			* excluded. The University of Notre Dame and its licensors shall not
31			* be liable for any damages suffered by licensee as a result of
32			* using, modifying or distributing the software or its
33			* derivatives. In no event will the University of Notre Dame or its
34			* licensors be liable for any lost revenue, profit or data, or for
35			* direct, indirect, special, consequential, incidental or punitive
36			* damages, however caused and regardless of the theory of liability,
37			* arising out of the use of or inability to use software, even if the
38			* University of Notre Dame has been advised of the possibility of
39			* such damages.
40	tim	1563	*/
41	gezelter	1930
42	tim	1563	/**
43			* @file SquareMatrix3.hpp
44			* @author Teng Lin
45			* @date 10/11/2004
46			* @version 1.0
47			*/
48	gezelter	2204	#ifndef MATH_SQUAREMATRIX3_HPP
49	tim	1592	#define MATH_SQUAREMATRIX3_HPP
50	tim	2611	#include <vector>
51	tim	1586	#include "Quaternion.hpp"
52	tim	1563	#include "SquareMatrix.hpp"
53	tim	1586	#include "Vector3.hpp"
54	tim	2146	#include "utils/NumericConstant.hpp"
55	tim	1563	namespace oopse {
56
57	gezelter	2204	template<typename Real>
58			class SquareMatrix3 : public SquareMatrix<Real, 3> {
59			public:
60	tim	1630
61	gezelter	2204	typedef Real ElemType;
62			typedef Real* ElemPoinerType;
63	tim	1563
64	gezelter	2204	/** default constructor */
65			SquareMatrix3() : SquareMatrix<Real, 3>() {
66			}
67	tim	1563
68	gezelter	2204	/** Constructs and initializes every element of this matrix to a scalar */
69			SquareMatrix3(Real s) : SquareMatrix<Real,3>(s){
70			}
71	tim	1644
72	gezelter	2204	/** Constructs and initializes from an array */
73			SquareMatrix3(Real* array) : SquareMatrix<Real,3>(array){
74			}
75	tim	1644
76
77	gezelter	2204	/** copy constructor */
78			SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) {
79			}
80	gezelter	1930
81	gezelter	2204	SquareMatrix3( const Vector3<Real>& eulerAngles) {
82			setupRotMat(eulerAngles);
83			}
84	tim	1586
85	gezelter	2204	SquareMatrix3(Real phi, Real theta, Real psi) {
86			setupRotMat(phi, theta, psi);
87			}
88	tim	1586
89	gezelter	2204	SquareMatrix3(const Quaternion<Real>& q) {
90			setupRotMat(q);
91	tim	1606
92	gezelter	2204	}
93	tim	1586
94	gezelter	2204	SquareMatrix3(Real w, Real x, Real y, Real z) {
95			setupRotMat(w, x, y, z);
96			}
97	tim	1586
98	gezelter	2204	/** copy assignment operator */
99			SquareMatrix3<Real>& operator =(const SquareMatrix<Real, 3>& m) {
100			if (this == &m)
101			return *this;
102			SquareMatrix<Real, 3>::operator=(m);
103			return *this;
104			}
105	tim	1569
106	gezelter	1930
107	gezelter	2204	SquareMatrix3<Real>& operator =(const Quaternion<Real>& q) {
108			this->setupRotMat(q);
109			return *this;
110			}
111	gezelter	1930
112	gezelter	2204	/**
113			* Sets this matrix to a rotation matrix by three euler angles
114			* @ param euler
115			*/
116			void setupRotMat(const Vector3<Real>& eulerAngles) {
117			setupRotMat(eulerAngles[0], eulerAngles[1], eulerAngles[2]);
118			}
119	tim	1569
120	gezelter	2204	/**
121			* Sets this matrix to a rotation matrix by three euler angles
122			* @param phi
123			* @param theta
124			* @psi theta
125			*/
126			void setupRotMat(Real phi, Real theta, Real psi) {
127			Real sphi, stheta, spsi;
128			Real cphi, ctheta, cpsi;
129	tim	1569
130	gezelter	2204	sphi = sin(phi);
131			stheta = sin(theta);
132			spsi = sin(psi);
133			cphi = cos(phi);
134			ctheta = cos(theta);
135			cpsi = cos(psi);
136	tim	1569
137	gezelter	2204	this->data_[0][0] = cpsi * cphi - ctheta * sphi * spsi;
138			this->data_[0][1] = cpsi * sphi + ctheta * cphi * spsi;
139			this->data_[0][2] = spsi * stheta;
140	tim	1586
141	gezelter	2204	this->data_[1][0] = -spsi * ctheta - ctheta * sphi * cpsi;
142			this->data_[1][1] = -spsi * stheta + ctheta * cphi * cpsi;
143			this->data_[1][2] = cpsi * stheta;
144	tim	1586
145	gezelter	2204	this->data_[2][0] = stheta * sphi;
146			this->data_[2][1] = -stheta * cphi;
147			this->data_[2][2] = ctheta;
148			}
149	tim	1586
150
151	gezelter	2204	/**
152			* Sets this matrix to a rotation matrix by quaternion
153			* @param quat
154			*/
155			void setupRotMat(const Quaternion<Real>& quat) {
156			setupRotMat(quat.w(), quat.x(), quat.y(), quat.z());
157			}
158	tim	1569
159	gezelter	2204	/**
160			* Sets this matrix to a rotation matrix by quaternion
161			* @param w the first element
162			* @param x the second element
163			* @param y the third element
164			* @param z the fourth element
165			*/
166			void setupRotMat(Real w, Real x, Real y, Real z) {
167			Quaternion<Real> q(w, x, y, z);
168			*this = q.toRotationMatrix3();
169			}
170	tim	1569
171	tim	2596	void setupSkewMat(Vector3<Real> v) {
172			setupSkewMat(v[0], v[1], v[2]);
173			}
174
175			void setupSkewMat(Real v1, Real v2, Real v3) {
176			this->data_[0][0] = 0;
177			this->data_[0][1] = -v3;
178			this->data_[0][2] = v2;
179			this->data_[1][0] = v3;
180			this->data_[1][1] = 0;
181			this->data_[1][2] = -v1;
182			this->data_[2][0] = -v2;
183			this->data_[2][1] = v1;
184			this->data_[2][2] = 0;
185
186
187			}
188
189
190
191	gezelter	2204	/**
192			* Returns the quaternion from this rotation matrix
193			* @return the quaternion from this rotation matrix
194			* @exception invalid rotation matrix
195			*/
196			Quaternion<Real> toQuaternion() {
197			Quaternion<Real> q;
198			Real t, s;
199			Real ad1, ad2, ad3;
200			t = this->data_[0][0] + this->data_[1][1] + this->data_[2][2] + 1.0;
201	tim	1569
202	tim	2336	if( t > NumericConstant::epsilon ){
203	tim	1586
204	gezelter	2204	s = 0.5 / sqrt( t );
205			q[0] = 0.25 / s;
206			q[1] = (this->data_[1][2] - this->data_[2][1]) * s;
207			q[2] = (this->data_[2][0] - this->data_[0][2]) * s;
208			q[3] = (this->data_[0][1] - this->data_[1][0]) * s;
209			} else {
210	tim	1586
211	tim	2332	ad1 = this->data_[0][0];
212			ad2 = this->data_[1][1];
213			ad3 = this->data_[2][2];
214	tim	1586
215	gezelter	2204	if( ad1 >= ad2 && ad1 >= ad3 ){
216	tim	1586
217	gezelter	2204	s = 0.5 / sqrt( 1.0 + this->data_[0][0] - this->data_[1][1] - this->data_[2][2] );
218			q[0] = (this->data_[1][2] - this->data_[2][1]) * s;
219			q[1] = 0.25 / s;
220			q[2] = (this->data_[0][1] + this->data_[1][0]) * s;
221			q[3] = (this->data_[0][2] + this->data_[2][0]) * s;
222			} else if ( ad2 >= ad1 && ad2 >= ad3 ) {
223			s = 0.5 / sqrt( 1.0 + this->data_[1][1] - this->data_[0][0] - this->data_[2][2] );
224			q[0] = (this->data_[2][0] - this->data_[0][2] ) * s;
225			q[1] = (this->data_[0][1] + this->data_[1][0]) * s;
226			q[2] = 0.25 / s;
227			q[3] = (this->data_[1][2] + this->data_[2][1]) * s;
228			} else {
229	tim	1586
230	gezelter	2204	s = 0.5 / sqrt( 1.0 + this->data_[2][2] - this->data_[0][0] - this->data_[1][1] );
231			q[0] = (this->data_[0][1] - this->data_[1][0]) * s;
232			q[1] = (this->data_[0][2] + this->data_[2][0]) * s;
233			q[2] = (this->data_[1][2] + this->data_[2][1]) * s;
234			q[3] = 0.25 / s;
235			}
236			}
237	tim	1586
238	gezelter	2204	return q;
239	tim	1586
240	gezelter	2204	}
241	tim	1586
242	gezelter	2204	/**
243			* Returns the euler angles from this rotation matrix
244			* @return the euler angles in a vector
245			* @exception invalid rotation matrix
246			* We use so-called "x-convention", which is the most common definition.
247	cli2	3520	* In this convention, the rotation given by Euler angles (phi, theta,
248			* psi), where the first rotation is by an angle phi about the z-axis,
249			* the second is by an angle theta (0 <= theta <= 180) about the x-axis,
250			* and the third is by an angle psi about the z-axis (again).
251	gezelter	2204	*/
252			Vector3<Real> toEulerAngles() {
253			Vector3<Real> myEuler;
254			Real phi;
255			Real theta;
256			Real psi;
257			Real ctheta;
258			Real stheta;
259	tim	1586
260	gezelter	2204	// set the tolerance for Euler angles and rotation elements
261	tim	1586
262	tim	2759	theta = acos(std::min((RealType)1.0, std::max((RealType)-1.0,this->data_[2][2])));
263	gezelter	2204	ctheta = this->data_[2][2];
264			stheta = sqrt(1.0 - ctheta * ctheta);
265	tim	1586
266	cli2	3520	// when sin(theta) is close to 0, we need to consider
267			// singularity In this case, we can assign an arbitary value to
268			// phi (or psi), and then determine the psi (or phi) or
269			// vice-versa. We'll assume that phi always gets the rotation,
270			// and psi is 0 in cases of singularity.
271	gezelter	2204	// we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
272	cli2	3520	// Since 0 <= theta <= 180, sin(theta) will be always
273			// non-negative. Therefore, it will never change the sign of both of
274			// the parameters passed to atan2.
275	tim	1586
276	cli2	3520	if (fabs(stheta) < 1e-6){
277	gezelter	2204	psi = 0.0;
278			phi = atan2(-this->data_[1][0], this->data_[0][0]);
279			}
280			// we only have one unique solution
281			else{
282			phi = atan2(this->data_[2][0], -this->data_[2][1]);
283			psi = atan2(this->data_[0][2], this->data_[1][2]);
284			}
285	tim	1586
286	gezelter	2204	//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
287			if (phi < 0)
288	cli2	3520	phi += 2.0 * M_PI;
289	tim	1586
290	gezelter	2204	if (psi < 0)
291	cli2	3520	psi += 2.0 * M_PI;
292	tim	1586
293	gezelter	2204	myEuler[0] = phi;
294			myEuler[1] = theta;
295			myEuler[2] = psi;
296	tim	1586
297	gezelter	2204	return myEuler;
298			}
299	tim	1563
300	gezelter	2204	/** Returns the determinant of this matrix. */
301			Real determinant() const {
302			Real x,y,z;
303	tim	1594
304	gezelter	2204	x = this->data_[0][0] * (this->data_[1][1] * this->data_[2][2] - this->data_[1][2] * this->data_[2][1]);
305			y = this->data_[0][1] * (this->data_[1][2] * this->data_[2][0] - this->data_[1][0] * this->data_[2][2]);
306			z = this->data_[0][2] * (this->data_[1][0] * this->data_[2][1] - this->data_[1][1] * this->data_[2][0]);
307	tim	1594
308	gezelter	2204	return(x + y + z);
309			}
310	gezelter	1930
311	gezelter	2204	/** Returns the trace of this matrix. */
312			Real trace() const {
313			return this->data_[0][0] + this->data_[1][1] + this->data_[2][2];
314			}
315	tim	1594
316	gezelter	2204	/**
317			* Sets the value of this matrix to the inversion of itself.
318			* @note since simple algorithm can be applied to inverse the 3 by 3 matrix, we hide the
319			* implementation of inverse in SquareMatrix class
320			*/
321			SquareMatrix3<Real> inverse() const {
322			SquareMatrix3<Real> m;
323	tim	2759	RealType det = determinant();
324	gezelter	2204	if (fabs(det) <= oopse::epsilon) {
325			//"The method was called on a matrix with \|determinant\| <= 1e-6.",
326			//"This is a runtime or a programming error in your application.");
327	tim	2611	std::vector<int> zeroDiagElementIndex;
328			for (int i =0; i < 3; ++i) {
329			if (fabs(this->data_[i][i]) <= oopse::epsilon) {
330			zeroDiagElementIndex.push_back(i);
331			}
332			}
333	tim	1563
334	tim	2611	if (zeroDiagElementIndex.size() == 2) {
335			int index = zeroDiagElementIndex[0];
336			m(index, index) = 1.0 / this->data_[index][index];
337			}else if (zeroDiagElementIndex.size() == 1) {
338	tim	1594
339	tim	2611	int a = (zeroDiagElementIndex[0] + 1) % 3;
340			int b = (zeroDiagElementIndex[0] + 2) %3;
341	tim	2759	RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b];
342	tim	2611	m(a, a) = this->data_[b][b] /denom;
343			m(b, a) = -this->data_[b][a]/denom;
344
345			m(a,b) = -this->data_[a][b]/denom;
346			m(b, b) = this->data_[a][a]/denom;
347
348			}
349
350			/*
351			for(std::vector<int>::iterator iter = zeroDiagElementIndex.begin(); iter != zeroDiagElementIndex.end() ++iter) {
352			if (this->data_[iter][0] > oopse::epsilon \|\| this->data_[iter][1] \|\|this->data_[*iter][2] \|\|
353			this->data_[0][iter] > oopse::epsilon \|\| this->data_[1][iter] \|\|this->data_[2][*iter] ) {
354			std::cout << "can not inverse matrix" << std::endl;
355			}
356			}
357			*/
358			} else {
359
360			m(0, 0) = this->data_[1][1]this->data_[2][2] - this->data_[1][2]this->data_[2][1];
361			m(1, 0) = this->data_[1][2]this->data_[2][0] - this->data_[1][0]this->data_[2][2];
362			m(2, 0) = this->data_[1][0]this->data_[2][1] - this->data_[1][1]this->data_[2][0];
363			m(0, 1) = this->data_[2][1]this->data_[0][2] - this->data_[2][2]this->data_[0][1];
364			m(1, 1) = this->data_[2][2]this->data_[0][0] - this->data_[2][0]this->data_[0][2];
365			m(2, 1) = this->data_[2][0]this->data_[0][1] - this->data_[2][1]this->data_[0][0];
366			m(0, 2) = this->data_[0][1]this->data_[1][2] - this->data_[0][2]this->data_[1][1];
367			m(1, 2) = this->data_[0][2]this->data_[1][0] - this->data_[0][0]this->data_[1][2];
368			m(2, 2) = this->data_[0][0]this->data_[1][1] - this->data_[0][1]this->data_[1][0];
369
370			m /= det;
371			}
372	gezelter	2204	return m;
373			}
374	tim	2583
375			SquareMatrix3<Real> transpose() const{
376			SquareMatrix3<Real> result;
377
378			for (unsigned int i = 0; i < 3; i++)
379			for (unsigned int j = 0; j < 3; j++)
380			result(j, i) = this->data_[i][j];
381
382			return result;
383			}
384	gezelter	2204	/**
385			* Extract the eigenvalues and eigenvectors from a 3x3 matrix.
386			* The eigenvectors (the columns of V) will be normalized.
387			* The eigenvectors are aligned optimally with the x, y, and z
388			* axes respectively.
389			* @param a symmetric matrix whose eigenvectors are to be computed. On return, the matrix is
390			* overwritten
391			* @param w will contain the eigenvalues of the matrix On return of this function
392			* @param v the columns of this matrix will contain the eigenvectors. The eigenvectors are
393			* normalized and mutually orthogonal.
394			* @warning a will be overwritten
395			*/
396			static void diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w, SquareMatrix3<Real>& v);
397			};
398			/*=========================================================================
399	tim	1569
400	tim	1616	Program: Visualization Toolkit
401			Module: $RCSfile: SquareMatrix3.hpp,v $
402	tim	1592
403	tim	1616	Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
404			All rights reserved.
405			See Copyright.txt or http://www.kitware.com/Copyright.htm for details.
406	tim	1594
407	gezelter	2204	This software is distributed WITHOUT ANY WARRANTY; without even
408			the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
409			PURPOSE. See the above copyright notice for more information.
410	tim	1594
411	gezelter	2204	=========================================================================*/
412			template<typename Real>
413			void SquareMatrix3<Real>::diagonalize(SquareMatrix3<Real>& a, Vector3<Real>& w,
414			SquareMatrix3<Real>& v) {
415			int i,j,k,maxI;
416			Real tmp, maxVal;
417			Vector3<Real> v_maxI, v_k, v_j;
418	tim	1594
419	gezelter	2204	// diagonalize using Jacobi
420			jacobi(a, w, v);
421			// if all the eigenvalues are the same, return identity matrix
422			if (w[0] == w[1] && w[0] == w[2] ) {
423			v = SquareMatrix3<Real>::identity();
424			return;
425			}
426	tim	1594
427	gezelter	2204	// transpose temporarily, it makes it easier to sort the eigenvectors
428			v = v.transpose();
429	tim	1616
430	gezelter	2204	// if two eigenvalues are the same, re-orthogonalize to optimally line
431			// up the eigenvectors with the x, y, and z axes
432			for (i = 0; i < 3; i++) {
433			if (w((i+1)%3) == w((i+2)%3)) {// two eigenvalues are the same
434			// find maximum element of the independant eigenvector
435			maxVal = fabs(v(i, 0));
436			maxI = 0;
437			for (j = 1; j < 3; j++) {
438			if (maxVal < (tmp = fabs(v(i, j)))){
439			maxVal = tmp;
440			maxI = j;
441			}
442			}
443	tim	1616
444	gezelter	2204	// swap the eigenvector into its proper position
445			if (maxI != i) {
446			tmp = w(maxI);
447			w(maxI) = w(i);
448			w(i) = tmp;
449	tim	1594
450	gezelter	2204	v.swapRow(i, maxI);
451			}
452			// maximum element of eigenvector should be positive
453			if (v(maxI, maxI) < 0) {
454			v(maxI, 0) = -v(maxI, 0);
455			v(maxI, 1) = -v(maxI, 1);
456			v(maxI, 2) = -v(maxI, 2);
457			}
458	tim	1594
459	gezelter	2204	// re-orthogonalize the other two eigenvectors
460			j = (maxI+1)%3;
461			k = (maxI+2)%3;
462	tim	1594
463	gezelter	2204	v(j, 0) = 0.0;
464			v(j, 1) = 0.0;
465			v(j, 2) = 0.0;
466			v(j, j) = 1.0;
467	tim	1594
468	gezelter	2204	/** @todo */
469			v_maxI = v.getRow(maxI);
470			v_j = v.getRow(j);
471			v_k = cross(v_maxI, v_j);
472			v_k.normalize();
473			v_j = cross(v_k, v_maxI);
474			v.setRow(j, v_j);
475			v.setRow(k, v_k);
476	tim	1594
477
478	gezelter	2204	// transpose vectors back to columns
479			v = v.transpose();
480			return;
481			}
482			}
483	tim	1594
484	gezelter	2204	// the three eigenvalues are different, just sort the eigenvectors
485			// to align them with the x, y, and z axes
486	tim	1594
487	gezelter	2204	// find the vector with the largest x element, make that vector
488			// the first vector
489			maxVal = fabs(v(0, 0));
490			maxI = 0;
491			for (i = 1; i < 3; i++) {
492			if (maxVal < (tmp = fabs(v(i, 0)))) {
493			maxVal = tmp;
494			maxI = i;
495			}
496			}
497	tim	1594
498	gezelter	2204	// swap eigenvalue and eigenvector
499			if (maxI != 0) {
500			tmp = w(maxI);
501			w(maxI) = w(0);
502			w(0) = tmp;
503			v.swapRow(maxI, 0);
504			}
505			// do the same for the y element
506			if (fabs(v(1, 1)) < fabs(v(2, 1))) {
507			tmp = w(2);
508			w(2) = w(1);
509			w(1) = tmp;
510			v.swapRow(2, 1);
511			}
512	tim	1594
513	gezelter	2204	// ensure that the sign of the eigenvectors is correct
514			for (i = 0; i < 2; i++) {
515			if (v(i, i) < 0) {
516			v(i, 0) = -v(i, 0);
517			v(i, 1) = -v(i, 1);
518			v(i, 2) = -v(i, 2);
519			}
520			}
521	tim	1563
522	gezelter	2204	// set sign of final eigenvector to ensure that determinant is positive
523			if (v.determinant() < 0) {
524			v(2, 0) = -v(2, 0);
525			v(2, 1) = -v(2, 1);
526			v(2, 2) = -v(2, 2);
527	tim	1616	}
528	gezelter	1930
529	gezelter	2204	// transpose the eigenvectors back again
530			v = v.transpose();
531			return ;
532			}
533	gezelter	1930
534	gezelter	2204	/**
535			* Return the multiplication of two matrixes (m1 * m2).
536			* @return the multiplication of two matrixes
537			* @param m1 the first matrix
538			* @param m2 the second matrix
539			*/
540			template<typename Real>
541			inline SquareMatrix3<Real> operator *(const SquareMatrix3<Real>& m1, const SquareMatrix3<Real>& m2) {
542			SquareMatrix3<Real> result;
543	gezelter	1930
544	gezelter	2204	for (unsigned int i = 0; i < 3; i++)
545			for (unsigned int j = 0; j < 3; j++)
546			for (unsigned int k = 0; k < 3; k++)
547			result(i, j) += m1(i, k) * m2(k, j);
548	gezelter	1930
549	gezelter	2204	return result;
550			}
551	gezelter	1930
552	gezelter	2204	template<typename Real>
553			inline SquareMatrix3<Real> outProduct(const Vector3<Real>& v1, const Vector3<Real>& v2) {
554			SquareMatrix3<Real> result;
555
556			for (unsigned int i = 0; i < 3; i++) {
557			for (unsigned int j = 0; j < 3; j++) {
558			result(i, j) = v1[i] * v2[j];
559			}
560			}
561	gezelter	1930
562	gezelter	2204	return result;
563			}
564	gezelter	1930
565
566	tim	2759	typedef SquareMatrix3<RealType> Mat3x3d;
567			typedef SquareMatrix3<RealType> RotMat3x3d;
568	tim	1586
569			} //namespace oopse
570			#endif // MATH_SQUAREMATRIX_HPP
571	tim	1616