Commit 6cde407f153dae5d7df68e270b35a4c3ac228e6d

Authored by Bjørn-Richard Pedersen
1 parent 4b653fff

Before starting to mess with DetectStateChange function

Showing 2 changed files with 224 additions and 72 deletions   Show diff stats
include/collision_library.cpp
@@ -125,9 +125,10 @@ namespace collision @@ -125,9 +125,10 @@ namespace collision
125 125
126 if( std::abs(Q) < epsilon ) 126 if( std::abs(Q) < epsilon )
127 { 127 {
  128 + // The sphere is "touching" the surface
128 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallelAndTouching); 129 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallelAndTouching);
129 } 130 }
130 - else if( std::abs(R) < epsilon) // Attached and Sliding 131 + else if( std::abs(R) < epsilon)
131 { 132 {
132 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallel); 133 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallel);
133 } 134 }
@@ -150,7 +151,7 @@ namespace collision @@ -150,7 +151,7 @@ namespace collision
150 const auto n = u ^ v; 151 const auto n = u ^ v;
151 const auto n_normal = GMlib::Vector<float,3>(n).getNormalized(); 152 const auto n_normal = GMlib::Vector<float,3>(n).getNormalized();
152 153
153 - const auto new_vel = S.velocity - (2*(S.velocity * n_normal) * n_normal); 154 + auto new_vel = S.velocity - (2*(S.velocity * n_normal) * n_normal) * 0.95;
154 155
155 S.velocity = new_vel; 156 S.velocity = new_vel;
156 157
@@ -298,8 +299,8 @@ namespace collision @@ -298,8 +299,8 @@ namespace collision
298 299
299 // Update physics 300 // Update physics
300 auto F = this->externalForces(); 301 auto F = this->externalForces();
301 - auto a = 0.5 * F * (this->mass) * std::pow(dt.count(), 2);  
302 - 302 + auto c = dt.count();
  303 + auto a = F*c;
303 this->velocity += a; 304 this->velocity += a;
304 } 305 }
305 306
@@ -321,15 +322,21 @@ namespace collision @@ -321,15 +322,21 @@ namespace collision
321 GMlib::Vector<double, 3> 322 GMlib::Vector<double, 3>
322 DynamicPhysObject<GMlib::PSphere<float> >::getTrajectory(seconds_type dt) { 323 DynamicPhysObject<GMlib::PSphere<float> >::getTrajectory(seconds_type dt) {
323 324
324 - return this->_trajectory; 325 + return this->_NewTrajectory;
325 } 326 }
326 327
327 void 328 void
328 - DynamicPhysObject<GMlib::PSphere<float> >::setTrajectory(GMlib::Vector<double, 3> t) { 329 + DynamicPhysObject<GMlib::PSphere<float> >::setTrajectory(GMlib::Vector<double, 3> ds, DynamicPSphere* sphere) {
329 330
330 // Update ds to modified DS' 331 // Update ds to modified DS'
331 - // Can maybe do check on state in here, probably leave it to state-loop  
332 - this->_trajectory = t; 332 + auto r = sphere->getRadius();
  333 + auto s = sphere->getPos();
  334 + auto p = ds + s;
  335 +// GMlib::Vector<d,3> test = (p * s).getNormalized();
  336 +
  337 +// auto adj_ds = ds + ( r + (p * s) ).getNormalized();
  338 +
  339 +
333 } 340 }
334 341
335 GMlib::Vector<double,3> 342 GMlib::Vector<double,3>
@@ -337,7 +344,7 @@ namespace collision @@ -337,7 +344,7 @@ namespace collision
337 344
338 assert(environment != nullptr); 345 assert(environment != nullptr);
339 346
340 - return this->environment->externalForces(); 347 + return this->environment->externalForces().toType<double>();
341 } 348 }
342 349
343 void 350 void
@@ -349,11 +356,17 @@ namespace collision @@ -349,11 +356,17 @@ namespace collision
349 sphere->curr_t_in_dt = seconds_type{0.0}; 356 sphere->curr_t_in_dt = seconds_type{0.0};
350 } 357 }
351 358
352 - // Collision detection algorithm 359 + // Singularity detection
  360 + singularityDetection(dt);
