From 3c5cd86ae25e607cb08c83e95d15a75a39d94a9a Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 08:37:44 -0500 Subject: [PATCH 1/8] add decoder for centroid position submodule --- src/decoders/index.ts | 2 ++ src/decoders/motion/centroid-position.ts | 31 ++++++++++++++++++++++++ src/models.ts | 10 ++++++++ tests/decode.test.js | 16 ++++++++++++ 4 files changed, 59 insertions(+) create mode 100644 src/decoders/motion/centroid-position.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index 1f55172..9f6d651 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -1,5 +1,7 @@ +import CentroidPosition from './motion/centroid-position'; import RTTrPHeader from './rttrp'; export const Decoders = { RTTrPHeader, + CentroidPosition, }; diff --git a/src/decoders/motion/centroid-position.ts b/src/decoders/motion/centroid-position.ts new file mode 100644 index 0000000..0f741b0 --- /dev/null +++ b/src/decoders/motion/centroid-position.ts @@ -0,0 +1,31 @@ +import { CentroidPosition } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): CentroidPosition => { + if (bytes.length !== 29) { + throw new Error('Centroid Position module must be 29 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x02) { + throw new Error('Centroid Position module must have a type of 0x02'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 29) { + throw new Error('Centroid Position module must be 29 bytes'); + } + const latency = view.getUint16(3, isLittleEndian); + const x = view.getFloat64(5, isLittleEndian); + const y = view.getFloat64(13, isLittleEndian); + const z = view.getFloat64(21, isLittleEndian); + return { + type, + size, + latency, + x, + y, + z, + }; +}; diff --git a/src/models.ts b/src/models.ts index 67745f7..d6b2a3b 100644 --- a/src/models.ts +++ b/src/models.ts @@ -8,4 +8,14 @@ export type RTTrPHeader = { context: number; subModuleCount: number; }; + +export type CentroidPosition = { + type: number; + size: number; + latency: number; + x: number; + y: number; + z: number; +}; + }; diff --git a/tests/decode.test.js b/tests/decode.test.js index fb57a0e..0d3beb3 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -19,6 +19,22 @@ const goodTests = [ }, decoder: Decoders.RTTrPHeader, }, + { + description: 'Centroid Position', + bytes: new Uint8Array([ + 0x02, 0x00, 0x1d, 0x00, 0x10, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + ]), + expected: { + type: 0x02, + size: 0x1d, + latency: 16, + x: 1.0, + y: 2.0, + z: 3.0, + }, + decoder: Decoders.CentroidPosition, + }, ]; describe('RTTrP Bytes Decoding', () => { From 122b546ba21077f2f10a252fdf06a0e028536cfd Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 08:39:27 -0500 Subject: [PATCH 2/8] add decoder for orientation submodules --- src/decoders/index.ts | 4 +++ src/decoders/motion/orientation-euler.ts | 33 ++++++++++++++++++ src/decoders/motion/orientation-quaternion.ts | 33 ++++++++++++++++++ src/models.ts | 19 +++++++++++ tests/decode.test.js | 34 +++++++++++++++++++ 5 files changed, 123 insertions(+) create mode 100644 src/decoders/motion/orientation-euler.ts create mode 100644 src/decoders/motion/orientation-quaternion.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index 9f6d651..8138cde 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -1,7 +1,11 @@ import CentroidPosition from './motion/centroid-position'; +import OrientationEuler from './motion/orientation-euler'; +import OrientationQuaternion from './motion/orientation-quaternion'; import RTTrPHeader from './rttrp'; export const Decoders = { RTTrPHeader, CentroidPosition, + OrientationQuaternion, + OrientationEuler, }; diff --git a/src/decoders/motion/orientation-euler.ts b/src/decoders/motion/orientation-euler.ts new file mode 100644 index 0000000..bd67dc9 --- /dev/null +++ b/src/decoders/motion/orientation-euler.ts @@ -0,0 +1,33 @@ +import { OrientationEuler } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): OrientationEuler => { + if (bytes.length !== 31) { + throw new Error('Orientation (Euler) module must be 31 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x04) { + throw new Error('Orientation (Euler) module must have a type of 0x04'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 31) { + throw new Error('Orientation (Euler) module must be 31 bytes'); + } + const latency = view.getUint16(3, isLittleEndian); + const order = view.getUint16(5, isLittleEndian); + const r1 = view.getFloat64(7, isLittleEndian); + const r2 = view.getFloat64(15, isLittleEndian); + const r3 = view.getFloat64(23, isLittleEndian); + return { + type, + size, + latency, + order, + r1, + r2, + r3, + }; +}; diff --git a/src/decoders/motion/orientation-quaternion.ts b/src/decoders/motion/orientation-quaternion.ts new file mode 100644 index 0000000..c5c88e3 --- /dev/null +++ b/src/decoders/motion/orientation-quaternion.ts @@ -0,0 +1,33 @@ +import { OrientationQuaternion } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): OrientationQuaternion => { + if (bytes.length !== 37) { + throw new Error('Orientation (Quaternion) module must be 37 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x03) { + throw new Error('Orientation (Quaternion) module must have a type of 0x03'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 37) { + throw new Error('Orientation (Quaternion) module must be 37 bytes'); + } + const latency = view.getUint16(3, isLittleEndian); + const Qx = view.getFloat64(5, isLittleEndian); + const Qy = view.getFloat64(13, isLittleEndian); + const Qz = view.getFloat64(21, isLittleEndian); + const Qw = view.getFloat64(29, isLittleEndian); + return { + type, + size, + latency, + Qx, + Qy, + Qz, + Qw, + }; +}; diff --git a/src/models.ts b/src/models.ts index d6b2a3b..6e22e46 100644 --- a/src/models.ts +++ b/src/models.ts @@ -18,4 +18,23 @@ export type CentroidPosition = { z: number; }; + +export type OrientationQuaternion = { + type: number; + size: number; + latency: number; + Qx: number; + Qy: number; + Qz: number; + Qw: number; +}; + +export type OrientationEuler = { + type: number; + size: number; + latency: number; + order: number; + r1: number; + r2: number; + r3: number; }; diff --git a/tests/decode.test.js b/tests/decode.test.js index 0d3beb3..e8522a7 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -35,6 +35,40 @@ const goodTests = [ }, decoder: Decoders.CentroidPosition, }, + { + description: 'Orientation (Quaternion)', + bytes: new Uint8Array([ + 0x03, 0x00, 37, 0x00, 0x10, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + ]), + expected: { + type: 0x03, + size: 37, + latency: 16, + Qx: 1.0, + Qy: 2.0, + Qz: 3.0, + Qw: 4.0, + }, + decoder: Decoders.OrientationQuaternion, + }, + { + description: 'Orientation (Euler)', + bytes: new Uint8Array([ + 0x04, 0x00, 31, 0x00, 0x10, 0x01, 0x23, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + ]), + expected: { + type: 0x04, + size: 31, + latency: 16, + order: 0x0123, + r1: 1.0, + r2: 2.0, + r3: 3.0, + }, + decoder: Decoders.OrientationEuler, + }, ]; describe('RTTrP Bytes Decoding', () => { From 38fdf9109256aaa64bcead412e0a5bee6850dd68 Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 08:40:50 -0500 Subject: [PATCH 3/8] add decoder for tracked point position submodule --- src/decoders/index.ts | 2 ++ src/decoders/motion/tracked-point-position.ts | 33 +++++++++++++++++++ src/models.ts | 10 ++++++ tests/decode.test.js | 17 ++++++++++ 4 files changed, 62 insertions(+) create mode 100644 src/decoders/motion/tracked-point-position.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index 8138cde..fba0161 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -1,11 +1,13 @@ import CentroidPosition from './motion/centroid-position'; import OrientationEuler from './motion/orientation-euler'; import OrientationQuaternion from './motion/orientation-quaternion'; +import TrackedPointPosition from './motion/tracked-point-position'; import RTTrPHeader from './rttrp'; export const Decoders = { RTTrPHeader, CentroidPosition, + TrackedPointPosition, OrientationQuaternion, OrientationEuler, }; diff --git a/src/decoders/motion/tracked-point-position.ts b/src/decoders/motion/tracked-point-position.ts new file mode 100644 index 0000000..cc74d64 --- /dev/null +++ b/src/decoders/motion/tracked-point-position.ts @@ -0,0 +1,33 @@ +import { TrackedPointPosition } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): TrackedPointPosition => { + if (bytes.length !== 30) { + throw new Error('Tracked Point Position module must be 30 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x06) { + throw new Error('Tracked Point Position module must have a type of 0x06'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 30) { + throw new Error('Tracked Point Position module must be 30 bytes'); + } + const latency = view.getUint16(3, isLittleEndian); + const x = view.getFloat64(5, isLittleEndian); + const y = view.getFloat64(13, isLittleEndian); + const z = view.getFloat64(21, isLittleEndian); + const index = view.getUint8(29); + return { + type, + size, + latency, + x, + y, + z, + index, + }; +}; diff --git a/src/models.ts b/src/models.ts index 6e22e46..e19abdb 100644 --- a/src/models.ts +++ b/src/models.ts @@ -19,6 +19,16 @@ export type CentroidPosition = { }; +export type TrackedPointPosition = { + type: number; + size: number; + latency: number; + x: number; + y: number; + z: number; + index: number; +}; + export type OrientationQuaternion = { type: number; size: number; diff --git a/tests/decode.test.js b/tests/decode.test.js index e8522a7..4fc98a3 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -35,6 +35,23 @@ const goodTests = [ }, decoder: Decoders.CentroidPosition, }, + { + description: 'Tracked Point Position', + bytes: new Uint8Array([ + 0x06, 0x00, 0x1e, 0x00, 0x10, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, + ]), + expected: { + type: 0x06, + size: 0x1e, + latency: 16, + x: 1.0, + y: 2.0, + z: 3.0, + index: 1, + }, + decoder: Decoders.TrackedPointPosition, + }, { description: 'Orientation (Quaternion)', bytes: new Uint8Array([ From 7564bcf6a9896656e023d6cf6081396a0889d5f9 Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 08:41:19 -0500 Subject: [PATCH 4/8] add decoder for accel velocity submodules --- src/decoders/index.ts | 4 ++ .../motion/centroid-accel-velocity.ts | 44 +++++++++++++++++ .../motion/tracked-point-accel-velocity.ts | 47 +++++++++++++++++++ src/models.ts | 28 +++++++++++ tests/decode.test.js | 45 ++++++++++++++++++ 5 files changed, 168 insertions(+) create mode 100644 src/decoders/motion/centroid-accel-velocity.ts create mode 100644 src/decoders/motion/tracked-point-accel-velocity.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index fba0161..90416bf 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -1,13 +1,17 @@ +import CentroidAccelVelocity from './motion/centroid-accel-velocity'; import CentroidPosition from './motion/centroid-position'; import OrientationEuler from './motion/orientation-euler'; import OrientationQuaternion from './motion/orientation-quaternion'; +import TrackedPointAccelVelocity from './motion/tracked-point-accel-velocity'; import TrackedPointPosition from './motion/tracked-point-position'; import RTTrPHeader from './rttrp'; export const Decoders = { RTTrPHeader, CentroidPosition, + CentroidAccelVelocity, TrackedPointPosition, + TrackedPointAccelVelocity, OrientationQuaternion, OrientationEuler, }; diff --git a/src/decoders/motion/centroid-accel-velocity.ts b/src/decoders/motion/centroid-accel-velocity.ts new file mode 100644 index 0000000..654a252 --- /dev/null +++ b/src/decoders/motion/centroid-accel-velocity.ts @@ -0,0 +1,44 @@ +import { CentroidAccelVelocity } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): CentroidAccelVelocity => { + if (bytes.length !== 51) { + throw new Error('Centroid Acceleration and Velocity module must be 51 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x20) { + throw new Error('Centroid Acceleration and Velocity module must have a type of 0x20'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 51) { + throw new Error('Centroid Acceleration and Velocity module must be 51 bytes'); + } + const x = view.getFloat64(3, isLittleEndian); + const y = view.getFloat64(11, isLittleEndian); + const z = view.getFloat64(19, isLittleEndian); + + const accelX = view.getFloat32(27, isLittleEndian); + const accelY = view.getFloat32(31, isLittleEndian); + const accelZ = view.getFloat32(35, isLittleEndian); + + const velocityX = view.getFloat32(39, isLittleEndian); + const velocityY = view.getFloat32(43, isLittleEndian); + const velocityZ = view.getFloat32(47, isLittleEndian); + + return { + type, + size, + x, + y, + z, + accelX, + accelY, + accelZ, + velocityX, + velocityY, + velocityZ, + }; +}; diff --git a/src/decoders/motion/tracked-point-accel-velocity.ts b/src/decoders/motion/tracked-point-accel-velocity.ts new file mode 100644 index 0000000..a64a857 --- /dev/null +++ b/src/decoders/motion/tracked-point-accel-velocity.ts @@ -0,0 +1,47 @@ +import { TrackedPointAccelVelocity } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): TrackedPointAccelVelocity => { + if (bytes.length !== 52) { + throw new Error('Tracked Point Acceleration and Velocity module must be 52 bytes'); + } + + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x21) { + throw new Error('Tracked Point Acceleration and Velocity module must have a type of 0x21'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== 52) { + throw new Error('Tracked Point Acceleration and Velocity module must be 52 bytes'); + } + const x = view.getFloat64(3, isLittleEndian); + const y = view.getFloat64(11, isLittleEndian); + const z = view.getFloat64(19, isLittleEndian); + + const accelX = view.getFloat32(27, isLittleEndian); + const accelY = view.getFloat32(31, isLittleEndian); + const accelZ = view.getFloat32(35, isLittleEndian); + + const velocityX = view.getFloat32(39, isLittleEndian); + const velocityY = view.getFloat32(43, isLittleEndian); + const velocityZ = view.getFloat32(47, isLittleEndian); + + const index = view.getUint8(51); + + return { + type, + size, + x, + y, + z, + accelX, + accelY, + accelZ, + velocityX, + velocityY, + velocityZ, + index, + }; +}; diff --git a/src/models.ts b/src/models.ts index e19abdb..7cccc4e 100644 --- a/src/models.ts +++ b/src/models.ts @@ -18,6 +18,19 @@ export type CentroidPosition = { z: number; }; +export type CentroidAccelVelocity = { + type: number; + size: number; + x: number; + y: number; + z: number; + accelX: number; + accelY: number; + accelZ: number; + velocityX: number; + velocityY: number; + velocityZ: number; +}; export type TrackedPointPosition = { type: number; @@ -29,6 +42,21 @@ export type TrackedPointPosition = { index: number; }; +export type TrackedPointAccelVelocity = { + type: number; + size: number; + x: number; + y: number; + z: number; + accelX: number; + accelY: number; + accelZ: number; + velocityX: number; + velocityY: number; + velocityZ: number; + index: number; +}; + export type OrientationQuaternion = { type: number; size: number; diff --git a/tests/decode.test.js b/tests/decode.test.js index 4fc98a3..ebb4f95 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -86,6 +86,51 @@ const goodTests = [ }, decoder: Decoders.OrientationEuler, }, + { + description: 'Centroid Acceleration and Velocity', + bytes: new Uint8Array([ + 0x20, 0x00, 51, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x80, 0x00, 0x00, 0x40, 0xa0, 0x00, 0x00, 0x40, 0xc0, 0x00, + 0x00, 0x40, 0xe0, 0x00, 0x00, 0x41, 0x00, 0x00, 0x00, 0x41, 0x10, 0x00, 0x00, + ]), + expected: { + type: 0x20, + size: 51, + x: 1.0, + y: 2.0, + z: 3.0, + accelX: 4.0, + accelY: 5.0, + accelZ: 6.0, + velocityX: 7.0, + velocityY: 8.0, + velocityZ: 9.0, + }, + decoder: Decoders.CentroidAccelVelocity, + }, + { + description: 'Tracked Point Acceleration and Velocity', + bytes: new Uint8Array([ + 0x21, 0x00, 52, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x80, 0x00, 0x00, 0x40, 0xa0, 0x00, 0x00, 0x40, 0xc0, 0x00, + 0x00, 0x40, 0xe0, 0x00, 0x00, 0x41, 0x00, 0x00, 0x00, 0x41, 0x10, 0x00, 0x00, 0x01, + ]), + expected: { + type: 0x21, + size: 52, + x: 1.0, + y: 2.0, + z: 3.0, + accelX: 4.0, + accelY: 5.0, + accelZ: 6.0, + velocityX: 7.0, + velocityY: 8.0, + velocityZ: 9.0, + index: 1, + }, + decoder: Decoders.TrackedPointAccelVelocity, + }, ]; describe('RTTrP Bytes Decoding', () => { From 80588c1ce4b50960414f3d8ec2bec2f36a65fc6f Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 09:54:58 -0500 Subject: [PATCH 5/8] add decoder for zone collision detection submodule --- src/decoders/index.ts | 4 +++ .../motion/zone-collision-detection.ts | 32 +++++++++++++++++ src/decoders/motion/zone-object.ts | 21 +++++++++++ src/models.ts | 13 +++++++ tests/decode.test.js | 35 +++++++++++++++++++ 5 files changed, 105 insertions(+) create mode 100644 src/decoders/motion/zone-collision-detection.ts create mode 100644 src/decoders/motion/zone-object.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index 90416bf..c96275a 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -4,6 +4,8 @@ import OrientationEuler from './motion/orientation-euler'; import OrientationQuaternion from './motion/orientation-quaternion'; import TrackedPointAccelVelocity from './motion/tracked-point-accel-velocity'; import TrackedPointPosition from './motion/tracked-point-position'; +import ZoneCollisionDetection from './motion/zone-collision-detection'; +import ZoneObject from './motion/zone-object'; import RTTrPHeader from './rttrp'; export const Decoders = { @@ -14,4 +16,6 @@ export const Decoders = { TrackedPointAccelVelocity, OrientationQuaternion, OrientationEuler, + ZoneCollisionDetection, + ZoneObject, }; diff --git a/src/decoders/motion/zone-collision-detection.ts b/src/decoders/motion/zone-collision-detection.ts new file mode 100644 index 0000000..4d01700 --- /dev/null +++ b/src/decoders/motion/zone-collision-detection.ts @@ -0,0 +1,32 @@ +import { Decoders } from '..'; +import { ZoneCollisionDetection, ZoneObject } from '../../models'; + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): ZoneCollisionDetection => { + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + const type = view.getUint8(0); + if (type !== 0x22) { + throw new Error('Zone Collision Detection module must have a type of 0x22'); + } + + const size = view.getUint16(1, isLittleEndian); + if (size !== bytes.byteLength) { + throw new Error('Zone Collision Detection module size mismatch'); + } + const numberOfZones = view.getUint8(3); + + const zones: ZoneObject[] = []; + let zoneObjectOffset = 4; + + for (let index = 0; index < numberOfZones; index++) { + const zone = Decoders.ZoneObject(bytes.subarray(zoneObjectOffset)); + zones.push(zone); + zoneObjectOffset += zone.size; + } + return { + type, + size, + numberOfZones, + zones, + }; +}; diff --git a/src/decoders/motion/zone-object.ts b/src/decoders/motion/zone-object.ts new file mode 100644 index 0000000..19b57c2 --- /dev/null +++ b/src/decoders/motion/zone-object.ts @@ -0,0 +1,21 @@ +import { ZoneObject } from '../../models'; +const textDecoder = new TextDecoder('utf-8'); +export default (bytes: Uint8Array): ZoneObject => { + const size = bytes[0]; + if (bytes.byteLength < size) { + throw new Error('Zone Object size does not have enough actual bytes'); + } + + const moduleBytes = bytes.subarray(0, size); + + const nameLength = moduleBytes[1]; + if (moduleBytes.byteLength - 2 !== nameLength) { + throw new Error('Zone Object name length does not match actual moduleBytes length'); + } + const name = textDecoder.decode(moduleBytes.subarray(2)); + return { + size, + nameLength, + name, + }; +}; diff --git a/src/models.ts b/src/models.ts index 7cccc4e..83a5883 100644 --- a/src/models.ts +++ b/src/models.ts @@ -76,3 +76,16 @@ export type OrientationEuler = { r2: number; r3: number; }; + +export type ZoneCollisionDetection = { + type: number; + size: number; + numberOfZones: number; + zones: ZoneObject[]; +}; + +export type ZoneObject = { + size: number; + nameLength: number; + name: string; +}; diff --git a/tests/decode.test.js b/tests/decode.test.js index ebb4f95..3061d14 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -131,6 +131,41 @@ const goodTests = [ }, decoder: Decoders.TrackedPointAccelVelocity, }, + { + description: 'Zone Collision Detection', + bytes: new Uint8Array([ + 0x22, 0x00, 0x14, 0x02, 0x08, 0x06, 0x7a, 0x6f, 0x6e, 0x65, 0x20, 0x31, 0x08, 0x06, 0x7a, 0x6f, 0x6e, 0x65, 0x20, + 0x32, + ]), + expected: { + type: 0x22, + size: 20, + numberOfZones: 2, + zones: [ + { + size: 0x08, + nameLength: 0x06, + name: 'zone 1', + }, + { + size: 0x08, + nameLength: 0x06, + name: 'zone 2', + }, + ], + }, + decoder: Decoders.ZoneCollisionDetection, + }, + { + description: 'Zone Object', + bytes: new Uint8Array([0x08, 0x06, 0x7a, 0x6f, 0x6e, 0x65, 0x20, 0x31]), + expected: { + size: 0x08, + nameLength: 0x06, + name: 'zone 1', + }, + decoder: Decoders.ZoneObject, + }, ]; describe('RTTrP Bytes Decoding', () => { From 00cf0e9301b222bd007356bf1d4fa51ab0673876 Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 10:19:56 -0500 Subject: [PATCH 6/8] add decoder for trackable --- src/decoders/index.ts | 2 + src/decoders/motion/trackable.ts | 98 ++++++++++++++++++++++++++++++++ src/models.ts | 19 +++++++ tests/decode.test.js | 53 +++++++++++++++++ 4 files changed, 172 insertions(+) create mode 100644 src/decoders/motion/trackable.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index c96275a..a7bfd5d 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -2,6 +2,7 @@ import CentroidAccelVelocity from './motion/centroid-accel-velocity'; import CentroidPosition from './motion/centroid-position'; import OrientationEuler from './motion/orientation-euler'; import OrientationQuaternion from './motion/orientation-quaternion'; +import Trackable from './motion/trackable'; import TrackedPointAccelVelocity from './motion/tracked-point-accel-velocity'; import TrackedPointPosition from './motion/tracked-point-position'; import ZoneCollisionDetection from './motion/zone-collision-detection'; @@ -10,6 +11,7 @@ import RTTrPHeader from './rttrp'; export const Decoders = { RTTrPHeader, + Trackable, CentroidPosition, CentroidAccelVelocity, TrackedPointPosition, diff --git a/src/decoders/motion/trackable.ts b/src/decoders/motion/trackable.ts new file mode 100644 index 0000000..bef1cb6 --- /dev/null +++ b/src/decoders/motion/trackable.ts @@ -0,0 +1,98 @@ +import { Decoders } from '..'; +import { Trackable, TrackableModule } from '../../models'; + +const textDecoder = new TextDecoder('utf-8'); + +export default (bytes: Uint8Array, isLittleEndian: boolean = false): Trackable => { + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + const type = view.getUint8(0); + dataOffset += 1; + if (type !== 0x01 && type !== 0x51) { + throw new Error('Trackable module must have a type of 0x01 or 0x51'); + } + + const size = view.getUint16(dataOffset, isLittleEndian); + dataOffset += 2; + if (size !== bytes.byteLength) { + throw new Error('Trackable module size mismatch'); + } + const nameLength = view.getUint8(dataOffset); + dataOffset += 1; + const name = textDecoder.decode(bytes.subarray(4, 4 + nameLength)); + dataOffset += nameLength; + let timestamp; + + if (type === 0x51) { + timestamp = view.getUint32(dataOffset, isLittleEndian); + dataOffset += 4; + } + const numberOfModules = view.getUint8(dataOffset); + dataOffset += 1; + + const modules: TrackableModule[] = []; + let trackableModuleOffset = dataOffset; + + for (let index = 0; index < numberOfModules; index++) { + const moduleData = bytes.subarray(trackableModuleOffset); + const moduleType = moduleData[0]; + switch (moduleType) { + case 0x02: { + const module = Decoders.CentroidPosition(moduleData, isLittleEndian); + modules.push(module); + trackableModuleOffset += module.size; + break; + } + case 0x03: { + const module = Decoders.OrientationQuaternion(moduleData, isLittleEndian); + modules.push(module); + trackableModuleOffset += module.size; + break; + } + case 0x04: { + const module = Decoders.OrientationEuler(moduleData, isLittleEndian); + modules.push(module); + trackableModuleOffset += module.size; + break; + } + case 0x06: { + const module = Decoders.TrackedPointPosition(moduleData, isLittleEndian); + modules.push(module); + trackableModuleOffset += module.size; + break; + } + case 0x20: { + const module = Decoders.CentroidAccelVelocity(moduleData, isLittleEndian); + modules.push(module); + trackableModuleOffset += module.size; + break; + } + case 0x21: { + const module = Decoders.TrackedPointAccelVelocity(moduleData, isLittleEndian); + modules.push(module); + trackableModuleOffset += module.size; + break; + } + case 0x22: { + const module = Decoders.ZoneCollisionDetection(moduleData, isLittleEndian); + modules.push(module); + trackableModuleOffset += module.size; + break; + } + default: + console.error(`unknown trackable submodule type: ${moduleType}`); + break; + } + } + + return { + type, + size, + nameLength, + name, + timestamp, + numberOfModules, + modules, + }; +}; diff --git a/src/models.ts b/src/models.ts index 83a5883..2b58719 100644 --- a/src/models.ts +++ b/src/models.ts @@ -9,6 +9,25 @@ export type RTTrPHeader = { subModuleCount: number; }; +export type TrackableModule = + | CentroidPosition + | CentroidAccelVelocity + | TrackedPointPosition + | TrackedPointAccelVelocity + | OrientationEuler + | OrientationQuaternion + | ZoneCollisionDetection; + +export type Trackable = { + type: number; + size: number; + nameLength: number; + name: string; + timestamp?: number; + numberOfModules: number; + modules: TrackableModule[]; +}; + export type CentroidPosition = { type: number; size: number; diff --git a/tests/decode.test.js b/tests/decode.test.js index 3061d14..d2abf26 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -166,6 +166,59 @@ const goodTests = [ }, decoder: Decoders.ZoneObject, }, + { + description: 'Trackable (with Timestamp) + Centroid Position Module', + bytes: new Uint8Array([ + 81, 0, 42, 4, 84, 101, 115, 116, 103, 213, 152, 27, 1, 2, 0, 29, 0, 100, 63, 240, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, + 0, 0, 0, 0, 64, 8, 0, 0, 0, 0, 0, 0, + ]), + expected: { + type: 0x51, + size: 42, + nameLength: 4, + name: 'Test', + timestamp: 1742051355, + numberOfModules: 1, + modules: [ + { + type: 0x02, + size: 29, + latency: 100, + x: 1, + y: 2, + z: 3, + }, + ], + }, + decoder: Decoders.Trackable, + }, + { + description: 'Trackable (with Timestamp) + Tracked Point Position Module', + bytes: new Uint8Array([ + 81, 0, 43, 4, 84, 101, 115, 116, 103, 213, 152, 27, 1, 6, 0, 30, 0, 100, 63, 240, 0, 0, 0, 0, 0, 0, 64, 0, 0, 0, + 0, 0, 0, 0, 64, 8, 0, 0, 0, 0, 0, 0, 1, + ]), + expected: { + type: 0x51, + size: 43, + nameLength: 4, + name: 'Test', + timestamp: 1742051355, + numberOfModules: 1, + modules: [ + { + type: 0x06, + size: 30, + latency: 100, + x: 1, + y: 2, + z: 3, + index: 1, + }, + ], + }, + decoder: Decoders.Trackable, + }, ]; describe('RTTrP Bytes Decoding', () => { From 7c04c6febea17757a108200fab910af5f60c69c3 Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 10:39:14 -0500 Subject: [PATCH 7/8] clean up header decoder --- src/decoders/index.ts | 2 +- src/decoders/{rttrp.ts => rttrp-header.ts} | 1 + src/models.ts | 4 +++- 3 files changed, 5 insertions(+), 2 deletions(-) rename src/decoders/{rttrp.ts => rttrp-header.ts} (98%) diff --git a/src/decoders/index.ts b/src/decoders/index.ts index a7bfd5d..def5275 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -7,7 +7,7 @@ import TrackedPointAccelVelocity from './motion/tracked-point-accel-velocity'; import TrackedPointPosition from './motion/tracked-point-position'; import ZoneCollisionDetection from './motion/zone-collision-detection'; import ZoneObject from './motion/zone-object'; -import RTTrPHeader from './rttrp'; +import RTTrPHeader from './rttrp-header'; export const Decoders = { RTTrPHeader, diff --git a/src/decoders/rttrp.ts b/src/decoders/rttrp-header.ts similarity index 98% rename from src/decoders/rttrp.ts rename to src/decoders/rttrp-header.ts index 192acf7..167374a 100644 --- a/src/decoders/rttrp.ts +++ b/src/decoders/rttrp-header.ts @@ -42,5 +42,6 @@ export default (bytes: Uint8Array): RTTrPHeader => { size, context, subModuleCount, + isLittleEndian, }; }; diff --git a/src/models.ts b/src/models.ts index 2b58719..48ad724 100644 --- a/src/models.ts +++ b/src/models.ts @@ -1,5 +1,5 @@ export type RTTrPHeader = { - intHeader?: number; + intHeader: number; floatHeader: number; version: number; packetID: number; @@ -7,6 +7,8 @@ export type RTTrPHeader = { size: number; context: number; subModuleCount: number; + isLittleEndian: boolean; +}; }; export type TrackableModule = From ec8e8bd76b2e4e03bbded6a187d232e38c63aa37 Mon Sep 17 00:00:00 2001 From: Joel Wetzell Date: Sat, 15 Mar 2025 10:39:48 -0500 Subject: [PATCH 8/8] add decoder for RTTrPM packet --- src/decoders/index.ts | 2 ++ src/decoders/motion/rttrpm.ts | 38 +++++++++++++++++++++++++++++++++++ src/models.ts | 4 ++++ tests/decode.test.js | 32 +++++++++++++++++++++++++++++ 4 files changed, 76 insertions(+) create mode 100644 src/decoders/motion/rttrpm.ts diff --git a/src/decoders/index.ts b/src/decoders/index.ts index def5275..e55d22a 100644 --- a/src/decoders/index.ts +++ b/src/decoders/index.ts @@ -2,6 +2,7 @@ import CentroidAccelVelocity from './motion/centroid-accel-velocity'; 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 TrackedPointAccelVelocity from './motion/tracked-point-accel-velocity'; import TrackedPointPosition from './motion/tracked-point-position'; @@ -11,6 +12,7 @@ import RTTrPHeader from './rttrp-header'; export const Decoders = { RTTrPHeader, + RTTrPM, Trackable, CentroidPosition, CentroidAccelVelocity, diff --git a/src/decoders/motion/rttrpm.ts b/src/decoders/motion/rttrpm.ts new file mode 100644 index 0000000..1261574 --- /dev/null +++ b/src/decoders/motion/rttrpm.ts @@ -0,0 +1,38 @@ +import { Decoders } from '..'; +import { RTTrPM, Trackable } from '../../models'; + +export default (bytes: Uint8Array): RTTrPM => { + if (bytes.byteLength < 18) { + throw new Error('RTTrPM packet must be at least 18 bytes long'); + } + const header = Decoders.RTTrPHeader(bytes.subarray(0, 18)); + + if (bytes.byteLength !== header.size) { + throw new Error('RTTrPM packet size mismatch'); + } + + const modules: Trackable[] = []; + let packetModuleOffset = 18; + + for (let index = 0; index < header.subModuleCount; index++) { + const moduleData = bytes.subarray(packetModuleOffset); + const moduleType = moduleData[0]; + switch (moduleType) { + case 0x01: + case 0x51: { + const module = Decoders.Trackable(moduleData, header.isLittleEndian); + modules.push(module); + packetModuleOffset += module.size; + break; + } + default: + console.error(`unknown RTTrPM submodule type: ${moduleType}`); + break; + } + } + + return { + header, + modules, + }; +}; diff --git a/src/models.ts b/src/models.ts index 48ad724..59b32e7 100644 --- a/src/models.ts +++ b/src/models.ts @@ -9,6 +9,10 @@ export type RTTrPHeader = { subModuleCount: number; isLittleEndian: boolean; }; + +export type RTTrPM = { + header: RTTrPHeader; + modules: Trackable[]; }; export type TrackableModule = diff --git a/tests/decode.test.js b/tests/decode.test.js index d2abf26..380e59c 100644 --- a/tests/decode.test.js +++ b/tests/decode.test.js @@ -16,6 +16,7 @@ const goodTests = [ size: 18, context: 305419896, subModuleCount: 0, + isLittleEndian: false, }, decoder: Decoders.RTTrPHeader, }, @@ -219,6 +220,37 @@ const goodTests = [ }, decoder: Decoders.Trackable, }, + { + description: 'RTTrPM + Empty Trackable', + bytes: new Uint8Array([ + 65, 84, 67, 52, 0, 2, 0, 0, 0, 1, 0, 0, 27, 18, 52, 86, 120, 1, 1, 0, 9, 4, 84, 101, 115, 116, 0, + ]), + expected: { + header: { + intHeader: 0x4154, + floatHeader: 0x4334, + version: 2, + packetID: 1, + packetFormat: 0, + size: 27, + context: 0x12345678, + subModuleCount: 1, + isLittleEndian: false, + }, + modules: [ + { + type: 0x01, + size: 9, + nameLength: 4, + name: 'Test', + timestamp: undefined, + numberOfModules: 0, + modules: [], + }, + ], + }, + decoder: Decoders.RTTrPM, + }, ]; describe('RTTrP Bytes Decoding', () => {