collision_library.h 9.44 KB
#ifndef COLLISION_LIBRARY_H
#define COLLISION_LIBRARY_H

// collision library interface
#include <collision_interface.h>

#include <typeinfo>
#include <unordered_map>

namespace collision
{

    // "Declaration" of the struct
    struct StateChangeObj;


class MyController : public Controller {
    GM_SCENEOBJECT (MyController)
public:

    explicit MyController () = default;

    void add (DynamicPSphere* const sphere);
    void add (StaticPSphere* const sphere);
    void add (StaticPPlane* const plane);
    void add (StaticPCylinder* const cylinder);
    void add (StaticPBezierSurf* const surf);

    // Change "value" and return type if the sphere should be able to be attached to objects other than planes
    std::unordered_set<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
//    std::vector<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);


//    void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
//    void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);

    void
    detectStateChange(StateChangeObj state);

    void
    singularityDetection(double dt);


    CollisionState detectCollision (const DynamicPSphere& S,
                                    const DynamicPCylinder& C, seconds_type dt);

    CollisionState detectCollision (const DynamicPCylinder& C0,
                                    const DynamicPCylinder& C, seconds_type dt);

    void computeImpactResponse (DynamicPSphere& S, DynamicPCylinder& C,
                                seconds_type dt);

    void computeImpactResponse (DynamicPCylinder& C0, DynamicPCylinder& C1,
                                seconds_type dt);

    void computeImpactResponse (DynamicPCylinder& C, const StaticPSphere& S,
                                seconds_type dt);

    void computeImpactResponse (DynamicPCylinder& C, const StaticPBezierSurf& B,
                                seconds_type dt);

    void computeImpactResponse (DynamicPCylinder& C, const StaticPPlane& P,
                                seconds_type dt);


private:

    template <typename T_s, typename T_o>
    void sphereStaticCollision(T_s* sphere, T_o* object, seconds_type dt);

    template <typename T_s, typename T_o>
    void sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type dt);

protected:
    void localSimulate (double dt) override;

    std::vector<DynamicPSphere*>                _dynamic_spheres;
    std::vector<DynamicPCylinder*>              _dynamic_cylinders;
    std::vector<StaticPSphere*>                 _static_spheres;
    std::vector<StaticPPlane*>                  _static_planes;
    std::vector<StaticPCylinder*>               _static_cylinders;
    std::vector<StaticPBezierSurf*>             _static_bezier_surf;

    std::vector<collision::CollisionObject>     _collisions;
    std::vector<StateChangeObj>                 _singularities;

    DefaultEnvironment                          _env;

    std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>>     _map;
//    std::unordered_map<DynamicPSphere*, std::vector<StaticPPlane*>>     _map;




};

template <>
class DynamicPhysObject<GMlib::PSphere<float>> : public DynamicPhysObject_Base<GMlib::PSphere<float>> {
public:
    using DynamicPhysObject_Base<GMlib::PSphere<float>>::DynamicPhysObject_Base;

    enum class States {
        Free,
        Rolling,
        AtRest
    };

    MyController*   _controller;
    States          _state = States::Free;     // Which state is the sphere in

    GMlib::Vector<double, 3>
    getTrajectory (seconds_type dt);

    void
    setTrajectory (GMlib::Vector<double, 3> ds, DynamicPSphere* sphere);

    GMlib::Vector<double, 3>        _NewTrajectory;

    // "Old" methods
    void            simulateToTInDt( seconds_type t ) override;

    GMlib::Vector<double, 3>
    computeTrajectory (seconds_type dt) const override; // [m]

    GMlib::Vector<double, 3> externalForces () const override; // [m / s^2]


};

// StateChangeObject struct
struct StateChangeObj {
    DynamicPSphere*                 obj1;   // Object whos state will change
    StaticPPlane*                   obj2;   // Object that obj1 will change state ACCORDING to
    seconds_type                    time;   // Time of singularity
//    collision::CollisionStateFlag   flag;
    DynamicPSphere::States  state;  // State that obj1 will change to

    StateChangeObj
    (DynamicPSphere* o1, StaticPPlane* o2, seconds_type t, DynamicPSphere::States s) :
        obj1{o1}, obj2{o2}, time{t} ,state{s} {}
};

template <class PSurf_T, typename... Arguments>
std::unique_ptr<DynamicPhysObject<PSurf_T>> unittestDynamicPhysObjectFactory(Arguments... parameters) {

    return std::make_unique<DynamicPhysObject<PSurf_T>> (parameters...);

}

