Commit 7d9d5302d7430c3ccb7e5ec406759100e3ba2d4b

Authored by Bjørn-Richard Pedersen
1 parent 3cc57c34

It seems to be Working now, but not doing closest point. Have to look more into that

include/collision_library.cpp
... ... @@ -98,7 +98,6 @@ namespace collision
98 98 const StaticPhysObject<GMlib::PPlane<float>>& P,
99 99 seconds_type dt)
100 100 {
101   -
102 101 const auto dt_max = dt;
103 102 const auto dt_min = S.curr_t_in_dt;
104 103 const auto new_dt = dt_max - dt_min;
... ... @@ -118,8 +117,16 @@ namespace collision
118 117  
119 118 const auto d = (plane_pos + s_radius * n_normal) - s_position;
120 119  
  120 + auto ds = S.computeTrajectory(new_dt);
  121 +
  122 + // If the sphere's state is Rolling, it should use an adjusted DS
  123 + if( S._state == DynamicPSphere::States::Rolling ) {
  124 + auto unconst_S = const_cast<DynamicPhysObject<GMlib::PSphere<float>>&>(S);
  125 + ds = unconst_S.adjustedTrajectory(new_dt);
  126 + }
  127 +
121 128 const auto Q = (d * n_normal);
122   - const auto R = ( S.computeTrajectory(new_dt) * n_normal ); // S.computeTrajectory(dt)
  129 + const auto R = ( ds * n_normal ); // S.computeTrajectory(dt)
123 130  
124 131 const auto epsilon = 0.00001;
125 132  
... ... @@ -219,6 +226,11 @@ namespace collision
219 226  
220 227 }
221 228  
  229 + GMlib::Point<float,2>
  230 + closestPoint(DynamicPSphere& S, StaticPPlane& P) {
  231 +
  232 + }
  233 +
222 234 void
223 235 computeImpactResponse (DynamicPSphere& S, const StaticPBezierSurf& B,
224 236 seconds_type dt) {
... ... @@ -289,21 +301,43 @@ namespace collision
289 301 void
290 302 DynamicPhysObject<GMlib::PSphere<float> >::simulateToTInDt(seconds_type t){
291 303  
292   - auto dt = t - this->curr_t_in_dt;
293   - auto MI = this->getMatrixToSceneInverse();
294   - auto ds = this->computeTrajectory(dt);
  304 + if( this->_state == DynamicPSphere::States::AtRest
  305 + or (this->_state == DynamicPSphere::States::Rolling and std::abs(this->velocity(2) <= 1e-2))) {
295 306  
296   - // Move
297   - this->translateParent( MI * ds);
298   - this->curr_t_in_dt = t;
  307 + this->curr_t_in_dt = t;
  308 + this->velocity = {0.0f, 0.0f, 0.0f};
  309 + this->environment = &this->_sphereController->_stillEnvironment;
299 310  
300   - // Update physics
301   - auto F = this->externalForces();
302   - auto c = dt.count();
303   - auto a = F*c;
304   - this->velocity += a;
305   - }
  311 + }
  312 + else {
  313 +
  314 + auto dt = t - this->curr_t_in_dt;
  315 + auto MI = this->getMatrixToSceneInverse();
  316 + GMlib::Vector<double,3> ds {0.0f, 0.0f, 0.0f};
  317 +
  318 + if( this->_state == DynamicPSphere::States::Rolling ) {
  319 +
  320 + ds = adjustedTrajectory(dt);
  321 + }
  322 + if( this->_state == DynamicPSphere::States::Free ) {
  323 +
  324 + ds = this->computeTrajectory(dt);
  325 + }
306 326  
  327 + // Move
  328 + this->translateParent( MI * ds);
  329 + this->curr_t_in_dt = t;
  330 +
  331 + // Update physics
  332 + auto F = this->externalForces();
  333 + auto c = dt.count();
  334 + auto a = F*c;
  335 + this->velocity += a;
  336 +
  337 + this->environment = &this->_sphereController->_env;
  338 + }
  339 +
  340 + }
