92 lines
2.7 KiB
C++
92 lines
2.7 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 =
|
|
Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
|
|
this->updated = true;
|
|
}
|
|
|
|
InterestingThing::InterestingThing(Sensor* sensor,
|
|
Spherical16 position,
|
|
Quaternion orientation) {
|
|
this->id = 0;
|
|
this->confidence = maxConfidence;
|
|
this->sensor = sensor;
|
|
this->position = position;
|
|
float angle;
|
|
Vector3 axis;
|
|
orientation.ToAngleAxis(&angle, &axis);
|
|
this->orientation = AngleAxis<float>(angle, axis);
|
|
this->updated = true;
|
|
}
|
|
|
|
// #include <Arduino.h>
|
|
bool InterestingThing::IsTheSameAs(InterestingThing* otherObj) {
|
|
if (id != 0 && id == otherObj->id)
|
|
return true;
|
|
if (type != otherObj->type)
|
|
return false;
|
|
|
|
float equalDistance = equalityDistance;
|
|
float equalAngle = equalityAngle;
|
|
if (otherObj->type == 0x80 || otherObj->id == 0x80) {
|
|
equalDistance = 0.1f;
|
|
equalAngle = 10;
|
|
}
|
|
// printf(" %d %f (%f %f) is same as %f (%f %f)\n", otherObj->type,
|
|
// otherObj->position.distance,
|
|
// (float)otherObj->position.horizontalAngle,
|
|
// (float)otherObj->position.verticalAngle, position.distance,
|
|
// (float)position.horizontalAngle, (float)position.verticalAngle);
|
|
if (fabsf(position.distance - otherObj->position.distance) > equalDistance)
|
|
return false;
|
|
if (fabsf(position.horizontal.ToFloat() -
|
|
otherObj->position.horizontal.ToFloat()) > equalAngle)
|
|
return false;
|
|
if (fabsf(position.vertical.ToFloat() -
|
|
otherObj->position.vertical.ToFloat()) > equalAngle)
|
|
return false;
|
|
// printf(" -> yes ");
|
|
return true;
|
|
}
|
|
|
|
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 =
|
|
Spherical16(position.distance, Angle16(position.angle.ToFloat()), 0);
|
|
this->confidence = maxConfidence;
|
|
this->updated = true;
|
|
}
|
|
|
|
void InterestingThing::Refresh(Spherical16 position, Quaternion orientation) {
|
|
this->position = position;
|
|
|
|
float angle;
|
|
Vector3 axis;
|
|
orientation.ToAngleAxis(&angle, &axis);
|
|
this->orientation = AngleAxis<float>(angle, axis);
|
|
this->confidence = maxConfidence;
|
|
this->updated = true;
|
|
}
|