Commit fa99a85279d9dd65886b0249bfa4d25541cb5ea0

Authored by Bjørn-Richard Pedersen
1 parent caaad402

Think detectStateChange is now working, still not sure about the time variable u…

…sed when returning the Struct object. Will start on sorting and make unique tomorrow
Showing 2 changed files with 292 additions and 97 deletions   Show diff stats
include/collision_library.cpp
... ... @@ -309,24 +309,13 @@ 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 + auto ds = vel * dtCount + 0.5 * xF * std::pow(dtCount, 2);
312 316  
313   - if(this->_state == DynamicPSphere::States::Rolling) {
  317 + return ds;
314 318  
315   - auto r = this->getRadius();
316   - auto s = this->getPos();
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   - }
330 319 }
331 320  
332 321 GMlib::Vector<double, 3>
... ... @@ -366,22 +355,14 @@ namespace collision
366 355 sphere->curr_t_in_dt = seconds_type{0.0};
367 356 }
368 357  
369   - // Singularity detection
370   - singularityDetection(dt);
371   -// sortAndMakeUnique(_singularities);
372   -// std::reverse(_singularities.begin(), _singularities.end());
  358 + // Detect state changes and fill up our state container
  359 + detectStateChanges(dt);
373 360  
374   - // StateChangeDetection
375   - for( auto& singularity : _singularities) {
376   -
377   - detectStateChange(singularity);
378   - }
379 361  
380 362 // Collision detection algorithm
381 363 for( auto& sphere : _dynamic_spheres) {
382 364  
383 365 // Sending in sphere twice in the initial call because the function will require a second object in future calls
384   - // Må da være en bedre måte å gjøre det på
385 366 sphereDynamicCollision(sphere, sphere, seconds_type(dt));
386 367  
387 368 for( auto& plane : _static_planes ) {
... ... @@ -509,55 +490,268 @@ namespace collision
509 490  
510 491 }
511 492  
512   - // Singularity Detection
513 493 void
514   - MyController::singularityDetection(double dt) {
  494 + MyController::detectStateChanges(double dt) {
515 495  
516   - // Sphere vs. Plane
517 496 for( auto& sphere : _dynamic_spheres) {
518   - for( auto& plane : _static_planes) {
519 497  
520   - auto r = sphere->getRadius();
521   - auto p = sphere->getMatrixToScene() * sphere->getPos();
  498 + auto singularity = detectStateChange(sphere, dt);
  499 +
  500 +// if (singularity.state != DynamicPSphere::States::NoChange) {
  501 + _singularities.push_back(singularity);
  502 +// }
  503 + }
  504 +
  505 + }
  506 +
  507 + //**** Code developed with help from Ghada Bouzidi and Fatemeh Heidari *****
  508 +
  509 + StateChangeObj
  510 + MyController::detectStateChange(DynamicPSphere *sphere, double dt) {
  511 +
  512 + std::unordered_set<StaticPPlane*> planeContainer; // Used for returning planes that the sphere is (not) attached to
  513 + DynamicPSphere::States state; // Holder variable for the state the sphere will enter
  514 +
  515 + const auto epsilon = 1e-5;
  516 +
  517 + // Sphere variables
  518 + const auto r = sphere->getRadius();
  519 + const auto pos = sphere->getMatrixToScene() * sphere->getPos();
522 520  
523   - auto epsilon = 1e-5;
  521 + // Time variables
  522 + const auto sphereTime = sphere->curr_t_in_dt;
  523 + const auto maxDt = seconds_type(dt);
  524 + const auto newDt = maxDt - sphereTime;
  525 + seconds_type returnTime = sphereTime;
  526 +// auto still = 0; // Needed to print out the still value further down
524 527  
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);
  528 + const auto ds = sphere->computeTrajectory(newDt); // Calculating "original ds"
  529 +// std::cout << "The DS is: " << ds << std::endl;
530 530  
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);
  531 + // Plane variables.
  532 + auto planes = getAttachedObjects(sphere);
  533 + GMlib::APoint<float,3> q;
  534 + GMlib::Vector<float,3> n {0.0f, 0.0f, 0.0f};
536 535  
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;
  536 + // If the sphere is NOT ATTACHED to any planes, we check if it BECOMES attached in this timestep
  537 + if( planes.empty() ) {
542 538  
543   - if ( std::abs( ((-n*r) * ds) -(ds*ds) ) < epsilon) {
  539 + // Go through each plane in the game and calculate whether or not the sphere is attached to any of them
  540 + for( auto& plane : _static_planes ) {
544 541  
545   - auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::AtRest);
546   - _singularities.push_back(singularity);
  542 + const auto M = plane->evaluateParent(0.5f, 0.5f, 1, 1);
  543 + const auto q = M(0)(0);
  544 + const auto u = M(1)(0);
  545 + const auto v = M(0)(1);
  546 + const auto n = GMlib::Vector<float,3> (u ^ v).getNormalized();
  547 + const auto d = (q + r * n) - pos;
  548 +
  549 + // Debug variables / criteria for states
  550 + const auto still = std::abs(((-n * r) * ds) - (ds * ds)); // Sphere is Resting on surface
  551 + const auto dsn = ds * n; // Sphere is Rolling on surface. (Cannot be still AND rolling)
  552 + const auto dn = d * n; // Sphere is Flying
  553 +
  554 +// std::cout << "The still is: " << still << std::endl; // Useful for seeing if the sphere should be still or not
  555 +
  556 + // Return time, used same method as for collision time between sphere and plane (Might need to be done in a different way!!!!)
  557 + // sphereTime = dt_min
  558 + const auto x = dn / dsn;
  559 + returnTime = (x * newDt) + sphereTime;
  560 +
  561 + // Check to see if the sphere has actually even touched the plane yet
  562 + if( std::abs(dn) < epsilon ) {
  563 +
  564 + /// If a sphere is attached to a surface, it can only be Still (AtRest) or Rolling, not both.
  565 + /// If either are true, we store the current plane, as the application is object centric and if a sphere is still or rolling
  566 + /// in relation to ONE plane, then it that is it's STATE in the game, and store the state the sphere should TRANSITION into.
  567 + if( still < epsilon ) {
  568 +
  569 + std::cout << "State will become AtRest" << std::endl;
  570 + planeContainer.insert( plane );
  571 + state = DynamicPSphere::States::AtRest;
  572 + }
  573 + else if( still > epsilon ) {
  574 +
  575 + std::cout << "State will become Rolling" << std::endl;
  576 + planeContainer.insert( plane );
  577 + state = DynamicPSphere::States::Rolling;
  578 + }
547 579 }
548   - else if(std::abs(((-n*r) * ds) -(ds*ds)) > epsilon) {
  580 + // The sphere did NOT ATTACH to any surface
  581 + else state = DynamicPSphere::States::NoChange;
  582 + }
  583 +
  584 + return StateChangeObj( sphere, planeContainer, returnTime, state );
  585 + }
  586 + // Else the sphere IS ATTACHED and we need to calculate if it will change state / detach
  587 + else {
  588 +
  589 + // Go through the planes that the sphere is attached to in order to get a common normal for use in computation
  590 + // The sphere won't always be attached to multiple planes
  591 + for( auto& plane : planes ) {
  592 +
  593 + const auto M = plane->evaluateParent(0.5f, 0.5f, 1, 1);
  594 + const auto q = M(0)(0);
  595 + const auto u = M(1)(0);
  596 + const auto v = M(0)(1);
  597 + auto normal = GMlib::Vector<float,3> (u ^ v);
  598 + n += normal;
  599 + }
  600 +
  601 + n = GMlib::Vector<float,3> (n / planes.size()).getNormalized();
549 602  
550   - auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Rolling);
551   - _singularities.push_back(singularity);
  603 + // Debug variables / criteria for states
  604 + const auto d = (q + r * n) - pos;
  605 + const auto still = std::abs(((-n * r) * ds) - (ds * ds)); // Sphere is Resting on surface
  606 + const auto dsn = ds * n; // Sphere is Rolling on surface. (Cannot be still AND rolling)
  607 + const auto dn = d * n; // Sphere is Flying
  608 +
  609 + // Return time, used same method as for collision time between sphere and plane (Might need to be done in a different way!!!!)
  610 + // sphereTime = dt_min
  611 + returnTime = ((dn / dsn) * newDt) + sphereTime;
  612 +
  613 + /// Check the sphere's current state, and if it needs to be changed.
  614 + // If there sphere is rolling..
  615 + if( sphere->_state == DynamicPSphere::States::Rolling) {
  616 +
  617 + // Detatchment is seen as a state change, and will be processed further in the algorithm
  618 + if( dsn > 0 ) {
  619 + std::cout << "State changing from Rolling to Free" << std::endl;
  620 + state = DynamicPSphere::States::Free;
  621 + return StateChangeObj( sphere, planes, returnTime, state );
552 622 }
553   - else if( (ds * n) <= 0) {
  623 + else if( still < epsilon ) {
  624 + std::cout << "State changing from Rolling to AtRest" << std::endl;
  625 + state = DynamicPSphere::States::AtRest;
  626 + return StateChangeObj( sphere, planes, returnTime, state );
  627 + }
  628 + // Else the sphere will keep being in a Rolling state
  629 + else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::NoChange );
  630 + }
  631 + // If the sphere is still..
  632 + if( sphere->_state == DynamicPSphere::States::AtRest ) {
554 633  
555   - auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Free);
556   - _singularities.push_back(singularity);
  634 + if( dsn > 0 ) {
  635 + std::cout << "State changing from AtRest to Free" << std::endl;
  636 + state = DynamicPSphere::States::Free;
  637 + return StateChangeObj( sphere, planes, returnTime, state );
557 638 }
  639 + else if( still > epsilon ) {
  640 + std::cout << "State changing from AtRest to Rolling" << std::endl;
  641 + state = DynamicPSphere::States::Rolling;
  642 + return StateChangeObj( sphere, planes, returnTime, state );
  643 + }
  644 + else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::NoChange );
558 645 }
559 646 }
560   - } // EOF
  647 +
  648 +
  649 +// // Get all the planes (static objects) that the sphere is attached to
  650 +// auto planes = getAttachedObjects(sphere);
  651 +
  652 +// // If the sphere is NOT attached to any surfaces, we need to check if it becomes attached to one of the planes in the game
  653 +// if( planes.empty() ) {
  654 +
  655 +// StaticPPlane* emptyplane {nullptr};
  656 +
  657 +// const auto r = sphere->getRadius();
  658 +// const auto p = sphere->getMatrixToScene() * sphere->getPos();
  659 +
  660 +// const auto epsilon = 1e-5;
  661 +
  662 +// auto dts = seconds_type(dt);
  663 +// const auto max_dt = dts;
  664 +// const auto min_dt = sphere->curr_t_in_dt;
  665 +// auto new_dt = max_dt -min_dt;
  666 +// const auto ds = sphere->computeTrajectory(new_dt);
  667 +
  668 +// for( auto& plane : _static_planes) {
  669 +
  670 +// const auto M = plane->evaluateParent(0.5f,0.5f,1,1);
  671 +// const auto q = M(0)(0);
  672 +// const auto u = M(1)(0);
  673 +// const auto v = M(0)(1);
  674 +// const auto n = GMlib::Vector<float,3>(u ^ v);
  675 +// const auto d = (q + r * n) - p;
  676 +
  677 +// const auto dn = d * n;
  678 +// const auto dsn = ds * n;
  679 +// const auto still = std::abs( ((-n*r) * ds) -(ds*ds) );
  680 +
  681 +// // Criteria for attachment
  682 +// if( dsn <= 0 ) {
  683 +
  684 +// // Sliding or At rest
  685 +
  686 +// }
  687 +// // Not attached, returning Free state
  688 +
  689 +// }
  690 +// }
  691 +// // Else the plane is attached to AT LEAST 1 surface, and we can change state
  692 +// else {
  693 +
  694 +// }
  695 +
  696 + }
  697 +
  698 +
  699 +
  700 + /// old SingularityDetection
  701 + // Singularity Detection
  702 +// void
  703 +// MyController::singularityDetection(double dt) {
  704 +
  705 +// // Sphere vs. Plane
  706 +// for( auto& sphere : _dynamic_spheres) {
  707 +// for( auto& plane : _static_planes) {
  708 +
  709 +// auto r = sphere->getRadius();
  710 +// auto p = sphere->getMatrixToScene() * sphere->getPos();
  711 +
  712 +// auto epsilon = 1e-5;
  713 +
  714 +// auto dts = seconds_type(dt);
  715 +// auto max_dt = dts;
  716 +// auto min_dt = sphere->curr_t_in_dt;
  717 +// auto new_dt = max_dt -min_dt;
  718 +// auto ds = sphere->computeTrajectory(new_dt);
  719 +
  720 +// auto M = plane->evaluateParent(0.5f,0.5f,1,1);
  721 +// auto q = M(0)(0);
  722 +// auto u = M(1)(0);
  723 +// auto v = M(0)(1);
  724 +// auto n = GMlib::Vector<float,3>(u ^ v);
  725 +
  726 +// // Helper variables for debugging
  727 +// auto d = (q + r * n) - p;
  728 +// auto still = std::abs(((-n*r) * ds) -(ds*ds)); // < epsilon => Criteria for Still. > epsilon => Criteria for Rolling
  729 +// auto dsn = ds * n;
  730 +// auto dn = d*n;
  731 +
  732 +// // Still
  733 +// if ( std::abs( ((-n*r) * ds) -(ds*ds) ) < epsilon) {
  734 +
  735 +// auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::AtRest);
  736 +// _singularities.push_back(singularity);
  737 +// }
  738 +
  739 +// // Rolling
  740 +// else if(std::abs(((-n*r) * ds) -(ds*ds)) > epsilon) {
  741 +
  742 +// auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Rolling);
  743 +// _singularities.push_back(singularity);
  744 +// }
  745 +
  746 +// // Flying
  747 +// else if( (ds * n) <= 0) {
  748 +
  749 +// auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Free);
  750 +// _singularities.push_back(singularity);
  751 +// }
  752 +// }
  753 +// }
  754 +// } // EOF
