Geometry.h

Go to the documentation of this file.

/*
    Enki - a fast 2D robot simulator
    Copyright (C) 1999-2016 Stephane Magnenat <stephane at magnenat dot net>
    Copyright (C) 2004-2005 Markus Waibel <markus dot waibel at epfl dot ch>
    Copyright (c) 2004-2005 Antoine Beyeler <abeyeler at ab-ware dot com>
    Copyright (C) 2005-2006 Laboratory of Intelligent Systems, EPFL, Lausanne
    Copyright (C) 2006-2008 Laboratory of Robotics Systems, EPFL, Lausanne
    See AUTHORS for details

    This program is free software; the authors of any publication 
    arising from research using this software are asked to add the 
    following reference:
    Enki - a fast 2D robot simulator
    http://home.gna.org/enki
    Stephane Magnenat <stephane at magnenat dot net>,
    Markus Waibel <markus dot waibel at epfl dot ch>
    Laboratory of Intelligent Systems, EPFL, Lausanne.

    You can redistribute this program and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program 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, write to the Free Software
    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef __ENKI_GEOMETRY_H
#define __ENKI_GEOMETRY_H

#ifdef WIN32
#define _USE_MATH_DEFINES
#include <math.h>
#ifndef NOMINMAX
#define NOMINMAX
#endif
#endif
#include <cmath>
#include <vector>
#include <limits>
#include <ostream>
#include <algorithm>

#ifdef _MSC_VER
#define round(x) floor((x) + 0.5)
#endif

namespace Enki
{

    struct Vector
    {
        double x;
        double y;
    
        Vector() { x = y = 0; }
        Vector(double v) { this->x = v; this->y = v; }
        Vector(double x, double y) { this->x = x; this->y = y; }
        Vector(double array[2]) { x = array[0]; y = array[1]; }
    
        void operator +=(const Vector &v) { x += v.x; y += v.y; }
        void operator -=(const Vector &v) { x -= v.x; y -= v.y; }
        void operator *=(double f) { x *= f; y *= f; }
        void operator /=(double f) { x /= f; y /= f; }
        Vector operator +(const Vector &v) const { Vector n; n.x = x + v.x; n.y = y + v.y; return n; }
        Vector operator -(const Vector &v) const { Vector n; n.x = x - v.x; n.y = y - v.y; return n; }
        Vector operator /(double f) const { Vector n; n.x = x/f; n.y = y/f; return n; }
        Vector operator *(double f) const { Vector n; n.x = x*f; n.y = y*f; return n; }
        Vector operator -() const { return Vector(-x, -y); }
    
        double operator *(const Vector &v) const { return x*v.x + y*v.y; }
        double norm(void) const { return sqrt(x*x + y*y); }
        double norm2(void) const { return x*x+y*y; }
        double cross(const Vector &v) const { return x * v.y - y * v.x; }
        Vector unitary(void) const { if (norm() < std::numeric_limits<double>::epsilon()) return Vector(); return *this / norm(); }
        double angle(void) const { return atan2(y, x); }
        Vector perp(void) const { return Vector(-y, x); }
        
        Vector crossWithZVector(double l) const { return Vector(y * l, -x * l); }
        Vector crossFromZVector(double l) const { return Vector(-y * l, x * l); }
        
        bool operator <(const Vector& that) const { if (this->x == that.x) return (this->y < that.y); else return (this->x < that.x); }
    };
    
    std::ostream & operator << (std::ostream & outs, const Vector &vector);
    

    typedef Vector Point;
    

    struct Matrix22
    {
        // line-column component
        double _11;
        double _21;
        double _12;
        double _22;
    
        Matrix22() { _11 = _21 = _12 = _22 = 0; }
        Matrix22(double _11, double _21, double _12, double _22) { this->_11 = _11; this->_21 = _21; this->_12 = _12; this->_22 = _22; }
        Matrix22(double alpha) { _11 = cos(alpha); _21 = sin(alpha); _12 = -_21; _22 = _11; }
        Matrix22(double array[4]) { _11=array[0]; _21=array[1]; _12=array[2]; _22=array[3]; }
        
        void zeros() { _11 = _21 = _12 = _22 = 0; }
        
        void operator +=(const Matrix22 &v) { _11 += v._11; _21 += v._21; _12 += v._12; _22 += v._22; }
        void operator -=(const Matrix22 &v) { _11 -= v._11; _21 -= v._21; _12 -= v._12; _22 -= v._22; }
        void operator *=(double f) { _11 *= f; _21 *= f; _12 *= f; _22 *= f; }
        void operator /=(double f) { _11 /= f; _21 /= f; _12 /= f; _22 /= f; }
        Matrix22 operator +(const Matrix22 &v) const { Matrix22 n; n._11 = _11 + v._11; n._21 = _21 + v._21; n._12 = _12 + v._12; n._22 = _22 + v._22; return n; }
        Matrix22 operator -(const Matrix22 &v) const { Matrix22 n; n._11 = _11 - v._11; n._21 = _21 - v._21; n._12 = _12 - v._12; n._22 = _22 - v._22; return n; }
        Matrix22 operator *(double f) const { Matrix22 n; n._11 = _11 * f; n._21 = _21 * f; n._12 = _12 * f; n._22 = _22 * f; return n; }
        Matrix22 operator /(double f) const { Matrix22 n; n._11 = _11 / f; n._21 = _21 / f; n._12 = _12 / f; n._22 = _22 / f; return n; }
        Matrix22 transpose() const { Matrix22 n; n._11 = _11; n._21 = _12; n._12 = _21; n._22 = _22; return n; }
        
        Point operator*(const Point &v) const { Point n; n.x = v.x*_11 + v.y*_12; n.y = v.x*_21 + v.y*_22; return n; }
        
        static Matrix22 fromDiag(double _1, double _2 ) { return Matrix22(_1, 0, 0, _2); }
        static Matrix22 identity() { return fromDiag(1,1); }
    };
    

    struct Segment
    {
        Segment(double ax, double ay, double bx, double by) { this->a.x = ax; this->a.y = ay; this->b.x = bx; this->b.y = by; }
        Segment(double array[4]) { a.x = array[0]; a.y = array[1]; b.x = array[2]; b.y = array[3]; }
        Segment(const Point &p1, const Point &p2) { a = p1; b = p2; }
        
        Point a;
        Point b;
    
        double dist(const Point &p) const
        {
            const Vector n(a.y-b.y, b.x-a.x);
            const Vector u = n.unitary();
            const Vector ap = p-a;
            return ap * u;
        }
    
        bool doesIntersect(const Segment &o) const
        {
            const double s2da = dist (o.a);
            const double s2db = dist (o.b);
            const double s1da = o.dist (a);
            const double s1db = o.dist (b);
            return (s2da*s2db<0) && (s1da*s1db<0);
        }
        
        Point getMiddlePoint() const
        {
            return (a + b) / 2;
        }
    };
    

    struct Polygone: public std::vector<Point>
    {
        Segment getSegment(size_t i) const
        {
            return Segment((*this)[i], (*this)[(i + 1) % size()]);
        }
        
        bool isPointInside(const Point& p) const
        {
            for (size_t i = 0; i < size(); i++)
            {
                if (getSegment(i).dist(p) < 0)
                    return false;
            }
            return true;
        }
        
        bool getAxisAlignedBoundingBox(Point& bottomLeft, Point& topRight) const
        {
            if (empty())
                return false;
            
            bottomLeft = (*this)[0];
            topRight = (*this)[0];
            
            extendAxisAlignedBoundingBox(bottomLeft, topRight);
            
            return true;
        }
        
        void extendAxisAlignedBoundingBox(Point& bottomLeft, Point& topRight) const
        {
            for (const_iterator it = begin(); it != end(); ++it)
            {
                const Point& p = *it;
                
                if (p.x < bottomLeft.x)
                    bottomLeft.x = p.x;
                else if (p.x > topRight.x)
                    topRight.x = p.x;
                
                if (p.y < bottomLeft.y)
                    bottomLeft.y = p.y;
                else if (p.y > topRight.y)
                    topRight.y = p.y;
            }
        }
        
        double getBoundingRadius() const
        {
            double radius = 0;
            for (size_t i = 0; i < size(); i++)
                radius = std::max<double>(radius, (*this)[i].norm());
            return radius;
        }
        
        void translate(const Vector& delta)
        {
            for (iterator it = begin(); it != end(); ++it)
                *it += delta;
        }
        
        void translate(const double x, const double y)
        {
            translate(Vector(x,y));
        }
        
        void rotate(const double angle)
        {
            Matrix22 rot(angle);
            for (iterator it = begin(); it != end(); ++it)
                *it = rot * (*it);
        }
        
        void flipX()
        {
            for (size_t i = 0; i < size(); i++)
                (*this)[i].x = -(*this)[i].x;
            for (size_t i = 0; i < size() / 2; i++)
            {
                Point p = (*this)[i];
                (*this)[i] = (*this)[size() - i - 1];
                (*this)[size() - i - 1] = p;
            }
        }
        
        void flipY()
        {
            for (size_t i = 0; i < size(); i++)
                (*this)[i].y = -(*this)[i].y;
            for (size_t i = 0; i < size() / 2; i++)
            {
                Point p = (*this)[i];
                (*this)[i] = (*this)[size() - i - 1];
                (*this)[size() - i - 1] = p;
            }
        }
        
        Polygone& operator << (const Point& p) { push_back(p); return *this; }
    };
    

    std::ostream & operator << (std::ostream & outs, const Polygone &polygone);
    

    inline double normalizeAngle(double angle)
    {
        while (angle > M_PI)
            angle -= 2*M_PI;
        while (angle < -M_PI)
            angle += 2*M_PI;
        return angle;
    }
    
    #if 0
    // NOTE: unused for now
    inline double getTriangleArea(const Point &a, const Point &b, const Point &c)
    {
        return (a.x-c.x) * (b.y-c.y) - (a.y-c.y) * (b.x-c.x);
    }
    #endif
  

    inline Point getIntersection(const Segment &s1, const Segment &s2)
    {
        // compute first segment's equation
        const double c1 = s1.a.y + (-s1.a.x / (s1.b.x - s1.a.x)) * (s1.b.y - s1.a.y);
        const double m1 = (s1.b.y - s1.a.y) / (s1.b.x - s1.a.x);
 
        // compute second segment's equation
        const double c2 = s2.a.y + (-s2.a.x / (s2.b.x - s2.a.x)) * (s2.b.y - s2.a.y);
        const double m2 = (s2.b.y - s2.a.y) / (s2.b.x - s2.a.x);

        // are the lines parallel ?
        if (m1 == m2)
            return Point(HUGE_VAL, HUGE_VAL);

        double x1 = s1.a.x;
        double x2 = s1.b.x;
        double x3 = s2.a.x;
        double x4 = s2.b.x;
        double y1 = s1.a.y;
        double y2 = s1.b.y;
        double y3 = s2.a.y;
        double y4 = s2.b.y;

        // make sure x1 < x2
        if (x1 > x2)
        {
            double temp = x1;
            x1 = x2;
            x2 = temp;
        }

        // make sure x3 < x4
        if (x3 > x4)
        {
            double temp = x3;
            x3 = x4;
            x4 = temp;
        }

        // make sure y1 < y2
        if (y1 > y2)
        {
            double temp = y1;
            y1 = y2;
            y2 = temp;
        }

        // make sure y3 < y4
        if (y3 > y4)
        {
            double temp = y3;
            y3 = y4;
            y4 = temp;
        }

        // intersection point in case of infinite slopes
        double x;
        double y;

        // infinite slope m1
        if (x1 == x2)
        {
            x = x1;
            y = m2 * x1 + c2;
            if (x > x3 && x < x4 && y > y1 && y <y2)
                return Point(x, y);
            else
                return Point(HUGE_VAL, HUGE_VAL);
        }

        // infinite slope m2
        if (x3 == x4)
        {
            x = x3;
            y = m1 * x3 + c1;
            if (x > x1 && x < x2 && y > y3 && y < y4)
                return Point(x, y);
            else
                return Point(HUGE_VAL, HUGE_VAL);
        }
        
        // compute lines intersection point
        x = (c2 - c1) / (m1 - m2);

        // see whether x in in both ranges [x1, x2] and [x3, x4]
        if (x > x1 && x < x2 && x > x3 && x < x4)
            return Point(x, m1 * x + c1);
  
        return Point(HUGE_VAL, HUGE_VAL);
    }
    

    inline double getTriangleAreaTwice(const Point &a, const Point &b, const Point &c)
    {
        return (a.x - c.x) * (b.y - c.y) - (a.y - c.y) * (b.x - c.x);
    }
    

    inline double getTriangleHeight(const Point &a, const Point &b, const Point &c)
    {
        const double ba_norm = (b-a).norm();
        if (ba_norm < std::numeric_limits<double>::epsilon())
            return 0;
        
        // area of the parallelogram divided by the length of the base
        return getTriangleAreaTwice(a, b, c) / ba_norm;
    }
}

#endif