3d.h

Go to the documentation of this file.

/*
 * Copyright (C) <2012-2014> <EDF-R&D> <FRANCE>
 * 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.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */
 
#ifndef TY_MC_3D
#define TY_MC_3D
 
#include <cmath>
#include <math.h>
#include <vector>
#include <ostream>
 
// Defines for intersection calculations.
#define INTERS_CONFONDU 2
#define INTERS_OUI 1
#define INTERS_NULLE 0
 
#define EPSILON_3 0.001            // 10e-3
#define EPSILON_5 0.00001          // 10e-5
#define EPSILON_6 0.000001         // 10e-6
#define EPSILON_7 0.0000001        // 10e-7
#define EPSILON_13 0.0000000000001 // 10e-13
#define EPSILON_50 1.e-50
 
/* From this threshold (in meters), two elements are considered as at the same
 * position
 * */
#define TYSEUILCONFONDUS EPSILON_3 // 1 mm
 
#ifndef M_PI
    #define M_PI 3.1415926535897932384626433832795
#endif
 
#define M_2PI 6.283185307179586476925287766559
 
// Macros
 
inline double ABS(double a)
{
    return (a >= 0.0 ? a : -a);
}
 
inline double SIGNE(double a)
{
    return (a >= 0.0 ? 1 : -1);
}
 
inline double MAX(double a, double b)
{
    return (a > b ? a : b);
}
 
inline double MAX3(double a, double b, double c)
{
    return MAX(MAX(a, b), c);
}
 
inline double MIN(double a, double b)
{
    return (a < b ? a : b);
}
 
inline double DEGTORAD(double a)
{
    return (a / 180.0 * M_PI);
}
 
inline double RADTODEG(double a)
{
    return (a / M_PI * 180.0);
}
 
inline double GRATORAD(double a)
{
    return (a / 200.0 * M_PI);
}
 
inline double RADTOGRA(double a)
{
    return (a / M_PI * 200.0);
}
 
inline double GRATODEG(double a)
{
    return (a / 200.0 * 180.0);
}
 
inline double DEGTOGRA(double a)
{
    return (a / 180.0 * 200.0);
}
 
inline int ROUND(double a)
{
    return (a >= 0.0 ? (int)(a + 0.5) : (int)(a - 0.5));
}
 
inline double BORNE(double a, double b, double c)
{
    return (a < b ? b : (a > c ? c : a));
}
 
inline int BORNE(int a)
{
    return (int)BORNE((double)a, 0, 255);
}
 
class OCoord3D
{
public:
    OCoord3D();
    OCoord3D(const OCoord3D& coord);
    OCoord3D(double x, double y, double z);
    virtual ~OCoord3D();
 
    OCoord3D& operator=(const OCoord3D& coord);   
    bool operator==(const OCoord3D& coord) const; 
    bool operator!=(const OCoord3D& coord) const; 
 
    void setCoords(double x, double y, double z);
    void setCoords(double coords[3]);
    void getCoords(double coords[3]);
    double* getCoords();
 
    operator const double*() const
    {
        return &_value[0];
    }
    operator double*()
    {
        return &_value[0];
    }
 
public:
    union
    {
        struct
        {
            double _x; 
            double _y; 
            double _z; 
        };
        double _value[3];
    };
};
 
::std::ostream& operator<<(::std::ostream& os, const OCoord3D& c);
 
class OVector3D : public OCoord3D
{
public:
    OVector3D();
    OVector3D(const OVector3D& vector);
    OVector3D(const OCoord3D& coord);
    OVector3D(const OCoord3D& coordFrom, const OCoord3D& coordTo);
    OVector3D(double x, double y, double z);
    virtual ~OVector3D();
 
    void reset();
    OVector3D& operator=(const OVector3D& vector);
    bool operator==(const OVector3D& vector) const;
    bool operator!=(const OVector3D& vector) const;
    OVector3D operator+(const OVector3D& vector) const;
    OVector3D operator-(const OVector3D& vector) const;
    // XXX meaningless component wise multiplication whereas a dot product would have been expected.
    OVector3D operator*(const OVector3D& vector) const;
    OVector3D operator*(const double a) const;
    friend OVector3D operator*(const double a, const OVector3D& vector);
 
