OpenHPS/openhps-imu

import {
    TimeService,
    TimeUnit,
    LengthUnit,
    AngularVelocity,
    ObjectProcessingNode,
    DataFrame,
    DataObject,
    LinearVelocity,
    Matrix4,
    Vector3,
    AxisAngle,
    Orientation,
} from '@openhps/core';

/**
 * Linear and angular velocity processing
 * @category Processing node
 */
export class VelocityProcessingNode<InOut extends DataFrame> extends ObjectProcessingNode<InOut> {
    public processObject(object: DataObject, frame: InOut): Promise<DataObject> {
        return new Promise<DataObject>((resolve, reject) => {
            if (object.getPosition()) {
                const lastPosition = object.getPosition();
                if (lastPosition.linearVelocity || lastPosition.angularVelocity) {
                    // Apply the angular or linear velocity
                    this.applyVelocity(object, frame).then(resolve).catch(reject);
                } else {
                    resolve(object);
                }
            } else {
                resolve(object);
            }
        });
    }

    public applyVelocity(object: DataObject, frame: InOut): Promise<DataObject> {
        return new Promise((resolve) => {
            const lastPosition = object.getPosition();
            // Time since current calculation and previous velocity
            const deltaTime = TimeService.getUnit().convert(
                frame.createdTimestamp - lastPosition.timestamp,
                TimeUnit.SECOND,
            );

            if (deltaTime < 0) {
                // Delta time is negative, this means the previous location
                // timestamp was incorrect
                return resolve(object);
            }

            const linear = lastPosition.linearVelocity || new LinearVelocity();
            const angular = lastPosition.angularVelocity || new AngularVelocity();
            const linearMovement = linear.clone().multiplyScalar(deltaTime);
            const angularMovement = angular.clone().multiplyScalar(deltaTime);

            // Relative position starts at the origin
            // We will rotate this final relative position using the orientation
            // and add it to the existing position vector of our last known position
            const relativePosition = Vector3.fromArray([0, 0, 0]);

            if (angular.equals(Vector3.fromArray([0, 0, 0]))) {
                // Simply apply the linear velocity
                relativePosition.applyMatrix4(
                    new Matrix4().makeTranslation(linearMovement.x, linearMovement.y, linearMovement.z),
                );
            } else if (!linear.equals(Vector3.fromArray([0, 0, 0]))) {
                // Apply linear and angular velocity
                const rX = linear.clone().divideScalar(angular.x === 0 ? 1 : angular.x);
                const rY = linear.clone().divideScalar(angular.y === 0 ? 1 : angular.y);
                const rZ = linear.clone().divideScalar(angular.z === 0 ? 1 : angular.z);
                const rMin = rX.min(rY).min(rZ); // Rotation point
                relativePosition.applyMatrix4(new Matrix4().makeTranslation(-rMin.x, -rMin.y, -rMin.z));
                relativePosition.applyMatrix4(
                    new AxisAngle(angularMovement.x, angularMovement.y, angularMovement.z).toRotationMatrix(),
                );
                relativePosition.applyMatrix4(new Matrix4().makeTranslation(rMin.x, rMin.y, rMin.z));
                relativePosition.applyMatrix4(
                    Matrix4.rotationFromAxisAngle(
                        new Vector3(angular.x !== 0 ? 1 : 0, angular.y !== 0 ? 1 : 0, angular.z !== 0 ? 1 : 0),
                        Math.PI / 2,
                    ),
                );
            }

            // Predict the next location
            const newPosition = lastPosition.clone();
            if (!newPosition.orientation) {
                newPosition.orientation = new Orientation();
            }
            newPosition.timestamp = frame.createdTimestamp;
            newPosition.fromVector(
                newPosition.toVector3(LengthUnit.METER).add(relativePosition.applyQuaternion(newPosition.orientation)),
                LengthUnit.METER,
            );

            // New orientation in radians
            const newOrientation = newPosition.orientation.toEuler().toVector3().add(angular.multiplyScalar(deltaTime));
            newPosition.orientation = Orientation.fromEuler(newOrientation);
            object.setPosition(newPosition);
            resolve(object);
        });
    }
}