Csc3dTypes.h

Go to the documentation of this file.

//
//  CscPoint3d.hpp
//  ConscienceRobotManager
//
//  Created by MACBOOK PRO on 05/03/2019.
//  Copyright © 2019 IliesZaoui. All rights reserved.
//
 
#ifndef Csc3dTypes_h
#define Csc3dTypes_h
 
#include <iostream>
#include <string>
#include <vector>
#include <cassert>
 
using std::to_string, std::string, std::vector;
 
namespace conscience_core::axiomes {
 
class CscPoint3d {
public:
    static CscPoint3d *zero();
 
    double x;
    double y;
    double z;
 
    CscPoint3d(CscPoint3d *fromPoint);
    CscPoint3d(double x = 0, double y = 0, double z = 0);
    virtual ~CscPoint3d();
 
    double getX() const;
    double getY() const;
    double getZ() const;
 
    double getByIndex(int index) const;
    void setByIndex(int index, double value);
 
    double distanceWith(const CscPoint3d *point) const;
 
    CscPoint3d *centerWith(const CscPoint3d *other) const;
 
    CscPoint3d *rotate(const CscPoint3d *rotationEulerXYZ, const CscPoint3d *rotationCenter = nullptr) const;
 
    CscPoint3d *rotate(double eulerX, double eulerY, double eulerZ, const CscPoint3d *rotationCenter = nullptr) const;
 
    CscPoint3d *minus(const CscPoint3d *rightOperand) const;
 
    CscPoint3d *plus(const CscPoint3d *rightOperand) const;
 
    CscPoint3d *plus(double x, double y, double z) const;
 
    CscPoint3d *multiplyWith(const CscPoint3d *rightOperand) const;
 
    CscPoint3d *operator-(const CscPoint3d &rightOperand) const;
    CscPoint3d *operator+(const CscPoint3d &rightOperand) const;
    CscPoint3d *operator*(const CscPoint3d &rightOperand) const;
    CscPoint3d *operator*(double scalar) const;
    CscPoint3d *operator/(double scalar) const;
    CscPoint3d &operator*=(double scalar);
    CscPoint3d &operator+=(const CscPoint3d &rightOperand);
    CscPoint3d crossProduct(const CscPoint3d &other) const;
    double dotProduct(const CscPoint3d &other) const;
 
    CscPoint3d *leftNormal();
    void normalize();
 
    /* 
    * Return the length of the point3D just as the length of a Vector
    */
    double distanceToOrigin() const;
    double squaredDistanceToOrigin() const;
 
 
    void set(double x, double y, double z);
    void set(const CscPoint3d &fromOther);
 
    bool equals(const CscPoint3d &theOtherOne, double epsilon = 0.000001) const;
    bool equalsValues(double x, double y, double z, double epsilon = 0.000001) const;
 
    virtual operator string() const;
    virtual string toString() const;
 
    virtual CscPoint3d *clone() const;
    void copyFrom(const CscPoint3d &otherPoint);
};
 
inline static const CscPoint3d *const POINT_ZERO = new CscPoint3d(0, 0, 0);
 
string pointsToString(const vector<CscPoint3d *> &points);
string pointsToString(const vector<const CscPoint3d *> &points);
 
bool comparePoints(const CscPoint3d *a, const CscPoint3d *b);
void deleteDuplicates(vector<const CscPoint3d *> &points);
vector<CscPoint3d *> *copyPoints(const vector<const CscPoint3d *> &points);
 
typedef CscPoint3d CscSize3d;
 
enum Axis3d {
    AXIS_3D_X,
    AXIS_3D_Y,
    AXIS_3D_Z
};
 
struct CscRotation3dEuler;
 
struct CscQuaternion {
    double w = 0;
    double x = 0;
    double y = 0;
    double z = 0;
    CscQuaternion(double w = 0, double x = 0, double y = 0, double z = 0);
    CscQuaternion(const double *values);
    virtual ~CscQuaternion() = default;
    double getX() const;
    double getY() const;
    double getZ() const;
    double getW() const;
    virtual operator string() const;
    virtual string toString() const;
    virtual CscQuaternion *clone() const;
 
    static CscQuaternion identity();
    static CscQuaternion fromAngleAxis(double angleRad, Axis3d axis);
    CscQuaternion operator*(const CscQuaternion &other) const;
    CscQuaternion &operator*=(const CscQuaternion &other);
    CscPoint3d operator*(const CscPoint3d &vector) const;
    CscQuaternion operator/(const CscQuaternion &other) const;
    CscQuaternion operator+(const CscQuaternion &other) const;
 
    void normalize();
 
    CscQuaternion conjugate() const;
 
    CscQuaternion inverse() const;
 
    void rotateVector(CscPoint3d &vector) const;
 
