trigo.h

Go to the documentation of this file.

/* ***********************************************************************************
        Writer:         Sebastien Bloc
        Copyright:      2003-2004
        eMail:          sebastien.bloc@free.fr
        URL:            http://mignonsoft.free.fr

        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.
        http://www.gnu.org/copyleft/gpl.html
*********************************************************************************** */

#ifndef INCLUDE_TRIGO_H
#define INCLUDE_TRIGO_H

#include <math.h>
#include <windows.h> // POINT

#define PI                              3.1415926535f
#define DOUBLE_PI               (PI*2.0f)
#define HALF_PI                 (PI/2.0f)
#define PRECISION               0.0001      // pour les calculs de IsOnXXXXX() : valeur par defaut
#define DEGTORAD(ang) ((ang) * PI / 180.0)
#define RADTODEG(ang) ((ang) * 180.0/ PI) 

double  Bound2_PI(double angle);
BOOL    GetTrigoSens(POINT *pt);
BOOL    GetTrigoSens(POINT *pt,int nbPoints);
void    ChangeTrigoSens(POINT *pt,int nbPoints);
int             GetNearExp2(int num);

template <class T> class Point3D; 
template <class T> class Point1D 
{
public:
        T value;

public:
        Point1D() { Zero(); }
        Point1D(T _value) { value=_value; }
        void Zero() { value=0; }
        BOOL operator==(Point1D<T> &src) { return (value==src.value); }
        void operator=(Point1D<T> &src) { value=src.value; }
        void operator=(T _value) { value=_value; }
        //void operator+=(T _value) { value+=_value; }
        //void operator-=(T _value) { value-=_value; }
        //void operator/=(T _value) { value/=_value; }
        //void operator*=(T _value) { value*=_value; }
        T operator+(T _value) { T t; t = value+_value; return t; }
        T operator-(T _value) { T t; t = value-_value; return t; }
        double operator/(double coef) { return value/coef; }
        double operator*(double coef) { return value*coef; }
        operator T() const { return value; }
        double LinearInterpolation(T value1,T value2,double alpha);
};

template <class T> class Point2D 
{
public:
        T x;
        T y;

public:
        Point2D() { Zero(); }
        Point2D(T _x,T _y) { Set(_x,_y); }
        void Set (T _x,T _y) { x=_x; y=_y; }
        void Zero() { x=0; y=0; }
        BOOL operator==(Point2D<T> &src) { return (x==src.x)&&(y==src.y); }
        BOOL operator!=(Point2D<T> &src) { return !((x==src.x)&&(y==src.y)); }
        void operator=(Point3D<T> &src);
        void operator=(Point2D<T> &src) { x=src.x; y=src.y; }
        void operator+=(Point2D<T> &src) { x+=src.x; y+=src.y; }
        Point2D<T> operator-(Point2D<T> &src);
        Point2D<T> operator+(Point2D<T> &src);
        Point2D<T> operator*(double coef);
        Point2D<T> operator/(double coef);
        void operator/=(double coef) { x/=coef; y/=coef;        }
        void operator*=(double coef) { x*=coef; y*=coef;        }
        double Power() { return sqrt(x*x+y*y); }

        BOOL IsOnSegment(Point2D<T> &a,Point2D<T> &b,double *alpha=NULL);
        BOOL IsOnLine(Point2D<T> &a,Point2D<T> &b,double *alpha=NULL,double precision=PRECISION);
        BOOL IsOnTriangle(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,double *alphaBC,double *alphaAPP,Point2D<T> *pp=NULL);
        BOOL IsOnQuadrilatere(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,Point2D<T> &d,double *alphaX=NULL,double *alphaY=NULL);
        BOOL IsOnRectangle(Point2D<T> &a,Point2D<T> &c,double *alphaX=NULL,double *alphaY=NULL);

        BOOL GetRelativePtOnQuadrilatere(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,Point2D<T> &d,double *deltaX,double *deltaY);
        BOOL GetRelativePtOn2Lines(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,Point2D<T> &d,double *alpha);

        void ToNormalVector(BOOL asTrigo=TRUE);
};


