Commit 4b653fff5fa2c6f2577f95ddd13895c06a738746

Authored by Bjørn-Richard Pedersen
1 parent 8e18b3d5

Started working on States

Showing 2 changed files with 139 additions and 36 deletions   Show diff stats
include/collision_library.cpp
... ... @@ -127,7 +127,7 @@ namespace collision
127 127 {
128 128 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallelAndTouching);
129 129 }
130   - else if( std::abs(R) < epsilon)
  130 + else if( std::abs(R) < epsilon) // Attached and Sliding
131 131 {
132 132 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallel);
133 133 }
... ... @@ -311,14 +311,27 @@ namespace collision
311 311 auto vel = this->velocity;
312 312 auto dtCount = dt.count(); // 0
313 313 auto xF = this->externalForces();
314   - auto mas = this->mass;
315 314  
316   - return vel * dtCount + 0.5 * xF * mas * std::pow(dtCount,2);
  315 + return vel * dtCount + 0.5 * xF * std::pow(dtCount,2);
317 316  
318 317 // return this->velocity * dt.count() + ( 0.5 * this->externalForces() * (this->mass) * std::pow(dt.count(),2) );
319 318  
320 319 }
321 320  
  321 + GMlib::Vector<double, 3>
  322 + DynamicPhysObject<GMlib::PSphere<float> >::getTrajectory(seconds_type dt) {
  323 +
  324 + return this->_trajectory;
  325 + }
  326 +
  327 + void
  328 + DynamicPhysObject<GMlib::PSphere<float> >::setTrajectory(GMlib::Vector<double, 3> t) {
  329 +
  330 + // Update ds to modified DS'
  331 + // Can maybe do check on state in here, probably leave it to state-loop
  332 + this->_trajectory = t;
  333 + }
  334 +
322 335 GMlib::Vector<double,3>
323 336 DynamicPhysObject<GMlib::PSphere<float> >::externalForces() const {
324 337  
... ... @@ -337,19 +350,23 @@ namespace collision
337 350 }
338 351  
339 352 // Collision detection algorithm
340   - _collisions.clear(); // Initialize
341   -
342 353  
343   - //Check for collisions between Spheres
  354 + //Check for collisions for sphere
344 355 for( auto& sphere : _dynamic_spheres) {
345 356  
  357 +
346 358 // Sending in sphere twice in the initial call because the function will require a second object in future calls
347 359 // Må da være en bedre måte å gjøre det på
348   -
349 360 sphereDynamicCollision(sphere, sphere, seconds_type(dt));
350   - sphereStaticCollision(sphere, sphere, seconds_type(dt));
  361 +
  362 + for( auto& plane : _static_planes ) {
  363 +
  364 + sphereStaticCollision(sphere, plane, seconds_type(dt));
  365 + }
351 366 }
352 367  
  368 + // Check for other (non-collision) singularities
  369 +
353 370 // Make Collision unique
354 371 sortAndMakeUnique(_collisions);
355 372 std::reverse(_collisions.begin(), _collisions.end());
... ... @@ -375,21 +392,23 @@ namespace collision
375 392  
376 393  
377 394  
378   - // First object is sphere
  395 + // Impact response
  396 + // If the first object is a sphere
