@viamrobotics/motion-tools 0.14.10 → 0.14.11
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
|
@@ -196,7 +196,7 @@ export class OrientationVector {
|
|
|
196
196
|
const cosTheta = MathUtils.clamp(cosThetaCand, -1, 1);
|
|
197
197
|
const theta = Math.acos(cosTheta);
|
|
198
198
|
if (theta > EPSILON) {
|
|
199
|
-
const newZImagUnit = vecF.copy(
|
|
199
|
+
const newZImagUnit = vecF.copy(newZimag).normalize();
|
|
200
200
|
const rotQuatUnit = quatD.setFromAxisAngle(newZImagUnit, -1 * theta);
|
|
201
201
|
const conj2 = quatE.copy(rotQuatUnit).conjugate();
|
|
202
202
|
const testZ = rotQuatUnit.multiplyQuaternions(rotQuatUnit.multiply(zAxis), conj2);
|