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:	1360
Committed:	Mon Sep 7 16:31:51 2009 UTC (15 years, 7 months ago) by cli2
Original Path:	*trunk/src/math/SquareMatrix3.hpp*
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	507	/*
2	gezelter	246	* Copyright (c) 2005 The University of Notre Dame. All Rights Reserved.
3	tim	70	*
4	gezelter	246	* 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	70	*/
41	gezelter	246
42	tim	70	/**
43			* @file SquareMatrix3.hpp
44			* @author Teng Lin
45			* @date 10/11/2004
46			* @version 1.0
47			*/
48	gezelter	507	#ifndef MATH_SQUAREMATRIX3_HPP
49	tim	99	#define MATH_SQUAREMATRIX3_HPP
50	tim	895	#include <vector>
51	tim	93	#include "Quaternion.hpp"
52	tim	70	#include "SquareMatrix.hpp"
53	tim	93	#include "Vector3.hpp"
54	tim	451	#include "utils/NumericConstant.hpp"
55	tim	70	namespace oopse {
56
57	gezelter	507	template<typename Real>
58			class SquareMatrix3 : public SquareMatrix<Real, 3> {
59			public:
60	tim	137
61	gezelter	507	typedef Real ElemType;
62			typedef Real* ElemPoinerType;
63	tim	70
64	gezelter	507	/** default constructor */
65			SquareMatrix3() : SquareMatrix<Real, 3>() {
66			}
67	tim	70
68	gezelter	507	/** Constructs and initializes every element of this matrix to a scalar */
69			SquareMatrix3(Real s) : SquareMatrix<Real,3>(s){
70			}
71	tim	151
72	gezelter	507	/** Constructs and initializes from an array */
73			SquareMatrix3(Real* array) : SquareMatrix<Real,3>(array){
74			}
75	tim	151
76
77	gezelter	507	/** copy constructor */
78			SquareMatrix3(const SquareMatrix<Real, 3>& m) : SquareMatrix<Real, 3>(m) {
79			}
80	gezelter	246
81	gezelter	507	SquareMatrix3( const Vector3<Real>& eulerAngles) {
82			setupRotMat(eulerAngles);
83			}
84	tim	93
85	gezelter	507	SquareMatrix3(Real phi, Real theta, Real psi) {
86			setupRotMat(phi, theta, psi);
87			}
88	tim	93
89	gezelter	507	SquareMatrix3(const Quaternion<Real>& q) {
90			setupRotMat(q);
91	tim	113
92	gezelter	507	}
93	tim	93
94	gezelter	507	SquareMatrix3(Real w, Real x, Real y, Real z) {
95			setupRotMat(w, x, y, z);
96			}
97	tim	93
98	gezelter	507	/** 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	76
106	gezelter	246
107	gezelter	507	SquareMatrix3<Real>& operator =(const Quaternion<Real>& q) {
108			this->setupRotMat(q);
109			return *this;
110			}
111	gezelter	246
112	gezelter	507	/**
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	76
120	gezelter	507	/**
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	76
130	gezelter	507	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	76
137	gezelter	507	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	93
141	gezelter	507	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	93
145	gezelter	507	this->data_[2][0] = stheta * sphi;
146			this->data_[2][1] = -stheta * cphi;
147			this->data_[2][2] = ctheta;
148			}
149	tim	93
150
151	gezelter	507	/**
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	76
159	gezelter	507	/**
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	76
171	tim	891	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	507	/**
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	76
202	tim	637	if( t > NumericConstant::epsilon ){
203	tim	93
204	gezelter	507	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	93
211	tim	633	ad1 = this->data_[0][0];
212			ad2 = this->data_[1][1];
213			ad3 = this->data_[2][2];
214	tim	93
215	gezelter	507	if( ad1 >= ad2 && ad1 >= ad3 ){
216	tim	93
217	gezelter	507	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	93
230	gezelter	507	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	93
238	gezelter	507	return q;
239	tim	93
240	gezelter	507	}
241	tim	93
242	gezelter	507	/**
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	1360	* 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	507	*/
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	93
260	gezelter	507	// set the tolerance for Euler angles and rotation elements
261	tim	93
262	tim	963	theta = acos(std::min((RealType)1.0, std::max((RealType)-1.0,this->data_[2][2])));
263	gezelter	507	ctheta = this->data_[2][2];
264			stheta = sqrt(1.0 - ctheta * ctheta);
265	tim	93
266	cli2	1360	// 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	507	// we use atan2 instead of atan, since atan2 will give us -Pi to Pi.
272	cli2	1360	// 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	93
276	cli2	1360	if (fabs(stheta) < 1e-6){
277	gezelter	507	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	93
286	gezelter	507	//wrap phi and psi, make sure they are in the range from 0 to 2*Pi
287			if (phi < 0)
288	cli2	1360	phi += 2.0 * M_PI;
289	tim	93
290	gezelter	507	if (psi < 0)
291	cli2	1360	psi += 2.0 * M_PI;
292	tim	93
293	gezelter	507	myEuler[0] = phi;
294			myEuler[1] = theta;
295			myEuler[2] = psi;
296	tim	93
297	gezelter	507	return myEuler;
298			}
299	tim	70
300	gezelter	507	/** Returns the determinant of this matrix. */
301			Real determinant() const {
302			Real x,y,z;
303	tim	101
304	gezelter	507	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	101
308	gezelter	507	return(x + y + z);
309			}
310	gezelter	246
311	gezelter	507	/** 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	101
316	gezelter	507	/**
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	963	RealType det = determinant();
324	gezelter	507	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	895	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	70
334	tim	895	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	101
339	tim	895	int a = (zeroDiagElementIndex[0] + 1) % 3;
340			int b = (zeroDiagElementIndex[0] + 2) %3;
341	tim	963	RealType denom = this->data_[a][a] * this->data_[b][b] - this->data_[b][a]*this->data_[a][b];
342	tim	895	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	507	return m;
373			}
374	tim	883
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	507	/**
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	76
400	tim	123	Program: Visualization Toolkit
401			Module: $RCSfile: SquareMatrix3.hpp,v $
402	tim	99
403	tim	123	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	101
407	gezelter	507	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	101
411	gezelter	507	=========================================================================*/
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	101
419	gezelter	507	// 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	101
427	gezelter	507	// transpose temporarily, it makes it easier to sort the eigenvectors
428			v = v.transpose();
429	tim	123
430	gezelter	507	// 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	123
444	gezelter	507	// 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	101
450	gezelter	507	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	101
459	gezelter	507	// re-orthogonalize the other two eigenvectors
460			j = (maxI+1)%3;
461			k = (maxI+2)%3;
462	tim	101
463	gezelter	507	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	101
468	gezelter	507	/** @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	101
477
478	gezelter	507	// transpose vectors back to columns
479			v = v.transpose();
480			return;
481			}
482			}
483	tim	101
484	gezelter	507	// the three eigenvalues are different, just sort the eigenvectors
485			// to align them with the x, y, and z axes
486	tim	101
487	gezelter	507	// 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	101
498	gezelter	507	// 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	101
513	gezelter	507	// 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	70
522	gezelter	507	// 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	123	}
528	gezelter	246
529	gezelter	507	// transpose the eigenvectors back again
530			v = v.transpose();
531			return ;
532			}
533	gezelter	246
534	gezelter	507	/**
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	246
544	gezelter	507	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	246
549	gezelter	507	return result;
550			}
551	gezelter	246
552	gezelter	507	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	246
562	gezelter	507	return result;
563			}
564	gezelter	246
565
566	tim	963	typedef SquareMatrix3<RealType> Mat3x3d;
567			typedef SquareMatrix3<RealType> RotMat3x3d;
568	tim	93
569			} //namespace oopse
570			#endif // MATH_SQUAREMATRIX_HPP
571	tim	123