Function computeNormalAlignment

src/utils/sim3dAlignment.ts:55–107

(
  point1: THREE.Vector3,
  point2: THREE.Vector3,
  point3: THREE.Vector3,
  flipNormal = false,
  targetUp: THREE.Vector3 = new THREE.Vector3(0, 1, 0)
)

 * The plane is defined by three points, and rotation is about their centroid.
 */
export function computeNormalAlignment(
  point1: THREE.Vector3,
  point2: THREE.Vector3,
  point3: THREE.Vector3,
  flipNormal = false,
  targetUp: THREE.Vector3 = new THREE.Vector3(0, 1, 0)
): Sim3d {
  const v1 = new THREE.Vector3().subVectors(point2, point1);
  const v2 = new THREE.Vector3().subVectors(point3, point1);
  const normal = new THREE.Vector3().crossVectors(v1, v2);

  if (normal.lengthSq() < 1e-10) {
    return identitySim3d();
  }

  normal.normalize();

  if (flipNormal) {
    normal.negate();
  }

  const upNormalized = targetUp.clone().normalize();
  const dot = normal.dot(upNormalized);
  if (Math.abs(dot - 1) < 1e-6) {
    return identitySim3d();
  }

  let rotation: THREE.Quaternion;

  if (Math.abs(dot + 1) < 1e-6) {
    const perpAxis = new THREE.Vector3(1, 0, 0);
    if (Math.abs(upNormalized.dot(perpAxis)) > 0.9) {
      perpAxis.set(0, 0, 1);
    }
    rotation = new THREE.Quaternion().setFromAxisAngle(perpAxis, Math.PI);
  } else {
    rotation = new THREE.Quaternion().setFromUnitVectors(normal, upNormalized);
  }

  const centroid = new THREE.Vector3()
    .add(point1)
    .add(point2)
    .add(point3)
    .divideScalar(3);
  const rotatedCentroid = centroid.clone().applyQuaternion(rotation);
  const translation = new THREE.Vector3().subVectors(centroid, rotatedCentroid);

  return {
    scale: 1,
    rotation,
    translation,
  };
}

/**
 * Compute a transform that aligns a detected floor plane with a target axis