From 5a2296ea559f5cf47dfd8e450d7eb5132086aa2a Mon Sep 17 00:00:00 2001 From: Shai Seger Date: Thu, 19 Oct 2017 09:41:32 +0300 Subject: [PATCH] initial sim engin added. initial python interface. --- src/Mod/Path/PathSimulator/App/CMakeLists.txt | 27 +- .../App/{PathSimulator.cpp => PathSim.cpp} | 63 +- .../App/{PathSimulator.h => PathSim.h} | 37 +- src/Mod/Path/PathSimulator/App/PathSimPy.xml | 39 + .../Path/PathSimulator/App/PathSimPyImp.cpp | 63 ++ src/Mod/Path/PathSimulator/App/PreCompiled.h | 4 +- src/Mod/Path/PathSimulator/App/VolSim.cpp | 720 ++++++++++++++++++ src/Mod/Path/PathSimulator/App/VolSim.h | 184 +++++ 8 files changed, 1107 insertions(+), 30 deletions(-) rename src/Mod/Path/PathSimulator/App/{PathSimulator.cpp => PathSim.cpp} (63%) rename src/Mod/Path/PathSimulator/App/{PathSimulator.h => PathSim.h} (73%) create mode 100644 src/Mod/Path/PathSimulator/App/PathSimPy.xml create mode 100644 src/Mod/Path/PathSimulator/App/PathSimPyImp.cpp create mode 100644 src/Mod/Path/PathSimulator/App/VolSim.cpp create mode 100644 src/Mod/Path/PathSimulator/App/VolSim.h diff --git a/src/Mod/Path/PathSimulator/App/CMakeLists.txt b/src/Mod/Path/PathSimulator/App/CMakeLists.txt index b6ae8ef65b..45d37fa534 100644 --- a/src/Mod/Path/PathSimulator/App/CMakeLists.txt +++ b/src/Mod/Path/PathSimulator/App/CMakeLists.txt @@ -1,5 +1,9 @@ include_directories( + ${CMAKE_BINARY_DIR} + ${CMAKE_SOURCE_DIR}/src + ${CMAKE_BINARY_DIR}/src + ${CMAKE_CURRENT_BINARY_DIR} ${Boost_INCLUDE_DIRS} ${OCC_INCLUDE_DIR} ${PYTHON_INCLUDE_DIRS} @@ -8,27 +12,40 @@ include_directories( ) set(PathSimulator_LIBS + Path + Part FreeCADApp ) +SET(Python_SRCS + PathSimPy.xml + PathSimPyImp.cpp +) + + SET(PathSimulator_SRCS AppPathSimulator.cpp - PathSimulator.cpp - PathSimulator.h + PathSim.cpp + PathSim.h + VolSim.cpp + VolSim.h PreCompiled.cpp PreCompiled.h + ${Python_SRCS} ) +generate_from_xml(PathSimPy) + add_library(PathSimulator SHARED ${PathSimulator_SRCS}) target_link_libraries(PathSimulator ${PathSimulator_LIBS}) fc_target_copy_resource(PathSimulator - ${CMAKE_SOURCE_DIR}/src/Mod/Path/PathSimulator - ${CMAKE_BINARY_DIR}/Mod/Path/PathSimulator + ${CMAKE_SOURCE_DIR}/src/Mod/Path + ${CMAKE_BINARY_DIR}/Mod/Path Init.py) -SET_BIN_DIR(PathSimulator PathSimulator /Mod/Path/PathSimulator) +SET_BIN_DIR(PathSimulator PathSimulator /Mod/Path) SET_PYTHON_PREFIX_SUFFIX(PathSimulator) install(TARGETS PathSimulator DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/src/Mod/Path/PathSimulator/App/PathSimulator.cpp b/src/Mod/Path/PathSimulator/App/PathSim.cpp similarity index 63% rename from src/Mod/Path/PathSimulator/App/PathSimulator.cpp rename to src/Mod/Path/PathSimulator/App/PathSim.cpp index 298d7fe908..865946ddb1 100644 --- a/src/Mod/Path/PathSimulator/App/PathSimulator.cpp +++ b/src/Mod/Path/PathSimulator/App/PathSim.cpp @@ -28,23 +28,62 @@ #include -#include -#include -#include +#include +#include #include -// KDL stuff - at the moment, not used -//#include "Mod/Robot/App/kdl_cp/path_line.hpp" -//#include "Mod/Robot/App/kdl_cp/path_circle.hpp" -//#include "Mod/Robot/App/kdl_cp/rotational_interpolation_sa.hpp" -//#include "Mod/Robot/App/kdl_cp/utilities/error.h" +#include "PathSim.h" +//#include "VolSim.h" -#include "PathSimulator.h" - -using namespace Path; using namespace Base; +using namespace PathSimulator; -TYPESYSTEM_SOURCE(Path::PathSimulator , Base::Persistence); +TYPESYSTEM_SOURCE(PathSimulator::PathSim , Base::BaseClass); + +PathSim::PathSim() +{ + m_stock = nullptr; + m_tool = nullptr; +} + +PathSim::~PathSim() +{ + if (m_stock != nullptr) + delete m_stock; + if (m_tool != nullptr) + delete m_tool; +} + + +void PathSim::BeginSimulation(Part::TopoShape * stock, float resolution) +{ + Base::BoundBox3d & bbox = stock->getBoundBox(); + m_stock = new cStock(bbox.MinX, bbox.MinY, bbox.MinZ, bbox.LengthX(), bbox.LengthY(), bbox.LengthZ(), resolution); +} + +void PathSim::SetCurrentTool(Tool * tool) +{ + cSimTool::Type tp = cSimTool::FLAT; + float angle = 180; + switch (tool->Type) + { + case Tool::BALLENDMILL: + tp = cSimTool::ROUND; + break; + + case Tool::CHAMFERMILL: + tp = cSimTool::CHAMFER; + angle = tool->CuttingEdgeAngle; + break; + } + m_tool = new cSimTool(tp, tool->Diameter / 2.0, angle); +} + + +void PathSim::ApplyCommand(Command * cmd) +{ + +} diff --git a/src/Mod/Path/PathSimulator/App/PathSimulator.h b/src/Mod/Path/PathSimulator/App/PathSim.h similarity index 73% rename from src/Mod/Path/PathSimulator/App/PathSimulator.h rename to src/Mod/Path/PathSimulator/App/PathSim.h index 01cf101da9..228a66aa48 100644 --- a/src/Mod/Path/PathSimulator/App/PathSimulator.h +++ b/src/Mod/Path/PathSimulator/App/PathSim.h @@ -21,33 +21,46 @@ ***************************************************************************/ -#ifndef PATH_PathSimulator_H -#define PATH_PathSimulator_H +#ifndef PATHSIMULATOR_PathSim_H +#define PATHSIMULATOR_PathSim_H + +// Exporting of App classes #include #include +#include +#include +#include +#include +#include +#include +#include "VolSim.h" -namespace Path +using namespace Path; + +namespace PathSimulator { /** The representation of a CNC Toolpath Simulator */ - class PathSimulatorAppExport PathSimulator : public Base::Persistence + class PathSimulatorExport PathSim : public Base::BaseClass { TYPESYSTEM_HEADER(); public: - PathSimulator() {}; - ~PathSimulator() {}; + PathSim(); + ~PathSim(); - virtual unsigned int getMemSize(void) const { - return 0; - }; - virtual void Save(Base::Writer &/*writer*/) const {}; - virtual void Restore(Base::XMLReader &/*reader*/) {}; + void BeginSimulation(Part::TopoShape * stock, float resolution); + void SetCurrentTool(Tool * tool); + void ApplyCommand(Command * cmd); + + public: + cStock * m_stock; + cSimTool *m_tool; }; } //namespace Path -#endif // PATH_PathSimulator_H +#endif // PATHSIMULATOR_PathSim_H diff --git a/src/Mod/Path/PathSimulator/App/PathSimPy.xml b/src/Mod/Path/PathSimulator/App/PathSimPy.xml new file mode 100644 index 0000000000..4de8b7f8c6 --- /dev/null +++ b/src/Mod/Path/PathSimulator/App/PathSimPy.xml @@ -0,0 +1,39 @@ + + + + + + FreeCAD python wrapper of PathSimulator\n +PathSimulator.PathSim():\n +Create a path simulator object\n + + + + BeginSimulation(stock, resolution):\n +Start a simulation process on a box shape stock with given resolution\n + + + + + SetCurrentTool(tool):\n +Set the current Path Tool for the subsequent simulator operations.\n + + + + + Return current simulation tool. + + + + + diff --git a/src/Mod/Path/PathSimulator/App/PathSimPyImp.cpp b/src/Mod/Path/PathSimulator/App/PathSimPyImp.cpp new file mode 100644 index 0000000000..915b7ef82f --- /dev/null +++ b/src/Mod/Path/PathSimulator/App/PathSimPyImp.cpp @@ -0,0 +1,63 @@ + +#include "PreCompiled.h" + +#include "Mod/Path/PathSimulator/App/PathSim.h" + +// inclusion of the generated files (generated out of PathSimPy.xml) +#include "PathSimPy.h" +#include "PathSimPy.cpp" + +using namespace PathSimulator; + +// returns a string which represents the object e.g. when printed in python +std::string PathSimPy::representation(void) const +{ + return std::string(""); +} + +PyObject *PathSimPy::PyMake(struct _typeobject *, PyObject *, PyObject *) // Python wrapper +{ + // create a new instance of PathSimPy and the Twin object + return new PathSimPy(new PathSim); +} + +// constructor method +int PathSimPy::PyInit(PyObject* /*args*/, PyObject* /*kwd*/) +{ + return 0; +} + + +PyObject* PathSimPy::BeginSimulation(PyObject * /*args*/, PyObject * /*kwds*/) +{ + PyErr_SetString(PyExc_NotImplementedError, "Not yet implemented"); + return 0; +} + +PyObject* PathSimPy::SetCurrentTool(PyObject * /*args*/) +{ + PyErr_SetString(PyExc_NotImplementedError, "Not yet implemented"); + return 0; +} + + + + + +Py::Object PathSimPy::getTool(void) const +{ + //return Py::Object(); + throw Py::AttributeError("Not yet implemented"); +} + +PyObject *PathSimPy::getCustomAttributes(const char* /*attr*/) const +{ + return 0; +} + +int PathSimPy::setCustomAttributes(const char* /*attr*/, PyObject* /*obj*/) +{ + return 0; +} + + diff --git a/src/Mod/Path/PathSimulator/App/PreCompiled.h b/src/Mod/Path/PathSimulator/App/PreCompiled.h index 428640436d..c1a83a5430 100644 --- a/src/Mod/Path/PathSimulator/App/PreCompiled.h +++ b/src/Mod/Path/PathSimulator/App/PreCompiled.h @@ -28,7 +28,9 @@ // Exporting of App classes #ifdef FC_OS_WIN32 -# define PathSimulatorAppExport __declspec(dllexport) +# define PathSimulatorExport __declspec(dllexport) +# define PathExport __declspec(dllexport) +# define PartExport __declspec(dllimport) #else // for Linux # define PathSimulatorAppExport #endif diff --git a/src/Mod/Path/PathSimulator/App/VolSim.cpp b/src/Mod/Path/PathSimulator/App/VolSim.cpp new file mode 100644 index 0000000000..07af032652 --- /dev/null +++ b/src/Mod/Path/PathSimulator/App/VolSim.cpp @@ -0,0 +1,720 @@ +/*************************************************************************** +* Copyright (c) Shsi Seger (shaise at gmail) 2017 * +* * +* 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 +#include "VolSim.h" + +//************************************************************************************************************ +// 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)(lx / res) + 1; + m_y = (int)(ly / res) + 1; + int memsize = m_x * m_y; + 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 || abs(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 || abs(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 || abs(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 || abs(z - m_stock[x][ty]) > m_res) + { + yd_ok = false; + break; + } + } + if (yd_ok) + { + y_size++; + yp--; + } + } + } + } + return z; +} + +int cStock::TesselTop(Model3D *model, 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); + model->AddQuad(pbl, pbr, ptr, ptl); + } + + 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(Model3D *model, 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); + model->AddQuad(pbl, ptl, ptr, pbr); + + if (farRect) + return -1; + return std::max(0, x_size - 1); + //return 0; +} + + +int cStock::TesselSidesX(Model3D *model, 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); + + //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 (abs(lastz1 - lastz2) > m_res) + { + if (abs(newz1 - lastz1) < m_res && abs(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); + if (lastz2 > lastz1) + model->AddQuad(pbl, pbr, ptr, ptl); + else + model->AddQuad(pbl, ptl, ptr, pbr); + } + lastz1 = newz1; + lastz2 = newz2; + lastpoint = x; + } + return 0; +} + +int cStock::TesselSidesY(Model3D *model, 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); + + //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 (abs(lastz1 - lastz2) > m_res) + { + if (abs(newz1 - lastz1) < m_res && abs(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); + if (lastz2 > lastz1) + model->AddQuad(pbl, pbr, ptr, ptl); + else + model->AddQuad(pbl, ptl, ptr, pbr); + } + lastz1 = newz1; + lastz2 = newz2; + lastpoint = y; + } + return 0; +} + +void cStock::AdjustCoordinates(Model3D *model) +{ + for (int i = 0; i < model->triangles.size(); i++) + { + for (int j = 0; j < 3; j++) + { + Point3D & t = model->triangles[i].points[j]; + t.x = (float)t.x * m_res + m_px; + t.y = (float)t.y * m_res + m_py; + } + } +} + + +Model3D *cStock::Tesselate() +{ + Model3D *model = new Model3D(); + 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(model, 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(model, x, y); + } + } + for (int y = 0; y <= m_y; y++) + TesselSidesX(model, y); + for (int x = 0; x <= m_x; x++) + TesselSidesY(model, x); + AdjustCoordinates(model); + return model; +} + + +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) +{ + // tanslate 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.5; r <= rad; r += SIM_WALK_RES) + { + Point3D cupCirc(perpDirX * r, perpDirY * r, pi2.z); + float rotang = 180 * SIM_WALK_RES / (3.1415926535 * 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) +{ + // tanslate 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); + //bool shortRad = (crad - rad) < 0.0001; + //if (shortRad) + // rad = crad; + 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 * 3.1415926; + if (isCCW && ang < 0) + ang += 2 * 3.1415926; + ang = abs(ang); + + // apply path + Point3D cupCirc; + float tstep = (float)SIM_WALK_RES / rad; + float t = -1; + for (float r = crad1; r <= crad2; r += 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.5; r <= rad; r += SIM_WALK_RES) + { + Point3D cupCirc(xynorm.x * r, xynorm.y * r, 0); + float rotang = (float)SIM_WALK_RES / r; + int ndivs = (int)(3.1415926535 / 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.x = pStart.y + pDir.y * dist; + retp.x = 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 * 3.1415926535 / 360); +} + +//************************************************************************************************************ +// 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: + { + if (pos < 0) return -chamRatio * pos; + return chamRatio * pos; + } + + case ROUND: + pos *= radius; + return radius - sqrt(dradius - pos * pos); + break; + } + return 0; +} + +void cSimTool::InitTool() // pos is 0..1 location along the radius of the tool +{ + switch (type) + { + case CHAMFER: + chamRatio = radius * tan((90.0 - tipAngle / 2) * 3.1415926535 / 180); + break; + + case ROUND: + dradius = radius * radius; + break; + } +} + +cVolSim::cVolSim() +{ +} + + +cVolSim::~cVolSim() +{ +} diff --git a/src/Mod/Path/PathSimulator/App/VolSim.h b/src/Mod/Path/PathSimulator/App/VolSim.h new file mode 100644 index 0000000000..1af34a137a --- /dev/null +++ b/src/Mod/Path/PathSimulator/App/VolSim.h @@ -0,0 +1,184 @@ +/*************************************************************************** +* Copyright (c) Shsi Seger (shaise at gmail) 2017 * +* * +* 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 * +* * +*************************************************************************** +* Volumetric Path simulation engine * +***************************************************************************/ + + +#ifndef PATHSIMULATOR_VolSim_H +#define PATHSIMULATOR_VolSim_H + +#include + +#define SIM_EPSILON 0.00001 +#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 Point3D +{ + Point3D() {} + Point3D(float x, float y, float z) : x(x), y(y), z(z) {} + inline void set(float px, float py, float pz) { x = px; y = py; z = pz; } + inline void Add(Point3D & p) { x += p.x; y += p.y; z += p.z; } + inline void Rotate() { float tx = x; x = x * cosa - y * sina; y = tx * sina + y * cosa; } + void SetRotationAngle(float angle); + void SetRotationAngleRad(float angle); + float x, y, z; + float sina, cosa; +}; + +// some vector manipulations +inline static Point3D operator + (const Point3D & a, const Point3D & b) { return Point3D(a.x + b.x, a.y + b.y, a.z + b.z); } +inline static Point3D operator - (const Point3D & a, const Point3D & b) { return Point3D(a.x - b.x, a.y - b.y, a.z - b.z); } +inline static Point3D operator * (const Point3D & a, double b) { return Point3D(a.x * b, a.y * b, a.z * b); } +inline static Point3D operator / (const Point3D & a, double b) { return a * (1.0f / b); } +inline static double dot(const Point3D & a, const Point3D & b) { return a.x * b.x + a.y * b.y + a.z * b.z; } +inline static double length(const Point3D & a) { return sqrtf(dot(a, a)); } +inline static Point3D unit(const Point3D & a) { return a / length(a); } + + +struct Triangle3D +{ + Triangle3D() {} + Triangle3D(Point3D & p1, Point3D & p2, Point3D & p3) + { + points[0] = p1; + points[1] = p2; + points[2] = p3; + } + Point3D points[3]; +}; + +struct cLineSegment +{ + cLineSegment() {} + cLineSegment(Point3D & p1, Point3D & p2) { SetPoints(p1, p2); } + void SetPoints(Point3D & p1, Point3D & p2); + void PointAt(float dist, Point3D & retp); + Point3D pStart; + Point3D pDir; + Point3D pDirXY; + float len; + float lenXY; +}; + +struct Model3D +{ + Model3D() {} + std::vector triangles; + inline void AddQuad(Point3D & p1, Point3D & p2, Point3D & p3, Point3D & p4) + { + Triangle3D t1(p1, p2, p3); + Triangle3D t2(p1, p3, p4); + triangles.push_back(t1); + triangles.push_back(t2); + } +}; + +class cSimTool +{ +public: + enum Type { + FLAT = 0, + CHAMFER, + ROUND + }; + cSimTool() {} + cSimTool(Type t, float rad, float tipang = 180) : type(t), radius(rad), tipAngle(tipang) { InitTool(); } + ~cSimTool() {} + void InitTool(); + + Type type; + float tipAngle; + float radius; + float dradius; + float chamRatio; + float GetToolProfileAt(float pos); +}; + +template +class Array2D +{ +public: + Array2D() : data(nullptr) {} + + ~Array2D() + { + if (data != nullptr) + delete[] data; + } + + void Init(int x, int y) + { + data = new T[x * y]; + height = y; + } + + T *operator [] (int i) { return data + i * height; } + +private: + T *data; + int height; +}; + + +class cStock +{ +public: + cStock(float px, float py, float pz, float lx, float ly, float lz, float res); + ~cStock(); + Model3D *Tesselate(); + void CreatePocket(float x, float y, float rad, float height); + void ApplyLinearTool(Point3D & p1, Point3D & p2, cSimTool &tool); + void ApplyCircularTool(Point3D & p1, Point3D & p2, Point3D & cent, cSimTool &tool, bool isCCW); + inline Point3D & ToInner(Point3D & p) { + return Point3D((p.x - m_px) / m_res, (p.y - m_py) / m_res, p.z); + } + +private: + float FindRectTop(int & xp, int & yp, int & x_size, int & y_size, bool scanHoriz); + void FindRectBot(int & xp, int & yp, int & x_size, int & y_size, bool scanHoriz); + int TesselTop(Model3D *model, int x, int y); + int TesselBot(Model3D *model, int x, int y); + int TesselSidesX(Model3D *model, int yp); + int TesselSidesY(Model3D *model, int xp); + void AdjustCoordinates(Model3D *model); + Array2D m_stock; + Array2D m_attr; + float m_px, m_py, m_pz; // stock zero position + float m_lx, m_ly, m_lz; // stock dimensions + float m_res; // resoulution + float m_plane; // stock plane height + int m_x, m_y; // stock array size +}; + +class cVolSim +{ +public: + cVolSim(); + ~cVolSim(); + void CreateStock(); + +private: + cStock *stock; +}; + +#endif // PATHSIMULATOR_VolSim_H