@vived/core 1.0.0

package/src/ValueObjects/ParametricLine.ts

@@ -0,0 +1,114 @@
+import { Vector3 } from "../ValueObjects/Vector3";
+
+export interface LineParameters {
+  x0: number;
+  y0: number;
+  z0: number;
+  a: number;
+  b: number;
+  c: number;
+}
+
+export class ParametricLine {
+  // See https://planetcalc.com/8253/
+
+  public static FromTwoPoint(p1: Vector3, p2: Vector3): ParametricLine {
+    const dir = Vector3.Subtract(p2, p1).unit;
+    return this.FromPointDirection(p1, dir);
+  }
+
+  public static FromPointDirection(
+    point: Vector3,
+    direction: Vector3
+  ): ParametricLine {
+    return new ParametricLine({
+      x0: point.x,
+      y0: point.y,
+      z0: point.z,
+      a: direction.x,
+      b: direction.y,
+      c: direction.z,
+    });
+  }
+
+  public static Forward(): ParametricLine {
+    const p = Vector3.Zero();
+    const d = new Vector3(0, 0, 1);
+    return this.FromPointDirection(p, d);
+  }
+
+  public static Backward(): ParametricLine {
+    const p = Vector3.Zero();
+    const d = new Vector3(0, 0, -1);
+    return this.FromPointDirection(p, d);
+  }
+
+  public static Up(): ParametricLine {
+    const p = Vector3.Zero();
+    const d = new Vector3(0, 1, 0);
+    return this.FromPointDirection(p, d);
+  }
+
+  public static Down(): ParametricLine {
+    const p = Vector3.Zero();
+    const d = new Vector3(0, -1, 0);
+    return this.FromPointDirection(p, d);
+  }
+
+  public static Left(): ParametricLine {
+    const p = Vector3.Zero();
+    const d = new Vector3(-1, 0, 0);
+    return this.FromPointDirection(p, d);
+  }
+
+  public static Right(): ParametricLine {
+    const p = Vector3.Zero();
+    const d = new Vector3(1, 0, 0);
+    return this.FromPointDirection(p, d);
+  }
+
+  public static GetPointAtDistance(
+    line: ParametricLine,
+    distance: number
+  ): Vector3 {
+    const x = line.x0 + line.a * distance;
+    const y = line.y0 + line.b * distance;
+    const z = line.z0 + line.c * distance;
+
+    return new Vector3(x, y, z);
+  }
+
+  public static GetDistanceToPoint(
+    line: ParametricLine,
+    point: Vector3
+  ): number {
+    return Vector3.Subtract(line.origin, point).magnitude;
+  }
+
+  readonly x0: number;
+  readonly y0: number;
+  readonly z0: number;
+  readonly a: number;
+  readonly b: number;
+  readonly c: number;
+
+  get origin(): Vector3 {
+    return new Vector3(this.x0, this.y0, this.z0);
+  }
+
+  get direction(): Vector3 {
+    return new Vector3(this.a, this.b, this.c);
+  }
+
+  constructor(parameters: LineParameters) {
+    this.x0 = parameters.x0;
+    this.y0 = parameters.y0;
+    this.z0 = parameters.z0;
+
+    const dir = new Vector3(parameters.a, parameters.b, parameters.c).unit;
+
+    this.a = dir.x;
+    this.b = dir.y;
+    this.c = dir.z;
+  }
+}

package/src/ValueObjects/ParametricPlane.test.ts

