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', () => {