Polygon.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#include "Polygon.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace visionx::ConvexPolygonCalculations
{
 
    //******************************************************************************************************************
    //          Basic tools
    //******************************************************************************************************************
 
    // Sorts the points in the array inout by their X-coordinate (descending)
    void
    SortByX(Vec3d* inout, int arraylength)
    {
        Vec3d temp;
        //bool newmax;
        int i, j;
 
        for (i = 0; i < arraylength; i++)
        {
            for (j = i + 1; j < arraylength;
                 j++) // evtl schneller: erst max-index suchen, nur am ende tauschen
            {
                // oder: j von oben nach unten laufen lassen
                if (inout[j].x > inout[i].x)
                {
                    temp = inout[j];
                    inout[j] = inout[i];
                    inout[i] = temp;
                }
            }
        }
    }
 
    // Sorts the points in the array inout by their Y-coordinate (descending)
    void
    SortByY(Vec3d* inout, int arraylength)
    {
        Vec3d temp;
        //bool newmax;
        int i, j;
 
        for (i = 0; i < arraylength; i++)
        {
            for (j = i + 1; j < arraylength; j++)
            {
                if (inout[j].y > inout[i].y)
                {
                    temp = inout[j];
                    inout[j] = inout[i];
                    inout[i] = temp;
                }
            }
        }
    }
 
    // returns true if line sequence definded by three points makes a turn to the left
    bool
    LeftTurn(Vec3d p1, Vec3d p2, Vec3d p3)
    {
        return (p2.x - p1.x) * (p3.y - p2.y) - (p3.x - p2.x) * (p2.y - p1.y) >= 0;
    }
 
    // returns true if line sequence definded by three points makes a turn to the right
    bool
    RightTurn(Vec3d p1, Vec3d p2, Vec3d p3)
    {
        return (p2.x - p1.x) * (p3.y - p2.y) - (p3.x - p2.x) * (p2.y - p1.y) <= 0;
    }
 
    // returns true if Point lies on the right side of the line defined by linePoint1 and linePoint2 (2D)
    bool
    PointIsOnTheRightSideOfTheLine(Vec3d Point, Vec3d linePoint1, Vec3d linePoint2)
    {
        return (Point.y - linePoint1.y) * (linePoint2.x - linePoint1.x) -
                   (Point.x - linePoint1.x) * (linePoint2.y - linePoint1.y) <
               0;
    }
 
    // Returns true if it is possible that the two polygons intersect
    bool
    PolygonsMightIntersect(Polygon* pol1, Polygon* pol2)
    {
        return ((pol1->maxX > pol2->minX) && (pol2->maxX > pol1->minX) &&
                (pol1->maxY > pol2->minY) && (pol2->maxY > pol1->minY));
    }
 
    //******************************************************************************************************************
    //          Calculate the convex hull of polygons
    //******************************************************************************************************************
 
    // Analog zu Sanders-VL Algorithmentechnik, Folien 16 vom 12.02.2008, S.28-32.
    // Calculates the convex hull of the points in "in", the left part of the hull is stored in hull_left,
    // the right part in hull_right. Both start with the lowest point, then the highest point, then the
    // points of the respective hull descending by y until the lowest point (again).
    // hull_left, hull_right, temp1 and temp2 have to be of size (max number of corners of a polygon) + 1
    void
    CalcConvexHull(Vec3d* in,
                   int num_all_points,
                   Vec3d* hull_left,
                   int* num_hull_points_left,
                   Vec3d* hull_right,
                   int* num_hull_points_right,
                   Vec3d* temp1,
                   Vec3d* temp2)
    {
        int left_points_counter, right_points_counter;
 
        for (int i = 0; i < num_all_points; i++)
        {
            temp1[i] = in[i];
        }
 
        SortByY(temp1, num_all_points);
 
        hull_left[0] = temp1[num_all_points - 1];
        hull_left[1] = temp1[0];
        hull_right[0] = temp1[num_all_points - 1];
        hull_right[1] = temp1[0];
        float divided_x_dif =
            (temp1[0].x - temp1[num_all_points - 1].x) / (temp1[0].y - temp1[num_all_points - 1].y);
 
 
        // Divide into points on the left and on the right of the line from highest to lowest point.
        // Left part is in the first part of the array, in descending order of y-values, right part
        // is in the upper part of the array in (from the end) descending order of y-values
        left_points_counter = 0;
        right_points_counter = 0;
        for (int i = 1; i < num_all_points - 1; i++)
        {
            if (temp1[i].x < temp1[0].x + (temp1[i].y - temp1[0].y) * divided_x_dif)
            {
                temp2[left_points_counter] = temp1[i];
                left_points_counter++;
            }
            else
            {
                temp2[num_all_points - 1 - right_points_counter] = temp1[i];
                right_points_counter++;
            }
        }
        // add lowest point
        temp2[left_points_counter] = temp1[num_all_points - 1];
        left_points_counter++;
        temp2[num_all_points - 1 - right_points_counter] = temp1[num_all_points - 1];
        right_points_counter++;
 
 
        // left part
        int hull_ind_l = 2;
        hull_left[2] = temp2[0];
        for (int i = 1; i < left_points_counter; i++)
        {
            while (RightTurn(hull_left[hull_ind_l - 1], hull_left[hull_ind_l], temp2[i]))
            {
                hull_ind_l--;
            }
            hull_ind_l++;
            hull_left[hull_ind_l] = temp2[i];
        }
        *num_hull_points_left = hull_ind_l + 1;
 
 
        //right part
        int hull_ind_r = 2;
        hull_right[2] = temp2[num_all_points - 1];
        for (int i = 2; i <= right_points_counter; i++)
        {
            while (LeftTurn(
                hull_right[hull_ind_r - 1], hull_right[hull_ind_r], temp2[num_all_points - i]))
            {
                hull_ind_r--;
            }
            hull_ind_r++;
            hull_right[hull_ind_r] = temp2[num_all_points - i];
        }
        *num_hull_points_right = hull_ind_r + 1;
    }
 
    void
    CalcConvexHull(Vec3d* in,
                   int num_all_points,
                   Vec3d* hull_left,
                   int* num_hull_points_left,
                   Vec3d* hull_right,
                   int* num_hull_points_right)
    {
        Vec3d* temp1 = new Vec3d[num_all_points + 1];
        Vec3d* temp2 = new Vec3d[num_all_points + 1];
        CalcConvexHull(in,
                       num_all_points,
                       hull_left,
                       num_hull_points_left,
                       hull_right,
                       num_hull_points_right,
                       temp1,
                       temp2);
    }
 
    // interface to create the convex polygon pol from the points in the array in
    void
    CalcConvexHull(Vec3d* in, int num_all_points, Polygon* pol, Vec3d* temp1, Vec3d* temp2)
    {
        int i, j;
 
        CalcConvexHull(in,
                       num_all_points,
                       (Vec3d*)&pol->hullLeft,
                       &pol->nCornersLeft,
                       (Vec3d*)&pol->hullRight,
                       &pol->nCornersRight,
                       temp1,
                       temp2);
 
        // merge left and right part of the hull with corners ordered by y descending
        pol->hull[0] = pol->hullLeft[1]; // first elem = highest point
        pol->hull[0].z = 3;
        int ind_l = 2;
        int ind_r = 2;
        int ind_h = 1;
        while (ind_l < pol->nCornersLeft - 1 && ind_r < pol->nCornersRight - 1)
        {
            if (pol->hullLeft[ind_l].y > pol->hullRight[ind_r].y)
            {
                pol->hull[ind_h] = pol->hullLeft[ind_l];
                pol->hull[ind_h].z = 1;
                ind_l++;
            }
            else
            {
                pol->hull[ind_h] = pol->hullRight[ind_r];
                pol->hull[ind_h].z = 2;
                ind_r++;
            }
            ind_h++;
        }
        // now get the elements which rest in the left or the right part
        while (ind_l < pol->nCornersLeft - 1)
        {
            pol->hull[ind_h] = pol->hullLeft[ind_l];
            pol->hull[ind_h].z = 1;
            ind_l++;
            ind_h++;
        }
        while (ind_r < pol->nCornersRight - 1)
        {
            pol->hull[ind_h] = pol->hullRight[ind_r];
            pol->hull[ind_h].z = 2;
            ind_r++;
            ind_h++;
        }
 
        pol->nCorners = pol->nCornersRight + pol->nCornersLeft - 4;
        pol->hull[pol->nCorners - 1] = pol->hullLeft[0]; // last elem = lowest point
        pol->hull[pol->nCorners - 1].z = 3;
 
 
        // calculate the hull with the corners ordered in clockwise sense
        i = 0;
        for (j = 1; j < pol->nCornersRight - 1; j++, i++)
        {
            pol->hullCircle[i] = pol->hullRight[j];
        }
        pol->hullCircle[i] = pol->hullRight[0];
        i++;
        for (j = pol->nCornersLeft - 2; j > 1; j--, i++)
        {
            pol->hullCircle[i] = pol->hullLeft[j];
        }
        pol->hullCircle[i] = pol->hullRight[1];
 
 
        // calculate the coordinates for the enveloping rectangle
        pol->minY = pol->hull[pol->nCorners - 1].y;
        pol->maxY = pol->hull[0].y;
        pol->minX = pol->hull[0].x;
        for (i = 2; i <= pol->nCornersLeft; i++)
            if (pol->hullLeft[i].x < pol->minX)
            {
                pol->minX = pol->hullLeft[i].x;
            }
        pol->maxX = pol->hull[0].x;
        for (i = 2; i <= pol->nCornersRight; i++)
            if (pol->hullRight[i].x > pol->maxX)
            {
                pol->maxX = pol->hullRight[i].x;
            }
    }
 
    void
    CalcConvexHull(Vec3d* in, int num_all_points, Polygon* pol)
    {
        Vec3d* temp1 = new Vec3d[num_all_points + 1];
        Vec3d* temp2 = new Vec3d[num_all_points + 1];
        CalcConvexHull(in, num_all_points, pol, temp1, temp2);
    }
 
    // interface to create the convex polygon pol from the points already in pol->hull
    void
    CalcConvexHull(Polygon* pol, Vec3d* temp1, Vec3d* temp2)
    {
        CalcConvexHull((Vec3d*)&pol->hull, pol->nCorners, pol, temp1, temp2);
    }
 
    void
    CalcConvexHull(Polygon* pol)
    {
        Vec3d* temp1 = new Vec3d[pol->nCorners + 1];
        Vec3d* temp2 = new Vec3d[pol->nCorners + 1];
        CalcConvexHull(pol, temp1, temp2);
    }
 
    // creates a convex polygon from the points in hullpoints, assuming that they all belong to the
    // convex hull
    void
    CreateConvexPolygonFromHullPoints(Vec3d* hullpoints, int nPoints, Polygon* pol)
    {
        int left_points_counter, right_points_counter;
 
        SortByY(hullpoints, nPoints);
 
        pol->hullLeft[0] = hullpoints[nPoints - 1];
        pol->hullLeft[1] = hullpoints[0];
        pol->hullRight[0] = hullpoints[nPoints - 1];
        pol->hullRight[1] = hullpoints[0];
        float divided_x_dif = (hullpoints[0].x - hullpoints[nPoints - 1].x) /
                              (hullpoints[0].y - hullpoints[nPoints - 1].y);
 
        pol->hull[0].x = hullpoints[0].x;
        pol->hull[0].y = hullpoints[0].y;
        pol->hull[0].z = 3;
        pol->hull[nPoints - 1].x = hullpoints[nPoints - 1].x;
        pol->hull[nPoints - 1].y = hullpoints[nPoints - 1].y;
        pol->hull[nPoints - 1].z = 3;
 
        left_points_counter = 2;
        right_points_counter = 2;
        for (int i = 1; i < nPoints - 1; i++)
        {
            pol->hull[i].x = hullpoints[i].x;
            pol->hull[i].y = hullpoints[i].y;
            if (hullpoints[i].x <
                hullpoints[0].x + (hullpoints[i].y - hullpoints[0].y) * divided_x_dif)
            {
                pol->hullLeft[left_points_counter] = hullpoints[i];
                pol->hull[i].z = 1;
                left_points_counter++;
            }
            else
            {
                pol->hullRight[right_points_counter] = hullpoints[i];
                pol->hull[i].z = 2;
                right_points_counter++;
            }
        }
 
        pol->hullLeft[left_points_counter] = hullpoints[nPoints - 1];
        pol->hullRight[right_points_counter] = hullpoints[nPoints - 1];
 
        pol->nCorners = nPoints;
        pol->nCornersLeft = left_points_counter + 1;
        pol->nCornersRight = right_points_counter + 1;
 
        // calculate the hull with the corners ordered in clockwise sense
        int i = 0;
        for (int j = 1; j < pol->nCornersRight - 1; j++, i++)
        {
            pol->hullCircle[i] = pol->hullRight[j];
        }
        pol->hullCircle[i] = pol->hullRight[0];
        i++;
        for (int j = pol->nCornersLeft - 2; j > 1; j--, i++)
        {
            pol->hullCircle[i] = pol->hullLeft[j];
        }
        pol->hullCircle[i] = pol->hullRight[1];
 
 
        // calculate the coordinates for the enveloping rectangle
        pol->minY = pol->hull[pol->nCorners - 1].y;
        pol->maxY = pol->hull[0].y;
        pol->minX = pol->hull[0].x;
        for (int i = 2; i <= pol->nCornersLeft; i++)
            if (pol->hullLeft[i].x < pol->minX)
            {
                pol->minX = pol->hullLeft[i].x;
            }
        pol->maxX = pol->hull[0].x;
        for (int i = 2; i <= pol->nCornersRight; i++)
            if (pol->hullRight[i].x > pol->maxX)
            {
                pol->maxX = pol->hullRight[i].x;
            }
    }
 
    //******************************************************************************************************************
    //          Calculate the intersection polygon of two convex polygons
    //******************************************************************************************************************
 
    // returns the convex polygon that is the intersection polygon of p1 and p2. pointAccu and boolTable have to
    // be of size 2*(maximal number of points in a normal polygon), clockwiseHullPoly1/2 of size (maximal number
    // of points in a normal polygon)+1
    void
    GetPolygonIntersection(Polygon* p1,
                           Polygon* p2,
                           Polygon* pInter,
                           Vec3d* pointAccu,
                           bool* boolTable,
                           Vec3d* clockwiseHullPoly1,
                           Vec3d* clockwiseHullPoly2)
    {
        int pointcount = 0;
        int i, j;
 
        //*******************************************************************
        // Get corners of polygon 1 that lie inside polygon 2
        //
        // The line segments are traversed in clockwise order. A point lies
        // inside the convex polygon iff it is on the right side of every
        // line segment.
        //*******************************************************************
 
        // boolTable contains booleans which indicate if point i is inside the polygon
        for (j = 0; j < p1->nCorners; j++)
        {
            boolTable[j] = true;
        }
 
        for (i = 0; i < p2->nCorners; i++)
            for (j = 0; j < p1->nCorners; j++)
            {
                boolTable[j] &= PointIsOnTheRightSideOfTheLine(
                    p1->hull[j], p2->hullCircle[i], p2->hullCircle[i + 1]);
            }
 
 
        for (j = 0; j < p1->nCorners; j++)
            if (boolTable[j] == true)
            {
                pointAccu[pointcount] = p1->hull[j];
                pointcount++;
            }
 
 
        //*******************************************************************
        // Get corners of polygon 2 that lie inside polygon 1
        //*******************************************************************
 
        for (j = 0; j < p2->nCorners; j++)
        {
            boolTable[j] = true;
        }
 
        for (i = 0; i < p1->nCorners; i++)
            for (j = 0; j < p2->nCorners; j++)
            {
                boolTable[j] &= PointIsOnTheRightSideOfTheLine(
                    p2->hull[j], p1->hullCircle[i], p1->hullCircle[i + 1]);
            }
 
 
        for (j = 0; j < p2->nCorners; j++)
            if (boolTable[j] == true)
            {
                pointAccu[pointcount] = p2->hull[j];
                pointcount++;
            }
 
 
        // optim: hier abbruchkriterium, wenn pointcount=0 und zentren nicht sehr nah beieinander
 
 
        //*******************************************************************
        // Calculate intersections
        //
        // A line is defined as p1 + c*(p2-p1). Solving the two equations
        // resulting from setting line1 = line2 gives the value of c for
        // which the above term gives the intersection point. If c is
        // in [0,1], the intersection lies on the line segment (p1,p2).
        // An analogue criterion exists for the other line segment.
        //*******************************************************************
 
        float x1, y1, x2, y2, x3, y3, x4, y4, c1, c2, denominator;
 
 
        for (i = 0; i < p1->nCorners; i++)
        {
            x1 = p1->hullCircle[i].x;
            y1 = p1->hullCircle[i].y;
            x2 = p1->hullCircle[i + 1].x;
            y2 = p1->hullCircle[i + 1].y;
            for (j = 0; j < p2->nCorners; j++)
            {
                x3 = p2->hullCircle[j].x;
                y3 = p2->hullCircle[j].y;
                x4 = p2->hullCircle[j + 1].x;
                y4 = p2->hullCircle[j + 1].y;
                denominator = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
                c1 = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / denominator;
                c2 = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / denominator;
                if (0 <= c1 && 1 >= c1 && 0 <= c2 && 1 >= c2)
                {
                    pointAccu[pointcount].x = x1 + c1 * (x2 - x1);
                    pointAccu[pointcount].y = y1 + c1 * (y2 - y1);
                    pointcount++;
                }
            }
        }
 
        if (pointcount >= 3)
        {
            CreateConvexPolygonFromHullPoints(pointAccu, pointcount, pInter);
        }
        else
        {
            pInter->nCorners = 0;
        }
    }
 
    void
    GetPolygonIntersection(Polygon* p1, Polygon* p2, Polygon* pInter)
    {
        Vec3d pointAccu[2 * DSHT_MAX_POLYGON_CORNERS];
        bool boolTable[2 * DSHT_MAX_POLYGON_CORNERS];
        Vec3d clockwiseHullPoly1[DSHT_MAX_POLYGON_CORNERS + 1];
        Vec3d clockwiseHullPoly2[DSHT_MAX_POLYGON_CORNERS + 1];
        GetPolygonIntersection(
            p1, p2, pInter, pointAccu, boolTable, clockwiseHullPoly1, clockwiseHullPoly2);
    }
 
} // namespace visionx::ConvexPolygonCalculations