379 397 if(d_sphere_1) {
380 398  
  399 +
381 400 if (d_sphere_2)
382   - computeImpactResponse( *d_sphere_1, *d_sphere_2, c.t_in_dt); // D_Sphere vs. D_Sphere
  401 + collision::computeImpactResponse( *d_sphere_1, *d_sphere_2, c.t_in_dt); // D_Sphere vs. D_Sphere
383 402 else if (d_sphere_1 && s_sphere_2)
384   - computeImpactResponse( *d_sphere_1, *s_sphere_2, c.t_in_dt); // D_Sphere vs. S_Sphere
  403 + collision::computeImpactResponse( *d_sphere_1, *s_sphere_2, c.t_in_dt); // D_Sphere vs. S_Sphere
385 404 else if (d_sphere_1 && d_cylinder_2)
386   - computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. D_Cylinder
  405 + collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. D_Cylinder
387 406 else if (d_sphere_1 && s_cylinder_2)
388   - computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. S_Cylinder
  407 + collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. S_Cylinder
389 408 else if (d_sphere_1 && s_plane_2)
390   - computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
  409 + collision::computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
391 410 else if (d_sphere_1 && s_bezier_2)
392   - computeImpactResponse( *d_sphere_1, *s_bezier_2, c.t_in_dt); // D_Sphere vs. S_Bezier
  411 + collision::computeImpactResponse( *d_sphere_1, *s_bezier_2, c.t_in_dt); // D_Sphere vs. S_Bezier
393 412  
394 413 }
395 414  
... ... @@ -397,17 +416,17 @@ namespace collision
397 416 if(d_cylinder_1) {
398 417  
399 418 if (d_cylinder_2)
400   - computeImpactResponse( *d_cylinder_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. D_Cylinder
  419 + collision::computeImpactResponse( *d_cylinder_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. D_Cylinder
401 420 else if (d_cylinder_1 && s_cylinder_2)
402   - computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. S_Cylinder
  421 + collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. S_Cylinder
403 422 else if (d_cylinder_1 && d_sphere_2)
404   - computeImpactResponse( *d_sphere_2, *d_cylinder_1, c.t_in_dt); // D_Cylinder vs. D_Sphere
  423 + collision::computeImpactResponse( *d_sphere_2, *d_cylinder_1, c.t_in_dt); // D_Cylinder vs. D_Sphere
405 424 else if (d_cylinder_1 && s_sphere_2)
406   - computeImpactResponse( *d_cylinder_1, *s_sphere_2, c.t_in_dt); // D_Cylinder vs. S_Sphere
  425 + collision::computeImpactResponse( *d_cylinder_1, *s_sphere_2, c.t_in_dt); // D_Cylinder vs. S_Sphere
407 426 else if (d_cylinder_1 && s_bezier_2)
408   - computeImpactResponse( *d_cylinder_1, *s_bezier_2, c.t_in_dt); // D_Cylinder vs. S_Bezier
  427 + collision::computeImpactResponse( *d_cylinder_1, *s_bezier_2, c.t_in_dt); // D_Cylinder vs. S_Bezier
409 428 else if (d_cylinder_1 && s_plane_2)
410   - computeImpactResponse( *d_cylinder_1, *s_plane_2, c.t_in_dt); // D_Cylinder vs. S_Plane
  429 + collision::computeImpactResponse( *d_cylinder_1, *s_plane_2, c.t_in_dt); // D_Cylinder vs. S_Plane
411 430  
412 431 }
413 432  
... ... @@ -417,8 +436,6 @@ namespace collision
417 436 // If the dynamic object (obj1) is a sphere
418 437 if( d_sphere_1) {
419 438  
420   -
421   -
422 439 sphereDynamicCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with same obj as last time
423 440 sphereStaticCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Can't with same obj as last time
424 441  
... ... @@ -437,7 +454,7 @@ namespace collision
437 454 }
438 455 }
439 456  
440   - // If the dynamic object (obj1) is a cylinder
  457 +// If the dynamic object (obj1) is a cylinder
441 458 if(d_cylinder_1) {
442 459  
443 460 //cylinderDynamicCollision(d_cylinder_1, c.obj2, seconds_type(dt));
... ... @@ -456,15 +473,13 @@ namespace collision
456 473 }
457 474 }
458 475  
  476 +
459 477 }
460 478  
461 479 // Start simulation for all objects
462 480 for( auto sphere : _dynamic_spheres) {
463 481  
464 482 sphere->simulateToTInDt(seconds_type(dt));
465   -
466   -
467   -
468 483 }
469 484  
470 485 }
... ... @@ -498,6 +513,33 @@ namespace collision
498 513 _static_bezier_surf.push_back(surf);
499 514 }
500 515  
  516 + StaticPPlane *
  517 + DynamicPhysObject<GMlib::PSphere<float> >::getAttachedTo()
  518 + {
  519 + return this->_plane;
  520 + }
  521 +
  522 + void
  523 + DynamicPhysObject<GMlib::PSphere<float> >::setAttachedTo(StaticPPlane *p)
  524 + {
  525 + this->_plane = p;
  526 + }
  527 +
  528 + std::vector<DynamicPhysObject<GMlib::PSphere<float> >::StateChangeObj *>
  529 + DynamicPhysObject<GMlib::PSphere<float> >::detectStateChange(DynamicPhysObject<GMlib::PSphere<float> > *S, double t)
  530 + {
  531 + auto s = S;
  532 + auto p = s->getAttachedTo();
  533 + }
  534 +
  535 + void
  536 + DynamicPhysObject<GMlib::PSphere<float> >::setState(States s) {
  537 +
  538 + this->_state = s;
  539 + }
  540 +
  541 +
  542 +
