62 lines
1.5 KiB
C++
62 lines
1.5 KiB
C++
#include "TrackedObject.h"
|
|
|
|
#include <math.h>
|
|
|
|
InterestingThing::InterestingThing(Sensor *sensor, Polar position) {
|
|
this->id = 0;
|
|
this->confidence = maxConfidence;
|
|
this->sensor = sensor;
|
|
this->position = Spherical(position);
|
|
}
|
|
|
|
InterestingThing::InterestingThing(Sensor *sensor, Spherical position) {
|
|
this->id = 0;
|
|
this->confidence = maxConfidence;
|
|
this->sensor = sensor;
|
|
this->position = position;
|
|
}
|
|
|
|
bool InterestingThing::IsTheSameAs(InterestingThing *otherObj) {
|
|
if (id != 0 && id == otherObj->id)
|
|
return true;
|
|
if (fabsf(position.magnitude - otherObj->position.magnitude) >
|
|
equalityDistance)
|
|
return false;
|
|
if (fabsf(position.horizontalAngle - otherObj->position.horizontalAngle) >
|
|
equalityAngle)
|
|
return false;
|
|
if (fabsf(position.verticalAngle - otherObj->position.verticalAngle) >
|
|
equalityAngle)
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
#include <Arduino.h>
|
|
|
|
bool InterestingThing::DegradeConfidence(float deltaTime) {
|
|
unsigned char confidenceDrop =
|
|
(unsigned char)((float)confidenceDropSpeed * deltaTime);
|
|
// Make sure the confidence always drops
|
|
if (confidenceDrop == 0)
|
|
confidenceDrop = 1;
|
|
|
|
if (confidence <= confidenceDrop) {
|
|
// object is dead
|
|
confidence = 0;
|
|
return false;
|
|
} else {
|
|
confidence -= confidenceDrop;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
void InterestingThing::Refresh(Polar position) {
|
|
this->position = Spherical(position);
|
|
this->confidence = maxConfidence;
|
|
}
|
|
|
|
void InterestingThing::Refresh(Spherical position) {
|
|
this->position = position;
|
|
this->confidence = maxConfidence;
|
|
}
|