diff --git a/src/encoders/index.ts b/src/encoders/index.ts index b3164a0..55be6cf 100644 --- a/src/encoders/index.ts +++ b/src/encoders/index.ts @@ -1,4 +1,6 @@ import CentroidPosition from './motion/centroid-position'; +import OrientationEuler from './motion/orientation-euler'; +import OrientationQuaternion from './motion/orientation-quaternion'; import RTTrPM from './motion/rttrpm'; import Trackable from './motion/trackable'; import RTTrPHeader from './rttrp-header'; @@ -8,4 +10,6 @@ export const Encoders = { RTTrPM, Trackable, CentroidPosition, + OrientationQuaternion, + OrientationEuler, }; diff --git a/src/encoders/motion/orientation-euler.ts b/src/encoders/motion/orientation-euler.ts new file mode 100644 index 0000000..7890170 --- /dev/null +++ b/src/encoders/motion/orientation-euler.ts @@ -0,0 +1,27 @@ +export default ( + latency: number, + order: number, + R1: number, + R2: number, + R3: number, + isLittleEndian: boolean = false +): Uint8Array => { + const bytes = new Uint8Array(31); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, 0x04); + dataOffset += 1; + view.setUint16(dataOffset, bytes.byteLength, isLittleEndian); + dataOffset += 2; + view.setUint16(dataOffset, latency, isLittleEndian); + dataOffset += 2; + view.setUint16(dataOffset, order, isLittleEndian); + dataOffset += 2; + view.setFloat64(dataOffset, R1, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, R2, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, R3, isLittleEndian); + return bytes; +}; diff --git a/src/encoders/motion/orientation-quaternion.ts b/src/encoders/motion/orientation-quaternion.ts new file mode 100644 index 0000000..64a8206 --- /dev/null +++ b/src/encoders/motion/orientation-quaternion.ts @@ -0,0 +1,27 @@ +export default ( + latency: number, + Qx: number, + Qy: number, + Qz: number, + Qw: number, + isLittleEndian: boolean = false +): Uint8Array => { + const bytes = new Uint8Array(37); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, 0x03); + dataOffset += 1; + view.setUint16(dataOffset, bytes.byteLength, isLittleEndian); + dataOffset += 2; + view.setUint16(dataOffset, latency, isLittleEndian); + dataOffset += 2; + view.setFloat64(dataOffset, Qx, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, Qy, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, Qz, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, Qw, isLittleEndian); + return bytes; +};