Commit caaad402949151ce3718b67f873ba21f7d9da219

Authored by Bjørn-Richard Pedersen
1 parent 6cde407f

Updated Detection / setting of states

Showing 2 changed files with 125 additions and 112 deletions   Show diff stats
include/collision_library.cpp
@@ -309,14 +309,24 @@ namespace collision @@ -309,14 +309,24 @@ namespace collision
309 GMlib::Vector<double,3> 309 GMlib::Vector<double,3>
310 DynamicPhysObject<GMlib::PSphere<float> >::computeTrajectory(seconds_type dt) const { 310 DynamicPhysObject<GMlib::PSphere<float> >::computeTrajectory(seconds_type dt) const {
311 311
312 - auto vel = this->velocity;  
313 - auto dtCount = dt.count(); // 0  
314 - auto xF = this->externalForces();  
315 312
316 - return vel * dtCount + 0.5 * xF * std::pow(dtCount,2); 313 + if(this->_state == DynamicPSphere::States::Rolling) {
317 314
318 -// return this->velocity * dt.count() + ( 0.5 * this->externalForces() * (this->mass) * std::pow(dt.count(),2) ); 315 + auto r = this->getRadius();
  316 + auto s = this->getPos();
319 317
  318 + }
  319 + else if( this->_state == DynamicPSphere::States::AtRest) {
  320 +
  321 + }
  322 + else {
  323 +
  324 + auto vel = this->velocity;
  325 + auto dtCount = dt.count(); // 0
  326 + auto xF = this->externalForces();
  327 +
  328 + return vel * dtCount + 0.5 * xF * std::pow(dtCount,2);
  329 + }
320 } 330 }
321 331
322 GMlib::Vector<double, 3> 332 GMlib::Vector<double, 3>
@@ -358,11 +368,13 @@ namespace collision @@ -358,11 +368,13 @@ namespace collision
358 368
359 // Singularity detection 369 // Singularity detection
360 singularityDetection(dt); 370 singularityDetection(dt);
  371 +// sortAndMakeUnique(_singularities);
  372 +// std::reverse(_singularities.begin(), _singularities.end());
361 373
362 // StateChangeDetection 374 // StateChangeDetection
363 - for(auto& s : _dynamic_spheres){ 375 + for( auto& singularity : _singularities) {
364 376
365 - detectStateChange(s, dt); 377 + detectStateChange(singularity);
366 } 378 }
367 379
368 // Collision detection algorithm 380 // Collision detection algorithm
@@ -505,60 +517,53 @@ namespace collision @@ -505,60 +517,53 @@ namespace collision
505 for( auto& sphere : _dynamic_spheres) { 517 for( auto& sphere : _dynamic_spheres) {
506 for( auto& plane : _static_planes) { 518 for( auto& plane : _static_planes) {
507 519
508 - auto detection = collision::detectCollision( *sphere, *plane, seconds_type(dt)); 520 + auto r = sphere->getRadius();
  521 + auto p = sphere->getMatrixToScene() * sphere->getPos();
509 522
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) { 523 + auto epsilon = 1e-5;
  524 +
  525 + auto dts = seconds_type(dt);
  526 + auto max_dt = dts;
  527 + auto min_dt = sphere->curr_t_in_dt;
  528 + auto new_dt = max_dt -min_dt;
  529 + auto ds = sphere->computeTrajectory(new_dt);
517 530
518 - auto planes = getAttachedObjects(sphere); 531 + auto M = plane->evaluateParent(0.5f,0.5f,1,1);
  532 + auto q = M(0)(0);
  533 + auto u = M(1)(0);
  534 + auto v = M(0)(1);
  535 + auto n = GMlib::Vector<float,3>(u ^ v);
519 536
520 - if( !planes.empty() ) { 537 + // Helper variables for debugging
  538 + auto d = (q + r * n) - p;
  539 + auto still = std::abs(((-n*r) * ds) -(ds*ds)); // < epsilon => Criteria for Still. > epsilon => Criteria for Rolling
  540 + auto dsn = ds * n;
  541 + auto dn = d*n;
521 542
522 - for( auto p : planes){ 543 + if ( std::abs( ((-n*r) * ds) -(ds*ds) ) < epsilon) {
523 544
524 - if( plane == p) detachObjects(p, sphere);  
525 - }  
526 - } 545 + auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::AtRest);
  546 + _singularities.push_back(singularity);
  547 + }
  548 + else if(std::abs(((-n*r) * ds) -(ds*ds)) > epsilon) {
  549 +
  550 + auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Rolling);
  551 + _singularities.push_back(singularity);
  552 + }
  553 + else if( (ds * n) <= 0) {
  554 +
  555 + auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Free);
  556 + _singularities.push_back(singularity);
527 } 557 }
528 - // if other flags  
529 } 558 }
530 } 559 }
531 -  
532 - // Sphere vs. Other types of surface  
533 } // EOF 560 } // EOF
534 561
535 562
536 - /// vector 563 + /// vector; Get, Set and Remove objects to / from Sphere
  564 + /*
537 // Get objects attached to sphere 565 // 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 *> 566 +// std::vector<StaticPPlane *>
562 // MyController::getAttachedObjects(DynamicPSphere* sphere) 567 // MyController::getAttachedObjects(DynamicPSphere* sphere)
563 // { 568 // {
564 // return (_map[sphere]); 569 // return (_map[sphere]);
@@ -568,16 +573,40 @@ namespace collision @@ -568,16 +573,40 @@ namespace collision
568 // void 573 // void
569 // MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere) 574 // MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
570 // { 575 // {
571 -// _map[sphere].emplace(plane); 576 +// _map[sphere].push_back(plane);
572 // } 577 // }
573 578
574 // // Remove objects from the set In the map 579 // // Remove objects from the set In the map
575 // void 580 // void
576 // MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){ 581 // MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
577 582
578 -// _map[sphere].erase(plane); 583 +//// _map[sphere].erase(plane);
579 // } 584 // }
  585 +*/
