Commit 540a54dc5c6c949dcbb9967cd113cac0796f2ab4

Authored by Bjørn-Richard Pedersen
1 parent 72b7045f

Closest point still acting up sometimes, States seem to be fine now though.

Showing 2 changed files with 61 additions and 23 deletions   Show diff stats
include/collision_library.cpp
... ... @@ -367,6 +367,14 @@ namespace collision
367 367 void
368 368 DynamicPhysObject<GMlib::PSphere<float> >::simulateToTInDt(seconds_type t){
369 369  
  370 + std::ofstream myFile;
  371 +
  372 + myFile.open("SimulateLog.txt", std::ios::app );
  373 +
  374 + myFile << this << "\t" << t.count() << "\t\t"
  375 + << int(this->_state) << "\t\t"
  376 + << "(" << this->getPos() << ")" << "\t\t";
  377 +
370 378 if( this->_state == DynamicPSphere::States::AtRest
371 379 or (this->_state == DynamicPSphere::States::Rolling and std::abs(this->velocity(2) <= 1e-2))) {
372 380  
... ... @@ -385,6 +393,7 @@ namespace collision
385 393  
386 394 ds = adjustedTrajectory(dt);
387 395 this->environment = &this->_sphereController->_env;
  396 +
388 397 }
389 398 if( this->_state == DynamicPSphere::States::Free ) {
390 399  
... ... @@ -403,6 +412,11 @@ namespace collision
403 412 this->velocity += a;
404 413  
405 414 this->environment = &this->_sphereController->_env;
  415 +
  416 + myFile << "(" << this->getPos() << ")" << "\t\t[" << ds << "]\t\t" << int(this->_state) << std::endl;
  417 +
  418 + myFile.close();
  419 +
406 420 }
407 421  
408 422 }
... ... @@ -656,6 +670,15 @@ namespace collision
656 670 auto time = state.time;
657 671 auto planes = state.attachedPlanes;
658 672  
  673 + std::ofstream myFile;
  674 +
  675 + myFile.open("SingularityLog.txt", std::ios::app );
  676 +
  677 + myFile << sphere << "\t\t" << time.count() << "\t\t\t"
  678 + << int(sphere->_state) << "\t\t\t\t";
  679 +
  680 + GMlib::Vector<double,3> ds {0.0f, 0.0f, 0.0f};
  681 +
659 682 std::cout << "handleStates says the state is now " << int(newState) << " after being " << int(sphere->_state) << std::endl;
660 683  
661 684 if( newState == DynamicPSphere::States::Free ) {
... ... @@ -669,6 +692,9 @@ namespace collision
669 692 sphere->_state = newState;
670 693 }
671 694  
  695 + myFile << int(newState) << std::endl;
  696 +
  697 + myFile.close();
