CartPosition.cpp

00001 #include "mbed.h" // debug
00002 #include "CartPosition.h"
00003 #include "util.h"
00004 #include "globals.h"
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 newx, float newy)
00025 {
00026     x = newx;
00027     y = newy;
00028 }
00029 
00030 
00031 float CartPosition::bearingTo(CartPosition to)
00032 {
00033     float result = clamp360(90 - 180/PI * atan2(to.y-y, to.x-x));
00034 
00035     return result;
00036 }
00037 
00038 
00039 float CartPosition::distanceTo(CartPosition to)
00040 {
00041     float dx = to.x-x;
00042     float dy = to.y-y;
00043     
00044     return sqrt( dx*dx + dy*dy );
00045 }
00046 
00047 void CartPosition::move(float bearing, float distance)
00048 {
00049     // x and y aren't backwards; it's to correct for the differences between
00050     // geometry and navigation. In the former, angles are measured from the x axis,
00051     // in the latter, from the y axis.
00052     float r = bearing * PI / 180;
00053     x += distance * sin( r );
00054     y += distance * cos( r );
00055     
00056     return;
00057 }
00058 
00059 CartPosition& CartPosition::operator= (CartPosition p)
00060 {
00061     set(p);
00062     return *this;
00063 }