Commit 3cc57c3494bd50e5fbfb9500a3821f3637b35f6c

Authored by Bjørn-Richard Pedersen
1 parent fa99a852

Made function for cross uniqueness. Should be working. Also backup before remaki…

…ng inner loop in localSimulate.
CMakeLists.txt
... ... @@ -30,9 +30,10 @@ endif()
30 30 include_directories(${COLLISION_INTERFACE_INCLUDE_DIR})
31 31  
32 32  
33   -set(HDRS include/collision_library.h)
  33 +set(HDRS include/collision_library.h)
34 34  
35   -set(SRCS include/collision_library.cpp)
  35 +set(SRCS include/collision_library.cpp
  36 + unittests/crossunique.cpp)
36 37  
37 38 add_library( ${CMAKE_PROJECT_NAME} SHARED ${HDRS} ${SRCS} )
38 39 target_link_libraries( ${CMAKE_PROJECT_NAME}
... ...
include/collision_library.cpp
... ... @@ -349,6 +349,9 @@ namespace collision
349 349 void
350 350 MyController::localSimulate(double dt) {
351 351  
  352 + // Testing
  353 +// testMethod();
  354 +
352 355 // Reset time variable for all objects
353 356 for( auto sphere : _dynamic_spheres) {
354 357  
... ... @@ -357,7 +360,7 @@ namespace collision
357 360  
358 361 // Detect state changes and fill up our state container
359 362 detectStateChanges(dt);
360   -
  363 + sortAndMakeUniqueStates(_singularities);
361 364  
362 365 // Collision detection algorithm
363 366 for( auto& sphere : _dynamic_spheres) {
... ... @@ -370,137 +373,188 @@ namespace collision
370 373 sphereStaticCollision(sphere, plane, seconds_type(dt));
371 374 }
372 375 }
373   -
374 376 // Make Collision unique
375 377 sortAndMakeUnique(_collisions);
376   - std::reverse(_collisions.begin(), _collisions.end());
377 378  
  379 + // Make both collisions and states unique in relation to each other
  380 + if( !_collisions.empty() and !_singularities.empty() ) {
378 381  
  382 + crossUnique(_collisions, _singularities);
  383 + }
  384 + else {
  385 + // Make sure that the newest event is at the front of the vector
  386 + std::reverse(_singularities.begin(), _singularities.end() );
  387 + std::reverse(_collisions.begin(), _collisions.end());
  388 + }
379 389  
380   - // Impact response
  390 +// while( !_collisions.empty() or !_singularities.empty() ) {
  391 +
  392 +// // IF BOTH NOT EMPTY
  393 +// if( !_collisions.empty() and !_singularities.empty() ) {
  394 +
  395 +
  396 +
  397 +// }
  398 +
  399 +// // IF COLLISIONS NOT EMPTY
  400 +
  401 +// // IF SINGULARITIES NOT EMPTY
  402 +// }
  403 +
  404 +
  405 +
  406 + // Impact response COLLISIONS
381 407 while( !_collisions.empty() ) {
382 408  
383 409 auto c = _collisions.back();
384 410 _collisions.pop_back();
385 411  
386   - c.obj1->simulateToTInDt(c.t_in_dt);
387   - c.obj2->simulateToTInDt(c.t_in_dt);
  412 + handleCollision(c, dt);
388 413  
389   - // Add more objects here if you end up using more
390   - auto d_sphere_1 = dynamic_cast<DynamicPSphere*>(c.obj1);
391   - auto d_sphere_2 = dynamic_cast<DynamicPSphere*>(c.obj2);
392   - auto s_sphere_2 = dynamic_cast<StaticPSphere*>(c.obj2);
393   - auto d_cylinder_1 = dynamic_cast<DynamicPCylinder*>(c.obj1);
394   - auto d_cylinder_2 = dynamic_cast<DynamicPCylinder*>(c.obj2);
395   - auto s_cylinder_2 = dynamic_cast<StaticPCylinder*>(c.obj2);
396   - auto s_plane_2 = dynamic_cast<StaticPPlane*>(c.obj2);
397   - auto s_bezier_2 = dynamic_cast<StaticPBezierSurf*>(c.obj2);
  414 + detectStateChanges(dt);
  415 + sortAndMakeUnique(_collisions);
  416 + sortAndMakeUniqueStates(_singularities);
398 417  
  418 + if( !_collisions.empty() and !_singularities.empty() ) {
399 419  
  420 + crossUnique(_collisions, _singularities);
  421 + }
  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());
  426 + }
400 427  
401   - // Impact response
402   - // If the first object is a sphere
403   - if(d_sphere_1) {
404 428  
  429 + }
405 430  
406   - if (d_sphere_2)
407   - collision::computeImpactResponse( *d_sphere_1, *d_sphere_2, c.t_in_dt); // D_Sphere vs. D_Sphere
408   - else if (d_sphere_1 && s_sphere_2)
409   - collision::computeImpactResponse( *d_sphere_1, *s_sphere_2, c.t_in_dt); // D_Sphere vs. S_Sphere
410   - else if (d_sphere_1 && d_cylinder_2)
411   - collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. D_Cylinder
412   - else if (d_sphere_1 && s_cylinder_2)
413   - collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. S_Cylinder
414   - else if (d_sphere_1 && s_plane_2)
415   - collision::computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
416   - else if (d_sphere_1 && s_bezier_2)
417   - collision::computeImpactResponse( *d_sphere_1, *s_bezier_2, c.t_in_dt); // D_Sphere vs. S_Bezier
  431 +// Start simulation for all objects
  432 + for( auto sphere : _dynamic_spheres) {
418 433  
419   - }
  434 + sphere->simulateToTInDt(seconds_type(dt));
  435 + }
