Added smoothing to interpolation.

Author: onnoowl

src/telemetry/data.ts

@@ -4,28 +4,85 @@ export class DataPacket {
     /**
      * Interpolate between two data packets
      * @param alpha Value 0-1, determines how close to this packet(0)
-     *              or the other packet(1) the output should be
-     * @param other The packet to interpolate between
+     *              or the after packet(1) the output should be
+     * @param before The packet before "this" packet, used to get the derivative for smoothing. Can be undefined.
+     * @param after The packet to interpolate to. Can be undefined.
+     * @param twoAfter The packet after "after", used to get the derivative for smoothing. Can be undefined.
      */
-    interpolate(alpha: number, other: DataPacket): DataPacket {
-        if(other === undefined) {
+    interpolate(alpha: number, before: DataPacket, after: DataPacket, twoAfter: DataPacket): DataPacket {
+        if(after === undefined) {
             return this;
         }
-        let old_ra = this.ra;
-        let new_ra = other.ra;
-        if(Math.abs(this.ra - other.ra) > Math.PI) {
-            if(this.ra < other.ra) {
-                old_ra += 2 * Math.PI;
+
+        let before_ra = undefined;
+        let cur_ra = this.ra;
+        let after_ra = after.ra;
+        let two_after_ra = undefined;
+
+        after_ra = this.loopAngleToBeCloseTo(after_ra, cur_ra);
+
+        if (before)
+            before_ra = this.loopAngleToBeCloseTo(before.ra, cur_ra);
+        if (twoAfter)
+            two_after_ra = this.loopAngleToBeCloseTo(twoAfter.ra, after_ra);
+
+        let before_dist = undefined;
+        let two_after_dist = undefined;
+        if (before)
+            before_dist = before.distance;
+        if (twoAfter)
+            two_after_dist = twoAfter.distance;
+
+        return new DataPacket(
+            this.interpolateValue(alpha, before_ra, cur_ra, after_ra, two_after_ra),
+            this.interpolateValue(alpha, before_dist, this.distance, after.distance, two_after_dist)
+        );
+    }
+
+    private loopAngleToBeCloseTo(to_be_looped: number, to_be_close_to: number): number {
+        if (Math.abs(to_be_close_to - to_be_looped) > Math.PI) {
+            if (to_be_close_to < to_be_looped) {
+                to_be_looped -= 2 * Math.PI;
             }
             else {
-                new_ra += 2 * Math.PI;
+                to_be_looped += 2 * Math.PI;
             }
         }
+        return to_be_looped;
+    }
 
-        return new DataPacket(
-            (alpha * new_ra + (1 - alpha) * old_ra),
-            (alpha * other.distance + (1 - alpha) * this.distance)
-        );
+    /**
+     * Interpolates beteween "curVal" and "afterVal", using beforeVal and twoAfterVal to get the derivatives in order to smooth.
+     * @param alpha Value 0-1, determines how close to this packet(0)
+     *              or the after packet(1) the output should be
+     * @param beforeVal The value before curVal, used to get the derivative for smoothing. Can be undefined.
+     * @param curVal The value to start interpolating from
+     * @param afterVal The value to interpolate to.
+     * @param twoAfterVal The value after afterVal, used to get the derivative for smoothing. Can be undefined.
+     */
+    private interpolateValue(alpha: number, beforeVal: number, curVal: number, afterVal: number, twoAfterVal: number): number {
+        // linear => (alpha * afterVal + (1 - alpha) * curVal)
+
+        let y1 = curVal;
+        let y2 = afterVal;
+
+        let middleDeriv = afterVal - curVal;
+
+        let m1 = middleDeriv;
+        let m2 = middleDeriv;
+
+        if (beforeVal)
+            m1 = ((curVal - beforeVal) + middleDeriv) / 2;
+
+        if (twoAfterVal)
+            m2 = ((twoAfterVal - afterVal) + middleDeriv) / 2;
+
+        let a = m2 + m1 - 2*y2 + 2*y1;
+        let b = -m2 - 2*m1 + 3*y2 - 3*y1;
+        let c = m1;
+        let d = y1;
+
+        return a * Math.pow(alpha, 3) + b * Math.pow(alpha, 2) + c * alpha + d;
     }
 }
 

src/telemetry/telemetry.ts

@@ -69,13 +69,21 @@ export class Telemetry {
             return undefined;
         }
 
-        let beforeKey = this.data.getKey(index - 1);
+        let beforeKey = undefined;
+        if (index >= 2)
+            beforeKey = this.data.getKey(index - 2);
+        let curKey = this.data.getKey(index - 1);
         let afterKey = this.data.getKey(index);
+        let twoAfterKey = undefined;
+        if (index < this.data.numKeys() - 1)
+            twoAfterKey = this.data.getKey(index + 1);
 
         let beforeData = this.data.get(beforeKey);
+        let curData = this.data.get(curKey);
         let afterData = this.data.get(afterKey);
+        let twoAfterData = this.data.get(twoAfterKey);
 
-        return beforeData.interpolate((timestamp - beforeKey) / (afterKey - beforeKey), afterData);
+        return curData.interpolate((timestamp - curKey) / (afterKey - curKey), beforeData, afterData, twoAfterData);
     }
 }
 

src/util/dict.ts

@@ -35,6 +35,10 @@ export class NearestKeyDict<T> {
             return this.findKeyIndexHelper(key, start, i);
         }
     }
+
+    numKeys(): number {
+        return this.keys.length;
+    }
 }
 
 export default NearestKeyDict;

tests/unit/data.spec.ts

@@ -11,13 +11,13 @@ describe('DataPacket', () => {
     it('can interpolate', () => {
         let d0 = new DataPacket(0, 0);
         let d2 = new DataPacket(2, 2);
-        let d1 = d0.interpolate(0.5, d2);
+        let d1 = d0.interpolate(0.5, undefined, d2, undefined);
 
         expect(d1).to.not.be.null;
         expect(d1.ra).to.equal(1);
         expect(d1.distance).to.equal(1);
 
-        let d = d0.interpolate(0.25, d2);
+        let d = d0.interpolate(0.25, undefined, d2, undefined);
 
         expect(d).to.not.be.null;
         expect(d.ra).to.equal(0.5);