GteIntrOrientedBox2Sector2.h

Go to the documentation of this file.

// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2017
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <Mathematics/GteSector2.h>
#include <Mathematics/GteOrientedBox.h>
#include <Mathematics/GteDistPointSegment.h>
#include <Mathematics/GteIntrHalfspace2Polygon2.h>
#include <Mathematics/GteTIQuery.h>

// The OrientedBox2 object is considered to be a solid.

namespace gte
{

template <typename Real>
class TIQuery<Real, OrientedBox2<Real>, Sector2<Real>>
{
public:
    struct Result
    {
        bool intersect;
    };

    Result operator()(OrientedBox2<Real> const& box, Sector2<Real> const& sector);
};

template <typename Real>
typename TIQuery<Real, OrientedBox2<Real>, Sector2<Real>>::Result
TIQuery<Real, OrientedBox2<Real>, Sector2<Real>>::operator()(
    OrientedBox2<Real> const& box, Sector2<Real> const& sector)
{
    Result result;

    // Determine whether the vertex is inside the box.
    Vector2<Real> CmV = box.center - sector.vertex;
    Vector2<Real> P{ Dot(box.axis[0], CmV), Dot(box.axis[1], CmV) };
    if (fabs(P[0]) <= box.extent[0] && fabs(P[1]) <= box.extent[1])
    {
        // The vertex is inside the box.
        result.intersect = true;
        return result;
    }

    // Test whether the box is outside the right ray boundary of the sector.
    Vector2<Real> U0
    {
        +sector.cosAngle * sector.direction[0] + sector.sinAngle * sector.direction[1],
        -sector.sinAngle * sector.direction[0] + sector.cosAngle * sector.direction[1]
    };
    Vector2<Real> N0 = Perp(U0);
    Real prjcen0 = Dot(N0, CmV);
    Real radius0 = box.extent[0] * fabs(Dot(N0, box.axis[0]))
        + box.extent[1] * fabs(Dot(N0, box.axis[1]));
    if (prjcen0 > radius0)
    {
        result.intersect = false;
        return result;
    }

    // Test whether the box is outside the ray of the left boundary of the
    // sector.
    Vector2<Real> U1
    {
        +sector.cosAngle * sector.direction[0] - sector.sinAngle * sector.direction[1],
        +sector.sinAngle * sector.direction[0] + sector.cosAngle * sector.direction[1]
    };
    Vector2<Real> N1 = -Perp(U1);
    Real prjcen1 = Dot(N1, CmV);
    Real radius1 = box.extent[0] * fabs(Dot(N1, box.axis[0]))
        + box.extent[1] * fabs(Dot(N1, box.axis[1]));
    if (prjcen1 > radius1)
    {
        result.intersect = false;
        return result;
    }

    // Initialize the polygon of intersection to be the box.
    Vector2<Real> e0U0 = box.extent[0] * box.axis[0];
    Vector2<Real> e1U1 = box.extent[1] * box.axis[1];
    std::vector<Vector2<Real>> polygon;
    polygon.reserve(8);
    polygon.push_back(box.center - e0U0 - e1U1);
    polygon.push_back(box.center + e0U0 - e1U1);
    polygon.push_back(box.center + e0U0 + e1U1);
    polygon.push_back(box.center - e0U0 + e1U1);

    FIQuery<Real, Halfspace<2, Real>, std::vector<Vector2<Real>>> hpQuery;
    typename FIQuery<Real, Halfspace<2, Real>, std::vector<Vector2<Real>>>::Result hpResult;
    Halfspace<2, Real> halfspace;

    // Clip the box against the right-ray sector boundary.
    if (prjcen0 >= -radius0)
    {
        halfspace.normal = -N0;
        halfspace.constant = Dot(halfspace.normal, sector.vertex);
        hpResult = hpQuery(halfspace, polygon);
        polygon = std::move(hpResult.polygon);
    }

    // Clip the box against the left-ray sector boundary.
    if (prjcen1 >= -radius1)
    {
        halfspace.normal = -N1;
        halfspace.constant = Dot(halfspace.normal, sector.vertex);
        hpResult = hpQuery(halfspace, polygon);
        polygon = std::move(hpResult.polygon);
    }

    DCPQuery<Real, Vector2<Real>, Segment2<Real>> psQuery;
    typename DCPQuery<Real, Vector2<Real>, Segment2<Real>>::Result psResult;
    int const numVertices = static_cast<int>(polygon.size());
    if (numVertices >= 2)
    {
        for (int i0 = numVertices - 1, i1 = 0; i1 < numVertices; i0 = i1++)
        {
            Segment2<Real> segment(polygon[i0], polygon[i1]);
            psResult = psQuery(sector.vertex, segment);
            if (psResult.distance <= sector.radius)
            {
                result.intersect = true;
                return result;
            }
        }
    }

    result.intersect = false;
    return result;
}

}