420 436  
421   - // First object is cylinder
422   - if(d_cylinder_1) {
423   -
424   - if (d_cylinder_2)
425   - collision::computeImpactResponse( *d_cylinder_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. D_Cylinder
426   - else if (d_cylinder_1 && s_cylinder_2)
427   - collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. S_Cylinder
428   - else if (d_cylinder_1 && d_sphere_2)
429   - collision::computeImpactResponse( *d_sphere_2, *d_cylinder_1, c.t_in_dt); // D_Cylinder vs. D_Sphere
430   - else if (d_cylinder_1 && s_sphere_2)
431   - collision::computeImpactResponse( *d_cylinder_1, *s_sphere_2, c.t_in_dt); // D_Cylinder vs. S_Sphere
432   - else if (d_cylinder_1 && s_bezier_2)
433   - collision::computeImpactResponse( *d_cylinder_1, *s_bezier_2, c.t_in_dt); // D_Cylinder vs. S_Bezier
434   - else if (d_cylinder_1 && s_plane_2)
435   - collision::computeImpactResponse( *d_cylinder_1, *s_plane_2, c.t_in_dt); // D_Cylinder vs. S_Plane
  437 + }
436 438  
437   - }
  439 + // Collision handeling
  440 + void
  441 + MyController::handleCollision(CollisionObject &c, double dt) {
438 442  
439   - // Additional collisions for the dynamic objects in the collision_object
440   - // Not allowed: Same collision twice, cannot collide with itself
  443 +// auto c = _collisions.back();
  444 +// _collisions.pop_back();
441 445  
442   - // If the dynamic object (obj1) is a sphere
443   - if( d_sphere_1) {
  446 + c.obj1->simulateToTInDt(c.t_in_dt);
  447 + c.obj2->simulateToTInDt(c.t_in_dt);
444 448  
445   - sphereDynamicCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with same obj as last time
446   - sphereStaticCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Can't with same obj as last time
  449 + // Add more objects here if you end up using more
  450 + auto d_sphere_1 = dynamic_cast<DynamicPSphere*>(c.obj1);
  451 + auto d_sphere_2 = dynamic_cast<DynamicPSphere*>(c.obj2);
  452 + auto s_sphere_2 = dynamic_cast<StaticPSphere*>(c.obj2);
  453 + auto d_cylinder_1 = dynamic_cast<DynamicPCylinder*>(c.obj1);
  454 + auto d_cylinder_2 = dynamic_cast<DynamicPCylinder*>(c.obj2);
  455 + auto s_cylinder_2 = dynamic_cast<StaticPCylinder*>(c.obj2);
  456 + auto s_plane_2 = dynamic_cast<StaticPPlane*>(c.obj2);
  457 + auto s_bezier_2 = dynamic_cast<StaticPBezierSurf*>(c.obj2);
447 458  
448   - // If sphere 1 collided with a dynamic sphere, check for that sphere's future collisions
449   - if(d_sphere_2) {
450 459  
451   - sphereDynamicCollision(d_sphere_2, d_sphere_1, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with sphere 1
452   - sphereStaticCollision(d_sphere_2, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Placeholder variable for "Last object"
453   - }
454 460  
455   - // Same if object 2 is a dynamic cylinder
456   - if(d_cylinder_2) {
  461 + // Impact response
  462 + // If the first object is a sphere
  463 + if(d_sphere_1) {
  464 +
  465 +
  466 + if (d_sphere_2)
  467 + collision::computeImpactResponse( *d_sphere_1, *d_sphere_2, c.t_in_dt); // D_Sphere vs. D_Sphere
  468 + else if (d_sphere_1 && s_sphere_2)
  469 + collision::computeImpactResponse( *d_sphere_1, *s_sphere_2, c.t_in_dt); // D_Sphere vs. S_Sphere
  470 + else if (d_sphere_1 && d_cylinder_2)
  471 + collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. D_Cylinder
  472 + else if (d_sphere_1 && s_cylinder_2)
  473 + 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
  476 + else if (d_sphere_1 && s_bezier_2)
  477 + collision::computeImpactResponse( *d_sphere_1, *s_bezier_2, c.t_in_dt); // D_Sphere vs. S_Bezier
457 478  
458   - //cylinderDynamicCollision(d_cylinder_2, d_sphere_1, seconds_type(dt));
459   - //cylinderStaticCollision(d_cylinder_2, c.obj2, seconds_type(dt));
460   - }
461   - }
  479 + }
462 480  
463   -// If the dynamic object (obj1) is a cylinder
464   - if(d_cylinder_1) {
  481 + // First object is cylinder
  482 + if(d_cylinder_1) {
  483 +
  484 + if (d_cylinder_2)
  485 + collision::computeImpactResponse( *d_cylinder_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. D_Cylinder
  486 + else if (d_cylinder_1 && s_cylinder_2)
  487 + collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. S_Cylinder
  488 + else if (d_cylinder_1 && d_sphere_2)
  489 + collision::computeImpactResponse( *d_sphere_2, *d_cylinder_1, c.t_in_dt); // D_Cylinder vs. D_Sphere
  490 + else if (d_cylinder_1 && s_sphere_2)
  491 + collision::computeImpactResponse( *d_cylinder_1, *s_sphere_2, c.t_in_dt); // D_Cylinder vs. S_Sphere
  492 + else if (d_cylinder_1 && s_bezier_2)
  493 + collision::computeImpactResponse( *d_cylinder_1, *s_bezier_2, c.t_in_dt); // D_Cylinder vs. S_Bezier
  494 + else if (d_cylinder_1 && s_plane_2)
  495 + collision::computeImpactResponse( *d_cylinder_1, *s_plane_2, c.t_in_dt); // D_Cylinder vs. S_Plane
465 496  
466   - //cylinderDynamicCollision(d_cylinder_1, c.obj2, seconds_type(dt));
467   - //cylinderStaticCollision(d_cylinder_1, c.obj2, seconds_type(dt));
  497 + }
468 498  
469   - if(d_cylinder_2) {
  499 + // Additional collisions for the dynamic objects in the collision_object
  500 + // Not allowed: Same collision twice, cannot collide with itself
470 501  
471   - //cylinderDynamicCollision(d_cylinder_2, d_cylinder_1, seconds_type(dt));
472   - //cylinderStaticCollision(d_cylinder_2, c.obj2, seconds_type(dt));
473   - }
  502 + // If the dynamic object (obj1) is a sphere
  503 + if( d_sphere_1) {
474 504  
475   - if(d_sphere_2) {
  505 + sphereDynamicCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with same obj as last time
  506 + sphereStaticCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Can't with same obj as last time
476 507  
477   - sphereDynamicCollision(d_sphere_2, d_sphere_1, seconds_type(dt));
478   - sphereStaticCollision(d_sphere_2, c.obj2, seconds_type(dt));
479   - }
  508 + // If sphere 1 collided with a dynamic sphere, check for that sphere's future collisions
  509 + if(d_sphere_2) {
  510 +
  511 + sphereDynamicCollision(d_sphere_2, d_sphere_1, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with sphere 1
  512 + sphereStaticCollision(d_sphere_2, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Placeholder variable for "Last object"
480 513 }
481 514  
  515 + // Same if object 2 is a dynamic cylinder
  516 + if(d_cylinder_2) {
482 517  
  518 + //cylinderDynamicCollision(d_cylinder_2, d_sphere_1, seconds_type(dt));
  519 + //cylinderStaticCollision(d_cylinder_2, c.obj2, seconds_type(dt));
  520 + }
483 521 }
484 522  
485   -// Start simulation for all objects
486   - for( auto sphere : _dynamic_spheres) {
  523 +// If the dynamic object (obj1) is a cylinder
  524 + if(d_cylinder_1) {
487 525  
488   - sphere->simulateToTInDt(seconds_type(dt));
489   - }
  526 + //cylinderDynamicCollision(d_cylinder_1, c.obj2, seconds_type(dt));
  527 + //cylinderStaticCollision(d_cylinder_1, c.obj2, seconds_type(dt));
  528 +
  529 + if(d_cylinder_2) {
490 530  
  531 + //cylinderDynamicCollision(d_cylinder_2, d_cylinder_1, seconds_type(dt));
  532 + //cylinderStaticCollision(d_cylinder_2, c.obj2, seconds_type(dt));
  533 + }
  534 +
  535 + if(d_sphere_2) {
  536 +
  537 + sphereDynamicCollision(d_sphere_2, d_sphere_1, seconds_type(dt));
  538 + sphereStaticCollision(d_sphere_2, c.obj2, seconds_type(dt));
  539 + }
  540 + }
491 541 }
492 542  
493 543 void
494 544 MyController::detectStateChanges(double dt) {
495 545  
496   - for( auto& sphere : _dynamic_spheres) {
  546 +// for( auto& sphere : _dynamic_spheres) {
497 547  
498   - auto singularity = detectStateChange(sphere, dt);
  548 +// auto singularity = detectStateChange(sphere, dt);
499 549  
500 550 // if (singularity.state != DynamicPSphere::States::NoChange) {
501   - _singularities.push_back(singularity);
  551 +// auto fake = StateChangeObj(sphere, fakePlanes, seconds_type(dt), DynamicPSphere::States::NoChange );
  552 +
  553 +// _singularities.push_back(singularity);
  554 +
  555 +// _singularities.push_back(fake);
502 556 // }
503   - }
  557 +// }
504 558  
505 559 }
506 560  
... ... @@ -611,7 +665,6 @@ namespace collision
611 665 returnTime = ((dn / dsn) * newDt) + sphereTime;
612 666  
613 667 /// Check the sphere's current state, and if it needs to be changed.
614   - // If there sphere is rolling..
615 668 if( sphere->_state == DynamicPSphere::States::Rolling) {
616 669  
617 670 // Detatchment is seen as a state change, and will be processed further in the algorithm
... ... @@ -628,7 +681,7 @@ namespace collision
628 681 // Else the sphere will keep being in a Rolling state
629 682 else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::NoChange );
630 683 }
631   - // If the sphere is still..
  684 +
632 685 if( sphere->_state == DynamicPSphere::States::AtRest ) {
633 686  
634 687 if( dsn > 0 ) {
... ... @@ -645,113 +698,36 @@ namespace collision
645 698 }
646 699 }
647 700  
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 701 }
697 702  
698   -
699   -
700   - /// old SingularityDetection
701   - // Singularity Detection
702 703 // 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);
  704 +// MyController::testMethod()
  705 +// {
  706 +// auto sphere1 = _dynamic_spheres[0];
  707 +// auto sphere2 = _dynamic_spheres[1];
  708 +// auto sphere3 = _dynamic_spheres[2];
  709 +// StaticPPlane* plane;
719 710  
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);
  711 +// auto fake_sin1 = StateChangeObj(sphere1, fakePlanes, seconds_type(0.2), DynamicPSphere::States::NoChange);
  712 +// auto fake_sin2 = StateChangeObj(sphere2, fakePlanes, seconds_type(0.4), DynamicPSphere::States::NoChange);
  713 +// auto fake_sin3 = StateChangeObj(sphere3, fakePlanes, seconds_type(0.7), DynamicPSphere::States::NoChange);
725 714  
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;
  715 +// auto fake_col1 = CollisionObject( sphere3, sphere2, seconds_type(0.3));
  716 +//// auto fake_col2 = CollisionObject( sphere2, sphere1, seconds_type(0.4));
  717 +//// auto fake_col3 = CollisionObject( sphere3, sphere2, seconds_type(0.5));
731 718  
732   -// // Still
733   -// if ( std::abs( ((-n*r) * ds) -(ds*ds) ) < epsilon) {
  719 +// _singularities.push_back(fake_sin1);
  720 +// _singularities.push_back(fake_sin2);
  721 +// _singularities.push_back(fake_sin3);
734 722  
735   -// auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::AtRest);
736   -// _singularities.push_back(singularity);
737   -// }
  723 +// _collisions.push_back( fake_col1 );
  724 +//// _collisions.push_back( fake_col2 );
  725 +//// _collisions.push_back( fake_col3 );
738 726  
739   -// // Rolling
740   -// else if(std::abs(((-n*r) * ds) -(ds*ds)) > epsilon) {
  727 +// crossUnique(_collisions, _singularities);
741 728  
742   -// auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Rolling);
743   -// _singularities.push_back(singularity);
744   -// }
  729 +// }
745 730  
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
755 731  
756 732  
757 733 /// vector; Get, Set and Remove objects to / from Sphere
... ... @@ -779,7 +755,6 @@ namespace collision
779 755 */
780 756  
781 757 /// unordered_set; Get, Set and Remove objects to / from Sphere
782   -
783 758 // Get objects attached to sphere
784 759 std::unordered_set<StaticPPlane *>
785 760 MyController::getAttachedObjects(DynamicPSphere* sphere)
... ... @@ -833,45 +808,6 @@ namespace collision
833 808 }
834 809  
835 810  
836   - // States
837   -// void
838   -// MyController::detectStateChange( StateChangeObj State ) {
839   -
840   -
841   -// auto sphere = State.obj1;
842   -// auto plane = State.obj2;
843   -// auto state = State.state;
844   -
845   -// if( state == DynamicPSphere::States::Free) {
846   -
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) {
851   -
852   -// // Remember to check state in CollisionDetection and SimulateToTInDt and make adjustments
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);
858   -
859   -// // Update physics?
860   -// }
861   -// else if( state == DynamicPSphere::States::Rolling) {
862   -
863   -// // Setting attached
864   -// sphere->_state = DynamicPSphere::States::Rolling;
865   -// std::cout << "The state is now: Rolling" << std::endl;
866   -// _map[sphere].emplace(plane);
867   -
868   -// // Update physics ?
869   -
870   -// }
871   -
872   -// } // EOF
873   -
874   -
875 811 } // END namespace collision
876 812  
877 813  
... ...
include/collision_library.h
... ... @@ -7,6 +7,7 @@
7 7 #include <typeinfo>
8 8 #include <unordered_map>
9 9  
  10 +
