Commit 1a3cf9637dab84823353fef4a9b64a04920c73f9

Authored by Bjørn-Richard Pedersen
1 parent 0883910a

Setting up new repository for collision library

CMakeLists.txt
... ... @@ -34,7 +34,13 @@ set(HDRS include/collision_library.h)
34 34  
35 35 set(SRCS include/collision_library.cpp)
36 36  
37   -add_library( ${CMAKE_PROJECT_NAME} ${HDRS} ${SRCS} )
  37 +add_library( ${CMAKE_PROJECT_NAME} SHARED ${HDRS} ${SRCS} )
  38 +target_link_libraries( ${CMAKE_PROJECT_NAME}
  39 + ${GMlib_LIBRARIES}
  40 + ${GLEW_LIBRARIES}
  41 + ${OPENGL_LIBRARIES}
  42 + ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES} pthread
  43 + )
38 44  
39 45  
40 46 option( UNITTESTS "Enable building of unittests" OFF )
... ...
include/collision_library.cpp
... ... @@ -11,6 +11,49 @@ namespace collision
11 11 const DynamicPhysObject<GMlib::PSphere<float>>& S1,
12 12 seconds_type dt)
13 13 {
  14 + const auto dt_max = dt;
  15 + const auto dt_min = std::max(S0.curr_t_in_dt, S1.curr_t_in_dt);
  16 + const auto new_dt = dt_max - dt_min;
  17 +
  18 + const auto S0_position = S0.getMatrixToScene() * S0.getPos();
  19 + const auto S1_position = S1.getMatrixToScene() * S1.getPos();
  20 +
  21 + const auto S0_radius = S0.getRadius();
  22 + const auto S1_radius = S1.getRadius();
  23 + const auto radius_sum = S0_radius + S1_radius;
  24 +
  25 + const auto Q = (S1_position - S0_position);
  26 + const auto R = (S1.computeTrajectory(new_dt) - S0.computeTrajectory(new_dt));
  27 +
  28 + const auto _QR = Q * R;
  29 + const auto _QRQR = std::pow( _QR, 2);
  30 +
  31 + const auto _RR = R * R;
  32 + const auto _QQ = Q * Q;
  33 +
  34 + const auto _rr = std::pow( radius_sum, 2);
  35 + const auto _square = std::sqrt(_QRQR - (_RR * (_QQ - _rr)));
  36 +
  37 + const auto epsilon = 0.00001;
  38 +
  39 + if ( _square < 0 )
  40 + {
  41 + return CollisionState(seconds_type(0.0), CollisionStateFlag::SingularityNoCollision);
  42 + }
  43 + else if ( (_QQ - _rr) < epsilon )
  44 + {
  45 + return CollisionState(seconds_type(0.0), CollisionStateFlag::SingularityParallelAndTouching);
  46 + }
  47 + else if ( _RR < epsilon )
  48 + {
  49 + return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallel);
  50 + }
  51 +
  52 + const auto x = (-_QR - _square) / _RR;
  53 +
  54 + return CollisionState(((x * new_dt) + dt_min), CollisionStateFlag::Collision);
  55 +
  56 +
14 57 }
15 58  
16 59 CollisionState
... ... @@ -18,22 +61,228 @@ namespace collision
18 61 const StaticPhysObject<GMlib::PPlane<float>>& P,
19 62 seconds_type dt)
20 63 {
21   - }
22 64  
  65 + const auto dt_max = dt;
  66 + const auto dt_min = S.curr_t_in_dt;
  67 + const auto new_dt = dt_max - dt_min;