307 341  
308 342  
309 343 GMlib::Vector<double,3>
... ... @@ -318,23 +352,37 @@ namespace collision
318 352  
319 353 }
320 354  
321   - GMlib::Vector<double, 3>
322   - DynamicPhysObject<GMlib::PSphere<float> >::getTrajectory(seconds_type dt) {
323 355  
324   - return this->_NewTrajectory;
325   - }
326 356  
327   - void
328   - DynamicPhysObject<GMlib::PSphere<float> >::setTrajectory(GMlib::Vector<double, 3> ds, DynamicPSphere* sphere) {
  357 + GMlib::Vector<double,3>
  358 + DynamicPhysObject<GMlib::PSphere<float> >::adjustedTrajectory(seconds_type dt) {
329 359  
330 360 // Update ds to modified DS'
331   - auto r = sphere->getRadius();
332   - auto s = sphere->getPos();
  361 + auto ds = this->computeTrajectory(seconds_type(dt));
  362 + auto r = this->getRadius();
  363 + auto s = this->getMatrixToScene() * this->getPos();
333 364 auto p = ds + s;
334   -// GMlib::Vector<d,3> test = (p * s).getNormalized();
335 365  
336   -// auto adj_ds = ds + ( r + (p * s) ).getNormalized();
  366 + auto planes = this->_sphereController->getAttachedObjects(this);
  367 + GMlib::Vector<float,3> n {0.0f, 0.0f, 0.0f};
  368 +
  369 + for( auto& plane : planes ) {
  370 +
  371 + const auto M = plane->evaluateParent(0.5f, 0.5f, 1, 1);
  372 + const auto q = M(0)(0);
  373 + const auto u = M(1)(0);
  374 + const auto v = M(0)(1);
  375 + auto normal = GMlib::Vector<float,3> (u ^ v);
  376 + n += normal;
  377 + }
  378 +
  379 + n = GMlib::Vector<float,3> (n / planes.size()).getNormalized();
  380 +
  381 +// auto adjusted_ds = ds + (r + (p * s) )* n;
  382 +
  383 + auto adjusted_ds = ds - (ds * n * n);
337 384  
  385 + return adjusted_ds;
338 386  
339 387 }
340 388  
... ... @@ -387,64 +435,181 @@ namespace collision
387 435 std::reverse(_collisions.begin(), _collisions.end());
388 436 }
389 437  
390   -// while( !_collisions.empty() or !_singularities.empty() ) {
  438 + while( !_collisions.empty() or !_singularities.empty() ) {
391 439  
392   -// // IF BOTH NOT EMPTY
393   -// if( !_collisions.empty() and !_singularities.empty() ) {
  440 + // IF BOTH NOT EMPTY
  441 + if( !_collisions.empty() and !_singularities.empty() ) {
  442 +
  443 + const auto col_time = _collisions.back().t_in_dt;
  444 + const auto sing_time = _singularities.back().time;
394 445  
  446 + // Resolve Collision
  447 + if( col_time < sing_time ) {
395 448  
  449 + auto c = _collisions.back();
  450 + _collisions.pop_back();;
396 451  
397   -// }
  452 + handleCollision(c, dt); // Also detects more collisions
398 453  
399   -// // IF COLLISIONS NOT EMPTY
  454 + detectStateChanges(dt);
400 455  
401   -// // IF SINGULARITIES NOT EMPTY
402   -// }
  456 + sortAndMakeUnique(_collisions);
  457 + sortAndMakeUniqueStates(_singularities);
403 458  
  459 + if( !_collisions.empty() and !_singularities.empty() ) {
  460 +
  461 + crossUnique(_collisions, _singularities);
  462 + }
  463 + else {
  464 + // Make sure that the newest event is at the front of the vector
  465 + std::reverse(_singularities.begin(), _singularities.end() );
  466 + std::reverse(_collisions.begin(), _collisions.end());
  467 + }
  468 + }
404 469  
  470 + // Resolve Singularity
  471 + else {
405 472  
406   - // Impact response COLLISIONS
407   - while( !_collisions.empty() ) {
  473 + auto s = _singularities.back();
  474 + _singularities.pop_back();
408 475  
409   - auto c = _collisions.back();
410   - _collisions.pop_back();
  476 + handleStates(s, dt);
411 477  
412   - handleCollision(c, dt);
  478 + // Collision detection algorithm
  479 + for( auto& sphere : _dynamic_spheres) {
413 480  
414   - detectStateChanges(dt);
415   - sortAndMakeUnique(_collisions);
416   - sortAndMakeUniqueStates(_singularities);
  481 + // Sending in sphere twice in the initial call because the function will require a second object in future calls
  482 + sphereDynamicCollision(sphere, sphere, seconds_type(dt));
417 483  
418   - if( !_collisions.empty() and !_singularities.empty() ) {
  484 + for( auto& plane : _static_planes ) {
419 485  
420   - crossUnique(_collisions, _singularities);
  486 + sphereStaticCollision(sphere, plane, seconds_type(dt));
  487 + }
  488 + }
  489 +
  490 + detectStateChanges(dt);
  491 +
  492 + sortAndMakeUnique(_collisions);
  493 + sortAndMakeUniqueStates(_singularities);
  494 +
  495 + if( !_collisions.empty() and !_singularities.empty() ) {
  496 +
  497 + crossUnique(_collisions, _singularities);
  498 + }
  499 + else {
  500 + // Make sure that the newest event is at the front of the vector
  501 + std::reverse(_singularities.begin(), _singularities.end() );
  502 + std::reverse(_collisions.begin(), _collisions.end());
  503 + }
  504 + }
421 505 }
422   - else {
423   - // Make sure that the newest event is at the front of the vector
424   - std::reverse(_singularities.begin(), _singularities.end() );
425   - std::reverse(_collisions.begin(), _collisions.end());
  506 +
  507 + // IF COLLISIONS NOT EMPTY
  508 + else if( !_collisions.empty() and _singularities.empty() ) {
  509 +
  510 + auto c = _collisions.back();
  511 + _collisions.pop_back();;
  512 +
  513 + handleCollision(c, dt); // Also detects more collisions
  514 +
  515 + detectStateChanges(dt);
  516 +
  517 + sortAndMakeUnique(_collisions);
  518 + sortAndMakeUniqueStates(_singularities);
  519 +
  520 + if( !_collisions.empty() and !_singularities.empty() ) {
  521 +
  522 + crossUnique(_collisions, _singularities);
  523 + }
  524 + else {
  525 + // Make sure that the newest event is at the front of the vector
  526 + std::reverse(_singularities.begin(), _singularities.end() );
  527 + std::reverse(_collisions.begin(), _collisions.end());
  528 + }
  529 +
426 530 }
427 531  
  532 + // IF SINGULARITIES NOT EMPTY
  533 + else if( _collisions.empty() and !_singularities.empty() ) {
  534 +
  535 + auto s = _singularities.back();
  536 + _singularities.pop_back();
  537 +
  538 + handleStates(s, dt);
  539 +
  540 + // Collision detection algorithm
  541 + for( auto& sphere : _dynamic_spheres) {
  542 +
  543 + // Sending in sphere twice in the initial call because the function will require a second object in future calls
  544 + sphereDynamicCollision(sphere, sphere, seconds_type(dt));
  545 +
  546 + for( auto& plane : _static_planes ) {
428 547  
  548 + sphereStaticCollision(sphere, plane, seconds_type(dt));
  549 + }
  550 + }
  551 +
  552 + detectStateChanges(dt);
  553 +
  554 + sortAndMakeUnique(_collisions);
  555 + sortAndMakeUniqueStates(_singularities);
  556 +
  557 + if( !_collisions.empty() and !_singularities.empty() ) {
  558 +
  559 + crossUnique(_collisions, _singularities);
  560 + }
  561 + else {
  562 + // Make sure that the newest event is at the front of the vector
  563 + std::reverse(_singularities.begin(), _singularities.end() );
  564 + std::reverse(_collisions.begin(), _collisions.end());
  565 + }
  566 + }
429 567 }
430 568  
  569 +
431 570 // Start simulation for all objects
432 571 for( auto sphere : _dynamic_spheres) {
433 572  
434   - sphere->simulateToTInDt(seconds_type(dt));
  573 + sphere->simulateToTInDt(seconds_type(dt));
  574 + }
  575 +
  576 + }
  577 +
  578 + // Singularity handeling
  579 + void
  580 + MyController::handleStates(StateChangeObj &state, double dt) {
  581 +
  582 + auto sphere = state.obj;
  583 + auto newState = state.state;
  584 + auto time = state.time;
  585 + auto planes = state.attachedPlanes;
  586 +
  587 + if( newState == DynamicPSphere::States::Free ) {
  588 +
  589 + detachObjects(sphere);
  590 + sphere->_state = newState;
435 591 }
  592 + else {
436 593  
  594 + setAttachedObjects(planes, sphere);
  595 + sphere->_state = newState;
  596 +
  597 + if( newState == DynamicPSphere::States::AtRest ) {
  598 +
  599 + sphere->environment = &_stillEnvironment;
  600 + sphere->velocity = GMlib::Vector<double,3> (0.0f, 0.0f, 0.0f);
  601 + }
  602 + }
  603 +
  604 + sphere->simulateToTInDt(time);
437 605 }
438 606  
439 607 // Collision handeling
440 608 void
441 609 MyController::handleCollision(CollisionObject &c, double dt) {
442 610  
443   -// auto c = _collisions.back();
444   -// _collisions.pop_back();
445   -
446   - c.obj1->simulateToTInDt(c.t_in_dt);
447   - c.obj2->simulateToTInDt(c.t_in_dt);
  611 +// c.obj1->simulateToTInDt(c.t_in_dt);
  612 +// c.obj2->simulateToTInDt(c.t_in_dt);
448 613  
449 614 // Add more objects here if you end up using more
450 615 auto d_sphere_1 = dynamic_cast<DynamicPSphere*>(c.obj1);
... ... @@ -471,8 +636,13 @@ namespace collision
471 636 collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. D_Cylinder
472 637 else if (d_sphere_1 && s_cylinder_2)
473 638 collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. S_Cylinder
474   - else if (d_sphere_1 && s_plane_2)
475   - collision::computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
  639 + else if (d_sphere_1 && s_plane_2) {
  640 +
  641 + if(d_sphere_1->_state != DynamicPSphere::States::AtRest)
  642 + d_sphere_1->simulateToTInDt(c.t_in_dt);
  643 + collision::computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
  644 +
  645 + }
476 646 else if (d_sphere_1 && s_bezier_2)
477 647 collision::computeImpactResponse( *d_sphere_1, *s_bezier_2, c.t_in_dt); // D_Sphere vs. S_Bezier
478 648  
... ... @@ -543,18 +713,15 @@ namespace collision
543 713 void
544 714 MyController::detectStateChanges(double dt) {
545 715  
546   -// for( auto& sphere : _dynamic_spheres) {
547   -
548   -// auto singularity = detectStateChange(sphere, dt);
  716 + for( auto& sphere : _dynamic_spheres) {
549 717  
550   -// if (singularity.state != DynamicPSphere::States::NoChange) {
551   -// auto fake = StateChangeObj(sphere, fakePlanes, seconds_type(dt), DynamicPSphere::States::NoChange );
  718 + auto singularity = detectStateChange(sphere, dt);
552 719  
553   -// _singularities.push_back(singularity);
  720 + if (singularity.state != sphere->_state) {
554 721  
555   -// _singularities.push_back(fake);
556   -// }
557   -// }
  722 + _singularities.push_back(singularity);
  723 + }
  724 + }
558 725  
559 726 }
560 727  
... ... @@ -587,6 +754,95 @@ namespace collision
587 754 GMlib::APoint<float,3> q;
588 755 GMlib::Vector<float,3> n {0.0f, 0.0f, 0.0f};
589 756  
  757 + if( planes.empty() ) { // Sphere NOT attached
  758 +
  759 + for (auto& plane : _static_planes){
  760 + auto M = plane->evaluateParent(0.5f,0.5f,1,1);
  761 + auto q = M(0)(0);
  762 + auto u = M(1)(0);
  763 + auto v = M(0)(1);
  764 + auto n = GMlib::Vector<float,3>(u ^ v).getNormalized();
  765 + auto d = (q + r * n) - pos;
  766 +
  767 + auto bla = std::abs(((-n*r) * ds) -(ds*ds));
  768 + auto dsn = ds * n;
  769 + auto dn = d*n;
  770 + const auto x = dn / dsn;
  771 + returnTime = (x * newDt) + sphereTime;
  772 +
  773 + if( bla < epsilon ) {
  774 +
  775 + planeContainer.insert(plane);
  776 + state = DynamicPSphere::States::AtRest;
  777 + std::cout << "detectStateChange says the state will become AtRest from Free" << std::endl;
  778 + }
  779 + else if( std::abs(dn) < epsilon and dsn < 0 ) {
  780 +
  781 + planeContainer.insert(plane);
  782 + state = DynamicPSphere::States::Rolling;
  783 + std::cout << "detectStateChange says the state will become Rolling from Free" << std::endl;
  784 + }
  785 + else state = DynamicPSphere::States::Free;
  786 + }
  787 +
  788 + return StateChangeObj(sphere, planeContainer, returnTime, state);
  789 + }
  790 + else { // Sphere ATTACHED
  791 +
  792 + for (auto &it :planes){
  793 + auto M = it->evaluateParent(0.5f,0.5f,1,1);
  794 + auto pos= M(0)(0);
  795 + auto u = M(1)(0);
  796 + auto v = M(0)(1);
  797 + auto normal = GMlib::Vector<float,3>(u ^ v);
  798 + n+=normal;
  799 + q=pos;
  800 + }
  801 + n= GMlib::Vector <float,3>(n/planes.size()).getNormalized();
  802 +
  803 + auto d = (q + r * n) - pos;
  804 + auto bla = std::abs(((-n*r) * ds) -(ds*ds));
  805 + auto dsn = ds * n;
  806 + auto dn = d * n;
  807 + const auto x = dn / dsn;
  808 + returnTime = (x * newDt) + sphereTime;
  809 +
  810 +
  811 + if( sphere->_state == DynamicPSphere::States::Rolling ) {
  812 +
  813 + if( dsn > 0) {
  814 +
  815 + std::cout << "detectStateChange says the state will become Free from Rolling" << std::endl;
  816 + state = DynamicPSphere::States::Free;
  817 + return StateChangeObj(sphere, planes, returnTime, state);
  818 + }
  819 + else if( bla < epsilon ) {
  820 +
  821 + std::cout << "detectStateChange says the state will become AtRest from Rolling" << std::endl;
  822 + state = DynamicPSphere::States::AtRest;
  823 + return StateChangeObj(sphere, planes, returnTime, state);
  824 + }
  825 + else return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
  826 + }
  827 +
  828 + else if( sphere->_state == DynamicPSphere::States::AtRest ) {
  829 +
  830 + if( bla > epsilon ) {
  831 +
  832 + std::cout << "detectStateChange says the state will become Rolling from AtRest" << std::endl;
  833 + state = DynamicPSphere::States::Rolling;
  834 + return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
  835 + }
  836 + else if( dsn > 0) {
  837 + std::cout << "detectStateChange says the state will become Free from AtRest" << std::endl;
  838 + state = DynamicPSphere::States::Free;
  839 + return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
  840 + }
  841 + else return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::AtRest);
  842 + }
  843 + }
  844 +
  845 + /*
590 846 // If the sphere is NOT ATTACHED to any planes, we check if it BECOMES attached in this timestep
591 847 if( planes.empty() ) {
592 848  
... ... @@ -632,7 +888,7 @@ namespace collision
632 888 }
633 889 }
634 890 // The sphere did NOT ATTACH to any surface
635   - else state = DynamicPSphere::States::NoChange;
  891 + else state = DynamicPSphere::States::Free;
636 892 }
637 893  
638 894 return StateChangeObj( sphere, planeContainer, returnTime, state );
... ... @@ -664,6 +920,8 @@ namespace collision
664 920 // sphereTime = dt_min
665 921 returnTime = ((dn / dsn) * newDt) + sphereTime;
666 922  
  923 + if( std::abs(dn) > epsilon ) {
  924 +
667 925 /// Check the sphere's current state, and if it needs to be changed.
668 926 if( sphere->_state == DynamicPSphere::States::Rolling) {
669 927  
... ... @@ -679,7 +937,7 @@ namespace collision
679 937 return StateChangeObj( sphere, planes, returnTime, state );
680 938 }
681 939 // Else the sphere will keep being in a Rolling state
682   - else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::NoChange );
  940 + else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::Rolling );
683 941 }
684 942  
685 943 if( sphere->_state == DynamicPSphere::States::AtRest ) {
... ... @@ -694,15 +952,18 @@ namespace collision
694 952 state = DynamicPSphere::States::Rolling;
695 953 return StateChangeObj( sphere, planes, returnTime, state );
696 954 }
697   - else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::NoChange );
  955 + else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::Free );
698 956 }
699 957 }
  958 + */
700 959  
701 960 }
702 961  
703   -// void
704   -// MyController::testMethod()
705   -// {
  962 + void
  963 + MyController::testMethod()
  964 + {
  965 +
  966 +
706 967 // auto sphere1 = _dynamic_spheres[0];
707 968 // auto sphere2 = _dynamic_spheres[1];
708 969 // auto sphere3 = _dynamic_spheres[2];
... ... @@ -726,7 +987,7 @@ namespace collision
726 987  
727 988 // crossUnique(_collisions, _singularities);
728 989  
729   -// }
  990 + }
730 991  
731 992  
732 993  
... ... @@ -759,21 +1020,31 @@ namespace collision
759 1020 std::unordered_set<StaticPPlane *>
760 1021 MyController::getAttachedObjects(DynamicPSphere* sphere)
761 1022 {
762   - return (_map[sphere]);
  1023 + static std::unordered_set<StaticPPlane*> empty {};
  1024 + auto iter = _map.find(sphere);
  1025 +
  1026 + if( iter != _map.end() ) {
  1027 +
  1028 + return iter->second;
  1029 + }
  1030 + else return empty;
763 1031 }
764 1032  
765 1033 // Set objects attached to sphere
766 1034 void
767   - MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
  1035 + MyController::setAttachedObjects(std::unordered_set<StaticPPlane *> plane, DynamicPSphere* sphere)
768 1036 {
769   - _map[sphere].emplace(plane);
  1037 + for( auto& p : plane) {
  1038 + _map[sphere].emplace(p);
  1039 + }
770 1040 }
771 1041  
772 1042 // Remove objects from the set In the map
773 1043 void
774   - MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
  1044 + MyController::detachObjects(DynamicPSphere *sphere){
  1045 +
  1046 + _map.erase(sphere);
775 1047  
776   - _map[sphere].erase(plane);
777 1048 }
778 1049  
779 1050 // Adding objects to vector
... ...
include/collision_library.h
... ... @@ -28,12 +28,12 @@ public:
28 28 void add (StaticPBezierSurf* const surf);
29 29  
30 30 // Change "value" and return type if the sphere should be able to be attached to objects other than planes
31   - std::unordered_set<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
  31 + std::unordered_set<StaticPPlane *> getAttachedObjects(DynamicPSphere* sphere);
32 32 // std::vector<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
33 33  
34 34  
35   - void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
36   - void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);
  35 + void setAttachedObjects( std::unordered_set<StaticPPlane*> plane, DynamicPSphere* sphere );
  36 + void detachObjects(DynamicPSphere* sphere);
