Subroutines.h

#pragma once
 
#include <iostream>
#include <iomanip>
#include <fstream>
#include <algorithm>
#include <vector>
#include <valarray>
#include <Eigen/Eigen>
 
#include "sfh/math/math.h"
 
namespace barge {
 
typedef Eigen::Matrix<double,6,6>   mat6;
typedef Eigen::Matrix<double,6,1>   vec6;
typedef Eigen::Matrix<double,3,3>   mat3;
typedef Eigen::Matrix<double,2,1>   vec2;
typedef Eigen::Matrix<int,2,1>      vec2i;
typedef Eigen::Matrix<double,3,1>   vec3;
typedef Eigen::Matrix<int,3,1>      vec3i;
typedef Eigen::Matrix<double,13,1>  vec13;
typedef Eigen::Quaternion<double>   quat;
 
// Convex Hull ==================================================================
typedef double iCoordinate;         // Coordinate type
typedef double oCoordinate;         // Must be big enough to hold 2*max(|Coordinate|)^2
// Convex Hull ==================================================================
 
// Ray-Triangle intersection ====================================================
int rayTri(const vec3 &V1, const vec3 &V2, const vec3 &V3, const vec3 &O, const vec3 &D, double &out);
// Ray-Triangle intersection ====================================================
 
// Segment-Plane intersection ===================================================
int SegmentPlane(const vec3 &S0, const vec3 &S1, const vec3 &P0, const vec3 &PN, vec3 &SPI);
// Segment-Plane intersection ===================================================
 
// Convex Hull ==================================================================
struct Point {
    iCoordinate x, y;
    bool operator <(const Point &p) const {
        return x < p.x || (x == p.x && y < p.y);
    }
};
 
// 2D cross product of OA and OB vectors
inline oCoordinate Cross2D(const Point &O, const Point &A, const Point &B)
{
    return (A.x - O.x) * (B.y - O.y) - (A.y - O.y) * (B.x - O.x);
}
// 2D cross product of OA and OB vectors
 
std::vector<Point> ConvexHull(std::vector<Point> P);
// Convex Hull ==================================================================
 
// Triangle Area 2D =============================================================
inline double TriangleArea2D(const vec2 &V0, const vec2 &V1, const vec2 &V2)
{
    vec2 V01 = V1-V0;
    vec2 V02 = V2-V0;
 
    return 0.5*std::abs(V01(0)*V02(1)-V02(0)*V01(1));
}
// Triangle Area 2D =============================================================
 
// Triangle Area 3D =============================================================
inline double TriangleArea3D(const vec3 &V0, const vec3 &V1, const vec3 &V2)
{
    vec3 V01 = V1-V0;
    vec3 V02 = V2-V0;
    
    vec3 V012 = V01.cross(V02);
 
    return 0.5*sqrt(V012(0)*V012(0)+V012(1)*V012(1)+V012(2)*V012(2));
}
// Triangle Area 3D =============================================================
 
// Point-Point distance 3D ======================================================
inline double PointDistance3D(const vec3 &PA, const vec3 &PB)
{
    return sqrt((PA(0)-PB(0))*(PA(0)-PB(0))+(PA(1)-PB(1))*(PA(1)-PB(1))+(PA(2)-PB(2))*(PA(2)-PB(2)));
}
// Point-Point distance 3D ======================================================
 
// Point-Plane distance =========================================================
inline double PointPlaneDis(const vec3 &V0, const vec3 &P0, const vec3 &PN)
{
    vec3 VP = V0-P0;
    return VP.dot(PN.normalized());
}
// Point-Plane distance =========================================================
 
// Polyplate structure ==========================================================
struct PolyplateSpec {
public:
    PolyplateSpec(){}
    PolyplateSpec(int &nPolyPoints, std::vector<double> &xPoints, std::vector<double> &yPoints, double &mThickness, double &mDensity);
    ~PolyplateSpec(){}
 
    void setPolyplate(int &nPolyPoints, std::vector<double> &xPoints, std::vector<double> &yPoints, double &mThickness, double &mDensity);
 