672 698  
673 699 sphere->simulateToTInDt(time);
674 700 }
... ... @@ -784,6 +810,8 @@ namespace collision
784 810 void
785 811 MyController::detectStateChanges(double dt) {
786 812  
  813 + // predicate here to check if allready in _singularities
  814 +
787 815 for( auto& sphere : _dynamic_spheres) {
788 816  
789 817 auto singularity = detectStateChange(sphere, dt);
... ... @@ -838,7 +866,10 @@ namespace collision
838 866 auto bla = std::abs(((-n*r) * ds) -(ds*ds));
839 867 auto dsn = ds * n;
840 868 auto dn = d*n;
841   - const auto x = dn / dsn;
  869 +
  870 + auto x = dn / dsn;
  871 + if( x == 0) x = sphereTime.count();
  872 +
842 873 returnTime = (x * newDt) + sphereTime;
843 874  
844 875  
... ... @@ -848,7 +879,7 @@ namespace collision
848 879 state = DynamicPSphere::States::Rolling;
849 880 std::cout << "detectStateChange says the state will become Rolling from Free" << std::endl;
850 881 }
851   - else if(std::abs(dn) < epsilon and bla < epsilon ) {
  882 + else if( std::abs(dn) < epsilon and bla < epsilon ) {
852 883  
853 884 planeContainer.insert(plane);
854 885 state = DynamicPSphere::States::AtRest;
... ... @@ -876,13 +907,15 @@ namespace collision
876 907 auto bla = std::abs(((-n*r) * ds) -(ds*ds));
877 908 auto dsn = ds * n;
878 909 auto dn = d * n;
879   - const auto x = dn / dsn;
880   - returnTime = (x * newDt) + sphereTime;
  910 +
  911 +
  912 + auto x = dn / dsn;
  913 + if( x == 0) x = sphereTime.count();
881 914  
882 915  
883 916 if( sphere->_state == DynamicPSphere::States::Rolling ) {
884 917  
885   - if( dsn > 0) {
  918 + if( std::abs(dn) > epsilon and dsn > 0) {
886 919  
887 920 std::cout << "detectStateChange says the state will become Free from Rolling" << std::endl;
888 921 state = DynamicPSphere::States::Free;
... ... @@ -915,35 +948,25 @@ namespace collision
915 948 }
916 949 }
917 950  
918   - void
919   - MyController::testMethod()
920   - {
  951 +// void
  952 +// MyController::testMethod()
  953 +// {
921 954  
922 955  
923 956 // auto sphere1 = _dynamic_spheres[0];
924 957 // auto sphere2 = _dynamic_spheres[1];
925   -// auto sphere3 = _dynamic_spheres[2];
926 958 // StaticPPlane* plane;
927 959  
928 960 // auto fake_sin1 = StateChangeObj(sphere1, fakePlanes, seconds_type(0.2), DynamicPSphere::States::NoChange);
929   -// auto fake_sin2 = StateChangeObj(sphere2, fakePlanes, seconds_type(0.4), DynamicPSphere::States::NoChange);
930   -// auto fake_sin3 = StateChangeObj(sphere3, fakePlanes, seconds_type(0.7), DynamicPSphere::States::NoChange);
931   -
932   -// auto fake_col1 = CollisionObject( sphere3, sphere2, seconds_type(0.3));
933   -//// auto fake_col2 = CollisionObject( sphere2, sphere1, seconds_type(0.4));
934   -//// auto fake_col3 = CollisionObject( sphere3, sphere2, seconds_type(0.5));
  961 +// auto fake_col1 = CollisionObject( sphere2, plane, seconds_type(0.2));
935 962  
936 963 // _singularities.push_back(fake_sin1);
937   -// _singularities.push_back(fake_sin2);
938   -// _singularities.push_back(fake_sin3);
939 964  
940 965 // _collisions.push_back( fake_col1 );
941   -//// _collisions.push_back( fake_col2 );
942   -//// _collisions.push_back( fake_col3 );
943 966  
944 967 // crossUnique(_collisions, _singularities);
945 968  
946   - }
  969 +// }
947 970  
948 971  
949 972  
... ...
include/collision_library.h
... ... @@ -7,6 +7,9 @@
7 7 #include <typeinfo>
8 8 #include <unordered_map>
9 9  
  10 +#include <iostream>
  11 +#include <fstream>
  12 +
10 13  
11 14 namespace collision
12 15 {
... ... @@ -225,7 +228,6 @@ void MyController::crossUnique(Container_1 container1, Container_2 container2) {
225 228 for( auto first_iter = std::end(container1) - 1; first_iter != std::begin(container1) -1; --first_iter) {
226 229 for( auto second_iter = start - 1; second_iter != std::begin(container2) -1; --second_iter ) {
227 230  
228   -
229 231 if(( objPred( *first_iter, *second_iter ) )) {
230 232  
231 233 if( timePred( *first_iter, *second_iter )) {
... ... @@ -251,8 +253,21 @@ void MyController::crossUnique(Container_1 container1, Container_2 container2) {
251 253 }
252 254 }
253 255  
254   - _collisions = _realCollisions;
255   - _singularities = _realSingularities;
  256 + if( !_realCollisions.empty() and !_realSingularities.empty() ) {
  257 +
  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() ) {
  266 +
  267 + _singularities = _realSingularities;
  268 + }
  269 +
  270 +
256 271 }
257 272  
258 273 // Sort and make Unique for states
... ...