Robotsystem from Robot Group Leipzig: oderobot.cpp Source File

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

- File List
- File Members

# oderobot.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  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  ***************************************************************************
    00022  *                                                                         *
    00023  *   $Log: oderobot.cpp,v $
    00024  *   Revision 1.1.2.3  2005/12/14 15:37:09  martius
    00025  *   robots are working with osg
    00026  *
    00027  *   Revision 1.1.2.2  2005/12/13 18:11:40  martius
    00028  *   still trying to port robots
    00029  *
    00030  *   Revision 1.1.2.1  2005/12/13 12:31:09  martius
    00031  *   moved to cpp file
    00032  *
    00033  *   Revision 1.1.2.1  2005/12/06 17:38:21  martius
    00034  *   *** empty log message ***
    00035  *
    00036  *                                                                 *
    00037  ***************************************************************************/
    00038
    00039 #include "oderobot.h"
    00040 #include "primitive.h"
    00041
    00042 namespace lpzrobots {
    00043
    00044   /**
    00045    * Constructor
    00046    * @param odehandle structure with all global ODE variables
    00047    */
    00048   OdeRobot::OdeRobot(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const char* name)
    00049     : AbstractRobot(name), odeHandle(odeHandle), osgHandle(osgHandle) {
    00050     parentspace = odeHandle.space;
    00051   };
    00052
    00053   OdeRobot::~OdeRobot(){}
    00054
    00055   /** sets color of the robot
    00056       @param col Color struct with desired Color
    00057    */
    00058   void OdeRobot::setColor(const Color& col){
    00059     osgHandle.color = col;
    00060   };
    00061
    00062   /** sets the vehicle to position pos
    00063       @params pos desired position of the robot
    00064   */
    00065   void OdeRobot::place(const Pos& pos){
    00066     place(osg::Matrix::translate(pos));
    00067   }
    00068
    00069   /** returns position of the object
    00070       @return vector of position (x,y,z)
    00071   */
    00072   Position OdeRobot::getPosition() const {
    00073     const Primitive* o = getMainPrimitive();
    00074     if (o && o->getBody()){
    00075       return Position(dBodyGetPosition(o->getBody()));
    00076     } else return Position(0,0,0);
    00077   }
    00078
    00079   /** returns linear speed vector of the object
    00080       @return vector  (vx,vy,vz)
    00081   */
    00082   Position OdeRobot::getSpeed() const {
    00083     const Primitive* o = getMainPrimitive();
    00084     if (o && o->getBody()){
    00085       return Position(dBodyGetLinearVel(o->getBody()));
    00086     } else return Position(0,0,0);
    00087   }
    00088
    00089   /** returns the orientation of the object
    00090       @return 3x3 rotation matrix
    00091   */
    00092   matrix::Matrix OdeRobot::getOrientation() const {
    00093     const Primitive* o = getMainPrimitive();
    00094     if (o && o->getBody()){
    00095       return odeRto3x3RotationMatrix(dBodyGetRotation(o->getBody()));
    00096     } else {
    00097       matrix::Matrix R(3,3);
    00098       return R^0; // identity
    00099     }
    00100   }
    00101
    00102
    00103   bool OdeRobot::isGeomInPrimitiveList(Primitive** ps, int len, dGeomID geom){
    00104     for(int i=0; i < len; i++){
    00105       if(geom == ps[i]->getGeom()) return true;
    00106     }
    00107     return false;
    00108   }
    00109
    00110   bool OdeRobot::isGeomInPrimitiveList(list<Primitive*> ps, dGeomID geom){
    00111     for(list<Primitive*>::iterator i=ps.begin(); i != ps.end(); i++){
    00112       if(geom == (*i)->getGeom()) return true;
    00113     }
    00114     return false;
    00115   }
    00116
    00117   matrix::Matrix OdeRobot::odeRto3x3RotationMatrixT ( const double R[12] ) {
    00118     matrix::Matrix matrix(3,3);
    00119     matrix.val(0,0)=R[0];
    00120     matrix.val(0,1)=R[4];
    00121     matrix.val(0,2)=R[8];
    00122     matrix.val(1,0)=R[1];
    00123     matrix.val(1,1)=R[5];
    00124     matrix.val(1,2)=R[9];
    00125     matrix.val(2,0)=R[2];
    00126     matrix.val(2,1)=R[6];
    00127     matrix.val(2,2)=R[10];
    00128     return matrix;
    00129   }
    00130
    00131   matrix::Matrix OdeRobot::odeRto3x3RotationMatrix ( const double R[12] ) {
    00132     matrix::Matrix matrix(3,3);
    00133     matrix.val(0,0)=R[0];
    00134     matrix.val(1,0)=R[4];
    00135     matrix.val(2,0)=R[8];
    00136     matrix.val(0,1)=R[1];
    00137     matrix.val(1,1)=R[5];
    00138     matrix.val(2,1)=R[9];
    00139     matrix.val(0,2)=R[2];
    00140     matrix.val(1,2)=R[6];
    00141     matrix.val(2,2)=R[10];
    00142     return matrix;
    00143   }
    00144
    00145 }