CContact.h

#pragma once
 
#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <Eigen/Eigen>
 
#include "sfh/math/math.h"
 
#include "SimObject.h"
 
 
namespace Contact {
 
typedef Eigen::Matrix<double,3,1>   cvec3;
typedef Eigen::Matrix<int,3,1>      cvec3i;
typedef Eigen::Quaternion<double>   cquat;
 
// Point-Point distance 3D ======================================================
inline double PointDistance3D(const cvec3 &PA, const cvec3 &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 cvec3 &V0, const cvec3 &P0, const cvec3 &PN)
{
    cvec3 VP = V0-P0;
    return VP.dot(PN.normalized());
}
// Point-Plane distance =========================================================
 
// Ray-Triangle intersection ====================================================
int rayTri(const cvec3 &V1, const cvec3 &V2, const cvec3 &V3, const cvec3 &O, const cvec3 &D, double &out);
// Ray-Triangle intersection ====================================================
 
// Segment-Plane intersection ===================================================
int SegmentPlane(const cvec3 &S0, const cvec3 &S1, const cvec3 &P0, const cvec3 &PN, cvec3 &SPI);
// Segment-Plane intersection ===================================================
 
// AABB class ===================================================================
class AABB {
public:
    cvec3 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 cvec3 &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, cvec3 &minol, cvec3 &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;
    }
 
    int border(const AABB &aabb, double &tol)
    {
        if(PointDistance3D(minloc,aabb.minloc)>tol) return 0;
        if(PointDistance3D(maxloc,aabb.maxloc)>tol) return 0;
        
        return 1;
    }
};
// AABB class ===================================================================
 
// ContactObject class ==========================================================
class ContactObject {
public:
    // Triangle mesh of the object
    int         numV;           // Number of vertices.
    cvec3*      xyzVC;          // Coordinate of each vertex relative to COG.
    cvec3*      xyzV;           // Coordinate of each vertex.
    int         numF;           // Number of faces.
    cvec3i*     indexF;         // Index of faces.
    
    double      cKne;           // Normal contact stiffness
    double      cKte;           // Tangential contact stiffness
    double      cKnv;           // Normal viscous damping
    double      cMui;           // Surface friction
    double      cMaxP;          // Maximum penetration depth in a contact point
 
    double      cMaxR;          // Maximum detection radius
 
    ISignalPort*        InObjPos;               
    ISignalPort*        InObjQuat;              
    ISignalPort*        InObjVel;               
    ISignalPort*        InObjOmega;             
 
    cvec3       objPos;
    cquat       objQuatL2G;
    cvec3       objVel;
    cvec3       objOmega;
 
    AABB        objAABB;
 
    ContactObject(){}
    virtual ~ContactObject();
 
    void InitialSetup(ISimObjectCreator* creator);
    void UpdateObjectState(const double T, const double* const X);
    
    int PointPenetration(const cvec3 &O, const double &r, double &oPen, cvec3 &oInt);
    void PointContactForce(const cvec3 &cPos, const cvec3 &cVel,  const double &cPen, const cvec3 &cInt, cvec3 &cFor, cvec3 &cMon);
 
    double PointDistance(const cvec3 &O, const double &r);
};
// ContactObject class ==========================================================
 
} // namespace