From 9cee4243837f35431302814cc7ace13a72ab5fed Mon Sep 17 00:00:00 2001
From: pjhatch <66844564+pjhatch@users.noreply.github.com>
Date: Sun, 5 Jul 2020 14:53:14 -0500
Subject: [PATCH] Update FX.cpp

---
 wled00/FX.cpp | 57 ++++++++++++++++++++-------------------------------
 1 file changed, 22 insertions(+), 35 deletions(-)

diff --git a/wled00/FX.cpp b/wled00/FX.cpp
index 02db3e71..c6e0d6f8 100644
--- a/wled00/FX.cpp
+++ b/wled00/FX.cpp
@@ -2612,7 +2612,7 @@ uint16_t WS2812FX::mode_balltrack(void) {
     for (uint8_t i = 0; i < maxNumBalls; i++) {
       balls[i].lastBounceUpdate = time;
       balls[i].velocity = 0;
-      while(abs(balls[i].velocity)<.5){
+      while(abs(balls[i].velocity)<.5){ // at the start make sure they are all moving
         balls[i].velocity=10*(-.5-float(random16(0, 10000)) / 10000.0); // time units are ms
       }
       balls[i].height=(float(random16(0, 10000)) / 10000.0); // from 0. to 1.
@@ -2626,19 +2626,9 @@ uint16_t WS2812FX::mode_balltrack(void) {
   fill(hasCol2 ? BLACK : SEGCOLOR(1));
 
   for (uint8_t i = 0; i < numBalls; i++) {
-    if(abs(balls[i].velocity)<0) { // then I am guessing we have a new ball as intensity changed
-      balls[i].lastBounceUpdate = time;
-      balls[i].velocity = 0;
-      while(abs(balls[i].velocity)<.5){
-        balls[i].velocity=10*(-.5-float(random16(0, 10000)) / 10000.0); // time units are ms
-      }
-      balls[i].height=(float(random16(0, 10000)) / 10000.0); // from 0. to 1.
-      balls[i].mass=(float(random16(5000, 10000)) / 10000.0); // from .5 to 1.
-    }
-
-    float timeSinceLastUpdate = (time - balls[i].lastBounceUpdate)/cfac;
+    float timeSinceLastUpdate = float((time - balls[i].lastBounceUpdate))/cfac;
     float thisHeight= balls[i].height + balls[i].velocity * timeSinceLastUpdate; // this method keeps higher resolution
-    // test if intensity level was increased and some balls are way off the track so put them back
+    // test if intensity level was increased and some balls are way off the track then put them back
     if(thisHeight<-.5 || thisHeight> 1.5){
       balls[i].height=(float(random16(0, 10000)) / 10000.0); // from 0. to 1.
       thisHeight=balls[i].height;
@@ -2654,28 +2644,23 @@ uint16_t WS2812FX::mode_balltrack(void) {
     bool docol=true;
     if(docol){
       for(uint8_t j= i+1; j < numBalls; j++){
-        float timeSinceLastUpdate2 = (time - balls[j].lastBounceUpdate)/cfac;
-        float thisHeight2= balls[j].height + balls[j].velocity * timeSinceLastUpdate2;
-        bool collided=false;
-        //if((balls[i].height<=balls[j].height)&&(thisHeight>=thisHeight2)) collided=true;
-        //if((balls[i].height>balls[j].height)&&(thisHeight<=thisHeight2)) collided=true;
-        if((balls[i].height<=thisHeight2)&&(thisHeight>=thisHeight2)) collided=true;
-        if((balls[i].height> thisHeight2)&&(thisHeight<=thisHeight2)) collided=true;
-        if(balls[i].velocity==balls[j].velocity) collided=false; // avoid divide by zero
-        if(collided){
-          double tcollided=((balls[j].height*cfac-balls[j].velocity*double(balls[j].lastBounceUpdate))
-                         - (balls[i].height*cfac-balls[i].velocity*double(balls[i].lastBounceUpdate)) )/
-                         ((balls[i].velocity-balls[j].velocity));
-          float hcollided=balls[i].height+ balls[i].velocity*(tcollided-balls[i].lastBounceUpdate)/cfac;
-          balls[i].height=hcollided;
-          balls[j].height=hcollided;
-          balls[i].lastBounceUpdate=(unsigned long)tcollided;
-          balls[j].lastBounceUpdate=(unsigned long)tcollided;
-          float vtmp=balls[i].velocity;
-          balls[i].velocity=((balls[i].mass-balls[j].mass)*vtmp+2.*balls[j].mass*balls[j].velocity)/(balls[i].mass+balls[j].mass);
-          balls[j].velocity=((balls[j].mass-balls[i].mass)*balls[j].velocity + 2.*balls[i].mass*vtmp)/(balls[i].mass+balls[j].mass);
-          timeSinceLastUpdate = (time - tcollided)/cfac; 
-          thisHeight= balls[i].height + balls[i].velocity * timeSinceLastUpdate; // 
+        if(balls[j].velocity != balls[i].velocity) {
+          //  tcollided + balls[j].lastBounceUpdate is acutal time of collision (this keeps precision with long to float conversions)
+          float tcollided= ( cfac*(balls[i].height-balls[j].height) +
+                balls[i].velocity*float(balls[j].lastBounceUpdate-balls[i].lastBounceUpdate))/
+               (balls[j].velocity-balls[i].velocity);
+
+          if( (tcollided>2)&&(tcollided<float(time-balls[j].lastBounceUpdate))){
+            balls[i].height=balls[i].height + balls[i].velocity*(tcollided+float(balls[j].lastBounceUpdate-balls[i].lastBounceUpdate))/cfac;
+            balls[j].height=balls[i].height;
+            balls[i].lastBounceUpdate=(unsigned long)(tcollided+.5)+balls[j].lastBounceUpdate;
+            balls[j].lastBounceUpdate=balls[i].lastBounceUpdate;
+            float vtmp=balls[i].velocity;
+            balls[i].velocity=((balls[i].mass-balls[j].mass)*vtmp              + 2.*balls[j].mass*balls[j].velocity)/(balls[i].mass+balls[j].mass);
+            balls[j].velocity=((balls[j].mass-balls[i].mass)*balls[j].velocity + 2.*balls[i].mass*vtmp)             /(balls[i].mass+balls[j].mass);           
+            //balls[i].height=balls[i].height + balls[i].velocity*(time-balls[i].lastBounceUpdate)/cfac;
+            thisHeight= balls[i].height + balls[i].velocity*(time-balls[i].lastBounceUpdate)/cfac;
+          }
         }
     }
     }
@@ -2691,6 +2676,8 @@ uint16_t WS2812FX::mode_balltrack(void) {
     if (thisHeight>1) thisHeight=1;
     uint16_t pos = round(thisHeight * (SEGLEN - 1));
     setPixelColor(pos, color);
+    balls[i].lastBounceUpdate=time;
+    balls[i].height=thisHeight;
   }
 
   return FRAMETIME;