    double dot(const OVector3D& v)
    {
        return _x * v._x + _y * v._y + _z * v._z;
    }
 
    OVector3D cross(const OVector3D& vector) const;
 
    void balance(double c1, const OVector3D& vector2, double c2);
 
    double scalar(const OVector3D& vector) const;
 
    double norme() const;
 
    OVector3D normal(const OVector3D& vector2, const OVector3D& vector3) const;
 
    void normalize();
 
    void invert();
 
    double angle(const OVector3D& vector) const;
 
    OVector3D getRotationOz(double alpha);
 
    OVector3D getRotationOzBase2(double alpha);
 
    OVector3D getRotationOy(double alpha);
 
    OVector3D getRotationOyBase2(double alpha);
 
    OVector3D getRotationOzOy(double alpha, double theta);
};
inline OCoord3D operator+(const OCoord3D& coord, const OVector3D& vect)
{
    return OCoord3D(coord._x + vect._x, coord._y + vect._y, coord._z + vect._z);
}
 
::std::ostream& operator<<(::std::ostream& os, const OVector3D& v);
 
class OPoint3D;
typedef std::vector<OPoint3D> TabPoint3D;
 
class OPoint3D : public OCoord3D
{
    // Methods
public:
    OPoint3D();
 
    OPoint3D(const OPoint3D& pt);
 
    OPoint3D(const OCoord3D& coord);
 
    OPoint3D(double x, double y, double z);
 
    OPoint3D(double v[]);
 
    virtual ~OPoint3D();
 
    virtual void set(double x, double y, double z);
 
    virtual void setFromOGL(double x, double y, double z);
    virtual void setFromOGL(float x, float y, float z);
    virtual void setFromOGL(float coords[3]);
    virtual void setFromOGL(double coords[3])
    {
        setFromOGL(coords[0], coords[1], coords[2]);
    }
    virtual void getToOGL(double& x, double& y, double& z);
    virtual void getToOGL(double coords[3]);
 
    double distFrom(const OPoint3D& pt) const;
 
    double dist2DFrom(const OPoint3D& pt) const;
 
    bool isEqual(const OPoint3D& oPoint) const
    {
        return *this == oPoint;
    }
 
    static TabPoint3D checkPointsMaxDistance(const TabPoint3D& points, const double& distanceMax);
 
    static TabPoint3D checkPointsMaxDistance(const OPoint3D& point1, const OPoint3D& point2,
                                             const double& distanceMax);
};
 
::std::ostream& operator<<(::std::ostream& os, const OPoint3D& v);
 
class OMatrix
{
public:
    OMatrix();
    OMatrix(const OMatrix& matrix);
    OMatrix(double matrix[4][4]);
    virtual ~OMatrix();
    OMatrix& operator=(const OMatrix& matrix);
    bool operator==(const OMatrix& matrix) const;
    bool operator!=(const OMatrix& matrix) const;
    OMatrix operator+(const OMatrix& matrix) const;
    OMatrix operator-(const OMatrix& matrix) const;
    OMatrix operator*(const OMatrix& matrix) const;
 
    OVector3D multNormal(const OVector3D& normal) const;
 
    void show();
 
    void reset();
 
    void unite();
 
    int setTranslation(double x, double y, double z);
 
    int setScale(double x, double y, double z);
 
    int setRotationOx(double a);
 
    int setRotationOy(double a);
 
    int setRotationOz(double a);
 
    int aligneVecteurSurOx(const OVector3D& vector);
 
    int aligneVecteurSurOy(const OVector3D& vector);
 
    int invert();
 
    OMatrix getInvert(int* ok = 0) const;
 
    void adjoint();
 
    OMatrix getAdjoint();
 
    double determinant();
 
