Commit 6cde407f153dae5d7df68e270b35a4c3ac228e6d

Authored by Bjørn-Richard Pedersen
1 parent 4b653fff

Before starting to mess with DetectStateChange function

Showing 2 changed files with 224 additions and 72 deletions   Show diff stats
include/collision_library.cpp
... ... @@ -125,9 +125,10 @@ namespace collision
125 125  
126 126 if( std::abs(Q) < epsilon )
127 127 {
  128 + // The sphere is "touching" the surface
128 129 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallelAndTouching);
129 130 }
130   - else if( std::abs(R) < epsilon) // Attached and Sliding
  131 + else if( std::abs(R) < epsilon)
131 132 {
132 133 return CollisionState( seconds_type(0.0), CollisionStateFlag::SingularityParallel);
133 134 }
... ... @@ -150,7 +151,7 @@ namespace collision
150 151 const auto n = u ^ v;
151 152 const auto n_normal = GMlib::Vector<float,3>(n).getNormalized();
152 153  
153   - const auto new_vel = S.velocity - (2*(S.velocity * n_normal) * n_normal);
  154 + auto new_vel = S.velocity - (2*(S.velocity * n_normal) * n_normal) * 0.95;
154 155  
155 156 S.velocity = new_vel;
156 157  
... ... @@ -298,8 +299,8 @@ namespace collision
298 299  
299 300 // Update physics
300 301 auto F = this->externalForces();
301   - auto a = 0.5 * F * (this->mass) * std::pow(dt.count(), 2);
302   -
  302 + auto c = dt.count();
  303 + auto a = F*c;
303 304 this->velocity += a;
304 305 }
305 306  
... ... @@ -321,15 +322,21 @@ namespace collision
321 322 GMlib::Vector<double, 3>
322 323 DynamicPhysObject<GMlib::PSphere<float> >::getTrajectory(seconds_type dt) {
323 324  
324   - return this->_trajectory;
  325 + return this->_NewTrajectory;
325 326 }
326 327  
327 328 void
328   - DynamicPhysObject<GMlib::PSphere<float> >::setTrajectory(GMlib::Vector<double, 3> t) {
  329 + DynamicPhysObject<GMlib::PSphere<float> >::setTrajectory(GMlib::Vector<double, 3> ds, DynamicPSphere* sphere) {
329 330  
330 331 // Update ds to modified DS'
331   - // Can maybe do check on state in here, probably leave it to state-loop
332   - this->_trajectory = t;
  332 + auto r = sphere->getRadius();
  333 + auto s = sphere->getPos();
  334 + auto p = ds + s;
  335 +// GMlib::Vector<d,3> test = (p * s).getNormalized();
  336 +
  337 +// auto adj_ds = ds + ( r + (p * s) ).getNormalized();
  338 +
  339 +
333 340 }
334 341  
335 342 GMlib::Vector<double,3>
... ... @@ -337,7 +344,7 @@ namespace collision
337 344  
338 345 assert(environment != nullptr);
339 346  
340   - return this->environment->externalForces();
  347 + return this->environment->externalForces().toType<double>();
341 348 }
342 349  
343 350 void
... ... @@ -349,11 +356,17 @@ namespace collision
349 356 sphere->curr_t_in_dt = seconds_type{0.0};
350 357 }
351 358  
352   - // Collision detection algorithm
  359 + // Singularity detection
  360 + singularityDetection(dt);
353 361  
354   - //Check for collisions for sphere
355   - for( auto& sphere : _dynamic_spheres) {
  362 + // StateChangeDetection
  363 + for(auto& s : _dynamic_spheres){
356 364  
  365 + detectStateChange(s, dt);
  366 + }
  367 +
  368 + // Collision detection algorithm
  369 + for( auto& sphere : _dynamic_spheres) {
357 370  
358 371 // Sending in sphere twice in the initial call because the function will require a second object in future calls
359 372 // Må da være en bedre måte å gjøre det på
... ... @@ -365,12 +378,12 @@ namespace collision
365 378 }
366 379 }
367 380  
368   - // Check for other (non-collision) singularities
369   -
370 381 // Make Collision unique
371 382 sortAndMakeUnique(_collisions);
372 383 std::reverse(_collisions.begin(), _collisions.end());
373 384  
  385 +
  386 +