580 586
  587 + /// unordered_set; Get, Set and Remove objects to / from Sphere
  588 + /*
  589 + // Get objects attached to sphere
  590 + std::unordered_set<StaticPPlane *>
  591 + MyController::getAttachedObjects(DynamicPSphere* sphere)
  592 + {
  593 + return (_map[sphere]);
  594 + }
  595 +
  596 + // Set objects attached to sphere
  597 + void
  598 + MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
  599 + {
  600 + _map[sphere].emplace(plane);
  601 + }
  602 +
  603 + // Remove objects from the set In the map
  604 + void
  605 + MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
  606 +
  607 + _map[sphere].erase(plane);
  608 + }
  609 +*/
581 // Adding objects to vector 610 // Adding objects to vector
582 611
583 void 612 void
@@ -609,62 +638,43 @@ namespace collision @@ -609,62 +638,43 @@ namespace collision
609 _static_bezier_surf.push_back(surf); 638 _static_bezier_surf.push_back(surf);
610 } 639 }
611 640
  641 +
612 // States 642 // States
  643 + void
  644 + MyController::detectStateChange( StateChangeObj State ) {
613 645
614 - // With vector  
615 - StateChangeObj  
616 - MyController::detectStateChange( DynamicPSphere* sphere, double dt) {  
617 646
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(); 647 + auto sphere = State.obj1;
  648 + auto plane = State.obj2;
  649 + auto state = State.state;
647 650
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; 651 + if( state == DynamicPSphere::States::Free) {
652 652
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 - } 653 + sphere->_state = DynamicPSphere::States::Free;
  654 + _map[sphere].clear(); // If Free, then the sphere should not be attached to ANY planes
  655 + }
  656 + else if( state == DynamicPSphere::States::AtRest) {
662 657
663 - }  
664 - } 658 + // Remember to check state in CollisionDetection and SimulateToTInDt and make adjustments
  659 +
  660 +
  661 + // Setting attached
  662 + sphere->_state = DynamicPSphere::States::AtRest;
  663 + std::cout << "The state is now: AtRest" << std::endl;
  664 + _map[sphere].emplace(plane);
  665 + }
  666 + else if( state == DynamicPSphere::States::Rolling) {
  667 +
  668 + // Setting attached
  669 + sphere->_state = DynamicPSphere::States::Rolling;
  670 + std::cout << "The state is now: Rolling" << std::endl;
  671 + _map[sphere].emplace(plane);
665 672
  673 + // Update physics
  674 +
  675 + }
666 676
667 -// } // EOF 677 + } // EOF
668 678
669 679
670 } // END namespace collision 680 } // END namespace collision
include/collision_library.h
@@ -27,13 +27,15 @@ public: @@ -27,13 +27,15 @@ public:
27 void add (StaticPBezierSurf* const surf); 27 void add (StaticPBezierSurf* const surf);
28 28
29 // Change "value" and return type if the sphere should be able to be attached to objects other than planes 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); 30 + std::unordered_set<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
  31 +// std::vector<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
34 32
35 - StateChangeObj  
36 - detectStateChange(DynamicPSphere* s, double time); 33 +
  34 +// void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
  35 +// void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);
  36 +
  37 + void
  38 + detectStateChange(StateChangeObj state);
37 39
38 void 40 void
39 singularityDetection(double dt); 41 singularityDetection(double dt);
@@ -84,8 +86,8 @@ protected: @@ -84,8 +86,8 @@ protected:
84 86
85 DefaultEnvironment _env; 87 DefaultEnvironment _env;
86 88
87 -// std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>> _map;  
88 - std::unordered_map<DynamicPSphere*, std::vector<StaticPPlane*>> _map; 89 + std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>> _map;
  90 +// std::unordered_map<DynamicPSphere*, std::vector<StaticPPlane*>> _map;
89 91
90 92
91 93
@@ -127,13 +129,14 @@ public: @@ -127,13 +129,14 @@ public:
127 129
128 // StateChangeObject struct 130 // StateChangeObject struct
129 struct StateChangeObj { 131 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 132 + DynamicPSphere* obj1; // Object whos state will change
  133 + StaticPPlane* obj2; // Object that obj1 will change state ACCORDING to
  134 + seconds_type time; // Time of singularity
  135 +// collision::CollisionStateFlag flag;
133 DynamicPSphere::States state; // State that obj1 will change to 136 DynamicPSphere::States state; // State that obj1 will change to
134 137
135 StateChangeObj 138 StateChangeObj
136 - (PhysObj_Base* o1, PhysObj_Base* o2, seconds_type t, DynamicPSphere::States s) : 139 + (DynamicPSphere* o1, StaticPPlane* o2, seconds_type t, DynamicPSphere::States s) :
137 obj1{o1}, obj2{o2}, time{t} ,state{s} {} 140 obj1{o1}, obj2{o2}, time{t} ,state{s} {}
138 }; 141 };
139 142
@@ -264,7 +267,7 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type @@ -264,7 +267,7 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type
264 // Sphere vs. Static Plane 267 // Sphere vs. Static Plane
265 if( plane ) { 268 if( plane ) {
266 269
267 - auto attachedPlanes = getAttachedObjects(sphere); 270 + auto attachedPlanes = _map[sphere];
268 271
269 for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) { 272 for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
270 273