@@ -0,0 +1,122 @@
+import { ParametricLine } from "./ParametricLine";
+import { ParametricPlane } from "./ParametricPlane";
+import { Vector3 } from "../ValueObjects";
+
+test("Setting from point and normal", () => {
+  const point = new Vector3(1, 2, 3);
+  const normal = new Vector3(4, 5, 6);
+  const plane = ParametricPlane.FromPointNormal(point, normal);
+
+  const parameters = plane.GetParameters();
+
+  const dot = Vector3.Dot(point, normal);
+  expect(parameters.a).toEqual(normal.x);
+  expect(parameters.b).toEqual(normal.y);
+  expect(parameters.c).toEqual(normal.z);
+  expect(parameters.d).toEqual(-dot);
+});
+
+test("From three points", () => {
+  // From https://keisan.casio.com/exec/system/1223596129
+  const A = new Vector3(1, 2, -2);
+  const B = new Vector3(3, -2, 1);
+  const C = new Vector3(5, 1, -4);
+
+  const plane = ParametricPlane.FromThreePoints(A, B, C);
+  const parameters = plane.GetParameters();
+
+  expect(parameters.a).toEqual(11);
+  expect(parameters.b).toEqual(16);
+  expect(parameters.c).toEqual(14);
+  expect(parameters.d).toEqual(-15);
+});
+
+test("Intersection", () => {
+  const point = new Vector3(1, 1, 1);
+  const dir = new Vector3(1, 0, 0);
+  const line = ParametricLine.FromPointDirection(point, dir);
+
+  const plane1 = new Vector3(-1, 0, 0);
+  const plane2 = new Vector3(-1, 1, 0);
+  const plane3 = new Vector3(-1, 0, 1);
+  const plane = ParametricPlane.FromThreePoints(plane1, plane2, plane3);
+
+  const intersect = plane.intersectLine(line);
+
+  expect(intersect?.x).toBeCloseTo(-1);
+  expect(intersect?.y).toBeCloseTo(1);
+  expect(intersect?.z).toBeCloseTo(1);
+});
+
+test("Intersection", () => {
+  // From http://www.ambrsoft.com/TrigoCalc/Plan3D/PlaneLineIntersection_.htm
+
+  const line1 = new Vector3(4, 1, 1);
+  const line2 = new Vector3(8, -2, -2);
+  const line = ParametricLine.FromTwoPoint(line1, line2);
+
+  const plane1 = new Vector3(-1, 2, 4);
+  const plane2 = new Vector3(2, 1, 3);
+  const plane3 = new Vector3(1, 3, 2);
+  const plane = ParametricPlane.FromThreePoints(plane1, plane2, plane3);
+
+  const intersect = plane.intersectLine(line);
+
+  expect(intersect?.x).toBeCloseTo(2.93);
+  expect(intersect?.y).toBeCloseTo(1.8);
+  expect(intersect?.z).toBeCloseTo(1.8);
+});
+
+test("If parallel, intersect should come back undefined", () => {
+  const line1 = new Vector3(0, 1, 0);
+  const line2 = new Vector3(0, -1, 0);
+  const line = ParametricLine.FromTwoPoint(line1, line2); // Vertical line going through the origin
+
+  const plane1 = new Vector3(0, 0, 0);
+  const plane2 = new Vector3(0, 1, 0);
+  const plane3 = new Vector3(0, 0, 1);
+  const plane = ParametricPlane.FromThreePoints(plane1, plane2, plane3); // y-z plane
+
+  const intersect = plane.intersectLine(line);
+
+  expect(intersect).toBeUndefined();
+});
+
+test("XY Plane", () => {
+  const plane = ParametricPlane.XY();
+  expect(plane.GetParameters()).toEqual({
+    a: 0,
+    b: 0,
+    c: 1,
+    d: 0,
+  });
+});
+
+test("ZX Plane", () => {
+  const plane = ParametricPlane.ZX();
+  expect(plane.GetParameters()).toEqual({
+    a: 0,
+    b: 1,
+    c: 0,
+    d: 0,
+  });
+});
+
+test("YZ Plane", () => {
+  const plane = ParametricPlane.YZ();
+  expect(plane.GetParameters()).toEqual({
+    a: 1,
+    b: 0,
+    c: 0,
+    d: 0,
+  });
+});
+
+test("Check for equality", () => {
+  const point = new Vector3(1, 2, 3);
+  const normal = new Vector3(4, 5, 6);
+  const plane1 = ParametricPlane.FromPointNormal(point, normal);
+  const plane2 = ParametricPlane.FromPointNormal(point, normal);
+
+  expect(plane1).toEqual(plane2);
+});

package/src/ValueObjects/ParametricPlane.ts

