MustafaMulla29/rp2040-bldc-motor-controller-new

assembly/attachments.ts

import { Euler, Matrix4, Vector3, MathUtils } from "three"
import type { CadComponent, CircuitJson } from "circuit-json"
import { dimensions as d } from "./dimensions"

export type Point = { x: number; y: number; z: number }
export type Pose = { position: Point; rotation: Point }
export type Port = {
  position: Point
  direction: Point
  kind: "fixed_wire" | "bullet_male" | "bullet_female"
  diameter: number
}
export type Part = { name: string; pose: Pose; ports: Record<string, Port> }
export const phases = ["U", "V", "W"] as const

/** CAD-local mm to right-handed circuit-world mm (+X right, +Y top, +Z above).
 * Paired with core renderAssemblyCadModel: intrinsic XYZ = Rx * Ry * Rz.
 * Points acquire translation; directions acquire rotation only.
 */
export function poseMatrix(pose: Pose) {
  const r = pose.rotation
  return new Matrix4().makeRotationFromEuler(new Euler(
    MathUtils.degToRad(r.x), MathUtils.degToRad(r.y), MathUtils.degToRad(r.z), "XYZ",
  )).setPosition(pose.position.x, pose.position.y, pose.position.z)
}
export const point = (v: Vector3): Point => ({ x: v.x, y: v.y, z: v.z })
export const vector = (p: Point) => new Vector3(p.x, p.y, p.z)

/** Resolve a named, component-owned local interface in circuit-world mm. */
export function matingPoint(part: Part, portName: string, insertion = 0) {
  const port = part.ports[portName]
  if (!port) throw new Error(`No ${portName} connection on ${part.name}`)
  const matrix = poseMatrix(part.pose)
  const direction = vector(port.direction).transformDirection(matrix)
  const position = vector(port.position).applyMatrix4(matrix).addScaledVector(direction, -insertion)
  return { position: point(position), direction: point(direction) }
}

export const defaultMotorPose: Pose = {
  position: { x: d.layout.motor[0], y: d.layout.motor[1], z: d.layout.motor[2] },
  rotation: { x: 0, y: 0, z: 0 },
}

export function motorPart(pose: Pose = defaultMotorPose): Part {
  return {
    name: "MOTOR_ENVELOPE", pose,
    ports: Object.fromEntries(phases.map((phase, i) => [phase, {
      position: { x: (i - 1) * 3, y: -d.motor.bodyDiameter / 2 + 1, z: 4 },
      direction: { x: 0, y: -1, z: 0 }, kind: "fixed_wire", diameter: d.illustrativeLeads.wireDiameter,
    } satisfies Port])),
  }
}

// Supplier OBJ-local measurements, not PCB/world positions. These stay with the
// exact C7437324 model; its imported origin, rotation and position come from PCB CAD.
// The raised circular lip is at +X (13.25 mm); -X is the rear face.
export const socketLocalMouth = { x: 13.25, y: 0, z: 7.5 }
export function socketPart(cad: CadComponent, phase: number): Part {
  const origin = cad.model_origin_position ?? { x: 0, y: 0, z: 0 }
  const scale = cad.model_unit_to_mm_scale_factor ?? 1
  return { name: `J${10 + phase}`, pose: { position: cad.position, rotation: cad.rotation ?? { x: 0, y: 0, z: 0 } }, ports: {
    mouth: {
      position: point(vector(socketLocalMouth).sub(vector(origin)).multiplyScalar(scale)),
      direction: { x: 1, y: 0, z: 0 }, kind: "bullet_female", diameter: d.socket.nominalBore,
    },
  } }
}

export function selectSocketCad(json: CircuitJson): CadComponent[] {
  return phases.map((_, i) => {
    const source = json.find(p => p.type === "source_component" && p.name === `J${10 + i}`)
    const cad = json.find((p): p is CadComponent => p.type === "cad_component" &&
      !!source && "source_component_id" in source && p.source_component_id === source.source_component_id)
    if (!cad?.model_obj_url?.includes("C7437324")) throw new Error(`Review the imported model for J${10 + i}`)
    return cad
  })
}

export const adapterShoulderZ = d.adapter.illustrativeMaleLength - d.adapter.illustrativeBoardInsertion
export const adapterMouthZ = adapterShoulderZ + d.adapter.illustrativeSleeveLength

/** Adapter-local +Z points out of the socket towards the motor; origin is the
 * PCB socket mouth. Reuse the socket CAD rotation, then turn local +Z to +X.
 * This seats the adapter from the current PCB geometry, including rotations.
 */
export function adapterPart(socket: Part, phase: number): Part {
  const mouth = matingPoint(socket, "mouth")
  const orientation = poseMatrix(socket.pose).multiply(new Matrix4().makeRotationY(Math.PI / 2))
  const r = new Euler().setFromRotationMatrix(orientation, "XYZ")
  return { name: `RIGID_ADAPTER_${phases[phase]}`, pose: {
    position: mouth.position,
    rotation: { x: MathUtils.radToDeg(r.x), y: MathUtils.radToDeg(r.y), z: MathUtils.radToDeg(r.z) },
  }, ports: {
    board: { position: { x: 0, y: 0, z: -d.adapter.illustrativeBoardInsertion },
      direction: { x: 0, y: 0, z: -1 }, kind: "bullet_male", diameter: d.adapter.maleNominalDiameter },
    motor: { position: { x: 0, y: 0, z: adapterMouthZ },
      direction: { x: 0, y: 0, z: 1 }, kind: "bullet_female", diameter: d.adapter.femaleNominalDiameter },
  } }
}

export function assemblyParts(socketCad: CadComponent[], motorPose = defaultMotorPose) {
  if (socketCad.length !== 3) throw new Error("The motor assembly needs three PCB phase sockets")
  const sockets = socketCad.map(socketPart)
  return { motor: motorPart(motorPose), sockets, adapters: sockets.map(adapterPart) }
}