Robotsystem from Robot Group Leipzig: cameramanipulator.cpp Source File

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

- File List
- File Members

# cameramanipulator.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  *                                                                         *
    00024  *                                                                         *
    00025  *   $Log: cameramanipulator.cpp,v $
    00026  *   Revision 1.1.2.14  2006/03/28 09:55:12  robot3
    00027  *   -main: fixed snake explosion bug
    00028  *   -odeconfig.h: inserted cameraspeed
    00029  *   -camermanipulator.cpp: fixed setbyMatrix,
    00030  *    updateFactor
    00031  *
    00032  *   Revision 1.1.2.13  2006/03/19 13:32:48  robot3
    00033  *   race mode now works
    00034  *
    00035  *   Revision 1.1.2.12  2006/03/18 12:03:25  robot3
    00036  *   some prints removed
    00037  *
    00038  *   Revision 1.1.2.11  2006/03/08 13:19:13  robot3
    00039  *   basic modifications, follow mode now works
    00040  *
    00041  *   Revision 1.1.2.10  2006/03/06 16:56:44  robot3
    00042  *   -more stable version
    00043  *   -code optimized
    00044  *   -some static variables used by all cameramanipulators
    00045  *
    00046  *   Revision 1.1.2.9  2006/03/05 15:01:57  robot3
    00047  *   camera moves now smooth
    00048  *
    00049  *   Revision 1.1.2.8  2006/03/04 15:04:33  robot3
    00050  *   cameramanipulator is now updated with every draw intervall
    00051  *
    00052  *   Revision 1.1.2.7  2006/03/03 12:08:50  robot3
    00053  *   preparations made for new cameramanipulators
    00054  *
    00055  *   Revision 1.1.2.6  2006/02/01 10:24:34  robot3
    00056  *   new camera manipulator added
    00057  *
    00058  *   Revision 1.1.2.5  2006/01/30 13:12:45  martius
    00059  *   bug in setByMatrix
    00060  *
    00061  *   Revision 1.1.2.4  2005/12/29 12:55:59  martius
    00062  *   setHome
    00063  *
    00064  *   Revision 1.1.2.3  2005/12/15 17:03:42  martius
    00065  *   cameramanupulator setPose is working
    00066  *   joints have setter and getter parameters
    00067  *   Primitives are not longer inherited from OSGPrimitive, moreover
    00068  *   they aggregate them
    00069  *
    00070  *   Revision 1.1.2.2  2005/12/11 23:35:08  martius
    00071  *   *** empty log message ***
    00072  *
    00073  *   Revision 1.1.2.1  2005/12/09 16:56:21  martius
    00074  *   camera is working now
    00075  *
    00076  *   Revision 1.1.2.1  2005/12/06 17:38:21  martius
    00077  *   *** empty log message ***
    00078  *
    00079  *                                                                 *
    00080  ***************************************************************************/
    00081
    00082 #include <osg/Notify>
    00083 #include "cameramanipulator.h"
    00084 #include "mathutils.h"
    00085 #include "pos.h"
    00086
    00087 #include <vector>
    00088 #include <iterator>
    00089
    00090 namespace lpzrobots {
    00091
    00092   using namespace osg;
    00093   using namespace osgGA;
    00094
    00095
    00096   osg::Vec3 CameraManipulator::eye(0,0,0);
    00097   osg::Vec3 CameraManipulator::view(0,0,0);
    00098   osg::Vec3 CameraManipulator::home_eye;
    00099   osg::Vec3 CameraManipulator::home_view;
    00100   osg::Vec3 CameraManipulator::desiredEye;
    00101   osg::Vec3 CameraManipulator::desiredView;
    00102   bool CameraManipulator::home_externally_set=false;
    00103   OdeAgent* CameraManipulator::watchingAgent;
    00104   bool CameraManipulator::watchingAgentDefined=false;
    00105   Position CameraManipulator::oldPositionOfAgent;
    00106   bool CameraManipulator::oldPositionOfAgentDefined=false;
    00107
    00108   // globalData braucht er für alles
    00109   CameraManipulator::CameraManipulator(osg::Node* node, GlobalData& global)
    00110     : node(node), globalData(global) {
    00111     if (this->node.get()) {
    00112       const BoundingSphere& boundingSphere=this->node->getBound();
    00113       modelScale = boundingSphere._radius;
    00114     }else
    00115       modelScale = 0.01f;
    00116     desiredEye=eye;
    00117     desiredView=view;
    00118     // default values for smoothness (function update())
    00119     // can be owerwritten by new cameramanipulator if needed
    00120     degreeSmoothness=0.03;
    00121     lengthSmoothness=0.03;
    00122     lengthAccuracy=0.02;
    00123     degreeAccuracy=0.03;
    00124   }
    00125
    00126   CameraManipulator::~CameraManipulator(){
    00127   }
    00128
    00129   void CameraManipulator::setNode(Node* node){
    00130     // we do not support this since we give it manually to the constructor
    00131   }
    00132   const Node* CameraManipulator::getNode() const{
    00133     return node.get();
    00134   }
    00135
    00136   Node* CameraManipulator::getNode(){
    00137     return node.get();
    00138   }
    00139
    00140   /// set the home position of the camera. (and place it there)
    00141   void CameraManipulator::setHome(const osg::Vec3& _eye, const osg::Vec3& _view){
    00142     home_eye = _eye;
    00143     home_view = _view;
    00144     home_externally_set=true;
    00145     eye  = home_eye;
    00146     view = home_view;
    00147     desiredEye=_eye;
    00148     desiredView=_view;
    00149     computeMatrix(); // i think we don't need this???
    00150   }
    00151
    00152
    00153   void CameraManipulator::home(const GUIEventAdapter& ea,GUIActionAdapter& us){
    00154     if(node.get() && !home_externally_set) {
    00155       const BoundingSphere& boundingSphere=node->getBound();
    00156
    00157       home_eye = boundingSphere._center+
    00158         Vec3(-boundingSphere._radius*1.2f,0, boundingSphere._radius*0.2f);
    00159
    00160       home_view = Vec3(-90,-10,0);
    00161     }
    00162     //   eye  = home_eye;
    00163     //    view = home_view;
    00164     desiredEye=home_eye;
    00165     desiredView=home_view;
    00166     computeMatrix();
    00167
    00168     us.requestRedraw();
    00169
    00170     us.requestWarpPointer((ea.getXmin()+ea.getXmax())/2.0f,(ea.getYmin()+ea.getYmax())/2.0f);
    00171
    00172     flushMouseEventStack();
    00173
    00174   }
    00175
    00176   void CameraManipulator::init(const GUIEventAdapter& ea,GUIActionAdapter& us){
    00177     flushMouseEventStack();
    00178
    00179     us.requestContinuousUpdate(false);
    00180
    00181     if (ea.getEventType()!=GUIEventAdapter::RESIZE)
    00182       {
    00183         us.requestWarpPointer((ea.getXmin()+ea.getXmax())/2.0f,(ea.getYmin()+ea.getYmax())/2.0f);
    00184       }
    00185   }
    00186
    00187   bool CameraManipulator::handle(const GUIEventAdapter& ea,GUIActionAdapter& us){
    00188     int key=0;
    00189     switch(ea.getEventType())
    00190       {
    00191       case(GUIEventAdapter::PUSH):
    00192         {
    00193           flushMouseEventStack();
    00194           return true;
    00195         }
    00196
    00197       case(GUIEventAdapter::RELEASE):
    00198         {
    00199           flushMouseEventStack();
    00200           return true;
    00201         }
    00202
    00203       case(GUIEventAdapter::DRAG):
    00204         {
    00205           addMouseEvent(ea);
    00206           us.requestContinuousUpdate(true);
    00207           if (calcMovement()) us.requestRedraw();
    00208           return true;
    00209         }
    00210
    00211       case(GUIEventAdapter::KEYDOWN):
    00212         key=ea.getKey();
    00213         // F-keys (F1 to F12)
    00214         if ((65470<=key)&&(key<=65481)) {
    00215           manageAgents(key-65469);
    00216           return true; // was handled
    00217         }
    00218         switch(key) {
    00219         case ' ':
    00220           {
    00221             flushMouseEventStack();
    00222             home(ea,us);
    00223             us.requestRedraw();
    00224             us.requestContinuousUpdate(false);
    00225             break;
    00226           }
    00227         case 'p':
    00228           {
    00229             printf("Camera Position/View: (Pos(%g, %g, %g), ", eye.x(), eye.y(), eye.z());
    00230             printf(" Pos(%g, %g, %g));\n", view.x(), view.y(), view.z());
    00231             break;
    00232           }
    00233           // TODO: pos1 for center on robot
    00234           // TODO: end for move behind robot
    00235         default:
    00236           return false;
    00237         }
    00238       case(GUIEventAdapter::RESIZE):
    00239         init(ea,us);
    00240         us.requestRedraw();
    00241         return true;
    00242
    00243       default:
    00244         return false;
    00245       }
    00246     return true;
    00247   }
    00248
    00249   void CameraManipulator::getUsage(ApplicationUsage& usage) const{
    00250     usage.addKeyboardMouseBinding("Camera: Space","Reset the viewing position to home");
    00251     usage.addKeyboardMouseBinding("Camera: p","Print position of the camera");
    00252     usage.addKeyboardMouseBinding("Camera: F1-F12","switch the Agent to be watched");
    00253   }
    00254
    00255   void CameraManipulator::flushMouseEventStack(){
    00256     event_old = NULL;
    00257     event = NULL;
    00258   }
    00259   void CameraManipulator::addMouseEvent(const GUIEventAdapter& ea){
    00260     event_old = event;
    00261     event = &ea;
    00262   }
    00263
    00264   /** normally called only when this manipulator is choosed
    00265    */
    00266   void CameraManipulator::setByMatrix(const Matrixd& matrix){
    00267
    00268     eye = matrix.getTrans();
    00269     Vec3 xaxis(1,0,0);
    00270     Pos head = Matrix::transform3x3(xaxis, matrix);
    00271     view.x() = RadiansToDegrees(getAngle(xaxis, head)) *
    00272       sign(head.y()); // this resolves the ambiguity of getAngle
    00273
    00274     Pos tilt = Matrix::transform3x3(Vec3(0,0,1), matrix);
    00275     //    head.print();
    00276     //    tilt.print();
    00277     std::cout << "Manipulator choosed: " <<  className() << std::endl;
    00278     view.y() = RadiansToDegrees(getAngle(Vec3(0,0,1), tilt)-M_PI/2);
    00279     desiredEye=eye;
    00280     desiredView=view;
    00281     computeMatrix();
    00282   }
    00283
    00284
    00285
    00286   // hier reinhängen nicht, wird nur beim switch des manipulators aufgerufen
    00287   Matrixd CameraManipulator::getMatrix() const {
    00288     return pose;
    00289   }
    00290
    00291   void CameraManipulator::update() {
    00292     // the call from simulation.cpp works, but is made for ALL cameramanipulators!
    00293     // which is now neccessary for the smoothness for the mouse interactions
    00294
    00295     // modify the desiredView and desiredEye by the movement of the agent
    00296     calcMovementByAgent();
    00297
    00298     // now do smoothness
    00299     float updateFactor;
    00300     updateFactor = globalData.odeConfig.drawInterval * globalData.odeConfig.simStepSize * globalData.odeConfig.cameraSpeed;
    00301       //    std::cout << "drawInt: " << globalData.odeConfig.drawInterval << ", realtimefactor: "
    00302       //      << globalData.odeConfig.realTimeFactor << ", updateFactor: "
    00303       //      << updateFactor << "\n";
    00304     for (int i=0;i<=2;i++) {
    00305       // view is in °, we must be careful for switches at the 360°-point
    00306       if ((desiredView[i]-view[i])>180) // desiredView is to high
    00307         view[i]+=360;
    00308       else if ((view[i]-desiredView[i])>180) // view is to high
    00309         desiredView[i]+=360;
    00310       if (abs(desiredView[i]-view[i])>degreeAccuracy)
    00311         view[i]= normalize360(degreeSmoothness * updateFactor * desiredView[i] +
    00312                               (1.0 - degreeSmoothness * updateFactor) * view[i]);
    00313       if (abs(desiredEye[i]-eye[i])>lengthAccuracy)
    00314         eye[i]= lengthSmoothness * updateFactor * desiredEye[i]
    00315           + (1.0 - lengthSmoothness * updateFactor) * eye[i];
    00316     }
    00317
    00318     // now set the current robots-position
    00319     if (watchingAgentDefined) {
    00320       oldPositionOfAgent = watchingAgent->getRobot()->getPosition();
    00321       oldPositionOfAgentDefined=true;
    00322     }
    00323     computeMatrix();
    00324   }
    00325
    00326
    00327
    00328   // hier reinhaengen?? is called every drawstep!!!! really? ;)
    00329   // should we call a CameraManipulator-routine from simulation.cpp?
    00330   Matrixd CameraManipulator::getInverseMatrix() const {
    00331     return Matrixd::inverse(pose);
    00332   }
    00333
    00334   void CameraManipulator::computeMatrix(){
    00335     for (int i=0; i<3; i++) {
    00336       while (view[i] > 180) view[i] -= 360;
    00337       while (view[i] < -180) view[i] += 360;
    00338     }
    00339     osg::Matrix rot;
    00340     rot.makeRotate( M_PI/2,                          osg::Vec3(1, 0, 0),
    00341                     osg::DegreesToRadians(view.x()), osg::Vec3(0, 0, 1), // heading
    00342                     osg::DegreesToRadians(view.y()), osg::Vec3(cos(osg::DegreesToRadians(view.x())),
    00343                                                                sin(osg::DegreesToRadians(view.x())),
    00344                                                                0) // pitch
    00345                     );
    00346
    00347     pose = rot * Matrix::translate(eye);
    00348   }
    00349
    00350   bool CameraManipulator::calcMovement(){
    00351     //    std::cout << "i calc mouse movement!" << std::endl;
    00352   // _camera->setFusionDistanceMode(Camera::PROPORTIONAL_TO_SCREEN_DISTANCE);
    00353
    00354     // return if less then two events have been added.
    00355     if (event.get()==NULL || event_old.get()==NULL) return false;
    00356
    00357     //    double dt = event->time()-event_old->time();
    00358     double dx = 10.0*(event->getXnormalized() - event_old->getXnormalized());
    00359     double dy = 10.0*(event->getYnormalized() - event_old->getYnormalized());
    00360     double s =  sin(osg::DegreesToRadians(view.x()));
    00361     double c =  cos(osg::DegreesToRadians(view.x()));
    00362
    00363     unsigned int buttonMask = event_old->getButtonMask();
    00364     if (buttonMask==GUIEventAdapter::LEFT_MOUSE_BUTTON) {
    00365       desiredView.x() += dx*3.0f;
    00366       desiredView.y() -= dy*3.0f;
    00367     } else if (buttonMask==GUIEventAdapter::MIDDLE_MOUSE_BUTTON ||
    00368                buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON | GUIEventAdapter::RIGHT_MOUSE_BUTTON)) {
    00369       desiredEye.z() += -dy;
    00370       desiredEye.x() += - c*dx;
    00371       desiredEye.y() += - s*dx;
    00372     } else if (buttonMask==GUIEventAdapter::RIGHT_MOUSE_BUTTON) {
    00373       desiredEye.x() +=  s*dy - c*dx;
    00374       desiredEye.y() +=  -c*dy - s*dx;
    00375     } else return false;
    00376     return true;
    00377   }
    00378
    00379
    00380
    00381   void CameraManipulator::manageAgents(const int& fkey) {
    00382     //std::cout << "new robot to choose: " << fkey << "\n";
    00383     watchingAgentDefined=false;
    00384     oldPositionOfAgentDefined=false;
    00385     int i=1;
    00386     // go through the agent list
    00387     for(OdeAgentList::iterator it=globalData.agents.begin(); it != globalData.agents.end(); it++){
    00388       if (fkey==i++) {
    00389         watchingAgent=(*it);
    00390         watchingAgentDefined=true;
    00391         break;
    00392       }
    00393     }
    00394     if (!watchingAgentDefined)
    00395       std::cout << "no agent was choosed!\n";
    00396     else {
    00397       std::cout << "the agent was choosed: " << i-1 << "\n";
    00398       setHomeViewByAgent();
    00399       setHomeEyeByAgent();
    00400     }
    00401   }
    00402
    00403   void CameraManipulator::calcMovementByAgent() {
    00404     if (watchingAgentDefined && oldPositionOfAgentDefined) {
    00405       // then manipulate desired view and desired eye
    00406       // the default camera manipulator does not need to change the eye and view
    00407     }
    00408   }
    00409
    00410   void CameraManipulator::setHomeViewByAgent() {
    00411     // the default camera manipulator does not need to change the view
    00412     // normally the desired view should be changed
    00413   }
    00414
    00415   void CameraManipulator::setHomeEyeByAgent() {
    00416     // the default camera manipulator does not need to change the eye
    00417     // normally the desired eye should be changed
    00418   }
    00419
    00420 }
    00421