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

# Representations/Cognition/RobotPose.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file RobotPose.cpp
    00003 *
    00004 * contains the implementation of the class RobotPose
    00005 */
    00006
    00007 #include "RobotPose.h"
    00008
    00009 double RobotPose::getPositionStandardDeviation() const
    00010 {
    00011   return sqrt(positionVariance);
    00012 }
    00013
    00014 void RobotPose::calculateMovement(const Pose2D & movementPerSec, double time, RobotPose & result) const
    00015 {
    00016   Vector2<double> speedVector(movementPerSec.translation.x,movementPerSec.translation.y);
    00017   double speed = speedVector.abs();
    00018   double diffBetweenOrientationAndSpeedVectorDirection = atan2(speedVector.y,speedVector.x);
    00019   result = *this;
    00020   result.rotation += diffBetweenOrientationAndSpeedVectorDirection;
    00021   Pose2D movement; // the transition will be calculated in a system alligned with the speed vector
    00022   if (movementPerSec.rotation == 0.0)
    00023   {
    00024     movement.rotation = 0;
    00025     movement.translation.x = speed * time;
    00026     movement.translation.y = 0;
    00027   }
    00028   else
    00029   {
    00030     movement.rotation = movementPerSec.rotation * time;
    00031     movement.translation.x = speed/movementPerSec.rotation * sin(movement.rotation);
    00032     movement.translation.y = speed/movementPerSec.rotation * (1.0 - cos(movement.rotation));
    00033   }
    00034   result += movement; // this automatically rotates the movement translation vector into the right orientation and so on...
    00035   result.rotation -= diffBetweenOrientationAndSpeedVectorDirection;
    00036 }
    00037
    00038 In& operator>>(In& stream,RobotPose& robotPose)
    00039 {
    00040
    00041   STREAM_REGISTER_BEGIN_EXT(robotPose);
    00042   STREAM_BASE_EXT( stream, (Pose2D&) robotPose);
    00043   STREAM_EXT(stream, robotPose.validity);
    00044   STREAM_EXT(stream, robotPose.positionVariance);
    00045   STREAM_EXT(stream, robotPose.frameNumber);
    00046   STREAM_EXT(stream, robotPose.timestamp);
    00047   STREAM_EXT(stream, robotPose.speedbyDistanceToGoal);
    00048   STREAM_EXT(stream, robotPose.directionOfGreatestUncertaintyExists);
    00049   STREAM_EXT(stream, robotPose.greatestUncertainty);
    00050   STREAM_REGISTER_FINISH();
    00051 /*
    00052   stream.read(&robotPose,sizeof(RobotPose));
    00053 */
    00054   return stream;
    00055 }
    00056
    00057 Out& operator<<(Out& stream, const RobotPose& robotPose)
    00058 {
    00059   STREAM_REGISTER_BEGIN_EXT(robotPose);
    00060   STREAM_BASE_EXT( stream, (Pose2D&) robotPose);
    00061   STREAM_EXT(stream, robotPose.validity);
    00062   STREAM_EXT(stream, robotPose.positionVariance);
    00063   STREAM_EXT(stream, robotPose.frameNumber);
    00064   STREAM_EXT(stream, robotPose.timestamp);
    00065   STREAM_EXT(stream, robotPose.speedbyDistanceToGoal);
    00066   STREAM_EXT(stream, robotPose.directionOfGreatestUncertaintyExists);
    00067   STREAM_EXT(stream, robotPose.greatestUncertainty);
    00068   STREAM_REGISTER_FINISH();
    00069 /*
    00070   stream.write(&robotPose,sizeof(RobotPose));
    00071 */
    00072   return stream;
    00073 }