10 11 namespace collision
11 12 {
12 13  
... ... @@ -34,13 +35,18 @@ public:
34 35 void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
35 36 void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);
36 37  
  38 + // States
37 39 void
38 40 detectStateChanges(double dt);
39 41  
40 42 StateChangeObj
41 43 detectStateChange(DynamicPSphere* sphere, double dt);
42 44  
  45 + void
  46 + testMethod();
  47 +
43 48  
  49 + // Collisions
44 50 CollisionState detectCollision (const DynamicPSphere& S,
45 51 const DynamicPCylinder& C, seconds_type dt);
46 52  
... ... @@ -62,6 +68,8 @@ public:
62 68 void computeImpactResponse (DynamicPCylinder& C, const StaticPPlane& P,
63 69 seconds_type dt);
64 70  
  71 + void handleCollision ( collision::CollisionObject& col, double dt);
  72 +
65 73  
66 74 private:
67 75  
... ... @@ -71,6 +79,12 @@ private:
71 79 template <typename T_s, typename T_o>
72 80 void sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type dt);
73 81  
  82 + template <class Container>
  83 + void sortAndMakeUniqueStates(Container& c);
  84 +
  85 + template <typename Container_1, typename Container_2>
  86 + void crossUnique( Container_1 container1, Container_2 container2);
  87 +