353 361
354 - //Check for collisions for sphere  
355 - for( auto& sphere : _dynamic_spheres) { 362 + // StateChangeDetection
  363 + for(auto& s : _dynamic_spheres){
356 364
  365 + detectStateChange(s, dt);
  366 + }
  367 +
  368 + // Collision detection algorithm
  369 + for( auto& sphere : _dynamic_spheres) {
357 370
358 // Sending in sphere twice in the initial call because the function will require a second object in future calls 371 // Sending in sphere twice in the initial call because the function will require a second object in future calls
359 // Må da være en bedre måte å gjøre det på 372 // Må da være en bedre måte å gjøre det på
@@ -365,12 +378,12 @@ namespace collision @@ -365,12 +378,12 @@ namespace collision
365 } 378 }
366 } 379 }
367 380
368 - // Check for other (non-collision) singularities  
369 -  
370 // Make Collision unique 381 // Make Collision unique
371 sortAndMakeUnique(_collisions); 382 sortAndMakeUnique(_collisions);
372 std::reverse(_collisions.begin(), _collisions.end()); 383 std::reverse(_collisions.begin(), _collisions.end());
373 384
  385 +
  386 +
374 // Impact response 387 // Impact response
375 while( !_collisions.empty() ) { 388 while( !_collisions.empty() ) {
376 389
@@ -484,12 +497,95 @@ namespace collision @@ -484,12 +497,95 @@ namespace collision
484 497
485 } 498 }
486 499
  500 + // Singularity Detection
  501 + void
  502 + MyController::singularityDetection(double dt) {
  503 +
  504 + // Sphere vs. Plane
  505 + for( auto& sphere : _dynamic_spheres) {
  506 + for( auto& plane : _static_planes) {
  507 +
  508 + auto detection = collision::detectCollision( *sphere, *plane, seconds_type(dt));
  509 +
  510 + // The Sphere is parallell and touching the sphere = Sphere attached to Plane
  511 + if(detection.CollisionState::flag == CollisionStateFlag::SingularityParallelAndTouching) {
  512 + auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Rolling );
  513 + _singularities.push_back(singularity);
  514 + }
  515 + // The Sphere is Not / no longer attached to the plane. Remove the plane from our map of attachments
  516 + if(detection.CollisionState::flag != CollisionStateFlag::SingularityParallelAndTouching) {
  517 +
  518 + auto planes = getAttachedObjects(sphere);
  519 +
  520 + if( !planes.empty() ) {
  521 +
  522 + for( auto p : planes){
  523 +
  524 + if( plane == p) detachObjects(p, sphere);
  525 + }
  526 + }
  527 + }
  528 + // if other flags
  529 + }
  530 + }
  531 +
  532 + // Sphere vs. Other types of surface
  533 + } // EOF
  534 +
  535 +
  536 + /// vector
  537 + // Get objects attached to sphere
  538 + std::vector<StaticPPlane *>
  539 + MyController::getAttachedObjects(DynamicPSphere* sphere)
  540 + {
  541 + return (_map[sphere]);
  542 + }
  543 +
  544 + // Set objects attached to sphere
  545 + void
  546 + MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
  547 + {
  548 + _map[sphere].push_back(plane);
  549 + }
  550 +
  551 + // Remove objects from the set In the map
  552 + void
  553 + MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
  554 +
  555 +// _map[sphere].erase(plane);
  556 + }
  557 +
  558 +
  559 + /// unordered_set
  560 + // Get objects attached to sphere
  561 +// std::unordered_set<StaticPPlane *>
  562 +// MyController::getAttachedObjects(DynamicPSphere* sphere)
  563 +// {
  564 +// return (_map[sphere]);
  565 +// }
  566 +
  567 +// // Set objects attached to sphere
  568 +// void
  569 +// MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
  570 +// {
  571 +// _map[sphere].emplace(plane);
  572 +// }
  573 +
  574 +// // Remove objects from the set In the map
  575 +// void
  576 +// MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
  577 +
  578 +// _map[sphere].erase(plane);
  579 +// }
  580 +
487 // Adding objects to vector 581 // Adding objects to vector
  582 +
