feat: pitch and yaw limits for inputs

src/core/blitz/applyPitchYawLimits.ts

@@ -0,0 +1,81 @@
+import { clamp } from 'lodash';
+import { PitchLimits, YawLimits } from '../blitzkrieg/modelDefinitions';
+import { normalizeAnglePI } from '../math/normalizeAngle180';
+
+export function applyPitchYawLimits(
+  pitch: number, // rad
+  yaw: number, // rad
+  pitchLimits: PitchLimits, // deg
+  yawLimits?: YawLimits, // deg
+) {
+  let newPitch = pitch;
+  let newYaw = yaw;
+
+  if (yawLimits) {
+    newYaw = normalizeAnglePI(
+      clamp(
+        newYaw,
+        -yawLimits.max * (Math.PI / 180),
+        -yawLimits.min * (Math.PI / 180),
+      ),
+    );
+  }
+
+  let lowerPitch = -pitchLimits.max * (Math.PI / 180);
+  let upperPitch = -pitchLimits.min * (Math.PI / 180);
+  const transition = (pitchLimits.transition ?? 10) * (Math.PI / 180);
+
+  if (pitchLimits.back) {
+    const range = pitchLimits.back.range * (Math.PI / 180);
+    const yawRotatedAbs = Math.abs(normalizeAnglePI(newYaw - Math.PI));
+
+    if (yawRotatedAbs <= range + transition) {
+      if (yawRotatedAbs <= range) {
+        // inside range
+        lowerPitch = -pitchLimits.back.max * (Math.PI / 180);
+        upperPitch = -pitchLimits.back.min * (Math.PI / 180);
+      } else {
+        // inside transition
+        const transitionProgress = (yawRotatedAbs - range) / transition;
+        const maxInterpolatedPitch =
+          (1 - transitionProgress) * pitchLimits.back.max +
+          transitionProgress * pitchLimits.max;
+        const minInterpolatedPitch =
+          (1 - transitionProgress) * pitchLimits.back.min +
+          transitionProgress * pitchLimits.min;
+
+        lowerPitch = -maxInterpolatedPitch * (Math.PI / 180);
+        upperPitch = -minInterpolatedPitch * (Math.PI / 180);
+      }
+    }
+  }
+
+  if (pitchLimits.front) {
+    const range = pitchLimits.front.range * (Math.PI / 180);
+    const yawAbs = Math.abs(normalizeAnglePI(newYaw));
+
+    if (yawAbs <= range + transition) {
+      if (yawAbs <= range) {
+        // inside range
+        lowerPitch = -pitchLimits.front.max * (Math.PI / 180);
+        upperPitch = -pitchLimits.front.min * (Math.PI / 180);
+      } else {
+        // inside transition
+        const transitionProgress = (yawAbs - range) / transition;
+        const maxInterpolatedPitch =
+          (1 - transitionProgress) * pitchLimits.front.max +
+          transitionProgress * pitchLimits.max;
+        const minInterpolatedPitch =
+          (1 - transitionProgress) * pitchLimits.front.min +
+          transitionProgress * pitchLimits.min;
+
+        lowerPitch = -maxInterpolatedPitch * (Math.PI / 180);
+        upperPitch = -minInterpolatedPitch * (Math.PI / 180);
+      }
+    }
+  }
+
+  newPitch = clamp(newPitch, lowerPitch, upperPitch);
+
+  return [newPitch, newYaw];
+}