74 88 protected:
75 89 void localSimulate (double dt) override;
76 90  
... ... @@ -83,11 +97,14 @@ protected:
83 97  
84 98 std::vector<collision::CollisionObject> _collisions;
85 99 std::vector<StateChangeObj> _singularities;
  100 + std::vector<StateChangeObj> _fakeSingularities;
86 101  
87 102 DefaultEnvironment _env;
  103 + Environment _stillEnvironment;
88 104  
89 105 std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>> _map;
90 106 // std::unordered_map<DynamicPSphere*, std::vector<StaticPPlane*>> _map;
  107 + std::unordered_set<StaticPPlane*> fakePlanes;
91 108  
92 109  
93 110  
... ... @@ -130,14 +147,14 @@ public:
130 147  
131 148 // StateChangeObject struct
132 149 struct StateChangeObj {
133   - DynamicPSphere* obj1; // Object whos state will change
  150 + DynamicPSphere* obj; // Object whos state will change
134 151 std::unordered_set<StaticPPlane*> attachedPlanes; // Object that obj1 will change state ACCORDING to
135 152 seconds_type time; // Time of singularity
136 153 DynamicPSphere::States state; // State that obj1 will change to
137 154  
138 155 StateChangeObj
139 156 (DynamicPSphere* o1, std::unordered_set<StaticPPlane*> planes, seconds_type t, DynamicPSphere::States s) :
140   - obj1{o1}, attachedPlanes{planes}, time{t} ,state{s} {}
  157 + obj{o1}, attachedPlanes{planes}, time{t} ,state{s} {}
141 158 };
142 159  
143 160 template <class PSurf_T, typename... Arguments>
... ... @@ -153,6 +170,126 @@ std::unique_ptr&lt;StaticPhysObject&lt;PSurf_T&gt;&gt; unittestStaticPhysObjectFactory(Argum
153 170 return std::make_unique<StaticPhysObject<PSurf_T>> (parameters...);
154 171 }
155 172  
  173 +// Make collisions and states crosswise unique
  174 +template <typename Container_1, typename Container_2>
  175 +void MyController::crossUnique(Container_1 container1, Container_2 container2) {
  176 +
  177 + auto objPred = [](const auto &a, const auto &b) {
  178 +
  179 + if( a.obj1 == b.obj or a.obj2 == b.obj ) return true;
  180 +
  181 + return false;
  182 + };
  183 +
  184 + auto timePred = [](const auto &a, const auto &b) {
  185 +
  186 + if( a.t_in_dt < b.time ) return true;
  187 +
  188 + return false;
  189 + };
  190 +
  191 + auto inCollisionsAlreadyPred = [](Container_1 a, const auto &b) {
  192 +
  193 + for( auto& c : a) {
  194 +
  195 + // Check if any of the collisions objects are in the collection already
  196 + if( c.obj1 == b.obj1 ) return true;
  197 + if( c.obj1 == b.obj2 ) return true;
  198 + if( c.obj2 == b.obj1 ) return true;
  199 + if( c.obj2 == b.obj2 ) return true;
  200 +
  201 + return false;
  202 + }
  203 + };
  204 +
  205 + auto inSingularitiesAlreadyPred = [](Container_2 a, const auto &b) {
  206 +
  207 + for( auto& c : a) {
  208 +
  209 + // Check if any of the state objects are in the collection already
  210 + if( c.obj == b.obj ) return true;
  211 +
  212 + return false;
  213 + }
  214 + };
  215 +
  216 + std::vector<collision::CollisionObject> _realCollisions;
  217 + std::vector<StateChangeObj> _realSingularities;
  218 +
  219 +// typename Container_1::iterator NE1 = std::end(container1);
  220 +// typename Container_2::iterator NE2 = std::end(container2);
  221 +
  222 + auto start = std::end(container2);
  223 +
  224 + // Check if any object that is in CONTAINER1 is also in CONTAINER2 and compares time values to remove one
  225 + for( auto first_iter = std::end(container1) - 1; first_iter != std::begin(container1) -1; --first_iter) {
  226 + for( auto second_iter = start - 1; second_iter != std::begin(container2) -1; --second_iter ) {
  227 +
  228 +
  229 + if(( objPred( *first_iter, *second_iter ) )) {
  230 +
  231 + if( timePred( *first_iter, *second_iter )) {
  232 +
  233 + // Keep collision
  234 + if( inCollisionsAlreadyPred(_realCollisions, *first_iter) == false) {
  235 +
  236 + _realCollisions.push_back(*first_iter);
  237 + }
  238 +// NE2--;
  239 + }
  240 + else {
  241 +
  242 + // Keep state
  243 + if( inSingularitiesAlreadyPred(_realSingularities, *second_iter) == false ) {
  244 +
  245 + _realSingularities.push_back(*second_iter);
  246 + }
  247 +// NE1--;
  248 + }
  249 + }
  250 +// else {
  251 +// break;
  252 +// }
  253 + }
  254 + }
  255 +
  256 + _collisions = _realCollisions;
  257 + _singularities = _realSingularities;
  258 +}
  259 +
  260 +// Sort and make Unique for states
  261 +template <class Container>
  262 +void MyController::sortAndMakeUniqueStates(Container& c) {
  263 +
  264 + // Sorting
  265 + std::sort( std::begin(c), std::end(c), [] (const auto& a, const auto&b) {
  266 + return a.time < b.time;
  267 + });
  268 +
  269 + // Make unique
  270 +
  271 + auto pred = [](const auto &a, const auto &b) {
  272 +
  273 + if( a.obj == b.obj ) return true;
  274 +
  275 + return false;
  276 + };
  277 +
  278 + typename Container::iterator NE = std::end(c);
  279 + for( auto first_iter = std::begin(c); first_iter != NE; ++first_iter) {
  280 +
  281 + for( auto r_iterator = NE - 1; r_iterator != first_iter; --r_iterator) {
  282 +
  283 + if( (pred(*first_iter, *r_iterator))) {
  284 + std::swap( *r_iterator, *(NE-1) );
  285 + NE--;
  286 +
  287 + }
  288 + }
  289 + }
  290 +
  291 + c.erase( NE, std::end( c ) );
  292 +}
156 293  
157 294 template <class Container_T >
158 295 void sortAndMakeUnique( Container_T& container) {
... ...
unittests/crossunique.cpp 0 → 100644
  1 +// gtest
  2 +#include <gtest/gtest.h> // googletest header file
  3 +
  4 +// collib
  5 +#include <collision_library.h>
  6 +using namespace collision;
  7 +
  8 +// gmlib
  9 +#include <gmParametricsModule>
  10 +using namespace GMlib;
  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;
  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);
  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));
  27 +
  28 + _singularities.push_back(fake_sin1);
  29 + _singularities.push_back(fake_sin2);
  30 + _singularities.push_back(fake_sin3);
  31 +
  32 + _collisions.push_back( fake_col1 );
  33 +// _collisions.push_back( fake_col2 );
  34 +// _collisions.push_back( fake_col3 );
  35 +
  36 + crossUnique(_collisions, _singularities);
  37 +
  38 +}
... ...