    static double mat2x2Det(double a, double b, double c, double d);
 
    static double mat3x3Det(double a1, double a2, double a3, double b1, double b2, double b3, double c1,
                            double c2, double c3);
 
    OCoord3D dot(const OCoord3D& coord) const;
 
    OCoord3D scale(const OCoord3D& coord) const;
 
    // Members
public:
    double _m[4][4];
};
 
OVector3D operator*(const OMatrix& mat, const OVector3D& vec);
OPoint3D operator*(const OMatrix& mat, const OPoint3D& pt);
 
class OGeometrie
{
public:
    static bool intersDemiSegmentAvecSegment(const OPoint3D& ptS, const OPoint3D& ptA, const OPoint3D& ptB);
 
    static int intersDroitesPoints(const OPoint3D& ptA, const OPoint3D& ptB, const OPoint3D& ptC,
                                   const OPoint3D& ptD, OPoint3D& ptI);
 
    static int intersDroitesPointVecteur(const OPoint3D& ptA, const OVector3D& vecA, const OPoint3D& ptB,
                                         const OVector3D& vecB, OPoint3D& ptI);
 
    static double symPointDroite(const OPoint3D& ptA, const OPoint3D& ptB, const OPoint3D& ptP,
                                 OPoint3D& ptI);
 
    static bool pointInPolygonAngleSum(const OPoint3D& ptP, const OPoint3D* pts, int nbPts);
 
    static bool pointInPolygonRayCasting(const OPoint3D& ptP, const OPoint3D* pts, int nbPts);
 
    static bool shortestSegBetween2Lines(const OPoint3D& pt1, const OPoint3D& pt2, const OPoint3D& pt3,
                                         const OPoint3D& pt4, OPoint3D& ptA, OPoint3D& ptB, double* mua,
                                         double* mub);
 
    static void boundingBox(OPoint3D* pts, int nbPts, OPoint3D& ptMin, OPoint3D& ptMax);
 
    static void computeNormal(OPoint3D* pts, int nbPts, OVector3D& normal);
};
 
class OSegment3D
{
public:
    OSegment3D();
    OSegment3D(const OSegment3D& other);
    OSegment3D(const OPoint3D& ptA, const OPoint3D& ptB);
    virtual ~OSegment3D();
 
    virtual OSegment3D& operator=(const OSegment3D& other); 
    virtual bool operator==(const OSegment3D& other) const; 
    virtual bool operator!=(const OSegment3D& other) const; 
 
    virtual OSegment3D operator*(const OMatrix& matrix) const;
 
    virtual double longueur() const;
 
    virtual int symetrieOf(const OPoint3D& pt, OPoint3D& ptSym) const;
 
    virtual int projection(const OPoint3D& pt, OPoint3D& ptProj, double seuilConfondus) const;
 
    virtual int intersects(const OSegment3D& seg, OPoint3D& pt, double seuilConfondus) const;
 
    virtual OPoint3D centreOf() const;
 
    virtual OPoint3D centerOfCurvedPath(const double& R) const;
 
    virtual double lengthOfCurvedPath(const double& R);
 
    virtual OVector3D toVector3D() const
    {
        return OVector3D(_ptA, _ptB);
    }
 
    virtual OSegment3D swap() const;
 
public:
    OPoint3D _ptA;
    OPoint3D _ptB;
};
 
class ORepere3D
{
public:
    ORepere3D();
    ORepere3D(const ORepere3D& repere);
    ORepere3D(const OPoint3D& origin, const OVector3D& vecI, const OVector3D& vecJ, const OVector3D& vecK);
    ORepere3D(const OPoint3D& origin, const OVector3D& vec);
    ORepere3D(const OMatrix& matrix);
    virtual ~ORepere3D();
 
    ORepere3D& operator=(const ORepere3D& repere);  
    bool operator==(const ORepere3D& repere) const; 
    bool operator!=(const ORepere3D& repere) const; 
 
    void set(const OPoint3D& origin, const OVector3D& vecI, const OVector3D& vecJ, const OVector3D& vecK);
 
