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