CollisionManager.h

#pragma once
#include <Eigen/StdVector>
#include "ConstraintSet.h"
#include "DataStructures.h"
 
namespace CoRiBoDynamics{
 
class CollisionManager : public ConstraintSet
{
public:
    CollisionManager(ConstraintSolver* solver, double timeConstant);
    ~CollisionManager();
 
    int AddCapsule(double radius, double height, double contactStiffness, double contactDamping, double frictionCoefficient, int index);
    int AddSphere(double radius, double contactStiffness, double contactDamping, double frictionCoefficient, int index);
    int AddTorus(double majorRadius, double minorRadius, double contactStiffness, double contactDamping, double frictionCoefficient, int index);
    int AddChamfer(double majorRadius, double minorRadius, double contactStiffness, double contactDamping, double frictionCoefficient, int index);
    int AddPlate(double lengthX, double lengthY, double thickness, double contactStiffness, double contactDamping, double frictionCoefficient, int index);
 
    int addExternalSphere(double radius, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity, double contactStiffness, double contactDamping, double frictionCoefficient);
    int addExternalCapsule(double length, double radius, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity, double contactStiffness, double contactDamping, double frictionCoefficient);
    int addExternalToroid(double majorRadius, double minorRadius, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity, double contactStiffness, double contactDamping, double frictionCoefficient);
    int addExternalChamfer(double majorRadius, double minorRadius, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity, double contactStiffness, double contactDamping, double frictionCoefficient);
    int addExternalPlate(double lengthX, double lengthY, double thickness, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity, double contactStiffness, double contactDamping, double frictionCoefficient);
 
    enum ShapeType {SPHERE=0, CAPSULE=1, CHAMFER=2, BOX=3, TOROID=4, PLATE=5}; 
 
    struct MaterialCharacteristic { 
        double contactDamping; 
        double contactStiffness; 
        double frictionCoefficient; 
        MaterialCharacteristic(double K, double D, double F){
            contactStiffness = K; contactDamping = D; frictionCoefficient = F;
        }
        MaterialCharacteristic(){
            contactStiffness = std::numeric_limits<double>::quiet_NaN(); contactDamping = std::numeric_limits<double>::quiet_NaN(); frictionCoefficient = std::numeric_limits<double>::quiet_NaN();
        }
    };
 
    /*
        Singly linked list. Contains the index of the element involved in the collition, and the response matrix of the contact.
    */
    struct contact_matrix_index_pair {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            mat6 constraint_matrix;
        int element_ix;
        contact_matrix_index_pair* next;
    };
 
    class GeometricObject{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        GeometricObject(){m_contact_matrix = nullptr;};
        virtual ~GeometricObject(){}
 
        void SetAsSphere(double radius, const MaterialCharacteristic& material);
        void SetAsCapsule(double radius, double length, const MaterialCharacteristic& material);
        void SetAsChamfer(double minor_radius, double major_radius, const MaterialCharacteristic& material);
        void SetAsTorus(double minor_radius, double major_radius, const MaterialCharacteristic& material);
        void SetAsPlate(double radius, double lx, double ly, const MaterialCharacteristic& material);
 
        GeometryTools::Sphere*  GetSphere() {return (GeometryTools::Sphere*)(&m_buffer);}
        GeometryTools::Capsule* GetCapsule(){return (GeometryTools::Capsule*)(&m_buffer);}
        GeometryTools::Chamfer* GetChamfer(){return (GeometryTools::Chamfer*)(&m_buffer);}
        GeometryTools::Torus*   GetTorus()  {return (GeometryTools::Torus*)(&m_buffer);}
        GeometryTools::Plate*   GetPlate()  {return (GeometryTools::Plate*)(&m_buffer);}
 
        GeometryTools::Point*   GetPoint()  {return &(GetSphere()->point);}
        GeometryTools::Segment* GetSegment(){return &(GetCapsule()->segment);}
        GeometryTools::Disk*    GetDisk()   {return &(GetChamfer()->disk);}
        GeometryTools::Ring*    GetRing()   {return &(GetTorus()->ring);}
        GeometryTools::Plane*   GetPlane()  {return &(GetPlate()->plane);}
 
        void Update();
 
        virtual vec3 P()=0;
        virtual vec3 V()=0;
        virtual Quat Q()=0;
        virtual vec3 W()=0;
 
        double m_bounding_sphere_radius;
        vec3 m_OBB; 
 
        double m_radius;
        ShapeType m_shape_type;
        char m_buffer[sizeof(GeometryTools::Plate)];
        GeometryTools::AABB m_aabb;
        MaterialCharacteristic m_material;
        vec6 m_contact_forces; 
        contact_matrix_index_pair* m_contact_matrix;
    };
 
    class InternalObject : public GeometricObject {
    public:
        explicit InternalObject(RigidElement* element){m_element = element;}
        RigidElement* m_element;
        vec3 P();
        vec3 V();
        Quat Q();
        vec3 W();
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    
 
    class ExternalObject : public GeometricObject {
    public:
        explicit ExternalObject(){
            UpdateStates(nullptr,nullptr,nullptr,nullptr);
            //contact = nullptr;
        }
 
        ExternalObject(const double* P, const double* Q, const double* V, const double* W){
            UpdateStates(P, Q, V, W);
            //contact = nullptr;
        }
 
        void UpdateStates(const double* P, const double* Q, const double* V, const double* W){
            m_P = P; m_Q = Q; m_V = V, m_W = W;
        }
        const double* m_P; 
        const double* m_Q; 
        const double* m_V; 
        const double* m_W; 
 
        vec3 P();
        vec3 V();
        Quat Q();
        vec3 W();
 
        //contact_matrix_index_pair* contact;
 
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    void updateExternalElement(int index, const double* Position, const double* Velocity, const double* Orientation, const double* AngularVelocity);
    vec6 getExternalObjectReactionForce(int index);
    vec6 getExternalObjectReactionForce(ExternalObject& external_object);
 
    vec6 getInternalObjectReactionForce(InternalObject& internal_object);
 
    virtual void ComputeConstraints(const double T, const double * const X);
 
    void setSpacePartitionOrigo(double x, double y, double z);
    void setSpacePartitionBlockSize(double length);
 
protected:
 
    std::vector<ExternalObject, Eigen::aligned_allocator<ExternalObject> > m_ExternalObjects; 
    std::vector<InternalObject, Eigen::aligned_allocator<InternalObject> > m_InternalObjects; 
 
    void CollisionTest( InternalObject* objectA, InternalObject* objectB , const double * const X);
 
    void CollisionTest( ExternalObject* ExtObj, InternalObject* IntObj , const double * const X);
 
    double ObjectDistance(GeometricObject* shapeA, GeometricObject* shapeB, GeometryTools::MinDistanceInfo& result);
 
    bool BoundingSphereRejectionTest(GeometricObject* sphereA, GeometricObject* sphereB);
 
    bool OBB_BoundingSphereRejectionTest(GeometricObject* box, GeometricObject* sphere);
 
    DataStructures::SpacePartitioner m_CollisionPartition;
    ObjectFactoryStack<contact_matrix_index_pair> m_external_contact;
 
    double m_timeConstant; 
};
 
}