    CscRotation3dEuler toEuler() const;
};
 
struct CscRotation3dEuler {
    double pitch = 0;
    double yaw = 0;
    double roll = 0;
    CscRotation3dEuler(double pitch = 0, double yaw = 0, double roll = 0);
    virtual ~CscRotation3dEuler() = default;
    double getPitch() const;
    double getYaw() const;
    double getRoll() const;
    virtual operator string() const;
    virtual string toString() const;
    virtual CscRotation3dEuler *clone() const;
};
 
class CscLine3d {
 
public:
    CscLine3d(const CscPoint3d *positionA = CscPoint3d::zero(), const CscPoint3d *positionB = CscPoint3d::zero());
    const CscPoint3d *const positionA;
    const CscPoint3d *const positionB;
 
    bool isIntersectPoint(const CscPoint3d *pointToCheck) const;
    static int getDirection(const CscPoint3d *a, const CscPoint3d *b, const CscPoint3d *c);
    bool isIntersectLine(const CscLine3d *lineToCheck) const;
 
    ~CscLine3d();
};
 
class CscArea3d {
 
public:
    CscPoint3d *position;
    double sizeX;
    double sizeY;
    double sizeZ;
 
    CscPoint3d getPositionMax() const;
 
    CscArea3d(CscPoint3d *position = new CscPoint3d(0, 0, 0), double sizeX = 0, double sizeY = 0, double sizeZ = 0);
    CscArea3d(double x, double y, double z, double sizeX, double sizeY, double sizeZ);
    virtual ~CscArea3d();
 
    const CscPoint3d &getPositionMin() const;
    double getSizeX() const;
    double getSizeY() const;
    double getSizeZ() const;
 
    CscArea3d *clone() const;
    void copyFrom(const CscArea3d &otherArea);
    bool isPointInside(const CscPoint3d *pointToCheck) const;
 
    operator string() const;
    string toString() const;
};
 
class CscPoint3dOriented : public CscPoint3d {
 
public:
    double eulerX;
    double eulerY;
    double eulerZ;
 
    static CscPoint3dOriented *zero();
 
    CscPoint3dOriented(const CscPoint3d &point, double eulerX, double eulerY, double eulerZ);
    CscPoint3dOriented(const CscPoint3d &point, const CscPoint3d &rotation);
    CscPoint3dOriented(double x = 0, double y = 0, double z = 0, double eulerX = 0, double eulerY = 0, double eulerZ = 0);
 
    // to be able to access to the parent method equals()
    using CscPoint3d::equals;
 
    bool equals(const CscPoint3dOriented& other, double epsilon = 0.000001) const;
    double getEulerX() const;
    double getEulerY() const;
    double getEulerZ() const;
 
    void setEuler(double eulerX, double eulerY, double eulerZ);
 
    CscPoint3dOriented *clone() const override;
 
    string eulerToString() const;
 
    operator string() const override {
        return "(" + to_string(x) + ", " + to_string(y) + ", " + to_string(z) + " - EulerXYZ=" + eulerToString() + ")";
    }
    [[deprecated("Use extractNavigationQuaternionFromEntityQuaternion() for navigation/LiDAR orientation.")]]
    double toStandardEulerY() const;
};
 
class CscPolygon {
public:
    const int ID;
 
    CscPolygon(
        const CscPoint3d *p0, const CscPoint3d *p1, const CscPoint3d *p2,
        const CscPoint3d *normal0 = nullptr, const CscPoint3d *normal1 = nullptr, const CscPoint3d *normal2 = nullptr,
        int ID = 0);
    CscPolygon(const CscPoint3d *positions[3], const CscPoint3d *normalVectors[3], int _ID);
    virtual ~CscPolygon();
 
    const CscPoint3d *getBarycentre() const;
    const CscPoint3d *getNormals() const;
 
    CscPolygon *clone() const;
    bool isPointInside(const CscPoint3d *pointToCheck) const;
 
    CscPolygon *rotate(const CscPoint3d *rotationEulerXYZ, const CscPoint3d *rotationCenter = nullptr) const;
    CscPolygon *translate(double x, double y, double z) const;
    CscPolygon *translate(const CscPoint3d &translation) const;
 
    inline const CscPoint3d *positionAt(int index) const {
        assert(((void)("polygon has only 3 positions - requestedIndex=" + to_string(index)), index >= 0 && index < 3));
        return positions[index];
    }
 
    inline const CscPoint3d *normalAt(int index) const {
        assert(((void)("polygon has only 3 normals - requestedIndex=" + to_string(index)), index >= 0 && index < 3));
        return normalVectors[index];
    }
 
    inline vector<const CscPoint3d *> getPositions() const {
        return {positions[0], positions[1], positions[2]};
    }
 
private:
    // Buffer of 3 CscPoint3d for Position Values
    const CscPoint3d *positions[3];
    const CscPoint3d *normalVectors[3];
};
 
struct Vec3 {
    double x, y, z;
    explicit Vec3(double x = 0, double y = 0, double z = 0);
};
 
struct Vec4 {
    double x, y, z, w;
    Vec4() : x(0), y(0), z(0), w(1) {}
    Vec4(double x, double y, double z, double w = 1) : x(x), y(y), z(z), w(w) {}
};
 
enum PositionFormat {
    LNG_ALTITUDE_LAT,
    XYZ
};
 
struct Csc3dSegment {
    CscPoint3d *start = nullptr;
    CscPoint3d *end = nullptr;
};
 
}
 
#endif