@@ -0,0 +1,82 @@
+import { ParametricLine } from "./ParametricLine";
+import { Vector3 } from "../ValueObjects/Vector3";
+
+// Plane equation: a*x + b*y + c*x + d = 0;
+
+export interface PlaneParameters {
+  a: number;
+  b: number;
+  c: number;
+  d: number;
+}
+
+export class ParametricPlane {
+  static FromPointNormal(point: Vector3, normal: Vector3): ParametricPlane {
+    return new ParametricPlane(point, normal);
+  }
+
+  static FromThreePoints(A: Vector3, B: Vector3, C: Vector3): ParametricPlane {
+    const point = A;
+    const AB = Vector3.Subtract(B, A);
+    const AC = Vector3.Subtract(C, A);
+    const normal = Vector3.Cross(AB, AC);
+
+    return new ParametricPlane(point, normal);
+  }
+
+  static XY(): ParametricPlane {
+    const p = Vector3.Zero();
+    const n = new Vector3(0, 0, 1);
+    return new ParametricPlane(p, n);
+  }
+
+  static ZX(): ParametricPlane {
+    const p = Vector3.Zero();
+    const n = new Vector3(0, 1, 0);
+    return new ParametricPlane(p, n);
+  }
+
+  static YZ(): ParametricPlane {
+    const p = Vector3.Zero();
+    const n = new Vector3(1, 0, 0);
+    return new ParametricPlane(p, n);
+  }
+
+  GetParameters(): PlaneParameters {
+    const a = this.normal.x;
+    const b = this.normal.y;
+    const c = this.normal.z;
+    let d = Vector3.Dot(this.point, this.normal);
+
+    if (d !== 0) {
+      d *= -1;
+    }
+
+    return {
+      a,
+      b,
+      c,
+      d,
+    };
+  }
+
+  public intersectLine(line: ParametricLine): Vector3 | undefined {
+    const { x0, y0, z0, a, b, c } = line;
+    const { a: A, b: B, c: C, d: D } = this.GetParameters();
+
+    const tNumerator = -(A * x0 + B * y0 + C * z0 + D);
+    const tDenominator = A * a + B * b + C * c;
+
+    if (tDenominator === 0) {
+      return undefined;
+    }
+
+    const t = tNumerator / tDenominator;
+
+    const intersect = ParametricLine.GetPointAtDistance(line, t);
+
+    return intersect;
+  }
+
+  constructor(private point: Vector3, private normal: Vector3) {}
+}

package/src/ValueObjects/Quaternion.test.ts

