Robotsystem from Robot Group Leipzig: camera.cpp Source File

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

- File List
- File Members

# camera.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: camera.cpp,v $
    00024  *   Revision 1.14.4.2  2005/12/06 10:13:26  martius
    00025  *   openscenegraph integration started
    00026  *
    00027  *   Revision 1.14.4.1  2005/11/14 17:37:24  martius
    00028  *   moved to selforg
    00029  *
    00030  *   Revision 1.14  2005/11/14 12:47:36  martius
    00031  *   removed printfs
    00032  *
    00033  *   Revision 1.13  2005/09/27 14:00:29  robot3
    00034  *   printf removed
    00035  *
    00036  *   Revision 1.12  2005/09/27 12:22:52  robot3
    00037  *   now changing camView by mouse movement really works :)
    00038  *
    00039  *   Revision 1.11  2005/09/27 12:11:16  robot3
    00040  *   fixed bug not be able to change the camView through mouse movement
    00041  *
    00042  *   Revision 1.10  2005/09/27 10:49:20  robot3
    00043  *   camera module rewritten, new features:
    00044  *   -everytime there is a smooth move and view
    00045  *   -some hacks are replaced through stable code
    00046  *
    00047  *   Revision 1.9  2005/09/20 10:55:14  robot3
    00048  *   camera module:
    00049  *   -pressing key c now centers on focused robot
    00050  *   -pressing key b now moves 5.0f behind the robot
    00051  *   -fixed a few bugs (nullpointer crashes etc.)
    00052  *
    00053  *   Revision 1.8  2005/09/02 17:20:18  martius
    00054  *   advancedTV disabled
    00055  *
    00056  *   Revision 1.7  2005/08/23 11:41:20  robot1
    00057  *   advancedFollowing mode included
    00058  *
    00059  *   Revision 1.6  2005/08/22 12:38:32  robot1
    00060  *   -advancedTV mode implemented, early version
    00061  *   -internal code optimized
    00062  *   -printMode prints now the current camera mode on stdout
    00063  *
    00064  *   Revision 1.5  2005/08/16 10:06:48  robot1
    00065  *   TV mode with horizontal centering implemented.
    00066  *
    00067  *   Revision 1.4  2005/08/12 11:56:46  robot1
    00068  *   tiny bugfixing
    00069  *
    00070  *   Revision 1.3  2005/08/09 11:08:49  robot1
    00071  *   following mode included
    00072  *
    00073  *
    00074  *   Revision 1.1  2005/08/08 11:06:46  martius
    00075  *   camera is a module for camera movements
    00076  *   includes cleaned
    00077  *
    00078  *                                                                         *
    00079  *                                                                         *
    00080  ***************************************************************************/
    00081
    00082 #include "camera.h"
    00083 #include <stdio.h>
    00084
    00085 namespace lpzrobots {
    00086
    00087 // #include <drawstuff/drawstuff.h>
    00088 // #include "ode/ode.h"
    00089
    00090 // CameraType oldCamType;
    00091 // const OdeRobot* oldRobot;
    00092
    00093 // // all desired values that have to be in the end state
    00094 // float desiredCamPos[3];
    00095 // float desiredCamView[3];
    00096
    00097 // // for camera and robot: current (read and calculated) values
    00098 // float currentCamPos[3];
    00099 // float currentCamView[3];
    00100 // double currentRobotPos[3];
    00101 // double currentRobotView[3];
    00102
    00103 // // for camera and robot: old values from one timestep before
    00104 // float oldCamPos[3];
    00105 // float oldCamView[3];
    00106 // double oldRobotPos[3];
    00107 // double oldRobotView[3];
    00108
    00109
    00110 // // for advanced TV mode
    00111 // float maxAllowedDistance=10.0f; // enabled advanvedTV ;)
    00112 // float robCamDistance;
    00113
    00114
    00115
    00116 // // this is a bit faster than the compiler definition:
    00117 // // #define SQUARE(x) (x)*(x)
    00118 // // x is only calculated once, not twice
    00119 // double SQUARE(double x) {
    00120 //      return (x*x);
    00121
    00122 // }
    00123
    00124 // // I didn't found the math library so fast, sorry.
    00125 // float ABS(float x) {
    00126 //   if (x<0)
    00127 //     return (x*(-1));
    00128 //   else
    00129 //     return x;
    00130 // }
    00131
    00132 // void followRobotsMove() {
    00133 //      // now adjusting the desired position of the camera
    00134 //      for (int i=0;i<=2;i++) {
    00135 //              desiredCamPos[i]+=currentRobotPos[i]-oldRobotPos[i];
    00136 //      }
    00137 // }
    00138
    00139 // // changes only the desiredCamView
    00140 // void centerViewOnRobot() {
    00141 //      // no change for z
    00142 //      if ((currentRobotPos[1]-desiredCamPos[1])!=0) { // division by zero
    00143 //              // calculate the horizontal angle
    00144 //              desiredCamView[0]=-atan((currentRobotPos[0]-desiredCamPos[0])/
    00145 //                                      (currentRobotPos[1]-desiredCamPos[1]))
    00146 //                                                      /M_PI*180.0f+270.0f;
    00147 //              if (desiredCamPos[1]-currentRobotPos[1]<0)
    00148 //                      desiredCamView[0]+=180.0f; // we must switch
    00149 //      }
    00150 //      if ((currentRobotPos[2]-desiredCamPos[2])!=0) { // division by zero
    00151 //              // calculate the vertical angle
    00152 //              // we need dz and sqrt(dx^2+dy^2) for calculation
    00153 //              desiredCamView[1]=
    00154 //                      atan(
    00155 //                              (sqrt(
    00156 //                              SQUARE(desiredCamPos[0]-currentRobotPos[0]) +
    00157 //                              SQUARE(desiredCamPos[1]-currentRobotPos[1])))
    00158 //                                      /(currentCamPos[2]-currentRobotPos[2]))
    00159 //                                      /M_PI*180.0f+270.0f; // TODO: normalize
    00160 //      }
    00161 //      // z-angle (rotation) is not adjusted
    00162 // }
    00163
    00164
    00165
    00166 // void getRobotPosAndView(double *pos, double *view,OdeRobot& robot) {
    00167 //      Position position=robot.getPosition();
    00168 //      pos[0]=position.x;
    00169 //      pos[1]=position.y;
    00170 //      pos[2]=position.z;
    00171 //      // now the view
    00172 //      for (int i=0;i<=2;i++) {
    00173 //              view[i]=pos[i]-oldRobotPos[i];
    00174 //         }
    00175 //      // if no view can be detected, use old view!
    00176 //      if (view[0]==0 && view[1]==0) {
    00177 //              for (int i=0;i<=2;i++) {
    00178 //                      view[i]=oldRobotView[i];
    00179 //              }
    00180 //      }
    00181 // }
    00182
    00183 // // prints the current (used) mode on stdout
    00184 // void printMode(CameraType camType) {
    00185 //      char* type;
    00186 //      printf("Current Mode is now: ");
    00187 //      switch (camType) {
    00188 //              case Static:
    00189 //                      type="Static";
    00190 //                      break;
    00191 //              case TV:
    00192 //                      type="TV";
    00193 //                      break;
    00194 //              case advancedTV:
    00195 //                      type="advancedTV";
    00196 //                      break;
    00197 //              case Following:
    00198 //                      type="Following";
    00199 //                      break;
    00200 //              case advancedFollowing:
    00201 //                      type="advancedFollowing";
    00202 //                      break;
    00203 //              default:
    00204 //                      type="unknown";
    00205 //      }
    00206 //      printf("%s\n",type);
    00207 // }
    00208
    00209 // // has to be called if CameraType or the Robot has changed.
    00210 // void initCamera(CameraType camType,OdeRobot& robot) {
    00211 //      // getting first the position and view of robot
    00212 //      getRobotPosAndView(oldRobotPos, oldRobotView, robot);
    00213 //      // now getting the current angle of the camera
    00214 //      dsGetViewpoint(oldCamPos,oldCamView);
    00215 //      if (oldCamType!=camType)
    00216 //              printMode(camType);
    00217 //      if (oldRobot!=&robot) // Robot is changed
    00218 //              // setting current cam values to desired values
    00219 //              for (int i=0;i<=2;i++) {
    00220 //                      desiredCamPos[i]=oldCamPos[i];
    00221 //                      desiredCamView[i]=oldCamView[i];
    00222 //              }
    00223 //      // setting the new Values
    00224 //      oldCamType=camType;
    00225 //      oldRobot=&robot;
    00226 // }
    00227
    00228 // void StaticMode(OdeRobot& robot) {
    00229 //      // do nothing
    00230 // }
    00231
    00232 // void TVMode(OdeRobot& robot) {
    00233 //      centerViewOnRobot();
    00234 // }
    00235
    00236 // void advancedTVMode(OdeRobot& robot) {
    00237 //      // check wether the camera is too far away
    00238 //      robCamDistance= (sqrt(
    00239 //                      SQUARE(currentCamPos[0]-currentRobotPos[0]) +
    00240 //                      SQUARE(currentCamPos[1]-currentRobotPos[1])));
    00241 //      if (robCamDistance>maxAllowedDistance) {
    00242 //              // then the camera must get a new position, centering on position of robot
    00243 //              desiredCamPos[0]=currentRobotPos[0];
    00244 //              desiredCamPos[1]=currentRobotPos[1];
    00245 //              desiredCamPos[2]=currentRobotPos[2]+2.0f; // must be higher than robPos...
    00246 //              //              printf("I am too far away from maximum distance(%f): %f\n",maxAllowedDistance,robCamDistance);
    00247 //      }
    00248 //      // now do the rest of TV
    00249 //      centerViewOnRobot();
    00250 // }
    00251
    00252 // void FollowingMode(OdeRobot& robot) {
    00253 //      followRobotsMove();
    00254 // }
    00255
    00256 // void print3DimFloat(float vec[3]) {
    00257 //      for (int i=0;i<=2;i++) {
    00258 //              printf("\t%f",vec[i]);
    00259 //      }
    00260 // }
    00261
    00262 // void moveOnRobot(OdeRobot& robot) {
    00263 //   //printf("moveOnRobot is now called!\n");
    00264 //   // now getting the current angle of the camera
    00265 //      for (int i=0;i<=1;i++) {
    00266 //              desiredCamPos[i]=currentRobotPos[i];
    00267 //      }
    00268 //      desiredCamPos[2]=currentRobotPos[2]+2.0f;
    00269 //      // center the view on the robot
    00270 //      centerViewOnRobot();
    00271 // }
    00272
    00273 // void moveBehindRobot(OdeRobot& robot) {
    00274 //   //printf("moveBehindRobot is now called!\n");
    00275 //      robCamDistance=4.0f; // the distance of camera position
    00276 //      // now get normalizing scalar of currentRobotView, only x and y is needed
    00277 //      float n=sqrt(SQUARE(currentRobotView[0])+SQUARE(currentRobotView[1]));
    00278 //      if (n>0) {
    00279 //              for (int i=0;i<=1;i++)
    00280 //                desiredCamPos[i]=currentRobotPos[i]-currentRobotView[i]/n*robCamDistance;
    00281 //              centerViewOnRobot();
    00282 //      } else { // we think this is the best
    00283 //              moveOnRobot(robot);
    00284 //      }
    00285 // }
    00286
    00287
    00288 // void advancedFollowingMode(OdeRobot& robot) {
    00289 //      float n;
    00290 //      // first calculate current distance and set the desired position behind the robot
    00291 //      robCamDistance= (sqrt(
    00292 //                      SQUARE(currentCamPos[0]-currentRobotPos[0]) +
    00293 //                      SQUARE(currentCamPos[1]-currentRobotPos[1])));
    00294 //      // the direction of the robot is stored in currentRobotView!
    00295 //      // now get normalizing scalar of currentRobotView, only x and y is needed
    00296 //      n=sqrt(SQUARE(currentRobotView[0])+SQUARE(currentRobotView[1]));
    00297 //      if (n>0) { // else no calculation can be made, then do not change the camPos
    00298 //              for (int i=0;i<=1;i++) {
    00299 //                      // now set the camPos to robotPos-robCamDistance*robotView
    00300 //                      // this is an approximation, normally a rotation is needed
    00301 //                      // store it in newCamPos, so TV mode can be called unmodified
    00302 //                      desiredCamPos[i]=currentRobotPos[i]
    00303 //                        -currentRobotView[i]*robCamDistance/n+currentRobotView[i];
    00304 //              }
    00305 //      }
    00306 //      // now do centering the view on robot
    00307 //      centerViewOnRobot();
    00308 // }
    00309
    00310
    00311 // void adjustPosByMouseMove() {
    00312 //      for (int i=0;i<=2;i++) {
    00313 //        desiredCamPos[i]+=currentCamPos[i]-oldCamPos[i];
    00314 //        //      if (oldCamPos[i]-currentCamPos[i]>0)
    00315 //        // desiredCamPos[i]=currentCamPos[i];
    00316 //      }
    00317 // }
    00318
    00319 // void adjustViewByMouseMove() {
    00320 //      for (int i=0;i<=2;i++) {
    00321 //              desiredCamView[i]-=oldCamView[i]-currentCamView[i];
    00322 //      }
    00323 // }
    00324
    00325 // void storeOldValues(float *setCamPos, float *setCamView) {
    00326 //      for (int i=0;i<=2;i++) {
    00327 //              oldRobotPos[i]=currentRobotPos[i];
    00328 //              oldRobotView[i]=currentRobotView[i];
    00329 //              oldCamPos[i]=setCamPos[i];
    00330 //              oldCamView[i]=setCamView[i];
    00331 //      }
    00332 // }
    00333
    00334
    00335 // // wrapper function
    00336 // void doubleTofloatA(const double *doubleArray,float *floatArray) {
    00337 //   for (int i=0;i<=2;i++) {
    00338 //     floatArray[i]=doubleArray[i];
    00339 //   }
    00340 // }
    00341
    00342 // // wrapper function
    00343 // void floatTodoubleA(const float *floatArray,double *doubleArray) {
    00344 //   for (int i=0;i<=2;i++) {
    00345 //     doubleArray[i]=floatArray[i];
    00346 //   }
    00347 // }
    00348
    00349
    00350 // Position smoothMove() {
    00351 //      double newPos[3];
    00352 //      for (int i=0;i<=2;i++) {
    00353 //        if (ABS(desiredCamPos[i]-currentCamPos[i])<0.1f) {
    00354 //          newPos[i]=desiredCamPos[i];
    00355 //        } else
    00356 //          newPos[i]=desiredCamPos[i]*0.1f+currentCamPos[i]*0.9f;
    00357 //      }
    00358 //      return Position(newPos);
    00359 // }
    00360
    00361 // // normalizes the angle
    00362 // float normalize360(float angle) {
    00363 //   float result=angle;
    00364 //     while (result>=360.0f)
    00365 //       result-=360.0f;
    00366 //     while (result<0.0f)
    00367 //       result+=360.0f;
    00368 //     return result;
    00369 // }
    00370
    00371 // // normalizes the angle
    00372 // double normalize360(double angle) {
    00373 //   double result=angle;
    00374 //     while (result>=360.0f)
    00375 //       result-=360.0f;
    00376 //     while (result<0.0f)
    00377 //       result+=360.0f;
    00378 //     return result;
    00379 // }
    00380
    00381 // // normalizes the angle of all 3 components
    00382 // void normalize360vec3(float *angle) {
    00383 //   for (int i=0;i<=2;i++) {
    00384 //     angle[i]=normalize360(angle[i]);
    00385 //   }
    00386 // }
    00387
    00388 // // normalizes the angle of all 3 components
    00389 // void normalize360vec3(double *angle) {
    00390 //   for (int i=0;i<=2;i++) {
    00391 //     angle[i]=normalize360(angle[i]);
    00392 //   }
    00393 // }
    00394
    00395
    00396 // Position smoothView() {
    00397 //      double newView[3];
    00398 //      normalize360vec3(desiredCamView);
    00399 //      normalize360vec3(currentCamView);
    00400 //      for (int i=0;i<=2;i++) {
    00401 //        if (ABS(desiredCamView[i]-currentCamView[i])<0.2f) {
    00402 //          newView[i]=desiredCamView[i];
    00403 //        } else if (ABS(desiredCamView[i]-currentCamView[i])>180.0f) {
    00404 //          // we must first transform the angles into linear gradient space (+180 degrees)
    00405 //          // then make a backtransformation (-180 degrees)
    00406 //          newView[i]=normalize360(desiredCamView[i]+180.0f)*0.1f
    00407 //            +normalize360(currentCamView[i]+180.0f)*0.9f-180.0f;
    00408 //        normalize360(newView[i]);
    00409 //        } else { // use normal sliding
    00410 //          newView[i]=desiredCamView[i]*0.1f+currentCamView[i]*0.9f;
    00411 //        }
    00412 //      }
    00413 //      return Position(newView);
    00414 // }
    00415
    00416
    00417 // void moveCamera( CameraType camType,OdeRobot& robot) {
    00418 //      // first look if someone is changed
    00419 //      // only otherwise change the camera position and/or view.
    00420 //      if (oldCamType!=camType || &robot!=oldRobot)
    00421 //              initCamera(camType,robot);
    00422 //      else {
    00423 //              // first get all needed values
    00424 //              // getting first the position and view of robot
    00425 //              getRobotPosAndView(currentRobotPos,currentRobotView, robot);
    00426 //              // now getting the current position and angle of the camera
    00427 //              dsGetViewpoint(currentCamPos,currentCamView);
    00428 //              // now compute
    00429 //              // calculate the adjusted new settings from mouse movement
    00430 //              adjustPosByMouseMove();
    00431 //              adjustViewByMouseMove();
    00432 //              // now adjust the desired values through the camera modes
    00433 //              switch (camType) {
    00434 //                      case Static:
    00435 //                              StaticMode(robot);
    00436 //                              break;
    00437 //                      case advancedTV:
    00438 //                              advancedTVMode(robot);
    00439 //                              break;
    00440 //                      case TV:
    00441 //                              TVMode(robot);
    00442 //                              break;
    00443 //                      case Following:
    00444 //                              FollowingMode(robot);
    00445 //                              break;
    00446 //                      case advancedFollowing:
    00447 //                              advancedFollowingMode(robot);
    00448 //                              break;
    00449 //              }
    00450 //              // now execute :)
    00451 //              float move[3];
    00452 //              float view[3];
    00453 //              doubleTofloatA(smoothMove().toArray(),move);
    00454 //              doubleTofloatA(smoothView().toArray(),view);
    00455 //              dsSetViewpoint(move,view);
    00456 //              // now store the current (set) values to old values
    00457 //              storeOldValues(move,view);
    00458 //      }
    00459 // }
    00460
    00461 }