recharge/src/common/math/box2d.cpp

#include "math/box2d.h"
#include "math/math_utils.h"
#include "math/polygon2d.h"

#include <cmath>
#include <sstream>

using namespace decision::math;

namespace {

double outer_prod(double x0, double y0, double x1, double y1, double x2, double y2)
{
    return (x1 - x0) * (y2 - y0) - (x2 - x0) * (y1 - y0);
}

double pt_seg_distance(double query_x, double query_y, double start_x, double start_y,
    double end_x, double end_y, double length)
{
    const double x0 = query_x - start_x;
    const double y0 = query_y - start_y;
    const double dx = end_x - start_x;
    const double dy = end_y - start_y;
    const double proj = x0 * dx + y0 * dy;
    if (proj <= 0.0) {
        return hypot(x0, y0);
    }
    if (proj >= length * length) {
        return hypot(x0 - dx, y0 - dy);
    }
    return std::abs(x0 * dy - y0 * dx) / length;
}

} // namespace

Box2d::Box2d(const Vec2d& center, const double heading,
    const double length, const double width)
    : _center(center)
    , _length(length)
    , _width(width)
    , _half_length(length / 2.0)
    , _half_width(width / 2.0)
    , _heading(heading)
    , _cos_heading(cos(heading))
    , _sin_heading(sin(heading))
{
    CHECK_GT(_length, -kMathEpsilon);
    CHECK_GT(_width, -kMathEpsilon);
}

Box2d::Box2d(const Segment2d& axis, const double width)
    : _center(axis.center())
    , _length(axis.length())
    , _width(width)
    , _half_length(axis.length() / 2.0)
    , _half_width(width / 2.0)
    , _heading(axis.heading())
    , _cos_heading(axis.cos_heading())
    , _sin_heading(axis.sin_heading())
{
    CHECK_GT(_length, -kMathEpsilon);
    CHECK_GT(_width, -kMathEpsilon);
}

Box2d::Box2d(const AABox2d& aabox)
    : _center(aabox.center())
    , _length(aabox.length())
    , _width(aabox.width())
    , _half_length(aabox.half_length())
    , _half_width(aabox.half_width())
    , _heading(0.0)
    , _cos_heading(1.0)
    , _sin_heading(0.0)
{
    CHECK_GT(_length, -kMathEpsilon);
    CHECK_GT(_width, -kMathEpsilon);
}

Box2d Box2d::create_aa_box(const Vec2d& one_corner, const Vec2d& opponent_corner)
{
    const double x1 = std::min(one_corner.x(), opponent_corner.x());
    const double x2 = std::max(one_corner.x(), opponent_corner.x());
    const double y1 = std::min(one_corner.y(), opponent_corner.y());
    const double y2 = std::max(one_corner.y(), opponent_corner.y());
    return Box2d({ (x1 + x2) / 2.0, (y1 + y2) / 2.0 }, 0.0, x2 - x1, y2 - y1);
}

void Box2d::get_all_corners(std::vector<Vec2d>* const corners) const
{
    if (corners == nullptr) {
        return;
    }
    const double dx1 = _cos_heading * _half_length;
    const double dy1 = _sin_heading * _half_length;
    const double dx2 = _sin_heading * _half_width;
    const double dy2 = -_cos_heading * _half_width;
    corners->clear();
    corners->reserve(4);
    corners->emplace_back(_center.x() + dx1 + dx2, _center.y() + dy1 + dy2);
    corners->emplace_back(_center.x() + dx1 - dx2, _center.y() + dy1 - dy2);
    corners->emplace_back(_center.x() - dx1 - dx2, _center.y() - dy1 - dy2);
    corners->emplace_back(_center.x() - dx1 + dx2, _center.y() - dy1 + dy2);
}

bool Box2d::is_point_in(const Vec2d& point) const
{
    const double x0 = point.x() - _center.x();
    const double y0 = point.y() - _center.y();
    const double dx = std::abs(x0 * _cos_heading + y0 * _sin_heading);
    const double dy = std::abs(x0 * _sin_heading - y0 * _cos_heading);
    return dx <= _half_length + kMathEpsilon && dy <= _half_width + kMathEpsilon;
}

