src/openbabel/matrix3x3.cpp

/**********************************************************************
matrix3x3.cpp - Handle 3D rotation matrix.
 
Copyright (C) 1998-2001 by OpenEye Scientific Software, Inc.
Some portions Copyright (C) 2001-2005 by Geoffrey R. Hutchison
 
This file is part of the Open Babel project.
For more information, see <http://openbabel.sourceforge.net/>
 
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation version 2 of the License.
 
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
***********************************************************************/

#include <math.h>

#include "mol.hpp"
#include "matrix3x3.hpp"

#ifndef true
#define true 1
#endif

#ifndef false
#define false 0
#endif

using namespace std;

namespace OpenBabel
{

/** \class matrix3x3
    \brief Represents a real 3x3 matrix.
 
 Rotating points in space can be performed by a vector-matrix
 multiplication. The matrix3x3 class is designed as a helper to the
 vector3 class for rotating points in space. The rotation matrix may be
 initialised by passing in the array of doubleing point values, by
 passing euler angles, or a rotation vector and angle of rotation about
 that vector. Once set, the matrix3x3 class can be used to rotate
 vectors by the overloaded multiplication operator. The following
 demonstrates the usage of the matrix3x3 class:
 
\code
  matrix3x3 mat;
  mat.SetupRotMat(0.0,180.0,0.0); //rotate theta by 180 degrees
  vector3 v = VX;
  v *= mat; //apply the rotation
\endcode
 
*/

/*! the axis of the rotation will be uniformly distributed on
  the unit sphere, the angle will be uniformly distributed in
  the interval 0..360 degrees. */
void matrix3x3::randomRotation(OBRandom &rnd)
{
    double rotAngle;
    vector3 v1;

    v1.randomUnitVector(&rnd);
    rotAngle = 360.0 * rnd.NextFloat();
    this->RotAboutAxisByAngle(v1,rotAngle);
}

void matrix3x3::SetupRotMat(double phi,double theta,double psi)
{
    double p  = phi * DEG_TO_RAD;
    double h  = theta * DEG_TO_RAD;
    double b  = psi * DEG_TO_RAD;

    double cx = cos(p);
    double sx = sin(p);
    double cy = cos(h);
    double sy = sin(h);
    double cz = cos(b);
    double sz = sin(b);

    ele[0][0] = cy*cz;
    ele[0][1] = cy*sz;
    ele[0][2] = -sy;

    ele[1][0] = sx*sy*cz-cx*sz;
    ele[1][1] = sx*sy*sz+cx*cz;
    ele[1][2] = sx*cy;

    ele[2][0] = cx*sy*cz+sx*sz;
    ele[2][1] = cx*sy*sz-sx*cz;
    ele[2][2] = cx*cy;
}

/*! Replaces *this with a matrix that represents reflection on
  the plane through 0 which is given by the normal vector norm.
        
  \warning If the vector norm has length zero, this method will
  generate the 0-matrix. If the length of the axis is close to
  zero, but not == 0.0, this method may behave in unexpected
  ways and return almost random results; details may depend on
  your particular doubleing point implementation. The use of this
  method is therefore highly discouraged, unless you are certain
  that the length is in a reasonable range, away from 0.0
  (Stefan Kebekus)
        
  \deprecated This method will probably replaced by a safer
  algorithm in the future.
        
  \todo Replace this method with a more fool-proof version.
        
  @param norm specifies the normal to the plane
*/
void matrix3x3::PlaneReflection(const vector3 &norm)
{
    //@@@ add a safety net

    vector3 normtmp = norm;
    normtmp.normalize();

    SetColumn(0, vector3(1,0,0) - 2*normtmp.x()*normtmp);
    SetColumn(1, vector3(0,1,0) - 2*normtmp.y()*normtmp);
    SetColumn(2, vector3(0,0,1) - 2*normtmp.z()*normtmp);
}

#define x vtmp.x()
#define y vtmp.y()
#define z vtmp.z()

/*! Replaces *this with a matrix that represents rotation about the
  axis by a an angle. 
        
  \warning If the vector axis has length zero, this method will
  generate the 0-matrix. If the length of the axis is close to
  zero, but not == 0.0, this method may behave in unexpected ways
  and return almost random results; details may depend on your
  particular doubleing point implementation. The use of this method
  is therefore highly discouraged, unless you are certain that the
  length is in a reasonable range, away from 0.0 (Stefan
  Kebekus)
        
  \deprecated This method will probably replaced by a safer
  algorithm in the future.
        
  \todo Replace this method with a more fool-proof version.
        
  @param v specifies the axis of the rotation
  @param angle angle in degrees (0..360)
*/
void matrix3x3::RotAboutAxisByAngle(const vector3 &v,const double angle)
{
    double theta = angle*DEG_TO_RAD;
    double s = sin(theta);
    double c = cos(theta);
    double t = 1 - c;

    vector3 vtmp = v;
    vtmp.normalize();

    ele[0][0] = t*x*x + c;
    ele[0][1] = t*x*y + s*z;
    ele[0][2] = t*x*z - s*y;

    ele[1][0] = t*x*y - s*z;
    ele[1][1] = t*y*y + c;
    ele[1][2] = t*y*z + s*x;

    ele[2][0] = t*x*z + s*y;
    ele[2][1] = t*y*z - s*x;
    ele[2][2] = t*z*z + c;
}

#undef x
#undef y
#undef z

void matrix3x3::SetColumn(int col, const vector3 &v)
{
    if (col > 2)
    {
        obErrorLog.ThrowError(__FUNCTION__,
                              "The method was called with col > 2.", obError);
    }

    ele[0][col] = v.x();
    ele[1][col] = v.y();
    ele[2][col] = v.z();
}

void matrix3x3::SetRow(int row, const vector3 &v)
{
    if (row > 2)
    {
        obErrorLog.ThrowError(__FUNCTION__,
                              "The method was called with row > 2.", obError);
    }

    ele[row][0] = v.x();
    ele[row][1] = v.y();
    ele[row][2] = v.z();
}

vector3 matrix3x3::GetColumn(unsigned int col)
{
    if (col > 2)
    {
        obErrorLog.ThrowError(__FUNCTION__,
                              "The method was called with col > 2.", obError);
    }

    return vector3(ele[0][col], ele[1][col], ele[2][col]);
}

vector3 matrix3x3::GetRow(unsigned int row)
{
    if (row > 2)
    {
        obErrorLog.ThrowError(__FUNCTION__,
                              "The method was called with row > 2.", obError);
    }

    return vector3(ele[row][0], ele[row][1], ele[row][2]);
}

/*! calculates the product m*v of the matrix m and the column
  vector represented by v
*/
vector3 operator *(const matrix3x3 &m,const vector3 &v)
{
    vector3 vv;

    vv._vx = v._vx*m.ele[0][0] + v._vy*m.ele[0][1] + v._vz*m.ele[0][2];
    vv._vy = v._vx*m.ele[1][0] + v._vy*m.ele[1][1] + v._vz*m.ele[1][2];
    vv._vz = v._vx*m.ele[2][0] + v._vy*m.ele[2][1] + v._vz*m.ele[2][2];

    return(vv);
}

matrix3x3 operator *(const matrix3x3 &A,const matrix3x3 &B)
{
    matrix3x3 result;

    result.ele[0][0] = A.ele[0][0]*B.ele[0][0] + A.ele[0][1]*B.ele[1][0] + A.ele[0][2]*B.ele[2][0];
    result.ele[0][1] = A.ele[0][0]*B.ele[0][1] + A.ele[0][1]*B.ele[1][1] + A.ele[0][2]*B.ele[2][1];
    result.ele[0][2] = A.ele[0][0]*B.ele[0][2] + A.ele[0][1]*B.ele[1][2] + A.ele[0][2]*B.ele[2][2];

    result.ele[1][0] = A.ele[1][0]*B.ele[0][0] + A.ele[1][1]*B.ele[1][0] + A.ele[1][2]*B.ele[2][0];
    result.ele[1][1] = A.ele[1][0]*B.ele[0][1] + A.ele[1][1]*B.ele[1][1] + A.ele[1][2]*B.ele[2][1];
    result.ele[1][2] = A.ele[1][0]*B.ele[0][2] + A.ele[1][1]*B.ele[1][2] + A.ele[1][2]*B.ele[2][2];

    result.ele[2][0] = A.ele[2][0]*B.ele[0][0] + A.ele[2][1]*B.ele[1][0] + A.ele[2][2]*B.ele[2][0];
    result.ele[2][1] = A.ele[2][0]*B.ele[0][1] + A.ele[2][1]*B.ele[1][1] + A.ele[2][2]*B.ele[2][1];
    result.ele[2][2] = A.ele[2][0]*B.ele[0][2] + A.ele[2][1]*B.ele[1][2] + A.ele[2][2]*B.ele[2][2];

    return(result);
}

/*! calculates the product m*(*this) of the matrix m and the
  column vector represented by *this
*/
vector3 &vector3::operator *= (const matrix3x3 &m)
{
    vector3 vv;

    vv.SetX(_vx*m.Get(0,0) + _vy*m.Get(0,1) + _vz*m.Get(0,2));
    vv.SetY(_vx*m.Get(1,0) + _vy*m.Get(1,1) + _vz*m.Get(1,2));
    vv.SetZ(_vx*m.Get(2,0) + _vy*m.Get(2,1) + _vz*m.Get(2,2));
    _vx = vv.x();
    _vy = vv.y();
    _vz = vv.z();

    return(*this);
}

/*! This method checks if the absolute value of the determinant is smaller than 1e-6. If
  so, nothing is done and an exception is thrown. Otherwise, the
  inverse matrix is calculated and returned. *this is not changed.
        
  \warning If the determinant is close to zero, but not == 0.0,
  this method may behave in unexpected ways and return almost
  random results; details may depend on your particular doubleing
  point implementation. The use of this method is therefore highly
  discouraged, unless you are certain that the determinant is in a
  reasonable range, away from 0.0 (Stefan Kebekus)
*/
matrix3x3 matrix3x3::inverse(void)
{
    double det = determinant();
    if (fabs(det) <= 1e-6)
    {
        obErrorLog.ThrowError(__FUNCTION__,
                              "The method was called on a matrix with |determinant| <= 1e-6.", obError);
    }

    matrix3x3 inverse;
    inverse.ele[0][0] = ele[1][1]*ele[2][2] - ele[1][2]*ele[2][1];
    inverse.ele[1][0] = ele[1][2]*ele[2][0] - ele[1][0]*ele[2][2];
    inverse.ele[2][0] = ele[1][0]*ele[2][1] - ele[1][1]*ele[2][0];
    inverse.ele[0][1] = ele[2][1]*ele[0][2] - ele[2][2]*ele[0][1];
    inverse.ele[1][1] = ele[2][2]*ele[0][0] - ele[2][0]*ele[0][2];
    inverse.ele[2][1] = ele[2][0]*ele[0][1] - ele[2][1]*ele[0][0];
    inverse.ele[0][2] = ele[0][1]*ele[1][2] - ele[0][2]*ele[1][1];
    inverse.ele[1][2] = ele[0][2]*ele[1][0] - ele[0][0]*ele[1][2];
    inverse.ele[2][2] = ele[0][0]*ele[1][1] - ele[0][1]*ele[1][0];

    inverse /= det;

    return(inverse);
}

/* This method returns the transpose of a matrix. The original
   matrix remains unchanged. */
matrix3x3 matrix3x3::transpose(void) const
{
    matrix3x3 transpose;

    for(unsigned int i=0; i<3; i++)
        for(unsigned int j=0; j<3; j++)
            transpose.ele[i][j] = ele[j][i];

    return(transpose);
}

double matrix3x3::determinant(void) const
{
    double x,y,z;

    x = ele[0][0] * (ele[1][1] * ele[2][2] - ele[1][2] * ele[2][1]);
    y = ele[0][1] * (ele[1][2] * ele[2][0] - ele[1][0] * ele[2][2]);
    z = ele[0][2] * (ele[1][0] * ele[2][1] - ele[1][1] * ele[2][0]);

    return(x + y + z);
}

/*! This method returns false if there are indices i,j such that
  fabs(*this[i][j]-*this[j][i]) > 1e-6. Otherwise, it returns
  true. */
bool matrix3x3::isSymmetric(void) const
{
    if (fabs(ele[0][1] - ele[1][0]) > 1e-6)
        return false;
    if (fabs(ele[0][2] - ele[2][0]) > 1e-6)
        return false;
    if (fabs(ele[1][2] - ele[2][1]) > 1e-6)
        return false;
    return true;
}

/*! This method returns false if there are indices i != j such
  that fabs(*this[i][j]) > 1e-6. Otherwise, it returns true. */
bool matrix3x3::isDiagonal(void) const
{
    if (fabs(ele[0][1]) > 1e-6)
        return false;
    if (fabs(ele[0][2]) > 1e-6)
        return false;
    if (fabs(ele[1][2]) > 1e-6)
        return false;

    if (fabs(ele[1][0]) > 1e-6)
        return false;
    if (fabs(ele[2][0]) > 1e-6)
        return false;
    if (fabs(ele[2][1]) > 1e-6)
        return false;

    return true;
}

/*! This method returns false if there are indices i != j such
  that fabs(*this[i][j]) > 1e-6, or if there is an index i such
  that fabs(*this[i][j]-1) > 1e-6. Otherwise, it returns
  true. */
bool matrix3x3::isUnitMatrix(void) const
{
    if (!isDiagonal())
        return false;

    if (fabs(ele[0][0]-1) > 1e-6)
        return false;
    if (fabs(ele[1][1]-1) > 1e-6)
        return false;
    if (fabs(ele[2][2]-1) > 1e-6)
        return false;

    return true;
}

/*! This method employs the static method matrix3x3::jacobi(...)
  to find the eigenvalues and eigenvectors of a symmetric
  matrix. On entry it is checked if the matrix really is
  symmetric: if isSymmetric() returns 'false', an OBError is
  thrown.
 
  \note The jacobi algorithm is should work great for all
  symmetric 3x3 matrices. If you need to find the eigenvectors
  of a non-symmetric matrix, you might want to resort to the
  sophisticated routines of LAPACK.
 
  @param eigenvals a reference to a vector3 where the
  eigenvalues will be stored. The eigenvalues are ordered so
  that eigenvals[0] <= eigenvals[1] <= eigenvals[2].
 
  @return an orthogonal matrix whose ith column is an
  eigenvector for the eigenvalue eigenvals[i]. Here 'orthogonal'
  means that all eigenvectors have length one and are mutually
  orthogonal. The ith eigenvector can thus be conveniently
  accessed by the GetColumn() method, as in the following
  example.
  \code
  // Calculate eigenvectors and -values
  vector3 eigenvals;
  matrix3x3 eigenmatrix = somematrix.findEigenvectorsIfSymmetric(eigenvals);
  
  // Print the 2nd eigenvector
  cout << eigenmatrix.GetColumn(1) << endl;
  \endcode
  With these conventions, a matrix is diagonalized in the following way:
  \code
  // Diagonalize the matrix
  matrix3x3 diagonalMatrix = eigenmatrix.inverse() * somematrix * eigenmatrix;
  \endcode
  
*/
matrix3x3 matrix3x3::findEigenvectorsIfSymmetric(vector3 &eigenvals)
{
    matrix3x3 result;

    if (!isSymmetric())
    {
        obErrorLog.ThrowError(__FUNCTION__,
                              "The method was called on a matrix that was not symmetric, i.e. where isSymetric() == false.", obError);
    }

    double d[3];
    matrix3x3 copyOfThis = *this;

    jacobi(3, copyOfThis.ele[0], d, result.ele[0]);
    eigenvals.Set(d);

    return result;
}

matrix3x3 &matrix3x3::operator/=(const double &c)
{
    for (int row = 0;row < 3; row++)
        for (int col = 0;col < 3; col++)
            ele[row][col] /= c;

    return(*this);
}

#ifndef SQUARE
#define SQUARE(x) ((x)*(x))
#endif

void matrix3x3::FillOrth(double Alpha,double Beta, double Gamma,
                         double A, double B, double C)
{
    double V;

    Alpha *= DEG_TO_RAD;
    Beta  *= DEG_TO_RAD;
    Gamma *= DEG_TO_RAD;

    // from the PDB specification:
    //  http://www.rcsb.org/pdb/docs/format/pdbguide2.2/part_75.html


    // since we'll ultimately divide by (a * b), we've factored those out here
    V = C * sqrt(1 - SQUARE(cos(Alpha)) - SQUARE(cos(Beta)) - SQUARE(cos(Gamma))
                 + 2 * cos(Alpha) * cos(Beta) * cos(Gamma));

    ele[0][0] = A;
    ele[0][1] = B*cos(Gamma);
    ele[0][2] = C*cos(Beta);

    ele[1][0] = 0.0;
    ele[1][1] = B*sin(Gamma);
    ele[1][2] = C*(cos(Alpha)-cos(Beta)*cos(Gamma))/sin(Gamma);

    ele[2][0] = 0.0;
    ele[2][1] = 0.0;
    ele[2][2] = V / (sin(Gamma)); // again, we factored out A * B when defining V
}

ostream& operator<< ( ostream& co, const matrix3x3& m )

{
    co << "[ "
    << m.ele[0][0]
    << ", "
    << m.ele[0][1]
    << ", "
    << m.ele[0][2]
    << " ]" << endl;

    co << "[ "
    << m.ele[1][0]
    << ", "
    << m.ele[1][1]
    << ", "
    << m.ele[1][2]
    << " ]" << endl;

    co << "[ "
    << m.ele[2][0]
    << ", "
    << m.ele[2][1]
    << ", "
    << m.ele[2][2]
    << " ]" << endl;

    return co ;
}

/*! This static function computes the eigenvalues and
  eigenvectors of a SYMMETRIC nxn matrix. This method is used
  internally by OpenBabel, but may be useful as a general
  eigenvalue finder.
  
  The algorithm uses Jacobi transformations. It is described
  e.g. in Wilkinson, Reinsch "Handbook for automatic computation,
  Volume II: Linear Algebra", part II, contribution II/1. The
  implementation is also similar to the implementation in this
  book. This method is adequate to solve the eigenproblem for
  small matrices, of size perhaps up to 10x10. For bigger
  problems, you might want to resort to the sophisticated routines
  of LAPACK.
  
  \note If you plan to find the eigenvalues of a symmetric 3x3
  matrix, you will probably prefer to use the more convenient
  method findEigenvectorsIfSymmetric()
  
  @param n the size of the matrix that should be diagonalized
  
  @param a array of size n^2 which holds the symmetric matrix
  whose eigenvectors are to be computed. The convention is that
  the entry in row r and column c is addressed as a[n*r+c] where,
  of course, 0 <= r < n and 0 <= c < n. There is no check that the
  matrix is actually symmetric. If it is not, the behaviour of
  this function is undefined.  On return, the matrix is
  overwritten with junk.
  
  @param d pointer to a field of at least n doubles which will be
  overwritten. On return of this function, the entries d[0]..d[n-1]
  will contain the eigenvalues of the matrix.
  
  @param v an array of size n^2 where the eigenvectors will be
  stored. On return, the columns of this matrix will contain the
  eigenvectors. The eigenvectors are normalized and mutually
  orthogonal.
*/
void matrix3x3::jacobi(unsigned int n, double *a, double *d, double *v)
{
    double onorm, dnorm;
    double b, dma, q, t, c, s;
    double  atemp, vtemp, dtemp;
    register int i, j, k, l;
    int nrot;

    int MAX_SWEEPS=50;

    // Set v to the identity matrix, set the vector d to contain the
    // diagonal elements of the matrix a
    for (j = 0; j < static_cast<int>(n); j++)
    {
        for (i = 0; i < static_cast<int>(n); i++)
            v[n*i+j] = 0.0;
        v[n*j+j] = 1.0;
        d[j] = a[n*j+j];
    }

    nrot = MAX_SWEEPS;
    for (l = 1; l <= nrot; l++)
    {
        // Set dnorm to be the maximum norm of the diagonal elements, set
        // onorm to the maximum norm of the off-diagonal elements
        dnorm = 0.0;
        onorm = 0.0;
        for (j = 0; j < static_cast<int>(n); j++)
        {
            dnorm += (double)fabs(d[j]);
            for (i = 0; i < j; i++)
                onorm += (double)fabs(a[n*i+j]);
        }
        // Normal end point of this algorithm.
        if((onorm/dnorm) <= 1.0e-12)
            goto Exit_now;

        for (j = 1; j < static_cast<int>(n); j++)
        {
            for (i = 0; i <= j - 1; i++)
            {

                b = a[n*i+j];
                if(fabs(b) > 0.0)
                {
                    dma = d[j] - d[i];
                    if((fabs(dma) + fabs(b)) <=  fabs(dma))
                        t = b / dma;
                    else
                    {
                        q = 0.5 * dma / b;
                        t = 1.0/((double)fabs(q) + (double)sqrt(1.0+q*q));
                        if (q < 0.0)
                            t = -t;
                    }

                    c = 1.0/(double)sqrt(t*t + 1.0);
                    s = t * c;
                    a[n*i+j] = 0.0;

                    for (k = 0; k <= i-1; k++)
                    {
                        atemp = c * a[n*k+i] - s * a[n*k+j];
                        a[n*k+j] = s * a[n*k+i] + c * a[n*k+j];
                        a[n*k+i] = atemp;
                    }

                    for (k = i+1; k <= j-1; k++)
                    {
                        atemp = c * a[n*i+k] - s * a[n*k+j];
                        a[n*k+j] = s * a[n*i+k] + c * a[n*k+j];
                        a[n*i+k] = atemp;
                    }

                    for (k = j+1; k < static_cast<int>(n); k++)
                    {
                        atemp = c * a[n*i+k] - s * a[n*j+k];
                        a[n*j+k] = s * a[n*i+k] + c * a[n*j+k];
                        a[n*i+k] = atemp;
                    }

                    for (k = 0; k < static_cast<int>(n); k++)
                    {
                        vtemp = c * v[n*k+i] - s * v[n*k+j];
                        v[n*k+j] = s * v[n*k+i] + c * v[n*k+j];
                        v[n*k+i] = vtemp;
                    }

                    dtemp = c*c*d[i] + s*s*d[j] - 2.0*c*s*b;
                    d[j] = s*s*d[i] + c*c*d[j] +  2.0*c*s*b;
                    d[i] = dtemp;
                } /* end if */
            } /* end for i */
        } /* end for j */
    } /* end for l */

Exit_now:

    // Now sort the eigenvalues (and the eigenvectors) so that the
    // smallest eigenvalues come first.
    nrot = l;

    for (j = 0; j < static_cast<int>(n)-1; j++)
    {
        k = j;
        dtemp = d[k];
        for (i = j+1; i < static_cast<int>(n); i++)
            if(d[i] < dtemp)
            {
                k = i;
                dtemp = d[k];
            }

        if(k > j)
        {
            d[k] = d[j];
            d[j] = dtemp;
            for (i = 0; i < static_cast<int>(n); i++)
            {
                dtemp = v[n*i+k];
                v[n*i+k] = v[n*i+j];
                v[n*i+j] = dtemp;
            }
        }
    }
}

} // end namespace OpenBabel

