Commit 2a0596d2027c489a74d57f563461f60aebe67286

Authored by Bjørn-Richard Pedersen
1 parent a4446351

Changed my collision detection functions, much more stable now

Showing 2 changed files with 94 additions and 459 deletions   Show diff stats
include/collision_library.cpp
... ... @@ -53,7 +53,7 @@ namespace collision
53 53 const auto _rr = std::pow( radius_sum, 2);
54 54 const auto _square = std::sqrt(_QRQR - (_RR * (_QQ - _rr)));
55 55  
56   - const auto epsilon = 0.00001;
  56 + const auto epsilon = 1e-7;
57 57  
58 58 if ( _square < 0 )
59 59 {
... ... @@ -249,12 +249,13 @@ namespace collision
249 249 auto max_dt = dt;
250 250 auto min_dt = sphere->curr_t_in_dt;
251 251 auto new_dt = max_dt -min_dt;
  252 +
252 253 float u = 0.4;
253 254 float v = 0.4;
254 255 float delta_u = 0.5;
255 256 float delta_v = 0.5;
256   - auto s = sphere->getMatrixToScene() * sphere->getPos();
257   - auto r = sphere->getRadius();
  257 +
  258 + auto s = sphere->getMatrixToScene() * sphere->getPos();
258 259 GMlib::Vector<double, 3> ds = sphere->computeTrajectory(new_dt);
259 260 auto p = s+ds;
260 261 GMlib::SqMatrix<float,2> A;
... ... @@ -279,25 +280,29 @@ namespace collision
279 280 for(auto it = planes.begin(); it != planes.end(); it++){
280 281 GMlib::Vector<float,3> normal{0.0f,0.0f,0.0f};
281 282 GMlib::DMatrix<GMlib::Vector<float,3>> M = (*it)->evaluateParent(u,v,2,2);
282   - q = M(0)(0);
283   - Su += M(1)(0);
284   - Sv += M(0)(1);
  283 + q = M(0)(0);
  284 + Su += M(1)(0);
  285 + Sv += M(0)(1);
285 286 Suu += M(2)(0);
286 287 Svv += M(0)(2);
287 288 Suv += M(1)(1);
288 289 }
289   - d =(q-p);
  290 + d =(q - p);
290 291 A[0][0] = d* Suu + Su * Su;
291 292 A[0][1] = d* Suv + Su * Sv;
292 293 A[1][0] = d* Suv + Su * Sv;
293 294 A[1][1] = d* Svv + Sv * Sv;
  295 +
294 296 GMlib::SqMatrix<float,2> A_inv = A;
295 297 A_inv.invert();
  298 +
296 299 b[0] = - d * Su;
297 300 b[1] = - d * Sv;
298   - GMlib::APoint<float, 3> X = A_inv*b;
  301 +
  302 + GMlib::APoint<float, 3> X = A_inv * b;
299 303 delta_u = X(0);
300 304 delta_v = X(1);
  305 +
301 306 u += delta_u;
302 307 v += delta_v;
303 308  
... ... @@ -306,67 +311,6 @@ namespace collision
306 311 return d;
307 312 }
308 313  
309   - void
310   - computeImpactResponse (DynamicPSphere& S, const StaticPBezierSurf& B,
311   - seconds_type dt) {
312   -
313   - }
314   -
315   - CollisionState detectCollision (const DynamicPSphere& S0,
316   - const StaticPSphere& S1, seconds_type dt) {
317   -
318   - }
319   -
320   - void
321   - computeImpactResponse (DynamicPSphere& S0, const StaticPSphere& S1,
322   - seconds_type dt) {
323   -
324   - }
325   -
326   -// CollisionState
327   -// detectCollision(const DynamicPSphere &S, const DynamicPCylinder &C, seconds_type dt) {
328   -
329   -// }
330   -
331   -// void
332   -// computeImpactResponse (DynamicPSphere& S, DynamicPCylinder& C,
333   -// seconds_type dt) {
334   -
335   -// }
336   -
337   - CollisionState
338   - detectCollision (const DynamicPSphere& S, const StaticPCylinder& C, seconds_type dt) {
339   -
340   - }
341   -
342   - void
343   - computeImpactResponse (DynamicPSphere& S, const StaticPCylinder& C,
344   - seconds_type dt);
345   -
346   -// CollisionState
347   -// detectCollision (const DynamicPCylinder& C0, const DynamicPCylinder& C, seconds_type dt);
348   -
349   -// void computeImpactResponse (DynamicPCylinder& C0, DynamicPCylinder& C1,
350   -// seconds_type dt) {
351   -
352   -// }
353   -
354   -// void computeImpactResponse (DynamicPCylinder& C, const StaticPSphere& S,
355   -// seconds_type dt){
356   -
357   -// }
358   -
359   -// void computeImpactResponse (DynamicPCylinder& C, const StaticPBezierSurf& B,
360   -// seconds_type dt){
361   -
362   -// }
363   -
364   -// void computeImpactResponse (DynamicPCylinder& C, const StaticPPlane& P,
365   -// seconds_type dt){
366   -
367   -// }
368   -
369   -
370 314 std::unique_ptr<Controller>
371 315 unittestCollisionControllerFactory() {
372 316  
... ... @@ -376,63 +320,54 @@ namespace collision
376 320 void
377 321 DynamicPhysObject<GMlib::PSphere<float> >::simulateToTInDt(seconds_type t){
378 322  
379   -// std::ofstream myFile;
380   -
381   -// myFile.open("SimulateLog.txt", std::ios::app );
382 323  
383   -// myFile << this << "\t" << t.count() << "\t\t"
384   -// << int(this->_state) << "\t\t"
385   -// << "(" << this->getPos() << ")" << "\t\t";
386   -
387   - GMlib::Vector<float,3> stopVector = {1e-2, 1e-2, 1e-2};
388   -
389   - if( this->_state == DynamicPSphere::States::AtRest or (this->velocity) < stopVector ) {
  324 + if( this->_state == DynamicPSphere::States::AtRest or this->velocity <= 0.04 ) {
390 325  
391 326 this->velocity = {0.0f, 0.0f, 0.0f };
392 327 this->environment = &_sphereController->_stillEnvironment;
393 328 }
394   - else if( this->_state == DynamicPSphere::States::Rolling) {
  329 + else {
395 330  
  331 + //move
396 332 auto dt0 = seconds_type(t - this->curr_t_in_dt);
397 333 auto Mi = this->getMatrixToSceneInverse();
398   - //move
399 334  
400 335 GMlib::Vector<double,3> ds = (0.0f,0.0f,0.0f);
401 336  
402   - ds = adjustedTrajectory(dt0);
  337 + if( this->_state == DynamicPSphere::States::Rolling ) {
  338 + ds = adjustedTrajectory(dt0);
403 339  
404   - this->translateParent(Mi*ds);
405   - this->curr_t_in_dt =t;
406   - //update physics
  340 + if(ds < 20 ) {
407 341  
408   - auto F = this->externalForces();
409   - auto c = dt0.count();
410   - auto a = F * c;
411   - this->velocity -= a;
  342 + this->velocity = {0.0f, 0.0f, 0.0f };
  343 + this->environment = &_sphereController->_stillEnvironment;
  344 + }
412 345  
413   - }
414   - else {
  346 + this->translateParent(Mi*ds);
  347 + this->curr_t_in_dt = t;
415 348  
416   - auto dt0 = seconds_type(t - this->curr_t_in_dt);
417   - auto Mi = this->getMatrixToSceneInverse();
418   - //move
  349 + //Update physics
  350 + auto F = this->externalForces();
  351 + auto c = dt0.count();
  352 + auto a = F * c;
  353 + this->velocity += a;
  354 + }
  355 + else {
419 356  
420   - GMlib::Vector<double,3> ds = (0.0f,0.0f,0.0f);
  357 + ds = computeTrajectory(dt0);
421 358  
422   - ds = computeTrajectory(dt0);
  359 + this->translateParent(Mi*ds);
  360 + this->curr_t_in_dt =t;
423 361  
424   - this->translateParent(Mi*ds);
425   - this->curr_t_in_dt =t;
426   - //update physics
  362 + //update physics
427 363  
428   - auto F = this->externalForces();
429   - auto c = dt0.count();
430   - auto a = F*c;
431   - this->velocity += a;
  364 + auto F = this->externalForces();
  365 + auto c = dt0.count();
  366 + auto a = F*c;
  367 + this->velocity += a;
  368 + }
432 369 }
433 370  
434   -// myFile << "(" << this->getPos() << ")" << "\t\t[" << ds << "]\t\t" << int(this->_state) << std::endl;
435   -
436 371 // myFile.close();
437 372  
438 373  
... ... @@ -478,12 +413,10 @@ namespace collision
478 413  
479 414 n = GMlib::Vector<float,3> (n / planes.size()).getNormalized();
480 415 auto closest = this->_sphereController->closestPoint(this,dt);
481   - auto d = n*r + closest;
  416 + auto d = (n * r) + closest;
482 417  
483 418 auto adjusted_ds = ds + d;
484 419  
485   -
486   -
487 420 return adjusted_ds;
488 421  
489 422 }
... ... @@ -499,9 +432,6 @@ namespace collision
499 432 void
500 433 MyController::localSimulate(double dt) {
501 434  
502   - // Testing
503   -// testMethod();
504   -
505 435 // Reset time variable for all objects
506 436 for( auto sphere : _dynamic_spheres) {
507 437  
... ... @@ -515,13 +445,8 @@ namespace collision
515 445 // Collision detection algorithm
516 446 for( auto& sphere : _dynamic_spheres) {
517 447  
518   - // Sending in sphere twice in the initial call because the function will require a second object in future calls
519   - sphereDynamicCollision(sphere, sphere, seconds_type(dt));
520   -
521   - for( auto& plane : _static_planes ) {
  448 + dynamicCollision(sphere, seconds_type(dt));
522 449  
523   - sphereStaticCollision(sphere, plane, seconds_type(dt));
524   - }
525 450 }
526 451 // Make Collision unique
527 452 sortAndMakeUnique(_collisions);
... ... @@ -578,22 +503,11 @@ namespace collision
578 503 handleStates(s, dt);
579 504  
580 505 // Collision detection algorithm
  506 + for( auto& sphere : _dynamic_spheres) {
581 507  
582   - sphereDynamicCollision(s.obj, s.obj, seconds_type(dt));
  508 + dynamicCollision(sphere, seconds_type(dt));
583 509  
584   - for( auto& plane : s.attachedPlanes ) {
585   - sphereStaticCollision(s.obj, plane, seconds_type(dt));
586 510 }
587   -// for( auto& sphere : _dynamic_spheres) {
588   -
589   -// // Sending in sphere twice in the initial call because the function will require a second object in future calls
590   -// sphereDynamicCollision(sphere, sphere, seconds_type(dt));
591   -
592   -// for( auto& plane : _static_planes ) {
593   -
594   -// sphereStaticCollision(sphere, plane, seconds_type(dt));
595   -// }
596   -// }
597 511  
598 512 detectStateChanges(dt);
599 513  
... ... @@ -646,21 +560,10 @@ namespace collision
646 560 handleStates(s, dt);
647 561  
648 562 // Collision detection algorithm
649   - sphereDynamicCollision(s.obj, s.obj, seconds_type(dt));
  563 + for( auto& sphere : _dynamic_spheres) {
650 564  
651   - for( auto& plane : s.attachedPlanes ) {
652   - sphereStaticCollision(s.obj, plane, seconds_type(dt));
  565 + dynamicCollision(sphere, seconds_type(dt));
653 566 }
654   -// for( auto& sphere : _dynamic_spheres) {
655   -
656   -// // Sending in sphere twice in the initial call because the function will require a second object in future calls
657   -// sphereDynamicCollision(sphere, sphere, seconds_type(dt));
658   -
659   -// for( auto& plane : _static_planes ) {
660   -
661   -// sphereStaticCollision(sphere, plane, seconds_type(dt));
662   -// }
663   -// }
664 567  
665 568 detectStateChanges(dt);
666 569  
... ... @@ -697,45 +600,21 @@ namespace collision
697 600 auto Statetime = state.time;
698 601 auto planes = state.attachedPlanes;
699 602  
700   -// std::ofstream myFile;
701   -
702   -// myFile.open("SingularityLog.txt", std::ios::app );
703   -
704   -// myFile << sphere << "\t\t" << time.count() << "\t\t\t"
705   -// << int(sphere->_state) << "\t\t\t\t";
706   -
707   - GMlib::Vector<double,3> ds {0.0f, 0.0f, 0.0f};
708   -
709 603 std::cout << "handleStates says the state is now " << int(newState) << " after being " << int(sphere->_state) << std::endl;
710 604  
711 605 if( newState == DynamicPSphere::States::Free ) {
712 606  
713 607 detachObjects(sphere);
714 608 sphere->_state = newState;
715   - sphere->simulateToTInDt(seconds_type(dt));
  609 + sphere->simulateToTInDt(Statetime);
716 610 }
  611 +
717 612 else {
718 613  
719 614 setAttachedObjects(planes, sphere);
720 615 sphere->_state = newState;
721   -
722   - if( sphere->_state == DynamicPSphere::States::AtRest ) {
723   -
724   - sphere->velocity = {0.0f, 0.0f, 0.0f};
725   - sphere->environment = &_stillEnvironment;
726   - sphere->curr_t_in_dt = Statetime;
727   - }
728   -
729   - else sphere->simulateToTInDt(seconds_type(dt));
  616 + sphere->simulateToTInDt(Statetime);
730 617 }
731   -
732   -
733   -
734   -// myFile << int(newState) << std::endl;
735   -
736   -// myFile.close();
737   -
738   -
739 618 }
740 619  
741 620 // Collision handeling
... ... @@ -795,14 +674,12 @@ namespace collision
795 674 // If the dynamic object (obj1) is a sphere
796 675 if( d_sphere_1) {
797 676  
798   - sphereDynamicCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with same obj as last time
799   - sphereStaticCollision(d_sphere_1, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Can't with same obj as last time
  677 + dynamicCollision(d_sphere_1, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with same obj as last time
800 678  
801 679 // If sphere 1 collided with a dynamic sphere, check for that sphere's future collisions
802 680 if(d_sphere_2) {
803 681  
804   - sphereDynamicCollision(d_sphere_2, d_sphere_1, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with sphere 1
805   - sphereStaticCollision(d_sphere_2, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Placeholder variable for "Last object"
  682 + dynamicCollision(d_sphere_2, seconds_type(dt)); // Does it collide with any dynamic objects? Can't with sphere 1
806 683 }
807 684  
808 685 }
... ... @@ -844,10 +721,8 @@ namespace collision
844 721 const auto maxDt = seconds_type(dt);
845 722 const auto newDt = maxDt - sphereTime;
846 723 seconds_type returnTime = sphereTime;
847   -// auto still = 0; // Needed to print out the still value further down
848 724  
849 725 const auto ds = sphere->computeTrajectory(newDt); // Calculating "original ds"
850   -// std::cout << "The DS is: " << ds << std::endl;
851 726  
852 727 // Plane variables.
853 728 auto planes = getAttachedObjects(sphere);
... ... @@ -872,14 +747,12 @@ namespace collision
872 747  
873 748 planeContainer.insert(plane);
874 749 state = DynamicPSphere::States::Rolling;
875   -// std::cout << "detectStateChange says the state will become Rolling from Free" << std::endl;
876 750  
877 751 }
878 752 else if( std::abs(dn) < epsilon and bla < epsilon ) {
879 753  
880 754 planeContainer.insert(plane);
881 755 state = DynamicPSphere::States::AtRest;
882   -// std::cout << "detectStateChange says the state will become AtRest from Free" << std::endl;
883 756  
884 757 }
885 758 else state = DynamicPSphere::States::Free;
... ... @@ -909,7 +782,6 @@ namespace collision
909 782  
910 783 if( std::abs(dn) > epsilon and dsn > epsilon) {
911 784  
912   -// std::cout << "detectStateChange says the state will become Free from Rolling" << std::endl;
913 785 state = DynamicPSphere::States::Free;
914 786  
915 787 auto x = dn / dsn;
... ... @@ -919,7 +791,6 @@ namespace collision
919 791 }
920 792 else if( bla < epsilon ) {
921 793  
922   -// std::cout << "detectStateChange says the state will become AtRest from Rolling" << std::endl;
923 794 state = DynamicPSphere::States::AtRest;
924 795  
925 796 auto x = dn / dsn;
... ... @@ -934,65 +805,20 @@ namespace collision
934 805  
935 806 if( bla > epsilon ) {
936 807  
937   -// std::cout << "detectStateChange says the state will become Rolling from AtRest" << std::endl;
938 808 state = DynamicPSphere::States::Rolling;
939 809 return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
940 810 }
941 811 else if( dsn > epsilon) {
942   -// std::cout << "detectStateChange says the state will become Free from AtRest" << std::endl;
  812 +
943 813 state = DynamicPSphere::States::Free;
944 814 return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
945 815 }
  816 +
946 817 else return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::AtRest);
947 818 }
948 819 }
949 820 }
950 821  
951   -// void
952   -// MyController::testMethod()
953   -// {
954   -
955   -
956   -// auto sphere1 = _dynamic_spheres[0];
957   -// auto sphere2 = _dynamic_spheres[1];
958   -// StaticPPlane* plane;
959   -
960   -// auto fake_sin1 = StateChangeObj(sphere1, fakePlanes, seconds_type(0.2), DynamicPSphere::States::NoChange);
961   -// auto fake_col1 = CollisionObject( sphere2, plane, seconds_type(0.2));
962   -
963   -// _singularities.push_back(fake_sin1);
964   -
965   -// _collisions.push_back( fake_col1 );
966   -
967   -// crossUnique(_collisions, _singularities);
968   -
969   -// }
970   -
971   -
972   -
973   - /// vector; Get, Set and Remove objects to / from Sphere
974   - /*
975   - // Get objects attached to sphere
976   -// std::vector<StaticPPlane *>
977   -// MyController::getAttachedObjects(DynamicPSphere* sphere)
978   -// {
979   -// return (_map[sphere]);
980   -// }
981   -
982   -// // Set objects attached to sphere
983   -// void
984   -// MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
985   -// {
986   -// _map[sphere].push_back(plane);
987   -// }
988   -
989   -// // Remove objects from the set In the map
990   -// void
991   -// MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
992   -
993   -//// _map[sphere].erase(plane);
994   -// }
995   -*/
996 822  
997 823 /// unordered_set; Get, Set and Remove objects to / from Sphere
998 824 // Get objects attached to sphere
... ...
include/collision_library.h
... ... @@ -30,13 +30,14 @@ public:
30 30 void add (StaticPCylinder* const cylinder);
31 31 void add (StaticPBezierSurf* const surf);
32 32  
33   - // Change "value" and return type if the sphere should be able to be attached to objects other than planes
34   - std::unordered_set<StaticPPlane *> getAttachedObjects(DynamicPSphere* sphere);
35   -// std::vector<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
  33 + std::unordered_set<StaticPPlane *>
  34 + getAttachedObjects(DynamicPSphere* sphere);
36 35  
  36 + void
  37 + setAttachedObjects( std::unordered_set<StaticPPlane*> plane, DynamicPSphere* sphere );
37 38  
38   - void setAttachedObjects( std::unordered_set<StaticPPlane*> plane, DynamicPSphere* sphere );
39   - void detachObjects(DynamicPSphere* sphere);
  39 + void
  40 + detachObjects(DynamicPSphere* sphere);
40 41  
41 42 // States
42 43 void
... ... @@ -48,13 +49,11 @@ public:
48 49 void
49 50 handleStates (StateChangeObj& state, double dt);
50 51  
51   - void
52   - testMethod();
53   -
54 52 GMlib::Vector<float,3>
55 53 closestPoint(DynamicPSphere* S, seconds_type dt);
56 54  
57   - void handleCollision ( collision::CollisionObject& col, double dt);
  55 + void
  56 + handleCollision ( collision::CollisionObject& col, double dt);
58 57  
59 58 Environment _stillEnvironment;
60 59 DefaultEnvironment _env;
... ... @@ -63,11 +62,8 @@ public:
63 62  
64 63 private:
65 64  
66   - template <typename T_s, typename T_o>
67   - void sphereStaticCollision(T_s* sphere, T_o* object, seconds_type dt);
68   -
69   - template <typename T_s, typename T_o>
70   - void sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type dt);
  65 + template <typename T_s>
  66 + void dynamicCollision(T_s* sphere, seconds_type dt);
71 67  
72 68 template <class Container>
73 69 void sortAndMakeUniqueStates(Container& c);
... ... @@ -79,7 +75,6 @@ protected:
79 75 void localSimulate (double dt) override;
80 76  
81 77 std::vector<DynamicPSphere*> _dynamic_spheres;
82   -// std::vector<DynamicPCylinder*> _dynamic_cylinders;
83 78 std::vector<StaticPSphere*> _static_spheres;
84 79 std::vector<StaticPPlane*> _static_planes;
85 80 std::vector<StaticPCylinder*> _static_cylinders;
... ... @@ -87,13 +82,8 @@ protected:
87 82  
88 83 std::vector<collision::CollisionObject> _collisions;
89 84 std::vector<StateChangeObj> _singularities;
90   - std::vector<StateChangeObj> _fakeSingularities;
91 85  
92 86 std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>> _map;
93   - std::unordered_set<StaticPPlane*> fakePlanes;
94   -
95   -
96   -
97 87  
98 88 };
99 89  
... ... @@ -117,13 +107,14 @@ public:
117 107 GMlib::Vector<double,3>
118 108 adjustedTrajectory (seconds_type dt);
119 109  
120   - // "Old" methods
121   - void simulateToTInDt( seconds_type t ) override;
  110 + void
  111 + simulateToTInDt( seconds_type t ) override;
122 112  
123 113 GMlib::Vector<double, 3>
124 114 computeTrajectory (seconds_type dt) const override; // [m]
125 115  
126   - GMlib::Vector<double, 3> externalForces () const override; // [m / s^2]
  116 + GMlib::Vector<double, 3>
  117 + externalForces () const override; // [m / s^2]
127 118  
128 119  
129 120 };
... ... @@ -156,8 +147,8 @@ std::unique_ptr&lt;StaticPhysObject&lt;PSurf_T&gt;&gt; unittestStaticPhysObjectFactory(Argum
156 147 template <typename Container_1, typename Container_2>
157 148 void MyController::crossUnique(Container_1 ColContainer, Container_2 stateContainer) {
158 149  
159   - std::vector<collision::CollisionObject> _newCollisions;
160   - std::vector<collision::StateChangeObj> _newStateOjects;
  150 + std::vector<collision::CollisionObject> _newCollisions;
  151 + std::vector<collision::StateChangeObj> _newStateOjects;
161 152  
162 153 auto amIinCollision = [](Container_1 a, const auto& b){
163 154 for(auto& c : a) {
... ... @@ -289,105 +280,8 @@ void MyController::crossUnique(Container_1 ColContainer, Container_2 stateContai
289 280 }
290 281  
291 282 }
292   -
293   -
294 283 }
295 284  
296   -
297   -// Make collisions and states crosswise unique
298   -//template <typename Container_1, typename Container_2>
299   -//void MyController::crossUnique(Container_1 container1, Container_2 container2) {
300   -
301   -// auto objPred = [](const auto &a, const auto &b) {
302   -
303   -// if( a.obj1 == b.obj or a.obj2 == b.obj ) return true;
304   -
305   -// return false;
306   -// };
307   -
308   -// auto timePred = [](const auto &a, const auto &b) {
309   -
310   -// if( a.t_in_dt < b.time ) return true;
311   -
312   -// return false;
313   -// };
314   -
315   -// auto inCollisionsAlreadyPred = [](Container_1 a, const auto &b) {
316   -
317   -// for( auto& c : a) {
318   -
319   -// // Check if any of the collisions objects are in the collection already
320   -// if( c.obj1 == b.obj1 ) return true;
321   -// if( c.obj1 == b.obj2 ) return true;
322   -// if( c.obj2 == b.obj1 ) return true;
323   -// if( c.obj2 == b.obj2 ) return true;
324   -
325   -// return false;
326   -// }
327   -// };
328   -
329   -// auto inSingularitiesAlreadyPred = [](Container_2 a, const auto &b) {
330   -
331   -// for( auto& c : a) {
332   -
333   -// // Check if any of the state objects are in the collection already
334   -// if( c.obj == b.obj ) return true;
335   -
336   -// return false;
337   -// }
338   -// };
339   -
340   -// std::vector<collision::CollisionObject> _realCollisions;
341   -// std::vector<StateChangeObj> _realSingularities;
342   -
343   -// auto start = std::end(container2);
344   -
345   -// // Check if any object that is in CONTAINER1 is also in CONTAINER2 and compares time values to remove one
346   -// for( auto first_iter = std::end(container1) - 1; first_iter != std::begin(container1) -1; --first_iter) {
347   -// for( auto second_iter = start - 1; second_iter != std::begin(container2) -1; --second_iter ) {
348   -
349   -// if(( objPred( *first_iter, *second_iter ) )) {
350   -
351   -// if( timePred( *first_iter, *second_iter )) {
352   -
353   -// // Keep collision
354   -// if( inCollisionsAlreadyPred(_realCollisions, *first_iter) == false) {
355   -
356   -// _realCollisions.push_back(*first_iter);
357   -// }
358   -// }
359   -// else {
360   -
361   -// // Keep state
362   -// if( inSingularitiesAlreadyPred(_realSingularities, *second_iter) == false ) {
363   -
364   -// _realSingularities.push_back(*second_iter);
365   -// }
366   -// }
367   -// }
368   -//// else {
369   -//// break;
370   -//// }
371   -// }
372   -// }
373   -
374   -// if( !_realCollisions.empty() and !_realSingularities.empty() ) {
375   -
376   -// _collisions = _realCollisions;
377   -// _singularities = _realSingularities;
378   -// }
379   -// else if( !_realCollisions.empty() and _realSingularities.empty() ) {
380   -
381   -// _collisions = _realCollisions;
382   -// }
383   -// else if( _realCollisions.empty() and !_realSingularities.empty() ) {
384   -
385   -// _singularities = _realSingularities;
386   -// }
387   -
388   -
389   -//}
390   -
391 285 // Sort and make Unique for states
392 286 template <class Container>
393 287 void MyController::sortAndMakeUniqueStates(Container& c) {
... ... @@ -470,9 +364,9 @@ void sortAndMakeUnique( Container_T&amp; container) {
470 364 } // EOF
471 365  
472 366  
473   -template <typename T_s, typename T_o>
  367 +template <typename T_s>
474 368 inline
475   -void MyController::sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type dt) {
  369 +void MyController::dynamicCollision(T_s* dyn_obj, seconds_type dt) {
476 370  
477 371 bool movingObject = false;
478 372  
... ... @@ -485,138 +379,53 @@ void MyController::sphereDynamicCollision(T_s* sphere, T_o* object, seconds_type
485 379  
486 380 if( movingObject ) {
487 381  
488   -// else {
489   -
490   - // Sphere vs. Dynamic Sphere
491   - for( auto iter = std::begin(_dynamic_spheres); iter != std::end(_dynamic_spheres); ++iter) {
  382 + // Dynamic Object vs. Sphere
  383 + for( auto iter = std::begin(_dynamic_spheres); iter != std::end(_dynamic_spheres); ++iter) {
492 384  
493   -// if( (*iter == sphere) or (*iter == object) or (sphere->_state == DynamicPSphere::States::AtRest)) {
494   -// break;
495   -// }
496   - /*else*/ {
  385 + // Check only for collisions for the first dynamic sphere
  386 + auto col = detectCollision(*dyn_obj, **iter, dt);
497 387  
498   - // Check only for collisions for the first dynamic sphere
499   - auto col = detectCollision(*sphere, **iter, dt);
  388 + // The check for if the second object is a dynamic sphere is done in the main algorithm
  389 + // and the method is called again, only with the 1'st and 2'nd sphere swapped
500 390  
501   - // The check for if the second object is a dynamic sphere is done in the main algorithm
502   - // and the method is called again only with the 1'st and 2'nd sphere swapped
  391 + if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
503 392  
504   - if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
  393 + auto& first_sphere = dyn_obj;
  394 + auto& second_sphere = *iter;
505 395  
506   - auto& first_sphere = sphere;
507   - auto& second_sphere = *iter;
  396 + auto new_t = std::max(first_sphere->curr_t_in_dt, second_sphere->curr_t_in_dt);
508 397  
509   - auto new_t = std::max(first_sphere->curr_t_in_dt, second_sphere->curr_t_in_dt);
  398 + if( col.time > seconds_type(new_t) and col.time < dt) {
  399 + auto col_obj = CollisionObject(first_sphere, second_sphere, col.time);
  400 + _collisions.push_back(col_obj);
510 401  
511   - if( col.time > seconds_type(new_t) and col.time < dt) {
512   - auto col_obj = CollisionObject(first_sphere, second_sphere, col.time);
513   - _collisions.push_back(col_obj);
514 402  
515   - // Simulate the objects to T in dt?
516   -
517   - }
518   - }
519 403 }
520 404 }
521   -// }
522   -
523   - }
524   -
525   -
526   -}
527   -
528   -template <typename T_s, typename T_o>
529   -inline
530   -void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type dt)
531   -{
532   -
533   - bool movingObject = false;
534   -
535   - for( auto& s : _dynamic_spheres ) {
536   - if( s->_state != DynamicPSphere::States::AtRest ) {
537   - movingObject = true;
538   - break;
539 405 }
540   - }
541   -
542   - if( movingObject ) {
543   -
544   -
545   - // The game will mainly have two types of static objects that can be collided with, planes and bezier surfaces
546   - // Checks to see which one the sphere has collided with
547   - auto plane = dynamic_cast<const StaticPPlane*>(object);
548   - auto bezier_s = dynamic_cast<const StaticPBezierSurf*>(object);
549 406  
  407 + // Dynamic Object vs. Planes
  408 + for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
550 409  
551   - // Sphere vs. Static Plane
552   - if( plane ) {
  410 + if( dyn_obj->_state != DynamicPSphere::States::AtRest) {
553 411  
554   - auto attachedPlanes = getAttachedObjects(sphere);
  412 + auto col = detectCollision(*dyn_obj, **iter, dt);
555 413  
556   - for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
  414 + if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
557 415  
558   - // The sphere cannot collide with the same plane it just collided with
559   - // if( *iter == plane ) {
560   - // break;
561   - // }
  416 + auto new_t = dyn_obj->curr_t_in_dt;
562 417  
563   - // If sphere is attached to the plane, it should NOT check for collisions with it
564   -// if( !attachedPlanes.empty() ) {
565   -// for( auto comperator = attachedPlanes.begin(); comperator != attachedPlanes.end(); ++comperator) {
566   -
567   -// if( *iter == *comperator) break;
568   -// else {
569   -
570   -// // Else look for collision
571   -// auto col = collision::detectCollision(*sphere, **iter, dt);
572   -
573   -// if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
574   -
575   -// auto& static_object = *iter;
576   -
577   -// auto new_t = sphere->curr_t_in_dt;
578   -
579   -// if( col.time > seconds_type(new_t) and col.time < dt)
580   -// {
581   -// auto col_obj = CollisionObject(sphere, static_object, col.time);
582   -// _collisions.push_back(col_obj);
583   -
584   -// }
585   -// }
586   -// }
587   -// }
588   -// }
589   - /*else*/ if( sphere->_state != DynamicPSphere::States::AtRest) {
590   -
591   - auto col = detectCollision(*sphere, **iter, dt);
592   -
593   - if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
594   -
595   - auto& static_object = *iter;
596   -
597   - auto new_t = sphere->curr_t_in_dt;
598   -
599   - if( col.time > seconds_type(new_t) and col.time < dt)
600   - {
601   - auto col_obj = CollisionObject(sphere, static_object, col.time);
602   - _collisions.push_back(col_obj);
603   -
604   - }
  418 + if( col.time > seconds_type(new_t) and col.time < dt)
  419 + {
  420 + auto col_obj = CollisionObject(dyn_obj, *iter, col.time);
  421 + _collisions.push_back(col_obj);
605 422 }
606 423 }
607 424 }
608 425 }
609   -
610   - // Same as above, only for bezier surfaces
611   - else if( bezier_s) {
612   -
613   - }
614 426 }
615   -
616   -
617 427 }
618 428  
619   -
620 429 } // END namespace collision
621 430  
622 431  
... ...