Geometry.h

Go to the documentation of this file.

/*
    liban - base function to do usefull things related to simulation and scientific work
    Copyright (C) 1999-2005 Stephane Magnenat <stephane at magnenat dot net>
    Copyright (c) 2004-2005 Antoine Beyeler <antoine dot beyeler at epfl dot ch>
    Copyright (C) 2005 Laboratory of Intelligent Systems, EPFL, Lausanne
    See AUTHORS for details

    This program is free software. You can redistribute it 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 __AN_GEOMETRY_H
#define __AN_GEOMETRY_H

#ifdef WIN32
#define _USE_MATH_DEFINES
#endif
#include <cmath>
#include <vector>


namespace An
{

    struct Vector
    {
        double x, 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; }
    
        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 { Vector n; if (norm()) { n = *this; n /= norm(); } return n; }
        double angle(void) const { return atan2(y, x); }
    };
    

    typedef Vector Point;
    

    struct Matrix22
    {
        double a, b, c, d;
    
        Matrix22() { a = b = c = d = 0; }
        Matrix22(double a, double b, double c, double d) { this->a = a; this->b = b; this->c = c; this->d = d; }
        Matrix22(double alpha) { a = cos(alpha); b = sin(alpha); c = -b; d = a; }
        Matrix22(double array[4]) { a=array[0]; b=array[1]; c=array[2]; d=array[3]; }
    
        void operator +=(const Matrix22 &v) { a += v.a; b += v.b; c += v.c; d += v.d; }
        void operator -=(const Matrix22 &v) { a -= v.a; b -= v.b; c -= v.c; d -= v.d; }
        void operator *=(double f) { a *= f; b *= f; c *= f; d *= f; }
        void operator /=(double f) { a /= f; b /= f; c /= f; d /= f; }
        Matrix22 operator +(const Matrix22 &v) const { Matrix22 n; n.a = a + v.a; n.b = b + v.b; n.c = c + v.c; n.d = d + v.d; return n; }
        Matrix22 operator -(const Matrix22 &v) const { Matrix22 n; n.a = a - v.a; n.b = b - v.b; n.c = c - v.c; n.d = d - v.d; return n; }
        Matrix22 operator *(double f) const { Matrix22 n; n.a = a * f; n.b = b * f; n.c = c * f; n.d = d * f; return n; }
        Matrix22 operator /(double f) const { Matrix22 n; n.a = a / f; n.b = b / f; n.c = c / f; n.d = d / f; return n; }
        
        Point operator*(const Point &v) { Point n; n.x = v.x*a + v.y*c; n.y = v.x*b + v.y*d; return n; }
    };
    

    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, b;
    
        double dist(const Point &p) const
        {
            Vector n(a.y-b.y, b.x-a.x);
            Vector u = n.unitary();
            Vector ap = p-a;
            return ap * u;
        }
    
        bool doesIntersect(const Segment &o) const
        {
            double s2da = dist (o.a);
            double s2db = dist (o.b);
            double s1da = o.dist (a);
            double s1db = o.dist (b);
            return (s2da*s2db<0) && (s1da*s1db<0);
        }
    };
    

    typedef std::vector<Point> Polygone;
    

    inline double normalizeAngle(double angle)
    {
        while (angle > M_PI)
            angle -= 2*M_PI;
        while (angle < -M_PI)
            angle += 2*M_PI;
        return angle;
    }
  

    inline Point getIntersection(const Segment &s1, const Segment &s2)
    {
        // compute first segment's equation
        double c1 = s1.a.y + (-s1.a.x / (s1.b.x - s1.a.x)) * (s1.b.y - s1.a.y);
        double m1 = (s1.b.y - s1.a.y) / (s1.b.x - s1.a.x);
 
        // compute second segment's equation
        double c2 = s2.a.y + (-s2.a.x / (s2.b.x - s2.a.x)) * (s2.b.y - s2.a.y);
        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);
    }
}

#endif

---