488 void 583 void
489 MyController::add(DynamicPSphere * const sphere) { 584 MyController::add(DynamicPSphere * const sphere) {
490 585
491 sphere->environment = &_env; 586 sphere->environment = &_env;
492 _dynamic_spheres.push_back(sphere); 587 _dynamic_spheres.push_back(sphere);
  588 + _map[sphere];
493 589
494 } 590 }
495 void 591 void
@@ -513,32 +609,63 @@ namespace collision @@ -513,32 +609,63 @@ namespace collision
513 _static_bezier_surf.push_back(surf); 609 _static_bezier_surf.push_back(surf);
514 } 610 }
515 611
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 - } 612 + // States
  613 +
  614 + // With vector
  615 + StateChangeObj
  616 + MyController::detectStateChange( DynamicPSphere* sphere, double dt) {
  617 +
  618 + auto planes = this->getAttachedObjects(sphere) ;
  619 + auto r = sphere->getRadius();
  620 + auto p = sphere->getMatrixToScene() * sphere->getPos();
  621 +
  622 + if (planes.empty()){
  623 + sphere->_state = DynamicPSphere::States::Free;
  624 + }
  625 + else{
  626 + auto epsilon = 1e-5;
  627 +
  628 + // Using original DS to compute states
  629 + auto dts = seconds_type(dt);
  630 + auto max_dt = dts;
  631 + auto min_dt = sphere->curr_t_in_dt;
  632 + auto new_dt = max_dt -min_dt;
  633 + auto ds = sphere->computeTrajectory(new_dt);
  634 +
  635 + GMlib::APoint<float,3> q;
  636 + GMlib::Vector <float,3>n {0.0f,0.0f,0.0f};
  637 + for (auto &it :planes){
  638 + auto M = it->evaluateParent(0.5f,0.5f,1,1);
  639 + auto pos= M(0)(0);
  640 + auto u = M(1)(0);
  641 + auto v = M(0)(1);
  642 + auto normal = GMlib::Vector<float,3>(u ^ v);
  643 + n+=normal;
  644 + q=pos;
  645 + }
  646 + n= GMlib::Vector <float,3>(n/planes.size()).getNormalized();
527 647
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 - } 648 + auto d = (q + r * n) - p;
  649 + auto bla=std::abs(((-n*r) * ds) -(ds*ds));
  650 + auto dsn= ds * n;
  651 + auto dn= d*n;
534 652
535 - void  
536 - DynamicPhysObject<GMlib::PSphere<float> >::setState(States s) { 653 + if (std::abs(((-n*r) * ds) -(ds*ds)) < epsilon) {
  654 + sphere->_state=DynamicPSphere::States::AtRest;
  655 + std::cout << "State is now: At Rest" << std::endl;
  656 + }
  657 + else if(std::abs(((-n*r) * ds) -(ds*ds)) > epsilon) {
  658 + sphere->_state = DynamicPSphere::States::Rolling;
  659 + std::cout << "State is now: Rolling" << std::endl;
  660 +// sphere->setTrajectory
  661 + }
537 662
538 - this->_state = s; 663 + }
539 } 664 }
540 665
541 666
  667 +// } // EOF
  668 +
542 669
543 } // END namespace collision 670 } // END namespace collision
544 671
include/collision_library.h
@@ -5,11 +5,15 @@ @@ -5,11 +5,15 @@
5 #include <collision_interface.h> 5 #include <collision_interface.h>
6 6
7 #include <typeinfo> 7 #include <typeinfo>
8 - 8 +#include <unordered_map>
9 9
10 namespace collision 10 namespace collision
11 { 11 {
12 12
  13 + // "Declaration" of the struct
  14 + struct StateChangeObj;
  15 +
  16 +
13 class MyController : public Controller { 17 class MyController : public Controller {
14 GM_SCENEOBJECT (MyController) 18 GM_SCENEOBJECT (MyController)
15 public: 19 public:
@@ -22,6 +26,19 @@ public: @@ -22,6 +26,19 @@ public:
22 void add (StaticPCylinder* const cylinder); 26 void add (StaticPCylinder* const cylinder);
23 void add (StaticPBezierSurf* const surf); 27 void add (StaticPBezierSurf* const surf);
24 28
  29 + // Change "value" and return type if the sphere should be able to be attached to objects other than planes
  30 +// std::unordered_set<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
  31 + std::vector<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
  32 + void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
  33 + void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);
  34 +
  35 + StateChangeObj
  36 + detectStateChange(DynamicPSphere* s, double time);
  37 +
  38 + void
  39 + singularityDetection(double dt);
  40 +
  41 +
25 CollisionState detectCollision (const DynamicPSphere& S, 42 CollisionState detectCollision (const DynamicPSphere& S,
26 const DynamicPCylinder& C, seconds_type dt); 43 const DynamicPCylinder& C, seconds_type dt);
27 44
@@ -61,10 +78,16 @@ protected: @@ -61,10 +78,16 @@ protected:
61 std::vector<StaticPPlane*> _static_planes; 78 std::vector<StaticPPlane*> _static_planes;
62 std::vector<StaticPCylinder*> _static_cylinders; 79 std::vector<StaticPCylinder*> _static_cylinders;
63 std::vector<StaticPBezierSurf*> _static_bezier_surf; 80 std::vector<StaticPBezierSurf*> _static_bezier_surf;
  81 +
64 std::vector<collision::CollisionObject> _collisions; 82 std::vector<collision::CollisionObject> _collisions;
  83 + std::vector<StateChangeObj> _singularities;
  84 +
  85 + DefaultEnvironment _env;
  86 +
  87 +// std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>> _map;
  88 + std::unordered_map<DynamicPSphere*, std::vector<StaticPPlane*>> _map;
65 89
66 90
67 - Environment _env;  
68 91
69 92
70 }; 93 };
@@ -76,47 +99,42 @@ public: @@ -76,47 +99,42 @@ public:
76 99
77 enum class States { 100 enum class States {
78 Free, 101 Free,
79 - Sliding, 102 + Rolling,
80 AtRest 103 AtRest
81 }; 104 };
82 105
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 - }; 106 + MyController* _controller;
  107 + States _state = States::Free; // Which state is the sphere in
