Ashin Walpola
d107534eb2
obu-firmware builds against the vanetza-idf C-ITS library, which until now came from the colleague's microbu-esp32c5 tree beside the repository and was not tracked here, so a clone of this repository could not build the firmware it ships. The library alone is now part of obu-firmware, as obu-firmware/external/vanetza-idf: their external/vanetza-idf at commit cf4b99f, unchanged (9775 files; see its PROVENANCE.md). CMake takes it from there by default; -DVANETZA_IDF_DIR still points the build elsewhere. The rest of the colleague's tree (their own VAM firmware, PKI tooling, station-link Python tools, the V2X2MAP bridge) stays out of this repository and gitignored; nothing is pushed to their repository. NOTES.md, docs/06, TODO.md and the pcap verifier's usage line point at the new location.
116 lines
3.4 KiB
C++
116 lines
3.4 KiB
C++
#include "areas.hpp"
|
|
#include <boost/math/constants/constants.hpp>
|
|
#include <boost/units/cmath.hpp>
|
|
#include <algorithm>
|
|
#include <cassert>
|
|
#include <limits>
|
|
|
|
namespace vanetza
|
|
{
|
|
namespace geonet
|
|
{
|
|
|
|
double geometric_function(const Circle& c, const CartesianPosition& p)
|
|
{
|
|
if (c.r.value() != 0.0) {
|
|
const double x_over_r = p.x / c.r;
|
|
const double y_over_r = p.y / c.r;
|
|
return 1.0 - (x_over_r * x_over_r) - (y_over_r * y_over_r);
|
|
} else {
|
|
return -std::numeric_limits<double>::infinity();
|
|
};
|
|
}
|
|
|
|
double geometric_function(const Rectangle& r, const CartesianPosition& p)
|
|
{
|
|
if (r.a.value() != 0.0 && r.b.value() != 0.0) {
|
|
const double x_over_a = p.x / r.a;
|
|
const double y_over_b = p.y / r.b;
|
|
return std::min(1.0 - x_over_a * x_over_a, 1.0 - y_over_b * y_over_b);
|
|
} else {
|
|
return -std::numeric_limits<double>::infinity();
|
|
}
|
|
}
|
|
|
|
double geometric_function(const Ellipse& e, const CartesianPosition& p)
|
|
{
|
|
if (e.a.value() != 0.0 && e.b.value() != 0.0) {
|
|
const double x_over_a = p.x / e.a;
|
|
const double y_over_b = p.y / e.b;
|
|
return 1.0 - (x_over_a * x_over_a) - (y_over_b * y_over_b);
|
|
} else {
|
|
return -std::numeric_limits<double>::infinity();
|
|
}
|
|
}
|
|
|
|
struct geometric_function_visitor : public boost::static_visitor<double>
|
|
{
|
|
geometric_function_visitor(const CartesianPosition& p) : point(p) {}
|
|
|
|
template<class SHAPE>
|
|
double operator()(const SHAPE& s) const
|
|
{
|
|
return geometric_function(s, point);
|
|
}
|
|
|
|
const CartesianPosition& point;
|
|
};
|
|
|
|
double geometric_function(const decltype(Area::shape)& shape, const CartesianPosition& p)
|
|
{
|
|
geometric_function_visitor visitor(p);
|
|
return boost::apply_visitor(visitor, shape);
|
|
}
|
|
|
|
CartesianPosition canonicalize(const CartesianPosition& point, units::Angle azimuth)
|
|
{
|
|
using namespace boost::math::double_constants;
|
|
// area.angle is azimuth angle of EN 302 931 V1.1.1
|
|
const units::Angle zenith = half_pi * units::si::radian - azimuth;
|
|
const double sin_z = sin(zenith);
|
|
const double cos_z = cos(zenith);
|
|
// rotate canonical point around origin clockwise: zenith = 90 deg - azimuth
|
|
// other interpretation: rotate shape's long side onto abscissa
|
|
CartesianPosition canonical;
|
|
canonical.x = cos_z * point.x + sin_z * point.y;
|
|
canonical.y = -sin_z * point.x + cos_z * point.y;
|
|
return canonical;
|
|
}
|
|
|
|
struct area_size_visitor : public boost::static_visitor<units::Area>
|
|
{
|
|
units::Area operator()(const Circle& circle) const
|
|
{
|
|
using namespace boost::math::double_constants;
|
|
return pi * circle.r * circle.r;
|
|
}
|
|
|
|
units::Area operator()(const Rectangle& rectangle) const
|
|
{
|
|
using namespace boost::math::double_constants;
|
|
return 4.0 * rectangle.a * rectangle.b;
|
|
}
|
|
|
|
units::Area operator()(const Ellipse& ellipse) const
|
|
{
|
|
using namespace boost::math::double_constants;
|
|
return pi * ellipse.a * ellipse.b;
|
|
}
|
|
};
|
|
|
|
units::Area area_size(const Area& area)
|
|
{
|
|
return boost::apply_visitor(area_size_visitor(), area.shape);
|
|
}
|
|
|
|
bool inside_or_at_border(const Area& area, const GeodeticPosition& geo_position)
|
|
{
|
|
const CartesianPosition local = local_cartesian(area.position, geo_position);
|
|
const CartesianPosition canonical = canonicalize(local, area.angle);
|
|
return !outside_shape(area.shape, canonical);
|
|
}
|
|
|
|
} // namespace geonet
|
|
} // namespace vanetza
|
|
|