@@ -0,0 +1,356 @@
+import { Angle } from "./Angle";
+import { Matrix } from "./Matrix";
+import { Quaternion } from "./Quaternion";
+import { Vector3 } from "./Vector3";
+describe("Quaterion Value Object", () => {
+  it("Checks for equality", () => {
+    const quat1 = new Quaternion(1, 2, 3, 4);
+    const quat2 = new Quaternion(1, 2, 3, 4);
+    const quat3 = new Quaternion(10, 20, 30, 40);
+
+    expect(Quaternion.Equal(quat1, quat2)).toEqual(true);
+    expect(Quaternion.Equal(quat1, quat3)).toEqual(false);
+  });
+
+  it("Constructs a new quaternion from an array", () => {
+    const quat = Quaternion.FromArray([1, 2, 3, 4]);
+    expect(quat.x).toEqual(1);
+    expect(quat.y).toEqual(2);
+    expect(quat.z).toEqual(3);
+    expect(quat.w).toEqual(4);
+  });
+
+  it("Constructs an identity quaternion", () => {
+    const quat = Quaternion.Identity();
+    expect(quat.x).toEqual(0);
+    expect(quat.y).toEqual(0);
+    expect(quat.z).toEqual(0);
+    expect(quat.w).toEqual(1);
+  });
+
+  it("Equates two quaternions for testing", () => {
+    const quat1 = new Quaternion(1, 2, 3, 4);
+    const quat2 = new Quaternion(1, 2, 3, 4);
+
+    expect(quat1).toEqual(quat2);
+  });
+
+  it("Returns an array", () => {
+    const quat1 = new Quaternion(1, 2, 3, 4);
+    expect(quat1.toArray()).toEqual([1, 2, 3, 4]);
+  });
+
+  it("Multiplies two quaternions", () => {
+    // See https://www.omnicalculator.com/math/quaternion
+    const quat1 = new Quaternion(1, 2, 3, 4);
+    const quat2 = new Quaternion(5, 6, 7, 8);
+    const product = Quaternion.Multiply(quat1, quat2);
+    const expected = new Quaternion(24, 48, 48, -6);
+    expect(product).toEqual(expected);
+  });
+
+  it("Inverts a quaternion", () => {
+    const quat1 = new Quaternion(1, 2, 3, 4);
+    const inverted = Quaternion.Inverse(quat1);
+    const expected = new Quaternion(-1, -2, -3, 4);
+    expect(inverted).toEqual(expected);
+  });
+
+  it("Creates an Quaterion from an Axis and Angle", () => {
+    // See https://www.andre-gaschler.com/rotationconverter
+    const axis = new Vector3(1, 2, 3);
+    const angle = Angle.FromRadians(4);
+    const quat = Quaternion.FromAngleAxis(axis, angle);
+
+    expect(quat.x).toBeCloseTo(0.24302);
+    expect(quat.y).toBeCloseTo(0.4860399);
+    expect(quat.z).toBeCloseTo(0.7290599);
+    expect(quat.w).toBeCloseTo(-0.4161468);
+  });
+
+  it("Creates an Quaterion from a Yaw Pitch Roll", () => {
+    // Numbers are from the babylon playground
+
+    const yaw = Angle.FromRadians(1);
+    const pitch = Angle.FromRadians(2);
+    const roll = Angle.FromRadians(3);
+    const quat = Quaternion.FromYawPitchRoll(yaw, pitch, roll);
+
+    expect(quat.x).toBeCloseTo(0.310622);
+    expect(quat.y).toBeCloseTo(-0.71828);
+    expect(quat.z).toBeCloseTo(0.444435);
+    expect(quat.w).toBeCloseTo(0.4359528);
+  });
+
+  it("Creates an Quaterion from Euler angles", () => {
+    // Numbers are from the babylon playground
+
+    const x = Angle.FromRadians(1);
+    const y = Angle.FromRadians(2);
+    const z = Angle.FromRadians(3);
+    const quat = Quaternion.FromEuler(x, y, z);
+
+    expect(quat.x).toBeCloseTo(0.754933);
+    expect(quat.y).toBeCloseTo(-0.206149);
+    expect(quat.z).toBeCloseTo(0.444435);
+    expect(quat.w).toBeCloseTo(0.4359528);
+  });
+
+  it("Slerps", () => {
+    // See https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
+    const q1 = new Quaternion(0.1, 0.2, 0.3, 0.9);
+    const q2 = new Quaternion(0.5, 0.8, 0.1, 0.4);
+
+    const initial = Quaternion.Slerp(q1, q2, 0);
+    expect(initial.x).toBeCloseTo(q1.x);
+    expect(initial.y).toBeCloseTo(q1.y);
+    expect(initial.z).toBeCloseTo(q1.z);
+    expect(initial.w).toBeCloseTo(q1.w);
+
+    const final = Quaternion.Slerp(q1, q2, 1);
+    expect(final.x).toBeCloseTo(q2.x);
+    expect(final.y).toBeCloseTo(q2.y);
+    expect(final.z).toBeCloseTo(q2.z);
+    expect(final.w).toBeCloseTo(q2.w);
+
+    const middle = Quaternion.Slerp(q1, q2, 0.35);
+    expect(middle.x).toBeCloseTo(0.27016);
+    expect(middle.y).toBeCloseTo(0.46061);
+    expect(middle.z).toBeCloseTo(0.25244);
+    expect(middle.w).toBeCloseTo(0.7972);
+  });
+
+  it("Makes a quaterion from a rotation matrix", () => {
+    const x = Angle.FromRadians(1);
+    const y = Angle.FromRadians(2);
+    const z = Angle.FromRadians(3);
+    const originalQ = Quaternion.FromEuler(x, y, z);
+
+    const matrix = Matrix.Compose(Vector3.One(), originalQ, Vector3.Zero());
+
+    const result = Quaternion.FromRotationMatrix(matrix);
+    expect(result.x).toBeCloseTo(originalQ.x);
+    expect(result.y).toBeCloseTo(originalQ.y);
+    expect(result.z).toBeCloseTo(originalQ.z);
+    expect(result.w).toBeCloseTo(originalQ.w);
+  });
+
+  it("Checks if two quaternions are close", () => {
+    const a = new Quaternion(1, 2, 3, 4);
+    const bigger = new Quaternion(1.01, 2.01, 3.01, 4.01);
+    const smaller = new Quaternion(0.99, 1.99, 2.99, 3.99);
+
+    expect(Quaternion.Close(a, bigger, 0.0101)).toEqual(true);
+    expect(Quaternion.Close(a, bigger, 0.0099)).toEqual(false);
+
+    expect(Quaternion.Close(a, smaller, 0.0101)).toEqual(true);
+    expect(Quaternion.Close(a, smaller, 0.0099)).toEqual(false);
+  });
+
+  it("Retuns the angle", () => {
+    const angle = Angle.FromDegrees(25);
+    const axis = new Vector3(1, 2, 3);
+    const q = Quaternion.FromAngleAxis(axis, angle);
+
+    expect(Angle.Close(q.angle, angle)).toEqual(true);
+  });
+
+  it("Retuns the axis", () => {
+    const angle = Angle.FromDegrees(25);
+    const axis = new Vector3(1, 2, 3);
+    const q = Quaternion.FromAngleAxis(axis, angle);
+
+    expect(Vector3.Close(q.axis, axis.unit)).toEqual(true);
+  });
+
+  it("Makes a quaternion into a rotation matrix", () => {
+    const x = Angle.FromRadians(1);
+    const y = Angle.FromRadians(2);
+    const z = Angle.FromRadians(3);
+    const originalQ = Quaternion.FromEuler(x, y, z);
+
+    const mat = Quaternion.ToRotationMatrix(originalQ);
+
+    expect(Quaternion.Close(mat.rotation, originalQ)).toEqual(true);
+    expect(Vector3.Close(mat.scale, Vector3.One())).toEqual(true);
+    expect(Vector3.Close(mat.translation, Vector3.Zero())).toEqual(true);
+  });
+
+  it("Gets the Data Transfer Object", () => {
+    const quat = new Quaternion(1, 2, 3, 4);
+
+    expect(quat.dto).toEqual({
+      x: 1,
+      y: 2,
+      z: 3,
+      w: 4,
+    });
+  });
+
+  it("Makes a Quaterion from a DTO", () => {
+    const quat = Quaternion.FromDTO({
+      x: 1,
+      y: 2,
+      z: 3,
+      w: 4,
+    });
+
+    expect(quat.x).toEqual(1);
+    expect(quat.y).toEqual(2);
+    expect(quat.z).toEqual(3);
+    expect(quat.w).toEqual(4);
+  });
+
+  it("Forms a quaternion from a forward direction", () => {
+    const dir = new Vector3(0, 0, 1);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.Identity();
+
+    expect(q).toEqual(expectedQ);
+  });
+
+  it("Forms a quaternion from a right direction", () => {
+    const dir = new Vector3(1, 0, 0);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(90),
+      Angle.FromRadians(0),
+      Angle.FromDegrees(0)
+    );
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Forms a quaternion from a left direction", () => {
+    const dir = new Vector3(-1, 0, 0);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(-90),
+      Angle.FromRadians(0),
+      Angle.FromDegrees(0)
+    );
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Forms a quaternion from a behind direction", () => {
+    const dir = new Vector3(0, 0, -1);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(180),
+      Angle.FromRadians(0),
+      Angle.FromDegrees(0)
+    );
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Forms a quaternion from a 45 degree up direction", () => {
+    const dir = new Vector3(0, 1, 1);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(-45),
+      Angle.FromDegrees(0)
+    );
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Forms a quaternion from a 45 degree up direction", () => {
+    const dir = new Vector3(1, 1, 0);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(90),
+      Angle.FromDegrees(-45),
+      Angle.FromDegrees(0)
+    );
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Returns an identity if the direction is zero", () => {
+    const dir = new Vector3(0, 0, 0);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.Identity();
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Works with an up vector", () => {
+    const dir = new Vector3(0, 1, 0);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(-90),
+      Angle.FromDegrees(0)
+    );
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Works with an down vector", () => {
+    const dir = new Vector3(0, -1, 0);
+    const q = Quaternion.FromDirectionVector(dir);
+
+    const expectedQ = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(90),
+      Angle.FromDegrees(0)
+    );
+
+    expect(Quaternion.Close(q, expectedQ)).toEqual(true);
+  });
+
+  it("Gets the angle between two quaternions", () => {
+    const q1 = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(90),
+      Angle.FromDegrees(0)
+    );
+
+    const q2 = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(80),
+      Angle.FromDegrees(0)
+    );
+
+    const ang = Quaternion.AngleBetween(q1, q2);
+    expect(ang.degrees).toBeCloseTo(10);
+  });
+
+  it("Keeps the angle between two quaternions less than 180", () => {
+    const q1 = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(0)
+    );
+
+    const q2 = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(200),
+      Angle.FromDegrees(0)
+    );
+
+    const ang = Quaternion.AngleBetween(q1, q2);
+    expect(ang.degrees).toBeCloseTo(160);
+  });
+
+  it("Returns 0 if the quaterions are the same", () => {
+    const q1 = Quaternion.FromYawPitchRoll(
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(0),
+      Angle.FromDegrees(0)
+    );
+    const ang = Quaternion.AngleBetween(q1, q1);
+    expect(ang.degrees).toEqual(0);
+  });
+});