CartPosition.cpp

00001 #include "mbed.h" // debug
00002 #include "CartPosition.h"
00003 
00004 #define PI 3.1415926535897932
00005 
00006 CartPosition::CartPosition(void)
00007 {
00008     set(0,0);
00009 }
00010 
00011 CartPosition::CartPosition(float x, float y)
00012 {
00013     set(x,y);
00014 }
00015 
00016 // TODO: 3 fix _x, _y to be private with getters setters
00017 
00018 void CartPosition::set(CartPosition p)
00019 {
00020     _x = p._x;
00021     _y = p._y;
00022 }
00023 
00024 void CartPosition::set(float x, float y)
00025 {
00026     _x = x;
00027     _y = y;
00028 }
00029 
00030 
00031 float CartPosition::bearingTo(CartPosition to)
00032 {
00033     // x and y aren't backwards; it's to correct for the differences between
00034     // geometry and navigation. In the former, angles are measured from the x axis,
00035     // in the latter, from the y axis.
00036     return 180/PI * atan2(to._x-_x, to._y-_y); 
00037 }
00038 
00039 
00040 float CartPosition::distanceTo(CartPosition to)
00041 {
00042     float dx = to._x-_x;
00043     float dy = to._y-_y;
00044     
00045     return sqrt( dx*dx + dy*dy );
00046 }
00047 
00048 void CartPosition::move(float bearing, float distance)
00049 {
00050     // x and y aren't backwards; it's to correct for the differences between
00051     // geometry and navigation. In the former, angles are measured from the x axis,
00052     // in the latter, from the y axis.
00053     float r = bearing * PI / 180;
00054     _x += distance * sin( r );
00055     _y += distance * cos( r );
00056     
00057     return;
00058 }