bool Box2d::is_point_on_boundary(const Vec2d& point) const
{
    const double x0 = point.x() - _center.x();
    const double y0 = point.y() - _center.y();
    const double dx = std::abs(x0 * _cos_heading + y0 * _sin_heading);
    const double dy = std::abs(x0 * _sin_heading - y0 * _cos_heading);
    return (std::abs(dx - _half_length) <= kMathEpsilon && dy <= _half_width + kMathEpsilon) || (std::abs(dy - _half_width) <= kMathEpsilon && dx <= _half_length + kMathEpsilon);
}

double Box2d::distance_to(const Vec2d& point) const
{
    const double x0 = point.x() - _center.x();
    const double y0 = point.y() - _center.y();
    const double dx = std::abs(x0 * _cos_heading + y0 * _sin_heading) - _half_length;
    const double dy = std::abs(x0 * _sin_heading - y0 * _cos_heading) - _half_width;
    if (dx <= 0.0) {
        return std::max(0.0, dy);
    }
    if (dy <= 0.0) {
        return dx;
    }
    return hypot(dx, dy);
}

bool Box2d::has_overlap(const Segment2d& segment) const
{
    if (segment.length() <= kMathEpsilon) {
        return is_point_in(segment.start());
    }
    return distance_to(segment) <= kMathEpsilon;
}

double Box2d::distance_to(const Segment2d& segment) const
{
    if (segment.length() <= kMathEpsilon) {
        return distance_to(segment.start());
    }
    const double ref_x1 = segment.start().x() - _center.x();
    const double ref_y1 = segment.start().y() - _center.y();
    double x1 = ref_x1 * _cos_heading + ref_y1 * _sin_heading;
    double y1 = ref_x1 * _sin_heading - ref_y1 * _cos_heading;
    double box_x = _half_length;
    double box_y = _half_width;
    int gx1 = (x1 >= box_x ? 1 : (x1 <= -box_x ? -1 : 0));
    int gy1 = (y1 >= box_y ? 1 : (y1 <= -box_y ? -1 : 0));
    if (gx1 == 0 && gy1 == 0) {
        return 0.0;
    }
    const double ref_x2 = segment.end().x() - _center.x();
    const double ref_y2 = segment.end().y() - _center.y();
    double x2 = ref_x2 * _cos_heading + ref_y2 * _sin_heading;
    double y2 = ref_x2 * _sin_heading - ref_y2 * _cos_heading;
    int gx2 = (x2 >= box_x ? 1 : (x2 <= -box_x ? -1 : 0));
    int gy2 = (y2 >= box_y ? 1 : (y2 <= -box_y ? -1 : 0));
    if (gx2 == 0 && gy2 == 0) {
        return 0.0;
    }
    if (gx1 < 0 || (gx1 == 0 && gx2 < 0)) {
        x1 = -x1;
        gx1 = -gx1;
        x2 = -x2;
        gx2 = -gx2;
    }
    if (gy1 < 0 || (gy1 == 0 && gy2 < 0)) {
        y1 = -y1;
        gy1 = -gy1;
        y2 = -y2;
        gy2 = -gy2;
    }
    if (gx1 < gy1 || (gx1 == gy1 && gx2 < gy2)) {
        std::swap(x1, y1);
        std::swap(gx1, gy1);
        std::swap(x2, y2);
        std::swap(gx2, gy2);
        std::swap(box_x, box_y);
    }
    if (gx1 == 1 && gy1 == 1) {
        switch (gx2 * 3 + gy2) {
        case 4:
            return pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
        case 3:
            return (x1 > x2) ? (x2 - box_x)
                             : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
        case 2:
            return (x1 > x2) ? pt_seg_distance(box_x, -box_y, x1, y1, x2, y2, segment.length())
                             : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
        case -1:
            return outer_prod(x1, y1, x2, y2, box_x, -box_y) >= 0.0 ? 0.0
                                                                    : pt_seg_distance(box_x, -box_y, x1, y1, x2, y2, segment.length());
        case -4:
            return outer_prod(x1, y1, x2, y2, box_x, -box_y) <= 0.0 ? pt_seg_distance(box_x, -box_y, x1, y1, x2, y2, segment.length())
                                                                    : (outer_prod(x1, y1, x2, y2, -box_x, box_y) <= 0.0 ? 0.0
                                                                                                                        : pt_seg_distance(-box_x, box_y, x1, y1, x2, y2, segment.length()));
        }
    } else {
        switch (gx2 * 3 + gy2) {
        case 4:
            return (x1 < x2) ? (x1 - box_x)
                             : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
        case 3:
            return std::min(x1, x2) - box_x;
        case 1:
        case -2:
            return outer_prod(x1, y1, x2, y2, box_x, box_y) <= 0.0 ? 0.0
                                                                   : pt_seg_distance(box_x, box_y, x1, y1, x2, y2, segment.length());
        case -3:
            return 0.0;
        }
    }
    // CHECK(0) << "unimplemented state: " << gx1 << " " << gy1 << " " << gx2 << " " << gy2;
    return 0.0;
}

