Commit a4446351c38fedbd29454d66a782042f9b2da0b2

Authored by Bjørn-Richard Pedersen
1 parent 540a54dc

Closest point and adjusted trajectory works, system seems stable. Looks like the…

…re is still a small inner loop issue. Will try to fix before delivery
Showing 2 changed files with 451 additions and 346 deletions   Show diff stats
include/collision_library.cpp
... ... @@ -9,8 +9,8 @@ namespace collision
9 9  
10 10  
11 11 CollisionState
12   - detectCollision (const DynamicPhysObject<GMlib::PSphere<float>>& S0,
13   - const DynamicPhysObject<GMlib::PSphere<float>>& S1,
  12 + detectCollision (DynamicPhysObject<GMlib::PSphere<float>>& S0,
  13 + DynamicPhysObject<GMlib::PSphere<float>>& S1,
14 14 seconds_type dt)
15 15 {
16 16 const auto dt_max = dt;
... ... @@ -29,14 +29,14 @@ namespace collision
29 29  
30 30 // If the sphere's state is Rolling, it should use an adjusted DS
31 31 if( S1._state == DynamicPSphere::States::Rolling ) {
32   - auto unconst_S1 = const_cast<DynamicPhysObject<GMlib::PSphere<float>>&>(S1);
33   - r1 = unconst_S1.adjustedTrajectory(new_dt);
  32 +// auto unconst_S1 = const_cast<DynamicPhysObject<GMlib::PSphere<float>>&>(S1);
  33 + r1 = S1.adjustedTrajectory(new_dt);
34 34 }
35 35  
36 36 // If the sphere's state is Rolling, it should use an adjusted DS
37 37 if( S0._state == DynamicPSphere::States::Rolling ) {
38   - auto unconst_S0 = const_cast<DynamicPhysObject<GMlib::PSphere<float>>&>(S0);
39   - r2 = unconst_S0.adjustedTrajectory(new_dt);
  38 +// auto unconst_S0 = const_cast<DynamicPhysObject<GMlib::PSphere<float>>&>(S0);
  39 + r2 = S0.adjustedTrajectory(new_dt);
40 40 }
41 41  
42 42  
... ... @@ -109,7 +109,7 @@ namespace collision
109 109 }
110 110  
111 111 CollisionState
112   - detectCollision (const DynamicPhysObject<GMlib::PSphere<float>>& S,
  112 + detectCollision (DynamicPhysObject<GMlib::PSphere<float>>& S,
113 113 const StaticPhysObject<GMlib::PPlane<float>>& P,
114 114 seconds_type dt)
115 115 {
... ... @@ -136,14 +136,14 @@ namespace collision
136 136  
137 137 // If the sphere's state is Rolling, it should use an adjusted DS
138 138 if( S._state == DynamicPSphere::States::Rolling ) {
139   - auto unconst_S = const_cast<DynamicPhysObject<GMlib::PSphere<float>>&>(S);
140   - ds = unconst_S.adjustedTrajectory(new_dt);
  139 +// auto unconst_S = const_cast<DynamicPhysObject<GMlib::PSphere<float>>&>(S);
  140 + ds = S.adjustedTrajectory(new_dt);
141 141 }
142 142  
143 143 const auto Q = (d * n_normal);
144 144 const auto R = ( ds * n_normal ); // S.computeTrajectory(dt)
145 145  
146   - const auto epsilon = 0.00001;
  146 + const auto epsilon = 1e-7;
147 147  
148 148 if( std::abs(Q) < epsilon )
149 149 {
... ... @@ -241,61 +241,70 @@ namespace collision
241 241  
242 242 }
243 243  
244   - GMlib::Point<float,2>
245   - MyController::closestPoint(DynamicPSphere& S, StaticPPlane& P)
  244 + GMlib::Vector<float,3>
  245 + MyController::closestPoint(DynamicPSphere* sphere, seconds_type dt)
246 246 {
247   - auto &_p= P;
248   - const auto S0_pos = S.getMatrixToScene() * S.getPos();
249   - //const auto epsilon = 1e-5;
250   -
251   - float u, v;
252   - u = 0.5;
253   - v = 0.5;
254   -
255   - for( int i=0 ; i< 10 ; i++)
256   - {
257   -
258   - const auto plane_pos = _p.evaluateParent(u,v,1,1);
259   - const auto su = plane_pos(1)(0) ;
260   - const auto sv = plane_pos(0)(1) ;
261   - const auto q = plane_pos(0)(0);
262   - const auto d = q - S0_pos;
263   - const auto suu = plane_pos(2)(0);
264   - const auto suv = plane_pos (1)(1);
265   - const auto svv = plane_pos(0)(2);
266   -
267   - GMlib::SqMatrix<float,2> aMatrix;
268   - const auto dsuu = d*suu + su*su;
269   - const auto dsvu = d*suv + su*sv;
270   - const auto dsuv = d*suv + su*sv;
271   - const auto dsvv = d*svv + sv*sv;
272   -
273   -
274   - aMatrix[0][0] = dsuu;
275   - aMatrix[0][1] = dsuv;
276   - aMatrix[1][0] = dsvu;
277   - aMatrix[1][1] = dsvv;
278   - aMatrix.invert();
279   -
280   - GMlib::Vector<float,2> b;
281   - const auto dsu = d*su;
282   - const auto dsv = d*sv;
283   - b[0] = -dsu;
284   - b[1]= -dsv;
285   -
286   - const auto x = aMatrix * b;
287   -
288   - const auto& delta_u = x(0);
289   - u += delta_u;
290   - const auto& delta_v = x(1);
291   - v += delta_v;
292   - }
293   -
294   - GMlib::Point<float,2> closest (u,v);
295   - return closest;
296   -
297   -
298   - }
  247 + //going to return q-p to adjustTrajectory method
  248 +
  249 + auto max_dt = dt;
  250 + auto min_dt = sphere->curr_t_in_dt;
  251 + auto new_dt = max_dt -min_dt;
  252 + float u = 0.4;
  253 + float v = 0.4;
  254 + float delta_u = 0.5;
  255 + float delta_v = 0.5;
  256 + auto s = sphere->getMatrixToScene() * sphere->getPos();
  257 + auto r = sphere->getRadius();
  258 + GMlib::Vector<double, 3> ds = sphere->computeTrajectory(new_dt);
  259 + auto p = s+ds;
  260 + GMlib::SqMatrix<float,2> A;
  261 + GMlib::Vector<float, 2> b;
  262 + GMlib::Vector<float,3> d{0.0f,0.0f,0.0f};
  263 +
  264 +
  265 + auto planes = _map[sphere];
  266 +
  267 +
  268 + //iteration
  269 +
  270 + for ( int i=0; i<10;i++/* delta_u > epsilon && delta_v > epsilon*/){
  271 + GMlib::Vector <float,3>Sn {0.0f,0.0f,0.0f};
  272 + GMlib::Vector <float,3>Su {0.0f,0.0f,0.0f};
  273 + GMlib::Vector <float,3>Sv {0.0f,0.0f,0.0f};
  274 + GMlib::Vector <float,3>Suu {0.0f,0.0f,0.0f};
  275 + GMlib::Vector <float,3>Svv {0.0f,0.0f,0.0f};
  276 + GMlib::Vector <float,3>Suv {0.0f,0.0f,0.0f};
  277 + GMlib::APoint <float,3>q;
  278 +
  279 + for(auto it = planes.begin(); it != planes.end(); it++){
  280 + GMlib::Vector<float,3> normal{0.0f,0.0f,0.0f};
  281 + 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);
  285 + Suu += M(2)(0);
  286 + Svv += M(0)(2);
  287 + Suv += M(1)(1);
  288 + }
  289 + d =(q-p);
  290 + A[0][0] = d* Suu + Su * Su;
  291 + A[0][1] = d* Suv + Su * Sv;
  292 + A[1][0] = d* Suv + Su * Sv;
  293 + A[1][1] = d* Svv + Sv * Sv;
  294 + GMlib::SqMatrix<float,2> A_inv = A;
  295 + A_inv.invert();
  296 + b[0] = - d * Su;
  297 + b[1] = - d * Sv;
  298 + GMlib::APoint<float, 3> X = A_inv*b;
  299 + delta_u = X(0);
  300 + delta_v = X(1);
  301 + u += delta_u;
  302 + v += delta_v;
  303 +
  304 +
  305 + }
  306 + return d;
  307 + }
299 308  
300 309 void
301 310 computeImpactResponse (DynamicPSphere& S, const StaticPBezierSurf& B,
... ... @@ -314,16 +323,16 @@ namespace collision
314 323  
315 324 }
316 325  
317   - CollisionState
318   - detectCollision(const DynamicPSphere &S, const DynamicPCylinder &C, seconds_type dt) {
  326 +// CollisionState
  327 +// detectCollision(const DynamicPSphere &S, const DynamicPCylinder &C, seconds_type dt) {
319 328  
320   - }
  329 +// }
321 330  
322   - void
323   - computeImpactResponse (DynamicPSphere& S, DynamicPCylinder& C,
324   - seconds_type dt) {
  331 +// void
  332 +// computeImpactResponse (DynamicPSphere& S, DynamicPCylinder& C,
  333 +// seconds_type dt) {
325 334  
326   - }
  335 +// }
327 336  
328 337 CollisionState
329 338 detectCollision (const DynamicPSphere& S, const StaticPCylinder& C, seconds_type dt) {
... ... @@ -334,28 +343,28 @@ namespace collision
334 343 computeImpactResponse (DynamicPSphere& S, const StaticPCylinder& C,
335 344 seconds_type dt);
336 345  
337   - CollisionState
338   - detectCollision (const DynamicPCylinder& C0, const DynamicPCylinder& C, seconds_type dt);
  346 +// CollisionState
  347 +// detectCollision (const DynamicPCylinder& C0, const DynamicPCylinder& C, seconds_type dt);
339 348  
340   - void computeImpactResponse (DynamicPCylinder& C0, DynamicPCylinder& C1,
341   - seconds_type dt) {
  349 +// void computeImpactResponse (DynamicPCylinder& C0, DynamicPCylinder& C1,
  350 +// seconds_type dt) {
342 351  
343   - }
  352 +// }
344 353  
345   - void computeImpactResponse (DynamicPCylinder& C, const StaticPSphere& S,
346   - seconds_type dt){
  354 +// void computeImpactResponse (DynamicPCylinder& C, const StaticPSphere& S,
  355 +// seconds_type dt){
347 356  
348   - }
  357 +// }
349 358  
350   - void computeImpactResponse (DynamicPCylinder& C, const StaticPBezierSurf& B,
351   - seconds_type dt){
  359 +// void computeImpactResponse (DynamicPCylinder& C, const StaticPBezierSurf& B,
  360 +// seconds_type dt){
352 361  
353   - }
  362 +// }
354 363  
355   - void computeImpactResponse (DynamicPCylinder& C, const StaticPPlane& P,
356   - seconds_type dt){
  364 +// void computeImpactResponse (DynamicPCylinder& C, const StaticPPlane& P,
  365 +// seconds_type dt){
357 366  
358   - }
  367 +// }
359 368  
360 369  
361 370 std::unique_ptr<Controller>
... ... @@ -367,57 +376,65 @@ namespace collision
367 376 void
368 377 DynamicPhysObject<GMlib::PSphere<float> >::simulateToTInDt(seconds_type t){
369 378  
370   - std::ofstream myFile;
  379 +// std::ofstream myFile;
371 380  
372   - myFile.open("SimulateLog.txt", std::ios::app );
  381 +// myFile.open("SimulateLog.txt", std::ios::app );
373 382  
374   - myFile << this << "\t" << t.count() << "\t\t"
375   - << int(this->_state) << "\t\t"
376   - << "(" << this->getPos() << ")" << "\t\t";
  383 +// myFile << this << "\t" << t.count() << "\t\t"
  384 +// << int(this->_state) << "\t\t"
  385 +// << "(" << this->getPos() << ")" << "\t\t";
377 386  
378   - if( this->_state == DynamicPSphere::States::AtRest
379   - or (this->_state == DynamicPSphere::States::Rolling and std::abs(this->velocity(2) <= 1e-2))) {
  387 + GMlib::Vector<float,3> stopVector = {1e-2, 1e-2, 1e-2};
380 388  
381   - this->curr_t_in_dt = t;
382   - this->velocity = {0.0f, 0.0f, 0.0f};
383   - this->environment = &this->_sphereController->_stillEnvironment;
  389 + if( this->_state == DynamicPSphere::States::AtRest or (this->velocity) < stopVector ) {
384 390  
  391 + this->velocity = {0.0f, 0.0f, 0.0f };
  392 + this->environment = &_sphereController->_stillEnvironment;
385 393 }
386   - else {
  394 + else if( this->_state == DynamicPSphere::States::Rolling) {
387 395  
388   - auto dt = t - this->curr_t_in_dt;
389   - auto MI = this->getMatrixToSceneInverse();
390   - GMlib::Vector<double,3> ds {0.0f, 0.0f, 0.0f};
  396 + auto dt0 = seconds_type(t - this->curr_t_in_dt);
  397 + auto Mi = this->getMatrixToSceneInverse();
  398 + //move
391 399  
392   - if( this->_state == DynamicPSphere::States::Rolling ) {
  400 + GMlib::Vector<double,3> ds = (0.0f,0.0f,0.0f);
393 401  
394   - ds = adjustedTrajectory(dt);
395   - this->environment = &this->_sphereController->_env;
  402 + ds = adjustedTrajectory(dt0);
396 403  
397   - }
398   - if( this->_state == DynamicPSphere::States::Free ) {
  404 + this->translateParent(Mi*ds);
  405 + this->curr_t_in_dt =t;
  406 + //update physics
399 407  
400   - ds = this->computeTrajectory(dt);
401   - this->environment = &this->_sphereController->_env;
402   - }
  408 + auto F = this->externalForces();
  409 + auto c = dt0.count();
  410 + auto a = F * c;
  411 + this->velocity -= a;
  412 +
  413 + }
  414 + else {
  415 +
  416 + auto dt0 = seconds_type(t - this->curr_t_in_dt);
  417 + auto Mi = this->getMatrixToSceneInverse();
  418 + //move
  419 +
  420 + GMlib::Vector<double,3> ds = (0.0f,0.0f,0.0f);
403 421  
404   - // Move
405   - this->translateParent( MI * ds);
406   - this->curr_t_in_dt = t;
  422 + ds = computeTrajectory(dt0);
  423 +
  424 + this->translateParent(Mi*ds);
  425 + this->curr_t_in_dt =t;
  426 + //update physics
407 427  
408   - // Update physics
409 428 auto F = this->externalForces();
410   - auto c = dt.count();
  429 + auto c = dt0.count();
411 430 auto a = F*c;
412 431 this->velocity += a;
  432 + }
413 433  
414   - this->environment = &this->_sphereController->_env;
415   -
416   - myFile << "(" << this->getPos() << ")" << "\t\t[" << ds << "]\t\t" << int(this->_state) << std::endl;
  434 +// myFile << "(" << this->getPos() << ")" << "\t\t[" << ds << "]\t\t" << int(this->_state) << std::endl;
417 435  
418   - myFile.close();
  436 +// myFile.close();
419 437  
420   - }
421 438  
422 439 }
423 440  
... ... @@ -457,16 +474,15 @@ namespace collision
457 474 const auto v = M(0)(1);
458 475 auto normal = GMlib::Vector<float,3> (u ^ v);
459 476 n += normal;
460   - q = _sphereController->closestPoint(*this, *plane);
461 477 }
462 478  
463 479 n = GMlib::Vector<float,3> (n / planes.size()).getNormalized();
  480 + auto closest = this->_sphereController->closestPoint(this,dt);
  481 + auto d = n*r + closest;
  482 +
  483 + auto adjusted_ds = ds + d;
464 484  
465   -// auto adjusted_ds = ds + (r + (p * s) )* n;
466   - auto adjusted_ds = ds + (q - (s + ds)) + (n * r);
467 485  
468   - // Ghada's adjusted ds
469   -// auto adjusted_ds = ds - (ds * n * n);
470 486  
471 487 return adjusted_ds;
472 488  
... ... @@ -562,16 +578,22 @@ namespace collision
562 578 handleStates(s, dt);
563 579  
564 580 // Collision detection algorithm
565   - for( auto& sphere : _dynamic_spheres) {
566   -
567   - // Sending in sphere twice in the initial call because the function will require a second object in future calls
568   - sphereDynamicCollision(sphere, sphere, seconds_type(dt));
569 581  
570   - for( auto& plane : _static_planes ) {
  582 + sphereDynamicCollision(s.obj, s.obj, seconds_type(dt));
571 583  
572   - sphereStaticCollision(sphere, plane, seconds_type(dt));
573   - }
  584 + for( auto& plane : s.attachedPlanes ) {
  585 + sphereStaticCollision(s.obj, plane, seconds_type(dt));
574 586 }
  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 +// }
575 597  
576 598 detectStateChanges(dt);
577 599  
... ... @@ -624,16 +646,21 @@ namespace collision
624 646 handleStates(s, dt);
625 647  
626 648 // Collision detection algorithm
627   - for( auto& sphere : _dynamic_spheres) {
  649 + sphereDynamicCollision(s.obj, s.obj, seconds_type(dt));
  650 +
  651 + for( auto& plane : s.attachedPlanes ) {
  652 + sphereStaticCollision(s.obj, plane, seconds_type(dt));
  653 + }
  654 +// for( auto& sphere : _dynamic_spheres) {
628 655  
629   - // Sending in sphere twice in the initial call because the function will require a second object in future calls
630   - sphereDynamicCollision(sphere, sphere, seconds_type(dt));
  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));
631 658  
632   - for( auto& plane : _static_planes ) {
  659 +// for( auto& plane : _static_planes ) {
633 660  
634   - sphereStaticCollision(sphere, plane, seconds_type(dt));
635   - }
636   - }
  661 +// sphereStaticCollision(sphere, plane, seconds_type(dt));
  662 +// }
  663 +// }
637 664  
638 665 detectStateChanges(dt);
639 666  
... ... @@ -667,15 +694,15 @@ namespace collision
667 694  
668 695 auto sphere = state.obj;
669 696 auto newState = state.state;
670   - auto time = state.time;
  697 + auto Statetime = state.time;
671 698 auto planes = state.attachedPlanes;
672 699  
673   - std::ofstream myFile;
  700 +// std::ofstream myFile;
674 701  
675   - myFile.open("SingularityLog.txt", std::ios::app );
  702 +// myFile.open("SingularityLog.txt", std::ios::app );
676 703  
677   - myFile << sphere << "\t\t" << time.count() << "\t\t\t"
678   - << int(sphere->_state) << "\t\t\t\t";
  704 +// myFile << sphere << "\t\t" << time.count() << "\t\t\t"
  705 +// << int(sphere->_state) << "\t\t\t\t";
679 706  
680 707 GMlib::Vector<double,3> ds {0.0f, 0.0f, 0.0f};
681 708  
... ... @@ -685,36 +712,40 @@ namespace collision
685 712  
686 713 detachObjects(sphere);
687 714 sphere->_state = newState;
  715 + sphere->simulateToTInDt(seconds_type(dt));
688 716 }
689 717 else {
690 718  
691 719 setAttachedObjects(planes, sphere);
692 720 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));
693 730 }
694 731  
695   - myFile << int(newState) << std::endl;
696 732  
697   - myFile.close();
698 733  
699   - sphere->simulateToTInDt(time);
  734 +// myFile << int(newState) << std::endl;
  735 +
  736 +// myFile.close();
  737 +
  738 +