#include "include/quaternion.h"

template <class T> class Point3D 
{
public:
        T x;
        T y;
        T z;

public:
        Point3D() { Zero(); }
        Point3D(T _x,T _y,T _z) { Set(_x,_y,_z); }

        void Set (T _x,T _y,T _z=0) { x=_x; y=_y; z=_z; }
        void Zero() { x=0; y=0; z=0; }
        BOOL IsEqual(T _x,T _y,T _z) { return (x==_x)&&(y==_y)&&(z==_z); }
        BOOL operator==(Point3D<T> &src) { return (x==src.x)&&(y==src.y)&&(z==src.z); }
        void operator=(Point3D<T> &src);
        void Add(T _x,T _y,T _z);
        void operator+=(Point3D<T> &src);
        void operator/=(double divide);

        void glRotated(double ratio);
        void glRotated();
        void glTranslated(double ratio);
        void glTranslated();
        void glRasterPos() { glRasterPos3d(x,y,z); }
        void glScale() { glScaled(x,y,z); }

        double Power() { return sqrt(x*x+y*y+z*z); }
        Point3D<T> QuaternionRotate (Point3D<T> &rotate);
        void operator=(Point2D<T> &src);
        BOOL IsOnTriangle(Point3D<T> &a,Point3D<T> &b,Point3D<T> &c);
        BOOL IsNotNull() { return (x||y||z)?TRUE:FALSE; }
};

template <class T> 
class LinearEqu2D
{
private:
        void Compute()
        {
                a=pt1.y-pt2.y;
                b=pt2.x-pt1.x;
                c=-(pt1.x*a+pt1.y*b); // <=> xa(yb-ya)-ya(xb-xa)
        }

        double a,b,c;
        Point2D<T> pt1,pt2;

public:
        LinearEqu2D() { Zero(); }
        LinearEqu2D(Point2D<T> &_pt1,Point2D<T> &_pt2) { Set(_pt1,_pt2); }

        void Zero()
        {
                pt1.Zero();
                pt2.Zero();
                a=b=c=0;
        }

        void Set(Point2D<T> &_pt1,Point2D<T> &_pt2)
        {
                pt1=_pt1;
                pt2=_pt2;
                Compute();
        }

        BOOL IsPoint() { return a||b?TRUE:FALSE; }
        BOOL IsVertical() { return a&&!b?TRUE:FALSE; }
        BOOL IsHorizontal() { return !a&&b?TRUE:FALSE; }

        BOOL GetCrossPtFromVertical(T x,Point2D<T> *pt=NULL)
        {       // donne le point a l'intesections de la ligne en cours et une vertical donnée
                Point2D<T> ptv1(x,0);
                Point2D<T> ptv2(x,10);          
                LinearEqu2D<T> vertEqu(ptv1,ptv2);
                return GetCrossPtFromLine(vertEqu,pt);
        }

        BOOL GetCrossPtFromHorizontal(T y,Point2D<T> *pt=NULL)
        {       // donne le point a l'intesections de la ligne en cours et une vertical donnée
                Point2D<T> pth1(0,y);
                Point2D<T> pth2(10,y);          
                LinearEqu2D<T> horiEqu(pth1,pth2);
                return GetCrossPtFromLine(horiEqu,pt);
        }
        BOOL GetCrossPtFromLine(LinearEqu2D<T> &equ,Point2D<T> *pt=NULL)
        {   // donne le point a l'interections des 2 lignes definis par leurs equation
                Point2D<T> _pt;
                if (!pt) pt=&_pt;
                if (!ComputePtFromeLine(*this,equ,pt)) 
                        return ComputePtFromeLine(equ,*this,pt);
                else return TRUE;
                return TRUE;
        }

private:
        BOOL ComputePtFromeLine(LinearEqu2D<T> &equ1,LinearEqu2D<T> &equ2,Point2D<T> *pt)
        {
                T denominateur = ((equ2.a*equ1.b)-(equ1.a*equ2.b));
                if (!denominateur) return FALSE; // pas de solution
                pt->y = ((equ1.a*equ2.c)-(equ2.a*equ1.c))/denominateur;
                if (!equ2.a) return FALSE;// la 2 eme equations et horizontal donc une infinité de solution sur equ
                pt->x = ((equ2.b*pt->y)+equ2.c)/-equ2.a;
                return TRUE;
        }
};

