implementation of p5.Vector slerp function

src/math/p5.Vector.js

@@ -1742,6 +1742,156 @@ p5.Vector = class {
     return this;
   }
 
+  /**
+ * Performs spherical linear interpolation with the other vector
+ * and returns the resulting vector.
+ * The result of slerping between 2D vectors is always a 2D vector.
+ *
+ * @method slerp
+ * @param {p5.Vector} v the p5.Vector to slerp to
+ * @param {Number} amt The amount of interpolation. some value between 0.0
+ *                     (old vector) and 1.0 (new vector). 0.9 is very near
+ *                     the new vector. 0.5 is halfway in between.
+ * @return {p5.Vector}
+ *
+ * @example
+ * <div class="norender">
+ * <code>
+ *
+ * const v1 = createVector(1, 0, 0);
+ * const v2 = createVector(0, 1, 0);
+ *
+ * const v = v1.slerp(v2, 1/3);
+ * print(v.toString());
+ * // v's components are almost [cos(30°), sin(30°), 0]
+ * </code>
+ * </div>
+ *
+ * <div>
+ * <code>
+ * let needle;
+ * function setup() {
+ *   createCanvas(100, 100);
+ *   stroke(0);
+ *   strokeWeight(4);
+ *
+ *   needle = createVector(50, 0);
+ * }
+ *
+ * function draw(){
+ *   background(255);
+ *   translate(50, 50);
+ *
+ *   const theta = Math.atan2(mouseY-50, mouseX-50);
+ *   const v = createVector(50 * Math.cos(theta), 50 * Math.sin(theta));
+ *   // slerp between v and needle
+ *   // needle vector is changed by slerp function.
+ *   needle.slerp(v, 0.05);
+ *
+ *   line(0, 0, needle.x, needle.y);
+ * }
+ * </code>
+ * </div>
+ *
+ * <div>
+ * <code>
+ * let v1, v2, v3;
+ * function setup(){
+ *   createCanvas(100, 100, WEBGL);
+ *   noStroke();
+ *   v1 = createVector(30, 0, 0);
+ *   v2 = createVector(0, 30, 0);
+ *   v3 = createVector(0, 0, 30);
+ * }
+ *
+ * function draw(){
+ *   background(0);
+ *   lights();
+ *   fill(255);
+ *   ambientMaterial(255);
+ *
+ *   const t = (frameCount % 60) / 60;
+ *   // v1, v2, v3 is not changed by slerp function.
+ *   // because this function is static version.
+ *   const v4 = p5.Vector.slerp(v1, v2, t);
+ *   const v5 = p5.Vector.slerp(v2, v3, t);
+ *   const v6 = p5.Vector.slerp(v3, v1, t);
+ *   translate(v4.x, v4.y, v4.z);
+ *   sphere(5);
+ *   translate(v5.x - v4.x, v5.y - v4.y, v5.z - v4.z);
+ *   sphere(5);
+ *   translate(v6.x - v5.x, v6.y - v5.y, v6.z - v5.z);
+ *   sphere(5);
+ * }
+ * </code>
+ * </div>
+ */
+  slerp(v, amt) {
+    // edge cases.
+    if (amt === 0) { return this; }
+    if (amt === 1) { return this.set(v); }
+
+    // calculate magnitudes
+    const selfMag = this.mag();
+    const vMag = v.mag();
+    const magmag = selfMag * vMag;
+    // if either is a zero vector, linearly interpolate by these vectors
+    if (magmag === 0) {
+      this.mult(1 - amt).add(v.x * amt, v.y * amt, v.z * amt);
+      return this;
+    }
+    // the cross product of 'this' and 'v' is the axis of rotation
+    const axis = this.cross(v);
+    const axisMag = axis.mag();
+    // Calculates the angle between 'this' and 'v'
+    const theta = Math.atan2(axisMag, this.dot(v));
+
+    // However, if the norm of axis is 0, normalization cannot be performed,
+    // so we will divide the cases
+    if (axisMag > 0) {
+      axis.x /= axisMag;
+      axis.y /= axisMag;
+      axis.z /= axisMag;
+    } else if (theta < Math.PI * 0.5) {
+      // if the norm is 0 and the angle is less than PI/2,
+      // the angle is very close to 0, so do linear interpolation.
+      this.mult(1 - amt).add(v.x * amt, v.y * amt, v.z * amt);
+      return this;
+    } else {
+      // If the norm is 0 and the angle is more than PI/2, the angle is
+      // very close to PI.
+      // In this case v can be regarded as '-this', so take any vector
+      // that is orthogonal to 'this' and use that as the axis.
+      if (this.z === 0 && v.z === 0) {
+        // if both this and v are 2D vectors, use (0,0,1)
+        // this makes the result also a 2D vector.
+        axis.set(0, 0, 1);
+      } else if (this.x !== 0) {
+        // if the x components is not 0, use (y, -x, 0)
+        axis.set(this.y, -this.x, 0).normalize();
+      } else {
+        // if the x components is 0, use (1,0,0)
+        axis.set(1, 0, 0);
+      }
+    }
+
+    // Since 'axis' is a unit vector, ey is a vector of the same length as 'result'.
+    const ey = axis.cross(this);
+    // interpolate the length with 'this' and 'v'.
+    const lerpedMagFactor = (1 - amt) + amt * vMag / selfMag;
+    // imagine an orthonormal basis where "axis", "result" and "ey" are
+    // the unit vectors of the z, x and y axes respectively.
+    // rotates "result" around "axis" by t*angle towards "ey".
+    const cosMultiplier = lerpedMagFactor * Math.cos(amt * theta);
+    const sinMultiplier = lerpedMagFactor * Math.sin(amt * theta);
+    // then, calculate 'result'.
+    this.x = this.x * cosMultiplier + ey.x * sinMultiplier;
+    this.y = this.y * cosMultiplier + ey.y * sinMultiplier;
+    this.z = this.z * cosMultiplier + ey.z * sinMultiplier;
+
+    return this;
+  }
+
   /**
  * Reflect a vector about a normal to a line in 2D, or about a normal to a
  * plane in 3D.
@@ -2358,6 +2508,36 @@ p5.Vector = class {
     return target;
   }
 
+  /**
+ * Performs spherical linear interpolation with the other vector
+ * and returns the resulting vector.
+ * The result of slerping between 2D vectors is always a 2D vector.
+ */
+  /**
+ * @method slerp
+ * @static
+ * @param {p5.Vector} v1 old vector
+ * @param {p5.Vector} v2 new vectpr
+ * @param {Number} amt
+ * @param {p5.Vector} [target] The vector to receive the result
+ * @return {p5.Vector} slerped vector between v1 and v2
+ */
+  static slerp(v1, v2, amt, target) {
+    if (!target) {
+      target = v1.copy();
+      if (arguments.length === 4) {
+        p5._friendlyError(
+          'The target parameter is undefined, it should be of type p5.Vector',
+          'p5.Vector.slerp'
+        );
+      }
+    } else {
+      target.set(v1);
+    }
+    target.slerp(v2, amt);
+    return target;
+  }
+
   /**
  * Calculates the magnitude (length) of the vector and returns the result as
  * a float (this is simply the equation `sqrt(x*x + y*y + z*z)`.)

test/unit/math/p5.Vector.js

@@ -1422,6 +1422,100 @@ suite('p5.Vector', function() {
     });
   });
 
+  suite('v.slerp(w, amt)', function() {
+    var w;
+    setup(function() {
+      v.set(1, 2, 3);
+      w = new p5.Vector(4, 6, 8);
+    });
+
+    test('if amt is 0, returns original vector', function() {
+      v.slerp(w, 0);
+      expect(v.x).to.eql(1);
+      expect(v.y).to.eql(2);
+      expect(v.z).to.eql(3);
+    });
+
+    test('if amt is 1, returns argument vector', function() {
+      v.slerp(w, 1);
+      expect(v.x).to.eql(4);
+      expect(v.y).to.eql(6);
+      expect(v.z).to.eql(8);
+    });
+
+    test('if both v and w are 2D, then result will also be 2D.', function() {
+      v.set(2, 3, 0);
+      w.set(3, -2, 0);
+      v.slerp(w, 0.3);
+      expect(v.z).to.eql(0);
+
+      v.set(1, 4, 0);
+      w.set(-1, -4, 0);
+      v.slerp(w, 0.8);
+      expect(v.z).to.eql(0);
+    });
+
+    test('if one side is a zero vector, linearly interpolate.', function() {
+      v.set(0, 0, 0);
+      w.set(2, 4, 6);
+      v.slerp(w, 0.5);
+      expect(v.x).to.eql(1);
+      expect(v.y).to.eql(2);
+      expect(v.z).to.eql(3);
+    });
+
+    test('If they are pointing in the same direction, linearly interpolate.', function() {
+      v.set(5, 11, 16);
+      w.set(15, 33, 48);
+      v.slerp(w, 0.5);
+      expect(v.x).to.eql(10);
+      expect(v.y).to.eql(22);
+      expect(v.z).to.eql(32);
+    });
+  });
+
+  suite('p5.Vector.slerp(v1, v2, amt)', function() {
+    var res, v1, v2;
+    setup(function() {
+      v1 = new p5.Vector(1, 0, 0);
+      v2 = new p5.Vector(0, 0, 1);
+      res = p5.Vector.slerp(v1, v2, 1/3);
+    });
+
+    test('should not be undefined', function() {
+      expect(res).to.not.eql(undefined);
+    });
+
+    test('should be a p5.Vector', function() {
+      expect(res).to.be.an.instanceof(p5.Vector);
+    });
+
+    test('should return neither v1 nor v2', function() {
+      expect(res).to.not.eql(v1);
+      expect(res).to.not.eql(v2);
+    });
+
+    test('Make sure the interpolation in 1/3 is correct', function() {
+      expect(res.x).to.be.closeTo(Math.cos(Math.PI/6), 0.00001);
+      expect(res.y).to.be.closeTo(0, 0.00001);
+      expect(res.z).to.be.closeTo(Math.sin(Math.PI/6), 0.00001);
+    });
+
+    test('Make sure the interpolation in -1/3 is correct', function() {
+      p5.Vector.slerp(v1, v2, -1/3, res);
+      expect(res.x).to.be.closeTo(Math.cos(-Math.PI/6), 0.00001);
+      expect(res.y).to.be.closeTo(0, 0.00001);
+      expect(res.z).to.be.closeTo(Math.sin(-Math.PI/6), 0.00001);
+    });
+
+    test('Make sure the interpolation in 5/3 is correct', function() {
+      p5.Vector.slerp(v1, v2, 5/3, res);
+      expect(res.x).to.be.closeTo(Math.cos(5*Math.PI/6), 0.00001);
+      expect(res.y).to.be.closeTo(0, 0.00001);
+      expect(res.z).to.be.closeTo(Math.sin(5*Math.PI/6), 0.00001);
+    });
+  });
+
   suite('p5.Vector.fromAngle(angle)', function() {
     var res, angle;
     setup(function() {