374 387 // Impact response
375 388 while( !_collisions.empty() ) {
376 389  
... ... @@ -484,12 +497,95 @@ namespace collision
484 497  
485 498 }
486 499  
  500 + // Singularity Detection
  501 + void
  502 + MyController::singularityDetection(double dt) {
  503 +
  504 + // Sphere vs. Plane
  505 + for( auto& sphere : _dynamic_spheres) {
  506 + for( auto& plane : _static_planes) {
  507 +
  508 + auto detection = collision::detectCollision( *sphere, *plane, seconds_type(dt));
  509 +
  510 + // The Sphere is parallell and touching the sphere = Sphere attached to Plane
  511 + if(detection.CollisionState::flag == CollisionStateFlag::SingularityParallelAndTouching) {
  512 + auto singularity = StateChangeObj(sphere, plane, seconds_type(dt), DynamicPSphere::States::Rolling );
  513 + _singularities.push_back(singularity);
  514 + }
  515 + // The Sphere is Not / no longer attached to the plane. Remove the plane from our map of attachments
  516 + if(detection.CollisionState::flag != CollisionStateFlag::SingularityParallelAndTouching) {
  517 +
  518 + auto planes = getAttachedObjects(sphere);
  519 +
  520 + if( !planes.empty() ) {
  521 +
  522 + for( auto p : planes){
  523 +
  524 + if( plane == p) detachObjects(p, sphere);
  525 + }
  526 + }
  527 + }
  528 + // if other flags
  529 + }
  530 + }
  531 +
  532 + // Sphere vs. Other types of surface
  533 + } // EOF
  534 +
  535 +
  536 + /// vector
  537 + // Get objects attached to sphere
  538 + std::vector<StaticPPlane *>
  539 + MyController::getAttachedObjects(DynamicPSphere* sphere)
  540 + {
  541 + return (_map[sphere]);
  542 + }
  543 +
  544 + // Set objects attached to sphere
  545 + void
  546 + MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
  547 + {
  548 + _map[sphere].push_back(plane);
  549 + }
  550 +
  551 + // Remove objects from the set In the map
  552 + void
  553 + MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
  554 +
  555 +// _map[sphere].erase(plane);
  556 + }
  557 +
  558 +
  559 + /// unordered_set
  560 + // Get objects attached to sphere
  561 +// std::unordered_set<StaticPPlane *>
  562 +// MyController::getAttachedObjects(DynamicPSphere* sphere)
  563 +// {
  564 +// return (_map[sphere]);
  565 +// }
  566 +
  567 +// // Set objects attached to sphere
  568 +// void
  569 +// MyController::setAttachedObjects(StaticPPlane* plane, DynamicPSphere* sphere)
  570 +// {
  571 +// _map[sphere].emplace(plane);
  572 +// }
  573 +
  574 +// // Remove objects from the set In the map
  575 +// void
  576 +// MyController::detachObjects(StaticPPlane *plane, DynamicPSphere *sphere){
  577 +
  578 +// _map[sphere].erase(plane);
  579 +// }
  580 +
487 581 // Adding objects to vector
  582 +