37 37  
38 38 // States
39 39 void
... ... @@ -43,8 +43,14 @@ public:
43 43 detectStateChange(DynamicPSphere* sphere, double dt);
44 44  
45 45 void
  46 + handleStates (StateChangeObj& state, double dt);
  47 +
  48 + void
46 49 testMethod();
47 50  
  51 + GMlib::Point<float,2>
  52 + closestPoint(DynamicPSphere& S, StaticPPlane& P);
  53 +
48 54  
49 55 // Collisions
50 56 CollisionState detectCollision (const DynamicPSphere& S,
... ... @@ -70,6 +76,10 @@ public:
70 76  
71 77 void handleCollision ( collision::CollisionObject& col, double dt);
72 78  
  79 + Environment _stillEnvironment;
  80 + DefaultEnvironment _env;
  81 +
  82 +
73 83  
74 84 private:
75 85  
... ... @@ -99,11 +109,7 @@ protected:
99 109 std::vector<StateChangeObj> _singularities;
100 110 std::vector<StateChangeObj> _fakeSingularities;
101 111  
102   - DefaultEnvironment _env;
103   - Environment _stillEnvironment;
104   -
105 112 std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>> _map;
106   -// std::unordered_map<DynamicPSphere*, std::vector<StaticPPlane*>> _map;
107 113 std::unordered_set<StaticPPlane*> fakePlanes;
108 114  
109 115  
... ... @@ -123,16 +129,13 @@ public:
123 129 NoChange
124 130 };
125 131  
126   - MyController* _controller;
  132 + MyController* _sphereController;