    int numPoints;                      // Points
    double thickness;                   // Plate thickness
    std::vector<double> xCoords;        // Polygon x-coords (Right-handed in NED, counter-clockwise from bottom, clockwise from top)
    std::vector<double> yCoords;        // Polygon y-coords (Right-handed in NED, counter-clockwise from bottom, clockwise from top)
    double density;                     // Density
    double mass;                        // Mass
    double volume;                      // Volume
    mat6 inertiaMatrix;                 // Plate inertia
    vec3 centroid;
 
    double projectedArea;
    double cubedGRZ;
    double cubedGRY;
    double cubedGRX;
    double cubedGRXY;
    double cubedGRYX;
 
    double rectGRX;
    double rectGRY;
};
 
mat3 GetRotation(const vec3& angle);
mat6 ReorientInertiaTranslateRotate(const mat6& Inertia, const vec6& orientation);
mat3 MakeDyadic(const vec3& vector);
// Polyplate structure ==========================================================
 
// AABB class ===================================================================
class AABB {
public:
    vec3 minloc, maxloc;
 
    AABB() {
        minloc(0) = minloc(1) = minloc(2) = std::numeric_limits<double>::max();         
        maxloc(0) = maxloc(1) = maxloc(2) = -std::numeric_limits<double>::max();
    }       
        
    AABB(const AABB &aabb) {
        minloc = aabb.minloc;
        maxloc = aabb.maxloc;
    }
        
    ~AABB(){}
 
    void add(const vec3 &v) {
        if (v(0) < minloc(0)) minloc(0) = v(0);
        if (v(1) < minloc(1)) minloc(1) = v(1);
        if (v(2) < minloc(2)) minloc(2) = v(2);
        if (v(0) > maxloc(0)) maxloc(0) = v(0);
        if (v(1) > maxloc(1)) maxloc(1) = v(1);
        if (v(2) > maxloc(2)) maxloc(2) = v(2);
    }
 
    void clear() {
        minloc(0) = minloc(1) = minloc(2) = std::numeric_limits<double>::max();
        maxloc(0) = maxloc(1) = maxloc(2) = -std::numeric_limits<double>::max();
    }
 
    int overlap(const AABB &aabb)
    {
        if(maxloc(0) < aabb.minloc(0) || minloc(0) > aabb.maxloc(0)) return 0;
        if(maxloc(1) < aabb.minloc(1) || minloc(1) > aabb.maxloc(1)) return 0;
        if(maxloc(2) < aabb.minloc(2) || minloc(2) > aabb.maxloc(2)) return 0;
 
        return 1;
    }
 
    double overlapvolume(const AABB &aabb, vec3 &minol, vec3 &maxol)
    {
        minol(0) = std::max(minloc(0),aabb.minloc(0));
        maxol(0) = std::min(maxloc(0),aabb.maxloc(0));      
        minol(1) = std::max(minloc(1),aabb.minloc(1));
        maxol(1) = std::min(maxloc(1),aabb.maxloc(1));
        minol(2) = std::max(minloc(2),aabb.minloc(2));
        maxol(2) = std::min(maxloc(2),aabb.maxloc(2));
 
        double volume = std::abs((maxol(0)-minol(0))*(maxol(1)-minol(1))*(maxol(2)-minol(2)));
 
        return volume;
    }
};
// AABB class ===================================================================
 
// AABB struct ==================================================================
struct AABBSpec {
    int     numE;               // Number of edges in object.
    int     numT;               // Number of triangles in object.
 
    int*    indexE;             // Index of edges in AABB.
    int*    indexT;             // Index of triangles in AABB.
};
// AABB struct ==================================================================
 
// AABB class 2D ================================================================
class AABB2D {
public:
    vec2 minloc, maxloc;
 
    AABB2D() {
        minloc(0) = minloc(1) = std::numeric_limits<double>::max();         
        maxloc(0) = maxloc(1) = -std::numeric_limits<double>::max();
    }       
        
    AABB2D(const AABB2D &aabb) {
        minloc = aabb.minloc;
        maxloc = aabb.maxloc;
    }
        
    ~AABB2D(){}
 
    void add(const vec2 &v) {
        if (v(0) < minloc(0)) minloc(0) = v(0);
        if (v(1) < minloc(1)) minloc(1) = v(1);
        if (v(0) > maxloc(0)) maxloc(0) = v(0);
        if (v(1) > maxloc(1)) maxloc(1) = v(1);
    }
 
