diff --git a/.github/workflows/plug-in-interface.yml b/.github/workflows/plug-in-interface.yml index 94ebb98..a3a080f 100644 --- a/.github/workflows/plug-in-interface.yml +++ b/.github/workflows/plug-in-interface.yml @@ -27,7 +27,7 @@ jobs: fail-fast: true matrix: include: - - os: windows-2019 + - os: windows-latest OUTPUT_DIR: C:/RoboDK/bin/plugins QT_VERSION: "5.15.2" @@ -51,14 +51,17 @@ jobs: cache: "true" cache-key-prefix: "install-qt-action-${{ matrix.os }}" + - name: Prepare Visual Studio environment on Windows + if: startsWith(matrix.os, 'windows') + uses: johnnynunez/vsdevenv-shell@v2 + - name: Configure and build Plug-In-Interface on Windows if: startsWith(matrix.os, 'windows') run: | cd ${{ github.workspace }} - call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" x86_amd64 qmake Plug-In-Interface.pro -spec win32-msvc CONFIG+=qtquickcompiler CONFIG+=${{ env.BUILD_TYPE }} call %IQTA_TOOLS%\QtCreator\bin\jom\jom.exe - shell: cmd + shell: vsdevenv x64 cmd {0} - name: Configure and build Plug-In-Interface on Unix if: (!startsWith(matrix.os, 'windows')) diff --git a/Plugin-OPC-UA/PluginOPCUA.pro b/Plugin-OPC-UA/PluginOPCUA.pro index 07e570a..632958f 100644 --- a/Plugin-OPC-UA/PluginOPCUA.pro +++ b/Plugin-OPC-UA/PluginOPCUA.pro @@ -118,18 +118,7 @@ FORMS += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginAppLoader/AppLoader.pro b/PluginAppLoader/AppLoader.pro index f6a6818..4c8f5b6 100644 --- a/PluginAppLoader/AppLoader.pro +++ b/PluginAppLoader/AppLoader.pro @@ -90,6 +90,8 @@ CONFIG(release, debug|release) { #-------------------------- # Add header and source files (use File->New File or Project and add your files) # This can be modified manually or automatically by Qt Creator +INCLUDEPATH += ./zip + HEADERS += \ applistdelegate.h \ apploader.h \ @@ -117,17 +119,5 @@ FORMS += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface ./zip +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- - diff --git a/PluginAttachObject/PluginAttachObject.pro b/PluginAttachObject/PluginAttachObject.pro index db55631..5d2663f 100644 --- a/PluginAttachObject/PluginAttachObject.pro +++ b/PluginAttachObject/PluginAttachObject.pro @@ -104,16 +104,5 @@ SOURCES += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginAttachView/PluginAttachView.pro b/PluginAttachView/PluginAttachView.pro index 7601e8c..20b0abe 100644 --- a/PluginAttachView/PluginAttachView.pro +++ b/PluginAttachView/PluginAttachView.pro @@ -103,16 +103,5 @@ SOURCES += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginBallbarTracker/PluginBallbarTracker.pro b/PluginBallbarTracker/PluginBallbarTracker.pro index 7e91b61..0f1b897 100644 --- a/PluginBallbarTracker/PluginBallbarTracker.pro +++ b/PluginBallbarTracker/PluginBallbarTracker.pro @@ -104,16 +104,5 @@ SOURCES += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginCollisionSensor/PluginCollisionSensor.pro b/PluginCollisionSensor/PluginCollisionSensor.pro index 36a17a6..2413cd1 100644 --- a/PluginCollisionSensor/PluginCollisionSensor.pro +++ b/PluginCollisionSensor/PluginCollisionSensor.pro @@ -103,16 +103,5 @@ SOURCES += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginEmbedding/PluginEmbedding.pro b/PluginEmbedding/PluginEmbedding.pro index 96b701a..952f531 100644 --- a/PluginEmbedding/PluginEmbedding.pro +++ b/PluginEmbedding/PluginEmbedding.pro @@ -96,17 +96,11 @@ SOURCES += \ pluginembedding.cpp \ pluginform.cpp +FORMS += \ + pluginform.ui #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- - -FORMS += \ - pluginform.ui diff --git a/PluginExample/PluginExample.pro b/PluginExample/PluginExample.pro index 1b8bc31..4b3d3db 100644 --- a/PluginExample/PluginExample.pro +++ b/PluginExample/PluginExample.pro @@ -104,20 +104,8 @@ RESOURCES += \ resources1.qrc - #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginLVDT/PluginLVDT.pro b/PluginLVDT/PluginLVDT.pro index 018c73a..ef49480 100644 --- a/PluginLVDT/PluginLVDT.pro +++ b/PluginLVDT/PluginLVDT.pro @@ -99,20 +99,8 @@ SOURCES += \ - #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginLockTCP/PluginLockTCP.pro b/PluginLockTCP/PluginLockTCP.pro index 659a8e3..bd42114 100644 --- a/PluginLockTCP/PluginLockTCP.pro +++ b/PluginLockTCP/PluginLockTCP.pro @@ -100,21 +100,8 @@ SOURCES += \ PluginLockTCP.cpp - - #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginOpenGL-Shaders/PluginChip8Opengl.pro b/PluginOpenGL-Shaders/PluginChip8Opengl.pro index ab6fa14..8f6bd14 100644 --- a/PluginOpenGL-Shaders/PluginChip8Opengl.pro +++ b/PluginOpenGL-Shaders/PluginChip8Opengl.pro @@ -137,18 +137,7 @@ SOURCES += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginOpenGL/PluginOpengl.pro b/PluginOpenGL/PluginOpengl.pro index 6e8abb2..77f7724 100644 --- a/PluginOpenGL/PluginOpengl.pro +++ b/PluginOpenGL/PluginOpengl.pro @@ -122,16 +122,5 @@ SOURCES += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginRealTime/PluginRealTime.pro b/PluginRealTime/PluginRealTime.pro index b42152d..3138225 100644 --- a/PluginRealTime/PluginRealTime.pro +++ b/PluginRealTime/PluginRealTime.pro @@ -107,16 +107,5 @@ RESOURCES += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/PluginRoboUI/PluginRoboUI.pro b/PluginRoboUI/PluginRoboUI.pro index d6306db..56c23a8 100644 --- a/PluginRoboUI/PluginRoboUI.pro +++ b/PluginRoboUI/PluginRoboUI.pro @@ -65,7 +65,6 @@ exists( "$$PWD/../../destdir_rdk_plugins.pri" ) { INCLUDEPATH += \ $${PROJECT_ROOT}/imgui \ - $${PROJECT_ROOT}/../robodk_interface SOURCES += \ imgui/imgui.cpp \ @@ -74,8 +73,6 @@ SOURCES += \ imgui/imgui_impl_qt.cpp \ imgui/imgui_tables.cpp \ imgui/imgui_widgets.cpp \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp \ roboui.cpp HEADERS += \ @@ -86,13 +83,13 @@ HEADERS += \ imgui/imstb_rectpack.h \ imgui/imstb_textedit.h \ imgui/imstb_truetype.h \ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h \ - ../robodk_interface/robodk_interface.h \ - ../robodk_interface/robodktools.h \ - ../robodk_interface/robodktypes.h \ roboui.h RESOURCES += \ roboui.qrc + +#-------------------------- +# Header and source files required by any RoboDK plugin +# Do not change this section, make sure to have the robodk_interface folder up one folder +include($$PWD/../robodk_interface/robodk_interface.pri) +#-------------------------- diff --git a/PluginRobotPilot/PluginRobotPilot.pro b/PluginRobotPilot/PluginRobotPilot.pro index 664c000..5b08fe6 100644 --- a/PluginRobotPilot/PluginRobotPilot.pro +++ b/PluginRobotPilot/PluginRobotPilot.pro @@ -107,16 +107,5 @@ FORMS += \ #-------------------------- # Header and source files required by any RoboDK plugin # Do not change this section, make sure to have the robodk_interface folder up one folder -HEADERS += \ - ../robodk_interface/iitem.h \ - ../robodk_interface/irobodk.h\ - ../robodk_interface/iapprobodk.h \ - ../robodk_interface/robodktypes.h \ - ../robodk_interface/robodktools.h \ - -SOURCES += \ - ../robodk_interface/robodktools.cpp \ - ../robodk_interface/robodktypes.cpp - -INCLUDEPATH += ../robodk_interface +include($$PWD/../robodk_interface/robodk_interface.pri) #-------------------------- diff --git a/README.md b/README.md index 0cc3378..f35c935 100644 --- a/README.md +++ b/README.md @@ -61,6 +61,11 @@ Requirements ------------- Each RoboDK Plug-In must be developed using the Qt Creator and follow Qt's project guidelines. +For convenience in connecting the Plugin Interface to third-party projects, the `robodk_interface` folder contains a file with all dependencies. +This file can be included using the `include` directive in the qmake project: +``` +include($$PWD/../robodk_interface/robodk_interface.pri) +``` It is recommended to use the [PluginExample](./PluginExample/) project to get started with your new RoboDK Plug-In (double click PluginExample.pro to open it with Qt Creator). RoboDK must be installed to develop Plug-Ins. The free version of RoboDK is enough to develop a Plug-In as a proof of concept. diff --git a/robodk_interface/constants.h b/robodk_interface/constants.h new file mode 100644 index 0000000..97331fe --- /dev/null +++ b/robodk_interface/constants.h @@ -0,0 +1,47 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#ifndef ROBODK_CONSTANTS_H +#define ROBODK_CONSTANTS_H + + +namespace robodk +{ + +namespace constants +{ + +constexpr double pi = 3.141592653589793; + +} // namespace constants + +} // namespace robodk + + +#endif // ROBODK_CONSTANTS_H diff --git a/robodk_interface/deprecated.h b/robodk_interface/deprecated.h new file mode 100644 index 0000000..6ea079b --- /dev/null +++ b/robodk_interface/deprecated.h @@ -0,0 +1,45 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#ifndef ROBODK_DEPRECATED_H +#define ROBODK_DEPRECATED_H + + +#ifndef ROBODK_DEPRECATED +# if (defined(_MSC_VER) && _MSC_VER <= 1900) || defined(__MINGW64__) +# define ROBODK_DEPRECATED(text) __declspec(deprecated(text)) +# elif defined __GNUC__ +# define ROBODK_DEPRECATED(text) __attribute__((deprecated)) +# else +# define ROBODK_DEPRECATED(text) [[deprecated(text)]] +# endif +#endif // ROBODK_DEPRECATED + + +#endif // ROBODK_DEPRECATED_H diff --git a/robodk_interface/joints.cpp b/robodk_interface/joints.cpp new file mode 100644 index 0000000..e9258ef --- /dev/null +++ b/robodk_interface/joints.cpp @@ -0,0 +1,233 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#include "joints.h" + +#include +#include + +#include + +#include "legacymatrix2d.h" + + +namespace robodk +{ + +Joints::Joints() + : _dofCount(0) +{ + std::memset(_joints, 0, sizeof(_joints)); + std::memset(_jointsFloat, 0, sizeof(_jointsFloat)); +} + +Joints::Joints(int ndofs) +{ + _dofCount = std::min(ndofs, MaximumJoints); + for (int i = 0; i < _dofCount; i++) + { + _joints[i] = 0.0; + } +} + +Joints::Joints(const double *joints, int ndofs) +{ + SetValues(joints, ndofs); +} + +Joints::Joints(const float *joints, int ndofs) +{ + int ndofs_ok = std::min(ndofs, MaximumJoints); + double jnts[MaximumJoints]; + + for (int i=0; i < ndofs_ok; i++) + { + jnts[i] = joints[i]; + } + SetValues(jnts, ndofs_ok); +} + +Joints::Joints(const legacy::Matrix2D *mat2d, int column, int ndofs) +{ + if (column >= Matrix2D_GetDimension(mat2d, 2)) + { + _dofCount = 0; + } + if (ndofs < 0) + { + ndofs = Matrix2D_GetDimension(mat2d, 1); + } + _dofCount = qMin(ndofs, MaximumJoints); + + double *ptr = Matrix2D_GetColumn(mat2d, column); + SetValues(ptr, _dofCount); +} + +Joints::Joints(const QString &str) +{ + _dofCount = 0; + FromString(str); +} + +const double* Joints::ValuesD() const +{ + return _joints; +} + +const float* Joints::ValuesF() const +{ + for (int i = 0; i < MaximumJoints; i++) + { + _jointsFloat[i] = _joints[i]; + } + + return _jointsFloat; +} + +#ifdef ROBODK_API_FLOATS +const float* Joints::Values() const +{ + return ValuesF(); +} +#else +const double* Joints::Values() const +{ + return _joints; +} +#endif + +double Joints::Compare(const Joints &other) const +{ + double sum_diff = 0.0; + for (int i = 0; i = 0 && new_length < _dofCount) + { + _dofCount = new_length; + } +} + +bool Joints::Valid() +{ + return _dofCount > 0; +} + +void Joints::SetValues(const double *values, int ndofs) +{ + if (ndofs >= 0) + { + _dofCount = qMin(ndofs, MaximumJoints); + } + + for (int i = 0; i < _dofCount; i++) + { + _joints[i] = values[i]; + } +} + +void Joints::SetValues(const float *values, int ndofs) +{ + if (ndofs >= 0) + { + _dofCount = qMin(ndofs, MaximumJoints); + } + + for (int i = 0; i < _dofCount; i++) + { + _joints[i] = values[i]; + } +} + +int Joints::GetValues(double *values) +{ + for (int i = 0; i < _dofCount; i++) + { + values[i] = _joints[i]; + } + + return _dofCount; +} + +#ifdef QT_GUI_LIB +QString Joints::ToString(const QString &separator, int precision) const +{ + QString values; + for (int i = 0; i < _dofCount; i++) + { + if (i > 0) + { + values.append(separator); + } + values.append(QString::number(_joints[i], 'f', precision)); + } + return values; +} + +bool Joints::FromString(const QString &str) +{ + const QLatin1Char separator(','); + + QString s = str; + s.replace(QLatin1Char(';'), separator); + s.replace(QLatin1Char('\t'), separator); + +#if (QT_VERSION < QT_VERSION_CHECK(5, 14, 0)) + const QString::SplitBehavior behavior = QString::SkipEmptyParts; +#else + const Qt::SplitBehavior behavior = Qt::SkipEmptyParts; +#endif + + QStringList jointList = s.split(separator, behavior); + _dofCount = qMin(jointList.length(), MaximumJoints); + for (int i = 0; i < _dofCount; i++) + { + _joints[i] = jointList[i].trimmed().toDouble(); + } + return true; +} +#endif // QT_GUI_LIB + +} // namespace robodk diff --git a/robodk_interface/joints.h b/robodk_interface/joints.h new file mode 100644 index 0000000..d433e97 --- /dev/null +++ b/robodk_interface/joints.h @@ -0,0 +1,175 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#ifndef ROBODK_JOINTS_H +#define ROBODK_JOINTS_H + + +#ifdef QT_GUI_LIB +#include +#else +#error "This class cannot yet be used without the Qt Framework" +#endif + + +namespace robodk +{ + +namespace legacy +{ +struct Matrix2D; +} + +/// The Joints class represents a joint position of a robot (robot axes). +class Joints +{ +public: + /// \brief Joints + Joints(); + + /// \brief Joints + /// \param ndofs number of robot joint axes or degrees of freedom + explicit Joints(int ndofs); + + /// \brief Set joint values given a double array and the number of joint values + /// \param joints Pointer to the joint values + /// \param ndofs Number of joints + Joints(const double* joints, int ndofs = 0); + + /// \brief Set joint values given a float array and the number of joint values + /// \param joints Pointer to the joint values + /// \param ndofs Number of joints + Joints(const float* joints, int ndofs = 0); + + /// \brief Create a copy of an object + /// \param jnts + Joints(const Joints& joints) = default; + + Joints& operator=(const Joints& joints) = default; + + /// \brief Create joint values given a 2D matrix and the column selecting the desired values + /// \param mat2d + /// \param column + /// \param ndofs + Joints(const legacy::Matrix2D* mat2d, int column = 0, int ndofs = -1); + + /// \brief Convert a string to joint values + /// \param str Comma separated joint values (spaces or tabs are also accepted) + Joints(const QString& str); + + ~Joints() = default; + + /// \brief Joint values + /// \return Returns a pointer to the joint data array (doubles) + const double* ValuesD() const; + + /// \brief Joint values + /// \return Returns a pointer to the joint data array (floats) + const float* ValuesF() const; + +#ifdef ROBODK_API_FLOATS + /// \brief Joint values + /// \return Returns a pointer to the joint data array (doubles or floats if ROBODK_API_FLOATS is defined) + const float* Values() const; +#else + /// \brief Joint values + /// \return Returns a pointer to the joint data array (doubles or floats if ROBODK_API_FLOATS is defined) + const double* Values() const; +#endif + + + double Compare(const Joints &other) const; + + /// \brief + /// \return Data same as Values. The only difference is that the array pointer is not const. This is provided for backwards compatibility. + double *Data(); + + /// \brief Number of joint axes of the robot (or degrees of freedom) + /// \return + int Length() const; + + /// Set the length of the array (only shrinking the array is allowed) + void setLength(int new_length); + + /// \brief Check if the joints are valid. For example, when we request the Inverse kinematics and there is no solution the joints will not be valid. + /// (for example, an invalid result after calling class: IItem::SolveIK returns a non valid joints) + /// \return true if it has 1 degree of freedom or more + bool Valid(); + + /// \brief GetValues + /// \param joints joint values in deg or mm + /// \return returns the number of degrees of freedom + int GetValues(double *joints); + + /// \brief Set the joint values in deg or mm. You can also important provide the number of degrees of freedom (6 for a 6 axis robot). + /// \param joints joint values in deg or mm + /// \param ndofs number of degrees of freedom (number of axes or joints) + void SetValues(const double *joints, int ndofs = -1); + + /// \brief Set the joint values in deg or mm (floats). You can also important provide the number of degrees of freedom (6 for a 6 axis robot). + /// \param joints joint values in deg or mm + /// \param ndofs number of degrees of freedom (number of axes or joints) + void SetValues(const float *joints, int ndofs = -1); + +#ifdef QT_GUI_LIB + /// To String operator (use with qDebug() << Joints; + inline operator QString() const { return ToString(); } + + /// \brief Retrieve a string representation of the joint values. + /// \param separator String to add between consecutive joint values + /// \param precision Number of decimals + /// \return string as a QString + QString ToString( + const QString& separator = QLatin1String(", "), + int precision = 3) const; + + /// \brief Set the joint values given a comma-separated string. Tabs and spaces are also allowed. + /// \param str string. Such as "10, 20, 30, 40, 50, 60" + /// \return false if parsing the string failed. True otherwise. + bool FromString(const QString& str); +#endif + +public: + static constexpr int MaximumJoints = 12; + + /// number of degrees of freedom + int _dofCount; + + /// joint values (doubles, used to store the joint values) + double _joints[MaximumJoints]; + + /// joint values (floats, used to return a copy as a float pointer) + mutable float _jointsFloat[MaximumJoints]; +}; + + +} // namespace robodk + + +#endif // ROBODK_JOINTS_H diff --git a/robodk_interface/legacymatrix2d.cpp b/robodk_interface/legacymatrix2d.cpp new file mode 100644 index 0000000..fbcc6cc --- /dev/null +++ b/robodk_interface/legacymatrix2d.cpp @@ -0,0 +1,350 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#include "legacymatrix2d.h" + +#include + +#ifdef QT_GUI_LIB +#include +#include +#include +#endif // QT_GUI_LIB + + +namespace robodk +{ + +namespace legacy +{ + +static void emxInit_real_T(Matrix2D** pMatrix, int dimensions) +{ + if (!pMatrix) + return; + + if (dimensions < 0) + { + *pMatrix = nullptr; + return; + } + + *pMatrix = (Matrix2D*) malloc(sizeof(Matrix2D)); + Matrix2D* matrix = *pMatrix; + if (!matrix) + return; + + matrix->data = nullptr; + matrix->numDimensions = dimensions; + matrix->size = (int*) malloc(sizeof(int) * dimensions); + matrix->allocatedSize = 0; + matrix->canFreeData = true; + + for (int i = 0; i < dimensions; i++) + matrix->size[i] = 0; +} + +static void emxFree_real_T(Matrix2D** pMatrix) +{ + if (!pMatrix) + return; + + auto matrix = *pMatrix; + if (!matrix) + return; + + if (matrix->canFreeData) + free(matrix->data); + + free(matrix->size); + free(matrix); + + *pMatrix = nullptr; +} + +static void emxEnsureCapacity(Matrix2D* matrix, int oldNumel, size_t elementSize) +{ + if (!matrix) + return; + + double *newData; + + if (oldNumel < 0) + oldNumel = 0; + + int newNumel = 1; + for (int i = 0; i < matrix->numDimensions; i++) + newNumel *= matrix->size[i]; + + if (newNumel <= matrix->allocatedSize) + return; + + int newSize = matrix->allocatedSize; + if (newSize < 16) + newSize = 16; + + while (newSize < newNumel) + { + if (newSize > 1073741823) + { + newSize = 2147483647; //MAX_int32_T; + } + else + { + newSize <<= 1; + } + } + + newData = (double*) calloc(newSize, elementSize); + if (matrix->data) + { + memcpy(newData, matrix->data, elementSize * oldNumel); + if (matrix->canFreeData) + free(matrix->data); + } + + matrix->data = newData; + matrix->allocatedSize = newSize; + matrix->canFreeData = true; +} + +Matrix2D* Matrix2D_Create() +{ + Matrix2D* matrix; + emxInit_real_T(&matrix, 2); + return matrix; +} + +void Matrix2D_Delete(Matrix2D** matrix) +{ + emxFree_real_T(matrix); +} + +void Matrix2D_SetDimensions(Matrix2D* matrix, int rows, int columns) +{ + int size = matrix->size[0] * matrix->size[1]; + matrix->size[0] = rows; + matrix->size[1] = columns; + emxEnsureCapacity(matrix, size, sizeof(double)); +} + +int Matrix2D_GetDimension(const Matrix2D* matrix, int dimension) +{ + // ONE BASED!! + if (matrix->numDimensions >= dimension) + { + return matrix->size[dimension - 1]; + } + else + { + return 0; + } +} + +int Matrix2D_ColumnCount(const Matrix2D* matrix) +{ + return Matrix2D_GetDimension(matrix, 2); +} + +int Matrix2D_RowCount(const Matrix2D* matrix) +{ + return Matrix2D_GetDimension(matrix, 1); +} + +double Matrix2D_Get(const Matrix2D* matrix, int row, int column) +{ + // ZERO BASED!! + return matrix->data[matrix->size[0] * column + row]; +} + +void Matrix2D_Set(const Matrix2D* matrix, int row, int column, double value) +{ + // ZERO BASED!! + matrix->data[matrix->size[0] * column + row] = value; +} + +double* Matrix2D_GetColumn(const Matrix2D* matrix, int column) +{ + // ZERO BASED!! + return (matrix->data + matrix->size[0] * column); +} + +bool Matrix2D_Copy(const Matrix2D* source, Matrix2D* destination) +{ + if (source->numDimensions != 2 || destination->numDimensions != 2) + { + Matrix2D_SetDimensions(destination, 0, 0); + return false; + } + + int sz1 = Matrix2D_GetDimension(source, 1); + int sz2 = Matrix2D_GetDimension(source, 2); + Matrix2D_SetDimensions(destination, sz1, sz2); + int numel = sz1 * sz2; + for (int i = 0; i < numel; i++) + { + destination->data[i] = source->data[i]; + } + + return true; +} + +#ifdef QT_GUI_LIB +void Matrix2D_DebugArray(const double *array, int size) +{ + int i; + QString strout; + for (i = 0; i < size; i++) { + strout.append(QString::number(array[i], 'f', 3)); + if (i < size - 1) { + strout.append(" , "); + } + } + qDebug().noquote() << strout; +} + +void Matrix2D_Debug(const Matrix2D* matrix) +{ + int size1; + int size2; + int j; + double *column; + size1 = Matrix2D_RowCount(matrix); + size2 = Matrix2D_ColumnCount(matrix); + qDebug().noquote() << "Matrix size = " << size1 << " x " << size2; + if (size1*size2 == 0) + { + return; + } + for (j = 0; jnumDimensions; + int size_values = 1; + for (i = 0; i < matrix->numDimensions; i++) + { + qint32 sizei = matrix->size[i]; + size_values = size_values * sizei; + *stream << sizei; + } + for (i = 0; i < size_values; i++) + { + *stream << matrix->data[i]; + } +} + +void Matrix2D_Load(QDataStream *stream, Matrix2D** matrix) +{ + if (stream->atEnd()) + { + qDebug() << "No data to read"; + return; + } + + if (*matrix != nullptr) + { + Matrix2D_Delete(matrix); + } + + int i; + qint32 ndim; + qint32 sizei; + *stream >> ndim; + qDebug() << "Loading matrix of dimensions: " << ndim; + emxInit_real_T(matrix, ndim); + int size_values = 1; + for (i = 0; i < ndim; i++) + { + *stream >> sizei; + size_values = size_values * sizei; + (*matrix)->size[i] = sizei; + } + //emxEnsureCapacity((emxArray__common *) *emx, 0, (int32_T)sizeof(real_T)); + emxEnsureCapacity(*matrix, 0, sizeof(double)); + double value; + for (i = 0; i < size_values; i++) + { + *stream >> value; + (*matrix)->data[i] = value; + } +} +#endif // QT_GUI_LIB + + +} // namespace legacy + +} // namespace robodk diff --git a/robodk_interface/legacymatrix2d.h b/robodk_interface/legacymatrix2d.h new file mode 100644 index 0000000..ad3a505 --- /dev/null +++ b/robodk_interface/legacymatrix2d.h @@ -0,0 +1,191 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#ifndef ROBODK_LEGACY_MATRIX2D_H +#define ROBODK_LEGACY_MATRIX2D_H + + +#ifdef QT_GUI_LIB +class QTextStream; +class QDataStream; +#endif // QT_GUI_LIB + + +namespace robodk +{ + +namespace legacy +{ + +/*! + \struct Matrix2D + \brief The Matrix2D struct represents a variable size 2D matrix. + + Use the Matrix2D_... functions to operate on this variable sized matrix. + This type of data can be used to get/set a program as a list. + This is also useful for backwards compatibility functions related to RoKiSim. +*/ +struct Matrix2D +{ + /*! + Pointer to the data. + */ + double* data; + + /*! + Pointer to the size array. + */ + int* size; + + /*! + Allocated size. + */ + int allocatedSize; + + /*! + Number of dimensions (usually 2). + */ + int numDimensions; + + bool canFreeData; +}; + +/*! + Creates a new \ref Matrix2D object with no dimensions. + + \returns pointer to the Matrix2D object if no allocation errors occurred; nullptr otherwise. + + \sa Matrix2D_Delete() +*/ +Matrix2D* Matrix2D_Create(); + +/*! + Deletes an existing \ref Matrix2D object. + + \param matrix pointer of the pointer variable to the matrix object. + + \sa Matrix2D_Create() +*/ +void Matrix2D_Delete(Matrix2D** matrix); + +/*! + Sets new dimensions for existing matrix object. + + \param matrix pointer to the matrix object. + \param rows number of rows. + \param columns number of columns. + + \sa Matrix2D_GetDimension() +*/ +void Matrix2D_SetDimensions(Matrix2D* matrix, int rows, int columns); + +/*! + Returns \a dimension (number of rows or columns) of \a matrix. + + \param matrix pointer to the matrix object. + \param dimension may have a value of 1 or 2 to retrieve number of + rows or columns respectively. + + \sa Matrix2D_SetDimensions(), Matrix2D_ColumnCount(), Matrix2D_RowCount() +*/ +int Matrix2D_GetDimension(const Matrix2D* matrix, int dimension); + +/*! + Returns the total number of columns in the \a matrix. + + \sa Matrix2D_SetDimensions(), Matrix2D_RowCount() +*/ +int Matrix2D_ColumnCount(const Matrix2D* matrix); + +/*! + Returns the total number of rows in the \a matrix. + + \sa Matrix2D_SetDimensions(), Matrix2D_RowCount() +*/ +int Matrix2D_RowCount(const Matrix2D* matrix); + +/*! + Returns the value of the element at position (\a row, \a column) in the \a matrix. +*/ +double Matrix2D_Get(const Matrix2D* matrix, int row, int column); + +/*! + Sets a new \a value to the element at position (\a row, \a column) in the \a matrix. +*/ +void Matrix2D_Set(const Matrix2D* matrix, int row, int column, double value); + +/*! + Returns the elements of \a column as a pointer to its values in the \a matrix. +*/ +double* Matrix2D_GetColumn(const Matrix2D* matrix, int column); + +/*! + \brief Copies the matrix \a source to the \a destination. + + Both matrices must be initialized and have the same size. + + \returns true if successful; otherwise returns false. +*/ +bool Matrix2D_Copy(const Matrix2D* source, Matrix2D* destination); + +#ifdef QT_GUI_LIB +/*! + Writes the contents of the \a array with \a size elements using qDebug(). +*/ +void Matrix2D_DebugArray(const double* array, int size); + +/*! + Writes the contents of the \a matrix using qDebug(). +*/ +void Matrix2D_Debug(const Matrix2D* matrix); + +/*! + \brief Writes the contents of the \a matrix into text \a stream. + + The values can be written in the CSV format. +*/ +void Matrix2D_Save(QTextStream* stream, Matrix2D* matrix, bool csv = false); + +/*! + Writes the contents of the \a matrix into binary \a stream. +*/ +void Matrix2D_Save(QDataStream* stream, Matrix2D* matrix); + +/*! + Creates a new \ref Matrix2D object by loading its contents from a binary \a stream. +*/ +void Matrix2D_Load(QDataStream* stream, Matrix2D** matrix); +#endif // QT_GUI_LIB + +} // namespace legacy + +} // namespace robodk + + +#endif // ROBODK_LEGACY_MATRIX2D_H diff --git a/robodk_interface/matrix4x4.cpp b/robodk_interface/matrix4x4.cpp new file mode 100644 index 0000000..01bd809 --- /dev/null +++ b/robodk_interface/matrix4x4.cpp @@ -0,0 +1,615 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#include "matrix4x4.h" + +#include + +#include "vector3.h" +#include "constants.h" + + +namespace robodk +{ + +Matrix4x4::Matrix4x4() + : QMatrix4x4() + , _valid(true) +{ + setToIdentity(); +} + +Matrix4x4::Matrix4x4(bool valid) + : QMatrix4x4() + , _valid(valid ? 1.0 : 0.0) +{ + setToIdentity(); +} + +Matrix4x4::Matrix4x4(const Matrix4x4 &matrix) + : QMatrix4x4(matrix) + , _valid(matrix._valid) +{ +} + +Matrix4x4::Matrix4x4(const double* values) + : QMatrix4x4( + values[0], values[4], values[8], values[12], + values[1], values[5], values[9], values[13], + values[2], values[6], values[10], values[14], + values[3], values[7], values[11], values[15]) + , _valid(1.0) +{ +} + +Matrix4x4::Matrix4x4(const float* values) + : QMatrix4x4( + values[0], values[4], values[8], values[12], + values[1], values[5], values[9], values[13], + values[2], values[6], values[10], values[14], + values[3], values[7], values[11], values[15]) + , _valid(1.0) +{ +} + +Matrix4x4::Matrix4x4(double x, double y, double z) + : QMatrix4x4( + 1.0f, 0.0f, 0.0f, x, + 0.0f, 1.0f, 0.0f, y, + 0.0f, 0.0f, 1.0f, z, + 0.0f, 0.0f, 0.0f, 1.0f) + , _valid(1.0) +{ + setToIdentity(); + (*this)(0, 3) = x; + (*this)(1, 3) = y; + (*this)(2, 3) = z; +} + + +Matrix4x4::Matrix4x4( + double nx, + double ox, + double ax, + double tx, + double ny, + double oy, + double ay, + double ty, + double nz, + double oz, + double az, + double tz) + : QMatrix4x4( + nx, ox, ax, tx, + ny, oy, ay, ty, + nz, oz, az, tz, + 0.0f, 0.0f, 0.0f, 1.0f) + , _valid(1.0) +{ +} + +void Matrix4x4::SetVX(const Vector3& n) +{ + (*this)(0, 0) = n.X(); + (*this)(1, 0) = n.Y(); + (*this)(2, 0) = n.Z(); +} + +void Matrix4x4::SetVY(const Vector3& o) +{ + (*this)(0, 1) = o.X(); + (*this)(1, 1) = o.Y(); + (*this)(2, 1) = o.Z(); +} + +void Matrix4x4::SetVZ(const Vector3& a) +{ + (*this)(0, 2) = a.X(); + (*this)(1, 2) = a.Y(); + (*this)(2, 2) = a.Z(); +} + +void Matrix4x4::SetVX(double x, double y, double z) +{ + Set(0,0, x); + Set(1,0, y); + Set(2,0, z); +} + +void Matrix4x4::SetVY(double x, double y, double z) +{ + Set(0,1, x); + Set(1,1, y); + Set(2,1, z); +} + +void Matrix4x4::SetVZ(double x, double y, double z) +{ + Set(0,2, x); + Set(1,2, y); + Set(2,2, z); +} + +void Matrix4x4::SetPos(double x, double y, double z) +{ + Set(0, 3, x); + Set(1, 3, y); + Set(2, 3, z); +} + +void Matrix4x4::SetVX(const double* xyz) +{ + Set(0, 0, xyz[0]); + Set(1, 0, xyz[1]); + Set(2, 0, xyz[2]); +} + +void Matrix4x4::SetVY(const double* xyz) +{ + Set(0, 1, xyz[0]); + Set(1, 1, xyz[1]); + Set(2, 1, xyz[2]); +} + +void Matrix4x4::SetVZ(const double* xyz) +{ + Set(0, 2, xyz[0]); + Set(1, 2, xyz[1]); + Set(2, 2, xyz[2]); +} + +void Matrix4x4::SetPos(const double* xyz) +{ + Set(0, 3, xyz[0]); + Set(1, 3, xyz[1]); + Set(2, 3, xyz[2]); +} + +Vector3 Matrix4x4::VX() const +{ + return Vector3( + Get(0, 0), + Get(1, 0), + Get(2, 0)); +} + +Vector3 Matrix4x4::VY() const +{ + return Vector3( + Get(0, 1), + Get(1, 1), + Get(2, 1)); +} + +Vector3 Matrix4x4::VZ() const +{ + return Vector3( + Get(0, 2), + Get(1, 2), + Get(2, 2)); +} + +void Matrix4x4::VX(double* xyz) const +{ + xyz[0] = Get(0, 0); + xyz[1] = Get(1, 0); + xyz[2] = Get(2, 0); +} + +void Matrix4x4::VY(double* xyz) const +{ + xyz[0] = Get(0, 1); + xyz[1] = Get(1, 1); + xyz[2] = Get(2, 1); +} + +void Matrix4x4::VZ(double* xyz) const +{ + xyz[0] = Get(0, 2); + xyz[1] = Get(1, 2); + xyz[2] = Get(2, 2); +} + +void Matrix4x4::Pos(double* xyz) const +{ + xyz[0] = Get(0, 3); + xyz[1] = Get(1, 3); + xyz[2] = Get(2, 3); +} + +void Matrix4x4::SetValues(const double* values) +{ + Set(0, 0, values[0]); + Set(1, 0, values[1]); + Set(2, 0, values[2]); + Set(3, 0, values[3]); + + Set(0, 1, values[4]); + Set(1, 1, values[5]); + Set(2, 1, values[6]); + Set(3, 1, values[7]); + + Set(0, 2, values[8]); + Set(1, 2, values[9]); + Set(2, 2, values[10]); + Set(3, 2, values[11]); + + Set(0, 3, values[12]); + Set(1, 3, values[13]); + Set(2, 3, values[14]); + Set(3, 3, values[15]); +} + +void Matrix4x4::Set(int row, int column, double value) +{ + (*this)(row, column) = value; +} + +double Matrix4x4::Get(int row, int column) const +{ + return (*this)(row, column); +} + +Matrix4x4 Matrix4x4::Inverted(bool* invertible) const +{ + return this->inverted(invertible); +} + +bool Matrix4x4::IsHomogeneous() const +{ + auto tx = VX(); + auto ty = VY(); + auto tz = VZ(); + + const double tol = 1e-7; + + if (std::fabs(Vector3::DotProduct(tx, ty)) > tol) + { + return false; + } + else if (std::fabs(Vector3::DotProduct(tx, tz)) > tol) + { + return false; + } + else if (std::fabs(Vector3::DotProduct(ty, tz)) > tol) + { + return false; + } + else if (std::fabs(tx.Length() - 1.0) > tol) + { + return false; + } + else if (std::fabs(ty.Length() - 1.0) > tol) + { + return false; + } + else if (std::fabs(tz.Length() - 1.0) > tol) + { + return false; + } + + return true; +} + +bool Matrix4x4::MakeHomogeneous() +{ + auto tx = VX(); + auto ty = VY(); + auto tz = VZ(); + + bool result = IsHomogeneous(); + + tx.Normalize(); + tz = Vector3::CrossProduct(tx, ty); + tz.Normalize(); + ty = Vector3::CrossProduct(tz, tx); + ty.Normalize(); + + SetVX(tx); + SetVY(ty); + SetVZ(tz); + + (*this)(3, 0) = 0.0f; + (*this)(3, 1) = 0.0f; + (*this)(3, 2) = 0.0f; + (*this)(3, 3) = 1.0f; + + return !result; +} + +void Matrix4x4::ToXYZRPW(double* xyzwpr) const +{ + double x = Get(0, 3); + double y = Get(1, 3); + double z = Get(2, 3); + double w, p, r; + + if (Get(2,0) > (1.0 - 1e-6)) + { + p = -constants::pi * 0.5; + r = 0; + w = atan2(-Get(1,2), Get(1,1)); + } + else if (Get(2,0) < -1.0 + 1e-6) + { + p = 0.5 * constants::pi; + r = 0; + w = atan2(Get(1,2),Get(1,1)); + } + else + { + p = atan2(-Get(2, 0), sqrt(Get(0, 0) * Get(0, 0) + Get(1, 0) * Get(1, 0))); + w = atan2(Get(1, 0), Get(0, 0)); + r = atan2(Get(2, 1), Get(2, 2)); + } + + xyzwpr[0] = x; + xyzwpr[1] = y; + xyzwpr[2] = z; + xyzwpr[3] = r * 180.0 / constants::pi; + xyzwpr[4] = p * 180.0 / constants::pi; + xyzwpr[5] = w * 180.0 / constants::pi; +} + +Matrix4x4 Matrix4x4::XYZRPW_2_Mat(double x, double y, double z, double r, double p, double w) +{ + double a = r * constants::pi / 180.0; + double b = p * constants::pi / 180.0; + double c = w * constants::pi / 180.0; + double ca = cos(a); + double sa = sin(a); + double cb = cos(b); + double sb = sin(b); + double cc = cos(c); + double sc = sin(c); + return Matrix4x4(cb * cc, cc * sa * sb - ca * sc, sa * sc + ca * cc * sb, x, + cb * sc, ca * cc + sa * sb * sc, ca * sb * sc - cc * sa, y, + -sb, cb * sa, ca * cb, z); +} + +Matrix4x4 Matrix4x4::XYZRPW_2_Mat(const double* xyzwpr) +{ + return XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]); +} + +void Matrix4x4::FromXYZRPW(const double* xyzwpr) +{ + Matrix4x4 newmat = Matrix4x4::XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], + xyzwpr[3], xyzwpr[4], xyzwpr[5]); + + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + this->Set(i, j, newmat.Get(i, j)); + } + } +} + +const double* Matrix4x4::ValuesD() const +{ + for(int i = 0; i < 16; ++i) + { + _md[i] = constData()[i]; + } + return _md; +} + +const float* Matrix4x4::ValuesF() const +{ + return constData(); +} + +#ifdef ROBODK_API_FLOATS +const float* Matrix4x4::Values() const +{ + return constData(); +} +#else +const double* Matrix4x4::Values() const +{ + return ValuesD(); +} +#endif + +void Matrix4x4::Values(double* data) const +{ + for(int i = 0; i < 16; ++i) + { + data[i] = constData()[i]; + } +} + +void Matrix4x4::Values(float* data) const +{ + for(int i = 0; i < 16; ++i) + { + data[i] = constData()[i]; + } +} + +bool Matrix4x4::Valid() const +{ + return _valid; +} + +Matrix4x4& Matrix4x4::operator=(const Matrix4x4& matrix) +{ + QMatrix4x4::operator=(matrix); + _valid = 1.0; + return *this; +} + +Matrix4x4 Matrix4x4::transl(double x, double y, double z) +{ + Matrix4x4 mat; + mat.setToIdentity(); + mat.SetPos(x, y, z); + return mat; +} + +Matrix4x4 Matrix4x4::rotx(double rx) +{ + double cx = cos(rx); + double sx = sin(rx); + return Matrix4x4(1, 0, 0, 0, 0, cx, -sx, 0, 0, sx, cx, 0); +} + +Matrix4x4 Matrix4x4::roty(double ry) +{ + double cy = cos(ry); + double sy = sin(ry); + return Matrix4x4(cy, 0, sy, 0, 0, 1, 0, 0, -sy, 0, cy, 0); +} + +Matrix4x4 Matrix4x4::rotz(double rz) +{ + double cz = cos(rz); + double sz = sin(rz); + return Matrix4x4(cz, -sz, 0, 0, sz, cz, 0, 0, 0, 0, 1, 0); +} + +#ifdef QT_GUI_LIB +Matrix4x4::Matrix4x4(const QMatrix4x4 &matrix) + : QMatrix4x4(matrix) + , _valid(1.0) +{ +} + +bool Matrix4x4::FromString(const QString &pose_str) +{ + QString pose_str2 = pose_str.trimmed(); + + const Qt::CaseSensitivity cs = Qt::CaseInsensitive; + if (pose_str2.startsWith("Mat(", cs)) + { + pose_str2.remove(0, 4); + pose_str2 = pose_str2.trimmed(); + } + + if (pose_str2.startsWith("XYZRPW_2_Mat(", cs)) + { + pose_str2.remove(0, 13); + pose_str2 = pose_str2.trimmed(); + } + + while (pose_str2.endsWith(')')) + { + pose_str2.chop(1); + } + + const QLatin1Char separator(','); + pose_str2.replace(QLatin1Char(';'), separator); + pose_str2.replace(QLatin1Char('\t'), separator); + +#if (QT_VERSION < QT_VERSION_CHECK(5, 14, 0)) + const QString::SplitBehavior behavior = QString::SkipEmptyParts; +#else + const Qt::SplitBehavior behavior = Qt::SkipEmptyParts; +#endif + + QStringList values_list = pose_str2.split(separator, behavior); + double xyzwpr[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + + if (values_list.length() < 6) + { + FromXYZRPW(xyzwpr); + return false; + } + + for (int i = 0; i < 6; i++) + { + xyzwpr[i] = values_list[i].trimmed().toDouble(); + } + + FromXYZRPW(xyzwpr); + return true; +} + +QString Matrix4x4::ToString(const QString &separator, int precision, bool xyzrpwOnly) const +{ + if (!Valid()) + { + return "Mat(Invalid)"; + } + + QString str; + if (!IsHomogeneous()) + { + str.append("Warning!! Pose is not homogeneous! Use Matrix4x4::MakeHomogeneous() to make this matrix homogeneous\n"); + } + + str.append("Mat(XYZRPW_2_Mat("); + + double xyzwpr[6]; + ToXYZRPW(xyzwpr); + + for (int i = 0; i < 6; i++) + { + if (i > 0) + { + str.append(separator); + } + str.append(QString::number(xyzwpr[i], 'f', precision)); + } + str.append("))"); + + if (xyzrpwOnly) + { + return str; + } + + str.append("\n"); + + for (int i = 0; i < 4; i++) + { + str.append("["); + for (int j = 0; j < 4; j++) + { + str.append(QString::number(row(i)[j], 'f', precision)); + if (j < 3) + { + str.append(separator); + } + } + str.append("];\n"); + } + return str; +} + +Matrix4x4& Matrix4x4::operator=(const QMatrix4x4& matrix) +{ + QMatrix4x4::operator=(matrix); + _valid = 1.0; + return *this; +} +#endif // QT_GUI_LIB + +} // namespace robodk diff --git a/robodk_interface/matrix4x4.h b/robodk_interface/matrix4x4.h new file mode 100644 index 0000000..a9da6cc --- /dev/null +++ b/robodk_interface/matrix4x4.h @@ -0,0 +1,526 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#ifndef ROBODK_MATRIX4X4_H +#define ROBODK_MATRIX4X4_H + + +#include "deprecated.h" +#include "vector3.h" + + +#ifdef QT_GUI_LIB +#include +#include + +namespace robodk +{ +typedef ::QMatrix4x4 BaseMatrix4x4; +} +#else // QT_GUI_LIB +#error "This class cannot yet be used without the Qt Framework" +#endif // QT_GUI_LIB + + +namespace robodk +{ + +/*! + \class Matrix4x4 + \brief The Matrix4x4 class represents a 4x4 transformation matrix in 3D space. + + The Matrix4x4 class in general is treated as a row-major matrix, in that the + constructors and operator() functions take data in row-major format, as is + familiar in C-style usage. + + Internally the data is stored as column-major format. + + When using these functions be aware that they return data in \b column-major format: + - Values() + - ValuesD() + - ValuesF() +*/ +class Matrix4x4 : public BaseMatrix4x4 +{ +public: + /*! + Constructs an identity matrix. + */ + Matrix4x4(); + + /*! + Constructs a valid or an invalid identity matrix. + */ + explicit Matrix4x4(bool valid); + + /*! + Constructs this Matrix4x4 object as a copy of \a matrix. + */ + Matrix4x4(const Matrix4x4& matrix); + + /*! + \brief Constructs a matrix from the given 16 double-precision \a values. + + The contents of the array \a values is assumed to be in \b column-major order. + + \param values + \f$ \begin{bmatrix} + v_1 & v_2 & v_3 & v_4 & v_5 & v_6 & v_7 & v_8 & + v_9 & v_{10} & v_{11} & v_{12} & v_{13} & v_{14} & v_{15} & v_{16} + \end{bmatrix} \f$ + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + v_1 & v_5 & v_9 & v_{13} \\ + v_2 & v_6 & v_{10} & v_{14} \\ + v_3 & v_7 & v_{11} & v_{15} \\ + v_4 & v_8 & v_{12} & v_{16} + \end{bmatrix} \f$ + */ + Matrix4x4(const double* values); + + /*! + \brief Constructs a matrix from the given 16 single-precision \a values. + + The contents of the array \a values is assumed to be in \b column-major order. + + \param values + \f$ \begin{bmatrix} + v_1 & v_2 & v_3 & v_4 & v_5 & v_6 & v_7 & v_8 & + v_9 & v_{10} & v_{11} & v_{12} & v_{13} & v_{14} & v_{15} & v_{16} + \end{bmatrix} \f$ + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + v_1 & v_5 & v_9 & v_{13} \\ + v_2 & v_6 & v_{10} & v_{14} \\ + v_3 & v_7 & v_{11} & v_{15} \\ + v_4 & v_8 & v_{12} & v_{16} + \end{bmatrix} \f$ + */ + Matrix4x4(const float* values); + + /*! + \brief Constructs a matrix that translates coordinates by the components + \a x, \a y, and \a z. + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + 1 & 0 & 0 & x \\ + 0 & 1 & 0 & y \\ + 0 & 0 & 1 & z \\ + 0 & 0 & 0 & 1 + \end{bmatrix} \f$ + */ + Matrix4x4(double x, double y, double z); + + /*! + \brief Constructs a matrix from the \a N, \a O, \a A, \a T vector components. + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + n_x & o_x & a_x & t_x \\ + n_y & o_y & a_y & t_y \\ + n_z & o_z & a_z & t_z \\ + 0 & 0 & 0 & 1 + \end{bmatrix} \f$ + */ + Matrix4x4( + double nx, + double ox, + double ax, + double tx, + double ny, + double oy, + double ay, + double ty, + double nz, + double oz, + double az, + double tz); + + ~Matrix4x4() = default; + + /*! + Sets the X vector (N vector). + */ + void SetVX(const Vector3& n); + + /*! + Sets the Y vector (O vector). + */ + void SetVY(const Vector3& o); + + /*! + Sets the Z vector (A vector). + */ + void SetVZ(const Vector3& a); + + /*! + Sets the X vector values (N vector). + */ + void SetVX(double x, double y, double z); + + /*! + Sets the Y vector values (O vector). + */ + void SetVY(double x, double y, double z); + + /*! + Sets the Z vector values (A vector). + */ + void SetVZ(double x, double y, double z); + + /*! + Sets the position (T). + */ + void SetPos(double x, double y, double z); + + /*! + Sets the X vector values (N vector). + */ + void SetVX(const double* xyz); + + /*! + Sets the Y vector values (O vector). + */ + void SetVY(const double* xyz); + + /*! + Sets the Z vector values (A vector). + */ + void SetVZ(const double* xyz); + + /*! + Sets the position (T). + */ + void SetPos(const double* xyz); + + /*! + \brief Sets the elements of matrix from the given 16 double-precision \a values. + + The contents of the array \a values is assumed to be in \b column-major order. + + \f$ \begin{bmatrix} + v_1 & v_5 & v_9 & v_{13} \\ + v_2 & v_6 & v_{10} & v_{14} \\ + v_3 & v_7 & v_{11} & v_{15} \\ + v_4 & v_8 & v_{12} & v_{16} + \end{bmatrix} \f$ + + \param values + \f$ \begin{bmatrix} + v_1 & v_2 & v_3 & v_4 & v_5 & v_6 & v_7 & v_8 & + v_9 & v_{10} & v_{11} & v_{12} & v_{13} & v_{14} & v_{15} & v_{16} + \end{bmatrix} \f$ + */ + void SetValues(const double* values); + + /*! + Returns the X vector (N vector). + */ + Vector3 VX() const; + + /*! + Returns the Y vector (O vector). + */ + Vector3 VY() const; + + /*! + Returns the Z vector (A vector). + */ + Vector3 VZ() const; + + /*! + Writes the X vector (N vector) into array of 3 double-precision \a xyz values. + */ + void VX(double* xyz) const; + + /*! + Writes the Y vector (O vector) into array of 3 double-precision \a xyz values. + */ + void VY(double* xyz) const; + + /*! + Writes the Z vector (A vector) into array of 3 double-precision \a xyz values. + */ + void VZ(double* xyz) const; + + /*! + Writes the position (T vector) into array of 3 double-precision \a xyz values. + */ + void Pos(double* xyz) const; + + /*! + Sets a new \a value to the element at position (\a row, \a column) in this matrix. + */ + void Set(int row, int column, double value); + + /*! + Returns the value of the element at position (\a row, \a column) in this matrix. + */ + double Get(int row, int column) const; + + /*! + Returns the inverse of this matrix. Returns the identity if + this matrix cannot be inverted; i.e. determinant() is zero. + */ + Matrix4x4 Inverted(bool* invertible = nullptr) const; + + /*! + Returns \c true if the matrix is homogeneous; false otherwise. + */ + bool IsHomogeneous() const; + + /*! + \brief Forces this matrix to be homogeneous. + The \a vx, \a vy, \a vz must be unitary vectors and respect: vx x vy = vz. + Returns \c true if the matrix was not homogeneous and it was modified to be homogeneous. + */ + bool MakeHomogeneous(); + + /*! + \brief Calculates the position and euler angles ([x,y,z,r,p,w] vector) from this matrix. + + Note: transl(x, y, z) * rotz(w * pi / 180) * roty(p * pi / 180) * rotx(r * pi / 180) + + \returns The XYZWPR translation and rotation in the length units and degrees. + \sa FromXYZRPW() + */ + void ToXYZRPW(double* xyzwpr) const; + + /*! + \brief Calculates this matrix from the position and euler angles ([x,y,z,r,p,w] vector). + + The result is the same as: + H = transl(x, y, z) * rotz(w * pi / 180) * roty(p * pi / 180) * rotx(r * pi / 180) + + \sa ToXYZRPW() + */ + void FromXYZRPW(const double* xyzwpr); + + + /*! + \brief Constructs a matrix from the position and euler angles ([x,y,z,r,p,w] vector). + + The result is the same as: + H = transl(x, y, z) * rotz(w * pi / 180) * roty(p * pi / 180) * rotx(r * pi / 180) + + \returns New homogeneous Matrix4x4 object. + + \sa FromXYZRPW() + */ + static Matrix4x4 XYZRPW_2_Mat(double x, double y, double z, double r, double p, double w); + + /*! + \brief Constructs a matrix from the position and euler angles ([x,y,z,r,p,w] vector). + + The result is the same as: + H = transl(x, y, z) * rotz(w * pi / 180) * roty(p * pi / 180) * rotx(r * pi / 180) + + \returns New homogeneous Matrix4x4 object. + + \sa FromXYZRPW() + */ + static Matrix4x4 XYZRPW_2_Mat(const double* xyzwpr); + + /*! + Returns a constant pointer to the raw data of this matrix as 16 double-precision numbers. + This raw data is stored in column-major format. + + \sa Values(), ValuesF() + */ + const double* ValuesD() const; + + /*! + Returns a constant pointer to the raw data of this matrix as 16 single-precision numbers. + This raw data is stored in column-major format. + + \sa Values(), ValuesD() + */ + const float* ValuesF() const; + +#ifdef ROBODK_API_FLOATS + /*! + Returns a constant pointer to the raw data of this matrix as + 16 double- or single-precision numbers. + This raw data is stored in column-major format. + + \sa ValuesF(), ValuesD() + */ + const float* Values() const; +#else + /*! + Returns a constant pointer to the raw data of this matrix as + 16 double- or single-precision numbers. + This raw data is stored in column-major format. + + \sa ValuesF(), ValuesD() + */ + const double* Values() const; +#endif + + /*! + Writes the contents of this matrix into array of 16 double-precision \a values. + + \sa Values(), ValuesF(), ValuesD() + */ + void Values(double* values) const; + + /*! + Writes the contents of this matrix into array of 16 single-precision \a values. + + \sa Values(), ValuesF(), ValuesD() + */ + void Values(float* values) const; + + /*! + Returns \c true if the matrix is marked as valid; false otherwise. + */ + bool Valid() const; + + /*! + Sets this Matrix4x4 object as a copy of \a matrix. + */ + Matrix4x4& operator=(const Matrix4x4& matrix); + + /*! + \brief Constructs a matrix that translates coordinates by the components + \a x, \a y, and \a z. + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + 1 & 0 & 0 & x \\ + 0 & 1 & 0 & y \\ + 0 & 0 & 1 & z \\ + 0 & 0 & 0 & 1 + \end{bmatrix} \f$ + */ + static Matrix4x4 transl(double x, double y, double z); + + /*! + \brief Constructs a matrix that rotates coordinates around X axis. + \param rx rotation angle in radians. + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + 1 & 0 & 0 & 0 \\ + 0 & \cos rx & - \sin rx & 0 \\ + 0 & \sin rx & \cos rx & 0 \\ + 0 & 0 & 0 & 1 + \end{bmatrix} \f$ + */ + static Matrix4x4 rotx(double rx); + + /*! + \brief Constructs a matrix that rotates coordinates around Y axis. + \param ry rotation angle in radians. + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + \cos ry & 0 & \sin ry & 0 \\ + 0 & 1 & 0 & 0 \\ + - \sin ry & 0 & \cos ry & 0 \\ + 0 & 0 & 0 & 1 + \end{bmatrix} \f$ + */ + static Matrix4x4 roty(double ry); + + /*! + \brief Constructs a matrix that rotates coordinates around Z axis. + \param rz rotation angle in radians. + + \returns New Matrix4x4 object with the following structure: + \f$ \begin{bmatrix} + \cos rz & - \sin rz & 0 & 0 \\ + \sin rz & \cos rx & 0 & 0 \\ + 0 & 0 & 1 & 0 \\ + 0 & 0 & 0 & 1 + \end{bmatrix} \f$ + */ + static Matrix4x4 rotz(double rz); + +#ifdef QT_GUI_LIB + /*! + Constructs this Matrix4x4 object as a copy of \a matrix. + */ + Matrix4x4(const BaseMatrix4x4& matrix); + + /*! + Returns string representation of this matrix. + \sa ToString() + */ + inline operator QString() const { return ToString(); } + + /// Set the matrix given a XYZRPW string array (6-values) + bool FromString(const QString& str); + + + /*! + Returns string representation of this matrix with each element separated by + the given \a separator (which can be an empty string). + \param separator string that separates elements of the matrix. + \param precision represents the number of digits after the decimal point. + \param xyzrpwOnly if set to \s true, the pose will be represented as + XYZWPR 6-dimensional array using ToXYZRPW(). + */ + QString ToString( + const ::QString& separator = QLatin1String(", "), + int precision = 3, + bool xyzrpwOnly = false) const; + + /*! + Sets this Matrix4x4 object as a copy of \a matrix. + */ + Matrix4x4& operator=(const QMatrix4x4& matrix); +#endif + + /* Deprecated methods */ +public: + ROBODK_DEPRECATED("Use Inverted() instead") + inline Matrix4x4 inv() { return Inverted(); } + + ROBODK_DEPRECATED("Use SetPos() instead") + inline void setPos(double x, double y, double z) { SetPos(x, y, z); } + + + +private: + /*! \cond */ + + double _valid; + mutable double _md[16]; + + /*! \endcond */ +}; + +} // namespace robodk + + +#endif // ROBODK_MATRIX4X4_H diff --git a/robodk_interface/robodk_interface.pri b/robodk_interface/robodk_interface.pri new file mode 100644 index 0000000..5d3f09c --- /dev/null +++ b/robodk_interface/robodk_interface.pri @@ -0,0 +1,25 @@ +INCLUDEPATH += $$PWD + +HEADERS += \ + $$PWD/constants.h \ + $$PWD/deprecated.h \ + $$PWD/iapprobodk.h \ + $$PWD/iitem.h \ + $$PWD/irobodk.h \ + $$PWD/joints.h \ + $$PWD/legacymatrix2d.h \ + $$PWD/matrix4x4.h \ + $$PWD/robodktools.h \ + $$PWD/robodktypes.h \ + $$PWD/robodk_interface.h \ + $$PWD/stationtreeeventmonitor.h \ + $$PWD/vector3.h + +SOURCES += \ + $$PWD/joints.cpp \ + $$PWD/legacymatrix2d.cpp \ + $$PWD/matrix4x4.cpp \ + $$PWD/robodktools.cpp \ + $$PWD/robodktypes.cpp \ + $$PWD/stationtreeeventmonitor.cpp \ + $$PWD/vector3.cpp diff --git a/robodk_interface/robodktypes.cpp b/robodk_interface/robodktypes.cpp index 14b6e22..e1647f2 100644 --- a/robodk_interface/robodktypes.cpp +++ b/robodk_interface/robodktypes.cpp @@ -1,1055 +1 @@ #include "robodktypes.h" - -#include -#include - - -//----------------------------------- tJoints class ------------------- -tJoints::tJoints(int ndofs) -{ - _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); - for (int i=0; i < _nDOFs; i++) - { - _Values[i] = 0.0; - } -} - -tJoints::tJoints(const tJoints ©) -{ - SetValues(copy._Values, copy._nDOFs); -} - -tJoints::tJoints(const double *joints, int ndofs) -{ - SetValues(joints, ndofs); -} - -tJoints::tJoints(const float *joints, int ndofs) -{ - int ndofs_ok = qMin(ndofs, RDK_SIZE_JOINTS_MAX); - double jnts[RDK_SIZE_JOINTS_MAX]; - - for (int i=0; i < ndofs_ok; i++) - { - jnts[i] = joints[i]; - } - SetValues(jnts, ndofs_ok); -} - -tJoints::tJoints(const tMatrix2D *mat2d, int column, int ndofs) -{ - if (column >= Matrix2D_Size(mat2d, 2)) - { - _nDOFs = 0; - qDebug() << "Warning: tMatrix2D column outside range when creating joints"; - } - if (ndofs < 0) - { - ndofs = Matrix2D_Size(mat2d, 1); - } - _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); - - double *ptr = Matrix2D_Get_col(mat2d, column); - SetValues(ptr, _nDOFs); -} - -tJoints::tJoints(const QString &str) -{ - _nDOFs = 0; - FromString(str); -} - -const double* tJoints::ValuesD() const -{ - return _Values; -} - -const float* tJoints::ValuesF() const -{ - for (int i = 0; i < RDK_SIZE_JOINTS_MAX; i++) - { - _ValuesF[i] = _Values[i]; - } - - return _ValuesF; -} - -#ifdef ROBODK_API_FLOATS -const float* tJoints::Values() const -{ - return ValuesF(); -} -#else -const double* tJoints::Values() const -{ - return _Values; -} -#endif - -double tJoints::Compare(const tJoints &other) const -{ - double sum_diff = 0.0; - for (int i = 0; i = 0) - { - _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); - } - - for (int i = 0; i < _nDOFs; i++) - { - _Values[i] = values[i]; - } -} - -void tJoints::SetValues(const float *values, int ndofs) -{ - if (ndofs >= 0) - { - _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX); - } - - for (int i = 0; i < _nDOFs; i++) - { - _Values[i] = values[i]; - } -} - -int tJoints::GetValues(double *values) -{ - for (int i = 0; i < _nDOFs; i++) - { - values[i] = _Values[i]; - } - - return _nDOFs; -} - -QString tJoints::ToString(const QString &separator, int precision) const -{ - QString values; - for (int i = 0; i < _nDOFs; i++) - { - if (i > 0) - { - values.append(separator); - } - values.append(QString::number(_Values[i], 'f', precision)); - } - return values; -} - -bool tJoints::FromString(const QString &str) -{ - const QLatin1Char separator(','); - - QString s = str; - s.replace(QLatin1Char(';'), separator); - s.replace(QLatin1Char('\t'), separator); - -#if (QT_VERSION < QT_VERSION_CHECK(5, 14, 0)) - const QString::SplitBehavior behavior = QString::SkipEmptyParts; -#else - const Qt::SplitBehavior behavior = Qt::SkipEmptyParts; -#endif - - QStringList jointList = s.split(separator, behavior); - _nDOFs = qMin(jointList.length(), RDK_SIZE_JOINTS_MAX); - for (int i = 0; i < _nDOFs; i++) - { - _Values[i] = jointList[i].trimmed().toDouble(); - } - return true; -} - -int tJoints::Length() const -{ - return _nDOFs; -} - -void tJoints::setLength(int new_length) -{ - if (new_length >= 0 && new_length < _nDOFs) - { - _nDOFs = new_length; - } -} - -bool tJoints::Valid() -{ - return _nDOFs > 0; -} -//--------------------------------------------------------------------- - - -//----------------------------------- Mat class ----------------------- -Mat transl(double x, double y, double z) -{ - return Mat::transl(x,y,z); -} - -Mat rotx(double rx) -{ - return Mat::rotx(rx); -} - -Mat roty(double ry) -{ - return Mat::roty(ry); -} - -Mat rotz(double rz) -{ - return Mat::rotz(rz); -} - -Mat::Mat() - : QMatrix4x4() - , _valid(true) -{ - setToIdentity(); -} - -Mat::Mat(bool valid) - : QMatrix4x4() - , _valid(valid) -{ - setToIdentity(); -} - -Mat::Mat(const QMatrix4x4 &matrix) - : QMatrix4x4(matrix) - , _valid(true) -{ -} - -Mat::Mat(const Mat &matrix) - : QMatrix4x4(matrix) - , _valid(matrix._valid) -{ -} - -Mat::Mat(double nx, double ox, double ax, double tx, - double ny, double oy, double ay, double ty, - double nz, double oz, double az, double tz) - : QMatrix4x4(nx, ox, ax, tx, ny, oy, ay, ty, nz, oz, az, tz, 0.0, 0.0, 0.0, 1.0) - , _valid(true) -{ -} - -Mat::Mat(const double v[16]) - : QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], - v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15]) - , _valid(true) -{ -} - -Mat::Mat(const float v[16]) - : QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], - v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15]) - , _valid(true) -{ -} - -Mat::Mat(double x, double y, double z) - : QMatrix4x4(1.0, 0.0, 0.0, x, 0.0, 1.0, 0.0, y, 0.0, 0.0, 1.0, z, 0.0, 0.0, 0.0, 1.0) - , _valid(true) -{ -} - -Mat::~Mat() -{ -} - -void Mat::VX(tXYZ xyz) const -{ - xyz[0] = Get(0, 0); - xyz[1] = Get(1, 0); - xyz[2] = Get(2, 0); -} - -void Mat::VY(tXYZ xyz) const -{ - xyz[0] = Get(0, 1); - xyz[1] = Get(1, 1); - xyz[2] = Get(2, 1); -} - -void Mat::VZ(tXYZ xyz) const -{ - xyz[0] = Get(0, 2); - xyz[1] = Get(1, 2); - xyz[2] = Get(2, 2); -} - -void Mat::Pos(tXYZ xyz) const -{ - xyz[0] = Get(0, 3); - xyz[1] = Get(1, 3); - xyz[2] = Get(2, 3); -} - -void Mat::setVX(double x, double y, double z) -{ - Set(0,0, x); - Set(1,0, y); - Set(2,0, z); -} - -void Mat::setVY(double x, double y, double z) -{ - Set(0,1, x); - Set(1,1, y); - Set(2,1, z); -} - -void Mat::setVZ(double x, double y, double z) -{ - Set(0,2, x); - Set(1,2, y); - Set(2,2, z); -} - -void Mat::setPos(double x, double y, double z) -{ - Set(0,3, x); - Set(1,3, y); - Set(2,3, z); -} - -void Mat::setVX(double xyz[3]) -{ - Set(0,0, xyz[0]); - Set(1,0, xyz[1]); - Set(2,0, xyz[2]); -} - -void Mat::setVY(double xyz[3]) -{ - Set(0,1, xyz[0]); - Set(1,1, xyz[1]); - Set(2,1, xyz[2]); -} - -void Mat::setVZ(double xyz[3]) -{ - Set(0,2, xyz[0]); - Set(1,2, xyz[1]); - Set(2,2, xyz[2]); -} - -void Mat::setPos(double xyz[3]) -{ - Set(0,3, xyz[0]); - Set(1,3, xyz[1]); - Set(2,3, xyz[2]); -} - -void Mat::setValues(double pose[16]) -{ - Set(0,0, pose[0]); - Set(1,0, pose[1]); - Set(2,0, pose[2]); - Set(3,0, pose[3]); - - Set(0,1, pose[4]); - Set(1,1, pose[5]); - Set(2,1, pose[6]); - Set(3,1, pose[7]); - - Set(0,2, pose[8]); - Set(1,2, pose[9]); - Set(2,2, pose[10]); - Set(3,2, pose[11]); - - Set(0,3, pose[12]); - Set(1,3, pose[13]); - Set(2,3, pose[14]); - Set(3,3, pose[15]); -} - -void Mat::Set(int i, int j, double value) -{ - QVector4D rw(this->row(i)); - rw[j] = value; - setRow(i, rw); -} - -double Mat::Get(int i, int j) const -{ - return row(i)[j]; -} - -Mat Mat::inv() const -{ - return this->inverted(); -} - -bool Mat::isHomogeneous() const -{ - const bool debug_info = false; - tXYZ vx, vy, vz; - const double tol = 1e-7; - VX(vx); - VY(vy); - VZ(vz); - - if (fabs((double) DOT(vx,vy)) > tol) - { - if (debug_info) - { - qDebug() << "Vector X and Y are not perpendicular!"; - } - return false; - } - else if (fabs((double) DOT(vx,vz)) > tol) - { - if (debug_info) - { - qDebug() << "Vector X and Z are not perpendicular!"; - } - return false; - } - else if (fabs((double) DOT(vy,vz)) > tol) - { - if (debug_info) - { - qDebug() << "Vector Y and Z are not perpendicular!"; - } - return false; - } - else if (fabs((double) (NORM(vx)-1.0)) > tol) - { - if (debug_info) - { - qDebug() << "Vector X is not unitary! " << NORM(vx); - } - return false; - } - else if (fabs((double) (NORM(vy)-1.0)) > tol) - { - if (debug_info) - { - qDebug() << "Vector Y is not unitary! " << NORM(vy); - } - return false; - } - else if (fabs((double) (NORM(vz)-1.0)) > tol) - { - if (debug_info) - { - qDebug() << "Vector Z is not unitary! " << NORM(vz); - } - return false; - } - return true; -} - -bool Mat::MakeHomogeneous() -{ - tXYZ vx, vy, vz; - VX(vx); - VY(vy); - VZ(vz); - bool is_homogeneous = isHomogeneous(); - - NORMALIZE(vx); - CROSS(vz, vx, vy); - NORMALIZE(vz); - CROSS(vy, vz, vx); - NORMALIZE(vy); - setVX(vx); - setVY(vy); - setVZ(vz); - Set(3,0, 0.0); - Set(3,1, 0.0); - Set(3,2, 0.0); - Set(3,3, 1.0); - return !is_homogeneous; -} - -void Mat::ToXYZRPW(tXYZWPR xyzwpr) const -{ - double x = Get(0,3); - double y = Get(1,3); - double z = Get(2,3); - double w, p, r; - - if (Get(2,0) > (1.0 - 1e-6)) - { - p = -M_PI*0.5; - r = 0; - w = atan2(-Get(1,2), Get(1,1)); - } - else if (Get(2,0) < -1.0 + 1e-6) - { - p = 0.5*M_PI; - r = 0; - w = atan2(Get(1,2),Get(1,1)); - } - else - { - p = atan2(-Get(2, 0), sqrt(Get(0, 0) * Get(0, 0) + Get(1, 0) * Get(1, 0))); - w = atan2(Get(1, 0), Get(0, 0)); - r = atan2(Get(2, 1), Get(2, 2)); - } - - xyzwpr[0] = x; - xyzwpr[1] = y; - xyzwpr[2] = z; - xyzwpr[3] = r*180.0/M_PI; - xyzwpr[4] = p*180.0/M_PI; - xyzwpr[5] = w*180.0/M_PI; -} - -QString Mat::ToString(const QString &separator, int precision, bool xyzwpr_only) const -{ - if (!Valid()) - { - return "Mat(Invalid)"; - } - - QString str; - if (!isHomogeneous()) - { - str.append("Warning!! Pose is not homogeneous! Use Mat::MakeHomogeneous() to make this matrix homogeneous\n"); - } - - str.append("Mat(XYZRPW_2_Mat("); - - tXYZWPR xyzwpr; - ToXYZRPW(xyzwpr); - - for (int i = 0; i < 6; i++) - { - if (i > 0) - { - str.append(separator); - } - str.append(QString::number(xyzwpr[i], 'f', precision)); - } - str.append("))"); - - if (xyzwpr_only) - { - return str; - } - - str.append("\n"); - - for (int i = 0; i < 4; i++) - { - str.append("["); - for (int j = 0; j < 4; j++) - { - str.append(QString::number(row(i)[j], 'f', precision)); - if (j < 3) - { - str.append(separator); - } - } - str.append("];\n"); - } - return str; -} - -bool Mat::FromString(const QString &pose_str) -{ - QString pose_str2 = pose_str.trimmed(); - - const Qt::CaseSensitivity cs = Qt::CaseInsensitive; - if (pose_str2.startsWith("Mat(", cs)) - { - pose_str2.remove(0, 4); - pose_str2 = pose_str2.trimmed(); - } - - if (pose_str2.startsWith("XYZRPW_2_Mat(", cs)) - { - pose_str2.remove(0, 13); - pose_str2 = pose_str2.trimmed(); - } - - while (pose_str2.endsWith(')')) - { - pose_str2.chop(1); - } - - const QLatin1Char separator(','); - pose_str2.replace(QLatin1Char(';'), separator); - pose_str2.replace(QLatin1Char('\t'), separator); - -#if (QT_VERSION < QT_VERSION_CHECK(5, 14, 0)) - const QString::SplitBehavior behavior = QString::SkipEmptyParts; -#else - const Qt::SplitBehavior behavior = Qt::SkipEmptyParts; -#endif - - QStringList values_list = pose_str2.split(separator, behavior); - tXYZWPR xyzwpr = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - - if (values_list.length() < 6) - { - FromXYZRPW(xyzwpr); - return false; - } - - for (int i = 0; i < 6; i++) - { - xyzwpr[i] = values_list[i].trimmed().toDouble(); - } - - FromXYZRPW(xyzwpr); - return true; -} - -Mat Mat::XYZRPW_2_Mat(double x, double y, double z, double r, double p, double w) -{ - double a = r * M_PI / 180.0; - double b = p * M_PI / 180.0; - double c = w * M_PI / 180.0; - double ca = cos(a); - double sa = sin(a); - double cb = cos(b); - double sb = sin(b); - double cc = cos(c); - double sc = sin(c); - return Mat(cb * cc, cc * sa * sb - ca * sc, sa * sc + ca * cc * sb, x, - cb * sc, ca * cc + sa * sb * sc, ca * sb * sc - cc * sa, y, - -sb, cb * sa, ca * cb, z); -} - -Mat Mat::XYZRPW_2_Mat(tXYZWPR xyzwpr) -{ - return XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]); -} - -void Mat::FromXYZRPW(tXYZWPR xyzwpr) -{ - Mat newmat = Mat::XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], - xyzwpr[3], xyzwpr[4], xyzwpr[5]); - - for (int i = 0; i < 4; i++) - { - for (int j = 0; j < 4; j++) - { - this->Set(i, j, newmat.Get(i, j)); - } - } -} - -const double* Mat::ValuesD() const -{ - for(int i = 0; i < 16; ++i) - { - _ddata16[i] = constData()[i]; - } - return _ddata16; -} - -const float* Mat::ValuesF() const -{ - return constData(); -} - -#ifdef ROBODK_API_FLOATS -const float* Mat::Values() const -{ - return constData(); -} -#else -const double* Mat::Values() const -{ - return ValuesD(); -} -#endif - -void Mat::Values(double data[16]) const -{ - for(int i = 0; i < 16; ++i) - { - data[i] = constData()[i]; - } -} - -void Mat::Values(float data[16]) const -{ - for(int i = 0; i < 16; ++i) - { - data[i] = constData()[i]; - } -} - -bool Mat::Valid() const -{ - return _valid; -} - -Mat Mat::transl(double x, double y, double z) -{ - Mat mat; - mat.setToIdentity(); - mat.setPos(x, y, z); - return mat; -} - -Mat Mat::rotx(double rx) -{ - double cx = cos(rx); - double sx = sin(rx); - return Mat(1, 0, 0, 0, 0, cx, -sx, 0, 0, sx, cx, 0); -} - -Mat Mat::roty(double ry) -{ - double cy = cos(ry); - double sy = sin(ry); - return Mat(cy, 0, sy, 0, 0, 1, 0, 0, -sy, 0, cy, 0); -} - -Mat Mat::rotz(double rz) -{ - double cz = cos(rz); - double sz = sin(rz); - return Mat(cz, -sz, 0, 0, sz, cz, 0, 0, 0, 0, 1, 0); -} -//--------------------------------------------------------------------- - - -//----------------------------------- 2D matrix functions ------------- -void emxInit_real_T(tMatrix2D **pEmxArray, int numDimensions) -{ - tMatrix2D *emxArray; - int i; - *pEmxArray = (tMatrix2D *)malloc(sizeof(tMatrix2D)); - emxArray = *pEmxArray; - emxArray->data = (double *)NULL; - emxArray->numDimensions = numDimensions; - emxArray->size = (int *)malloc((unsigned int)(sizeof(int) * numDimensions)); - emxArray->allocatedSize = 0; - emxArray->canFreeData = true; - for (i = 0; i < numDimensions; i++) - { - emxArray->size[i] = 0; - } -} - -tMatrix2D* Matrix2D_Create() -{ - tMatrix2D *matrix; - emxInit_real_T((tMatrix2D**)(&matrix), 2); - return matrix; -} - -void emxFree_real_T(tMatrix2D **pEmxArray) -{ - if (*pEmxArray != (tMatrix2D *)NULL) - { - if (((*pEmxArray)->data != (double *)NULL) && (*pEmxArray)->canFreeData) - { - free((void *)(*pEmxArray)->data); - } - free((void *)(*pEmxArray)->size); - free((void *)*pEmxArray); - *pEmxArray = (tMatrix2D *)NULL; - } -} - -void Matrix2D_Delete(tMatrix2D **mat) -{ - emxFree_real_T((tMatrix2D**)(mat)); -} - -void emxEnsureCapacity(tMatrix2D *emxArray, int oldNumel, unsigned int elementSize) -{ - int newNumel; - int i; - double *newData; - if (oldNumel < 0) - { - oldNumel = 0; - } - newNumel = 1; - for (i = 0; i < emxArray->numDimensions; i++) - { - newNumel *= emxArray->size[i]; - } - - if (newNumel > emxArray->allocatedSize) - { - i = emxArray->allocatedSize; - if (i < 16) - { - i = 16; - } - while (i < newNumel) - { - if (i > 1073741823) - { - i =(2147483647);//MAX_int32_T; - } - else - { - i <<= 1; - } - } - - newData = (double*) calloc((unsigned int)i, elementSize); - if (emxArray->data != NULL) - { - memcpy(newData, emxArray->data, elementSize * oldNumel); - if (emxArray->canFreeData) - { - free(emxArray->data); - } - } - emxArray->data = newData; - emxArray->allocatedSize = i; - emxArray->canFreeData = true; - } -} - -void Matrix2D_Set_Size(tMatrix2D *mat, int rows, int cols) -{ - int old_numel; - old_numel = mat->size[0] * mat->size[1]; - mat->size[0] = rows; - mat->size[1] = cols; - emxEnsureCapacity(mat, old_numel, sizeof(double)); -} - -int Matrix2D_Size(const tMatrix2D *var, int dim) -{ - // ONE BASED!! - if (var->numDimensions >= dim) - { - return var->size[dim - 1]; - } - else - { - return 0; - } -} - -int Matrix2D_Get_ncols(const tMatrix2D *var) -{ - return Matrix2D_Size(var, 2); -} - -int Matrix2D_Get_nrows(const tMatrix2D *var) -{ - return Matrix2D_Size(var, 1); -} - -double Matrix2D_Get_ij(const tMatrix2D *var, int i, int j) -{ - // ZERO BASED!! - return var->data[var->size[0] * j + i]; -} - -void Matrix2D_Set_ij(const tMatrix2D *var, int i, int j, double value) -{ - // ZERO BASED!! - var->data[var->size[0] * j + i] = value; -} - -double *Matrix2D_Get_col(const tMatrix2D *var, int col) -{ - // ZERO BASED!! - return (var->data + var->size[0] * col); -} - -bool Matrix2D_Copy(const tMatrix2D *from, tMatrix2D *to) -{ - if (from->numDimensions != 2 || to->numDimensions != 2) - { - Matrix2D_Set_Size(to, 0,0); - return false; - } - - int sz1 = Matrix2D_Size(from,1); - int sz2 = Matrix2D_Size(from,2); - Matrix2D_Set_Size(to, sz1, sz2); - int numel = sz1*sz2; - for (int i = 0; i < numel; i++) - { - to->data[i] = from->data[i]; - } - - return true; -} - - -void Matrix2D_Add(tMatrix2D *var, const double *array, int numel) -{ - int oldnumel; - int size1 = var->size[0]; - int size2 = var->size[1]; - oldnumel = size1*size2; - var->size[1] = size2 + 1; - emxEnsureCapacity(var, oldnumel, (int)sizeof(double)); - numel = qMin(numel, size1); - for (int i=0; idata[size1*size2 + i] = array[i]; - } -} - -void Matrix2D_Add(tMatrix2D *var, const tMatrix2D *varadd) -{ - int oldnumel; - int size1 = var->size[0]; - int size2 = var->size[1]; - int size1_ap = varadd->size[0]; - int size2_ap = varadd->size[1]; - int numel = size1_ap*size2_ap; - if (size1 != size1_ap) - { - return; - } - oldnumel = size1*size2; - var->size[1] = size2 + size2_ap; - emxEnsureCapacity(var, oldnumel, (int)sizeof(double)); - for (int i=0; idata[size1*size2 + i] = varadd->data[i]; - } -} - -void Debug_Array(const double *array, int arraysize) -{ - int i; - QString strout; - for (i = 0; i < arraysize; i++) { - strout.append(QString::number(array[i], 'f', 3)); - if (i < arraysize - 1) { - strout.append(" , "); - } - } - qDebug().noquote() << strout; -} - -void Debug_Matrix2D(const tMatrix2D *emx) -{ - int size1; - int size2; - int j; - double *column; - size1 = Matrix2D_Get_nrows(emx); - size2 = Matrix2D_Get_ncols(emx); - qDebug().noquote() << "Matrix size = " << size1 << " x " << size2; - if (size1*size2 == 0) - { - return; - } - for (j = 0; jnumDimensions; - int size_values = 1; - for (i = 0; i < emx->numDimensions; i++) - { - qint32 sizei = emx->size[i]; - size_values = size_values * sizei; - *st << sizei; - } - for (i = 0; i < size_values; i++) - { - *st << emx->data[i]; - } -} - -void Matrix2D_Save(QTextStream *st, tMatrix2D *emx, bool csv) -{ - int size1; - int size2; - int j; - double *column; - size1 = Matrix2D_Get_nrows(emx); - size2 = Matrix2D_Get_ncols(emx); - //*st << "% Matrix size = " << size1 << " x " << size2; - if (size1 * size2 == 0) - { - return; - } - - if (csv) - { - for (j = 0; jatEnd()) - { - qDebug() << "No data to read"; - return; - } - - if (*emx != nullptr) - { - Matrix2D_Delete(emx); - } - - int i; - qint32 ndim; - qint32 sizei; - *st >> ndim; - qDebug() << "Loading matrix of dimensions: " << ndim; - emxInit_real_T(emx, ndim); - int size_values = 1; - for (i = 0; i < ndim; i++) - { - *st >> sizei; - size_values = size_values * sizei; - (*emx)->size[i] = sizei; - } - //emxEnsureCapacity((emxArray__common *) *emx, 0, (int32_T)sizeof(real_T)); - emxEnsureCapacity(*emx, 0, sizeof(double)); - double value; - for (i = 0; i < size_values; i++) - { - *st >> value; - (*emx)->data[i] = value; - } -} diff --git a/robodk_interface/robodktypes.h b/robodk_interface/robodktypes.h index 3776f53..187e41c 100644 --- a/robodk_interface/robodktypes.h +++ b/robodk_interface/robodktypes.h @@ -6,6 +6,10 @@ #include #include +#include "matrix4x4.h" +#include "legacymatrix2d.h" +#include "joints.h" + #ifndef M_PI #define M_PI 3.14159265358979323846264338327950288 @@ -133,514 +137,204 @@ struct tColor{ - - - -//------------------------------------------------------------------------------------------------------------ - - - -/// \brief The tMatrix2D struct represents a variable size 2d Matrix. Use the Matrix2D_... functions to oeprate on this variable sized matrix. -/// This type of data can be used to get/set a program as a list. This is also useful for backwards compatibility functions related to RoKiSim. -struct tMatrix2D { - /// Pointer to the data - double *data; - - /// Pointer to the size array. - int *size; - - /// Allocated size. - int allocatedSize; - - /// Number of dimensions (usually 2) - int numDimensions; - - bool canFreeData; -}; - - - - -// ------------------------------------------- - - -/// @brief Creates a new 2D matrix \ref Matrix2D.. Use \ref Matrix2D_Delete to delete the matrix (to free the memory). -/// The Procedure \ref Debug_Matrix2D shows an example to read data from a tMatrix2D -tMatrix2D* Matrix2D_Create(); - -/// @brief Deletes a \ref tMatrix2D. -/// @param[in] mat: Pointer of the pointer to the matrix -void Matrix2D_Delete(tMatrix2D **mat); - -/// @brief Sets the size of a \ref tMatrix2D. -/// @param[in/out] mat: Pointer to the matrix -/// @param[in] rows: The number of rows. -/// @param[in] cols: The number of columns. -void Matrix2D_Set_Size(tMatrix2D *mat, int rows, int cols); - -/// @brief Sets the size of a \ref tMatrix2D. -/// @param[in/out] mat: Pointer to the matrix -/// @param[in] dim: Dimension (1 or 2) -int Matrix2D_Size(const tMatrix2D *mat, int dim); - -/// @brief Returns the number of columns of a \ref tMatrix2D. -/// @param[in] mat: Pointer to the matrix -/// Returns the number of columns (Second dimension) -int Matrix2D_Get_ncols(const tMatrix2D *var); - -/// @brief Returns the number of rows of a \ref tMatrix2D. -/// @param[in] mat: Pointer to the matrix -/// Returns the number of rows (First dimension) -int Matrix2D_Get_nrows(const tMatrix2D *var); - -/// @brief Returns the value at location [i,j] of a \ref tMatrix2D. -/// @param[in] mat: Pointer to the matrix -/// Returns the value of the cell -double Matrix2D_Get_ij(const tMatrix2D *var, int i, int j); - -/// @brief Set the value at location [i,j] of a \ref tMatrix2D. -/// @param[in] mat: Pointer to the matrix -/// @param[in] i: Row -/// @param[in] j: Column -/// @param[in] value: matrix value -void Matrix2D_Set_ij(const tMatrix2D *var, int i, int j, double value); - -/// @brief Returns the pointer of a column of a \ref tMatrix2D. -/// A column has \ref Matrix2D_Get_nrows(mat) values that can be accessed/modified from the returned pointer continuously. -/// @param[in] mat: Pointer to the matrix -/// @param[in] col: Column to retreive. -/// /return double array (internal pointer) to the column -double* Matrix2D_Get_col(const tMatrix2D *var, int col); - -/// @brief Copy a Matrix2D -bool Matrix2D_Copy(const tMatrix2D *in, tMatrix2D *out); - -/// @brief Show an array through STDOUT -/// Given an array of doubles, it generates a string -void Debug_Array(const double *array, int arraysize); - -/// @brief Display the content of a \ref tMatrix2D through STDOUT. This is only intended for debug purposes. -/// @param[in] mat: Pointer to the matrix -void Debug_Matrix2D(const tMatrix2D *mat); - -/// @brief Save a matrix as binary data -void Matrix2D_Save(QDataStream *st, tMatrix2D *emx); - -/// @brief Save a matrix as text -void Matrix2D_Save(QTextStream *st, tMatrix2D *emx, bool csv=false); - -/// @brief Load a matrix -void Matrix2D_Load(QDataStream *st, tMatrix2D **emx); - - -//--------------------- Joints class ----------------------- - -/// The tJoints class represents a joint position of a robot (robot axes). -class tJoints { - -public: - /// \brief tJoints - /// \param ndofs number of robot joint axes or degrees of freedom - tJoints(int ndofs = 0); - - /// \brief Set joint values given a double array and the number of joint values - /// \param joints Pointer to the joint values - /// \param ndofs Number of joints - tJoints(const double *joints, int ndofs = 0); - - /// \brief Set joint values given a float array and the number of joint values - /// \param joints Pointer to the joint values - /// \param ndofs Number of joints - tJoints(const float *joints, int ndofs = 0); - - /// \brief Create a copy of an object - /// \param jnts - tJoints(const tJoints &jnts); - - /// \brief Create joint values given a 2D matrix and the column selecting the desired values - /// \param mat2d - /// \param column - /// \param ndofs - tJoints(const tMatrix2D *mat2d, int column=0, int ndofs=-1); - - /// \brief Convert a string to joint values - /// \param str Comma separated joint values (spaces or tabs are also accepted) - tJoints(const QString &str); - - /// To String operator (use with qDebug() << tJoints; - operator QString() const { return ToString(); } - - /// \brief Joint values - /// \return Returns a pointer to the joint data array (doubles) - const double *ValuesD() const; - - /// \brief Joint values - /// \return Returns a pointer to the joint data array (floats) - const float *ValuesF() const; - -#ifdef ROBODK_API_FLOATS - /// \brief Joint values - /// \return Returns a pointer to the joint data array (doubles or floats if ROBODK_API_FLOATS is defined) - const float *Values() const; -#else - /// \brief Joint values - /// \return Returns a pointer to the joint data array (doubles or floats if ROBODK_API_FLOATS is defined) - const double *Values() const; -#endif - - - - double Compare(const tJoints &other) const; - - /// \brief - /// \return Data same as Values. The only difference is that the array pointer is not const. This is provided for backwards compatibility. - double *Data(); - - /// \brief Number of joint axes of the robot (or degrees of freedom) - /// \return - int Length() const; - - /// Set the length of the array (only shrinking the array is allowed) - void setLength(int new_length); - - /// \brief Check if the joints are valid. For example, when we request the Inverse kinematics and there is no solution the joints will not be valid. - /// (for example, an invalid result after calling class: IItem::SolveIK returns a non valid joints) - /// \return true if it has 1 degree of freedom or more - bool Valid(); - - /// \brief GetValues - /// \param joints joint values in deg or mm - /// \return returns the number of degrees of freedom - int GetValues(double *joints); - - /// \brief Set the joint values in deg or mm. You can also important provide the number of degrees of freedom (6 for a 6 axis robot). - /// \param joints joint values in deg or mm - /// \param ndofs number of degrees of freedom (number of axes or joints) - void SetValues(const double *joints, int ndofs = -1); - - /// \brief Set the joint values in deg or mm (floats). You can also important provide the number of degrees of freedom (6 for a 6 axis robot). - /// \param joints joint values in deg or mm - /// \param ndofs number of degrees of freedom (number of axes or joints) - void SetValues(const float *joints, int ndofs = -1); - - /// \brief Retrieve a string representation of the joint values. - /// \param separator String to add between consecutive joint values - /// \param precision Number of decimals - /// \return string as a QString - QString ToString(const QString &separator=", ", int precision = 3) const ; - - /// \brief Set the joint values given a comma-separated string. Tabs and spaces are also allowed. - /// \param str string. Such as "10, 20, 30, 40, 50, 60" - /// \return false if parsing the string failed. True otherwise. - bool FromString(const QString &str); - - -public: - /// number of degrees of freedom - int _nDOFs; - - /// joint values (doubles, used to store the joint values) - double _Values[RDK_SIZE_JOINTS_MAX]; - - /// joint values (floats, used to return a copy as a float pointer) - mutable float _ValuesF[RDK_SIZE_JOINTS_MAX]; -}; - - - - -/// \brief The Mat class represents a 4x4 pose matrix. The main purpose of this object is to represent a pose in the 3D space (position and orientation). -/// In other words, a pose is a 4x4 matrix that represents the position and orientation of one reference frame with respect to another one, in the 3D space. -/// Poses are commonly used in robotics to place objects, reference frames and targets with respect to each other. -///
-/// \f$ transl(x,y,z) rotx(r) roty(p) rotz(w) = \\ -/// \begin{bmatrix} n_x & o_x & a_x & x \\ -/// n_y & o_y & a_y & y \\ -/// n_z & o_z & a_z & z \\ -/// 0 & 0 & 0 & 1 \end{bmatrix} \f$ -class Mat : public QMatrix4x4 { - -public: - - /// Create the identity matrix - Mat(); - - /// Create a valid or an invalid matrix - Mat(bool valid); - - /// Create a copy of the matrix - Mat(const QMatrix4x4 &matrix); - - /// \brief Create a copy of the matrix - /// \param matrix - Mat(const Mat &matrix); - - /// - /// Matrix class constructor for a 4x4 homogeneous matrix given N, O, A & T vectors - /// - /// Matrix[0,0] - /// Matrix[0,1] - /// Matrix[0,2] - /// Matrix[0,3] - /// Matrix[1,0] - /// Matrix[1,1] - /// Matrix[1,2] - /// Matrix[1,3] - /// Matrix[2,0] - /// Matrix[2,1] - /// Matrix[2,2] - /// Matrix[2,3] - /// \f$ \begin{bmatrix} n_x & o_x & a_x & x \\ - /// n_y & o_y & a_y & y \\ - /// n_z & o_z & a_z & z \\ - /// 0 & 0 & 0 & 1 \end{bmatrix} \f$ - /// - Mat(double nx, double ox, double ax, double tx, double ny, double oy, double ay, double ty, double nz, double oz, double az, double tz); - - /// \brief Create a homogeneoux matrix given a one dimensional 16-value array (doubles) - /// \param values [nx,ny,nz,0, ox,oy,oz,0, ax,ay,az,0, tx,ty,tz,1] - /// \f$ \begin{bmatrix} n_x & o_x & a_x & x \\ - /// n_y & o_y & a_y & y \\ - /// n_z & o_z & a_z & z \\ - /// 0 & 0 & 0 & 1 \end{bmatrix} \f$ - /// - Mat(const double values[16]); - - /// \brief Create a homogeneoux matrix given a one dimensional 16-value array (floats) - /// \param values [nx,ny,nz,0, ox,oy,oz,0, ax,ay,az,0, tx,ty,tz,1] - /// \f$ transl(x,y,z) rotx(r) roty(p) rotz(w) = \\ - /// \begin{bmatrix} n_x & o_x & a_x & x \\ - /// n_y & o_y & a_y & y \\ - /// n_z & o_z & a_z & z \\ - /// 0 & 0 & 0 & 1 \end{bmatrix} \f$ - /// - Mat(const float values[16]); - - /// - /// Create a translation matrix. - /// - /// translation along X (mm) - /// translation along Y (mm) - /// translation along Z (mm) - /// - /// \f$ rotx(\theta) = \begin{bmatrix} 1 & 0 & 0 & x \\ - /// 0 & 1 & 0 & y \\ - /// 0 & 0 & 1 & z \\ - /// 0 & 0 & 0 & 1 \\ - /// \end{bmatrix} \f$ - /// - Mat(double x, double y, double z); - - ~Mat(); - - /// To String operator (use with qDebug() << tJoints; - operator QString() const { return ToString(); } - - /// Set the X vector values (N vector) - void setVX(double x, double y, double z); - - /// Set the Y vector values (O vector) - void setVY(double x, double y, double z); - - /// Set the Z vector values (A vector) - void setVZ(double x, double y, double z); - - /// Set the position (T position) in mm - void setPos(double x, double y, double z); - - /// Set the X vector values (N vector) - void setVX(double xyz[3]); - - /// Set the Y vector values (O vector) - void setVY(double xyz[3]); - - /// Set the Z vector values (A vector) - void setVZ(double xyz[3]); - - /// Set the position (T position) in mm - void setPos(double xyz[3]); - - /// Set the pose values - void setValues(double pose[16]); - - /// Get the X vector (N vector) - void VX(tXYZ xyz) const; - - /// Get the Y vector (O vector) - void VY(tXYZ xyz) const; - - /// Get the Z vector (A vector) - void VZ(tXYZ xyz) const; - - /// Get the position (T position), in mm - void Pos(tXYZ xyz) const; - - /// \brief Set a matrix value - /// \param r row - /// \param c column - /// \param value value - void Set(int r, int c, double value); - - /// \brief Get a matrix value - /// \param r row - /// \param c column - /// \return value - double Get(int r, int c) const; - - /// Invert the pose (homogeneous matrix assumed) - Mat inv() const; - - /// Returns true if the matrix is homogeneous, otherwise it returns false - bool isHomogeneous() const; - - /// Forces 4x4 matrix to be homogeneous (vx,vy,vz must be unitary vectors and respect: vx x vy = vz). Returns True if the matrix was not homogeneous and it was be modified to make it homogeneous. - bool MakeHomogeneous(); - - //------ Pose to xyzrpw and xyzrpw to pose------------ - - /// - /// Calculates the equivalent position and euler angles ([x,y,z,r,p,w] vector) of the given pose - /// Note: transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180) - /// See also: FromXYZRPW() - /// - /// XYZWPR translation and rotation in mm and degrees - void ToXYZRPW(tXYZWPR xyzwpr) const; - - /// \brief Retrieve a string representation of the pose - /// \param separator String separator - /// \param precision Number of decimals - /// \param in_xyzwpr if set to true (default), the pose will be represented as XYZWPR 6-dimensional array using ToXYZRPW - /// \return - QString ToString(const QString &separator=", ", int precision = 3, bool xyzrpw_only = false) const; - - /// Set the matrix given a XYZRPW string array (6-values) - bool FromString(const QString &str); - - /// - /// Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector) - /// The result is the same as: - ///
- /// H = transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180) - /// - /// Homogeneous matrix (4x4) - void FromXYZRPW(tXYZWPR xyzwpr); - - /// - /// Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector) - /// The result is the same as: - ///
- /// H = transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180) - /// - /// Homogeneous matrix (4x4) - static Mat XYZRPW_2_Mat(double x, double y, double z, double r, double p, double w); - static Mat XYZRPW_2_Mat(tXYZWPR xyzwpr); - - /// Get a pointer to the 16-digit double array. - const double* ValuesD() const; - - /// Get a pointer to the 16-digit array as an array of floats. - const float* ValuesF() const; - -#ifdef ROBODK_API_FLOATS - /// Get a pointer to the 16-digit array (doubles or floats if ROBODK_API_FLOATS is defined). - const float* Values() const; -#else - /// Get a pointer to the 16-digit array (doubles or floats if ROBODK_API_FLOATS is defined). - const double* Values() const; -#endif - - /// Copy the 16-values of the 4x4 matrix to a double array. - void Values(double values[16]) const; - - /// Copy the 16-values of the 4x4 matrix to a double array. - void Values(float values[16]) const; - - /// Check if the matrix is valid - bool Valid() const; - - /// - /// Return a translation matrix. - /// - /// translation along X (mm) - /// translation along Y (mm) - /// translation along Z (mm) - /// - /// \f$ rotx(\theta) = \begin{bmatrix} 1 & 0 & 0 & x \\ - /// 0 & 1 & 0 & y \\ - /// 0 & 0 & 1 & z \\ - /// 0 & 0 & 0 & 1 \\ - /// \end{bmatrix} \f$ - /// - static Mat transl(double x, double y, double z); - - /// - /// Return the X-axis rotation matrix. - /// - /// Rotation around X axis (in radians). - /// - /// \f$ rotx(\theta) = \begin{bmatrix} 1 & 0 & 0 & 0 \\ - /// 0 & c_\theta & -s_\theta & 0 \\ - /// 0 & s_\theta & c_\theta & 0 \\ - /// 0 & 0 & 0 & 1 \\ - /// \end{bmatrix} \f$ - /// - static Mat rotx(double rx); - - /// - /// Return a Y-axis rotation matrix - /// - /// Rotation around Y axis (in radians) - /// - /// \f$ roty(\theta) = \begin{bmatrix} c_\theta & 0 & s_\theta & 0 \\ - /// 0 & 1 & 0 & 0 \\ - /// -s_\theta & 0 & c_\theta & 0 \\ - /// 0 & 0 & 0 & 1 \\ - /// \end{bmatrix} \f$ - /// - static Mat roty(double ry); - - /// - /// Return a Z-axis rotation matrix. - /// - /// Rotation around Z axis (in radians) - /// - /// \f$ rotz(\theta) = \begin{bmatrix} c_\theta & -s_\theta & 0 & 0 \\ - /// s_\theta & c_\theta & 0 & 0 \\ - /// 0 & 0 & 1 & 0 \\ - /// 0 & 0 & 0 & 1 \\ - /// \end{bmatrix} \f$ - /// - static Mat rotz(double rz); - -private: - /// Flags if a matrix is not valid. - double _valid; - -// this is a dummy variable to easily obtain a pointer to a 16-double-array for matrix multiplications -private: - /// Copy of the data as a double array. - mutable double _ddata16[16]; -}; - -/// Translation matrix class: Mat::transl. -Mat transl(double x, double y, double z); - -/// Translation matrix class: Mat::rotx. -Mat rotx(double rx); - -/// Translation matrix class: Mat::roty. -Mat roty(double ry); - -/// Translation matrix class: Mat::rotz. -Mat rotz(double rz); - -//QDataStream &operator<<(QDataStream &data, const QMatrix4x4 &); -inline QDebug operator<<(QDebug dbg, const Mat &m){ return dbg.noquote() << m.ToString(); } -inline QDebug operator<<(QDebug dbg, const tJoints &jnts){ return dbg.noquote() << jnts.ToString(); } - -inline QDebug operator<<(QDebug dbg, const Mat *m){ return dbg.noquote() << (m == nullptr ? "Mat(null)" : m->ToString()); } -inline QDebug operator<<(QDebug dbg, const tJoints *jnts){ return dbg.noquote() << (jnts == nullptr ? "tJoints(null)" : jnts->ToString()); } - +typedef robodk::legacy::Matrix2D tMatrix2D; + +/*! + Creates a new \ref Matrix2D object with no dimensions. + + \returns pointer to the Matrix2D object if no allocation errors occurred; nullptr otherwise. + + \sa Matrix2D_Delete() +*/ +inline tMatrix2D* Matrix2D_Create() +{ + return robodk::legacy::Matrix2D_Create(); +} + +/*! + Deletes an existing \ref Matrix2D object. + + \param matrix pointer of the pointer variable to the matrix object. + + \sa Matrix2D_Create() +*/ +inline void Matrix2D_Delete(tMatrix2D** matrix) +{ + robodk::legacy::Matrix2D_Delete(matrix); +} + +/*! + Sets new dimensions for existing matrix object. + + \param matrix pointer to the matrix object. + \param rows number of rows. + \param columns number of columns. + + \sa Matrix2D_Size() +*/ +inline void Matrix2D_Set_Size(tMatrix2D* matrix, int rows, int columns) +{ + robodk::legacy::Matrix2D_SetDimensions(matrix, rows, columns); +} + +/*! + Returns \a dimension (number of rows or columns) of \a matrix. + + \deprecated Use \ref Matrix2D_ColumnCount() and \ref Matrix2D_RowCount() instead of this one. + + \param matrix pointer to the matrix object. + \param dimension may have a value of 1 or 2 to retrieve number of + rows or columns respectively. + + \sa Matrix2D_Set_Size(), Matrix2D_Get_ncols(), Matrix2D_Get_nrows() +*/ +inline int Matrix2D_Size(const tMatrix2D* matrix, int dimension) +{ + return robodk::legacy::Matrix2D_GetDimension(matrix, dimension); +} + +/*! + Returns the total number of columns in the \a matrix. + + \sa Matrix2D_Set_Size(), Matrix2D_Get_nrows() +*/ +inline int Matrix2D_Get_ncols(const tMatrix2D* matrix) +{ + return robodk::legacy::Matrix2D_ColumnCount(matrix); +} + +/*! + Returns the total number of rows in the \a matrix. + + \sa Matrix2D_Set_Size(), Matrix2D_Get_ncols() +*/ +inline int Matrix2D_Get_nrows(const tMatrix2D* matrix) +{ + return robodk::legacy::Matrix2D_RowCount(matrix); +} + +/*! + Returns the value of the element at position (\a row, \a column) in the \a matrix. +*/ +inline double Matrix2D_Get_ij(const tMatrix2D* matrix, int row, int column) +{ + return robodk::legacy::Matrix2D_Get(matrix, row, column); +} + +/*! + Sets a new \a value to the element at position (\a row, \a column) in the \a matrix. +*/ +inline void Matrix2D_Set_ij(const tMatrix2D* matrix, int row, int column, double value) +{ + robodk::legacy::Matrix2D_Set(matrix, row, column, value); +} + +/*! + Returns the elements of \a column as a pointer to its values in the \a matrix. +*/ +inline double* Matrix2D_Get_col(const tMatrix2D* matrix, int column) +{ + return robodk::legacy::Matrix2D_GetColumn(matrix, column); +} + +/*! + \brief Copies the matrix \a source to the \a destination. + + Both matrices must be initialized and have the same size. + + \returns true if successful; otherwise returns false. +*/ +inline bool Matrix2D_Copy(const tMatrix2D* source, tMatrix2D* destination) +{ + return robodk::legacy::Matrix2D_Copy(source, destination); +} + +/*! + Writes the contents of the \a array with \a size elements using qDebug(). +*/ +inline void Debug_Array(const double* array, int size) +{ + robodk::legacy::Matrix2D_DebugArray(array, size); +} + +/*! + Writes the contents of the \a matrix using qDebug(). +*/ +inline void Debug_Matrix2D(const tMatrix2D* matrix) +{ + robodk::legacy::Matrix2D_Debug(matrix); +} + +/*! + \brief Writes the contents of the \a matrix into text \a stream. + + The values can be written in the CSV format. +*/ +inline void Matrix2D_Save(QTextStream* stream, tMatrix2D* matrix, bool csv = false) +{ + robodk::legacy::Matrix2D_Save(stream, matrix, csv); +} + +/*! + Writes the contents of the \a matrix into binary \a stream. +*/ +inline void Matrix2D_Save(QDataStream* stream, tMatrix2D* matrix) +{ + robodk::legacy::Matrix2D_Save(stream, matrix); +} + +/*! + Creates a new \ref Matrix2D object by loading its contents from a binary \a stream. +*/ +inline void Matrix2D_Load(QDataStream* stream, tMatrix2D** matrix) +{ + robodk::legacy::Matrix2D_Load(stream, matrix); +} + + +typedef robodk::Joints tJoints; +typedef robodk::Matrix4x4 Mat; + +inline Mat transl(double x, double y, double z) +{ + return Mat::transl(x,y,z); +} + +inline Mat rotx(double rx) +{ + return Mat::rotx(rx); +} + +inline Mat roty(double ry) +{ + return Mat::roty(ry); +} + +inline Mat rotz(double rz) +{ + return Mat::rotz(rz); +} + + +inline QDebug operator<<(QDebug dbg, const Mat &m) +{ + return dbg.noquote() << m.ToString(); +} + +inline QDebug operator<<(QDebug dbg, const tJoints &jnts) +{ + return dbg.noquote() << jnts.ToString(); +} + +inline QDebug operator<<(QDebug dbg, const Mat *m) +{ + return dbg.noquote() << (m == nullptr ? "Mat(null)" : m->ToString()); +} + +inline QDebug operator<<(QDebug dbg, const tJoints *jnts) +{ + return dbg.noquote() << (jnts == nullptr ? "tJoints(null)" : jnts->ToString()); +} #endif // ROBODKTYPES_H diff --git a/robodk_interface/stationtreeeventmonitor.cpp b/robodk_interface/stationtreeeventmonitor.cpp new file mode 100644 index 0000000..32f4a3b --- /dev/null +++ b/robodk_interface/stationtreeeventmonitor.cpp @@ -0,0 +1,348 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#include "stationtreeeventmonitor.h" + +#include + +#include +#include +#include +#include + +#include "iitem.h" + + +namespace robodk +{ + +StationTreeEventMonitor::StationTreeEventMonitor(IRoboDK* rdk, QObject* parent) + : QObject(parent) + , _rdk(rdk) +{ + if (!_rdk) + return; + + const auto items = _rdk->getItemList(); + for (const auto& item : items) + { + auto treeItem = dynamic_cast(item); + if (treeItem && treeItem->treeWidget()) + { + _tree = treeItem->treeWidget(); + break; + } + } + + if (!_tree) + return; + + auto model = _tree->model(); + + using ModelClass = QAbstractItemModel; + using ThisClass = StationTreeEventMonitor; + connect(model, &ModelClass::modelReset, this, &ThisClass::refresh); + connect(model, &ModelClass::dataChanged, this, &ThisClass::onModelDataChanged); + connect(model, &ModelClass::rowsInserted, this, &ThisClass::onModelRowsInserted); + connect(model, &ModelClass::rowsRemoved, this, &ThisClass::onModelRowsRemoved); + + refresh(); +} + +void StationTreeEventMonitor::refresh() +{ + _nameTable.clear(); + _nameCache.clear(); + + if (!_tree || !_tree->model()) + return; + + iterateOverTree(QModelIndex(), [this] (const QModelIndex& index) + { + auto item = itemFromIndex(index); + if (!item) + return; + + const QString name = index.data().toString(); + _nameTable.insert({name, item}); + _nameCache[item] = name; + }); +} + +void StationTreeEventMonitor::submit() +{ + bool child = false; + + auto addItem = [this, &child] (const QModelIndex& index) + { + auto item = itemFromIndex(index); + if (!item) + return; + + const QString name = index.data().toString(); + _nameTable.insert({name, item}); + _nameCache[item] = name; + + if (_filter & IgnoreAdd) + return; + + if ((_filter & IgnoreChildren) && child) + return; + + if ((_filter & IgnoreInactiveStations) && !isActiveStationItem(index)) + return; + + emit itemAdded(item); + }; + + for (const auto& index : _addedIndices) + { + child = false; + addItem(index); + + child = true; + iterateOverTree(index, addItem); + } + + _addedIndices.clear(); +} + +void StationTreeEventMonitor::onModelDataChanged( + const QModelIndex& topLeft, + const QModelIndex& bottomRight, + const QVector& roles) +{ + if (!topLeft.isValid() || !bottomRight.isValid() || topLeft.parent() != bottomRight.parent()) + return; + + bool nameChanged = false; + bool iconChanged = false; + + for (int role : roles) + { + switch (role) + { + case Qt::DisplayRole: + case Qt::EditRole: + nameChanged = true; + break; + case Qt::DecorationRole: + iconChanged = true; + break; + default: + break; + } + } + + bool isActive = (_filter & IgnoreInactiveStations) == 0 || isActiveStationItem(topLeft); + const auto parent = topLeft.parent(); + + auto updateItem = [this, nameChanged, iconChanged, isActive, parent] (const QModelIndex& index) + { + auto item = itemFromIndex(index); + if (!item) + return; + + const auto cache = _nameCache.find(item); + if (cache == _nameCache.end()) + return; + + const auto range = _nameTable.equal_range(cache->second); + for (auto it = range.first; it != range.second; ++it) + { + if (it->second == item) + { + _nameTable.erase(it); + break; + } + } + + const QString name = index.data().toString(); + _nameTable.insert({name, item}); + _nameCache[item] = name; + + if (!isActive) + return; + + if ((_filter & IgnoreChildren) && parent != index.parent()) + return; + + if (nameChanged && (_filter & IgnoreNameChange) == 0) + emit itemNameChanged(item, name); + + if (iconChanged && (_filter & IgnoreIconChange) == 0) + emit itemIconChanged(item, qvariant_cast(index.data(Qt::DecorationRole))); + }; + + for (int row = topLeft.row(); row <= bottomRight.row(); ++row) + { + auto index = _tree->model()->index(row, 0, topLeft.parent()); + if (!index.isValid()) + continue; + + updateItem(index); + iterateOverTree(index, updateItem); + } +} + +void StationTreeEventMonitor::onModelRowsInserted(const QModelIndex& parent, int first, int last) +{ + for (int row = first; row <= last; ++row) + { + auto index = _tree->model()->index(row, 0, parent); + if (index.isValid()) + _addedIndices.push_back(index); + } + + if (_policy == AutoSubmit) + QTimer::singleShot(0, this, &StationTreeEventMonitor::submit); +} + +void StationTreeEventMonitor::onModelRowsRemoved(const QModelIndex& parent, int first, int last) +{ + auto removeItem = [this, &parent] (const QModelIndex& index) + { + auto item = itemFromIndex(index); + if (!item) + return; + + const auto cache = _nameCache.find(item); + if (cache == _nameCache.end()) + return; + + const auto range = _nameTable.equal_range(cache->second); + for (auto it = range.first; it != range.second; ++it) + { + if (it->second == item) + { + _nameTable.erase(it); + break; + } + } + + _nameCache.erase(cache); + + if (_filter & IgnoreRemove) + return; + + if ((_filter & IgnoreChildren) && parent != index.parent()) + return; + + if ((_filter & IgnoreInactiveStations) && !isActiveStationItem(index)) + return; + + emit itemRemoved(item); + }; + + for (int row = last; row >= first; --row) + { + auto index = _tree->model()->index(row, 0, parent); + if (!index.isValid()) + continue; + + iterateOverTree(index, removeItem, true); + removeItem(index); + } +} + +void StationTreeEventMonitor::iterateOverTree( + const QModelIndex& parent, + const TreeCallback& callback, + bool reverse) +{ + auto model = _tree->model(); + + using StackEntry = std::pair; + std::stack stack; + stack.push({0, parent}); + + while (!stack.empty()) + { + auto& entry = stack.top(); + const auto& parent = entry.second; + const int start = entry.first; + const int count = model->rowCount(parent); + + bool goBack = true; + + if (reverse && start > 0) + { + auto index = model->index(count - start, 0, parent); + callback(index); + } + + for (int i = start; i < count; ++i) + { + int row = reverse ? (count - i - 1) : i; + + auto index = model->index(row, 0, parent); + if (!index.isValid()) + continue; + + if (!reverse) + callback(index); + + if (model->hasChildren(index)) + { + entry.first = i + 1; + stack.push({0, index}); + goBack = false; + break; + } + else if (reverse) + { + callback(index); + } + } + + if (goBack) + stack.pop(); + } +} + +IItem* StationTreeEventMonitor::itemFromIndex(const QModelIndex& index) const +{ + auto treeItem = static_cast(index.internalPointer()); + return dynamic_cast(treeItem); +} + +bool StationTreeEventMonitor::isActiveStationItem(const QModelIndex& index) const +{ + QModelIndex parent = index; + while (parent.parent().isValid()) + parent = parent.parent(); + + if (!parent.isValid()) + return false; + + auto item = itemFromIndex(parent); + return item && _rdk && item == _rdk->getActiveStation(); +} + +} // namespace robodk diff --git a/robodk_interface/stationtreeeventmonitor.h b/robodk_interface/stationtreeeventmonitor.h new file mode 100644 index 0000000..58243bd --- /dev/null +++ b/robodk_interface/stationtreeeventmonitor.h @@ -0,0 +1,142 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#ifndef ROBODK_STATIONTREEEVENTMONITOR_H +#define ROBODK_STATIONTREEEVENTMONITOR_H + +#include +#include +#include +#include + +#include +#include +#include +#include + + +class QIcon; +class QTreeWidget; +class QTreeWidgetItem; +class IRoboDK; +class IItem; + + +namespace robodk +{ + +class StationTreeEventMonitor : public QObject +{ + Q_OBJECT + +public: + enum Filter : uint32_t + { + NoFilter = 0x00000000, + IgnoreInactiveStations = 0x00000001, + IgnoreNameChange = 0x00000002, + IgnoreIconChange = 0x00000004, + IgnoreAdd = 0x00000008, + IgnoreRemove = 0x00000010, + IgnoreChildren = 0x00000020, + }; + + enum SubmitPolicy + { + AutoSubmit, + ManualSubmit + }; + +public: + explicit StationTreeEventMonitor(IRoboDK* rdk, QObject* parent = nullptr); + + void setFilter(uint32_t filter); + void setSubmitPolicy(SubmitPolicy policy); + +signals: + void itemNameChanged(IItem* item, const QString& name); + void itemIconChanged(IItem* item, const QIcon& icon); + void itemAdded(IItem* item); + void itemRemoved(IItem* item); + +public slots: + void refresh(); + void submit(); + +private slots: + void onModelDataChanged( + const QModelIndex& topLeft, + const QModelIndex& bottomRight, + const QVector& roles = QVector()); + void onModelRowsInserted(const QModelIndex& parent, int first, int last); + void onModelRowsRemoved(const QModelIndex& parent, int first, int last); + +private: + struct QStringHash + { + inline size_t operator()(const QString& s) const { return qHash(s); }; + }; + + using TreeCallback = std::function; + +private: + void iterateOverTree( + const QModelIndex& parent, + const TreeCallback& callback, + bool reverse = false); + IItem* itemFromIndex(const QModelIndex& index) const; + bool isActiveStationItem(const QModelIndex& index) const; + +private: + IRoboDK* _rdk = nullptr; + QTreeWidget* _tree = nullptr; + + uint32_t _filter = IgnoreInactiveStations; + SubmitPolicy _policy = AutoSubmit; + + std::unordered_multimap _nameTable; + std::unordered_map _nameCache; + + std::list _addedIndices; +}; + + +inline void StationTreeEventMonitor::setFilter(uint32_t filter) +{ + _filter = filter; +} + +inline void StationTreeEventMonitor::setSubmitPolicy(SubmitPolicy policy) +{ + _policy = policy; +} + +} // namespace robodk + +#endif // STATIONTREEEVENTMONITOR_H diff --git a/robodk_interface/vector3.cpp b/robodk_interface/vector3.cpp new file mode 100644 index 0000000..3fa2801 --- /dev/null +++ b/robodk_interface/vector3.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#include "vector3.h" + +#include + + +namespace robodk +{ + +Vector3::Vector3(double x, double y, double z) +{ + _v[0] = x; + _v[1] = y; + _v[2] = z; +} + +double Vector3::Length() const +{ + return std::sqrt(_v[0] * _v[0] + _v[1] * _v[1] + _v[2] * _v[2]); +} + +void Vector3::Normalize() +{ + double len = Length(); + if (std::abs(len) <= 0.000000000001) + return; + + _v[0] /= len; + _v[1] /= len; + _v[2] /= len; +} + +double Vector3::DotProduct(const Vector3& v1, const Vector3& v2) +{ + return v1._v[0] * v2._v[0] + v1._v[1] * v2._v[1] + v1._v[2] * v2._v[2]; +} + +Vector3 Vector3::CrossProduct(const Vector3& v1, const Vector3& v2) +{ + return Vector3( + v1._v[1] * v2._v[2] - v1._v[2] * v2._v[1], + v1._v[2] * v2._v[0] - v1._v[0] * v2._v[2], + v1._v[0] * v2._v[1] - v1._v[1] * v2._v[0]); +} + +} // namespace robodk diff --git a/robodk_interface/vector3.h b/robodk_interface/vector3.h new file mode 100644 index 0000000..0ca9ee9 --- /dev/null +++ b/robodk_interface/vector3.h @@ -0,0 +1,145 @@ +/**************************************************************************** +** +** Copyright (c) 2015-2025 RoboDK Inc. +** Contact: https://robodk.com/ +** +** This file is part of the RoboDK API. +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is +** furnished to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. +** +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +** LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +** OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +** SOFTWARE. +** +** RoboDK is a registered trademark of RoboDK Inc. +** +****************************************************************************/ + +#ifndef ROBODK_VECTOR3_H +#define ROBODK_VECTOR3_H + + +#include + + +namespace robodk +{ + +/*! + \class Vector3 + \brief The Vector3 class represents a vector or vertex in 3D space. + + Vectors are one of the main building blocks of 3D representation and + drawing. They consist of three coordinates, traditionally called + x, y, and z. +*/ +class Vector3 +{ +public: + /*! + Constructs a null vector, i.e. with coordinates (0, 0, 0). + */ + Vector3() = default; + + /*! + Constructs a vector with coordinates (\a x, \a y, \a z). + */ + Vector3(double x, double y, double z); + + /*! + Constructs a vector object as a copy of \a v. + */ + Vector3(const Vector3& v) = default; + + /*! + Returns the length of the vector from the origin. + */ + double Length() const; + + /*! + Normalizes the currect vector in place. Nothing happens if this + vector is a null vector or the length of the vector is very close to 1. + */ + void Normalize(); + + /*! + Returns the x coordinate of this vector. + */ + inline double X() const { return _v[0]; } + + /*! + Returns the y coordinate of this vector. + */ + inline double Y() const { return _v[1]; } + + /*! + Returns the z coordinate of this vector. + */ + inline double Z() const { return _v[2]; } + + /*! + Sets the x coordinate of this vector to the given \a x coordinate. + */ + inline void SetX(double x) { _v[0] = x; } + + /*! + Sets the y coordinate of this vector to the given \a y coordinate. + */ + inline void SetY(double y) { _v[0] = y; } + + /*! + Sets the z coordinate of this vector to the given \a z coordinate. + */ + inline void SetZ(double z) { _v[0] = z; } + + /*! + Returns the component of the vector at index position \a i. + */ + inline const double& operator[](size_t i) const { return _v[i]; } + + /*! + Returns the component of the vector at index position \a i + as a modifiable reference. + */ + inline double& operator[](size_t i) { return _v[i]; } + + /*! + Returns the dot product of \a v1 and \a v2. + */ + static double DotProduct(const Vector3& v1, const Vector3& v2); + + /*! + Returns the cross-product of vectors \a v1 and \a v2, which corresponds + to the normal vector of a plane defined by \a v1 and \a v2. + */ + static Vector3 CrossProduct(const Vector3& v1, const Vector3& v2); + + /*! + Sets this Vector3 object as a copy of \a v. + */ + Vector3& operator=(const Vector3& v) = default; + +private: + /*! \cond */ + + double _v[3] = {0.0, 0.0, 0.0}; + + /*! \endcond */ +}; + +} // namespace robodk + + +#endif // ROBODK_VECTOR3_H