diff --git a/src/encoders/index.ts b/src/encoders/index.ts index e174b89..bde4ce3 100644 --- a/src/encoders/index.ts +++ b/src/encoders/index.ts @@ -1,8 +1,10 @@ +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 RTTrPHeader from './rttrp-header'; @@ -14,4 +16,6 @@ export const Encoders = { OrientationQuaternion, OrientationEuler, TrackedPointPosition, + CentroidAccelVelocity, + TrackedPointAccelVelocity, }; diff --git a/src/encoders/motion/centroid-accel-velocity.ts b/src/encoders/motion/centroid-accel-velocity.ts new file mode 100644 index 0000000..c98cfa4 --- /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, 0x02); + 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/tracked-point-accel-velocity.ts b/src/encoders/motion/tracked-point-accel-velocity.ts new file mode 100644 index 0000000..815c802 --- /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, 0x02); + 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; +};