Skip to content

Commit

Permalink
Merge pull request #15 from viamrobotics/ov-enhancements
Browse files Browse the repository at this point in the history 
Add units, add w prop
  • Loading branch information
@micheal-parks
micheal-parks authored Feb 29, 2024
2 parents 3aea62f + ccbd495 commit db5b44e
Show file tree
Hide file tree
Showing 22 changed files with 1,069 additions and 767 deletions.
5 changes: 3 additions & 2 deletions .prettierrc.cjs
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
'use strict'
'use strict';

module.exports = '@viamrobotics/prettier-config';
/* eslint-disable @typescript-eslint/no-unsafe-call */
module.exports = require('@viamrobotics/prettier-config');
23 changes: 13 additions & 10 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -1,29 +1,32 @@
# @viamrobotics/three

Viam-related utilities for THREE.js

### Orientation Vector

A class for Viam's [Orientation Vector](https://docs.viam.com/internals/orientation-vector/#edit-on-github) rotation type.

This class closely resembles other rotation formats like `THREE.Quaternion` or `THREE.Euler`.

```ts
import * as THREE from 'three'
import { OrientationVector } from '@viamrobotics/three'
import * as THREE from 'three';
import { OrientationVector } from '@viamrobotics/three';

const ov = new OrientationVector()
const quat = new THREE.Quaternion()
const euler = new THREE.Euler()
const ov = new OrientationVector();
const quat = new THREE.Quaternion();
const euler = new THREE.Euler();

// Common conversions:
ov.toQuaternion(quat)
ov.toEuler(euler)
ov.toQuaternion(quat);
ov.toEuler(euler);

ov.setFromQuaternion(quat)
ov.setFromQuaternion(quat);
```

### ViamObject3D

Extends THREE.Object3D and adds an `.orientationVector` that auto-updates when other rotation formats are updated.

