@openhps/core
Version:
Open Hybrid Positioning System - Core component
86 lines • 3.13 kB
JavaScript
import { RelativeAngle } from '../../data';
import { AngleUnit, Vector3 } from '../../utils';
import { RelativePositionProcessing } from './RelativePositionProcessing';
/**
* Triangulation processing node
* Supported position types:
* - {@link Absolute2DPosition}
* - {@link Absolute3DPosition}
* - {@link GeographicalPosition}
* @rdf {@link http://purl.org/poso/Triangulation}
* @category Processing node
*/
export class TriangulationNode extends RelativePositionProcessing {
constructor(options) {
super(RelativeAngle, options);
}
processRelativePositions(dataObject, relativePositions, dataFrame) {
return new Promise((resolve, reject) => {
const objects = [];
const points = [];
const angles = [];
relativePositions.forEach((object, relativePosition) => {
if (object.getPosition()) {
objects.push(object);
points.push(object.getPosition());
angles.push(relativePosition.unit.convert(relativePosition.angle, AngleUnit.RADIAN));
}
});
switch (objects.length) {
case 0:
case 1:
return resolve(dataObject);
case 2:
break;
case 3:
// TODO: Currently only for 2d
this.triangulate(points, angles).then(position => {
if (position !== null) {
position.timestamp = dataFrame.createdTimestamp;
dataObject.setPosition(position);
}
resolve(dataObject);
}).catch(reject);
break;
default:
return resolve(dataObject);
}
});
}
/**
* Triangulate a absolute 3d location
* @see {@link https://ieeexplore.ieee.org/document/6693716?tp=&arnumber=6693716}
* @param {AbsolutePosition[]} points Points to triangulate
* @param {number[]} angles Angles
* @returns {Promise<AbsolutePosition>} Promise for the triangulated absolute position
*/
triangulate(points, angles) {
return new Promise((resolve, reject) => {
const vectors = [points[0].toVector3(), points[1].toVector3(), points[2].toVector3()];
const x1 = vectors[0].x - vectors[1].x;
const y1 = vectors[0].y - vectors[1].y;
const x3 = vectors[2].x - vectors[1].x;
const y3 = vectors[2].y - vectors[1].y;
const t12 = 1 / Math.tan(angles[1] - angles[0]);
const t23 = 1 / Math.tan(angles[2] - angles[1]);
const t31 = (1 - t12 * t23) / (t12 + t23);
const x12 = x1 + t12 * y1;
const y12 = y1 - t12 * x1;
const x23 = x3 - t23 * y3;
const y23 = y3 + t23 * x3;
const x31 = x3 + x1 + t31 * (y3 - y1);
const y31 = y3 + y1 - t31 * (x3 - x1);
const k31 = x1 * x3 + y1 * y3 + t31 * (x1 * y3 - x3 * y1);
const d = (x12 - x23) * (y23 - y31) - (y12 - y23) * (x23 - x31);
if (d === 0) {
return reject();
}
const xr = vectors[1].x + k31 * (y12 - y23) / d;
const yr = vectors[1].y + k31 * (x23 - x12) / d;
const point = points[0].clone();
point.unit = points[0].unit;
point.fromVector(new Vector3(xr, yr, 0));
return resolve(point);
});
}
}