    void clear() {
        minloc(0) = minloc(1) = std::numeric_limits<double>::max();
        maxloc(0) = maxloc(1) = -std::numeric_limits<double>::max();
    }
 
    int overlap(const AABB2D &aabb)
    {
        if(maxloc(0) < aabb.minloc(0) || minloc(0) > aabb.maxloc(0)) return 0;
        if(maxloc(1) < aabb.minloc(1) || minloc(1) > aabb.maxloc(1)) return 0;
 
        return 1;
    }
 
    double overlaparea(const AABB2D &aabb, vec2 &minol, vec2 &maxol)
    {
        minol(0) = std::max(minloc(0),aabb.minloc(0));
        maxol(0) = std::min(maxloc(0),aabb.maxloc(0));      
        minol(1) = std::max(minloc(1),aabb.minloc(1));
        maxol(1) = std::min(maxloc(1),aabb.maxloc(1));
 
        double area = std::abs((maxol(0)-minol(0))*(maxol(1)-minol(1)));
 
        return area;
    }
};
// AABB class 2D ================================================================
 
// Object struct ================================================================
struct ObjectSpec {
    int     numP;               // Number of points in object.
    int     numT;               // Number of triangles in object.
 
    double* xyzP;               // Coordinate of points in object.
    int*    indexT;             // Index of triangles in object.
    double* normT;
};
// Object struct ================================================================
 
// Quaternion class =============================================================
class QuatC {
public:
    quat L2G;                   // Quaternion transfers local coordinates to global coordinates
    quat G2L;                   // Quaternion transfers global coordinates to local coordinates
};
// Quaternion class =============================================================
 
// Particle class ===============================================================
class Particle {
public:
    vec3        xyzCT;          // Coordinate of centroid.
    int         numV;           // Number of vertices in a polyhedral particle.
    vec3*       xyzV;           // Coordinate of each vertex.
    int*        numE;           // Number of edges that share each vertex.
    int**       indexE;         // Index of edges that share each vertex.
    int*        numF;           // Number of faces that share each vertex.
    vec2i**     indexF;         // Index of faces that share each vertex.
 
    Particle(){}
 
    Particle(const Particle &P0);
 
    ~Particle();
 
    void newPolyplate(int &nPolyPoints);
    void setPolyplate(int &nPolyPoints, const PolyplateSpec &iPolyplate, vec3 &iCT, quat &iQuatL2G);
    vec2i ClosestV(const Particle &PB, double &dAB);
    double DistancePL(const vec3 &P0, const vec3 &PN, int &numCV, std::vector<int> &indexCV);
    double DistancePL1V(const vec3 &P0, const vec3 &PN);
    int IntersectionPL(const vec3 &P0, const vec3 &PN, int &numInt, std::vector<vec3> &xyzInt, double &IND);
};
// Particle class ===============================================================
 
// Common Plane =================================================================
class CommonPlane {
public:
    double  dBA;                // Gap between two particles
    int     Cont;               // 1: Real contact; 0: Potential contact; -1: Not in contact
 
    CommonPlane(){}
    ~CommonPlane(){}
 
    int CalCommonPlane(Particle &PA, Particle &PB, const double &TOL, const double &TRAN, const vec3 &P0, const vec3 &PN, vec3 &CP0, vec3 &CPN);    
};
// Common Plane =================================================================
 
// Contact Area =================================================================
double ContactArea(Particle &PA, Particle &PB, const vec3 &P0, const vec3 &PN, double &IND, vec3 &CONP);
 
double ContactAreaPolyplate(Particle &PA, Particle &PB, const vec3 &P0, const vec3 &PN, double &IND, vec3 &CONP);
// Contact Area =================================================================
 
// Quaternion to Roll-Pitch-Yaw Angles ==========================================
inline vec3 QuaternionToAngle(const quat &QuatL2G)
{
    vec3 RollPitchYaw = QuatL2G.toRotationMatrix().eulerAngles(0,1,2);
 
    return RollPitchYaw;
}
// Quaternion to Roll-Pitch-Yaw Angles ==========================================
} // namespace