GeometryTools.h

#pragma once
 
#ifdef FH_VISUALIZATION
#pragma warning(push)
#pragma warning(disable : 4267)
#pragma warning(disable : 4251)
#include <Ogre.h>
#pragma warning(pop)
#endif
 
#include "eigen_matrix_defs.h"
 
namespace CoRiBoDynamics{
 
namespace GeometryTools{
struct MinDistanceInfo{ 
    vec6 JacobianA; 
    vec6 JacobianB; 
    double J_dot_V; 
    double minDistance; 
    double minDistance_dot; 
    vec3 PA; 
    vec3 PB; 
    vec3 u; 
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
 
struct Point{// Spatial point P
    vec3 P; // position
    vec3 V; // velocity
};
struct Segment{// Spatial line segment, defined by center point P, and endpoint P +-N
    vec3 P; // position
    vec3 N; // directional vector
    vec3 V; // velocity
    vec3 W; // angular velocity
};
struct Plane{ // finite spatial plane. spanned by vectors +-SA and +-SB
    vec3 P; // position
    vec3 SA; // spanning vector A
    vec3 SB; // spanning vector B
    vec3 V; // velocity
    vec3 W; // angular velocity
};
struct Ring{// Spatial ring defined by center point P, normal vector N and radius
    vec3 P; // position
    vec3 N; // unit normal vector
    vec3 V; // velocity
    vec3 W; // angular velocity
    double radius; // ring radius
};
struct Disk{// Spatial disk defined by center point P, normal vector N and radius
    vec3 P; // position
    vec3 N; // normal vector
    vec3 V; // velocity
    vec3 W; // angular velocity
    double radius; // disk radius
};
 
struct Capsule {Segment segment; double radius; double half_height;}; 
struct Sphere {Point point; double radius;}; 
struct Torus {Ring ring; double minorRadius;}; 
struct Chamfer {Disk disk; double radius;}; 
struct Plate {Plane plane; double radius; double lx; double ly;}; 
 
void Distance(MinDistanceInfo& result, Point& pointA,   Point& pointB);
void Distance(MinDistanceInfo& result, Point& point,    Segment& segment);
void Distance(MinDistanceInfo& result, Point& point,    Ring& ring);
void Distance(MinDistanceInfo& result, Point& point,    Disk& disk);
void Distance(MinDistanceInfo& result, Point& point,    Plane& plane);
 
void Distance(MinDistanceInfo& result, Segment& segment,    Point& point);
void Distance(MinDistanceInfo& result, Segment& segmentA,   Segment& segmentB);
void Distance(MinDistanceInfo& result, Segment& segment,    Ring& ring);
void Distance(MinDistanceInfo& result, Segment& segment,    Disk& disk);
void Distance(MinDistanceInfo& result, Segment& segment,    Plane& plane);
 
void Distance(MinDistanceInfo& result, Plane& plane,    Point& point);
void Distance(MinDistanceInfo& result, Plane& plane,    Segment& segment);
void Distance(MinDistanceInfo& result, Plane& planeA,   Plane& planeB);
 
void Distance(MinDistanceInfo& result, Ring& ring,  Point& point);
void Distance(MinDistanceInfo& result, Ring& ring,  Segment& segment);
 
void Distance(MinDistanceInfo& result, Disk& disk,  Segment& segment);
void Distance(MinDistanceInfo& result, Disk& disk,  Point& point);
 
struct AABB{double xm; double ym; double zm; double xp; double yp; double zp;
AABB(){}
AABB(double XM,double YM,double ZM,double XP,double YP,double ZP){xm=XM;ym=YM;zm=ZM;xp=XP;yp=YP;zp=ZP;}
AABB(const vec3& center, const vec3& half_span){
    xm = center.x()-half_span.x();
    ym = center.y()-half_span.y();
    zm = center.z()-half_span.z();
    xp = center.x()+half_span.x();
    yp = center.y()+half_span.y();
    zp = center.z()+half_span.z();
}
bool isContainedIn(const AABB& box)const{return xm>=box.xm && ym>=box.ym && zm>=box.zm && xp<=box.xp && yp<=box.yp && zp<=box.zp;}
};
 
AABB AxisAlignedBoundingBox(const Point& point);
AABB AxisAlignedBoundingBox(const Segment& segment);
AABB AxisAlignedBoundingBox(const Ring& ring);
AABB AxisAlignedBoundingBox(const Disk& disk);
AABB AxisAlignedBoundingBox(const Plane& plane);
 
AABB AxisAlignedBoundingBox(const Sphere& sphere);
AABB AxisAlignedBoundingBox(const Capsule& capsule);
AABB AxisAlignedBoundingBox(const Torus& torus);
AABB AxisAlignedBoundingBox(const Chamfer& chamfer);
AABB AxisAlignedBoundingBox(const Plate& plate);
 
AABB MergeAxisAlignedBoundingBox(const AABB& A,const AABB& B);
 
#ifdef FH_VISUALIZATION
void CreateCapsule( Ogre::ManualObject* capsule_manual, double radius, double length, int N = 32, std::string material = "DiffuseSpecular");
void CreateChamfer( Ogre::ManualObject* chamfer_manual, double majorRadius, double minorRadius, int N = 32, std::string material = "DiffuseSpecular");
void CreatePlate( Ogre::ManualObject* plate_manual, double lx, double ly, double t, int N = 32, std::string material = "DiffuseSpecular");
void CreateTorus( Ogre::ManualObject* chamfer_manual, double majorRadius, double minorRadius, int N = 32, std::string material = "DiffuseSpecular");
void CreateSphere( Ogre::ManualObject* sphere_manual, double radius, int N = 32, std::string material = "DiffuseSpecular");
void CreateEquilateralTriangle( Ogre::ManualObject* sphere_manual, double side, double thickness, std::string material = "DiffuseSpecular");
void CreateBox( Ogre::ManualObject* manual, double l0, double l1, double l2, std::string material = "DiffuseSpecular");
 
void CreateLinedrawingChamfer( Ogre::ManualObject* chamfer_manual, double majorRadius, double minorRadius, Ogre::ColourValue color, int N = 8);
#endif
}
 
}