23 68  
  69 + const auto s_position = S.getMatrixToScene() * S.getPos();
  70 + const auto s_radius = S.getRadius();
  71 +
  72 + auto &plane = const_cast<StaticPhysObject<GMlib::PPlane<float>>&>(P);
  73 + const auto p = plane.evaluateParent(0.5f, 0.5f, 1, 1); // plane.getMatrixToScene * plane.evaluateParent(0.5f, 0.5f, 1, 1)
  74 + const auto plane_pos = p(0)(0);
  75 + const auto u = p(1)(0);
  76 + const auto v = p(0)(1);
  77 +
  78 +
  79 + const auto n = u ^ v;
  80 + const auto n_normal = GMlib::Vector<float,3>(n).getNormalized();
  81 +
  82 + const auto d = (plane_pos + s_radius * n_normal) - s_position;
  83 +
  84 + const auto Q = (d * n_normal);
  85 + const auto R = ( S.computeTrajectory(new_dt) * n_normal ); // S.computeTrajectory(dt)
  86 +
  87 + const auto epsilon = 0.00001;
  88 +
  89 + if( std::abs(Q) < epsilon )
  90 + {
  91 + return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallelAndTouching);
  92 + }
  93 + else if( std::abs(R) < epsilon)
  94 + {
  95 + return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallel);
  96 + }
  97 +
  98 + const auto x = Q / R;
  99 +
  100 + return CollisionState( (x * new_dt) + dt_min);
  101 + }
