Function parseRigsBinary

src/parsers/rigs.ts:23–86

(buffer: ArrayBuffer)

 *     - if has_pose: double[7] (qw, qx, qy, qz, tx, ty, tz)
 */
export function parseRigsBinary(buffer: ArrayBuffer): Map<number, Rig> {
  const reader = new BinaryReader(buffer);
  const rigs = new Map<number, Rig>();

  const numRigs = reader.readUint64AsNumber();

  for (let i = 0; i < numRigs; i++) {
    const rigId = reader.readUint32();
    const numSensors = reader.readUint32();

    const sensors: RigSensor[] = [];
    let refSensorId: SensorId | null = null;

    if (numSensors > 0) {
      // Read reference sensor (first sensor, always has identity pose)
      const refType = parseSensorType(reader.readInt32(), `binary rig ${rigId} reference sensor`);
      const refId = reader.readUint32();
      refSensorId = { type: refType, id: refId };

      sensors.push({
        sensorId: refSensorId,
        hasPose: false, // Reference sensor has identity pose (implicit)
      });

      // Read additional sensors
      for (let j = 1; j < numSensors; j++) {
        const sensorType = parseSensorType(reader.readInt32(), `binary rig ${rigId} sensor ${j}`);
        const sensorId = reader.readUint32();
        const hasPose = reader.readUint8() !== 0;

        const sensor: RigSensor = {
          sensorId: { type: sensorType, id: sensorId },
          hasPose,
        };

        if (hasPose) {
          // Read sensor_from_rig pose: qw, qx, qy, qz, tx, ty, tz
          const qw = reader.readFloat64();
          const qx = reader.readFloat64();
          const qy = reader.readFloat64();
          const qz = reader.readFloat64();
          const tx = reader.readFloat64();
          const ty = reader.readFloat64();
          const tz = reader.readFloat64();

          sensor.pose = {
            qvec: [qw, qx, qy, qz],
            tvec: [tx, ty, tz],
          };
        }

        sensors.push(sensor);
      }
    }

    rigs.set(rigId, {
      rigId,
      refSensorId,