561 755  
562 756  
563 757 /// vector; Get, Set and Remove objects to / from Sphere
... ... @@ -585,7 +779,7 @@ namespace collision
585 779 */
586 780  
587 781 /// unordered_set; Get, Set and Remove objects to / from Sphere
588   - /*
  782 +
589 783 // Get objects attached to sphere
590 784 std::unordered_set<StaticPPlane *>
591 785 MyController::getAttachedObjects(DynamicPSphere* sphere)
... ... @@ -606,7 +800,7 @@ namespace collision
606 800  
607 801 _map[sphere].erase(plane);
608 802 }
609   -*/
  803 +
610 804 // Adding objects to vector
611 805  
612 806 void
... ... @@ -640,41 +834,42 @@ namespace collision
640 834  
641 835  
642 836 // States
643   - void
644   - MyController::detectStateChange( StateChangeObj State ) {
  837 +// void
  838 +// MyController::detectStateChange( StateChangeObj State ) {
645 839  
646 840  
647   - auto sphere = State.obj1;
648   - auto plane = State.obj2;
649   - auto state = State.state;
  841 +// auto sphere = State.obj1;
  842 +// auto plane = State.obj2;
  843 +// auto state = State.state;
650 844  
651   - if( state == DynamicPSphere::States::Free) {
  845 +// if( state == DynamicPSphere::States::Free) {
652 846  
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) {
  847 +// sphere->_state = DynamicPSphere::States::Free;
  848 +// _map[sphere].clear(); // If Free, then the sphere should not be attached to ANY planes
  849 +// }
  850 +// else if( state == DynamicPSphere::States::AtRest) {
657 851  
658   - // Remember to check state in CollisionDetection and SimulateToTInDt and make adjustments
  852 +// // Remember to check state in CollisionDetection and SimulateToTInDt and make adjustments
659 853  
  854 +// // Setting attached
  855 +// sphere->_state = DynamicPSphere::States::AtRest;
  856 +// std::cout << "The state is now: AtRest" << std::endl;
  857 +// _map[sphere].emplace(plane);
660 858  
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) {
  859 +// // Update physics?
  860 +// }
  861 +// else if( state == DynamicPSphere::States::Rolling) {
667 862  
668   - // Setting attached
669   - sphere->_state = DynamicPSphere::States::Rolling;
670   - std::cout << "The state is now: Rolling" << std::endl;
671   - _map[sphere].emplace(plane);
  863 +// // Setting attached
  864 +// sphere->_state = DynamicPSphere::States::Rolling;
  865 +// std::cout << "The state is now: Rolling" << std::endl;
  866 +// _map[sphere].emplace(plane);
672 867  
673   - // Update physics
  868 +// // Update physics ?
674 869  
675   - }
  870 +// }
676 871  
677   - } // EOF
  872 +// } // EOF