501 543 } // END namespace collision
502 544  
503 545  
... ...
include/collision_library.h
... ... @@ -22,6 +22,27 @@ public:
22 22 void add (StaticPCylinder* const cylinder);
23 23 void add (StaticPBezierSurf* const surf);
24 24  
  25 + CollisionState detectCollision (const DynamicPSphere& S,
  26 + const DynamicPCylinder& C, seconds_type dt);
  27 +
  28 + CollisionState detectCollision (const DynamicPCylinder& C0,
  29 + const DynamicPCylinder& C, seconds_type dt);
  30 +
  31 + void computeImpactResponse (DynamicPSphere& S, DynamicPCylinder& C,
  32 + seconds_type dt);
  33 +
  34 + void computeImpactResponse (DynamicPCylinder& C0, DynamicPCylinder& C1,
  35 + seconds_type dt);
  36 +
  37 + void computeImpactResponse (DynamicPCylinder& C, const StaticPSphere& S,
  38 + seconds_type dt);
  39 +
  40 + void computeImpactResponse (DynamicPCylinder& C, const StaticPBezierSurf& B,
  41 + seconds_type dt);
  42 +
  43 + void computeImpactResponse (DynamicPCylinder& C, const StaticPPlane& P,
  44 + seconds_type dt);
  45 +
25 46  
26 47 private:
27 48  
... ... @@ -43,7 +64,6 @@ protected:
43 64 std::vector<collision::CollisionObject> _collisions;
44 65  
45 66  
46   -
47 67 Environment _env;
48 68  
49 69  
... ... @@ -54,12 +74,49 @@ class DynamicPhysObject&lt;GMlib::PSphere&lt;float&gt;&gt; : public DynamicPhysObject_Base&lt;G
54 74 public:
55 75 using DynamicPhysObject_Base<GMlib::PSphere<float>>::DynamicPhysObject_Base;
56 76  
57   - void simulateToTInDt( seconds_type t ) override;
  77 + enum class States {
  78 + Free,
  79 + Sliding,
  80 + AtRest
  81 + };
  82 +
  83 + struct StateChangeObj {
  84 + DynamicPSphere* obj;
  85 + seconds_type time;
  86 + States state;
  87 +
  88 + StateChangeObj
  89 + ( PhysObj_Base* o, seconds_type t, DynamicPSphere::States s) {}
  90 + };
  91 +
  92 + StaticPPlane* getAttachedTo();
  93 + void setAttachedTo(StaticPPlane* plane);
  94 +
  95 + void setState(States s);
  96 +
  97 + std::vector<StateChangeObj*>
  98 + detectStateChange(DynamicPhysObject<GMlib::PSphere<float>>* S, double t);
  99 +
  100 + void simulateToTInDt( seconds_type t ) override;
58 101  
59 102 GMlib::Vector<double, 3>
60 103 computeTrajectory (seconds_type dt) const override; // [m]
61 104  
  105 + GMlib::Vector<double, 3>
  106 + getTrajectory (seconds_type dt);
  107 +
  108 + void
  109 + setTrajectory (GMlib::Vector<double, 3>);
  110 +
  111 + GMlib::Vector<double, 3> _trajectory;
  112 +
62 113 GMlib::Vector<double, 3> externalForces () const override; // [m / s^2]
  114 +
  115 +private:
  116 +
  117 + States _state;
  118 + StaticPPlane* _plane;
  119 +
63 120 };
64 121  
65 122 template <class PSurf_T, typename... Arguments>
... ... @@ -150,7 +207,7 @@ void MyController::sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type
150 207 else {
151 208  
152 209 // Check only for collisions for the first dynamic sphere
153   - auto col = detectCollision(*sphere, **iter, dt);
  210 + auto col = collision::detectCollision(*sphere, **iter, dt);
154 211  
155 212 // The check for if the second object is a dynamic sphere is done in the main algorithm
156 213 // and the method is called again only with the 1'st and 2'nd sphere swapped
... ... @@ -165,13 +222,14 @@ void MyController::sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type
165 222 if( col.time > seconds_type(new_t) and col.time < dt) {
166 223 auto col_obj = CollisionObject(first_sphere, second_sphere, col.time);
167 224 _collisions.push_back(col_obj);
  225 +
  226 + // Simulate the objects to T in dt?
  227 +
168 228 }
169 229 }
170 230 }
171 231 }
172 232 }
173   -
174   -
175 233 }
176 234  
177 235 template <typename T_s, typename T_o>
... ... @@ -191,11 +249,11 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type
191 249 for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
192 250  
193 251 // The sphere cannot collide with the same plane it just collided with
194   - if( *iter == plane ) {
195   - break;
196   - }
  252 +// if( *iter == plane ) {
  253 +// break;
  254 +// }
197 255  
198   - auto col = detectCollision(*sphere, **iter, dt);
  256 + auto col = collision::detectCollision(*sphere, **iter, dt);
199 257  
200 258 if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
201 259  
... ... @@ -207,7 +265,10 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type
207 265 {
208 266 auto col_obj = CollisionObject(sphere, static_object, col.time);
209 267 _collisions.push_back(col_obj);
  268 +
210 269 }
  270 +
  271 + // Other singularities
211 272 }
212 273 }
213 274 }
... ...