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 309 GMlib::Vector<double,3>
310 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 332 GMlib::Vector<double, 3>
... ... @@ -358,11 +368,13 @@ namespace collision
358 368  
359 369 // Singularity detection
360 370 singularityDetection(dt);
  371 +// sortAndMakeUnique(_singularities);
  372 +// std::reverse(_singularities.begin(), _singularities.end());
361 373  
362 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 380 // Collision detection algorithm
... ... @@ -505,60 +517,53 @@ namespace collision
505 517 for( auto& sphere : _dynamic_spheres) {
506 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 560 } // EOF
534 561  
535 562  
536   - /// vector
  563 + /// vector; Get, Set and Remove objects to / from Sphere
  564 + /*
537 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 567 // MyController::getAttachedObjects(DynamicPSphere* sphere)
563 568 // {
564 569 // return (_map[sphere]);
... ... @@ -568,16 +573,40 @@ namespace collision
568 573 // void
569 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 579 // // Remove objects from the set In the map
575 580 // void
576 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 610 // Adding objects to vector
582 611  
583 612 void
... ... @@ -609,62 +638,43 @@ namespace collision
609 638 _static_bezier_surf.push_back(surf);
610 639 }
611 640  
  641 +
612 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 680 } // END namespace collision
... ...
include/collision_library.h
... ... @@ -27,13 +27,15 @@ public:
27 27 void add (StaticPBezierSurf* const surf);
28 28  
29 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 40 void
39 41 singularityDetection(double dt);
... ... @@ -84,8 +86,8 @@ protected:
84 86  
85 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 129  
128 130 // StateChangeObject struct
129 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 136 DynamicPSphere::States state; // State that obj1 will change to
134 137  
135 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 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 267 // Sphere vs. Static Plane
265 268 if( plane ) {
266 269  
267   - auto attachedPlanes = getAttachedObjects(sphere);
  270 + auto attachedPlanes = _map[sphere];
268 271  
269 272 for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
270 273  
... ...