Merge branch 'main' into feat/motion-decode

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);

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,
 };

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;
+};

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;
+};

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;
+};

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;
+};

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;
+};

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);
+};

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;
+};

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;
+};

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;
+};

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;
+};

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', [])]);
+    },
   },
 ];