91 108
92 - StaticPPlane* getAttachedTo();  
93 - void setAttachedTo(StaticPPlane* plane); 109 + GMlib::Vector<double, 3>
  110 + getTrajectory (seconds_type dt);
94 111
95 - void setState(States s); 112 + void
  113 + setTrajectory (GMlib::Vector<double, 3> ds, DynamicPSphere* sphere);
96 114
97 - std::vector<StateChangeObj*>  
98 - detectStateChange(DynamicPhysObject<GMlib::PSphere<float>>* S, double t); 115 + GMlib::Vector<double, 3> _NewTrajectory;
99 116
  117 + // "Old" methods
100 void simulateToTInDt( seconds_type t ) override; 118 void simulateToTInDt( seconds_type t ) override;
101 119
102 GMlib::Vector<double, 3> 120 GMlib::Vector<double, 3>
103 computeTrajectory (seconds_type dt) const override; // [m] 121 computeTrajectory (seconds_type dt) const override; // [m]
104 122
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 -  
113 GMlib::Vector<double, 3> externalForces () const override; // [m / s^2] 123 GMlib::Vector<double, 3> externalForces () const override; // [m / s^2]
114 124
115 -private:  
116 125
117 - States _state;  
118 - StaticPPlane* _plane; 126 +};
  127 +
  128 +// StateChangeObject struct
  129 +struct StateChangeObj {
  130 + PhysObj_Base* obj1; // Object whos state will change
  131 + PhysObj_Base* obj2; // Object that obj1 will change state ACCORDING to
  132 + seconds_type time; // Time of singularity
  133 + DynamicPSphere::States state; // State that obj1 will change to
119 134
  135 + StateChangeObj
  136 + (PhysObj_Base* o1, PhysObj_Base* o2, seconds_type t, DynamicPSphere::States s) :
  137 + obj1{o1}, obj2{o2}, time{t} ,state{s} {}
120 }; 138 };
121 139
122 template <class PSurf_T, typename... Arguments> 140 template <class PSurf_T, typename... Arguments>
@@ -246,6 +264,8 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type @@ -246,6 +264,8 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type
246 // Sphere vs. Static Plane 264 // Sphere vs. Static Plane
247 if( plane ) { 265 if( plane ) {
248 266
  267 + auto attachedPlanes = getAttachedObjects(sphere);
  268 +
249 for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) { 269 for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
250 270
251 // The sphere cannot collide with the same plane it just collided with 271 // The sphere cannot collide with the same plane it just collided with
@@ -253,23 +273,28 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type @@ -253,23 +273,28 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type
253 // break; 273 // break;
254 // } 274 // }
255 275
256 - auto col = collision::detectCollision(*sphere, **iter, dt); 276 + // If sphere is attached to the plane, it should NOT check for collisions with it
  277 +// for( auto pl = attachedPlanes.begin(); pl != attachedPlanes.end(); pl++) {
257 278
258 - if( col.CollisionState::flag == CollisionStateFlag::Collision ) { 279 +// if( *iter == *pl) break;
259 280
260 - auto& static_object = *iter; 281 + // Else look for collision
  282 + auto col = collision::detectCollision(*sphere, **iter, dt);
261 283
262 - auto new_t = sphere->curr_t_in_dt; 284 + if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
263 285
264 - if( col.time > seconds_type(new_t) and col.time < dt)  
265 - {  
266 - auto col_obj = CollisionObject(sphere, static_object, col.time);  
267 - _collisions.push_back(col_obj); 286 + auto& static_object = *iter;
268 287
269 - } 288 + auto new_t = sphere->curr_t_in_dt;
270 289
271 - // Other singularities  
272 - } 290 + if( col.time > seconds_type(new_t) and col.time < dt)
  291 + {
  292 + auto col_obj = CollisionObject(sphere, static_object, col.time);
  293 + _collisions.push_back(col_obj);
  294 +
  295 + }
  296 + }
  297 +// } // THIS ONE
273 } 298 }
274 } 299 }
275 300