Robotsystem from Robot Group Leipzig: mathutils.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# mathutils.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *    frankguettler@gmx.de                                                 *
    00007  *                                                                         *
    00008  *   This program is free software; you can redistribute it and/or modify  *
    00009  *   it under the terms of the GNU General Public License as published by  *
    00010  *   the Free Software Foundation; either version 2 of the License, or     *
    00011  *   (at your option) any later version.                                   *
    00012  *                                                                         *
    00013  *   This program is distributed in the hope that it will be useful,       *
    00014  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00015  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00016  *   GNU General Public License for more details.                          *
    00017  *                                                                         *
    00018  *   You should have received a copy of the GNU General Public License     *
    00019  *   along with this program; if not, write to the                         *
    00020  *   Free Software Foundation, Inc.,                                       *
    00021  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00022  *                                                                         *
    00023  *   $Log: mathutils.cpp,v $
    00024  *   Revision 1.2.4.6  2006/03/30 12:35:07  martius
    00025  *   documentation updated
    00026  *
    00027  *   Revision 1.2.4.5  2006/03/29 15:10:11  martius
    00028  *   osgMatrix2matrixlib
    00029  *
    00030  *   Revision 1.2.4.4  2006/02/07 15:49:03  martius
    00031  *   axis
    00032  *
    00033  *   Revision 1.2.4.3  2005/12/15 17:04:32  martius
    00034  *   getAngle
    00035  *   min, max and so on are template functions now
    00036  *
    00037  *   Revision 1.2.4.2  2005/12/14 15:37:38  martius
    00038  *   rotation matrix for axis
    00039  *
    00040  *   Revision 1.2.4.1  2005/11/24 16:21:45  fhesse
    00041  *   multMatrixPosition added
    00042  *
    00043  *   Revision 1.2  2005/10/27 14:16:11  martius
    00044  *   some bugs fixed, module now works
    00045  *   some functions from controller_misc.h are here now
    00046  *
    00047  *   Revision 1.1  2005/10/27 12:15:22  robot3
    00048  *   several useful functions that provide mathematic operations
    00049  *
    00050  *                                                                         *
    00051  ***************************************************************************/
    00052 #include <math.h>
    00053 #include <osg/Matrix>
    00054 #include "mathutils.h"
    00055 #include "axis.h"
    00056
    00057 using namespace matrix;
    00058
    00059 namespace lpzrobots {
    00060
    00061   /*
    00062      converts osg matrix to matrix of matrixlib
    00063    */
    00064   Matrix osgMatrix2Matrixlib(const osg::Matrix& m){
    00065     Matrix m2(4,4);
    00066     for(int i=0; i<4; i++){
    00067       for(int j=0; j<4; j++){
    00068         m2.val(i,j) = m(i,j);
    00069       }
    00070     }
    00071     return m2;
    00072   }
    00073
    00074
    00075   /*
    00076      returns a Rotation matrix that rotates the x-axis along with the given axis.
    00077      The other 2 axis (y,z) are ambiguous.
    00078   */
    00079   osg::Matrix rotationMatrixFromAxisX(const Axis& axis){
    00080     return osg::Matrix::rotate(osg::Vec3(1,0,0), axis.vec3());
    00081   }
    00082
    00083   /*
    00084      returns a Rotation matrix that rotates the z-axis along with the given axis.
    00085      The other 2 axis (x,y) are ambiguous.
    00086   */
    00087   osg::Matrix rotationMatrixFromAxisZ(const Axis& axis){
    00088     return osg::Matrix::rotate(osg::Vec3(0,0,1), axis.vec3());
    00089   }
    00090
    00091
    00092   /*
    00093    * returns the angle between two vectors (in rad)
    00094    */
    00095   double getAngle(const osg::Vec3& a, const osg::Vec3& b) {
    00096     // Cosinus Satz
    00097     // here a*b is the dot product (Skalarprodukt)
    00098     return acos(a*b / (a.length()*b.length()));
    00099   }
    00100
    00101
    00102   /******************************************************************************/
    00103
    00104
    00105   Position multMatrixPosition(const Matrix& r, Position& p){
    00106     assert(r.getM()==3 && r.getN()==3);
    00107     Matrix pm(3,1,p.toArray());
    00108     Matrix rv = r*pm;
    00109     return Position(rv.val(0,0), rv.val(1,0), rv.val(2,0));
    00110   }
    00111
    00112   /*
    00113    * returns a rotation matrix for a rotation in x, y plane about the given angle
    00114    */
    00115   Matrix getRotationMatrix(const double& angle) {
    00116     double data[16]={cos(angle),sin(angle),0,0,
    00117                      -sin(angle),cos(angle),0,0,
    00118                      0,0,1,0,
    00119                      0,0,0,1};
    00120     return Matrix(4,4,data);
    00121   }
    00122
    00123   /*
    00124    * returns a translation matrix with the given Position
    00125    */
    00126   Matrix getTranslationMatrix(const Position& p) {
    00127     double data[16]={0,0,0,p.x,
    00128                      0,0,0,p.y,
    00129                      0,0,1,p.z,
    00130                      0,0,0,1};
    00131     return Matrix(4,4,data);
    00132   }
    00133
    00134   /*
    00135    * removes the translation in the matrix
    00136    */
    00137   Matrix removeTranslationInMatrix(const Matrix& pose){
    00138     Matrix t(pose);
    00139     // remove the three last values of the column 3
    00140     t.val(0,3)=0.0f;
    00141     t.val(1,3)=0.0f;
    00142     t.val(2,3)=0.0f;
    00143     return t;
    00144   }
    00145
    00146   /*
    00147    * removes the rotation in the matrix
    00148    */
    00149   Matrix removeRotationInMatrix(const Matrix& pose){
    00150     Matrix t(pose);
    00151     t.val(0,0)=1.0f; t.val(0,1)=0.0f;
    00152     t.val(1,0)=0.0f; t.val(1,1)=1.0f;
    00153     return t;
    00154   }
    00155
    00156   /*
    00157    * returns the angle between two vectors
    00158    */
    00159   double getAngle(Position a, Position b) {
    00160     Matrix p(3,1,a.toArray()); // row wise
    00161     Matrix q(1,3,b.toArray()); // column wise
    00162     return acos((p * q).val(0,0) / (a.length()*b.length()) );
    00163   }
    00164
    00165 }