double Box2d::distance_to(const Box2d& box) const
{
    return Polygon2d(box).distance_to(*this);
}

bool Box2d::has_overlap(const Box2d& box) const
{
    const double shift_x = box.center_x() - _center.x();
    const double shift_y = box.center_y() - _center.y();

    const double dx1 = _cos_heading * _half_length;
    const double dy1 = _sin_heading * _half_length;
    const double dx2 = _sin_heading * _half_width;
    const double dy2 = -_cos_heading * _half_width;
    const double dx3 = box.cos_heading() * box.half_length();
    const double dy3 = box.sin_heading() * box.half_length();
    const double dx4 = box.sin_heading() * box.half_width();
    const double dy4 = -box.cos_heading() * box.half_width();

    return std::abs(shift_x * _cos_heading + shift_y * _sin_heading) <= std::abs(dx3 * _cos_heading + dy3 * _sin_heading) + std::abs(dx4 * _cos_heading + dy4 * _sin_heading) + _half_length && std::abs(shift_x * _sin_heading - shift_y * _cos_heading) <= std::abs(dx3 * _sin_heading - dy3 * _cos_heading) + std::abs(dx4 * _sin_heading - dy4 * _cos_heading) + _half_width && std::abs(shift_x * box.cos_heading() + shift_y * box.sin_heading()) <= std::abs(dx1 * box.cos_heading() + dy1 * box.sin_heading()) + std::abs(dx2 * box.cos_heading() + dy2 * box.sin_heading()) + box.half_length() && std::abs(shift_x * box.sin_heading() - shift_y * box.cos_heading()) <= std::abs(dx1 * box.sin_heading() - dy1 * box.cos_heading()) + std::abs(dx2 * box.sin_heading() - dy2 * box.cos_heading()) + box.half_width();
}

AABox2d Box2d::get_aa_box() const
{
    const double dx1 = std::abs(_cos_heading * _half_length);
    const double dy1 = std::abs(_sin_heading * _half_length);
    const double dx2 = std::abs(_sin_heading * _half_width);
    const double dy2 = std::abs(_cos_heading * _half_width);
    return AABox2d(_center, (dx1 + dx2) * 2.0, (dy1 + dy2) * 2.0);
}

void Box2d::rotate_from_center(double rotate_angle)
{
    _heading = normalize_angle(_heading + rotate_angle);
    _cos_heading = cos(_heading);
    _sin_heading = sin(_heading);
}

void Box2d::shift(const Vec2d& shift_vec)
{
    _center += shift_vec;
}

std::string Box2d::debug_string() const
{
    std::ostringstream sout;
    sout << "box2d ( center = " << _center.debug_string()
         << "  heading = " << _heading
         << "  length = " << _length
         << "  width = " << _width << " )";
    sout.flush();
    return sout.str();
}