Robotsystem from Robot Group Leipzig: odeagent.cpp Source File

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

- File List
- File Members

# odeagent.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  *   $Log: odeagent.cpp,v $
    00023  *   Revision 1.1.2.3  2006/03/31 16:16:58  fhesse
    00024  *   changed trace() to init_tracing()
    00025  *   and check for init at beginning of step
    00026  *
    00027  *   Revision 1.1.2.2  2006/03/30 12:32:46  fhesse
    00028  *   trace via trackrobot
    00029  *
    00030  *   Revision 1.1.2.1  2006/03/28 14:14:44  fhesse
    00031  *   tracing of a given primitive (in the osg window) added
    00032  *                                                *
    00033  *                                                                         *
    00034  *                                                                         *
    00035  ***************************************************************************/
    00036
    00037 #include "odeagent.h"
    00038 #include "oderobot.h"
    00039 #include "pos.h"
    00040
    00041 namespace lpzrobots {
    00042
    00043
    00044   void OdeAgent::init_tracing(int tracelength/*=10*/,double tracethickness/*=0.003*/){
    00045     trace_length=tracelength;
    00046     trace_thickness=tracethickness;
    00047
    00048     segments = (OSGPrimitive**) malloc(sizeof(OSGPrimitive*) * trace_length);
    00049     for (int i=0; i<trace_length; i++){
    00050       segments[i]=0;
    00051     }
    00052     // init lastpos with position of body_to_follow
    00053     Pos pos(robot->getPosition());
    00054     lastpos=pos;
    00055
    00056     counter=0;
    00057
    00058     tracing_initialized=true;
    00059   }
    00060
    00061
    00062
    00063   void OdeAgent::step(double noise){
    00064     Agent::step(noise);
    00065     // todo: do this (trackrobot.trace()) with friend class OdeAgent or the like
    00066     // to be able to directly use trackrobot.tracePos
    00067     if (trackrobot.trace()){
    00068       if (!tracing_initialized) {
    00069         init_tracing();
    00070       }
    00071       Pos pos(robot->getPosition());
    00072      /* if construct used to draw cylinder only when length between actual
    00073         and last point is larger then a specific value
    00074      */
    00075      //if(counter==0 || ((pos - lastpos).length2() > 0.00005)  ) {
    00076       double len = (pos - lastpos).length();
    00077       if(segments[counter%trace_length]) delete segments[counter%trace_length];
    00078       OSGPrimitive* s = new OSGCylinder(trace_thickness, len);
    00079 //       OsgHandle osgHandle_white=((OdeRobot*)robot)->osgHandle;
    00080 //       osgHandle_white.changeColor(Color(255, 255, 255));
    00081 //       s->init(osgHandle_white, OSGPrimitive::Low);
    00082       s->init(((OdeRobot*)robot)->osgHandle, OSGPrimitive::Low);
    00083       s->setMatrix(osg::Matrix::rotate(osg::Vec3(0,0,1), (pos - lastpos)) *
    00084                    osg::Matrix::translate(pos+(pos - lastpos)/2));
    00085       segments[counter%trace_length] = s;
    00086       lastpos = pos;
    00087       counter++;
    00088      //}
    00089     }
    00090   }
    00091 }
    00092
    00093