```ts
import { ViamObject3D } from '@viamrobotics/three'
```
import { ViamObject3D } from '@viamrobotics/three';
```
33 changes: 17 additions & 16 deletions package.json
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
{
"name": "@viamrobotics/three",
"version": "0.0.4",
"version": "0.0.5",
"license": "Apache-2.0",
"type": "module",
"files": [
Expand Down Expand Up @@ -39,26 +39,27 @@
],
"devDependencies": {
"@0b5vr/tweakpane-plugin-rotation": "^0.2.0",
"@tweakpane/core": "^2.0.1",
"@tweakpane/plugin-essentials": "^0.2.0",
"@types/three": "^0.159.0",
"@typescript-eslint/eslint-plugin": "^6.14.0",
"@typescript-eslint/parser": "^6.14.0",
"@viamrobotics/eslint-config": "^0.3.0",
"@tweakpane/core": "^2.0.3",
"@tweakpane/plugin-essentials": "^0.2.1",
"@types/three": "^0.162.0",
"@typescript-eslint/eslint-plugin": "^7.1.0",
"@typescript-eslint/parser": "^7.1.0",
"@viamrobotics/eslint-config": "^0.4.0",
"@viamrobotics/prettier-config": "^0.3.4",
"@viamrobotics/typescript-config": "^0.1.0",
"eslint": "^8.55.0",
"eslint": "^8.57.0",
"eslint-config-prettier": "^9.1.0",
"eslint-plugin-sonarjs": "^0.23.0",
"eslint-plugin-unicorn": "^49.0.0",
"prettier": "^3.1.1",
"three": "^0.159.0",
"eslint-plugin-sonarjs": "^0.24.0",
"eslint-plugin-unicorn": "^51.0.1",
"eslint-plugin-vitest": "^0.3.22",
"prettier": "^3.2.5",
"three": "^0.162.0",
"three-inspect": "^0.3.4",
"trzy": "^0.3.17",
"tweakpane": "^4.0.1",
"typescript": "^5.3.2",
"vite": "^5.0.5",
"vitest": "^0.34.6"
"tweakpane": "^4.0.3",
"typescript": "^5.3.3",
"vite": "^5.1.4",
"vitest": "^1.3.1"
},
"peerDependencies": {
"three": "*"
Expand Down
247 changes: 121 additions & 126 deletions playground/conversions.ts
View file
Original file line number Diff line number Diff line change
@@ -1,56 +1,43 @@
import * as THREE from 'three'
import { Units, type InputTypes } from './types'
import { rotations } from './rotations'
import { OrientationVector, ViamObject3D } from '../src/main'

// These globals are just added to allow playing around in the console as well.
window.THREE = THREE

// @ts-expect-error For debugging
window.OrientationVector = OrientationVector

// @ts-expect-error For debugging
window.ViamObject3D = ViamObject3D

const v3 = new THREE.Vector3()
const quat = new THREE.Quaternion()
const euler = new THREE.Euler()
const m4 = new THREE.Matrix4()
const ov = new OrientationVector()
/* eslint-disable @typescript-eslint/no-non-null-assertion */
/* eslint-disable id-length */
/* eslint-disable unicorn/prefer-switch */
/* eslint-disable @typescript-eslint/no-unnecessary-condition */
import * as THREE from 'three';
import { Units, type InputTypes } from './types';
import { rotations } from './rotations';
import { OrientationVector } from '../src/main';

const v3 = new THREE.Vector3();
const quat = new THREE.Quaternion();
const euler = new THREE.Euler();
const m4 = new THREE.Matrix4();
const ov = new OrientationVector();

const toRad = (x: number, unit: Units): number => {
if (unit === Units.degrees) {
return x / 180 * Math.PI
} else {
return x
}
}
return unit === Units.degrees ? (x / 180) * Math.PI : x;
};

const toAngle = (x: number, unit: Units): number => {
if (unit === Units.degrees) {
return x * 180 / Math.PI
} else {
return x
}
}
return unit === Units.degrees ? (x * 180) / Math.PI : x;
};

const toReal = (x: number): number => {
if (!Number.isNaN(x) && Number.isFinite(x)) {
return Number.parseFloat(x.toFixed(7))
} else {
return x
}
}
return !Number.isNaN(x) && Number.isFinite(x)
? Number.parseFloat(x.toFixed(7))
: x;
};

const toFixedWidth = (x: number): number => {
if (!Number.isNaN(x) && Number.isFinite(x)) {
let s = x.toFixed(7)
if (x >= 0) s = ' ' + s
return Number.parseFloat(s)
} else {
return x
}
}
if (!Number.isNaN(x) && Number.isFinite(x)) {
let s = x.toFixed(7);
if (x >= 0) {
s = ` ${s}`;
}
return Number.parseFloat(s);
}

return x;
};

export const conversion = (type: InputTypes, units: Units) => {
if (type === 'ov') {
Expand All @@ -59,117 +46,125 @@ export const conversion = (type: InputTypes, units: Units) => {
rotations.ov.y,
rotations.ov.z,
toRad(rotations.ov.w, units)
)

quat.copy(ov.toQuaternion(quat))
);

quat.copy(ov.toQuaternion(quat));
} else if (type === 'matrix') {
const { row1, row2, row3 } = rotations.matrix
const { row1, row2, row3 } = rotations.matrix;

m4.set(
row1.x, row1.y, row1.z, 1,
row2.x, row2.y, row2.z, 1,
row3.x, row3.y, row3.z, 1,
0, 0, 0, 1
)

quat.setFromRotationMatrix(m4)

row1.x,
row1.y,
row1.z,
1,
row2.x,
row2.y,
row2.z,
1,
row3.x,
row3.y,
row3.z,
1,
0,
0,
0,
1
);

quat.setFromRotationMatrix(m4);
} else if (type === 'quaternion') {
quat.set(
rotations.quaternion.x,
rotations.quaternion.y,
rotations.quaternion.z,
rotations.quaternion.w
)

);
} else if (type === 'axis angle') {
const aa = rotations.axisAngle
const axis = v3
axis.set(aa.xyz.x, aa.xyz.y, aa.xyz.z)
axis.normalize()

quat.setFromAxisAngle(axis, toRad(aa.angle, units))
const aa = rotations.axisAngle;
const axis = v3;
axis.set(aa.xyz.x, aa.xyz.y, aa.xyz.z);
axis.normalize();

quat.setFromAxisAngle(axis, toRad(aa.angle, units));
} else if (type === 'axis with angle magnitude') {
const aa = rotations.axisAngleMagnitude
const axis = v3
axis.set(aa.x, aa.y, aa.z)

const angle = toRad(axis.length(), units)
axis.normalize()
const aa = rotations.axisAngleMagnitude;
const axis = v3;
axis.set(aa.x, aa.y, aa.z);

quat.setFromAxisAngle(axis, angle)
const angle = toRad(axis.length(), units);
axis.normalize();

quat.setFromAxisAngle(axis, angle);
} else if (type === 'euler') {
const eu = rotations.euler

quat.setFromEuler(euler.set(
toRad(eu.xyz.x, units),
toRad(eu.xyz.y, units),
toRad(eu.xyz.z, units),
eu.order as THREE.EulerOrder
))

const eu = rotations.euler;

quat.setFromEuler(
euler.set(
toRad(eu.xyz.x, units),
toRad(eu.xyz.y, units),
toRad(eu.xyz.z, units),
eu.order as THREE.EulerOrder
)
);
}

quat.normalize()
quat.normalize();

rotations.quaternion.x = toReal(quat.x)
rotations.quaternion.y = toReal(quat.y)
rotations.quaternion.z = toReal(quat.z)
rotations.quaternion.w = toReal(quat.w)
rotations.quaternion.x = toReal(quat.x);
rotations.quaternion.y = toReal(quat.y);
rotations.quaternion.z = toReal(quat.z);
rotations.quaternion.w = toReal(quat.w);

ov.setFromQuaternion(quat)
rotations.ov.w = toAngle(ov.th, units)
rotations.ov.x = ov.x
rotations.ov.y = ov.y
rotations.ov.z = ov.z
ov.setFromQuaternion(quat);
rotations.ov.w = toAngle(ov.th, units);
rotations.ov.x = ov.x;
rotations.ov.y = ov.y;
rotations.ov.z = ov.z;

m4.makeRotationFromQuaternion(quat)
m4.makeRotationFromQuaternion(quat);

{
const r = m4.elements
rotations.matrix.row1.x = toFixedWidth(r[0]!)
rotations.matrix.row1.y = toFixedWidth(r[4]!)
rotations.matrix.row1.z = toFixedWidth(r[8]!)
rotations.matrix.row2.x = toFixedWidth(r[1]!)
rotations.matrix.row2.y = toFixedWidth(r[5]!)
rotations.matrix.row2.z = toFixedWidth(r[9]!)
rotations.matrix.row3.x = toFixedWidth(r[2]!)
rotations.matrix.row3.y = toFixedWidth(r[6]!)
rotations.matrix.row3.z = toFixedWidth(r[10]!)
const r = m4.elements;
rotations.matrix.row1.x = toFixedWidth(r[0]!);
rotations.matrix.row1.y = toFixedWidth(r[4]!);
rotations.matrix.row1.z = toFixedWidth(r[8]!);
rotations.matrix.row2.x = toFixedWidth(r[1]!);
rotations.matrix.row2.y = toFixedWidth(r[5]!);
rotations.matrix.row2.z = toFixedWidth(r[9]!);
rotations.matrix.row3.x = toFixedWidth(r[2]!);
rotations.matrix.row3.y = toFixedWidth(r[6]!);
rotations.matrix.row3.z = toFixedWidth(r[10]!);
}

const axis = [0, 0, 0]
const angle = 2 * Math.acos(quat.w)
const axis = [0, 0, 0];
const angle = 2 * Math.acos(quat.w);

if (1 - (quat.w * quat.w) < 0.000001) {
axis[0] = quat.x
axis[1] = quat.y
axis[2] = quat.z
if (1 - quat.w * quat.w < 0.000_001) {
axis[0] = quat.x;
axis[1] = quat.y;
axis[2] = quat.z;
} else {
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/
const s = Math.sqrt(1 - (quat.w * quat.w))
axis[0] = quat.x / s
axis[1] = quat.y / s
axis[2] = quat.z / s
const s = Math.sqrt(1 - quat.w * quat.w);
axis[0] = quat.x / s;
axis[1] = quat.y / s;
axis[2] = quat.z / s;
}

rotations.axisAngle.xyz.x = toReal(axis[0])
rotations.axisAngle.xyz.y = toReal(axis[1])
rotations.axisAngle.xyz.z = toReal(axis[2])
rotations.axisAngle.angle = toReal(toAngle(angle, units))
rotations.axisAngle.xyz.x = toReal(axis[0]);
rotations.axisAngle.xyz.y = toReal(axis[1]);
rotations.axisAngle.xyz.z = toReal(axis[2]);
rotations.axisAngle.angle = toReal(toAngle(angle, units));

rotations.axisAngleMagnitude.x = toReal(toAngle(axis[0] * angle, units));
rotations.axisAngleMagnitude.y = toReal(toAngle(axis[1] * angle, units));
rotations.axisAngleMagnitude.z = toReal(toAngle(axis[2] * angle, units));

rotations.axisAngleMagnitude.x = toReal(toAngle(axis[0] * angle, units))
rotations.axisAngleMagnitude.y = toReal(toAngle(axis[1] * angle, units))
rotations.axisAngleMagnitude.z = toReal(toAngle(axis[2] * angle, units))

{
euler.setFromRotationMatrix(m4, rotations.euler.order as THREE.EulerOrder)
const [x, y, z] = euler.toArray() as [number, number, number]
rotations.euler.xyz.x = toReal(toAngle(x, units))
rotations.euler.xyz.y = toReal(toAngle(y, units))
rotations.euler.xyz.z = toReal(toAngle(z, units))
euler.setFromRotationMatrix(m4, rotations.euler.order as THREE.EulerOrder);
const [x, y, z] = euler.toArray() as [number, number, number];
rotations.euler.xyz.x = toReal(toAngle(x, units));
rotations.euler.xyz.y = toReal(toAngle(y, units));
rotations.euler.xyz.z = toReal(toAngle(z, units));
}
}
};
Loading

0 comments on commit db5b44e

Please sign in to comment.