Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Representations/Cognition/GTCamWorldState.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GTCamWorldState.cpp
    00003 *
    00004 * Implementation of the GTCamWorldState.
    00005 *
    00006 * @author Matthias Meyer
    00007 */
    00008
    00009 #include "GTCamWorldState.h"
    00010 #include "Platform/GTAssert.h"
    00011
    00012 //#include "Platform/UDPEndpoint.h"
    00013 #include "Tools/Player.h"
    00014 #include <Tools/debugging/debugging.h>
    00015 #include "Representations/Cognition/GTCamWorldState.h"
    00016 //#include "NetConfig.h"
    00017 #include "Tools/TRingBufferWithSum.h"
    00018 #include <stdio.h>
    00019 #include <float.h>
    00020 //#include <iostream.h>
    00021
    00022 /**
    00023 * Contructor
    00024 */
    00025 GTCamWorldState::GTCamWorldState(bool myFlip) {
    00026   setFlip(myFlip);
    00027   setTimestamp(0.0);
    00028   for (int i = 0; i < SIZEOFWORLDSTATE; i++) {
    00029     camWorldstate[i] = 0;
    00030   }
    00031 }
    00032
    00033 void GTCamWorldState::setFlip(bool myFlip) {
    00034 //  INFO(sendOptionRatings,idText,text,"Flipped State");
    00035   if (myFlip) {
    00036     flip = -1;
    00037   } else {
    00038     flip = 1;
    00039   }
    00040 }
    00041
    00042 GTCamWorldState::~GTCamWorldState() {
    00043 }
    00044
    00045
    00046 /**
    00047 * Get player position from worldstate
    00048 */
    00049 Vector2<double> GTCamWorldState::getPlayerPos(int player,int color) const {
    00050   Vector2<double> pos;
    00051   if (playerWasSeen(player, color))
    00052   {
    00053     if (color == Player::blue) {
    00054       player += 4;
    00055     }
    00056     pos.x = camWorldstate[(((player )* 5) + 6) + 0] * flip;
    00057     pos.y = camWorldstate[(((player )* 5) + 6) + 1] * flip;
    00058   }
    00059   else
    00060   {
    00061     pos.x = 0.0;
    00062     pos.y = 0.0;
    00063   }
    00064   return pos;
    00065 }
    00066
    00067 /**
    00068 * Get player speed from worldstate
    00069 */
    00070 Vector2<double> GTCamWorldState::getPlayerSpeed(int player,int color) const {
    00071   Vector2<double> speed;
    00072   if (playerWasSeen(player, color))
    00073   {
    00074     if (color == Player::blue) {
    00075       player += 4;
    00076     }
    00077     speed.x = camWorldstate[(((player )* 5) + 6) + 3] * flip;
    00078     speed.y = camWorldstate[(((player )* 5) + 6) + 4] * flip;
    00079   }
    00080   else
    00081   {
    00082     speed.x = 0.0;
    00083     speed.y = 0.0;
    00084   }
    00085   return speed;
    00086 }
    00087
    00088 /**
    00089 * Get player orientation from worldstate
    00090 */
    00091 double GTCamWorldState::getPlayerOrientation(int player,int color) const {
    00092   if (playerWasSeen(player, color))
    00093   {
    00094     if (color == Player::blue) {
    00095       player += 4;
    00096     }
    00097     //  return camWorldstate[(((player )* 5) + 6) + 2] * flip;
    00098
    00099     if (flip == 1) {
    00100       // Red dog (no flip needed)
    00101       return camWorldstate[(((player )* 5) + 6) + 2];
    00102     } else {
    00103       // Blue dog (need flipping)
    00104       if (camWorldstate[(((player )* 5) + 6) + 2] > 0) {
    00105         return camWorldstate[(((player )* 5) + 6) + 2] - pi;
    00106       } else {
    00107         return camWorldstate[(((player )* 5) + 6) + 2] + pi;
    00108       }
    00109     }
    00110   }
    00111   return 0.0;
    00112 }
    00113
    00114 /**
    00115 * Get timestamp from worldstate
    00116 */
    00117 double GTCamWorldState::getTimestamp() const {
    00118   if (!is_NAN(camWorldstate[0]))
    00119   {
    00120     return camWorldstate[0];
    00121   }
    00122   else
    00123   {
    00124     return 0.0;
    00125   }
    00126 }
    00127
    00128 /**
    00129 * Set player position in worldstate
    00130 */
    00131 void GTCamWorldState::setPlayerPos(Vector2<double> pos,int player,int color) {
    00132   if (color == Player::blue) {
    00133     player += 4;
    00134   }
    00135   camWorldstate[(((player )* 5) + 6) + 0] = pos.x;
    00136   camWorldstate[(((player )* 5) + 6) + 1] = pos.y;
    00137 }
    00138
    00139 /**
    00140 * set player speed in worldstate
    00141 */
    00142 void GTCamWorldState::setPlayerSpeed(Vector2<double> speed,int player,int color) {
    00143   if (color == Player::blue) {
    00144     player += 4;
    00145   }
    00146   camWorldstate[(((player )* 5) + 6) + 3] = speed.x;
    00147   camWorldstate[(((player )* 5) + 6) + 4] = speed.y;
    00148 }
    00149
    00150 /**
    00151 * set player orientation in worldstate
    00152 */
    00153 void GTCamWorldState::setPlayerOrientation(double angle, int player = 0,int color) {
    00154   if (color == Player::blue) {
    00155     player += 4;
    00156   }
    00157   camWorldstate[(((player )* 5) + 6) + 2] = angle;
    00158 }
    00159
    00160 /**
    00161 * set timestamp in worldstate
    00162 */
    00163 void GTCamWorldState::setTimestamp(double stamp) {
    00164   camWorldstate[0] = stamp;
    00165 }
    00166
    00167
    00168 /**
    00169 * Set Sync Timestamp
    00170 */
    00171 void GTCamWorldState::setSyncTimestamp(double stamp) {
    00172   camWorldstate[1] = stamp;
    00173 }
    00174
    00175
    00176 /**
    00177 * Return the sync timestamp of the worldstate
    00178 */
    00179 double GTCamWorldState::getSyncTimestamp() const {
    00180   if (!is_NAN(camWorldstate[1]))
    00181   {
    00182     return camWorldstate[1];
    00183   }
    00184   else
    00185   {
    00186     return 0.0;
    00187   }
    00188 }
    00189
    00190 /**
    00191 * get ballModel from worldstate
    00192 */
    00193 BallModel GTCamWorldState::getBallModel() const {
    00194   BallModel ball;
    00195
    00196   if (!is_NAN(camWorldstate[2]))
    00197   {
    00198     RobotPose poseDummy;
    00199     ball.seen.setBallDataRelativeToField(
    00200       poseDummy,
    00201       camWorldstate[2]*flip, camWorldstate[3]*flip,
    00202       camWorldstate[4]*flip, camWorldstate[5]*flip);
    00203     ball.ballWasSeen = true;
    00204   }
    00205   else
    00206   {
    00207     ball.ballWasSeen = false;
    00208   }
    00209
    00210   /*
    00211   ball.seen.x = camWorldstate[2] * flip;
    00212   ball.seen.y = camWorldstate[3]  * flip;
    00213   ball.seen.speed.x = camWorldstate[4] * flip;
    00214   ball.seen.speed.y = camWorldstate[5] * flip;
    00215   */
    00216 //  OUTPUT(idText,text,"Flipstate: " << flip);
    00217   //cout << "============================\n";
    00218   //cout << "FlipState: " << flip << "\n";
    00219   //cout << "Ball.x : " << ball.seen.x << "\n";
    00220   //cout << "Ball.y : " << ball.seen.y << "\n";
    00221   return ball;
    00222 }
    00223
    00224
    00225 /**
    00226 * Return robot pose of player
    00227 */
    00228 RobotPose GTCamWorldState::getRobotPose(int player, int color) const
    00229 {
    00230   RobotPose pose;
    00231   if (playerWasSeen(player, color))
    00232   {
    00233     pose.translation = getPlayerPos(player,color);
    00234     pose.rotation = getPlayerOrientation(player,color);
    00235     pose.setValidity(1.0);
    00236   }
    00237   else
    00238   {
    00239     pose.translation.x = 0.0;
    00240     pose.translation.y = 0.0;
    00241     pose.rotation = 0.0;
    00242     pose.setValidity(0.0);
    00243   }
    00244   return pose;
    00245 }
    00246
    00247 RobotPose GTCamWorldState::getRobotPose() const
    00248 {
    00249   return getRobotPose(getPlayer().getPlayerNumber(), getPlayer().getTeamColor());
    00250 }
    00251
    00252 /** Does not work for now */
    00253 PlayerPoseCollection GTCamWorldState::getPlayerPoseCollection(int player, int color) {
    00254   PlayerPoseCollection c;
    00255   PlayerPose p;
    00256
    00257   return c;
    00258 }
    00259
    00260 /**
    00261 * Set the ball position
    00262 */
    00263 void GTCamWorldState::setBallPos(double X, double Y)
    00264 {
    00265   camWorldstate[2] = X;
    00266   camWorldstate[3] = X;
    00267 }
    00268
    00269 /**
    00270 * Set the X ball speed
    00271 */
    00272 void GTCamWorldState::setBallSpeedX(double speedX) {
    00273   camWorldstate[4] = speedX;
    00274 }
    00275
    00276 /**
    00277 * Set the Y Ball speed
    00278 */
    00279 void GTCamWorldState::setBallSpeedY(double speedY) {
    00280   camWorldstate[5] = speedY;
    00281 }
    00282
    00283 /**
    00284 * Set the Ball speed
    00285 */
    00286 void GTCamWorldState::setBallSpeed(double speedX, double speedY) {
    00287   setBallSpeedX(speedX);
    00288   setBallSpeedY(speedY);
    00289 }
    00290
    00291
    00292 unsigned long GTCamWorldState::getFrameNumber() const
    00293 {
    00294   return frameNumber;
    00295 }
    00296
    00297 /**
    00298 * Was the player seen?
    00299 */
    00300 bool GTCamWorldState::playerWasSeen(int player, int color) const
    00301 {
    00302   if (color == Player::blue) {
    00303     player += 4;
    00304   }
    00305 /*  double test=(camWorldstate[player*5 + 6]);
    00306
    00307   bool result1 = !is_NAN_d(test);
    00308   bool result2 = !is_NAN_d(test);
    00309
    00310   unsigned long* pTest;
    00311   pTest = (unsigned long*) &(camWorldstate[player*5 + 6]);
    00312   unsigned long nan[2]={0xffffffff, 0x7fffffff};
    00313
    00314   bool result = !( ((pTest[0] - nan[0])==0) && ((pTest[1] - nan[1])==0) );
    00315
    00316 //#ifdef APERIOS1_3_2
    00317   OUTPUT (idText, text, "NaNso: " << nan[0] << ", " << nan[1]);
    00318   OUTPUT (idText, text, "NaNis: " << pTest[0] << ", " << pTest[1] << ", " << result1 << ", " << result2 << ", " << result);
    00319 //#endif
    00320 */
    00321   return !is_NAN(camWorldstate[player*5 + 6]);
    00322 }
    00323
    00324 /**
    00325 * Is test NaN?
    00326 */
    00327 bool GTCamWorldState::is_NAN(double test) const
    00328 {
    00329 #ifdef _WIN32
    00330   return (_isnan(test)!=0);
    00331 #else
    00332   return (isnan(test)!=0);
    00333 #endif
    00334
    00335 /*  return (test!=test);
    00336   unsigned long* pTest;
    00337   pTest = (unsigned long*) &test;
    00338   unsigned long nan[2]={0xffffffff, 0x7fffffff};
    00339
    00340   bool result = ( ((pTest[0] - nan[0])==0) && ((pTest[1] - nan[1])==0) );
    00341   return result;
    00342 //  return ( (pTest[0] == nan[0]) && (pTest[1] == nan[1]) );*/
    00343 }
    00344
    00345 const int GTCamWorldState::SIZEOFWORLDSTATE = 46;
    00346
    00347 /**
    00348 * Do the streaming
    00349 */
    00350 In& operator>>(In& stream,GTCamWorldState& state) {
    00351   //  stream >> state.camWorldstate;
    00352 //  stream.read(&state.camWorldstate,sizeof(state.camWorldstate));
    00353   stream.read(&state,sizeof(state));
    00354   return stream;
    00355 }
    00356
    00357 Out& operator<<(Out& stream, const GTCamWorldState& state) {
    00358   //  stream << state.camWorldstate;
    00359 //  stream.write(&state.camWorldstate,sizeof(state.camWorldstate));
    00360   stream.write(&state,sizeof(state));
    00361   return stream;
    00362 }