Go to the documentation of this file.00001 #include "movement.h"
00002 #include <gmapping/utils/gvalues.h>
00003
00004 namespace GMapping {
00005
00006
00007 FSRMovement::FSRMovement(double f, double s, double r) {
00008 this->f = f;
00009 this->s = s;
00010 this->r = r;
00011 }
00012
00013 FSRMovement::FSRMovement(const FSRMovement& src) {
00014 *this = src;
00015 }
00016
00017 FSRMovement::FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2) {
00018 *this = moveBetweenPoints(pt1, pt2);
00019 }
00020
00021
00022 FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) {
00023 *this = composeMoves(move1, move2);
00024 }
00025
00026 void FSRMovement::normalize()
00027 {
00028 if (r >= -M_PI && r < M_PI)
00029 return;
00030
00031 int multiplier = (int)(r / (2*M_PI));
00032 r = r - multiplier*2*M_PI;
00033 if (r >= M_PI)
00034 r -= 2*M_PI;
00035 if (r < -M_PI)
00036 r += 2*M_PI;
00037 }
00038
00039 OrientedPoint FSRMovement::move(const OrientedPoint& pt) const {
00040 return movePoint(pt, *this);
00041 }
00042
00043 void FSRMovement::invert() {
00044 *this = invertMove(*this);
00045 }
00046
00047 void FSRMovement::compose(const FSRMovement& move2) {
00048 *this = composeMoves(*this, move2);
00049 }
00050
00051
00052 FSRMovement FSRMovement::composeMoves(const FSRMovement& move1,
00053 const FSRMovement& move2) {
00054 FSRMovement comp;
00055 comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f;
00056 comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s;
00057 comp.r = (move1.r + move2.r);
00058 comp.normalize();
00059 return comp;
00060 }
00061
00062 OrientedPoint FSRMovement::movePoint(const OrientedPoint& pt, const FSRMovement& move1) {
00063 OrientedPoint pt2(pt);
00064 pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta);
00065 pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta);
00066 pt2.theta = (move1.r + pt.theta);
00067 pt2.normalize();
00068 return pt2;
00069 }
00070
00071 FSRMovement FSRMovement::moveBetweenPoints(const OrientedPoint& pt1,
00072 const OrientedPoint& pt2) {
00073 FSRMovement move;
00074 move.f = (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta);
00075 move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta);
00076 move.r = (pt2.theta - pt1.theta);
00077 move.normalize();
00078 return move;
00079
00080 }
00081
00082 FSRMovement FSRMovement::invertMove(const FSRMovement& move1) {
00083 FSRMovement p_inv;
00084 p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s;
00085 p_inv.s = sin(move1.r) * move1.f - cos(move1.r) * move1.s;
00086 p_inv.r = (-move1.r);
00087 p_inv.normalize();
00088 return p_inv;
00089 }
00090
00091
00092 OrientedPoint FSRMovement::frameTransformation(const OrientedPoint& reference_pt_frame1,
00093 const OrientedPoint& reference_pt_frame2,
00094 const OrientedPoint& pt_frame1) {
00095 OrientedPoint zero;
00096
00097 FSRMovement itrans_refp1(zero, reference_pt_frame1);
00098 itrans_refp1.invert();
00099
00100 FSRMovement trans_refp2(zero, reference_pt_frame2);
00101 FSRMovement trans_pt(zero, pt_frame1);
00102
00103 FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt);
00104 return tmp.move(zero);
00105 }
00106
00107
00108 }