kindred/create
1
1
Fork 0
Code Issues 53 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity
Files
650ebf693ebb5eafcab5931e01f1388719d17b39
create/src/Mod/CAM/PathSimulator/AppGL/MillPathSegment.cpp
Billy Huddleston 650ebf693e CAM: Add threshold for treating large-radius arcs as linear in simulator 
Arc segments with extremely large radius compared to chord length can
cause floating-point precision issues in simulation. This change
introduces ARC_LINEARIZATION_THRESHOLD to treat such arcs as linear,
improving numerical stability in the simulator.

The following g-code was causing erractic behavior in the simulator due
to large-radius arcs:
```
G2 I88775.835760 J-1936991.545892 K0.000000 X102.063107 Y100.102815 Z12.700001
```

src/Mod/CAM/PathSimulator/AppGL/MillPathSegment.cpp:
- Added ARC_LINEARIZATION_THRESHOLD constant
- Updated arc motion detection to treat large-radius arcs as linear
- Improved handling of arc vs. linear segment classification
2026-01-12 10:52:06 -06:00

280 lines
9.3 KiB
C++
Raw Blame History

// SPDX-License-Identifier: LGPL-2.1-or-later
/***************************************************************************
* Copyright (c) 2024 Shai Seger <shaise at gmail> *
* *
* This file is part of the FreeCAD CAx development system. *
* *
* This library is free software; you can redistribute it and/or *
* modify it under the terms of the GNU Library General Public *
* License as published by the Free Software Foundation; either *
* version 2 of the License, or (at your option) any later version. *
* *
* This library is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Library General Public License for more details. *
* *
* You should have received a copy of the GNU Library General Public *
* License along with this library; see the file COPYING.LIB. If not, *
* write to the Free Software Foundation, Inc., 59 Temple Place, *
* Suite 330, Boston, MA 02111-1307, USA *
* *
***************************************************************************/
#include "OpenGlWrapper.h"
#include "MillPathSegment.h"
#include "SimShapes.h"
#include "linmath.h"
#include "GlUtils.h"
#include <iostream>
constexpr auto pi = std::numbers::pi_v<float>;
#define N_MILL_SLICES 8
#define MAX_SEG_DEG (pi / 2.0f) // 90 deg
#define NIN_SEG_DEG (pi / 90.0f) // 2 deg
#define SWEEP_ARC_PAD 1.05f
#define PX 0
#define PY 1
#define PZ 2
// Maximum ratio of radius to chord length for treating arc as curved
// Ratios above this indicate the arc is essentially a straight line
// and should be treated as linear to avoid numerical precision issues
constexpr float ARC_LINEARIZATION_THRESHOLD = 100000.0f;
namespace MillSim
{
bool IsVerticalMotion(MillMotion* m1, MillMotion* m2)
{
return (m1->z != m2->z && EQ_FLOAT(m1->x, m2->x) && EQ_FLOAT(m1->y, m2->y));
}
bool IsArcMotion(MillMotion* m)
{
if (m->cmd != eRotateCCW && m->cmd != eRotateCW) {
return false;
}
return fabs(m->i) > EPSILON || fabs(m->j) > EPSILON;
}
float MillPathSegment::mResolution = 1;
float MillPathSegment::mSmallRadStep = (pi / 8);
MillPathSegment::MillPathSegment(EndMill* _endmill, MillMotion* from, MillMotion* to)
{
mat4x4_identity(mShearMat);
MotionPosToVec(mStartPos, from);
MotionPosToVec(mDiff, to);
vec3_sub(mDiff, mDiff, mStartPos);
mXYDistance = sqrtf(mDiff[PX] * mDiff[PX] + mDiff[PY] * mDiff[PY]);
mZDistance = fabsf(mDiff[PY]);
mXYZDistance = sqrtf(mXYDistance * mXYDistance + mDiff[PZ] * mDiff[PZ]);
mXYAngle = atan2f(mDiff[PY], mDiff[PX]);
endmill = _endmill;
mStartAngRad = mStepAngRad = 0;
// Check if this is an arc motion and whether it should be treated as curved
bool isArc = IsArcMotion(to);
bool treatAsCurved = false;
if (isArc) {
mRadius = sqrtf(to->j * to->j + to->i * to->i);
// Check if arc is essentially a straight line by comparing radius to chord length
// When radius >> chord length, floating-point precision issues occur in angle calculations
treatAsCurved = (mRadius <= mXYDistance * ARC_LINEARIZATION_THRESHOLD);
}
if (treatAsCurved) {
mMotionType = MTCurved;
mSmallRad = mRadius <= endmill->radius;
if (mSmallRad) {
mStepAngRad = mSmallRadStep;
}
else {
mStepAngRad = asinf(mResolution / mRadius);
if (mStepAngRad > MAX_SEG_DEG) {
mStepAngRad = MAX_SEG_DEG;
}
else if (mStepAngRad < NIN_SEG_DEG) {
mStepAngRad = NIN_SEG_DEG;
}
}
MotionPosToVec(mCenter, from);
mCenter[PX] += to->i;
mCenter[PY] += to->j;
mArcDir = to->cmd == eRotateCCW ? -1.f : 1.f;
mStartAngRad = atan2f(mCenter[PX] - from->x, from->y - mCenter[PY]);
float endAng = atan2f(mCenter[PX] - to->x, to->y - mCenter[PY]);
mSweepAng = (mStartAngRad - endAng) * mArcDir;
if (mSweepAng < EPSILON) {
mSweepAng += pi * 2;
}
numSimSteps = (int)(mSweepAng / mStepAngRad) + 1;
mStepAngRad = mArcDir * mSweepAng / numSimSteps;
if (mSmallRad) {
// when the radius is too small, we just use the tool itself to carve the stock
mShape = endmill->toolShape;
}
else {
endmill->GenerateArcSegmentDL(
mRadius,
mStepAngRad * SWEEP_ARC_PAD,
mDiff[PZ] / numSimSteps,
&mShape
);
numSimSteps++;
}
isMultyPart = true;
}
else {
numSimSteps = (int)(mXYZDistance / mResolution);
if (numSimSteps == 0) {
numSimSteps = 1;
}
isMultyPart = false;
mStepDistance = mXYDistance / numSimSteps;
mStepLength[PX] = mDiff[PX];
mStepLength[PY] = mDiff[PY];
mStepLength[PZ] = mDiff[PZ];
vec3_scale(mStepLength, mStepLength, 1.f / (float)numSimSteps);
if (IsVerticalMotion(from, to)) {
mMotionType = MTVertical;
}
else {
mMotionType = MTHorizontal;
mShearMat[0][2] = mDiff[PZ] / mXYDistance;
}
}
}
MillPathSegment::~MillPathSegment()
{
mShape.FreeResources();
}
void MillPathSegment::AppendPathPoints(std::vector<MillPathPosition>& pointsBuffer)
{
MillPathPosition mpPos;
if (mMotionType == MTCurved) {
float ang = mStartAngRad;
float z = mStartPos[PZ];
float zStep = mDiff[PZ] / numSimSteps;
for (int i = 1; i < numSimSteps; i++) {
ang -= mStepAngRad;
z += zStep;
mpPos.X = mCenter[PX] - sinf(ang) * mRadius;
mpPos.Y = mCenter[PY] + cosf(ang) * mRadius;
mpPos.Z = z;
mpPos.SegmentId = segmentIndex;
pointsBuffer.push_back(mpPos);
}
}
else {
mpPos.X = mStartPos[PX] + mDiff[PX];
mpPos.Y = mStartPos[PY] + mDiff[PY];
mpPos.Z = mStartPos[PZ] + mDiff[PZ];
mpPos.SegmentId = segmentIndex;
pointsBuffer.push_back(mpPos);
}
}
void MillPathSegment::render(int step)
{
mStepNumber = step;
mat4x4 mat, mat2, rmat;
mat4x4_identity(mat);
mat4x4_identity(rmat);
if (mMotionType == MTCurved) {
mat4x4_translate_in_place(
mat,
mCenter[PX],
mCenter[PY],
mCenter[PZ] + mDiff[PZ] * (step - 1) / numSimSteps
);
mat4x4_rotate_Z(mat, mat, mStartAngRad - (step - 1) * mStepAngRad);
mat4x4_rotate_Z(rmat, rmat, mStartAngRad - (step - 1) * mStepAngRad);
if (mSmallRad || step == numSimSteps) {
mat4x4_translate_in_place(mat, 0, mRadius, 0);
endmill->toolShape.Render(mat, rmat);
}
else {
mShape.Render(mat, rmat);
}
}
else {
if (mMotionType == MTVertical) {
if (mStepLength[PZ] > 0) {
mat4x4_translate_in_place_v(mat, mStartPos);
}
else {
mat4x4_translate_in_place(
mat,
mStartPos[PX],
mStartPos[PY],
mStartPos[PZ] + mStepNumber * mStepLength[PZ]
);
}
endmill->toolShape.Render(mat, rmat);
}
else {
float renderDist = step * mStepDistance;
mat4x4_translate_in_place_v(mat, mStartPos);
mat4x4_rotate_Z(mat, mat, mXYAngle);
mat4x4_rotate_Z(rmat, rmat, mXYAngle);
mat4x4_dup(mat2, mat);
if (mDiff[PZ] != 0.0) {
mat4x4_mul(mat2, mat2, mShearMat);
}
mat4x4_scale_aniso(mat2, mat2, renderDist, 1, 1);
endmill->pathShape.Render(mat2, rmat);
mat4x4_translate_in_place(mat, renderDist, 0, mDiff[PZ]);
endmill->halfToolShape.Render(mat, rmat);
}
}
}
void MillPathSegment::GetHeadPosition(vec3 headPos)
{
if (mMotionType == MTCurved) {
float angRad = mStartAngRad - mStepNumber * mStepAngRad;
vec3_set(mHeadPos, -mRadius * sinf(angRad), mRadius * cosf(angRad), 0);
vec3_add(mHeadPos, mHeadPos, mCenter);
}
else {
vec3_dup(mHeadPos, mStepLength);
vec3_scale(mHeadPos, mHeadPos, (float)mStepNumber);
vec3_add(mHeadPos, mHeadPos, mStartPos);
}
vec3_dup(headPos, mHeadPos);
}
float MillPathSegment::SetQuality(float quality, float maxStockDimension)
{
mResolution = maxStockDimension * 0.05 / quality;
if (mResolution > 4) {
mResolution = 4;
}
if (mResolution < 0.5) {
mResolution = 0.5;
}
mSmallRadStep = pi / 8;
if (quality < 4) {
mSmallRadStep = pi / 2;
}
else if (quality < 8) {
mSmallRadStep = pi / 4;
}
return mResolution;
}
} // namespace MillSim
Reference in New Issue View Git Blame Copy Permalink