24 102  
25 103 void
26 104 computeImpactResponse (DynamicPhysObject<GMlib::PSphere<float>>& S0,
27 105 DynamicPhysObject<GMlib::PSphere<float>>& S1,
28 106 seconds_type dt)
29 107 {
  108 + const auto S0_old_vel = S0.velocity; // 2.1
  109 + const auto S1_old_vel = S1.velocity; // -2.1
  110 +
  111 + const auto S0_pos = S0.getPos();
  112 + const auto S1_pos = S1.getPos();
  113 +
  114 + const auto S0_mass = S0.mass;
  115 + const auto S1_mass = S1.mass;
  116 +
  117 + const auto distance_vector_d = GMlib::Vector<float,3>(S1_pos - S0_pos);
  118 + const auto normal_d = distance_vector_d.getNormalized();
  119 + const auto n = (GMlib::Vector<float,3>(distance_vector_d).getLinIndVec()).getNormalized();
  120 +
  121 + const auto v0_d = (S0_old_vel * normal_d);
  122 + const auto v1_d = (S1_old_vel * normal_d);
  123 + const auto v0_n = (S0_old_vel * n);
  124 + const auto v1_n = (S1_old_vel * n);
  125 +
  126 + const auto new_v0_d = (((S0_mass - S1_mass) / (S0_mass + S1_mass) ) * v0_d ) + (((2 * S1_mass) / (S0_mass + S1_mass) ) * v1_d );
  127 + const auto new_v1_d = (((S1_mass - S0_mass) / (S0_mass + S1_mass) ) * v1_d ) + (((2 * S0_mass) / (S0_mass + S1_mass) ) * v0_d );
  128 +
  129 + const auto S0_new_vel = (v0_n * n) + (new_v0_d * normal_d); // -2.1
  130 + const auto S1_new_vel = v1_n * n + new_v1_d * normal_d; // 2.1
  131 +
  132 + S0.velocity = S0_new_vel;
  133 + S1.velocity = S1_new_vel;
  134 + }
  135 +
  136 + CollisionState detectCollision (const DynamicPSphere& S,
  137 + const StaticPBezierSurf& B, seconds_type dt) {
  138 +
  139 + const auto dt_max = dt;
  140 + const auto dt_min = S.curr_t_in_dt;
  141 + const auto new_dt = dt_max - dt_min;
  142 +
  143 + const auto p0 = S.getPos();
  144 + const auto r = S.getRadius();
  145 +
  146 +
  147 + float u, v, t;
  148 + u = 0.5;
  149 + v = 0.5;
  150 + t = 0.0;
  151 + const auto epsilon = 1e-5;
  152 +
  153 + for ( int i = 0; i < 50; i++) {
  154 +
  155 + auto ds = S.computeTrajectory( new_dt );
  156 + auto &Surf = const_cast<StaticPBezierSurf&>(B);
  157 + const auto surf = Surf.evaluate(u, v, 1, 1);
  158 + const auto p = p0 + ds*t;
  159 + const auto q = surf(0)(0);
  160 + const auto Su = surf(1)(0);
  161 + const auto Sv = surf(0)(1);
  162 + const auto Sn = GMlib::Vector<float,3>(Su ^ Sv).getNormalized();
  163 +
  164 +
  165 +
  166 + GMlib::SqMatrix<float,3> A;
  167 + A.setCol(Su, 0);
  168 + A.setCol(Sv, 1);
  169 + A.setCol(-ds, 2);
  170 + auto A_inv = A;
  171 +
  172 + A_inv.invert();
  173 +
  174 + const auto b = GMlib::Vector<float,3> {p-q-Sn*r};
  175 +
  176 + const auto x = A_inv * b;
  177 +
  178 + const auto deltaU = x(0);
  179 + const auto deltaV = x(1);
  180 + const auto deltaT = x(2);
  181 +
  182 + u += deltaU;
  183 + v += deltaV;
  184 + t += deltaT;
  185 +
  186 + if( (std::abs(deltaU) < epsilon) and (std::abs(deltaV) < epsilon) and (std::abs(deltaT) < epsilon) ) {
  187 +
  188 + return CollisionState(seconds_type(deltaT), CollisionStateFlag::Collision);
  189 +
  190 + }
  191 + }
  192 +
  193 + return CollisionState(seconds_type(dt_min), CollisionStateFlag::SingularityNoCollision);
  194 +
  195 +
  196 + }
  197 +
  198 + void computeImpactResponse (DynamicPSphere& S, const StaticPBezierSurf& B,
  199 + seconds_type dt) {
  200 +
30 201 }
31 202  
32 203 void
33   - computeImpactResponse (DynamicPhysObject<GMlib::PSphere<float>>& S,
34   - StaticPhysObject<GMlib::PPlane<float>>& P,
35   - seconds_type dt)
36   - {
  204 + computeImpactResponse (DynamicPhysObject<GMlib::PSphere<float>>& S,
  205 + const StaticPhysObject<GMlib::PCylinder<float>>& C,
  206 + seconds_type dt) {
  207 +
  208 +
  209 + }
  210 +
  211 + void
  212 + computeImpactResponse (DynamicPhysObject<GMlib::PSphere<float>>& S,
  213 + const StaticPhysObject<GMlib::PPlane<float>>& P,
  214 + seconds_type dt) {
  215 +
  216 + auto &plane = const_cast<StaticPhysObject<GMlib::PPlane<float>>&>(P);
  217 + const auto p = plane.evaluateParent(0.5f, 0.5f, 1, 1);
  218 + const auto u = p(1)(0);
  219 + const auto v = p(0)(1);
  220 +
  221 + const auto n = u ^ v;
  222 + const auto n_normal = GMlib::Vector<float,3>(n).getNormalized();
  223 +
  224 + const auto new_vel = S.velocity - (2*(S.velocity * n_normal) * n_normal);
  225 +
  226 + S.velocity = new_vel;
  227 +
  228 + }
  229 +
  230 + std::unique_ptr<Controller>
  231 + unittestCollisionControllerFactory() {
  232 +
  233 + return std::make_unique<MyController> ();
  234 + }
  235 +
  236 + void
  237 + DynamicPhysObject<GMlib::PSphere<float> >::simulateToTInDt(seconds_type t){
  238 +
  239 + }
  240 +
  241 + GMlib::Vector<float,3>
  242 + DynamicPhysObject<GMlib::PSphere<float> >::computeTrajectory(seconds_type dt) const {
  243 +
  244 + return this->velocity * dt.count() + ( 0.5 * this->externalForces() * std::pow(dt.count(),2) );
37 245 }
  246 +
  247 + GMlib::Vector<float,3>
  248 + DynamicPhysObject<GMlib::PSphere<float> >::externalForces() const {
  249 +
  250 + assert(environment != nullptr);
  251 +
  252 + return this->environment->externalForces();
  253 + }
  254 +
  255 + void
  256 + MyController::localSimulate(double dt) {
  257 +
  258 + }
  259 +
  260 + // Adding objects to vector
  261 + void
  262 + MyController::add(DynamicPSphere * const sphere) {
  263 +
  264 + _dynamic_spheres.push_back(sphere);
  265 + }
  266 + void
  267 + MyController::add(StaticPSphere * const sphere) {
  268 +
  269 + _static_spheres.push_back(sphere);
  270 + }
  271 + void
  272 + MyController::add(StaticPPlane * const plane) {
  273 +
  274 + _static_planes.push_back(plane);
  275 + }
  276 + void
  277 + MyController::add(StaticPCylinder * const cylinder) {
  278 +
  279 + _static_cylinders.push_back(cylinder);
  280 + }
  281 + void
  282 + MyController::add(StaticPBezierSurf * const surf) {
  283 +
  284 + _static_bezier_surf.push_back(surf);
  285 + }
  286 +
38 287 } // END namespace collision
39 288  
... ...
include/collision_library.h
... ... @@ -9,6 +9,64 @@
9 9 namespace collision
10 10 {
11 11  
  12 +class MyController : public Controller {
  13 + GM_SCENEOBJECT (MyController)
  14 +public:
  15 +
  16 + explicit MyController () = default;
  17 +
  18 + void add (DynamicPSphere* const sphere);
  19 + void add (StaticPSphere* const sphere);
  20 + void add (StaticPPlane* const plane);
  21 + void add (StaticPCylinder* const cylinder);
  22 + void add (StaticPBezierSurf* const surf);
  23 +
  24 +protected:
  25 + void localSimulate (double dt) override;
  26 +
  27 + std::vector<DynamicPSphere*> _dynamic_spheres;
  28 + std::vector<StaticPSphere*> _static_spheres;
  29 + std::vector<StaticPPlane*> _static_planes;
  30 + std::vector<StaticPCylinder*> _static_cylinders;
  31 + std::vector<StaticPBezierSurf*> _static_bezier_surf;
  32 +
  33 + std::vector<collision::CollisionObject> _collisions;
  34 +
  35 +
  36 +};
  37 +
  38 +template <>
  39 +class DynamicPhysObject<GMlib::PSphere<float>> : public DynamicPhysObject_Base<GMlib::PSphere<float>> {
  40 +public:
  41 + using DynamicPhysObject_Base<GMlib::PSphere<float>>::DynamicPhysObject_Base;
  42 +
  43 + void simulateToTInDt( seconds_type t ) override;
  44 +
  45 + GMlib::Vector<float, 3>
  46 + computeTrajectory (seconds_type dt) const override; // [m]
  47 +
  48 + GMlib::Vector<float, 3> externalForces () const override; // [m / s^2]
  49 +};
  50 +
  51 +template <class PSurf_T, typename... Arguments>
  52 +std::unique_ptr<DynamicPhysObject<PSurf_T>> unittestDynamicPhysObjectFactory(Arguments... parameters) {
  53 +
  54 + return std::make_unique<DynamicPhysObject<PSurf_T>> (parameters...);
  55 +
  56 +}
  57 +
  58 +template <class PSurf_T, typename... Arguments>
  59 +std::unique_ptr<StaticPhysObject<PSurf_T>> unittestStaticPhysObjectFactory(Arguments... parameters) {
  60 +
  61 + return std::make_unique<StaticPhysObject<PSurf_T>> (parameters...);
  62 +}
  63 +
  64 +
  65 +template <class Container_T >
  66 +void sortAndMakeUnique( Container_T& container) {
  67 +
  68 +
  69 +}
12 70  
13 71 } // END namespace collision
14 72  
... ...