// Point1D implementation ********************************************************

template <class T>
double Point1D<T>::LinearInterpolation(T value1,T value2,double alpha)
{
        value= value1+(double)(value2-value1)*alpha;
        return value;
}

// Point2D implementation ********************************************************

template <class T>
Point2D<T> Point2D<T>::operator-(Point2D<T> &src)
{
        Point2D<T> pt;
        pt.x=x-src.x;
        pt.y=y-src.y;
        return pt;
}

template <class T>
Point2D<T> Point2D<T>::operator+(Point2D<T> &src)
{
        Point2D<T> pt;
        pt.x=x+src.x;
        pt.y=y+src.y;
        return pt;
}

template <class T>
Point2D<T> Point2D<T>::operator*(double coef)
{
        Point2D<T> pt;
        pt.x=(T)(x*coef);
        pt.y=(T)(y*coef);
        return pt;
}

template <class T>
Point2D<T> Point2D<T>::operator/(double coef)
{
        Point2D<T> pt;
        pt.x=(T)(x/coef);
        pt.y=(T)(y/coef);
        return pt;
}

template <class T>
void Point2D<T>::operator=(Point3D<T> &src)
{
        x=src.x;
        y=src.y;
}

/* Determine si un point (p) est sur la tragectoire d'un segment [ab] ******************
Si un pointeur alhpa est donnée, et si il y a interesection, donne le pourcentage
de la position d'intersection par rapport ou point (a)
Ex.:
alpha = 0; p = a
alpha = 1; p = b
alpha = 0.5; p.x= (b.x+a.x)/2; p.y = (b.y+a.y)/2
************************************************************************************* */

template <class T>
BOOL Point2D<T>::IsOnSegment(Point2D<T> &a,Point2D<T> &b,double *alpha)
{
        double _alpha;
        if (!alpha) alpha=&_alpha;
        if (IsOnLine(a,b,alpha)) return ((*alpha>=0)&&(*alpha<=1))?TRUE:FALSE;
        return FALSE;
}

template <class T>
BOOL Point2D<T>::IsOnLine(Point2D<T> &a,Point2D<T> &b,double *alpha,double precision)
{
        double dx = b.x-a.x;
        double dy = b.y-a.y;
        if (!dx&&!dy)
        {       // a et b sont confondu
                if (a==*this)
                {
                        if (alpha) *alpha = 0; // en fait infinit de solution car les 2 points sont confondu
                        return TRUE;
                }
                return FALSE;
        }

        double dxap = x-a.x;
        double dyap = y-a.y;
        double reste=dxap*dy-dyap*dx;
        if (fabs(reste)<=precision)
        {
                if (alpha) 
                {
                        if (dx) *alpha = dxap/dx;
                        else if (dy) *alpha = dyap/dy;
                        else *alpha = 0; // en fait infinit de solution car les 2 points sont confondu
                }
                return TRUE;
        }
        return FALSE;
}

/* consitere le point comme un vecteur, on peut alors en faire une rotation a 90° ******
en sens trigonometirque ou horaire
************************************************************************************* */

template <class T>
void Point2D<T>::ToNormalVector(BOOL asTrigo)
{
        T temp = x;
        if (asTrigo)
        {
                x = -y;
                y = temp;
        }
        else 
        {
                x = y;
                y = -temp;
        }
}