//! \file matrix3x3.cpp
//! \brief Handle 3D rotation matrix.

Revision:	2477
Committed:	Fri Dec 2 20:11:25 2005 UTC (18 years, 9 months ago) by gezelter
File size:	20600 byte(s)
Log Message:	Errors are no longer thrown with consts (fixes compilation bug on the Mac).
#	User	Rev	Content
1	tim	2440	/**********************************************************************
2			matrix3x3.cpp - Handle 3D rotation matrix.
3
4			Copyright (C) 1998-2001 by OpenEye Scientific Software, Inc.
5			Some portions Copyright (C) 2001-2005 by Geoffrey R. Hutchison
6
7			This file is part of the Open Babel project.
8			For more information, see <http://openbabel.sourceforge.net/>
9
10			This program is free software; you can redistribute it and/or modify
11			it under the terms of the GNU General Public License as published by
12			the Free Software Foundation version 2 of the License.
13
14			This program is distributed in the hope that it will be useful,
15			but WITHOUT ANY WARRANTY; without even the implied warranty of
16			MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17			GNU General Public License for more details.
18			***********************************************************************/
19
20			#include <math.h>
21
22			#include "mol.hpp"
23			#include "matrix3x3.hpp"
24
25			#ifndef true
26			#define true 1
27			#endif
28
29			#ifndef false
30			#define false 0
31			#endif
32
33			using namespace std;
34
35			namespace OpenBabel
36			{
37
38			/** \class matrix3x3
39			\brief Represents a real 3x3 matrix.
40
41			Rotating points in space can be performed by a vector-matrix
42			multiplication. The matrix3x3 class is designed as a helper to the
43			vector3 class for rotating points in space. The rotation matrix may be
44			initialised by passing in the array of doubleing point values, by
45			passing euler angles, or a rotation vector and angle of rotation about
46			that vector. Once set, the matrix3x3 class can be used to rotate
47			vectors by the overloaded multiplication operator. The following
48			demonstrates the usage of the matrix3x3 class:
49
50			\code
51			matrix3x3 mat;
52			mat.SetupRotMat(0.0,180.0,0.0); //rotate theta by 180 degrees
53			vector3 v = VX;
54			v *= mat; //apply the rotation
55			\endcode
56
57			*/
58
59			/*! the axis of the rotation will be uniformly distributed on
60			the unit sphere, the angle will be uniformly distributed in
61			the interval 0..360 degrees. */
62			void matrix3x3::randomRotation(OBRandom &rnd)
63			{
64			double rotAngle;
65			vector3 v1;
66
67			v1.randomUnitVector(&rnd);
68			rotAngle = 360.0 * rnd.NextFloat();
69			this->RotAboutAxisByAngle(v1,rotAngle);
70			}
71
72			void matrix3x3::SetupRotMat(double phi,double theta,double psi)
73			{
74			double p = phi * DEG_TO_RAD;
75			double h = theta * DEG_TO_RAD;
76			double b = psi * DEG_TO_RAD;
77
78			double cx = cos(p);
79			double sx = sin(p);
80			double cy = cos(h);
81			double sy = sin(h);
82			double cz = cos(b);
83			double sz = sin(b);
84
85			ele[0][0] = cy*cz;
86			ele[0][1] = cy*sz;
87			ele[0][2] = -sy;
88
89			ele[1][0] = sxsycz-cx*sz;
90			ele[1][1] = sxsysz+cx*cz;
91			ele[1][2] = sx*cy;
92
93			ele[2][0] = cxsycz+sx*sz;
94			ele[2][1] = cxsysz-sx*cz;
95			ele[2][2] = cx*cy;
96			}
97
98			/! Replaces this with a matrix that represents reflection on
99			the plane through 0 which is given by the normal vector norm.
100
101			\warning If the vector norm has length zero, this method will
102			generate the 0-matrix. If the length of the axis is close to
103			zero, but not == 0.0, this method may behave in unexpected
104			ways and return almost random results; details may depend on
105			your particular doubleing point implementation. The use of this
106			method is therefore highly discouraged, unless you are certain
107			that the length is in a reasonable range, away from 0.0
108			(Stefan Kebekus)
109
110			\deprecated This method will probably replaced by a safer
111			algorithm in the future.
112
113			\todo Replace this method with a more fool-proof version.
114
115			@param norm specifies the normal to the plane
116			*/
117			void matrix3x3::PlaneReflection(const vector3 &norm)
118			{
119			//@@@ add a safety net
120
121			vector3 normtmp = norm;
122			normtmp.normalize();
123
124			SetColumn(0, vector3(1,0,0) - 2normtmp.x()normtmp);
125			SetColumn(1, vector3(0,1,0) - 2normtmp.y()normtmp);
126			SetColumn(2, vector3(0,0,1) - 2normtmp.z()normtmp);
127			}
128
129			#define x vtmp.x()
130			#define y vtmp.y()
131			#define z vtmp.z()
132
133			/! Replaces this with a matrix that represents rotation about the
134			axis by a an angle.
135
136			\warning If the vector axis has length zero, this method will
137			generate the 0-matrix. If the length of the axis is close to
138			zero, but not == 0.0, this method may behave in unexpected ways
139			and return almost random results; details may depend on your
140			particular doubleing point implementation. The use of this method
141			is therefore highly discouraged, unless you are certain that the
142			length is in a reasonable range, away from 0.0 (Stefan
143			Kebekus)
144
145			\deprecated This method will probably replaced by a safer
146			algorithm in the future.
147
148			\todo Replace this method with a more fool-proof version.
149
150			@param v specifies the axis of the rotation
151			@param angle angle in degrees (0..360)
152			*/
153			void matrix3x3::RotAboutAxisByAngle(const vector3 &v,const double angle)
154			{
155			double theta = angle*DEG_TO_RAD;
156			double s = sin(theta);
157			double c = cos(theta);
158			double t = 1 - c;
159
160			vector3 vtmp = v;
161			vtmp.normalize();
162
163			ele[0][0] = txx + c;
164			ele[0][1] = txy + s*z;
165			ele[0][2] = txz - s*y;
166
167			ele[1][0] = txy - s*z;
168			ele[1][1] = tyy + c;
169			ele[1][2] = tyz + s*x;
170
171			ele[2][0] = txz + s*y;
172			ele[2][1] = tyz - s*x;
173			ele[2][2] = tzz + c;
174			}
175
176			#undef x
177			#undef y
178			#undef z
179
180	gezelter	2477	void matrix3x3::SetColumn(int col, const vector3 &v)
181	tim	2440	{
182			if (col > 2)
183			{
184	gezelter	2477	obErrorLog.ThrowError(__FUNCTION__,
185			"The method was called with col > 2.", obError);
186	tim	2440	}
187
188			ele[0][col] = v.x();
189			ele[1][col] = v.y();
190			ele[2][col] = v.z();
191			}
192
193	gezelter	2477	void matrix3x3::SetRow(int row, const vector3 &v)
194	tim	2440	{
195			if (row > 2)
196			{
197	gezelter	2477	obErrorLog.ThrowError(__FUNCTION__,
198			"The method was called with row > 2.", obError);
199	tim	2440	}
200
201			ele[row][0] = v.x();
202			ele[row][1] = v.y();
203			ele[row][2] = v.z();
204			}
205
206	gezelter	2477	vector3 matrix3x3::GetColumn(unsigned int col)
207	tim	2440	{
208			if (col > 2)
209			{
210	gezelter	2477	obErrorLog.ThrowError(__FUNCTION__,
211			"The method was called with col > 2.", obError);
212	tim	2440	}
213
214			return vector3(ele[0][col], ele[1][col], ele[2][col]);
215			}
216
217	gezelter	2477	vector3 matrix3x3::GetRow(unsigned int row)
218	tim	2440	{
219			if (row > 2)
220			{
221	gezelter	2477	obErrorLog.ThrowError(__FUNCTION__,
222			"The method was called with row > 2.", obError);
223	tim	2440	}
224
225			return vector3(ele[row][0], ele[row][1], ele[row][2]);
226			}
227
228			/! calculates the product mv of the matrix m and the column
229			vector represented by v
230			*/
231			vector3 operator *(const matrix3x3 &m,const vector3 &v)
232			{
233			vector3 vv;
234
235			vv._vx = v._vxm.ele[0][0] + v._vym.ele[0][1] + v._vz*m.ele[0][2];
236			vv._vy = v._vxm.ele[1][0] + v._vym.ele[1][1] + v._vz*m.ele[1][2];
237			vv._vz = v._vxm.ele[2][0] + v._vym.ele[2][1] + v._vz*m.ele[2][2];
238
239			return(vv);
240			}
241
242			matrix3x3 operator *(const matrix3x3 &A,const matrix3x3 &B)
243			{
244			matrix3x3 result;
245
246			result.ele[0][0] = A.ele[0][0]B.ele[0][0] + A.ele[0][1]B.ele[1][0] + A.ele[0][2]*B.ele[2][0];
247			result.ele[0][1] = A.ele[0][0]B.ele[0][1] + A.ele[0][1]B.ele[1][1] + A.ele[0][2]*B.ele[2][1];
248			result.ele[0][2] = A.ele[0][0]B.ele[0][2] + A.ele[0][1]B.ele[1][2] + A.ele[0][2]*B.ele[2][2];
249
250			result.ele[1][0] = A.ele[1][0]B.ele[0][0] + A.ele[1][1]B.ele[1][0] + A.ele[1][2]*B.ele[2][0];
251			result.ele[1][1] = A.ele[1][0]B.ele[0][1] + A.ele[1][1]B.ele[1][1] + A.ele[1][2]*B.ele[2][1];
252			result.ele[1][2] = A.ele[1][0]B.ele[0][2] + A.ele[1][1]B.ele[1][2] + A.ele[1][2]*B.ele[2][2];
253
254			result.ele[2][0] = A.ele[2][0]B.ele[0][0] + A.ele[2][1]B.ele[1][0] + A.ele[2][2]*B.ele[2][0];
255			result.ele[2][1] = A.ele[2][0]B.ele[0][1] + A.ele[2][1]B.ele[1][1] + A.ele[2][2]*B.ele[2][1];
256			result.ele[2][2] = A.ele[2][0]B.ele[0][2] + A.ele[2][1]B.ele[1][2] + A.ele[2][2]*B.ele[2][2];
257
258			return(result);
259			}
260
261			/! calculates the product m(*this) of the matrix m and the
262			column vector represented by *this
263			*/
264			vector3 &vector3::operator *= (const matrix3x3 &m)
265			{
266			vector3 vv;
267
268			vv.SetX(_vxm.Get(0,0) + _vym.Get(0,1) + _vz*m.Get(0,2));
269			vv.SetY(_vxm.Get(1,0) + _vym.Get(1,1) + _vz*m.Get(1,2));
270			vv.SetZ(_vxm.Get(2,0) + _vym.Get(2,1) + _vz*m.Get(2,2));
271			_vx = vv.x();
272			_vy = vv.y();
273			_vz = vv.z();
274
275			return(*this);
276			}
277
278			/*! This method checks if the absolute value of the determinant is smaller than 1e-6. If
279			so, nothing is done and an exception is thrown. Otherwise, the
280			inverse matrix is calculated and returned. *this is not changed.
281
282			\warning If the determinant is close to zero, but not == 0.0,
283			this method may behave in unexpected ways and return almost
284			random results; details may depend on your particular doubleing
285			point implementation. The use of this method is therefore highly
286			discouraged, unless you are certain that the determinant is in a
287			reasonable range, away from 0.0 (Stefan Kebekus)
288			*/
289	gezelter	2477	matrix3x3 matrix3x3::inverse(void)
290	tim	2440	{
291			double det = determinant();
292			if (fabs(det) <= 1e-6)
293			{
294	gezelter	2477	obErrorLog.ThrowError(__FUNCTION__,
295			"The method was called on a matrix with \|determinant\| <= 1e-6.", obError);
296	tim	2440	}
297
298			matrix3x3 inverse;
299			inverse.ele[0][0] = ele[1][1]ele[2][2] - ele[1][2]ele[2][1];
300			inverse.ele[1][0] = ele[1][2]ele[2][0] - ele[1][0]ele[2][2];
301			inverse.ele[2][0] = ele[1][0]ele[2][1] - ele[1][1]ele[2][0];
302			inverse.ele[0][1] = ele[2][1]ele[0][2] - ele[2][2]ele[0][1];
303			inverse.ele[1][1] = ele[2][2]ele[0][0] - ele[2][0]ele[0][2];
304			inverse.ele[2][1] = ele[2][0]ele[0][1] - ele[2][1]ele[0][0];
305			inverse.ele[0][2] = ele[0][1]ele[1][2] - ele[0][2]ele[1][1];
306			inverse.ele[1][2] = ele[0][2]ele[1][0] - ele[0][0]ele[1][2];
307			inverse.ele[2][2] = ele[0][0]ele[1][1] - ele[0][1]ele[1][0];
308
309			inverse /= det;
310
311			return(inverse);
312			}
313
314			/* This method returns the transpose of a matrix. The original
315			matrix remains unchanged. */
316			matrix3x3 matrix3x3::transpose(void) const
317			{
318			matrix3x3 transpose;
319
320			for(unsigned int i=0; i<3; i++)
321			for(unsigned int j=0; j<3; j++)
322			transpose.ele[i][j] = ele[j][i];
323
324			return(transpose);
325			}
326
327			double matrix3x3::determinant(void) const
328			{
329			double x,y,z;
330
331			x = ele[0][0] * (ele[1][1] * ele[2][2] - ele[1][2] * ele[2][1]);
332			y = ele[0][1] * (ele[1][2] * ele[2][0] - ele[1][0] * ele[2][2]);
333			z = ele[0][2] * (ele[1][0] * ele[2][1] - ele[1][1] * ele[2][0]);
334
335			return(x + y + z);
336			}
337
338			/*! This method returns false if there are indices i,j such that
339			fabs(this[i][j]-this[j][i]) > 1e-6. Otherwise, it returns
340			true. */
341			bool matrix3x3::isSymmetric(void) const
342			{
343			if (fabs(ele[0][1] - ele[1][0]) > 1e-6)
344			return false;
345			if (fabs(ele[0][2] - ele[2][0]) > 1e-6)
346			return false;
347			if (fabs(ele[1][2] - ele[2][1]) > 1e-6)
348			return false;
349			return true;
350			}
351
352			/*! This method returns false if there are indices i != j such
353			that fabs(this[i][j]) > 1e-6. Otherwise, it returns true. /
354			bool matrix3x3::isDiagonal(void) const
355			{
356			if (fabs(ele[0][1]) > 1e-6)
357			return false;
358			if (fabs(ele[0][2]) > 1e-6)
359			return false;
360			if (fabs(ele[1][2]) > 1e-6)
361			return false;
362
363			if (fabs(ele[1][0]) > 1e-6)
364			return false;
365			if (fabs(ele[2][0]) > 1e-6)
366			return false;
367			if (fabs(ele[2][1]) > 1e-6)
368			return false;
369
370			return true;
371			}
372
373			/*! This method returns false if there are indices i != j such
374			that fabs(*this[i][j]) > 1e-6, or if there is an index i such
375			that fabs(*this[i][j]-1) > 1e-6. Otherwise, it returns
376			true. */
377			bool matrix3x3::isUnitMatrix(void) const
378			{
379			if (!isDiagonal())
380			return false;
381
382			if (fabs(ele[0][0]-1) > 1e-6)
383			return false;
384			if (fabs(ele[1][1]-1) > 1e-6)
385			return false;
386			if (fabs(ele[2][2]-1) > 1e-6)
387			return false;
388
389			return true;
390			}
391
392			/*! This method employs the static method matrix3x3::jacobi(...)
393			to find the eigenvalues and eigenvectors of a symmetric
394			matrix. On entry it is checked if the matrix really is
395			symmetric: if isSymmetric() returns 'false', an OBError is
396			thrown.
397
398			\note The jacobi algorithm is should work great for all
399			symmetric 3x3 matrices. If you need to find the eigenvectors
400			of a non-symmetric matrix, you might want to resort to the
401			sophisticated routines of LAPACK.
402
403			@param eigenvals a reference to a vector3 where the
404			eigenvalues will be stored. The eigenvalues are ordered so
405			that eigenvals[0] <= eigenvals[1] <= eigenvals[2].
406
407			@return an orthogonal matrix whose ith column is an
408			eigenvector for the eigenvalue eigenvals[i]. Here 'orthogonal'
409			means that all eigenvectors have length one and are mutually
410			orthogonal. The ith eigenvector can thus be conveniently
411			accessed by the GetColumn() method, as in the following
412			example.
413			\code
414			// Calculate eigenvectors and -values
415			vector3 eigenvals;
416			matrix3x3 eigenmatrix = somematrix.findEigenvectorsIfSymmetric(eigenvals);
417
418			// Print the 2nd eigenvector
419			cout << eigenmatrix.GetColumn(1) << endl;
420			\endcode
421			With these conventions, a matrix is diagonalized in the following way:
422			\code
423			// Diagonalize the matrix
424			matrix3x3 diagonalMatrix = eigenmatrix.inverse() * somematrix * eigenmatrix;
425			\endcode
426
427			*/
428	gezelter	2477	matrix3x3 matrix3x3::findEigenvectorsIfSymmetric(vector3 &eigenvals)
429	tim	2440	{
430			matrix3x3 result;
431
432			if (!isSymmetric())
433			{
434	gezelter	2477	obErrorLog.ThrowError(__FUNCTION__,
435			"The method was called on a matrix that was not symmetric, i.e. where isSymetric() == false.", obError);
436	tim	2440	}
437
438			double d[3];
439			matrix3x3 copyOfThis = *this;
440
441			jacobi(3, copyOfThis.ele[0], d, result.ele[0]);
442			eigenvals.Set(d);
443
444			return result;
445			}
446
447			matrix3x3 &matrix3x3::operator/=(const double &c)
448			{
449			for (int row = 0;row < 3; row++)
450			for (int col = 0;col < 3; col++)
451			ele[row][col] /= c;
452
453			return(*this);
454			}
455
456			#ifndef SQUARE
457			#define SQUARE(x) ((x)*(x))
458			#endif
459
460			void matrix3x3::FillOrth(double Alpha,double Beta, double Gamma,
461			double A, double B, double C)
462			{
463			double V;
464
465			Alpha *= DEG_TO_RAD;
466			Beta *= DEG_TO_RAD;
467			Gamma *= DEG_TO_RAD;
468
469			// from the PDB specification:
470			// http://www.rcsb.org/pdb/docs/format/pdbguide2.2/part_75.html
471
472
473			// since we'll ultimately divide by (a * b), we've factored those out here
474			V = C * sqrt(1 - SQUARE(cos(Alpha)) - SQUARE(cos(Beta)) - SQUARE(cos(Gamma))
475			+ 2 * cos(Alpha) * cos(Beta) * cos(Gamma));
476
477			ele[0][0] = A;
478			ele[0][1] = B*cos(Gamma);
479			ele[0][2] = C*cos(Beta);
480
481			ele[1][0] = 0.0;
482			ele[1][1] = B*sin(Gamma);
483			ele[1][2] = C(cos(Alpha)-cos(Beta)cos(Gamma))/sin(Gamma);
484
485			ele[2][0] = 0.0;
486			ele[2][1] = 0.0;
487			ele[2][2] = V / (sin(Gamma)); // again, we factored out A * B when defining V
488			}
489
490			ostream& operator<< ( ostream& co, const matrix3x3& m )
491
492			{
493			co << "[ "
494			<< m.ele[0][0]
495			<< ", "
496			<< m.ele[0][1]
497			<< ", "
498			<< m.ele[0][2]
499			<< " ]" << endl;
500
501			co << "[ "
502			<< m.ele[1][0]
503			<< ", "
504			<< m.ele[1][1]
505			<< ", "
506			<< m.ele[1][2]
507			<< " ]" << endl;
508
509			co << "[ "
510			<< m.ele[2][0]
511			<< ", "
512			<< m.ele[2][1]
513			<< ", "
514			<< m.ele[2][2]
515			<< " ]" << endl;
516
517			return co ;
518			}
519
520			/*! This static function computes the eigenvalues and
521			eigenvectors of a SYMMETRIC nxn matrix. This method is used
522			internally by OpenBabel, but may be useful as a general
523			eigenvalue finder.
524
525			The algorithm uses Jacobi transformations. It is described
526			e.g. in Wilkinson, Reinsch "Handbook for automatic computation,
527			Volume II: Linear Algebra", part II, contribution II/1. The
528			implementation is also similar to the implementation in this
529			book. This method is adequate to solve the eigenproblem for
530			small matrices, of size perhaps up to 10x10. For bigger
531			problems, you might want to resort to the sophisticated routines
532			of LAPACK.
533
534			\note If you plan to find the eigenvalues of a symmetric 3x3
535			matrix, you will probably prefer to use the more convenient
536			method findEigenvectorsIfSymmetric()
537
538			@param n the size of the matrix that should be diagonalized
539
540			@param a array of size n^2 which holds the symmetric matrix
541			whose eigenvectors are to be computed. The convention is that
542			the entry in row r and column c is addressed as a[n*r+c] where,
543			of course, 0 <= r < n and 0 <= c < n. There is no check that the
544			matrix is actually symmetric. If it is not, the behaviour of
545			this function is undefined. On return, the matrix is
546			overwritten with junk.
547
548			@param d pointer to a field of at least n doubles which will be
549			overwritten. On return of this function, the entries d[0]..d[n-1]
550			will contain the eigenvalues of the matrix.
551
552			@param v an array of size n^2 where the eigenvectors will be
553			stored. On return, the columns of this matrix will contain the
554			eigenvectors. The eigenvectors are normalized and mutually
555			orthogonal.
556			*/
557			void matrix3x3::jacobi(unsigned int n, double a, double d, double *v)
558			{
559			double onorm, dnorm;
560			double b, dma, q, t, c, s;
561			double atemp, vtemp, dtemp;
562			register int i, j, k, l;
563			int nrot;
564
565			int MAX_SWEEPS=50;
566
567			// Set v to the identity matrix, set the vector d to contain the
568			// diagonal elements of the matrix a
569			for (j = 0; j < static_cast<int>(n); j++)
570			{
571			for (i = 0; i < static_cast<int>(n); i++)
572			v[n*i+j] = 0.0;
573			v[n*j+j] = 1.0;
574			d[j] = a[n*j+j];
575			}
576
577			nrot = MAX_SWEEPS;
578			for (l = 1; l <= nrot; l++)
579			{
580			// Set dnorm to be the maximum norm of the diagonal elements, set
581			// onorm to the maximum norm of the off-diagonal elements
582			dnorm = 0.0;
583			onorm = 0.0;
584			for (j = 0; j < static_cast<int>(n); j++)
585			{
586			dnorm += (double)fabs(d[j]);
587			for (i = 0; i < j; i++)
588			onorm += (double)fabs(a[n*i+j]);
589			}
590			// Normal end point of this algorithm.
591			if((onorm/dnorm) <= 1.0e-12)
592			goto Exit_now;
593
594			for (j = 1; j < static_cast<int>(n); j++)
595			{
596			for (i = 0; i <= j - 1; i++)
597			{
598
599			b = a[n*i+j];
600			if(fabs(b) > 0.0)
601			{
602			dma = d[j] - d[i];
603			if((fabs(dma) + fabs(b)) <= fabs(dma))
604			t = b / dma;
605			else
606			{
607			q = 0.5 * dma / b;
608			t = 1.0/((double)fabs(q) + (double)sqrt(1.0+q*q));
609			if (q < 0.0)
610			t = -t;
611			}
612
613			c = 1.0/(double)sqrt(t*t + 1.0);
614			s = t * c;
615			a[n*i+j] = 0.0;
616
617			for (k = 0; k <= i-1; k++)
618			{
619			atemp = c * a[nk+i] - s a[n*k+j];
620			a[nk+j] = s a[nk+i] + c a[n*k+j];
621			a[n*k+i] = atemp;
622			}
623
624			for (k = i+1; k <= j-1; k++)
625			{
626			atemp = c * a[ni+k] - s a[n*k+j];
627			a[nk+j] = s a[ni+k] + c a[n*k+j];
628			a[n*i+k] = atemp;
629			}
630
631			for (k = j+1; k < static_cast<int>(n); k++)
632			{
633			atemp = c * a[ni+k] - s a[n*j+k];
634			a[nj+k] = s a[ni+k] + c a[n*j+k];
635			a[n*i+k] = atemp;
636			}
637
638			for (k = 0; k < static_cast<int>(n); k++)
639			{
640			vtemp = c * v[nk+i] - s v[n*k+j];
641			v[nk+j] = s v[nk+i] + c v[n*k+j];
642			v[n*k+i] = vtemp;
643			}
644
645			dtemp = ccd[i] + ssd[j] - 2.0cs*b;
646			d[j] = ssd[i] + ccd[j] + 2.0cs*b;
647			d[i] = dtemp;
648			} /* end if */
649			} /* end for i */
650			} /* end for j */
651			} /* end for l */
652
653			Exit_now:
654
655			// Now sort the eigenvalues (and the eigenvectors) so that the
656			// smallest eigenvalues come first.
657			nrot = l;
658
659			for (j = 0; j < static_cast<int>(n)-1; j++)
660			{
661			k = j;
662			dtemp = d[k];
663			for (i = j+1; i < static_cast<int>(n); i++)
664			if(d[i] < dtemp)
665			{
666			k = i;
667			dtemp = d[k];
668			}
669
670			if(k > j)
671			{
672			d[k] = d[j];
673			d[j] = dtemp;
674			for (i = 0; i < static_cast<int>(n); i++)
675			{
676			dtemp = v[n*i+k];
677			v[ni+k] = v[ni+j];
678			v[n*i+j] = dtemp;
679			}
680			}
681			}
682			}
683
684			} // end namespace OpenBabel
685
686			//! \file matrix3x3.cpp
687			//! \brief Handle 3D rotation matrix.
688