678 873  
679 874  
680 875 } // END namespace collision
... ...
include/collision_library.h
... ... @@ -31,14 +31,14 @@ public:
31 31 // std::vector<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
32 32  
33 33  
34   -// void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
35   -// void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);
  34 + void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
  35 + void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);
36 36  
37 37 void
38   - detectStateChange(StateChangeObj state);
  38 + detectStateChanges(double dt);
39 39  
40   - void
41   - singularityDetection(double dt);
  40 + StateChangeObj
  41 + detectStateChange(DynamicPSphere* sphere, double dt);
42 42  
43 43  
44 44 CollisionState detectCollision (const DynamicPSphere& S,
... ... @@ -102,7 +102,8 @@ public:
102 102 enum class States {
103 103 Free,
104 104 Rolling,
105   - AtRest
  105 + AtRest,
  106 + NoChange
106 107 };
107 108  
108 109 MyController* _controller;
... ... @@ -129,15 +130,14 @@ public:
129 130  
130 131 // StateChangeObject struct
131 132 struct StateChangeObj {
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;
136   - DynamicPSphere::States state; // State that obj1 will change to
  133 + DynamicPSphere* obj1; // Object whos state will change
  134 + std::unordered_set<StaticPPlane*> attachedPlanes; // Object that obj1 will change state ACCORDING to
  135 + seconds_type time; // Time of singularity
  136 + DynamicPSphere::States state; // State that obj1 will change to
137 137  
138 138 StateChangeObj
139   - (DynamicPSphere* o1, StaticPPlane* o2, seconds_type t, DynamicPSphere::States s) :
140   - obj1{o1}, obj2{o2}, time{t} ,state{s} {}
  139 + (DynamicPSphere* o1, std::unordered_set<StaticPPlane*> planes, seconds_type t, DynamicPSphere::States s) :
  140 + obj1{o1}, attachedPlanes{planes}, time{t} ,state{s} {}
141 141 };
142 142  
143 143 template <class PSurf_T, typename... Arguments>
... ...