Upload folder using huggingface_hub

985c397 verified 9 months ago

24.4 kB

// SPDX-License-Identifier: LGPL-2.1-or-later

/**************************************************************************
 *   Copyright (c) 2017 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 <algorithm>


#include <BRepBndLib.hxx>
#include <BRepCheck_Analyzer.hxx>
#include <BRepClass3d_SolidClassifier.hxx>
#include <gp_Pnt.hxx>

#include "VolSim.h"

using std::numbers::pi;

//************************************************************************************************************
// stock
//************************************************************************************************************
cStock::cStock(float px, float py, float pz, float lx, float ly, float lz, float res)
    : m_px(px)
    , m_py(py)
    , m_pz(pz)
    , m_lx(lx)
    , m_ly(ly)
    , m_lz(lz)
    , m_res(res)
{
    m_x = (int)(m_lx / res) + 1;
    m_y = (int)(m_ly / res) + 1;
    m_stock.Init(m_x, m_y);
    m_attr.Init(m_x, m_y);
    m_plane = pz + lz;
    for (int y = 0; y < m_y; y++) {
        for (int x = 0; x < m_x; x++) {
            m_stock[x][y] = m_plane;
            m_attr[x][y] = 0;
        }
    }
}

cStock::~cStock()
{}


float cStock::FindRectTop(int& xp, int& yp, int& x_size, int& y_size, bool scanHoriz)
{
    float z = m_stock[xp][yp];
    bool xr_ok = true;
    bool xl_ok = scanHoriz;
    bool yu_ok = true;
    bool yd_ok = !scanHoriz;
    x_size = 1;
    y_size = 1;
    while (xr_ok || xl_ok || yu_ok || yd_ok) {
        // sweep right x direction
        if (xr_ok) {
            int tx = xp + x_size;
            if (tx >= m_x) {
                xr_ok = false;
            }
            else {
                for (int y = yp; y < yp + y_size; y++) {
                    if ((m_attr[tx][y] & SIM_TESSEL_TOP) != 0 || fabs(z - m_stock[tx][y]) > m_res) {
                        xr_ok = false;
                        break;
                    }
                }
                if (xr_ok) {
                    x_size++;
                }
            }
        }

        // sweep left x direction
        if (xl_ok) {
            int tx = xp - 1;
            if (tx < 0) {
                xl_ok = false;
            }
            else {
                for (int y = yp; y < yp + y_size; y++) {
                    if ((m_attr[tx][y] & SIM_TESSEL_TOP) != 0 || fabs(z - m_stock[tx][y]) > m_res) {
                        xl_ok = false;
                        break;
                    }
                }
                if (xl_ok) {
                    x_size++;
                    xp--;
                }
            }
        }

        // sweep up y direction
        if (yu_ok) {
            int ty = yp + y_size;
            if (ty >= m_y) {
                yu_ok = false;
            }
            else {
                for (int x = xp; x < xp + x_size; x++) {
                    if ((m_attr[x][ty] & SIM_TESSEL_TOP) != 0 || fabs(z - m_stock[x][ty]) > m_res) {
                        yu_ok = false;
                        break;
                    }
                }
                if (yu_ok) {
                    y_size++;
                }
            }
        }

        // sweep down y direction
        if (yd_ok) {
            int ty = yp - 1;
            if (ty < 0) {
                yd_ok = false;
            }
            else {
                for (int x = xp; x < xp + x_size; x++) {
                    if ((m_attr[x][ty] & SIM_TESSEL_TOP) != 0 || fabs(z - m_stock[x][ty]) > m_res) {
                        yd_ok = false;
                        break;
                    }
                }
                if (yd_ok) {
                    y_size++;
                    yp--;
                }
            }
        }
    }
    return z;
}

int cStock::TesselTop(int xp, int yp)
{
    int x_size, y_size;
    float z = FindRectTop(xp, yp, x_size, y_size, true);
    bool farRect = false;
    while (y_size / x_size > 5) {
        farRect = true;
        yp += x_size * 5;
        z = FindRectTop(xp, yp, x_size, y_size, true);
    }

    while (x_size / y_size > 5) {
        farRect = true;
        xp += y_size * 5;
        z = FindRectTop(xp, yp, x_size, y_size, false);
    }

    // mark all points inside
    for (int y = yp; y < yp + y_size; y++) {
        for (int x = xp; x < xp + x_size; x++) {
            m_attr[x][y] |= SIM_TESSEL_TOP;
        }
    }

    if (z > m_pz + m_res) {
        // generate 4 3d points
        Point3D pbl(xp, yp, z);
        Point3D pbr(xp + x_size, yp, z);
        Point3D ptl(xp, yp + y_size, z);
        Point3D ptr(xp + x_size, yp + y_size, z);
        if (fabs(m_pz + m_lz - z) < SIM_EPSILON) {
            AddQuad(pbl, pbr, ptr, ptl, facetsOuter);
        }
        else {
            AddQuad(pbl, pbr, ptr, ptl, facetsInner);
        }
    }

    if (farRect) {
        return -1;
    }
    return std::max(0, x_size - 1);
    // return 0;
}


void cStock::FindRectBot(int& xp, int& yp, int& x_size, int& y_size, bool scanHoriz)
{
    bool xr_ok = true;
    bool xl_ok = scanHoriz;
    bool yu_ok = true;
    bool yd_ok = !scanHoriz;
    x_size = 1;
    y_size = 1;
    while (xr_ok || xl_ok || yu_ok || yd_ok) {
        // sweep right x direction
        if (xr_ok) {
            int tx = xp + x_size;
            if (tx >= m_x) {
                xr_ok = false;
            }
            else {
                for (int y = yp; y < yp + y_size; y++) {
                    if ((m_attr[tx][y] & SIM_TESSEL_BOT) != 0 || (m_stock[tx][y] - m_pz) < m_res) {
                        xr_ok = false;
                        break;
                    }
                }
                if (xr_ok) {
                    x_size++;
                }
            }
        }

        // sweep left x direction
        if (xl_ok) {
            int tx = xp - 1;
            if (tx < 0) {
                xl_ok = false;
            }
            else {
                for (int y = yp; y < yp + y_size; y++) {
                    if ((m_attr[tx][y] & SIM_TESSEL_BOT) != 0 || (m_stock[tx][y] - m_pz) < m_res) {
                        xl_ok = false;
                        break;
                    }
                }
                if (xl_ok) {
                    x_size++;
                    xp--;
                }
            }
        }

        // sweep up y direction
        if (yu_ok) {
            int ty = yp + y_size;
            if (ty >= m_y) {
                yu_ok = false;
            }
            else {
                for (int x = xp; x < xp + x_size; x++) {
                    if ((m_attr[x][ty] & SIM_TESSEL_BOT) != 0 || (m_stock[x][ty] - m_pz) < m_res) {
                        yu_ok = false;
                        break;
                    }
                }
                if (yu_ok) {
                    y_size++;
                }
            }
        }

        // sweep down y direction
        if (yd_ok) {
            int ty = yp - 1;
            if (ty < 0) {
                yd_ok = false;
            }
            else {
                for (int x = xp; x < xp + x_size; x++) {
                    if ((m_attr[x][ty] & SIM_TESSEL_BOT) != 0 || (m_stock[x][ty] - m_pz) < m_res) {
                        yd_ok = false;
                        break;
                    }
                }
                if (yd_ok) {
                    y_size++;
                    yp--;
                }
            }
        }
    }
}


int cStock::TesselBot(int xp, int yp)
{
    int x_size, y_size;
    FindRectBot(xp, yp, x_size, y_size, true);
    bool farRect = false;
    while (y_size / x_size > 5) {
        farRect = true;
        yp += x_size * 5;
        FindRectTop(xp, yp, x_size, y_size, true);
    }

    while (x_size / y_size > 5) {
        farRect = true;
        xp += y_size * 5;
        FindRectTop(xp, yp, x_size, y_size, false);
    }

    // mark all points inside
    for (int y = yp; y < yp + y_size; y++) {
        for (int x = xp; x < xp + x_size; x++) {
            m_attr[x][y] |= SIM_TESSEL_BOT;
        }
    }

    // generate 4 3d points
    Point3D pbl(xp, yp, m_pz);
    Point3D pbr(xp + x_size, yp, m_pz);
    Point3D ptl(xp, yp + y_size, m_pz);
    Point3D ptr(xp + x_size, yp + y_size, m_pz);
    AddQuad(pbl, ptl, ptr, pbr, facetsOuter);

    if (farRect) {
        return -1;
    }
    return std::max(0, x_size - 1);
    // return 0;
}


int cStock::TesselSidesX(int yp)
{
    float lastz1 = m_pz;
    if (yp < m_y) {
        lastz1 = std::max(m_stock[0][yp], m_pz);
    }
    float lastz2 = m_pz;
    if (yp > 0) {
        lastz2 = std::max(m_stock[0][yp - 1], m_pz);
    }

    std::vector<MeshCore::MeshGeomFacet>* facets = &facetsInner;
    if (yp == 0 || yp == m_y) {
        facets = &facetsOuter;
    }

    // bool lastzclip = (lastz - m_pz) < m_res;
    int lastpoint = 0;
    for (int x = 1; x <= m_x; x++) {
        float newz1 = m_pz;
        if (yp < m_y && x < m_x) {
            newz1 = std::max(m_stock[x][yp], m_pz);
        }
        float newz2 = m_pz;
        if (yp > 0 && x < m_x) {
            newz2 = std::max(m_stock[x][yp - 1], m_pz);
        }

        if (fabs(lastz1 - lastz2) > m_res) {
            if (fabs(newz1 - lastz1) < m_res && fabs(newz2 - lastz2) < m_res) {
                continue;
            }
            Point3D pbl(lastpoint, yp, lastz1);
            Point3D pbr(x, yp, lastz1);
            Point3D ptl(lastpoint, yp, lastz2);
            Point3D ptr(x, yp, lastz2);
            AddQuad(pbl, ptl, ptr, pbr, *facets);
        }
        lastz1 = newz1;
        lastz2 = newz2;
        lastpoint = x;
    }
    return 0;
}

int cStock::TesselSidesY(int xp)
{
    float lastz1 = m_pz;
    if (xp < m_x) {
        lastz1 = std::max(m_stock[xp][0], m_pz);
    }
    float lastz2 = m_pz;
    if (xp > 0) {
        lastz2 = std::max(m_stock[xp - 1][0], m_pz);
    }

    std::vector<MeshCore::MeshGeomFacet>* facets = &facetsInner;
    if (xp == 0 || xp == m_x) {
        facets = &facetsOuter;
    }

    // bool lastzclip = (lastz - m_pz) < m_res;
    int lastpoint = 0;
    for (int y = 1; y <= m_y; y++) {
        float newz1 = m_pz;
        if (xp < m_x && y < m_y) {
            newz1 = std::max(m_stock[xp][y], m_pz);
        }
        float newz2 = m_pz;
        if (xp > 0 && y < m_y) {
            newz2 = std::max(m_stock[xp - 1][y], m_pz);
        }

        if (fabs(lastz1 - lastz2) > m_res) {
            if (fabs(newz1 - lastz1) < m_res && fabs(newz2 - lastz2) < m_res) {
                continue;
            }
            Point3D pbr(xp, lastpoint, lastz1);
            Point3D pbl(xp, y, lastz1);
            Point3D ptr(xp, lastpoint, lastz2);
            Point3D ptl(xp, y, lastz2);
            AddQuad(pbl, ptl, ptr, pbr, *facets);
        }
        lastz1 = newz1;
        lastz2 = newz2;
        lastpoint = y;
    }
    return 0;
}

void cStock::SetFacetPoints(MeshCore::MeshGeomFacet& facet, Point3D& p1, Point3D& p2, Point3D& p3)
{
    facet._aclPoints[0][0] = p1.x * m_res + m_px;
    facet._aclPoints[0][1] = p1.y * m_res + m_py;
    facet._aclPoints[0][2] = p1.z;
    facet._aclPoints[1][0] = p2.x * m_res + m_px;
    facet._aclPoints[1][1] = p2.y * m_res + m_py;
    facet._aclPoints[1][2] = p2.z;
    facet._aclPoints[2][0] = p3.x * m_res + m_px;
    facet._aclPoints[2][1] = p3.y * m_res + m_py;
    facet._aclPoints[2][2] = p3.z;
    facet.CalcNormal();
}

void cStock::AddQuad(
    Point3D& p1,
    Point3D& p2,
    Point3D& p3,
    Point3D& p4,
    std::vector<MeshCore::MeshGeomFacet>& facets
)
{
    MeshCore::MeshGeomFacet facet;
    SetFacetPoints(facet, p1, p2, p3);
    facets.push_back(facet);
    SetFacetPoints(facet, p1, p3, p4);
    facets.push_back(facet);
}

void cStock::Tessellate(Mesh::MeshObject& meshOuter, Mesh::MeshObject& meshInner)
{
    // reset attribs
    for (int y = 0; y < m_y; y++) {
        for (int x = 0; x < m_x; x++) {
            m_attr[x][y] = 0;
        }
    }

    facetsOuter.clear();
    facetsInner.clear();

    for (int y = 0; y < m_y; y++) {
        for (int x = 0; x < m_x; x++) {
            int attr = m_attr[x][y];
            if ((attr & SIM_TESSEL_TOP) == 0) {
                x += TesselTop(x, y);
            }
        }
    }
    for (int y = 0; y < m_y; y++) {
        for (int x = 0; x < m_x; x++) {
            if ((m_stock[x][y] - m_pz) < m_res) {
                m_attr[x][y] |= SIM_TESSEL_BOT;
            }
            if ((m_attr[x][y] & SIM_TESSEL_BOT) == 0) {
                x += TesselBot(x, y);
            }
        }
    }
    for (int y = 0; y <= m_y; y++) {
        TesselSidesX(y);
    }
    for (int x = 0; x <= m_x; x++) {
        TesselSidesY(x);
    }
    meshOuter.addFacets(facetsOuter);
    meshInner.addFacets(facetsInner);
    facetsOuter.clear();
    facetsInner.clear();
}


void cStock::CreatePocket(float cxf, float cyf, float radf, float height)
{
    int cx = (int)((cxf - m_px) / m_res);
    int cy = (int)((cyf - m_py) / m_res);
    int rad = (int)(radf / m_res);
    int drad = rad * rad;
    int ys = std::max(0, cy - rad);
    int ye = std::min(m_x, cy + rad);
    int xs = std::max(0, cx - rad);
    int xe = std::min(m_x, cx + rad);
    for (int y = ys; y < ye; y++) {
        for (int x = xs; x < xe; x++) {
            if (((x - cx) * (x - cx) + (y - cy) * (y - cy)) < drad) {
                if (m_stock[x][y] > height) {
                    m_stock[x][y] = height;
                }
            }
        }
    }
}

void cStock::ApplyLinearTool(Point3D& p1, Point3D& p2, cSimTool& tool)
{
    // translate coordinates
    Point3D pi1 = ToInner(p1);
    Point3D pi2 = ToInner(p2);
    float rad = tool.radius;
    rad /= m_res;
    float cupAngle = 180;

    // strait motion
    float perpDirX = 1;
    float perpDirY = 0;
    cLineSegment path(pi1, pi2);
    if (path.lenXY > SIM_EPSILON)  // only if moving along xy
    {
        perpDirX = -path.pDirXY.y;
        perpDirY = path.pDirXY.x;
        Point3D start(perpDirX * rad + pi1.x, perpDirY * rad + pi1.y, pi1.z);
        Point3D mainWay = path.pDir * SIM_WALK_RES;
        Point3D sideWay(-perpDirX * SIM_WALK_RES, -perpDirY * SIM_WALK_RES, 0);
        int lenSteps = (int)(path.len / SIM_WALK_RES) + 1;
        int radSteps = (int)(rad * 2 / SIM_WALK_RES) + 1;
        float zstep = (pi2.z - pi1.z) / radSteps;
        float tstep = 2.0 / radSteps;
        float t = -1;
        for (int j = 0; j < radSteps; j++) {
            float z = pi1.z + tool.GetToolProfileAt(t);
            Point3D p = start;
            for (int i = 0; i < lenSteps; i++) {
                int x = (int)p.x;
                int y = (int)p.y;
                if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
                    if (m_stock[x][y] > z) {
                        m_stock[x][y] = z;
                    }
                }
                p.Add(mainWay);
                z += zstep;
            }
            t += tstep;
            start.Add(sideWay);
        }
    }
    else {
        cupAngle = 360;
    }

    // end cup
    for (float r = 0.5f; r <= rad; r += (float)SIM_WALK_RES) {
        Point3D cupCirc(perpDirX * r, perpDirY * r, pi2.z);
        float rotang = 180 * SIM_WALK_RES / (pi * r);
        cupCirc.SetRotationAngle(-rotang);
        float z = pi2.z + tool.GetToolProfileAt(r / rad);
        for (float a = 0; a < cupAngle; a += rotang) {
            int x = (int)(pi2.x + cupCirc.x);
            int y = (int)(pi2.y + cupCirc.y);
            if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
                if (m_stock[x][y] > z) {
                    m_stock[x][y] = z;
                }
            }
            cupCirc.Rotate();
        }
    }
}

void cStock::ApplyCircularTool(Point3D& p1, Point3D& p2, Point3D& cent, cSimTool& tool, bool isCCW)
{
    // translate coordinates
    Point3D pi1 = ToInner(p1);
    Point3D pi2 = ToInner(p2);
    Point3D centi(cent.x / m_res, cent.y / m_res, cent.z);
    float rad = tool.radius;
    rad /= m_res;
    float cpx = centi.x;
    float cpy = centi.y;

    Point3D xynorm = unit(Point3D(-cpx, -cpy, 0));
    float crad = sqrt(cpx * cpx + cpy * cpy);
    float crad1 = std::max((float)0.5, crad - rad);
    float crad2 = crad + rad;

    float sang = atan2(-cpy, -cpx);  // start angle

    cpx += pi1.x;
    cpy += pi1.y;
    double eang = atan2(pi2.y - cpy, pi2.x - cpx);  // end angle

    double ang = eang - sang;
    if (!isCCW && ang > 0) {
        ang -= 2 * pi;
    }
    if (isCCW && ang < 0) {
        ang += 2 * pi;
    }
    ang = fabs(ang);

    // apply path
    Point3D cupCirc;
    float tstep = (float)SIM_WALK_RES / rad;
    float t = -1;
    for (float r = crad1; r <= crad2; r += (float)SIM_WALK_RES) {
        cupCirc.x = xynorm.x * r;
        cupCirc.y = xynorm.y * r;
        float rotang = (float)SIM_WALK_RES / r;
        int ndivs = (int)(ang / rotang) + 1;
        if (!isCCW) {
            rotang = -rotang;
        }
        cupCirc.SetRotationAngleRad(rotang);
        float z = pi1.z + tool.GetToolProfileAt(t);
        float zstep = (pi2.z - pi1.z) / ndivs;
        for (int i = 0; i < ndivs; i++) {
            int x = (int)(cpx + cupCirc.x);
            int y = (int)(cpy + cupCirc.y);
            if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
                if (m_stock[x][y] > z) {
                    m_stock[x][y] = z;
                }
            }
            z += zstep;
            cupCirc.Rotate();
        }
        t += tstep;
    }

    // apply end cup
    xynorm.SetRotationAngleRad(ang);
    xynorm.Rotate();
    for (float r = 0.5f; r <= rad; r += (float)SIM_WALK_RES) {
        Point3D cupCirc(xynorm.x * r, xynorm.y * r, 0);
        float rotang = (float)SIM_WALK_RES / r;
        int ndivs = (int)(pi / rotang) + 1;
        if (!isCCW) {
            rotang = -rotang;
        }
        cupCirc.SetRotationAngleRad(rotang);
        float z = pi2.z + tool.GetToolProfileAt(r / rad);
        for (int i = 0; i < ndivs; i++) {
            int x = (int)(pi2.x + cupCirc.x);
            int y = (int)(pi2.y + cupCirc.y);
            if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
                if (m_stock[x][y] > z) {
                    m_stock[x][y] = z;
                }
            }
            cupCirc.Rotate();
        }
    }
}


//************************************************************************************************************
// Line Segment
//************************************************************************************************************

void cLineSegment::SetPoints(Point3D& p1, Point3D& p2)
{
    pStart = p1;
    pDir = unit(p2 - p1);
    Point3D dirXY(pDir.x, pDir.y, 0);
    lenXY = length(dirXY);
    len = length(p2 - p1);
    if (len > SIM_EPSILON) {
        pDirXY = unit(dirXY);
    }
}

void cLineSegment::PointAt(float dist, Point3D& retp)
{
    retp.x = pStart.x + pDir.x * dist;
    retp.y = pStart.y + pDir.y * dist;
    retp.z = pStart.z + pDir.z * dist;
}

//************************************************************************************************************
// Point (or vector)
//************************************************************************************************************

void Point3D::SetRotationAngleRad(float angle)
{
    sina = sin(angle);
    cosa = cos(angle);
}

void Point3D::SetRotationAngle(float angle)
{
    SetRotationAngleRad(angle * 2 * pi / 360);
}

void Point3D::UpdateCmd(Path::Command& cmd)
{
    if (cmd.has("X")) {
        x = cmd.getPlacement().getPosition()[0];
    }
    if (cmd.has("Y")) {
        y = cmd.getPlacement().getPosition()[1];
    }
    if (cmd.has("Z")) {
        z = cmd.getPlacement().getPosition()[2];
    }
}

//************************************************************************************************************
// Simulation tool
//************************************************************************************************************
cSimTool::cSimTool(const TopoDS_Shape& toolShape, float res)
{

    BRepCheck_Analyzer aChecker(toolShape);
    bool shapeIsValid = aChecker.IsValid() ? true : false;

    if (!shapeIsValid) {
        throw Base::RuntimeError("Path Simulation: Error in tool geometry");
    }

    Bnd_Box boundBox;
    BRepBndLib::Add(toolShape, boundBox);

    boundBox.SetGap(0.0);
    Standard_Real xMin, yMin, zMin, xMax, yMax, zMax;
    boundBox.Get(xMin, yMin, zMin, xMax, yMax, zMax);
    radius = (xMax - xMin) / 2;
    length = zMax - zMin;

    Base::Vector3d pnt;
    pnt.x = 0;
    pnt.y = 0;
    pnt.z = 0;

    int radValue = (int)(radius / res) + 1;

    // Measure the performance of the profile extraction
    // auto start = std::chrono::high_resolution_clock::now();

    for (int x = 0; x < radValue; x++) {
        // find the face of the tool by checking z points across the
        // radius to see if the point is inside the shape
        pnt.x = static_cast<double>(x) * res;
        bool inside = isInside(toolShape, pnt, res);

        // move down until the point is outside the shape
        while (inside && std::abs(pnt.z) < length) {
            pnt.z -= res;
            inside = isInside(toolShape, pnt, res);
        }

        // move up until the point is first inside the shape and record the position
        while (!inside && pnt.z < length) {
            pnt.z += res;
            inside = isInside(toolShape, pnt, res);

            if (inside) {
                toolShapePoint shapePoint;
                shapePoint.radiusPos = pnt.x;
                shapePoint.heightPos = pnt.z;
                m_toolShape.push_back(shapePoint);
                break;
            }
        }
    }

    // Report the performance of the profile extraction
    // auto stop = std::chrono::high_resolution_clock::now();
    // auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
    // Base::Console().log("cSimTool::cSimTool - Tool Profile Extraction Took: %i ms\n",
    // duration.count() / 1000);
}

float cSimTool::GetToolProfileAt(float pos)  // pos is -1..1 location along the radius of the tool
                                             // (0 is center)
{
    toolShapePoint test;
    test.radiusPos = std::abs(pos) * radius;

    auto it
        = std::lower_bound(m_toolShape.begin(), m_toolShape.end(), test, toolShapePoint::less_than());
    return it != m_toolShape.end() ? it->heightPos : 0.0f;
}

bool cSimTool::isInside(const TopoDS_Shape& toolShape, Base::Vector3d pnt, float res)
{
    bool checkFace = true;
    TopAbs_State stateIn = TopAbs_IN;

    try {
        BRepClass3d_SolidClassifier solidClassifier(toolShape);
        gp_Pnt vertex = gp_Pnt(pnt.x, pnt.y, pnt.z);
        solidClassifier.Perform(vertex, res);
        bool inside = (solidClassifier.State() == stateIn);
        if (checkFace && solidClassifier.IsOnAFace()) {
            inside = true;
        }
        return inside;
    }
    catch (...) {
        return false;
    }
}

cVolSim::cVolSim()
    : stock(nullptr)
{}


cVolSim::~cVolSim()
{}

	// SPDX-License-Identifier: LGPL-2.1-or-later

	/**************************************************************************
	* Copyright (c) 2017 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 <algorithm>


	#include <BRepBndLib.hxx>
	#include <BRepCheck_Analyzer.hxx>
	#include <BRepClass3d_SolidClassifier.hxx>
	#include <gp_Pnt.hxx>

	#include "VolSim.h"

	using std::numbers::pi;

	//************************************************************************************************************
	// stock
	//************************************************************************************************************
	cStock::cStock(float px, float py, float pz, float lx, float ly, float lz, float res)
	: m_px(px)
	, m_py(py)
	, m_pz(pz)
	, m_lx(lx)
	, m_ly(ly)
	, m_lz(lz)
	, m_res(res)
	{
	m_x = (int)(m_lx / res) + 1;
	m_y = (int)(m_ly / res) + 1;
	m_stock.Init(m_x, m_y);
	m_attr.Init(m_x, m_y);
	m_plane = pz + lz;
	for (int y = 0; y < m_y; y++) {
	for (int x = 0; x < m_x; x++) {
	m_stock[x][y] = m_plane;
	m_attr[x][y] = 0;
	}
	}
	}

	cStock::~cStock()
	{}


	float cStock::FindRectTop(int& xp, int& yp, int& x_size, int& y_size, bool scanHoriz)
	{
	float z = m_stock[xp][yp];
	bool xr_ok = true;
	bool xl_ok = scanHoriz;
	bool yu_ok = true;
	bool yd_ok = !scanHoriz;
	x_size = 1;
	y_size = 1;
	while (xr_ok \|\| xl_ok \|\| yu_ok \|\| yd_ok) {
	// sweep right x direction
	if (xr_ok) {
	int tx = xp + x_size;
	if (tx >= m_x) {
	xr_ok = false;
	}
	else {
	for (int y = yp; y < yp + y_size; y++) {
	if ((m_attr[tx][y] & SIM_TESSEL_TOP) != 0 \|\| fabs(z - m_stock[tx][y]) > m_res) {
	xr_ok = false;
	break;
	}
	}
	if (xr_ok) {
	x_size++;
	}
	}
	}

	// sweep left x direction
	if (xl_ok) {
	int tx = xp - 1;
	if (tx < 0) {
	xl_ok = false;
	}
	else {
	for (int y = yp; y < yp + y_size; y++) {
	if ((m_attr[tx][y] & SIM_TESSEL_TOP) != 0 \|\| fabs(z - m_stock[tx][y]) > m_res) {
	xl_ok = false;
	break;
	}
	}
	if (xl_ok) {
	x_size++;
	xp--;
	}
	}
	}

	// sweep up y direction
	if (yu_ok) {
	int ty = yp + y_size;
	if (ty >= m_y) {
	yu_ok = false;
	}
	else {
	for (int x = xp; x < xp + x_size; x++) {
	if ((m_attr[x][ty] & SIM_TESSEL_TOP) != 0 \|\| fabs(z - m_stock[x][ty]) > m_res) {
	yu_ok = false;
	break;
	}
	}
	if (yu_ok) {
	y_size++;
	}
	}
	}

	// sweep down y direction
	if (yd_ok) {
	int ty = yp - 1;
	if (ty < 0) {
	yd_ok = false;
	}
	else {
	for (int x = xp; x < xp + x_size; x++) {
	if ((m_attr[x][ty] & SIM_TESSEL_TOP) != 0 \|\| fabs(z - m_stock[x][ty]) > m_res) {
	yd_ok = false;
	break;
	}
	}
	if (yd_ok) {
	y_size++;
	yp--;
	}
	}
	}
	}
	return z;
	}

	int cStock::TesselTop(int xp, int yp)
	{
	int x_size, y_size;
	float z = FindRectTop(xp, yp, x_size, y_size, true);
	bool farRect = false;
	while (y_size / x_size > 5) {
	farRect = true;
	yp += x_size * 5;
	z = FindRectTop(xp, yp, x_size, y_size, true);
	}

	while (x_size / y_size > 5) {
	farRect = true;
	xp += y_size * 5;
	z = FindRectTop(xp, yp, x_size, y_size, false);
	}

	// mark all points inside
	for (int y = yp; y < yp + y_size; y++) {
	for (int x = xp; x < xp + x_size; x++) {
	m_attr[x][y] \|= SIM_TESSEL_TOP;
	}
	}

	if (z > m_pz + m_res) {
	// generate 4 3d points
	Point3D pbl(xp, yp, z);
	Point3D pbr(xp + x_size, yp, z);
	Point3D ptl(xp, yp + y_size, z);
	Point3D ptr(xp + x_size, yp + y_size, z);
	if (fabs(m_pz + m_lz - z) < SIM_EPSILON) {
	AddQuad(pbl, pbr, ptr, ptl, facetsOuter);
	}
	else {
	AddQuad(pbl, pbr, ptr, ptl, facetsInner);
	}
	}

	if (farRect) {
	return -1;
	}
	return std::max(0, x_size - 1);
	// return 0;
	}


	void cStock::FindRectBot(int& xp, int& yp, int& x_size, int& y_size, bool scanHoriz)
	{
	bool xr_ok = true;
	bool xl_ok = scanHoriz;
	bool yu_ok = true;
	bool yd_ok = !scanHoriz;
	x_size = 1;
	y_size = 1;
	while (xr_ok \|\| xl_ok \|\| yu_ok \|\| yd_ok) {
	// sweep right x direction
	if (xr_ok) {
	int tx = xp + x_size;
	if (tx >= m_x) {
	xr_ok = false;
	}
	else {
	for (int y = yp; y < yp + y_size; y++) {
	if ((m_attr[tx][y] & SIM_TESSEL_BOT) != 0 \|\| (m_stock[tx][y] - m_pz) < m_res) {
	xr_ok = false;
	break;
	}
	}
	if (xr_ok) {
	x_size++;
	}
	}
	}

	// sweep left x direction
	if (xl_ok) {
	int tx = xp - 1;
	if (tx < 0) {
	xl_ok = false;
	}
	else {
	for (int y = yp; y < yp + y_size; y++) {
	if ((m_attr[tx][y] & SIM_TESSEL_BOT) != 0 \|\| (m_stock[tx][y] - m_pz) < m_res) {
	xl_ok = false;
	break;
	}
	}
	if (xl_ok) {
	x_size++;
	xp--;
	}
	}
	}

	// sweep up y direction
	if (yu_ok) {
	int ty = yp + y_size;
	if (ty >= m_y) {
	yu_ok = false;
	}
	else {
	for (int x = xp; x < xp + x_size; x++) {
	if ((m_attr[x][ty] & SIM_TESSEL_BOT) != 0 \|\| (m_stock[x][ty] - m_pz) < m_res) {
	yu_ok = false;
	break;
	}
	}
	if (yu_ok) {
	y_size++;
	}
	}
	}

	// sweep down y direction
	if (yd_ok) {
	int ty = yp - 1;
	if (ty < 0) {
	yd_ok = false;
	}
	else {
	for (int x = xp; x < xp + x_size; x++) {
	if ((m_attr[x][ty] & SIM_TESSEL_BOT) != 0 \|\| (m_stock[x][ty] - m_pz) < m_res) {
	yd_ok = false;
	break;
	}
	}
	if (yd_ok) {
	y_size++;
	yp--;
	}
	}
	}
	}
	}


	int cStock::TesselBot(int xp, int yp)
	{
	int x_size, y_size;
	FindRectBot(xp, yp, x_size, y_size, true);
	bool farRect = false;
	while (y_size / x_size > 5) {
	farRect = true;
	yp += x_size * 5;
	FindRectTop(xp, yp, x_size, y_size, true);
	}

	while (x_size / y_size > 5) {
	farRect = true;
	xp += y_size * 5;
	FindRectTop(xp, yp, x_size, y_size, false);
	}

	// mark all points inside
	for (int y = yp; y < yp + y_size; y++) {
	for (int x = xp; x < xp + x_size; x++) {
	m_attr[x][y] \|= SIM_TESSEL_BOT;
	}
	}

	// generate 4 3d points
	Point3D pbl(xp, yp, m_pz);
	Point3D pbr(xp + x_size, yp, m_pz);
	Point3D ptl(xp, yp + y_size, m_pz);
	Point3D ptr(xp + x_size, yp + y_size, m_pz);
	AddQuad(pbl, ptl, ptr, pbr, facetsOuter);

	if (farRect) {
	return -1;
	}
	return std::max(0, x_size - 1);
	// return 0;
	}


	int cStock::TesselSidesX(int yp)
	{
	float lastz1 = m_pz;
	if (yp < m_y) {
	lastz1 = std::max(m_stock[0][yp], m_pz);
	}
	float lastz2 = m_pz;
	if (yp > 0) {
	lastz2 = std::max(m_stock[0][yp - 1], m_pz);
	}

	std::vector<MeshCore::MeshGeomFacet>* facets = &facetsInner;
	if (yp == 0 \|\| yp == m_y) {
	facets = &facetsOuter;
	}

	// bool lastzclip = (lastz - m_pz) < m_res;
	int lastpoint = 0;
	for (int x = 1; x <= m_x; x++) {
	float newz1 = m_pz;
	if (yp < m_y && x < m_x) {
	newz1 = std::max(m_stock[x][yp], m_pz);
	}
	float newz2 = m_pz;
	if (yp > 0 && x < m_x) {
	newz2 = std::max(m_stock[x][yp - 1], m_pz);
	}

	if (fabs(lastz1 - lastz2) > m_res) {
	if (fabs(newz1 - lastz1) < m_res && fabs(newz2 - lastz2) < m_res) {
	continue;
	}
	Point3D pbl(lastpoint, yp, lastz1);
	Point3D pbr(x, yp, lastz1);
	Point3D ptl(lastpoint, yp, lastz2);
	Point3D ptr(x, yp, lastz2);
	AddQuad(pbl, ptl, ptr, pbr, *facets);
	}
	lastz1 = newz1;
	lastz2 = newz2;
	lastpoint = x;
	}
	return 0;
	}

	int cStock::TesselSidesY(int xp)
	{
	float lastz1 = m_pz;
	if (xp < m_x) {
	lastz1 = std::max(m_stock[xp][0], m_pz);
	}
	float lastz2 = m_pz;
	if (xp > 0) {
	lastz2 = std::max(m_stock[xp - 1][0], m_pz);
	}

	std::vector<MeshCore::MeshGeomFacet>* facets = &facetsInner;
	if (xp == 0 \|\| xp == m_x) {
	facets = &facetsOuter;
	}

	// bool lastzclip = (lastz - m_pz) < m_res;
	int lastpoint = 0;
	for (int y = 1; y <= m_y; y++) {
	float newz1 = m_pz;
	if (xp < m_x && y < m_y) {
	newz1 = std::max(m_stock[xp][y], m_pz);
	}
	float newz2 = m_pz;
	if (xp > 0 && y < m_y) {
	newz2 = std::max(m_stock[xp - 1][y], m_pz);
	}

	if (fabs(lastz1 - lastz2) > m_res) {
	if (fabs(newz1 - lastz1) < m_res && fabs(newz2 - lastz2) < m_res) {
	continue;
	}
	Point3D pbr(xp, lastpoint, lastz1);
	Point3D pbl(xp, y, lastz1);
	Point3D ptr(xp, lastpoint, lastz2);
	Point3D ptl(xp, y, lastz2);
	AddQuad(pbl, ptl, ptr, pbr, *facets);
	}
	lastz1 = newz1;
	lastz2 = newz2;
	lastpoint = y;
	}
	return 0;
	}

	void cStock::SetFacetPoints(MeshCore::MeshGeomFacet& facet, Point3D& p1, Point3D& p2, Point3D& p3)
	{
	facet._aclPoints[0][0] = p1.x * m_res + m_px;
	facet._aclPoints[0][1] = p1.y * m_res + m_py;
	facet._aclPoints[0][2] = p1.z;
	facet._aclPoints[1][0] = p2.x * m_res + m_px;
	facet._aclPoints[1][1] = p2.y * m_res + m_py;
	facet._aclPoints[1][2] = p2.z;
	facet._aclPoints[2][0] = p3.x * m_res + m_px;
	facet._aclPoints[2][1] = p3.y * m_res + m_py;
	facet._aclPoints[2][2] = p3.z;
	facet.CalcNormal();
	}

	void cStock::AddQuad(
	Point3D& p1,
	Point3D& p2,
	Point3D& p3,
	Point3D& p4,
	std::vector<MeshCore::MeshGeomFacet>& facets
	)
	{
	MeshCore::MeshGeomFacet facet;
	SetFacetPoints(facet, p1, p2, p3);
	facets.push_back(facet);
	SetFacetPoints(facet, p1, p3, p4);
	facets.push_back(facet);
	}

	void cStock::Tessellate(Mesh::MeshObject& meshOuter, Mesh::MeshObject& meshInner)
	{
	// reset attribs
	for (int y = 0; y < m_y; y++) {
	for (int x = 0; x < m_x; x++) {
	m_attr[x][y] = 0;
	}
	}

	facetsOuter.clear();
	facetsInner.clear();

	for (int y = 0; y < m_y; y++) {
	for (int x = 0; x < m_x; x++) {
	int attr = m_attr[x][y];
	if ((attr & SIM_TESSEL_TOP) == 0) {
	x += TesselTop(x, y);
	}
	}
	}
	for (int y = 0; y < m_y; y++) {
	for (int x = 0; x < m_x; x++) {
	if ((m_stock[x][y] - m_pz) < m_res) {
	m_attr[x][y] \|= SIM_TESSEL_BOT;
	}
	if ((m_attr[x][y] & SIM_TESSEL_BOT) == 0) {
	x += TesselBot(x, y);
	}
	}
	}
	for (int y = 0; y <= m_y; y++) {
	TesselSidesX(y);
	}
	for (int x = 0; x <= m_x; x++) {
	TesselSidesY(x);
	}
	meshOuter.addFacets(facetsOuter);
	meshInner.addFacets(facetsInner);
	facetsOuter.clear();
	facetsInner.clear();
	}


	void cStock::CreatePocket(float cxf, float cyf, float radf, float height)
	{
	int cx = (int)((cxf - m_px) / m_res);
	int cy = (int)((cyf - m_py) / m_res);
	int rad = (int)(radf / m_res);
	int drad = rad * rad;
	int ys = std::max(0, cy - rad);
	int ye = std::min(m_x, cy + rad);
	int xs = std::max(0, cx - rad);
	int xe = std::min(m_x, cx + rad);
	for (int y = ys; y < ye; y++) {
	for (int x = xs; x < xe; x++) {
	if (((x - cx) * (x - cx) + (y - cy) * (y - cy)) < drad) {
	if (m_stock[x][y] > height) {
	m_stock[x][y] = height;
	}
	}
	}
	}
	}

	void cStock::ApplyLinearTool(Point3D& p1, Point3D& p2, cSimTool& tool)
	{
	// translate coordinates
	Point3D pi1 = ToInner(p1);
	Point3D pi2 = ToInner(p2);
	float rad = tool.radius;
	rad /= m_res;
	float cupAngle = 180;

	// strait motion
	float perpDirX = 1;
	float perpDirY = 0;
	cLineSegment path(pi1, pi2);
	if (path.lenXY > SIM_EPSILON) // only if moving along xy
	{
	perpDirX = -path.pDirXY.y;
	perpDirY = path.pDirXY.x;
	Point3D start(perpDirX * rad + pi1.x, perpDirY * rad + pi1.y, pi1.z);
	Point3D mainWay = path.pDir * SIM_WALK_RES;
	Point3D sideWay(-perpDirX * SIM_WALK_RES, -perpDirY * SIM_WALK_RES, 0);
	int lenSteps = (int)(path.len / SIM_WALK_RES) + 1;
	int radSteps = (int)(rad * 2 / SIM_WALK_RES) + 1;
	float zstep = (pi2.z - pi1.z) / radSteps;
	float tstep = 2.0 / radSteps;
	float t = -1;
	for (int j = 0; j < radSteps; j++) {
	float z = pi1.z + tool.GetToolProfileAt(t);
	Point3D p = start;
	for (int i = 0; i < lenSteps; i++) {
	int x = (int)p.x;
	int y = (int)p.y;
	if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
	if (m_stock[x][y] > z) {
	m_stock[x][y] = z;
	}
	}
	p.Add(mainWay);
	z += zstep;
	}
	t += tstep;
	start.Add(sideWay);
	}
	}
	else {
	cupAngle = 360;
	}

	// end cup
	for (float r = 0.5f; r <= rad; r += (float)SIM_WALK_RES) {
	Point3D cupCirc(perpDirX * r, perpDirY * r, pi2.z);
	float rotang = 180 * SIM_WALK_RES / (pi * r);
	cupCirc.SetRotationAngle(-rotang);
	float z = pi2.z + tool.GetToolProfileAt(r / rad);
	for (float a = 0; a < cupAngle; a += rotang) {
	int x = (int)(pi2.x + cupCirc.x);
	int y = (int)(pi2.y + cupCirc.y);
	if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
	if (m_stock[x][y] > z) {
	m_stock[x][y] = z;
	}
	}
	cupCirc.Rotate();
	}
	}
	}

	void cStock::ApplyCircularTool(Point3D& p1, Point3D& p2, Point3D& cent, cSimTool& tool, bool isCCW)
	{
	// translate coordinates
	Point3D pi1 = ToInner(p1);
	Point3D pi2 = ToInner(p2);
	Point3D centi(cent.x / m_res, cent.y / m_res, cent.z);
	float rad = tool.radius;
	rad /= m_res;
	float cpx = centi.x;
	float cpy = centi.y;

	Point3D xynorm = unit(Point3D(-cpx, -cpy, 0));
	float crad = sqrt(cpx * cpx + cpy * cpy);
	float crad1 = std::max((float)0.5, crad - rad);
	float crad2 = crad + rad;

	float sang = atan2(-cpy, -cpx); // start angle

	cpx += pi1.x;
	cpy += pi1.y;
	double eang = atan2(pi2.y - cpy, pi2.x - cpx); // end angle

	double ang = eang - sang;
	if (!isCCW && ang > 0) {
	ang -= 2 * pi;
	}
	if (isCCW && ang < 0) {
	ang += 2 * pi;
	}
	ang = fabs(ang);

	// apply path
	Point3D cupCirc;
	float tstep = (float)SIM_WALK_RES / rad;
	float t = -1;
	for (float r = crad1; r <= crad2; r += (float)SIM_WALK_RES) {
	cupCirc.x = xynorm.x * r;
	cupCirc.y = xynorm.y * r;
	float rotang = (float)SIM_WALK_RES / r;
	int ndivs = (int)(ang / rotang) + 1;
	if (!isCCW) {
	rotang = -rotang;
	}
	cupCirc.SetRotationAngleRad(rotang);
	float z = pi1.z + tool.GetToolProfileAt(t);
	float zstep = (pi2.z - pi1.z) / ndivs;
	for (int i = 0; i < ndivs; i++) {
	int x = (int)(cpx + cupCirc.x);
	int y = (int)(cpy + cupCirc.y);
	if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
	if (m_stock[x][y] > z) {
	m_stock[x][y] = z;
	}
	}
	z += zstep;
	cupCirc.Rotate();
	}
	t += tstep;
	}

	// apply end cup
	xynorm.SetRotationAngleRad(ang);
	xynorm.Rotate();
	for (float r = 0.5f; r <= rad; r += (float)SIM_WALK_RES) {
	Point3D cupCirc(xynorm.x * r, xynorm.y * r, 0);
	float rotang = (float)SIM_WALK_RES / r;
	int ndivs = (int)(pi / rotang) + 1;
	if (!isCCW) {
	rotang = -rotang;
	}
	cupCirc.SetRotationAngleRad(rotang);
	float z = pi2.z + tool.GetToolProfileAt(r / rad);
	for (int i = 0; i < ndivs; i++) {
	int x = (int)(pi2.x + cupCirc.x);
	int y = (int)(pi2.y + cupCirc.y);
	if (x >= 0 && y >= 0 && x < m_x && y < m_y) {
	if (m_stock[x][y] > z) {
	m_stock[x][y] = z;
	}
	}
	cupCirc.Rotate();
	}
	}
	}


	//************************************************************************************************************
	// Line Segment
	//************************************************************************************************************

	void cLineSegment::SetPoints(Point3D& p1, Point3D& p2)
	{
	pStart = p1;
	pDir = unit(p2 - p1);
	Point3D dirXY(pDir.x, pDir.y, 0);
	lenXY = length(dirXY);
	len = length(p2 - p1);
	if (len > SIM_EPSILON) {
	pDirXY = unit(dirXY);
	}
	}

	void cLineSegment::PointAt(float dist, Point3D& retp)
	{
	retp.x = pStart.x + pDir.x * dist;
	retp.y = pStart.y + pDir.y * dist;
	retp.z = pStart.z + pDir.z * dist;
	}

	//************************************************************************************************************
	// Point (or vector)
	//************************************************************************************************************

	void Point3D::SetRotationAngleRad(float angle)
	{
	sina = sin(angle);
	cosa = cos(angle);
	}

	void Point3D::SetRotationAngle(float angle)
	{
	SetRotationAngleRad(angle * 2 * pi / 360);
	}

	void Point3D::UpdateCmd(Path::Command& cmd)
	{
	if (cmd.has("X")) {
	x = cmd.getPlacement().getPosition()[0];
	}
	if (cmd.has("Y")) {
	y = cmd.getPlacement().getPosition()[1];
	}
	if (cmd.has("Z")) {
	z = cmd.getPlacement().getPosition()[2];
	}
	}

	//************************************************************************************************************
	// Simulation tool
	//************************************************************************************************************
	cSimTool::cSimTool(const TopoDS_Shape& toolShape, float res)
	{

	BRepCheck_Analyzer aChecker(toolShape);
	bool shapeIsValid = aChecker.IsValid() ? true : false;

	if (!shapeIsValid) {
	throw Base::RuntimeError("Path Simulation: Error in tool geometry");
	}

	Bnd_Box boundBox;
	BRepBndLib::Add(toolShape, boundBox);

	boundBox.SetGap(0.0);
	Standard_Real xMin, yMin, zMin, xMax, yMax, zMax;
	boundBox.Get(xMin, yMin, zMin, xMax, yMax, zMax);
	radius = (xMax - xMin) / 2;
	length = zMax - zMin;

	Base::Vector3d pnt;
	pnt.x = 0;
	pnt.y = 0;
	pnt.z = 0;

	int radValue = (int)(radius / res) + 1;

	// Measure the performance of the profile extraction
	// auto start = std::chrono::high_resolution_clock::now();

	for (int x = 0; x < radValue; x++) {
	// find the face of the tool by checking z points across the
	// radius to see if the point is inside the shape
	pnt.x = static_cast<double>(x) * res;
	bool inside = isInside(toolShape, pnt, res);

	// move down until the point is outside the shape
	while (inside && std::abs(pnt.z) < length) {
	pnt.z -= res;
	inside = isInside(toolShape, pnt, res);
	}

	// move up until the point is first inside the shape and record the position
	while (!inside && pnt.z < length) {
	pnt.z += res;
	inside = isInside(toolShape, pnt, res);

	if (inside) {
	toolShapePoint shapePoint;
	shapePoint.radiusPos = pnt.x;
	shapePoint.heightPos = pnt.z;
	m_toolShape.push_back(shapePoint);
	break;
	}
	}
	}

	// Report the performance of the profile extraction
	// auto stop = std::chrono::high_resolution_clock::now();
	// auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
	// Base::Console().log("cSimTool::cSimTool - Tool Profile Extraction Took: %i ms\n",
	// duration.count() / 1000);
	}

	float cSimTool::GetToolProfileAt(float pos) // pos is -1..1 location along the radius of the tool
	// (0 is center)
	{
	toolShapePoint test;
	test.radiusPos = std::abs(pos) * radius;

	auto it
	= std::lower_bound(m_toolShape.begin(), m_toolShape.end(), test, toolShapePoint::less_than());
	return it != m_toolShape.end() ? it->heightPos : 0.0f;
	}

	bool cSimTool::isInside(const TopoDS_Shape& toolShape, Base::Vector3d pnt, float res)
	{
	bool checkFace = true;
	TopAbs_State stateIn = TopAbs_IN;

	try {
	BRepClass3d_SolidClassifier solidClassifier(toolShape);
	gp_Pnt vertex = gp_Pnt(pnt.x, pnt.y, pnt.z);
	solidClassifier.Perform(vertex, res);
	bool inside = (solidClassifier.State() == stateIn);
	if (checkFace && solidClassifier.IsOnAFace()) {
	inside = true;
	}
	return inside;
	}
	catch (...) {
	return false;
	}
	}

	cVolSim::cVolSim()
	: stock(nullptr)
	{}


	cVolSim::~cVolSim()
	{}