mapillary-js/test/geo/Spatial.test.ts
2021-03-29 10:35:45 +02:00

718 lines
20 KiB
TypeScript

import * as THREE from "three";
import { Spatial } from "../../src/geo/Spatial";
let precision: number = 5;
let epsilon: number = 1e-8;
describe("Spatial.rotationMatrix", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should return a rotation matrix rotating 90 degrees around the x-axis", () => {
let angleAxis: number[] = [Math.PI / 2, 0, 0];
let matrix: THREE.Matrix4 = spatial.rotationMatrix(angleAxis);
let elements: number[] = matrix.elements;
// elements is a column-major list
expect(elements[0]).toBe(1);
expect(elements[1]).toBe(0);
expect(elements[2]).toBe(0);
expect(elements[3]).toBe(0);
expect(elements[4]).toBe(0);
expect(elements[5]).toBeLessThan(epsilon);
expect(elements[6]).toBe(1);
expect(elements[7]).toBe(0);
expect(elements[8]).toBe(0);
expect(elements[9]).toBe(-1);
expect(elements[10]).toBeLessThan(epsilon);
expect(elements[11]).toBe(0);
expect(elements[12]).toBe(0);
expect(elements[13]).toBe(0);
expect(elements[14]).toBe(0);
expect(elements[15]).toBe(1);
});
});
describe("Spatial.azimuthalToBearing", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should convert correctly", () => {
expect(spatial.azimuthalToBearing(0)).toBeCloseTo(Math.PI / 2, precision);
expect(spatial.azimuthalToBearing(Math.PI / 2)).toBeCloseTo(0, precision);
expect(spatial.azimuthalToBearing(Math.PI)).toBeCloseTo(-Math.PI / 2, precision);
expect(spatial.azimuthalToBearing(3 * Math.PI / 2)).toBeCloseTo(-Math.PI, precision);
expect(spatial.azimuthalToBearing(-Math.PI / 4)).toBeCloseTo(3 * Math.PI / 4, precision);
});
});
describe("Spatial.rotate", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should return a vector rotated 90 degrees around the x-axis", () => {
let vector: number[] = [0, 0, 1];
let angleAxis: number[] = [Math.PI / 2, 0, 0];
let rotated: THREE.Vector3 = spatial.rotate(vector, angleAxis);
// counter-clockwise rotation about the x-axis pointing towards the observer
expect(rotated.x).toBe(0);
expect(rotated.y).toBe(-1);
expect(rotated.z).toBeLessThan(epsilon);
});
});
describe("Spatial.opticalCenter", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should return the correct optical center", () => {
let C: number[] = [1, 0, 0];
// random rotation by 120 degrees
let r: THREE.Vector3 = new THREE.Vector3(1, 1, 1).normalize().multiplyScalar(2 * Math.PI / 3);
let R: THREE.Matrix4 = spatial.rotationMatrix(r.toArray());
// t = -RC
let t: THREE.Vector3 = new THREE.Vector3().fromArray(C).applyMatrix4(R).multiplyScalar(-1);
let opticalCenter: THREE.Vector3 = spatial.opticalCenter(r.toArray(), t.toArray());
expect(opticalCenter.x).toBeCloseTo(C[0], precision);
expect(opticalCenter.y).toBeCloseTo(C[1], precision);
expect(opticalCenter.z).toBeCloseTo(C[2], precision);
});
it("should return the correct optical center", () => {
let C: number[] = [54, 22, -34];
// random rotation by 60 degrees
let r: THREE.Vector3 = new THREE.Vector3(-1, 1, -2).normalize().multiplyScalar(Math.PI / 3);
let R: THREE.Matrix4 = spatial.rotationMatrix(r.toArray());
// t = -RC
let t: THREE.Vector3 = new THREE.Vector3().fromArray(C).applyMatrix4(R).multiplyScalar(-1);
let opticalCenter: THREE.Vector3 = spatial.opticalCenter(r.toArray(), t.toArray());
expect(opticalCenter.x).toBeCloseTo(C[0], precision);
expect(opticalCenter.y).toBeCloseTo(C[1], precision);
expect(opticalCenter.z).toBeCloseTo(C[2], precision);
});
});
describe("Spatial.viewingDirection", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should return a viewing direction in the x-axis direction", () => {
let rotation: number[] = [0, -Math.PI / 2, 0];
let viewingDirection: THREE.Vector3 = spatial.viewingDirection(rotation);
// counter-clockwise rotation about the y-axis pointing towards the observer
expect(viewingDirection.x).toBeCloseTo(1, precision);
expect(viewingDirection.y).toBeCloseTo(0, precision);
expect(viewingDirection.z).toBeCloseTo(0, precision);
});
});
describe("Spatial.wrap", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should not wrap when in interval", () => {
let value: number = 0;
let min: number = -1;
let max: number = 1;
let result: number = spatial.wrap(value, min, max);
expect(result).toBe(value);
});
it("should wrap when below interval", () => {
let value: number = -2.5;
let min: number = -1;
let max: number = 1;
let result: number = spatial.wrap(value, min, max);
expect(result).toBe(-0.5);
});
it("should wrap when above interval", () => {
let value: number = 2;
let min: number = -1;
let max: number = 1;
let result: number = spatial.wrap(value, min, max);
expect(result).toBe(0);
});
it("should wrap repeatedly until within interval when above", () => {
let value: number = 30;
let min: number = -1;
let max: number = 1;
let result: number = spatial.wrap(value, min, max);
expect(result).toBe(0);
});
it("should wrap repeatedly until within interval when below", () => {
let value: number = -54.5;
let min: number = -1;
let max: number = 1;
let result: number = spatial.wrap(value, min, max);
expect(result).toBe(-0.5);
});
});
describe("Spatial.wrap", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be equal to itself when it is zero", () => {
let angle: number = 0;
let result: number = spatial.wrapAngle(angle);
expect(result).toBe(angle);
});
it("should be equal to itself when it is below or equal to Pi", () => {
let angle: number = Math.PI;
let result: number = spatial.wrapAngle(angle);
expect(result).toBe(angle);
});
it("should be equal to itself when it is above or equal to minus Pi", () => {
let angle: number = -Math.PI;
let result: number = spatial.wrapAngle(angle);
expect(result).toBe(angle);
});
it("should be wrapped by two Pi when it is bigger than Pi", () => {
let angle: number = 3 / 2 * Math.PI;
let result: number = spatial.wrapAngle(angle);
expect(result).toBe(angle - 2 * Math.PI);
});
it("should be wrapped by two Pi when it is smaller than Pi", () => {
let angle: number = - 3 / 2 * Math.PI;
let result: number = spatial.wrapAngle(angle);
expect(result).toBe(angle + 2 * Math.PI);
});
});
describe("Spatial.clamp", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be equal to itself when it is inside the interval", () => {
let value: number = 0.5;
let min: number = 0;
let max: number = 1;
let result: number = spatial.clamp(value, min, max);
expect(result).toBe(value);
});
it("should be clamped to min smaller than min", () => {
let value: number = -1;
let min: number = 0;
let max: number = 1;
let result: number = spatial.clamp(value, min, max);
expect(result).toBe(min);
});
it("should be clamped to max larger than max", () => {
let value: number = 2;
let min: number = 0;
let max: number = 1;
let result: number = spatial.clamp(value, min, max);
expect(result).toBe(max);
});
});
describe("Spatial.angleBetweenVector2", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be zero", () => {
let v1: number[] = [1, 1];
let v2: number[] = [1, 1];
let result: number = spatial.angleBetweenVector2(v1[0], v1[1], v2[0], v2[1]);
expect(result).toBe(0);
});
it("should be 90 degrees", () => {
let v1: number[] = [1, 1];
let v2: number[] = [-1, 1];
let result: number = spatial.angleBetweenVector2(v1[0], v1[1], v2[0], v2[1]);
expect(result).toBeCloseTo(Math.PI / 2, precision);
});
it("should be minus 90 degrees", () => {
let v1: number[] = [-1, 1];
let v2: number[] = [1, 1];
let result: number = spatial.angleBetweenVector2(v1[0], v1[1], v2[0], v2[1]);
expect(result).toBeCloseTo(-Math.PI / 2, precision);
});
it("should be minus 45 degrees", () => {
let v1: number[] = [1, -1];
let v2: number[] = [0, -1];
let result: number = spatial.angleBetweenVector2(v1[0], v1[1], v2[0], v2[1]);
expect(result).toBeCloseTo(-Math.PI / 4, precision);
});
it("should be 180 degrees", () => {
let v1: number[] = [-1, 0];
let v2: number[] = [1, 0];
let result: number = spatial.angleBetweenVector2(v1[0], v1[1], v2[0], v2[1]);
expect(Math.abs(result)).toBeCloseTo(Math.PI, precision);
});
});
describe("Spatial.relativeRotationAngle", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be 0 degrees when there is no rotation", () => {
let rotation1: number[] = [0, 0, 0];
let rotation2: number[] = [0, 0, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(0, precision);
});
it("should be 0 degrees when rotation is the same", () => {
let rotation1: number[] = [Math.PI / 2, 0, 0];
let rotation2: number[] = [Math.PI / 2, 0, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(0, precision);
});
it("should be 45 degrees when rotating 45 degrees", () => {
let rotation1: number[] = [Math.PI / 4, 0, 0];
let rotation2: number[] = [Math.PI / 2, 0, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI / 4, precision);
});
it("should be 90 degrees when rotating 90 degrees around X-axis", () => {
let rotation1: number[] = [Math.PI / 2, 0, 0];
let rotation2: number[] = [Math.PI, 0, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI / 2, precision);
});
it("should be 90 degrees when rotating 90 degrees around Y-axis", () => {
let rotation1: number[] = [0, Math.PI / 2, 0];
let rotation2: number[] = [0, Math.PI, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI / 2, precision);
});
it("should be 90 degrees when rotating 90 degrees around Z-axis", () => {
let rotation1: number[] = [0, 0, Math.PI / 2];
let rotation2: number[] = [0, 0, Math.PI];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI / 2, precision);
});
it("should be 180 degrees when rotating 180 degrees", () => {
let rotation1: number[] = [0, 0, 0];
let rotation2: number[] = [Math.PI, 0, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI, precision);
});
it("should be 90 degrees when rotating 90 degrees in negative direction", () => {
let rotation1: number[] = [0, 0, 0];
let rotation2: number[] = [-Math.PI / 2, 0, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI / 2, precision);
});
it("should be 180 degrees when rotating 180 degrees in negative direction", () => {
let rotation1: number[] = [0, 0, 0];
let rotation2: number[] = [-Math.PI, 0, 0];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI, precision);
});
it("should be 90 degrees when rotating 90 degrees in general direction", () => {
let k1: number = Math.PI / Math.sqrt(3);
let k2: number = Math.PI / Math.sqrt(12);
let rotation1: number[] = [k1, k1, k1];
let rotation2: number[] = [k2, k2, k2];
let theta: number = spatial.relativeRotationAngle(rotation1, rotation2);
expect(theta).toBeCloseTo(Math.PI / 2, precision);
});
});
describe("Spatial.angleDifference", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be 0 degrees when angles are zero", () => {
let angle1: number = 0;
let angle2: number = 0;
let result: number = spatial.angleDifference(angle1, angle2);
expect(result).toBeCloseTo(0, precision);
});
it("should be 0 degrees when angles are equal", () => {
let angle1: number = - 3 * Math.PI / 4;
let angle2: number = - 3 * Math.PI / 4;
let result: number = spatial.angleDifference(angle1, angle2);
expect(result).toBeCloseTo(0, precision);
});
it("should be 45 degrees", () => {
let angle1: number = Math.PI / 4;
let angle2: number = Math.PI / 2;
let result: number = spatial.angleDifference(angle1, angle2);
expect(result).toBeCloseTo(Math.PI / 4, precision);
});
it("should be minus 45 degrees", () => {
let angle1: number = Math.PI / 2;
let angle2: number = Math.PI / 4;
let result: number = spatial.angleDifference(angle1, angle2);
expect(result).toBeCloseTo(-Math.PI / 4, precision);
});
it("should be 135 degrees", () => {
let angle1: number = Math.PI / 4;
let angle2: number = Math.PI;
let result: number = spatial.angleDifference(angle1, angle2);
expect(result).toBeCloseTo(3 * Math.PI / 4, precision);
});
it("should be 45 degress when passing PI", () => {
let angle1: number = 7 * Math.PI / 8;
let angle2: number = -7 * Math.PI / 8;
let result: number = spatial.angleDifference(angle1, angle2);
expect(result).toBeCloseTo(Math.PI / 4, precision);
});
it("should be minus 45 degress when passing PI", () => {
let angle1: number = -7 * Math.PI / 8;
let angle2: number = 7 * Math.PI / 8;
let result: number = spatial.angleDifference(angle1, angle2);
expect(result).toBeCloseTo(-Math.PI / 4, precision);
});
it("should be 180 degress", () => {
let angle1: number = -7 * Math.PI / 8;
let angle2: number = 1 * Math.PI / 8;
let result: number = spatial.angleDifference(angle1, angle2);
expect(Math.abs(result)).toBeCloseTo(Math.PI, precision);
});
});
describe("Spatial.angleToPlane", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be 0 degrees when the vector lies in the XY plane", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [1, 1, 0];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(0, precision);
});
it("should be 0 degrees when the vector lies in the XY plane", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [-1, -1, 0];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(0, precision);
});
it("should be 90 degrees when the vector is orthogonal to the XY plane", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [0, 0, 1];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(Math.PI / 2, precision);
});
it("should be minus 90 degrees when the vector is orthogonal to the XY plane", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [0, 0, -1];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(-Math.PI / 2, precision);
});
it("should be 45 degrees", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [-1, 0, 1];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(Math.PI / 4, precision);
});
it("should be 45 degrees", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [0, 1, 1];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(Math.PI / 4, precision);
});
it("should be minus 45 degrees", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [1, 0, -1];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(-Math.PI / 4, precision);
});
it("should be minus 45 degrees", () => {
let n: number[] = [0, 0, 1];
let v: number[] = [0, -1, -1];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(-Math.PI / 4, precision);
});
it("should be 45 degrees", () => {
let n: number[] = [0, 1, 0];
let v: number[] = [1, 1, 0];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(Math.PI / 4, precision);
});
it("should be 0 degrees for null vector", () => {
let n: number[] = [0, 1, 0];
let v: number[] = [0, 0, 0];
let theta: number = spatial.angleToPlane(v, n);
expect(theta).toBeCloseTo(0, precision);
});
});
describe("Spatial.degToRad", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be 0 radiansa when 0 degress", () => {
let deg: number = 0;
let rad: number = spatial.degToRad(deg);
expect(rad).toBeCloseTo(0, precision);
});
it("should be PI / 2 radians when 90 degress", () => {
let deg: number = 90;
let rad: number = spatial.degToRad(deg);
expect(rad).toBeCloseTo(Math.PI / 2, precision);
});
it("should be minus PI / 2 radians when minus 90 degress", () => {
let deg: number = -90;
let rad: number = spatial.degToRad(deg);
expect(rad).toBeCloseTo(-Math.PI / 2, precision);
});
});
describe("Spatial.radToDeg", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be 0 degrees when 0 radians", () => {
let rad: number = 0;
let deg: number = spatial.radToDeg(rad);
expect(deg).toBeCloseTo(0, precision);
});
it("should be 90 degrees when PI / 2 radians", () => {
let rad: number = Math.PI / 2;
let deg: number = spatial.radToDeg(rad);
expect(deg).toBeCloseTo(90, precision);
});
it("should be minus 90 degrees when minus PI / 2 radians", () => {
let rad: number = -Math.PI / 2;
let deg: number = spatial.radToDeg(rad);
expect(deg).toBeCloseTo(-90, precision);
});
});
describe("Spatial.distanceFromLatLon", () => {
let spatial: Spatial;
beforeEach(() => {
spatial = new Spatial();
});
it("should be approximately 100 meters for Bredgatan", () => {
let lat1: number = 55.60804;
let lng1: number = 13.01846;
let lat2: number = 55.60719;
let lng2: number = 13.01788;
let distance: number = spatial
.distanceFromLngLat(
lng1,
lat1,
lng2,
lat2);
expect(Math.abs(distance - 100)).toBeLessThan(10);
});
it("should be approximately 20 meters for Karlskronaplan", () => {
let lat1: number = 55.59381;
let lng1: number = 13.01861;
let lat2: number = 55.59373;
let lng2: number = 13.01890;
let distance: number = spatial
.distanceFromLngLat(
lng1,
lat1,
lng2,
lat2);
expect(Math.abs(distance - 20)).toBeLessThan(2);
});
});