700 739 }
701 740  
702 741 // Collision handeling
703 742 void
704 743 MyController::handleCollision(CollisionObject &c, double dt) {
705 744  
706   -// c.obj1->simulateToTInDt(c.t_in_dt);
707   -// c.obj2->simulateToTInDt(c.t_in_dt);
708   -
709 745 // Add more objects here if you end up using more
710 746 auto d_sphere_1 = dynamic_cast<DynamicPSphere*>(c.obj1);
711 747 auto d_sphere_2 = dynamic_cast<DynamicPSphere*>(c.obj2);
712   - auto s_sphere_2 = dynamic_cast<StaticPSphere*>(c.obj2);
713   - auto d_cylinder_1 = dynamic_cast<DynamicPCylinder*>(c.obj1);
714   - auto d_cylinder_2 = dynamic_cast<DynamicPCylinder*>(c.obj2);
715   - auto s_cylinder_2 = dynamic_cast<StaticPCylinder*>(c.obj2);
716 748 auto s_plane_2 = dynamic_cast<StaticPPlane*>(c.obj2);
717   - auto s_bezier_2 = dynamic_cast<StaticPBezierSurf*>(c.obj2);
718 749  
719 750  
720 751  
... ... @@ -723,14 +754,29 @@ namespace collision
723 754 if(d_sphere_1) {
724 755  
725 756  
726   - if (d_sphere_2)
727   - collision::computeImpactResponse( *d_sphere_1, *d_sphere_2, c.t_in_dt); // D_Sphere vs. D_Sphere
728   - else if (d_sphere_1 && s_sphere_2)
729   - collision::computeImpactResponse( *d_sphere_1, *s_sphere_2, c.t_in_dt); // D_Sphere vs. S_Sphere
730   - else if (d_sphere_1 && d_cylinder_2)
731   - collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. D_Cylinder
732   - else if (d_sphere_1 && s_cylinder_2)
733   - collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Sphere vs. S_Cylinder
  757 + if (d_sphere_2) {
  758 +
  759 + if( d_sphere_2->_state == DynamicPSphere::States::AtRest) {
  760 +
  761 + d_sphere_1->simulateToTInDt(c.t_in_dt);
  762 +
  763 + d_sphere_2->curr_t_in_dt = d_sphere_1->curr_t_in_dt;
  764 + d_sphere_2->environment = &_env;
  765 + d_sphere_2->_state = DynamicPSphere::States::Rolling;
  766 +
  767 +
  768 + collision::computeImpactResponse( *d_sphere_1, *d_sphere_2, c.t_in_dt); // D_Sphere vs. D_Sphere
  769 + }
  770 + else {
  771 +
  772 + d_sphere_1->simulateToTInDt(c.t_in_dt);
  773 + d_sphere_2->simulateToTInDt(c.t_in_dt);
  774 +
  775 + collision::computeImpactResponse( *d_sphere_1, *d_sphere_2, c.t_in_dt); // D_Sphere vs. D_Sphere
  776 +
  777 + }
  778 +
  779 + }
734 780 else if (d_sphere_1 && s_plane_2) {
735 781  
736 782 if(d_sphere_1->_state != DynamicPSphere::States::AtRest)
... ... @@ -738,28 +784,8 @@ namespace collision
738 784 d_sphere_1->simulateToTInDt(c.t_in_dt);
739 785 }
740 786 collision::computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
741   -
742 787 }
743   - else if (d_sphere_1 && s_bezier_2)
744   - collision::computeImpactResponse( *d_sphere_1, *s_bezier_2, c.t_in_dt); // D_Sphere vs. S_Bezier
745   -
746   - }
747 788  
748   - // First object is cylinder
749   - if(d_cylinder_1) {
750   -
751   - if (d_cylinder_2)
752   - collision::computeImpactResponse( *d_cylinder_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. D_Cylinder
753   - else if (d_cylinder_1 && s_cylinder_2)
754   - collision::computeImpactResponse( *d_sphere_1, *d_cylinder_2, c.t_in_dt); // D_Cylinder vs. S_Cylinder
755   - else if (d_cylinder_1 && d_sphere_2)
756   - collision::computeImpactResponse( *d_sphere_2, *d_cylinder_1, c.t_in_dt); // D_Cylinder vs. D_Sphere
757   - else if (d_cylinder_1 && s_sphere_2)
758   - collision::computeImpactResponse( *d_cylinder_1, *s_sphere_2, c.t_in_dt); // D_Cylinder vs. S_Sphere
759   - else if (d_cylinder_1 && s_bezier_2)
760   - collision::computeImpactResponse( *d_cylinder_1, *s_bezier_2, c.t_in_dt); // D_Cylinder vs. S_Bezier
761   - else if (d_cylinder_1 && s_plane_2)
762   - collision::computeImpactResponse( *d_cylinder_1, *s_plane_2, c.t_in_dt); // D_Cylinder vs. S_Plane
763 789  
764 790 }
765 791  
... ... @@ -779,31 +805,6 @@ namespace collision
779 805 sphereStaticCollision(d_sphere_2, c.obj2, seconds_type(dt)); // Does it collide with any static objects? Placeholder variable for "Last object"
780 806 }
781 807  
782   - // Same if object 2 is a dynamic cylinder
783   - if(d_cylinder_2) {
784   -
785   - //cylinderDynamicCollision(d_cylinder_2, d_sphere_1, seconds_type(dt));
786   - //cylinderStaticCollision(d_cylinder_2, c.obj2, seconds_type(dt));
787   - }
788   - }
789   -
790   -// If the dynamic object (obj1) is a cylinder
791   - if(d_cylinder_1) {
792   -
793   - //cylinderDynamicCollision(d_cylinder_1, c.obj2, seconds_type(dt));
794   - //cylinderStaticCollision(d_cylinder_1, c.obj2, seconds_type(dt));
795   -
796   - if(d_cylinder_2) {
797   -
798   - //cylinderDynamicCollision(d_cylinder_2, d_cylinder_1, seconds_type(dt));
799   - //cylinderStaticCollision(d_cylinder_2, c.obj2, seconds_type(dt));
800   - }
801   -
802   - if(d_sphere_2) {
803   -
804   - sphereDynamicCollision(d_sphere_2, d_sphere_1, seconds_type(dt));
805   - sphereStaticCollision(d_sphere_2, c.obj2, seconds_type(dt));
806   - }
807 808 }
808 809 }
809 810  
... ... @@ -867,23 +868,19 @@ namespace collision
867 868 auto dsn = ds * n;
868 869 auto dn = d*n;
869 870  
870   - auto x = dn / dsn;
871   - if( x == 0) x = sphereTime.count();
872   -
873   - returnTime = (x * newDt) + sphereTime;
874   -
875   -
876   - if( std::abs(dn) < epsilon and dsn <= 0 ) {
  871 + if( std::abs(dn) < epsilon and dsn <= epsilon ) {
877 872  
878 873 planeContainer.insert(plane);
879 874 state = DynamicPSphere::States::Rolling;
880   - std::cout << "detectStateChange says the state will become Rolling from Free" << std::endl;
  875 +// std::cout << "detectStateChange says the state will become Rolling from Free" << std::endl;
  876 +
881 877 }
882 878 else if( std::abs(dn) < epsilon and bla < epsilon ) {
883 879  
884 880 planeContainer.insert(plane);
885 881 state = DynamicPSphere::States::AtRest;
886   - std::cout << "detectStateChange says the state will become AtRest from Free" << std::endl;
  882 +// std::cout << "detectStateChange says the state will become AtRest from Free" << std::endl;
  883 +
887 884 }
888 885 else state = DynamicPSphere::States::Free;
889 886 }
... ... @@ -908,23 +905,26 @@ namespace collision
908 905 auto dsn = ds * n;
909 906 auto dn = d * n;
910 907  
911   -
912   - auto x = dn / dsn;
913   - if( x == 0) x = sphereTime.count();
914   -
915   -
916 908 if( sphere->_state == DynamicPSphere::States::Rolling ) {
917 909  
918   - if( std::abs(dn) > epsilon and dsn > 0) {
  910 + if( std::abs(dn) > epsilon and dsn > epsilon) {
919 911  
920   - std::cout << "detectStateChange says the state will become Free from Rolling" << std::endl;
  912 +// std::cout << "detectStateChange says the state will become Free from Rolling" << std::endl;
921 913 state = DynamicPSphere::States::Free;
  914 +
  915 + auto x = dn / dsn;
  916 + returnTime = (x * newDt) + sphereTime;
  917 +
922 918 return StateChangeObj(sphere, planes, returnTime, state);
923 919 }
924 920 else if( bla < epsilon ) {
925 921  
926   - std::cout << "detectStateChange says the state will become AtRest from Rolling" << std::endl;
  922 +// std::cout << "detectStateChange says the state will become AtRest from Rolling" << std::endl;
927 923 state = DynamicPSphere::States::AtRest;
  924 +
  925 + auto x = dn / dsn;
  926 + returnTime = (x * newDt) + sphereTime;
  927 +
928 928 return StateChangeObj(sphere, planes, returnTime, state);
929 929 }
930 930 else return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
... ... @@ -934,12 +934,12 @@ namespace collision
934 934  
935 935 if( bla > epsilon ) {
936 936  
937   - std::cout << "detectStateChange says the state will become Rolling from AtRest" << std::endl;
  937 +// std::cout << "detectStateChange says the state will become Rolling from AtRest" << std::endl;
938 938 state = DynamicPSphere::States::Rolling;
939 939 return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
940 940 }
941   - else if( dsn > 0) {
942   - std::cout << "detectStateChange says the state will become Free from AtRest" << std::endl;
  941 + else if( dsn > epsilon) {
  942 +// std::cout << "detectStateChange says the state will become Free from AtRest" << std::endl;
943 943 state = DynamicPSphere::States::Free;
944 944 return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::Rolling);
945 945 }
... ... @@ -1060,4 +1060,3 @@ namespace collision
1060 1060  
1061 1061 } // END namespace collision
1062 1062  
1063   -
... ...
include/collision_library.h
... ... @@ -51,31 +51,8 @@ public:
51 51 void
52 52 testMethod();
53 53  
54   - GMlib::Point<float,2>
55   - closestPoint(DynamicPSphere& S, StaticPPlane& P);
56   -
57   -
58   - // Collisions
59   - CollisionState detectCollision (const DynamicPSphere& S,
60   - const DynamicPCylinder& C, seconds_type dt);
61   -
62   - CollisionState detectCollision (const DynamicPCylinder& C0,
63   - const DynamicPCylinder& C, seconds_type dt);
64   -
65   - void computeImpactResponse (DynamicPSphere& S, DynamicPCylinder& C,
66   - seconds_type dt);
67   -
68   - void computeImpactResponse (DynamicPCylinder& C0, DynamicPCylinder& C1,
69   - seconds_type dt);
70   -
71   - void computeImpactResponse (DynamicPCylinder& C, const StaticPSphere& S,
72   - seconds_type dt);
73   -
74   - void computeImpactResponse (DynamicPCylinder& C, const StaticPBezierSurf& B,
75   - seconds_type dt);
76   -
77   - void computeImpactResponse (DynamicPCylinder& C, const StaticPPlane& P,
78   - seconds_type dt);
  54 + GMlib::Vector<float,3>
  55 + closestPoint(DynamicPSphere* S, seconds_type dt);
79 56  
80 57 void handleCollision ( collision::CollisionObject& col, double dt);
81 58  
... ... @@ -102,7 +79,7 @@ protected:
102 79 void localSimulate (double dt) override;
103 80  
104 81 std::vector<DynamicPSphere*> _dynamic_spheres;
105   - std::vector<DynamicPCylinder*> _dynamic_cylinders;
  82 +// std::vector<DynamicPCylinder*> _dynamic_cylinders;
106 83 std::vector<StaticPSphere*> _static_spheres;
107 84 std::vector<StaticPPlane*> _static_planes;
108 85 std::vector<StaticPCylinder*> _static_cylinders;
... ... @@ -176,100 +153,241 @@ std::unique_ptr&lt;StaticPhysObject&lt;PSurf_T&gt;&gt; unittestStaticPhysObjectFactory(Argum
176 153 return std::make_unique<StaticPhysObject<PSurf_T>> (parameters...);
177 154 }
178 155  
179   -// Make collisions and states crosswise unique
180 156 template <typename Container_1, typename Container_2>
181   -void MyController::crossUnique(Container_1 container1, Container_2 container2) {
  157 +void MyController::crossUnique(Container_1 ColContainer, Container_2 stateContainer) {
182 158  
183   - auto objPred = [](const auto &a, const auto &b) {
  159 + std::vector<collision::CollisionObject> _newCollisions;
  160 + std::vector<collision::StateChangeObj> _newStateOjects;
184 161  
185   - if( a.obj1 == b.obj or a.obj2 == b.obj ) return true;
  162 + auto amIinCollision = [](Container_1 a, const auto& b){
  163 + for(auto& c : a) {
  164 + if(c.obj1==b.obj1) return true;
  165 + if(c.obj1==b.obj2) return true;
  166 + if(c.obj2==b.obj1) return true;
  167 + if(c.obj2==b.obj2) return true;
  168 + return false;
  169 + }
186 170  
187   - return false;
188 171 };
189 172  
190   - auto timePred = [](const auto &a, const auto &b) {
191   -
192   - if( a.t_in_dt < b.time ) return true;
  173 + auto amIinState = [](Container_2 a, const auto& b){
  174 + for(auto& d:a){
  175 + if(d.obj==b.obj) return true;
  176 + return false;
  177 + }
  178 + };
193 179  
  180 + auto objPred = [](const auto& a, const auto& b) {
  181 + if(a.obj1 == b.obj or a.obj2 == b.obj ) return true;
194 182 return false;
195 183 };
196 184  
197   - auto inCollisionsAlreadyPred = [](Container_1 a, const auto &b) {
  185 + auto timePred = [](const auto&a, const auto&b) {
  186 + if(a.t_in_dt < b.time ) return true;
  187 + return false;
  188 + };
198 189  
199   - for( auto& c : a) {
  190 + bool colBigger;
200 191  
201   - // Check if any of the collisions objects are in the collection already
202   - if( c.obj1 == b.obj1 ) return true;
203   - if( c.obj1 == b.obj2 ) return true;
204   - if( c.obj2 == b.obj1 ) return true;
205   - if( c.obj2 == b.obj2 ) return true;
  192 + if( ColContainer.size() > stateContainer.size() ) {
206 193  
207   - return false;
208   - }
209   - };
  194 + colBigger = true;
210 195  
211   - auto inSingularitiesAlreadyPred = [](Container_2 a, const auto &b) {
  196 + }
  197 + else colBigger = false;
  198 +
  199 + if( colBigger == true ) {
  200 +
  201 + bool placed = false;
  202 +
  203 + for(auto firstIter = std::end(ColContainer) - 1; firstIter!= std::begin(ColContainer) - 1;--firstIter)
  204 + {
  205 + for(auto secondIter = std::end(stateContainer) - 1; secondIter!= std::begin(stateContainer) - 1;--secondIter)
  206 + {
  207 + placed = false;
  208 +
  209 + //Check for same Objects
  210 + if(objPred(*firstIter,*secondIter))
  211 + {
  212 + //check for time of objects in both container
  213 + if(timePred(*firstIter,*secondIter))
  214 + {
  215 + //check if already in new container
  216 + if(amIinCollision(_newCollisions,*firstIter)==false) {
  217 + _newCollisions.push_back(*firstIter);
  218 + placed = true;
  219 + }
  220 + }
212 221  
213   - for( auto& c : a) {
  222 + else
  223 + {
  224 + //check if already in new container
  225 + if(amIinState(_newStateOjects,*secondIter)==false)
  226 + {
  227 + _newStateOjects.push_back(*secondIter);
  228 + placed = true;
  229 + }
  230 + }
  231 + }
  232 + }
214 233  
215   - // Check if any of the state objects are in the collection already
216   - if( c.obj == b.obj ) return true;
  234 + // If col object NOT in states
  235 + if (placed == false) _newCollisions.push_back(*firstIter);
217 236  
218   - return false;
219 237 }
220   - };
221 238  
222   - std::vector<collision::CollisionObject> _realCollisions;
223   - std::vector<StateChangeObj> _realSingularities;
  239 + for( auto& state : stateContainer) {
224 240  
225   - auto start = std::end(container2);
  241 + if(amIinState(_newStateOjects, state) == false ) _newStateOjects.push_back(state);
  242 + }
  243 + }
226 244  
227   - // Check if any object that is in CONTAINER1 is also in CONTAINER2 and compares time values to remove one
228   - for( auto first_iter = std::end(container1) - 1; first_iter != std::begin(container1) -1; --first_iter) {
229   - for( auto second_iter = start - 1; second_iter != std::begin(container2) -1; --second_iter ) {
  245 + // States are bigger
  246 + else {
230 247  
231   - if(( objPred( *first_iter, *second_iter ) )) {
  248 + bool placed = false;
232 249  
233   - if( timePred( *first_iter, *second_iter )) {
  250 + for(auto firstIter = std::end(stateContainer) - 1; firstIter!= std::begin(stateContainer) - 1;--firstIter)
  251 + {
  252 + for(auto secondIter = std::end(ColContainer) - 1; secondIter!= std::begin(ColContainer) - 1;--secondIter)
  253 + {
234 254  
235   - // Keep collision
236   - if( inCollisionsAlreadyPred(_realCollisions, *first_iter) == false) {
  255 + placed = false;
237 256  
238   - _realCollisions.push_back(*first_iter);
  257 + //Check for same Objects
  258 + if(objPred(*secondIter,*firstIter))
  259 + {
  260 + //check for time of objects in both container
  261 + if(timePred(*secondIter,*firstIter))
  262 + {
  263 + //check if already in new container
  264 + if(amIinCollision(_newCollisions,*secondIter)==false) {
  265 + _newCollisions.push_back(*secondIter);
  266 + placed = true;
  267 + }
239 268 }
240   - }
241   - else {
242   -
243   - // Keep state
244   - if( inSingularitiesAlreadyPred(_realSingularities, *second_iter) == false ) {
245 269  
246   - _realSingularities.push_back(*second_iter);
  270 + else
  271 + {
  272 + //check if already in new container
  273 + if(amIinState(_newStateOjects,*firstIter)==false)
  274 + {
  275 + _newStateOjects.push_back(*firstIter);
  276 + placed = true;
  277 + }
247 278 }
248 279 }
249 280 }
250   -// else {
251   -// break;
252   -// }
  281 +
  282 + // If col object NOT in states
  283 + _newStateOjects.push_back(*firstIter);
253 284 }
254   - }
255 285  
256   - if( !_realCollisions.empty() and !_realSingularities.empty() ) {
  286 + for( auto& collision : ColContainer) {
257 287  
258   - _collisions = _realCollisions;
259   - _singularities = _realSingularities;
260   - }
261   - else if( !_realCollisions.empty() and _realSingularities.empty() ) {
262   -
263   - _collisions = _realCollisions;
264   - }
265   - else if( _realCollisions.empty() and !_realSingularities.empty() ) {
  288 + if(amIinCollision(_newCollisions, collision) == false ) _newCollisions.push_back(collision);
  289 + }
266 290  
267   - _singularities = _realSingularities;
268 291 }
269 292  
270 293  
271 294 }
272 295  
  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 ) )) {