Commit 72b7045fc292d702c6cd29b61b888097e2c43cc2

Authored by Bjørn-Richard Pedersen
1 parent 7d9d5302

Working....?

Showing 1 changed file with 92 additions and 136 deletions   Show diff stats
include/collision_library.cpp
... ... @@ -24,9 +24,24 @@ namespace collision
24 24 const auto S1_radius = S1.getRadius();
25 25 const auto radius_sum = S0_radius + S1_radius;
26 26  
  27 + auto r1 = S1.computeTrajectory(new_dt);
  28 + auto r2 = S0.computeTrajectory(new_dt);
  29 +
  30 + // If the sphere's state is Rolling, it should use an adjusted DS
  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);
  34 + }
  35 +
  36 + // If the sphere's state is Rolling, it should use an adjusted DS
  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);
  40 + }
  41 +
  42 +
27 43 const auto Q = (S1_position - S0_position);
28   - const auto r1 = S1.computeTrajectory(new_dt);
29   - const auto r2 = S0.computeTrajectory(new_dt);
  44 +
30 45 const auto R = r1 - r2;
31 46  
32 47 const auto _QR = Q * R;
... ... @@ -227,7 +242,58 @@ namespace collision
227 242 }
228 243  
229 244 GMlib::Point<float,2>
230   - closestPoint(DynamicPSphere& S, StaticPPlane& P) {
  245 + MyController::closestPoint(DynamicPSphere& S, StaticPPlane& P)
  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 +
231 297  
232 298 }
233 299  
... ... @@ -318,10 +384,12 @@ namespace collision
318 384 if( this->_state == DynamicPSphere::States::Rolling ) {
319 385  
320 386 ds = adjustedTrajectory(dt);
  387 + this->environment = &this->_sphereController->_env;
321 388 }
322 389 if( this->_state == DynamicPSphere::States::Free ) {
323 390  
324 391 ds = this->computeTrajectory(dt);
  392 + this->environment = &this->_sphereController->_env;
325 393 }
326 394  
327 395 // Move
... ... @@ -346,7 +414,7 @@ namespace collision
346 414 auto vel = this->velocity;
347 415 auto dtCount = dt.count(); // 0
348 416 auto xF = this->externalForces();
349   - auto ds = vel * dtCount + 0.5 * xF * std::pow(dtCount, 2);
  417 + GMlib::Vector<double,3> ds = vel * dtCount + 0.5 * xF * std::pow(dtCount, 2);
350 418  
351 419 return ds;
352 420  
... ... @@ -366,21 +434,25 @@ namespace collision
366 434 auto planes = this->_sphereController->getAttachedObjects(this);
367 435 GMlib::Vector<float,3> n {0.0f, 0.0f, 0.0f};
368 436  
  437 + GMlib::Point<float,2> q;
  438 +
369 439 for( auto& plane : planes ) {
370 440  
371 441 const auto M = plane->evaluateParent(0.5f, 0.5f, 1, 1);
372   - const auto q = M(0)(0);
373 442 const auto u = M(1)(0);
374 443 const auto v = M(0)(1);
375 444 auto normal = GMlib::Vector<float,3> (u ^ v);
376 445 n += normal;
  446 + q = _sphereController->closestPoint(*this, *plane);
377 447 }
378 448  
379 449 n = GMlib::Vector<float,3> (n / planes.size()).getNormalized();
380 450  
381 451 // auto adjusted_ds = ds + (r + (p * s) )* n;
  452 + auto adjusted_ds = ds + (q - (s + ds)) + (n * r);
382 453  
383   - auto adjusted_ds = ds - (ds * n * n);
  454 + // Ghada's adjusted ds
  455 +// auto adjusted_ds = ds - (ds * n * n);
384 456  
385 457 return adjusted_ds;
386 458  
... ... @@ -584,6 +656,8 @@ namespace collision
584 656 auto time = state.time;
585 657 auto planes = state.attachedPlanes;
586 658  
  659 + std::cout << "handleStates says the state is now " << int(newState) << " after being " << int(sphere->_state) << std::endl;
  660 +
