kazu
9947dced15
added new helper methods/classes (e.g. for heading) new test cases optimize the dijkstra cleanups/refactoring added timed-benchmarks to the log many more...
64 lines
1.4 KiB
C++
64 lines
1.4 KiB
C++
#ifndef HEADING_H
|
|
#define HEADING_H
|
|
|
|
#include <cmath>
|
|
|
|
#include "Angle.h"
|
|
|
|
struct Heading {
|
|
|
|
#define _2PI (2*M_PI)
|
|
|
|
private:
|
|
|
|
/** heading in radians. 0 = to-the-right */
|
|
float rad;
|
|
|
|
public:
|
|
|
|
/** ctor with radians */
|
|
Heading(const float rad) : rad(rad) {
|
|
_assertBetween(rad, 0, _2PI, "radians out of bounds");
|
|
}
|
|
|
|
/** ctor from(x1,y1) to(x2,y2) */
|
|
Heading(const float x1, const float y1, const float x2, const float y2) : rad(Angle::getRAD_2PI(x1,y1,x2,y2)) {
|
|
_assertBetween(rad, 0, _2PI, "radians out of bounds");
|
|
}
|
|
|
|
/** angular difference [0:PI] */
|
|
float getDiffHalfRAD(const Heading other) const {
|
|
return Angle::getDiffRAD_2PI_PI(rad, other.rad);
|
|
}
|
|
|
|
/** update the angle but ensure we stay within [0:2PI] */
|
|
Heading& operator += (const float _rad) {
|
|
_assertBetween(_rad, -_2PI*0.99, +_2PI*0.99, "radians out of bounds");
|
|
rad += _rad;
|
|
if (rad > _2PI) {rad -= _2PI;}
|
|
if (rad < 0) {rad += _2PI;}
|
|
return *this;
|
|
}
|
|
|
|
/** get an inverted version of this heading (upwards -> downwards, left -> right, ...) */
|
|
Heading getInverted() const {
|
|
Heading out(rad);
|
|
out += M_PI;
|
|
return out;
|
|
}
|
|
|
|
float getRAD() const {return rad;}
|
|
|
|
#undef _2PI
|
|
|
|
};
|
|
|
|
namespace Headings {
|
|
static const Heading RIGHT = Heading(M_PI*0/2);
|
|
static const Heading UP = Heading(M_PI*1/2);
|
|
static const Heading LEFT = Heading(M_PI*2/2);
|
|
static const Heading DOWN = Heading(M_PI*3/2);
|
|
}
|
|
|
|
#endif // HEADING_H
|