488 583 void
489 584 MyController::add(DynamicPSphere * const sphere) {
490 585  
491 586 sphere->environment = &_env;
492 587 _dynamic_spheres.push_back(sphere);
  588 + _map[sphere];
493 589  
494 590 }
495 591 void
... ... @@ -513,32 +609,63 @@ namespace collision
513 609 _static_bezier_surf.push_back(surf);
514 610 }
515 611  
516   - StaticPPlane *
517   - DynamicPhysObject<GMlib::PSphere<float> >::getAttachedTo()
518   - {
519   - return this->_plane;
520   - }
521   -
522   - void
523   - DynamicPhysObject<GMlib::PSphere<float> >::setAttachedTo(StaticPPlane *p)
524   - {
525   - this->_plane = p;
526   - }
  612 + // States
  613 +
  614 + // With vector
  615 + StateChangeObj
  616 + MyController::detectStateChange( DynamicPSphere* sphere, double dt) {
  617 +
  618 + auto planes = this->getAttachedObjects(sphere) ;
  619 + auto r = sphere->getRadius();
  620 + auto p = sphere->getMatrixToScene() * sphere->getPos();
  621 +
  622 + if (planes.empty()){
  623 + sphere->_state = DynamicPSphere::States::Free;
  624 + }
  625 + else{
  626 + auto epsilon = 1e-5;
  627 +
  628 + // Using original DS to compute states
  629 + auto dts = seconds_type(dt);
  630 + auto max_dt = dts;
  631 + auto min_dt = sphere->curr_t_in_dt;
  632 + auto new_dt = max_dt -min_dt;
  633 + auto ds = sphere->computeTrajectory(new_dt);
  634 +
  635 + GMlib::APoint<float,3> q;
  636 + GMlib::Vector <float,3>n {0.0f,0.0f,0.0f};
  637 + for (auto &it :planes){
  638 + auto M = it->evaluateParent(0.5f,0.5f,1,1);
  639 + auto pos= M(0)(0);
  640 + auto u = M(1)(0);
  641 + auto v = M(0)(1);
  642 + auto normal = GMlib::Vector<float,3>(u ^ v);
  643 + n+=normal;
  644 + q=pos;
  645 + }
  646 + n= GMlib::Vector <float,3>(n/planes.size()).getNormalized();
527 647  
528   - std::vector<DynamicPhysObject<GMlib::PSphere<float> >::StateChangeObj *>
529   - DynamicPhysObject<GMlib::PSphere<float> >::detectStateChange(DynamicPhysObject<GMlib::PSphere<float> > *S, double t)
530   - {
531   - auto s = S;
532   - auto p = s->getAttachedTo();
533   - }
  648 + auto d = (q + r * n) - p;
  649 + auto bla=std::abs(((-n*r) * ds) -(ds*ds));
  650 + auto dsn= ds * n;
  651 + auto dn= d*n;
534 652  
535   - void
536   - DynamicPhysObject<GMlib::PSphere<float> >::setState(States s) {
  653 + if (std::abs(((-n*r) * ds) -(ds*ds)) < epsilon) {
  654 + sphere->_state=DynamicPSphere::States::AtRest;
  655 + std::cout << "State is now: At Rest" << std::endl;
  656 + }
  657 + else if(std::abs(((-n*r) * ds) -(ds*ds)) > epsilon) {
  658 + sphere->_state = DynamicPSphere::States::Rolling;
  659 + std::cout << "State is now: Rolling" << std::endl;
  660 +// sphere->setTrajectory
  661 + }
537 662  
538   - this->_state = s;
  663 + }
539 664 }
540 665  
541 666  
  667 +// } // EOF
  668 +
542 669  
543 670 } // END namespace collision
544 671  
... ...
include/collision_library.h
... ... @@ -5,11 +5,15 @@
5 5 #include <collision_interface.h>
6 6  
7 7 #include <typeinfo>
8   -
  8 +#include <unordered_map>
