Goal::Goal(VFO_ID id, const Quad &corners)
{
m_id = id;
m_corners = corners;
m_location.screen = corners.getBottomCentre();
m_size_on_screen = Vector2<double>(corners.getAverageWidth(), corners.getAverageHeight());
// if(VisionConstants::DO_RADIAL_CORRECTION) {
// VisionBlackboard* vbb = VisionBlackboard::getInstance();
// Vector2<float> corr_bottom_centre = vbb->correctDistortion(Vector2<float>(m_bottom_centre.x, m_bottom_centre.y));
// m_bottom_centre.x = mathGeneral::roundNumberToInt(corr_bottom_centre.x);
// m_bottom_centre.y = mathGeneral::roundNumberToInt(corr_bottom_centre.y);
// }
//CALCULATE DISTANCE AND BEARING VALS
valid = calculatePositions();
//valid = valid && check();
valid = check();
}
arma::vec2 Quad::getSize() const {
Quad boundingBox = getBoundingBox(getVertices());
return {boundingBox.getAverageWidth(), boundingBox.getAverageHeight()};
}