template <class T>
BOOL Point2D<T>::IsOnTriangle(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,double *alphaBC,double *alphaAPP,Point2D<T> *pp)
{
        Point2D<T> _pp;
        Point1D<T> interpolation;
        double _alphaBC,_alphaAPP;
        if (!alphaAPP) alphaAPP=&_alphaAPP;
        if (!alphaBC) alphaBC=&_alphaBC;
        if (!pp) pp=&_pp;

        // etape 1: calcul du point PP
        LinearEqu2D<T> equAP(a,*this);  // equation de [AP]
        LinearEqu2D<T> equBC(b,c);              // equation de [BC]
        if (!equAP.GetCrossPtFromLine(equBC,pp)) return FALSE;
        // etape 2: calcul de l'alpha de P' sur segment [BC]
        if (!pp->IsOnSegment(b,c,alphaBC)) return FALSE;
        // etape 3: calcul de l'alpha de P sur segment [AP']
        if (!IsOnSegment(a,*pp,alphaAPP)) return FALSE;
        return TRUE;
}


template <class T>
BOOL Point2D<T>::IsOnRectangle(Point2D<T> &a,Point2D<T> &c,double *alphaX,double *alphaY)
{
        Point2D<T> b(c.x,a.y),Point2D<T> d(a.x,c.y);
        return IsOnQuadrilatere(a,b,c,d,alphaX,alphaY);
}


template <class T>
BOOL Point2D<T>::IsOnQuadrilatere(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,Point2D<T> &d,double *alphaX,double *alphaY)
{
        double _alphaX,_alphaY;
        if (!alphaX) _alphaX=&alphaX;
        if (!alphaY) _alphaY=&alphaY;
        BOOL res = GetRelativePtOnQuadrilatere(a,b,c,d,&_alphaX,&_alphaY);
        return (!res||*alphaX<0||*alphaY<0)?FALSE:TRUE; 
}

template <class T>
BOOL Point2D<T>::GetRelativePtOnQuadrilatere(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,Point2D<T> &d,double *alphaX,double *alphaY)
{
        double _alphaX,_alphaY;
        if (!alphaX) alphaX=&_alphaX;
        if (!alphaY) alphaY=&_alphaY;

        // calcul de alpha
        if (!GetRelativePtOn2Lines(a,b,d,c,alphaX)) return FALSE;
        if (!GetRelativePtOn2Lines(a,d,b,c,alphaY)) return FALSE;
        return TRUE;
}

template <class T>
BOOL Point2D<T>::GetRelativePtOn2Lines(Point2D<T> &a,Point2D<T> &b,Point2D<T> &c,Point2D<T> &d,double *alpha)
{
        double _alpha;
        if (!alpha) alpha=&_alpha;

        // calcul des constante de l'eqation: _a*alpha^2+_b*alpha+_c=0
        double _a,_b,_c;
        _a = 
                (a.y-b.y)*(d.x-c.x)
                -(a.x-b.x)*(d.y-c.y);
        _b = 
                (a.y-b.y)*(c.x-a.x)
                -(a.x-b.x)*(c.y-a.y)
                +(a.x-b.x-c.x+d.x)*(y-a.y)
                -(a.y-b.y-c.y+d.y)*(x-a.x);
        _c = 
                (y-a.y)*(c.x-a.x)
                -(x-a.x)*(c.y-a.y);

        // resolution des eventuel solution de alpha
        if (!_a) 
        {       // l'equation n'est plus du 2°degrée donc un solution simple
                if (!_b) return FALSE;
                *alpha=-_c/_b;
        }
        else
        {       // l'eqution est du 2°degrée 
                // solution1 donne P inclus dans [P1,P2]
                // solution2 donné apres intersection des 2 droites
                double delta=_b*_b-4.0*_a*_c;
                if (delta<0) return FALSE; // pas de solution
                double alpha1,alpha2;
                double sqrtdelta = sqrt(delta);
                alpha1 = (-_b-sqrtdelta)/(2.0*_a); 
                alpha2 = (-_b+sqrtdelta)/(2.0*_a); 

                if (fabs(alpha1)<fabs(alpha2)) *alpha=alpha1;
                else *alpha=alpha2;
                //Point2D<double> p1((b.x-a.x)*alpha1+a.x,(b.y-a.y)*alpha1+a.y);
                //Point2D<double> p2((d.x-c.x)*alpha1+c.x,(d.y-c.y)*alpha1+c.y);
                //if (IsOnLine(p1,p2)) *alpha = alpha1; 
                //else *alpha = alpha2;
                /*
                // test alpha pour savoir si P1=P2
                double precision_rest = 0.000001;
                double restX = alpha1*(b.x-a.x-d.x+c.x)-c.x+a.x;
                double restY = alpha1*(b.y-a.y-d.y+c.y)-c.y+a.y;
                if (fabs(restX)<precision_rest&&fabs(restY)<precision_rest) 
                         *alpha = alpha2; // alpha1 fait que P1=P2
                else *alpha = alpha1; // alpha2 fait que P1=P2
                */
        }
        return TRUE;
}


