From 980d9a929a72e2e22e1c54846e491b6748420e17 Mon Sep 17 00:00:00 2001 From: Daniel Wood Date: Wed, 29 Apr 2020 19:41:42 +0100 Subject: [PATCH] Extract the shape of the tool from the solid shape --- src/Mod/Path/PathSimulator/App/VolSim.cpp | 121 +++++++++++++++++----- src/Mod/Path/PathSimulator/App/VolSim.h | 32 +++--- 2 files changed, 111 insertions(+), 42 deletions(-) diff --git a/src/Mod/Path/PathSimulator/App/VolSim.cpp b/src/Mod/Path/PathSimulator/App/VolSim.cpp index 2ba445058695..e313e12fc8f9 100644 --- a/src/Mod/Path/PathSimulator/App/VolSim.cpp +++ b/src/Mod/Path/PathSimulator/App/VolSim.cpp @@ -21,6 +21,10 @@ ***************************************************************************/ #include "PreCompiled.h" +#include + +#include +#include #ifndef _PreComp_ # include @@ -712,42 +716,103 @@ void Point3D::UpdateCmd(Path::Command & cmd) //************************************************************************************************************ // Simulation tool //************************************************************************************************************ -float cSimTool::GetToolProfileAt(float pos) // pos is -1..1 location along the radius of the tool (0 is center) -{ - switch (type) - { - case FLAT: - return 0; - - case CHAMFER: +cSimTool::cSimTool(const TopoDS_Shape& toolShape, float res){ + + 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; + // Report the size of the bounding box + //Base::Console().Log("PathSim::SetToolShape: radius: %f length: %f\n", radius, length); + + Base::Vector3d pnt; + pnt.x = 0; + pnt.y = 0; + pnt.z = 0; + //float res = 0.025; // resolution + Base::Console().Log("PathSim::SetToolShape: res: %f\n", res); + + 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++) { - if (pos < 0) return -chamRatio * pos; - return chamRatio * pos; + // find the face of the tool by checking z points accross the + // radius to see if the point is inside the shape + pnt.x = x * res; + bool inside = isInside(toolShape, pnt); + + // move down until the point is outside the shape + while(inside && std::abs(pnt.z) < length ){ + //Base::Console().Log("PathSim::BeginSimulation: Pnt Inside: X Pos %f\n", pnt.x); + pnt.z -= res; + inside = isInside(toolShape, pnt); + } + + // 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); + + if (inside){ + toolShapePoint shapePoint; + shapePoint.radiusPos = pnt.x; + shapePoint.heightPos = pnt.z; + m_toolShape.push_back(shapePoint); + //Base::Console().Log("PathSim::BeginSimulation: Add height profile X: radius: %f height: %f\n", pnt.x, pnt.z); + break; + } + } } - case ROUND: - pos *= radius; - return radius - sqrt(dradius - pos * pos); - break; - } - return 0; + // Report the performance of the profile extraction + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); + Base::Console().Log("cSimTool::cSimTool - Tool Profile Extraction Took: %i ms\n", duration.count() / 1000); + } -void cSimTool::InitTool() // pos is 0..1 location along the radius of the tool +float cSimTool::GetToolProfileAt(float pos) // pos is -1..1 location along the radius of the tool (0 is center) { - switch (type) - { - case CHAMFER: - chamRatio = radius * tan((90.0 - tipAngle / 2) * 3.1415926535 / 180); - break; + float radPos = std::abs(pos) * radius; + toolShapePoint test; test.radiusPos = radPos; + auto it = std::lower_bound(m_toolShape.begin(), m_toolShape.end(), test, toolShapePoint::less_than()); + float diff = std::abs(radPos - it->radiusPos); + + if (diff > 0.05){ + Base::Console().Log("requested pos: %f rad: %f diff: %f\n", radPos, it->radiusPos, diff); + } - case ROUND: - dradius = radius * radius; - break; + return it->heightPos; +} + +bool cSimTool::isInside(const TopoDS_Shape& toolShape, Base::Vector3d pnt) +{ + double tolerance = 0.011; + 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, tolerance); + bool inside = (solidClassifier.State() == stateIn); + if (checkFace && solidClassifier.IsOnAFace()){ + inside = true; + //Base::Console().Log("PathSim::isInside: Touching Face: True\n"); + } + return inside; + }catch (...) { + return false; + } - case FLAT: - break; - } } cVolSim::cVolSim() : stock(nullptr) diff --git a/src/Mod/Path/PathSimulator/App/VolSim.h b/src/Mod/Path/PathSimulator/App/VolSim.h index 39fb371adfea..cc1ae06f696c 100644 --- a/src/Mod/Path/PathSimulator/App/VolSim.h +++ b/src/Mod/Path/PathSimulator/App/VolSim.h @@ -34,6 +34,18 @@ #define SIM_TESSEL_TOP 1 #define SIM_TESSEL_BOT 2 #define SIM_WALK_RES 0.6 // step size in pixel units (to make sure all pixels in the path are visited) + +struct toolShapePoint { + float radiusPos; + float heightPos; + + struct less_than{ + bool operator()(const toolShapePoint &a, const toolShapePoint &b){ + return a.radiusPos < b.radiusPos; + } + }; +}; + struct Point3D { Point3D() : x(0), y(0), z(0), sina(0), cosa(0) {} @@ -88,22 +100,15 @@ struct cLineSegment class cSimTool { public: - enum Type { - FLAT = 0, - CHAMFER, - ROUND - }; - cSimTool() : type(FLAT), radius(0), tipAngle(0), dradius(0), chamRatio(0) {} - cSimTool(Type t, float rad, float tipang = 180) : type(t), radius(rad), tipAngle(tipang) { InitTool(); } + cSimTool(const TopoDS_Shape& toolShape, float res); ~cSimTool() {} - void InitTool(); - Type type; - float radius; - float tipAngle; - float dradius; - float chamRatio; float GetToolProfileAt(float pos); + bool isInside(const TopoDS_Shape& toolShape, Base::Vector3d pnt); + + std::vector< toolShapePoint > m_toolShape; + float radius; + float length; }; template @@ -131,7 +136,6 @@ class Array2D int height; }; - class cStock { public: