Util.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * 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    <PACKAGE_NAME>::<CATEGORY>::Util
 * @author     Stefan Reither ( stef dot reither at web dot de )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace armarx
{
    // Mapping from labels used in topic from SecondHands to corresponding ids and labals in the MPI-Model
    const std::map<std::string, std::pair<unsigned int, std::string>> MPII_TO_MPI{
        {"head", {0, "Head"}},
        {"neck", {1, "Neck"}},
        {"right_shoulder", {2, "RShoulder"}},
        {"right_elbow", {3, "RElbow"}},
        {"right_hand", {4, "RWrist"}},
        {"left_shoulder", {5, "LShoulder"}},
        {"left_elbow", {6, "LElbow"}},
        {"left_hand", {7, "LWrist"}},
        {"right_waist", {8, "RHip"}},
        {"right_knee", {9, "RKnee"}},
        {"right_foot", {10, "RAnkle"}},
        {"left_waist", {11, "LHip"}},
        {"left_knee", {12, "LKnee"}},
        {"left_foot", {13, "LAnkle"}},
    };
 
    struct Polygon2D
    {
        struct Point
        {
            float x;
            float y;
 
            bool
            operator==(const Point& p) const
            {
                return this->x - p.x < std::numeric_limits<float>::epsilon() &&
                       this->y - p.y < std::numeric_limits<float>::epsilon();
            }
        };
 
        using PointList = std::vector<Point>;
        // ordered list of polygon points
        // assert(points[0] == points[n]
        std::vector<Point> points;
 
        Polygon2D()
        {
        }
 
        Polygon2D(PointList points)
        {
            ARMARX_CHECK_EXPRESSION(points[0] == points[points.size() - 1]);
            this->points = points;
        }
 
        // isLeft(): tests if a point is Left|On|Right of an infinite line.
        //    Input:  three points p0, p1, and p2
        //    Return: >0 for p2 left of the line through p0 and p1
        //            =0 for p2  on the line
        //            <0 for p2  right of the line
        int
        isLeft(Point p0, Point p1, Point p2) const
        {
            return static_cast<int>((p1.x - p0.x) * (p2.y - p0.y) - (p2.x - p0.x) * (p1.y - p0.y));
        }
 
        // Algorithm from http://geomalgorithms.com/a03-_inclusion.html#wn_PnPoly() (Date: 05/16/2018)
        bool
        isInside(FramedPositionPtr point) const
        {
            Point p;
            p.x = point->x;
            p.y = point->y;
            int wn = 0; // the  winding number counter
            // loop through all edges of the polygon
            for (unsigned int i = 0; i < points.size() - 1; i++) // edge from V[i] to  V[i+1]
            {
                if (points[i].y <= p.y) // start y <= P.y
                {
                    if (points[i + 1].y > p.y) // an upward crossing
                        if (isLeft(points[i], points[i + 1], p) > 0) // P left of  edge
                        {
                            ++wn; // have  a valid up intersect
                        }
                }
                else // start y > P.y (no test needed)
                {
                    if (points[i + 1].y <= p.y) // a downward crossing
                        if (isLeft(points[i], points[i + 1], p) < 0) // P right of  edge
                        {
                            --wn; // have  a valid down intersect
                        }
                }
            }
            return wn == 1;
        }
    };
} // namespace armarx