// Point3D implementation ********************************************************

template <class T>
Point3D<T> Point3D<T>::QuaternionRotate (Point3D<T> &rotate)
{               
        return *this;   
        Quaternion qbase,qrotate;

        qbase.from_euler(*this);
        qrotate.from_euler(rotate);
        qbase*=qrotate;
        return qbase.to_euler();
}

template <class T>
BOOL Point3D<T>::IsOnTriangle(Point3D<T> &a,Point3D<T> &b,Point3D<T> &c)
{
        Point2D<T> _p,_a,_b,_c,_pp;
        double alphaBC,alphaAPP;
        _p=this;
        _a=a;
        _b=b;
        _c=c;
        if (!_p.IsOnTriangle(_a,_b,_c,&alphaBC,&alphaAPP,&_pp)) return FALSE;
        Point3D pp;
        pp=_pp;
        pp.z.LinearInterpolation(b.z,c.z,alphaBC);
        z.LinearInterpolation(a.z,pp.z,alphaAPP);
        return TRUE;
}

template <class T>
void Point3D<T>::operator= (Point2D<T> &src)
{
        x=src.x;
        y=src.y;
        z=0;
}

template <class T>
void Point3D<T>::operator=(Point3D<T> &src)
{
        x=src.x;
        y=src.y;
        z=src.z;
}

template <class T>
void Point3D<T>::Add(T _x,T _y,T _z)
{
        x+=_x;
        y+=_y;
        z+=_z;
}

template <class T>
void Point3D<T>::operator+=(Point3D<T> &src)
{
        x+=src.x;
        y+=src.y;
        z+=src.z;
}

template <class T>
void Point3D<T>::operator/=(double divide)
{
        x/=divide;
        y/=divide;
        z/=divide;
}

template <class T>
void Point3D<T>::glRotated(double ratio)
{
        if (!ratio) return;
        if (ratio==1) glRotated();
        else
        {
                if (x) ::glRotated(x*ratio,1,0,0);
                if (y) ::glRotated(y*ratio,0,1,0);
                if (z) ::glRotated(z*ratio,0,0,1);
        }
}

template <class T>
void Point3D<T>::glRotated()
{
        if (x) ::glRotated(x,1,0,0);
        if (y) ::glRotated(y,0,1,0);
        if (z) ::glRotated(z,0,0,1);
}

template <class T>
void Point3D<T>::glTranslated(double ratio)
{
        if (!ratio) return;
        if (x||y||z) ::glTranslated(x*ratio,y*ratio,z*ratio);
}

template <class T>
void Point3D<T>::glTranslated()
{
        if (x||y||z) ::glTranslated(x,y,z);
}

#endif

---