127 133 States _state = States::Free; // Which state is the sphere in
128 134  
129   - GMlib::Vector<double, 3>
130   - getTrajectory (seconds_type dt);
131 135  
132   - void
133   - setTrajectory (GMlib::Vector<double, 3> ds, DynamicPSphere* sphere);
134 136  
135   - GMlib::Vector<double, 3> _NewTrajectory;
  137 + GMlib::Vector<double,3>
  138 + adjustedTrajectory (seconds_type dt);
136 139  
137 140 // "Old" methods
138 141 void simulateToTInDt( seconds_type t ) override;
... ... @@ -216,9 +219,6 @@ void MyController::crossUnique(Container_1 container1, Container_2 container2) {
216 219 std::vector<collision::CollisionObject> _realCollisions;
217 220 std::vector<StateChangeObj> _realSingularities;
218 221  
219   -// typename Container_1::iterator NE1 = std::end(container1);
220   -// typename Container_2::iterator NE2 = std::end(container2);
221   -
222 222 auto start = std::end(container2);
223 223  
224 224 // Check if any object that is in CONTAINER1 is also in CONTAINER2 and compares time values to remove one
... ... @@ -235,7 +235,6 @@ void MyController::crossUnique(Container_1 container1, Container_2 container2) {
235 235  
236 236 _realCollisions.push_back(*first_iter);
237 237 }
238   -// NE2--;
239 238 }
240 239 else {
241 240  
... ... @@ -244,7 +243,6 @@ void MyController::crossUnique(Container_1 container1, Container_2 container2) {
244 243  
245 244 _realSingularities.push_back(*second_iter);
246 245 }
247   -// NE1--;
248 246 }
249 247 }
250 248 // else {
... ... @@ -343,51 +341,66 @@ template &lt;typename T_s, typename T_o&gt;
343 341 inline
344 342 void MyController::sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type dt) {
345 343  
346   - auto cylinder = dynamic_cast<DynamicPCylinder*>(object);
347   -// auto plane = dynamic_cast<DynamicPPlane*>(object);
348   -
349   - if( cylinder ) {
  344 + bool movingObject = false;
350 345  
351   - // Sphere vs. Dynamic Cylinder
  346 + for( auto& s : _dynamic_spheres ) {
  347 + if( s->_state != DynamicPSphere::States::AtRest ) {
  348 + movingObject = true;
  349 + break;
  350 + }
352 351 }
353   -// else if( plane ) {
354 352  
355   - // Sphere vs. Dynamic Plane
356   -// }
357   - else {
  353 + if( movingObject ) {
358 354  
359   - // Sphere vs. Dynamic Sphere
360   - for( auto iter = std::begin(_dynamic_spheres); iter != std::end(_dynamic_spheres); ++iter) {
  355 + auto cylinder = dynamic_cast<DynamicPCylinder*>(object);
  356 + // auto plane = dynamic_cast<DynamicPPlane*>(object);
361 357  
362   - if( (*iter == sphere) or (*iter == object) ) {
363   - break;
364   - }
365   - else {
  358 + if( cylinder ) {
  359 +
  360 + // Sphere vs. Dynamic Cylinder
  361 + }
  362 + // else if( plane ) {
  363 +
  364 + // Sphere vs. Dynamic Plane
  365 + // }
  366 + else {
  367 +
  368 + // Sphere vs. Dynamic Sphere
  369 + for( auto iter = std::begin(_dynamic_spheres); iter != std::end(_dynamic_spheres); ++iter) {
  370 +
  371 + if( (*iter == sphere) or (*iter == object) or (sphere->_state == DynamicPSphere::States::AtRest)) {
  372 + break;
  373 + }
  374 + else {
366 375  
367   - // Check only for collisions for the first dynamic sphere
368   - auto col = collision::detectCollision(*sphere, **iter, dt);
  376 + // Check only for collisions for the first dynamic sphere
  377 + auto col = collision::detectCollision(*sphere, **iter, dt);
369 378  
370   - // The check for if the second object is a dynamic sphere is done in the main algorithm
371   - // and the method is called again only with the 1'st and 2'nd sphere swapped
  379 + // The check for if the second object is a dynamic sphere is done in the main algorithm
  380 + // and the method is called again only with the 1'st and 2'nd sphere swapped
372 381  
373   - if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
  382 + if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
374 383  
375   - auto& first_sphere = sphere;
376   - auto& second_sphere = *iter;
  384 + auto& first_sphere = sphere;
  385 + auto& second_sphere = *iter;
377 386  
378   - auto new_t = std::max(first_sphere->curr_t_in_dt, second_sphere->curr_t_in_dt);
  387 + auto new_t = std::max(first_sphere->curr_t_in_dt, second_sphere->curr_t_in_dt);
379 388  
380   - if( col.time > seconds_type(new_t) and col.time < dt) {
381   - auto col_obj = CollisionObject(first_sphere, second_sphere, col.time);
382   - _collisions.push_back(col_obj);
  389 + if( col.time > seconds_type(new_t) and col.time < dt) {
  390 + auto col_obj = CollisionObject(first_sphere, second_sphere, col.time);
  391 + _collisions.push_back(col_obj);
383 392  
384   - // Simulate the objects to T in dt?
  393 + // Simulate the objects to T in dt?
385 394  
  395 + }
386 396 }
387 397 }
388 398 }
389 399 }
  400 +
390 401 }
  402 +
  403 +
391 404 }
392 405  
393 406 template <typename T_s, typename T_o>
... ... @@ -395,52 +408,87 @@ inline
395 408 void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type dt)
396 409 {
397 410  
398   - // The game will mainly have two types of static objects that can be collided with, planes and bezier surfaces
399   - // Checks to see which one the sphere has collided with
400   - auto plane = dynamic_cast<const StaticPPlane*>(object);
401   - auto bezier_s = dynamic_cast<const StaticPBezierSurf*>(object);
  411 + bool movingObject = false;
402 412  
  413 + for( auto& s : _dynamic_spheres ) {
  414 + if( s->_state != DynamicPSphere::States::AtRest ) {
  415 + movingObject = true;
  416 + break;
  417 + }
  418 + }
403 419  
404   - // Sphere vs. Static Plane
405   - if( plane ) {
  420 + if( movingObject ) {
406 421  
407   - auto attachedPlanes = _map[sphere];
408 422  
409   - for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
  423 + // The game will mainly have two types of static objects that can be collided with, planes and bezier surfaces
  424 + // Checks to see which one the sphere has collided with
  425 + auto plane = dynamic_cast<const StaticPPlane*>(object);
  426 + auto bezier_s = dynamic_cast<const StaticPBezierSurf*>(object);
410 427  
411   - // The sphere cannot collide with the same plane it just collided with
412   -// if( *iter == plane ) {
413   -// break;
414   -// }
415 428  
416   - // If sphere is attached to the plane, it should NOT check for collisions with it
417   -// for( auto pl = attachedPlanes.begin(); pl != attachedPlanes.end(); pl++) {
  429 + // Sphere vs. Static Plane
  430 + if( plane ) {
  431 +
  432 + auto attachedPlanes = getAttachedObjects(sphere);
  433 +
  434 + for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
  435 +
  436 + // The sphere cannot collide with the same plane it just collided with
  437 + // if( *iter == plane ) {
  438 + // break;
  439 + // }
  440 +
  441 + // If sphere is attached to the plane, it should NOT check for collisions with it
  442 + if( !attachedPlanes.empty() ) {
  443 + for( auto comperator = attachedPlanes.begin(); comperator != attachedPlanes.end(); ++comperator) {
418 444  
419   -// if( *iter == *pl) break;
  445 + if( *iter == *comperator) break;
  446 + else {
420 447  
421   - // Else look for collision
422   - auto col = collision::detectCollision(*sphere, **iter, dt);
  448 + // Else look for collision
  449 + auto col = collision::detectCollision(*sphere, **iter, dt);
423 450  
424   - if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
  451 + if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
425 452  
426   - auto& static_object = *iter;
  453 + auto& static_object = *iter;
427 454  
428   - auto new_t = sphere->curr_t_in_dt;
  455 + auto new_t = sphere->curr_t_in_dt;
429 456  
430   - if( col.time > seconds_type(new_t) and col.time < dt)
431   - {
432   - auto col_obj = CollisionObject(sphere, static_object, col.time);
433   - _collisions.push_back(col_obj);
  457 + if( col.time > seconds_type(new_t) and col.time < dt)
  458 + {
  459 + auto col_obj = CollisionObject(sphere, static_object, col.time);
  460 + _collisions.push_back(col_obj);
434 461  
  462 + }
  463 + }
  464 + }
435 465 }
436 466 }
437   -// } // THIS ONE
  467 + else if( sphere->_state != DynamicPSphere::States::AtRest) {
  468 +
  469 + auto col = collision::detectCollision(*sphere, **iter, dt);
  470 +
  471 + if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
  472 +
  473 + auto& static_object = *iter;
  474 +
  475 + auto new_t = sphere->curr_t_in_dt;
  476 +
  477 + if( col.time > seconds_type(new_t) and col.time < dt)
  478 + {
  479 + auto col_obj = CollisionObject(sphere, static_object, col.time);
  480 + _collisions.push_back(col_obj);
  481 +
  482 + }
  483 + }
  484 + }
  485 + }
438 486 }
439   - }
440 487  
441   - // Same as above, only for bezier surfaces
442   - else if( bezier_s) {
  488 + // Same as above, only for bezier surfaces
  489 + else if( bezier_s) {
443 490  
  491 + }
444 492 }
445 493  
446 494  
... ...
unittests/crossunique.cpp
... ... @@ -9,30 +9,30 @@ using namespace collision;
9 9 #include <gmParametricsModule>
10 10 using namespace GMlib;
11 11  
12   -void
13   -collision::MyController::testMethod()
14   -{
15   - auto sphere1 = _dynamic_spheres[0];
16   - auto sphere2 = _dynamic_spheres[1];
17   - auto sphere3 = _dynamic_spheres[2];
18   - StaticPPlane* plane;
  12 +//void
  13 +//collision::MyController::testMethod()
  14 +//{
  15 +// auto sphere1 = _dynamic_spheres[0];
  16 +// auto sphere2 = _dynamic_spheres[1];
  17 +// auto sphere3 = _dynamic_spheres[2];
  18 +// StaticPPlane* plane;
19 19  
20   - auto fake_sin1 = StateChangeObj(sphere1, fakePlanes, seconds_type(0.2), DynamicPSphere::States::NoChange);
21   - auto fake_sin2 = StateChangeObj(sphere2, fakePlanes, seconds_type(0.4), DynamicPSphere::States::NoChange);
22   - auto fake_sin3 = StateChangeObj(sphere3, fakePlanes, seconds_type(0.7), DynamicPSphere::States::NoChange);
  20 +// auto fake_sin1 = StateChangeObj(sphere1, fakePlanes, seconds_type(0.2), DynamicPSphere::States::NoChange);
  21 +// auto fake_sin2 = StateChangeObj(sphere2, fakePlanes, seconds_type(0.4), DynamicPSphere::States::NoChange);
  22 +// auto fake_sin3 = StateChangeObj(sphere3, fakePlanes, seconds_type(0.7), DynamicPSphere::States::NoChange);
23 23  
24   - auto fake_col1 = CollisionObject( sphere3, sphere2, seconds_type(0.3));
25   -// auto fake_col2 = CollisionObject( sphere2, sphere1, seconds_type(0.4));
26   -// auto fake_col3 = CollisionObject( sphere3, sphere2, seconds_type(0.5));
  24 +// auto fake_col1 = CollisionObject( sphere3, sphere2, seconds_type(0.3));
  25 +//// auto fake_col2 = CollisionObject( sphere2, sphere1, seconds_type(0.4));
  26 +//// auto fake_col3 = CollisionObject( sphere3, sphere2, seconds_type(0.5));
27 27  
28   - _singularities.push_back(fake_sin1);
29   - _singularities.push_back(fake_sin2);
30   - _singularities.push_back(fake_sin3);
  28 +// _singularities.push_back(fake_sin1);
  29 +// _singularities.push_back(fake_sin2);
  30 +// _singularities.push_back(fake_sin3);
31 31  
32   - _collisions.push_back( fake_col1 );
33   -// _collisions.push_back( fake_col2 );
34   -// _collisions.push_back( fake_col3 );
  32 +// _collisions.push_back( fake_col1 );
  33 +//// _collisions.push_back( fake_col2 );
  34 +//// _collisions.push_back( fake_col3 );
35 35  
36   - crossUnique(_collisions, _singularities);
  36 +// crossUnique(_collisions, _singularities);
37 37  
38   -}
  38 +//}
... ...