/
VelocityProcessingNode.ts
104 lines (96 loc) · 4.43 KB
/
VelocityProcessingNode.ts
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
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);
});
}
}