9 9  
10 10 namespace collision
11 11 {
12 12  
  13 + // "Declaration" of the struct
  14 + struct StateChangeObj;
  15 +
  16 +
13 17 class MyController : public Controller {
14 18 GM_SCENEOBJECT (MyController)
15 19 public:
... ... @@ -22,6 +26,19 @@ public:
22 26 void add (StaticPCylinder* const cylinder);
23 27 void add (StaticPBezierSurf* const surf);
24 28  
  29 + // Change "value" and return type if the sphere should be able to be attached to objects other than planes
  30 +// std::unordered_set<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
  31 + std::vector<StaticPPlane*> getAttachedObjects(DynamicPSphere* sphere);
  32 + void setAttachedObjects( StaticPPlane* plane, DynamicPSphere* sphere );
  33 + void detachObjects( StaticPPlane* plane, DynamicPSphere* sphere);
  34 +
  35 + StateChangeObj
  36 + detectStateChange(DynamicPSphere* s, double time);
  37 +
  38 + void
  39 + singularityDetection(double dt);
  40 +
  41 +
25 42 CollisionState detectCollision (const DynamicPSphere& S,
26 43 const DynamicPCylinder& C, seconds_type dt);
27 44  
... ... @@ -61,10 +78,16 @@ protected:
61 78 std::vector<StaticPPlane*> _static_planes;
62 79 std::vector<StaticPCylinder*> _static_cylinders;
63 80 std::vector<StaticPBezierSurf*> _static_bezier_surf;
  81 +
64 82 std::vector<collision::CollisionObject> _collisions;
  83 + std::vector<StateChangeObj> _singularities;
  84 +
  85 + DefaultEnvironment _env;
  86 +
  87 +// std::unordered_map<DynamicPSphere*, std::unordered_set<StaticPPlane*>> _map;
  88 + std::unordered_map<DynamicPSphere*, std::vector<StaticPPlane*>> _map;
65 89  
66 90  
67   - Environment _env;
68 91  
69 92  
70 93 };
... ... @@ -76,47 +99,42 @@ public:
76 99  
77 100 enum class States {
78 101 Free,
79   - Sliding,
  102 + Rolling,
80 103 AtRest
81 104 };
82 105  
83   - struct StateChangeObj {
84   - DynamicPSphere* obj;
85   - seconds_type time;
86   - States state;
87   -
88   - StateChangeObj
89   - ( PhysObj_Base* o, seconds_type t, DynamicPSphere::States s) {}
90   - };
  106 + MyController* _controller;
  107 + States _state = States::Free; // Which state is the sphere in
91 108  
92   - StaticPPlane* getAttachedTo();
93   - void setAttachedTo(StaticPPlane* plane);
  109 + GMlib::Vector<double, 3>
  110 + getTrajectory (seconds_type dt);
94 111  
95   - void setState(States s);
  112 + void
  113 + setTrajectory (GMlib::Vector<double, 3> ds, DynamicPSphere* sphere);
96 114  
97   - std::vector<StateChangeObj*>
98   - detectStateChange(DynamicPhysObject<GMlib::PSphere<float>>* S, double t);
  115 + GMlib::Vector<double, 3> _NewTrajectory;
99 116  
  117 + // "Old" methods
100 118 void simulateToTInDt( seconds_type t ) override;
101 119  
102 120 GMlib::Vector<double, 3>
103 121 computeTrajectory (seconds_type dt) const override; // [m]
104 122  
105   - GMlib::Vector<double, 3>
106   - getTrajectory (seconds_type dt);
107   -
108   - void
109   - setTrajectory (GMlib::Vector<double, 3>);
110   -
111   - GMlib::Vector<double, 3> _trajectory;
112   -
113 123 GMlib::Vector<double, 3> externalForces () const override; // [m / s^2]
114 124  
115   -private:
116 125  
117   - States _state;
118   - StaticPPlane* _plane;
  126 +};
  127 +
  128 +// StateChangeObject struct
  129 +struct StateChangeObj {
  130 + PhysObj_Base* obj1; // Object whos state will change
  131 + PhysObj_Base* obj2; // Object that obj1 will change state ACCORDING to
  132 + seconds_type time; // Time of singularity
  133 + DynamicPSphere::States state; // State that obj1 will change to
119 134  
  135 + StateChangeObj
  136 + (PhysObj_Base* o1, PhysObj_Base* o2, seconds_type t, DynamicPSphere::States s) :
  137 + obj1{o1}, obj2{o2}, time{t} ,state{s} {}
120 138 };
121 139  
122 140 template <class PSurf_T, typename... Arguments>
... ... @@ -246,6 +264,8 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type
246 264 // Sphere vs. Static Plane
247 265 if( plane ) {
248 266  
  267 + auto attachedPlanes = getAttachedObjects(sphere);
  268 +
249 269 for( auto iter = std::begin(_static_planes); iter != std::end(_static_planes); ++iter) {
250 270  
251 271 // The sphere cannot collide with the same plane it just collided with
... ... @@ -253,23 +273,28 @@ void MyController::sphereStaticCollision(T_s* sphere, T_o* object, seconds_type
253 273 // break;
254 274 // }
255 275  
256   - auto col = collision::detectCollision(*sphere, **iter, dt);
  276 + // If sphere is attached to the plane, it should NOT check for collisions with it
  277 +// for( auto pl = attachedPlanes.begin(); pl != attachedPlanes.end(); pl++) {
257 278  
258   - if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
  279 +// if( *iter == *pl) break;
259 280  
260   - auto& static_object = *iter;
  281 + // Else look for collision
  282 + auto col = collision::detectCollision(*sphere, **iter, dt);
261 283  
262   - auto new_t = sphere->curr_t_in_dt;
  284 + if( col.CollisionState::flag == CollisionStateFlag::Collision ) {
263 285  
264   - if( col.time > seconds_type(new_t) and col.time < dt)
265   - {
266   - auto col_obj = CollisionObject(sphere, static_object, col.time);
267   - _collisions.push_back(col_obj);
  286 + auto& static_object = *iter;
268 287  
269   - }
  288 + auto new_t = sphere->curr_t_in_dt;
270 289  
271   - // Other singularities
272   - }
  290 + if( col.time > seconds_type(new_t) and col.time < dt)
  291 + {
  292 + auto col_obj = CollisionObject(sphere, static_object, col.time);
  293 + _collisions.push_back(col_obj);
  294 +
  295 + }
  296 + }
  297 +// } // THIS ONE
273 298 }
274 299 }
275 300  
... ...