Dwinzo_dev/app/src/modules/simulation/roboticArm/functions/collectArcPoints.ts

29 lines
1.2 KiB
TypeScript

import { findNearestIndex } from "./findNearestIndex";
// Helper function to collect points and check forbidden degrees
type PointWithDegree = {
position: [number, number, number];
degree: number;
};
export function collectArcPoints(circlePointsWithDegrees: PointWithDegree[], startIdx: number, endIdx: number, clockwise: boolean) {
const totalSegments = 64;
const arcPoints: [number, number, number][] = [];
let i = startIdx;
while (i !== (endIdx + (clockwise ? 1 : -1) + totalSegments) % totalSegments) {
const { degree, position } = circlePointsWithDegrees[i];
// Skip over
arcPoints.push(position);
i = (i + (clockwise ? 1 : -1) + totalSegments) % totalSegments;
}
return arcPoints;
};
//Range to restrict angle
export function hasForbiddenDegrees(circlePoints: [number, number, number][], circlePointsWithDegrees: PointWithDegree[], arc: [number, number, number][]) {
return arc.some(p => {
const idx = findNearestIndex(p, circlePoints);
const degree = circlePointsWithDegrees[idx]?.degree || 0;
return degree >= 271 && degree <= 300; // Forbidden range: 271° to 300°
});
};