diff --git a/example/encode.js b/example/encode.js new file mode 100644 index 0000000..f35d4df --- /dev/null +++ b/example/encode.js @@ -0,0 +1,20 @@ +const { Encoders } = require('../'); +const dgram = require('dgram'); + +const motionBytes = Encoders.RTTrPM(0x01, 0x12345678, [ + Encoders.Trackable( + 'Test', + [ + Encoders.CentroidPosition(100, 1.0, 2.0, 3.0), + Encoders.CentroidAccelVelocity(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0), + Encoders.TrackedPointPosition(100, 1.0, 2.0, 3.0, 1), + Encoders.TrackedPointAccelVelocity(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 1), + Encoders.TrackedPointPosition(100, 1.0, 2.0, 3.0, 2), + Encoders.TrackedPointAccelVelocity(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 2), + Encoders.OrientationEuler(100, 0x0123, 1, 2, 3), + Encoders.OrientationQuaternion(100, 1, 2, 3, 4), + ], + Math.floor(Date.now() / 1000) + ), +]); +console.log(motionBytes); diff --git a/src/encoders/index.ts b/src/encoders/index.ts index 15fcaa5..77121f3 100644 --- a/src/encoders/index.ts +++ b/src/encoders/index.ts @@ -1,5 +1,25 @@ -import RTTrPHeader from './rttrp'; +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'; +import ZoneCollisionDetection from './motion/zone-collision-detection'; +import ZoneObject from './motion/zone-object'; +import RTTrPHeader from './rttrp-header'; export const Encoders = { RTTrPHeader, + RTTrPM, + Trackable, + CentroidPosition, + OrientationQuaternion, + OrientationEuler, + TrackedPointPosition, + CentroidAccelVelocity, + TrackedPointAccelVelocity, + ZoneCollisionDetection, + ZoneObject, }; diff --git a/src/encoders/motion/centroid-accel-velocity.ts b/src/encoders/motion/centroid-accel-velocity.ts new file mode 100644 index 0000000..aedd91f --- /dev/null +++ b/src/encoders/motion/centroid-accel-velocity.ts @@ -0,0 +1,43 @@ +export default ( + x: number, + y: number, + z: number, + accelX: number, + accelY: number, + accelZ: number, + velocityX: number, + velocityY: number, + velocityZ: number, + isLittleEndian: boolean = false +): Uint8Array => { + const bytes = new Uint8Array(51); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, 0x20); + dataOffset += 1; + view.setUint16(dataOffset, bytes.byteLength, isLittleEndian); + dataOffset += 2; + + view.setFloat64(dataOffset, x, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, y, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, z, isLittleEndian); + dataOffset += 8; + + view.setFloat32(dataOffset, accelX, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, accelY, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, accelZ, isLittleEndian); + dataOffset += 4; + + view.setFloat32(dataOffset, velocityX, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, velocityY, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, velocityZ, isLittleEndian); + + return bytes; +}; diff --git a/src/encoders/motion/centroid-position.ts b/src/encoders/motion/centroid-position.ts new file mode 100644 index 0000000..da271ef --- /dev/null +++ b/src/encoders/motion/centroid-position.ts @@ -0,0 +1,18 @@ +export default (latency: number, x: number, y: number, z: number, isLittleEndian: boolean = false): Uint8Array => { + const bytes = new Uint8Array(29); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, 0x02); + dataOffset += 1; + view.setUint16(dataOffset, bytes.byteLength, isLittleEndian); + dataOffset += 2; + view.setUint16(dataOffset, latency, isLittleEndian); + dataOffset += 2; + view.setFloat64(dataOffset, x, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, y, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, z, isLittleEndian); + return bytes; +}; 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; +}; diff --git a/src/encoders/motion/rttrpm.ts b/src/encoders/motion/rttrpm.ts new file mode 100644 index 0000000..6c2ab41 --- /dev/null +++ b/src/encoders/motion/rttrpm.ts @@ -0,0 +1,27 @@ +import RTTrPHeader from '../rttrp-header'; + +export default ( + packetId: number, + context: number, + trackables: Uint8Array[], + isLittleEndian: boolean = false +): Uint8Array => { + let totalPacketSize = 18; //header size + + trackables.forEach((trackable) => { + totalPacketSize += trackable.length; + }); + const bytes = new Uint8Array(totalPacketSize); + + let dataOffset = 0; + + const headerBytes = RTTrPHeader(0x4334, 0x02, packetId, 0x00, context, trackables, isLittleEndian); + + bytes.set(headerBytes, dataOffset); + dataOffset += headerBytes.length; + trackables.forEach((trackable) => { + bytes.set(trackable, dataOffset); + dataOffset += trackable.length; + }); + return bytes; +}; diff --git a/src/encoders/motion/trackable.ts b/src/encoders/motion/trackable.ts new file mode 100644 index 0000000..3349ae6 --- /dev/null +++ b/src/encoders/motion/trackable.ts @@ -0,0 +1,47 @@ +const textEncoder = new TextEncoder(); + +export default ( + name: string, + subModules: Uint8Array[], + timestamp?: number, + isLittleEndian: boolean = false +): Uint8Array => { + let totalTrackableSize = 1 + 2 + 1 + name.length + 1; + let type = 0x01; + if (timestamp !== undefined) { + totalTrackableSize += 4; + type = 0x51; + } + subModules.forEach((subModule) => { + totalTrackableSize += subModule.length; + }); + const bytes = new Uint8Array(totalTrackableSize); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, type); + dataOffset += 1; + view.setUint16(dataOffset, totalTrackableSize, isLittleEndian); + dataOffset += 2; + const nameBytes = textEncoder.encode(name); + view.setUint8(dataOffset, nameBytes.length); + dataOffset += 1; + + nameBytes.forEach((byte) => { + view.setUint8(dataOffset, byte); + dataOffset += 1; + }); + + if (timestamp !== undefined) { + view.setUint32(dataOffset, timestamp, isLittleEndian); + dataOffset += 4; + } + + view.setUint8(dataOffset, subModules.length); + dataOffset += 1; + subModules.forEach((subModule) => { + bytes.set(subModule, dataOffset); + dataOffset += subModule.length; + }); + return new Uint8Array(view.buffer, view.byteOffset, view.byteLength); +}; diff --git a/src/encoders/motion/tracked-point-accel-velocity.ts b/src/encoders/motion/tracked-point-accel-velocity.ts new file mode 100644 index 0000000..c8e4e0a --- /dev/null +++ b/src/encoders/motion/tracked-point-accel-velocity.ts @@ -0,0 +1,46 @@ +export default ( + x: number, + y: number, + z: number, + accelX: number, + accelY: number, + accelZ: number, + velocityX: number, + velocityY: number, + velocityZ: number, + index: number, + isLittleEndian: boolean = false +): Uint8Array => { + const bytes = new Uint8Array(52); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, 0x21); + dataOffset += 1; + view.setUint16(dataOffset, bytes.byteLength, isLittleEndian); + dataOffset += 2; + + view.setFloat64(dataOffset, x, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, y, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, z, isLittleEndian); + dataOffset += 8; + + view.setFloat32(dataOffset, accelX, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, accelY, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, accelZ, isLittleEndian); + dataOffset += 4; + + view.setFloat32(dataOffset, velocityX, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, velocityY, isLittleEndian); + dataOffset += 4; + view.setFloat32(dataOffset, velocityZ, isLittleEndian); + dataOffset += 4; + view.setUint8(dataOffset, index); + + return bytes; +}; diff --git a/src/encoders/motion/tracked-point-position.ts b/src/encoders/motion/tracked-point-position.ts new file mode 100644 index 0000000..f13f2c4 --- /dev/null +++ b/src/encoders/motion/tracked-point-position.ts @@ -0,0 +1,28 @@ +export default ( + latency: number, + x: number, + y: number, + z: number, + index: number, + isLittleEndian: boolean = false +): Uint8Array => { + const bytes = new Uint8Array(30); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, 0x06); + dataOffset += 1; + view.setUint16(dataOffset, bytes.byteLength, isLittleEndian); + dataOffset += 2; + view.setUint16(dataOffset, latency, isLittleEndian); + dataOffset += 2; + view.setFloat64(dataOffset, x, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, y, isLittleEndian); + dataOffset += 8; + view.setFloat64(dataOffset, z, isLittleEndian); + dataOffset += 8; + view.setUint8(dataOffset, index); + + return bytes; +}; diff --git a/src/encoders/motion/zone-collision-detection.ts b/src/encoders/motion/zone-collision-detection.ts new file mode 100644 index 0000000..8c37ddc --- /dev/null +++ b/src/encoders/motion/zone-collision-detection.ts @@ -0,0 +1,23 @@ +export default (zoneObjectModules: Uint8Array[], isLittleEndian: boolean = false): Uint8Array => { + let totalZoneModuleSize = 1 + 2 + 1; + + zoneObjectModules.forEach((zoneObjectModule) => { + totalZoneModuleSize += zoneObjectModule.byteLength; + }); + + const bytes = new Uint8Array(totalZoneModuleSize); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, 0x22); + dataOffset += 1; + view.setUint16(dataOffset, bytes.byteLength, isLittleEndian); + dataOffset += 2; + view.setUint8(dataOffset, zoneObjectModules.length); + dataOffset += 1; + zoneObjectModules.forEach((zoneObjectModule) => { + bytes.set(zoneObjectModule, dataOffset); + dataOffset += zoneObjectModule.length; + }); + return bytes; +}; diff --git a/src/encoders/motion/zone-object.ts b/src/encoders/motion/zone-object.ts new file mode 100644 index 0000000..e9731c1 --- /dev/null +++ b/src/encoders/motion/zone-object.ts @@ -0,0 +1,20 @@ +const textEncoder = new TextEncoder(); + +export default (name: string): Uint8Array => { + const totalZoneObjectModuleSize = 1 + 1 + name.length; + const bytes = new Uint8Array(totalZoneObjectModuleSize); + const view = new DataView(bytes.buffer, bytes.byteOffset, bytes.byteLength); + + let dataOffset = 0; + view.setUint8(dataOffset, totalZoneObjectModuleSize); + dataOffset += 1; + view.setUint8(dataOffset, name.length); + dataOffset += 1; + + const nameBytes = textEncoder.encode(name); + nameBytes.forEach((byte) => { + view.setUint8(dataOffset, byte); + dataOffset += 1; + }); + return bytes; +}; diff --git a/tests/encode.test.js b/tests/encode.test.js index 519c523..bc86b13 100644 --- a/tests/encode.test.js +++ b/tests/encode.test.js @@ -12,6 +12,23 @@ const goodTests = [ return Encoders.RTTrPHeader(0x4334, 2, 0x12345678, 0, 0x12345678, [], false); }, }, + { + description: 'RTTrP Header Little Endian', + expected: new Uint8Array([ + 0x54, 0x41, 0x34, 0x43, 0x00, 0x02, 0x78, 0x56, 0x34, 0x12, 0x00, 0x12, 0x00, 0x78, 0x56, 0x34, 0x12, 0x00, + ]), + bytes: () => { + return Encoders.RTTrPHeader(0x4334, 2, 0x12345678, 0, 0x12345678, [], true); + }, + }, + { + description: 'Empty RTTrPM', + expected: 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, + ]), + bytes: () => { + return Encoders.RTTrPM(0x01, 0x12345678, [Encoders.Trackable('Test', [])]); + }, }, ];