    void set(const OMatrix& matrix);
 
    void normalize();
 
    OMatrix asMatrix() const;
 
public:
    OPoint3D _origin;
    OVector3D _vecI;
    OVector3D _vecJ;
    OVector3D _vecK;
};
 
class OBox
{
public:
    OBox();
    OBox(const OBox& box);
    OBox(const OCoord3D& min, const OCoord3D& max);
    OBox(double x1, double y1, double z1, double x2, double y2, double z2);
    virtual ~OBox() {}
 
    virtual OBox& operator=(const OBox& box);
    virtual bool operator==(const OBox& box) const;
    virtual bool operator!=(const OBox& box) const
    {
        return !operator==(box);
    }
 
    virtual bool isInside(const OPoint3D& pt) const;
 
    virtual bool isInside2D(const OPoint3D& pt) const;
 
    virtual bool isInContact(const OBox& box) const;
 
    virtual void Enlarge(const OPoint3D& pt);
 
    virtual void Enlarge(float x, float y, float z);
 
    virtual void Enlarge(const OBox& box);
 
    virtual void Translate(const OPoint3D& vectorTranslate);
 
public:
    OPoint3D _min; 
    OPoint3D _max; 
};
 
class OBox2 : public OBox
{
    // Methods
public:
    OBox2();
    OBox2(const OBox2& box);
    OBox2(const OBox& box);
    OBox2(const double& length, const double& width, const double& height);
 
private: // Set private for security seems to an "af hoc" adaptation and is used only by an internal member
         // function
    OBox2(const OBox2& box, const ORepere3D& repere, const OPoint3D& centre);
 
public:
    virtual ~OBox2() {}
 
    virtual OBox2& operator=(const OBox2& box);
    virtual bool operator==(const OBox2& box) const;
    virtual bool operator!=(const OBox2& box) const
    {
        return !operator==(box);
    }
 
    OPoint3D BoxCoord(int N) const;
    virtual bool isInside(const OPoint3D& pt) const;
    virtual bool isInside2D(const OPoint3D& pt) const;
    virtual void Translate(const OVector3D& vect);
    OBox2 boxRotation(const OPoint3D& O, const OPoint3D& P2);
    void BoxRotationOzOy(double alpha, double theta);
    void moveAndRotate(const OPoint3D& origin, const OVector3D& vec);
 
private:
    OBox2 rotInXOYOnly(const OVector3D& v1, const OVector3D& v2, const OPoint3D& O, const OPoint3D& P2);
    OBox2 rotInXOZOnly(const OVector3D& v1, const OVector3D& v2, const OPoint3D& O, const OPoint3D& P2);
    OBox2 rot3D(const OVector3D& v1, const OVector3D& v2, const OPoint3D& O, const OPoint3D& P2);
 
public:
    ORepere3D _repere;
    OPoint3D _center;
    OPoint3D _A;
    OPoint3D _B;
    OPoint3D _C;
    OPoint3D _D;
    OPoint3D _E;
    OPoint3D _F;
    OPoint3D _G;
    OPoint3D _H;
    double _length;
    double _height;
    double _width;
};
 
class OHPlane3D
{
public:
    OHPlane3D();
    OHPlane3D(const OHPlane3D& Plane);
    OHPlane3D(const OVector3D& normal, const OPoint3D& origin);
    virtual ~OHPlane3D();
 
    void set(const OVector3D& normal, const OPoint3D& origin);
 
    OHPlane3D& operator=(const OHPlane3D& Plane);
    bool operator==(const OHPlane3D& Plane) const;
    bool operator!=(const OHPlane3D& Plane) const;
 
    int intersects(const OPoint3D& pt1, const OPoint3D& pt2, OPoint3D& ptIntersec, double& t) const;
    int intersects(const OSegment3D& seg, OPoint3D& ptIntersec) const;
 
private:
    OVector3D _N;
    OPoint3D _O;
    double _p;
};
 
#endif // TY_MC_3D