587 661 if( newState == DynamicPSphere::States::Free ) {
588 662  
589 663 detachObjects(sphere);
... ... @@ -593,14 +667,9 @@ namespace collision
593 667  
594 668 setAttachedObjects(planes, sphere);
595 669 sphere->_state = newState;
596   -
597   - if( newState == DynamicPSphere::States::AtRest ) {
598   -
599   - sphere->environment = &_stillEnvironment;
600   - sphere->velocity = GMlib::Vector<double,3> (0.0f, 0.0f, 0.0f);
601   - }
602 670 }
603 671  
  672 +
604 673 sphere->simulateToTInDt(time);
605 674 }
606 675  
... ... @@ -639,8 +708,10 @@ namespace collision
639 708 else if (d_sphere_1 && s_plane_2) {
640 709  
641 710 if(d_sphere_1->_state != DynamicPSphere::States::AtRest)
  711 + {
642 712 d_sphere_1->simulateToTInDt(c.t_in_dt);
643   - collision::computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
  713 + }
  714 + collision::computeImpactResponse( *d_sphere_1, *s_plane_2, c.t_in_dt); // D_Sphere vs. S_Plane
644 715  
645 716 }
646 717 else if (d_sphere_1 && s_bezier_2)
... ... @@ -756,7 +827,7 @@ namespace collision
756 827  
757 828 if( planes.empty() ) { // Sphere NOT attached
758 829  
759   - for (auto& plane : _static_planes){
  830 + for (auto& plane : _static_planes) {
760 831 auto M = plane->evaluateParent(0.5f,0.5f,1,1);
761 832 auto q = M(0)(0);
762 833 auto u = M(1)(0);
... ... @@ -770,18 +841,19 @@ namespace collision
770 841 const auto x = dn / dsn;
771 842 returnTime = (x * newDt) + sphereTime;
772 843  
773   - if( bla < epsilon ) {
774 844  
775   - planeContainer.insert(plane);
776   - state = DynamicPSphere::States::AtRest;
777   - std::cout << "detectStateChange says the state will become AtRest from Free" << std::endl;
778   - }
779   - else if( std::abs(dn) < epsilon and dsn < 0 ) {
  845 + if( std::abs(dn) < epsilon and dsn <= 0 ) {
780 846  
781 847 planeContainer.insert(plane);
782 848 state = DynamicPSphere::States::Rolling;
783 849 std::cout << "detectStateChange says the state will become Rolling from Free" << std::endl;
784 850 }
  851 + else if(std::abs(dn) < epsilon and bla < epsilon ) {
  852 +
  853 + planeContainer.insert(plane);
  854 + state = DynamicPSphere::States::AtRest;
  855 + std::cout << "detectStateChange says the state will become AtRest from Free" << std::endl;
  856 + }
785 857 else state = DynamicPSphere::States::Free;
786 858 }
787 859  
... ... @@ -841,122 +913,6 @@ namespace collision
841 913 else return StateChangeObj(sphere, planes, returnTime, DynamicPSphere::States::AtRest);
842 914 }
843 915 }
844   -
845   - /*
846   - // If the sphere is NOT ATTACHED to any planes, we check if it BECOMES attached in this timestep
847   - if( planes.empty() ) {
848   -
849   - // Go through each plane in the game and calculate whether or not the sphere is attached to any of them
850   - for( auto& plane : _static_planes ) {
851   -
852   - const auto M = plane->evaluateParent(0.5f, 0.5f, 1, 1);
853   - const auto q = M(0)(0);
854   - const auto u = M(1)(0);
855   - const auto v = M(0)(1);
856   - const auto n = GMlib::Vector<float,3> (u ^ v).getNormalized();
857   - const auto d = (q + r * n) - pos;
858   -
859   - // Debug variables / criteria for states
860   - const auto still = std::abs(((-n * r) * ds) - (ds * ds)); // Sphere is Resting on surface
861   - const auto dsn = ds * n; // Sphere is Rolling on surface. (Cannot be still AND rolling)
862   - const auto dn = d * n; // Sphere is Flying
863   -
864   -// std::cout << "The still is: " << still << std::endl; // Useful for seeing if the sphere should be still or not
865   -
866   - // Return time, used same method as for collision time between sphere and plane (Might need to be done in a different way!!!!)
867   - // sphereTime = dt_min
868   - const auto x = dn / dsn;
869   - returnTime = (x * newDt) + sphereTime;
870   -
871   - // Check to see if the sphere has actually even touched the plane yet
872   - if( std::abs(dn) < epsilon ) {
873   -
874   - /// If a sphere is attached to a surface, it can only be Still (AtRest) or Rolling, not both.
875   - /// If either are true, we store the current plane, as the application is object centric and if a sphere is still or rolling
876   - /// in relation to ONE plane, then it that is it's STATE in the game, and store the state the sphere should TRANSITION into.
877   - if( still < epsilon ) {
878   -
879   - std::cout << "State will become AtRest" << std::endl;
880   - planeContainer.insert( plane );
881   - state = DynamicPSphere::States::AtRest;
882   - }
883   - else if( still > epsilon ) {
884   -
885   - std::cout << "State will become Rolling" << std::endl;
886   - planeContainer.insert( plane );
887   - state = DynamicPSphere::States::Rolling;
888   - }
889   - }
890   - // The sphere did NOT ATTACH to any surface
891   - else state = DynamicPSphere::States::Free;
892   - }
893   -
894   - return StateChangeObj( sphere, planeContainer, returnTime, state );
895   - }
896   - // Else the sphere IS ATTACHED and we need to calculate if it will change state / detach
897   - else {
898   -
899   - // Go through the planes that the sphere is attached to in order to get a common normal for use in computation
900   - // The sphere won't always be attached to multiple planes
901   - for( auto& plane : planes ) {
902   -
903   - const auto M = plane->evaluateParent(0.5f, 0.5f, 1, 1);
904   - const auto q = M(0)(0);
905   - const auto u = M(1)(0);
906   - const auto v = M(0)(1);
907   - auto normal = GMlib::Vector<float,3> (u ^ v);
908   - n += normal;
909   - }
910   -
911   - n = GMlib::Vector<float,3> (n / planes.size()).getNormalized();
912   -
913   - // Debug variables / criteria for states
914   - const auto d = (q + r * n) - pos;
915   - const auto still = std::abs(((-n * r) * ds) - (ds * ds)); // Sphere is Resting on surface
916   - const auto dsn = ds * n; // Sphere is Rolling on surface. (Cannot be still AND rolling)
917   - const auto dn = d * n; // Sphere is Flying
918   -
919   - // Return time, used same method as for collision time between sphere and plane (Might need to be done in a different way!!!!)
920   - // sphereTime = dt_min
921   - returnTime = ((dn / dsn) * newDt) + sphereTime;
922   -
923   - if( std::abs(dn) > epsilon ) {
924   -
925   - /// Check the sphere's current state, and if it needs to be changed.
926   - if( sphere->_state == DynamicPSphere::States::Rolling) {
927   -
928   - // Detatchment is seen as a state change, and will be processed further in the algorithm
929   - if( dsn > 0 ) {
930   - std::cout << "State changing from Rolling to Free" << std::endl;
931   - state = DynamicPSphere::States::Free;
932   - return StateChangeObj( sphere, planes, returnTime, state );
933   - }
934   - else if( still < epsilon ) {
935   - std::cout << "State changing from Rolling to AtRest" << std::endl;
936   - state = DynamicPSphere::States::AtRest;
937   - return StateChangeObj( sphere, planes, returnTime, state );
938   - }
939   - // Else the sphere will keep being in a Rolling state
940   - else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::Rolling );
941   - }
942   -
943   - if( sphere->_state == DynamicPSphere::States::AtRest ) {
944   -
945   - if( dsn > 0 ) {
946   - std::cout << "State changing from AtRest to Free" << std::endl;
947   - state = DynamicPSphere::States::Free;
948   - return StateChangeObj( sphere, planes, returnTime, state );
949   - }
950   - else if( still > epsilon ) {
951   - std::cout << "State changing from AtRest to Rolling" << std::endl;
952   - state = DynamicPSphere::States::Rolling;
953   - return StateChangeObj( sphere, planes, returnTime, state );
954   - }
955   - else return StateChangeObj( sphere, planes, returnTime, DynamicPSphere::States::Free );
956   - }
957   - }
958   - */
959   -
960 916 }
961 917  
962 918 void
... ...