b9b9d8f9ac
filter is updated every step and every 1000 ms an evaluation is calculated also added sink or swim method from frank, works great
74 lines
1.5 KiB
C++
74 lines
1.5 KiB
C++
#ifndef MESH_STATE_H
|
|
#define MESH_STATE_H
|
|
|
|
#include <Indoor/navMesh/NavMesh.h>
|
|
#include <Indoor/navMesh/NavMeshTriangle.h>
|
|
#include <Indoor/geo/Heading.h>
|
|
|
|
namespace MeshBased {
|
|
|
|
struct MyState {
|
|
|
|
NM::NavMeshLocation<NM::NavMeshTriangle> loc;
|
|
Heading heading;
|
|
|
|
/** ctor */
|
|
MyState() : loc(), heading(0) {
|
|
;
|
|
}
|
|
|
|
/** ctor */
|
|
MyState(NM::NavMeshLocation<NM::NavMeshTriangle> loc, Heading h) : loc(loc), heading(h) {
|
|
;
|
|
}
|
|
|
|
MyState& operator += (const MyState& o) {
|
|
loc.pos += o.loc.pos;
|
|
return *this;
|
|
}
|
|
|
|
MyState& operator /= (const float val) {
|
|
loc.pos /= val;
|
|
return *this;
|
|
}
|
|
|
|
MyState operator * (const float val) const {
|
|
MyState copy = *this;
|
|
copy.loc.pos = copy.loc.pos * val;
|
|
return copy;
|
|
}
|
|
|
|
float getX(){
|
|
return loc.pos.x;
|
|
}
|
|
|
|
float getY() {
|
|
return loc.pos.y;
|
|
}
|
|
|
|
float getZ() {
|
|
return loc.pos.z;
|
|
}
|
|
|
|
void setPosition(Point3 pos){
|
|
loc.pos = pos;
|
|
}
|
|
|
|
float getBinValue(const int dim) const {
|
|
switch (dim) {
|
|
case 0: return this->loc.pos.x;
|
|
case 1: return this->loc.pos.y;
|
|
case 2: return this->loc.pos.z;
|
|
case 3: return this->heading.getRAD();
|
|
}
|
|
throw "cant find this value within the bin";
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
}
|
|
|
|
#endif // MESH_STATE_H
|