template <class PSurf_T, typename... Arguments>
std::unique_ptr<StaticPhysObject<PSurf_T>> unittestStaticPhysObjectFactory(Arguments... parameters) {

    return std::make_unique<StaticPhysObject<PSurf_T>> (parameters...);
}


template <class Container_T >
void sortAndMakeUnique( Container_T& container) {

    // Sort

    std::sort( std::begin(container), std::end(container), [](const auto& a, const auto& b) {
        return a.t_in_dt < b.t_in_dt;
    });

    // Make unique

    auto pred =  [](const auto &a, const auto &b) {

        auto is_d_pred = []( const auto* obj ) {
          if(dynamic_cast<const DynamicPSphere*>(obj)) return true;



          return false;
        };

        if( a.obj1 == b.obj1 ) return true;
        if( a.obj1 == b.obj2 ) return true;
        if( a.obj2 == b.obj1 ) return true;
        if( ( is_d_pred(a.obj2) or is_d_pred(b.obj2) )
                and a.obj2 == b.obj2 ) return true;

        return false;
    };

    typename Container_T::iterator NE = std::end(container);
    for( auto first_iter = std::begin(container); first_iter != NE; ++first_iter) {

        for( auto r_iterator = NE - 1; r_iterator != first_iter; --r_iterator) {

                if( (pred(*first_iter, *r_iterator))) {
                        std::swap( *r_iterator, *(NE-1) );
                        NE--;

                    }
            }
        }

        container.erase( NE, std::end( container ) );

    } // EOF


template <typename T_s, typename T_o>
inline
void MyController::sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type dt) {

    auto cylinder = dynamic_cast<DynamicPCylinder*>(object);
//    auto plane = dynamic_cast<DynamicPPlane*>(object);

    if( cylinder ) {

        // Sphere vs. Dynamic Cylinder
    }
//    else if( plane ) {

        // Sphere vs. Dynamic Plane
//    }
    else {

        // Sphere vs. Dynamic Sphere
        for( auto iter = std::begin(_dynamic_spheres); iter != std::end(_dynamic_spheres); ++iter) {

            if( (*iter == sphere) or (*iter == object) ) {
                break;
            }
            else {

                // Check only for collisions for the first dynamic sphere
                auto col = collision::detectCollision(*sphere, **iter, dt);

                // The check for if the second object is a dynamic sphere is done in the main algorithm
                // and the method is called again only with the 1'st and 2'nd sphere swapped

                if( col.CollisionState::flag == CollisionStateFlag::Collision ) {

                    auto& first_sphere = sphere;
                    auto& second_sphere = *iter;

                    auto new_t = std::max(first_sphere->curr_t_in_dt, second_sphere->curr_t_in_dt);

                    if( col.time > seconds_type(new_t) and col.time < dt) {
                        auto col_obj = CollisionObject(first_sphere, second_sphere, col.time);
                        _collisions.push_back(col_obj);

                        // Simulate the objects to T in dt?

                    }
                }
            }
        }
    }
}

template <typename T_s, typename T_o>
inline
void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type dt)
{

    // The game will mainly have two types of static objects that can be collided with, planes and bezier surfaces
    // Checks to see which one the sphere has collided with
    auto plane = dynamic_cast<const StaticPPlane*>(object);
    auto bezier_s = dynamic_cast<const StaticPBezierSurf*>(object);


    // Sphere vs. Static Plane
    if( plane ) {

        auto attachedPlanes = _map[sphere];

        for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {

            // The sphere cannot collide with the same plane it just collided with
//            if( *iter == plane ) {
//                break;
//            }

            // If sphere is attached to the plane, it should NOT check for collisions with it
//            for( auto pl = attachedPlanes.begin(); pl != attachedPlanes.end(); pl++) {

//                if( *iter == *pl) break;

                // Else look for collision
                auto col = collision::detectCollision(*sphere, **iter, dt);

                if( col.CollisionState::flag == CollisionStateFlag::Collision ) {

                    auto& static_object = *iter;

                    auto new_t = sphere->curr_t_in_dt;

                    if( col.time > seconds_type(new_t) and col.time < dt)
                    {
                        auto col_obj = CollisionObject(sphere, static_object, col.time);
                        _collisions.push_back(col_obj);

                    }
                }
//            }     // THIS ONE
        }
    }

    // Same as above, only for bezier surfaces
    else if( bezier_s) {

    }


}


} // END namespace collision



#endif