From 2146e12a2d07497010be208e7750cda25a0ca6b3 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 22 Aug 2026 00:29:41 +1000 Subject: [PATCH 1/4] tests: remove mathtest.c Nothing builds it. src/tests has no Submakefile and is not in SUBDIRS, so the file has never been part of any target; it only tested that a 2004-era kernel module linking libm came out with no unresolved symbols. The concern it stood for is handled by the -fno-builtin-sin, -cos and -sincos flags at src/Makefile:917 and by the realtime link itself. It also defines its own isnan() over the bytes of a double, assuming little-endian IEEE 754, and math_test() has no caller. --- src/tests/mathtest.c | 145 ------------------------------------------- 1 file changed, 145 deletions(-) delete mode 100644 src/tests/mathtest.c diff --git a/src/tests/mathtest.c b/src/tests/mathtest.c deleted file mode 100644 index 1e33b4544dc..00000000000 --- a/src/tests/mathtest.c +++ /dev/null @@ -1,145 +0,0 @@ -/******************************************************************** -* Description: mathtest.c - A small file to test for unresolved -* symbols in loadable kernel modules. -* -* Author: Paul_C - Derived from a file by Fred Proctor. -* Created at: Mon Feb 16 20:39:42 GMT 2004 -* Computer: Morphix -* System: Linux -* -* Copyright (c) 2004 All rights reserved. -* -* Last change: -********************************************************************/ -/* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation; either version 2 of the License, or -* (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software -* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -*/ - -#define MODULE -#include -#include -#include -#include /* sin(), cos(), isnan() etc. */ -#include /* DBL_MAX */ -#include /* errno, EDOM */ -#include "sincos.h" - -/* - math functions are: - - extern double sin(double); used in posemath, siggen, & noncartesian kins - extern double cos(double); used in posemath, siggen, & noncartesian kins - extern double tan(double); not used in RT - extern double asin(double); not used in RT - extern double acos(double); used in posemath & noncartesian kins - extern double atan2(double, double); used in posemath & noncartesian kins - extern double sinh(double); not used in RT - extern double cosh(double); not used in RT - extern double tanh(double); not used in RT - extern double exp(double); not used in RT - extern double log(double); not used in RT - extern double log10(double); not used in RT - extern double pow(double, double); not used in RT - extern double sqrt(double); used in tc, segmot, & noncartesean kins. - extern double ceil(double); used in segmot & emcpid - extern double floor(double); used by siggen & segmot - extern double fabs(double); used a lot in RT - extern double ldexp(double, int); not used in RT - - extern double sincos(double, double *, double *); Is called at four places in - posemath - None of the resulting - functions are used in EMC. - Extras: - - extern int isnan(double); Not used directly in RT - But is called - by several (all ?) of the floating point - math functions. -*/ - -/* Declare as volatile and gcc *shouldn't* optimize too much... */ -volatile double a = 0; -volatile double b = 0; -volatile double c = 0; -volatile double x; -double d; -int n; - -/* - isnan() - C99 extensions to the math library functions - - IEEE double-precision floating point: - - N = (-1)^S * 2^(E-1023) * 1.F - - where S, E and F are composed of bits as labeled in these 8 bytes: - - [7] [6] [5] [0] - SEEEEEEE EEEEFFFF FFFFFFFF ... FFFFFFFF - - If E = 2047 and F is nonzero, N = Not a Number (NaN) - If E = 2047 and F = 0 and S = 1, N = -Inf - If E = 2047 and F = 0 and S = 0, N = Inf - If 0 < E < 2047, N = (-1)^S * 2^(E-1023) * 1.F - If E = 0 and F is nonzero, N = (-1)^S * 2^(-1022) * 0.F ("unnormalized") - If E = 0 and F = 0 and S = 1, N = -0 - If E = 0 and F = 0 and S = 0, N = 0 - - Example: for N = -1, S = 1, E = 1023, F = 0, and - byte[0] = 10111111 = BF - byte[1] = 11110000 = F0 - byte[2-7] = 0 -*/ - -int isnan(double d) -{ - int e; - unsigned char * c = (unsigned char *) &d; - - e = (int) (c[7] & 0x7F); - e <<= 4; - e += (int) ((c[6] & 0xF0) >> 4); - - return (e == 2047) && (c[0] || c[1] || c[2] || c[3] || - c[4] || c[5] || (c[6] & 0x0F)); -} - -int math_test(void) -{ - double v, u; - a = 1.00039276; - b = 1.9999 * a; - c = a / 2; - - /* force a domain error, EDOM, and check that we got it */ - x = acos(b); - if (isnan(x)) x = 0.0; - - /* force a range error, ERANGE, and check that we got it */ - x = pow(DBL_MAX, b); - if (isnan(x)) x = 0.0; - - /* do some legit math */ - x = sin(a) + cos(a) + tan(a) + - asin(c) + acos(c) + atan2(a, b) + - sinh(a) + cosh(a) + tanh(a) + - exp(a) + log(a) + log10(a) + - pow(a, b) + sqrt(a) + ceil(a) + - floor(a) + fabs(a) + ldexp(a, b); - - /* Test the sincos func */ - pm_sincos(x, &v, &u); - - return 0; -} From 4015e8d36cefb09921f0f3abc8f76f8a12f9a474 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 17 Aug 2026 00:09:45 +1000 Subject: [PATCH 2/4] posemath: split the header by language posemath.h declares two libraries in one file: a set of C++ classes with their operators, and the C API that the real time side uses. The tree already has a shape for that, in inifile.h and inifile.hh and in hal.h and hal.hh, where the plain suffix is the C interface and the double one is the C++ interface, so follow it. posemath.h keeps the C API and gains posemath_types.h, which holds the data types and the constants. posemath.hh takes the classes, the operators and the copy templates, and refuses to be compiled as C. posemath.h ends by including posemath.hh under __cplusplus, so C++ code that includes posemath.h still gets the classes and nothing out of tree changes. The two headers include each other and the guards make either order safe. What this buys is for C++ code: a file that wants no more than PmCartesian, which is most of what reaches posemath through emcpos.h, can include posemath_types.h and stop parsing four hundred lines of class declarations. C compilers were never affected either way, since the classes have always sat behind an ifdef. Declaration text is moved verbatim, indentation included. The one edit is PmAxis, which was an unnamed enum in C and a typedef of the C++ enum PM_AXIS in C++; the types come first now, so the enum takes its tag and one definition serves both languages. Preprocessing posemath.h before and after, in C and in C++, gives the same declarations in both languages, differing only in the order they appear and in that PmAxis line. libposemath.so.0 exports the same 390 symbols. --- src/Makefile | 2 + src/libposemath/Submakefile | 6 + src/libposemath/posemath.h | 633 +------------------------------ src/libposemath/posemath.hh | 433 +++++++++++++++++++++ src/libposemath/posemath_types.h | 221 +++++++++++ 5 files changed, 670 insertions(+), 625 deletions(-) create mode 100644 src/libposemath/posemath.hh create mode 100644 src/libposemath/posemath_types.h diff --git a/src/Makefile b/src/Makefile index a0214c850f6..98d6a8bd7ed 100644 --- a/src/Makefile +++ b/src/Makefile @@ -412,6 +412,8 @@ SRCHEADERS := \ libposemath/gomath.h \ libposemath/gotypes.h \ libposemath/posemath.h \ + libposemath/posemath.hh \ + libposemath/posemath_types.h \ libposemath/sincos.h \ rtapi/rtapi.h \ rtapi/rtapi_app.h \ diff --git a/src/libposemath/Submakefile b/src/libposemath/Submakefile index 2750a846bd7..b95497f589f 100644 --- a/src/libposemath/Submakefile +++ b/src/libposemath/Submakefile @@ -13,7 +13,13 @@ POSEMATHINCS = \ ./libposemath/gomath.h \ ./libposemath/gotypes.h \ ./libposemath/posemath.h \ + ./libposemath/posemath_types.h \ ./libposemath/sincos.h +POSEMATHCXXINCS = \ + ./libposemath/posemath.hh + $(patsubst ./libposemath/%,../include/%,$(POSEMATHINCS)): ../include/%.h: ./libposemath/%.h cp $^ $@ +$(patsubst ./libposemath/%,../include/%,$(POSEMATHCXXINCS)): ../include/%.hh: ./libposemath/%.hh + cp $^ $@ diff --git a/src/libposemath/posemath.h b/src/libposemath/posemath.h index 4c6ecd6f150..51ea880eb59 100644 --- a/src/libposemath/posemath.h +++ b/src/libposemath/posemath.h @@ -71,602 +71,20 @@ #ifndef __LINUXCNC_POSEMATH_H #define __LINUXCNC_POSEMATH_H -#ifdef __cplusplus - -/* forward declarations-- conversion ctors will need these */ - -/* translation types */ -struct PM_CARTESIAN; /* Cart */ -struct PM_SPHERICAL; /* Sph */ -struct PM_CYLINDRICAL; /* Cyl */ - -/* rotation types */ -struct PM_ROTATION_VECTOR; /* Rot */ -struct PM_ROTATION_MATRIX; /* Mat */ -struct PM_QUATERNION; /* Quat */ -struct PM_EULER_ZYZ; /* Zyz */ -struct PM_EULER_ZYX; /* Zyx */ -struct PM_RPY; /* Rpy */ - -/* pose types */ -struct PM_POSE; /* Pose */ -struct PM_HOMOGENEOUS; /* Hom */ - -/* PM_CARTESIAN */ - -struct PM_CARTESIAN { - /* ctors/dtors */ - PM_CARTESIAN() { - }; - PM_CARTESIAN(double _x, double _y, double _z); - - PM_CARTESIAN(const PM_CYLINDRICAL & c); /* conversion */ - PM_CARTESIAN(const PM_SPHERICAL & s); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - PM_CARTESIAN & operator += (const PM_CARTESIAN &o); - PM_CARTESIAN & operator -= (const PM_CARTESIAN &o); - - // Scalar operations - PM_CARTESIAN & operator *= (double o); - PM_CARTESIAN & operator /= (double o); - - /* data */ - double x, y, z; /* this.x, etc. */ -}; - -/* PM_SPHERICAL */ - -struct PM_SPHERICAL { - /* ctors/dtors */ - PM_SPHERICAL() { - }; - PM_SPHERICAL(double _theta, double _phi, double _r); - PM_SPHERICAL(const PM_CYLINDRICAL & v); /* conversion */ - PM_SPHERICAL(const PM_CARTESIAN & v); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - double theta, phi, r; -}; - -/* PM_CYLINDRICAL */ - -struct PM_CYLINDRICAL { - /* ctors/dtors */ - PM_CYLINDRICAL() { - }; - PM_CYLINDRICAL(double _theta, double _r, double _z); - PM_CYLINDRICAL(const PM_CARTESIAN & v); /* conversion */ - PM_CYLINDRICAL(const PM_SPHERICAL & v); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - double theta, r, z; -}; - -/* PM_ROTATION_VECTOR */ - -struct PM_ROTATION_VECTOR { - /* ctors/dtors */ - PM_ROTATION_VECTOR() { - }; - PM_ROTATION_VECTOR(double _r, double _x, double _y, double _z); - PM_ROTATION_VECTOR(const PM_QUATERNION & q); /* conversion - */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - double s, x, y, z; -}; - -/* PM_ROTATION_MATRIX */ - -struct PM_ROTATION_MATRIX { - /* ctors/dtors */ - PM_ROTATION_MATRIX() { - }; - PM_ROTATION_MATRIX(double xx, double xy, double xz, - double yx, double yy, double yz, double zx, double zy, double zz); - PM_ROTATION_MATRIX(const PM_CARTESIAN& _x, const PM_CARTESIAN& _y, const PM_CARTESIAN& _z); - PM_ROTATION_MATRIX(const PM_ROTATION_VECTOR & v); /* conversion - */ - PM_ROTATION_MATRIX(const PM_QUATERNION & q); /* conversion - */ - PM_ROTATION_MATRIX(const PM_EULER_ZYZ & zyz); /* conversion - */ - PM_ROTATION_MATRIX(const PM_EULER_ZYX & zyx); /* conversion - */ - PM_ROTATION_MATRIX(const PM_RPY & rpy); /* conversion */ - - /* operators */ - PM_CARTESIAN & operator[](int n); /* this[n] */ - - /* data */ - PM_CARTESIAN x, y, z; -}; - -/* PM_QUATERNION */ - -enum PM_AXIS { PM_X, PM_Y, PM_Z }; - -struct PM_QUATERNION { - /* ctors/dtors */ - PM_QUATERNION() { - }; - PM_QUATERNION(double _s, double _x, double _y, double _z); - PM_QUATERNION(const PM_ROTATION_VECTOR & v); /* conversion - */ - PM_QUATERNION(const PM_ROTATION_MATRIX & m); /* conversion - */ - PM_QUATERNION(const PM_EULER_ZYZ & zyz); /* conversion */ - PM_QUATERNION(const PM_EULER_ZYX & zyx); /* conversion */ - PM_QUATERNION(const PM_RPY & rpy); /* conversion */ - PM_QUATERNION(PM_AXIS axis, double angle); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* functions */ - void axisAngleMult(PM_AXIS axis, double angle); - - /* data */ - double s, x, y, z; /* this.s, etc. */ -}; - -/* PM_EULER_ZYZ */ - -struct PM_EULER_ZYZ { - /* ctors/dtors */ - PM_EULER_ZYZ() { - }; - PM_EULER_ZYZ(double _z, double _y, double _zp); - PM_EULER_ZYZ(const PM_QUATERNION & q); /* conversion */ - PM_EULER_ZYZ(const PM_ROTATION_MATRIX & m); /* conversion */ - - /* operators */ - double &operator[] (int n); - - /* data */ - double z, y, zp; -}; - -/* PM_EULER_ZYX */ - -struct PM_EULER_ZYX { - /* ctors/dtors */ - PM_EULER_ZYX() { - }; - PM_EULER_ZYX(double _z, double _y, double _x); - PM_EULER_ZYX(const PM_QUATERNION & q); /* conversion */ - PM_EULER_ZYX(const PM_ROTATION_MATRIX & m); /* conversion */ - - /* operators */ - double &operator[] (int n); - - /* data */ - double z, y, x; -}; - -/* PM_RPY */ - -struct PM_RPY { - /* ctors/dtors */ - PM_RPY() { - }; - PM_RPY(double _r, double _p, double _y); - PM_RPY(const PM_QUATERNION & q); /* conversion */ - PM_RPY(const PM_ROTATION_MATRIX & m); /* conversion */ - - /* operators */ - double &operator[] (int n); - - /* data */ - double r, p, y; -}; - -/* PM_POSE */ - -struct PM_POSE { - /* ctors/dtors */ - PM_POSE() { - }; - PM_POSE(const PM_CARTESIAN& v, const PM_QUATERNION& q); - PM_POSE(double x, double y, double z, - double s, double sx, double sy, double sz); - PM_POSE(const PM_HOMOGENEOUS & h); /* conversion */ - - /* operators */ - double &operator[] (int n); /* this[n] */ - - /* data */ - PM_CARTESIAN tran; - PM_QUATERNION rot; -}; - -/* PM_HOMOGENEOUS */ - -struct PM_HOMOGENEOUS { - /* ctors/dtors */ - PM_HOMOGENEOUS() { - }; - PM_HOMOGENEOUS(const PM_CARTESIAN& v, const PM_ROTATION_MATRIX& m); - PM_HOMOGENEOUS(const PM_POSE & p); /* conversion */ - - /* operators */ - PM_CARTESIAN & operator[](int n); /* column vector */ - - /* data ( [ 0 0 0 1 ] element is manually returned by [] if needed ) */ - PM_CARTESIAN tran; - PM_ROTATION_MATRIX rot; -}; - -/* PM_LINE */ - -struct PM_LINE { - /* ctors/dtors */ - PM_LINE() { - }; - - /* functions */ - int init(const PM_POSE& start, const PM_POSE& end); - int point(double len, PM_POSE * point); - - /* data */ - PM_POSE start; /* where motion was started */ - PM_POSE end; /* where motion is going */ - PM_CARTESIAN uVec; /* unit vector from start to end */ -}; - -/* PM_CIRCLE */ - -struct PM_CIRCLE { - /* ctors/dtors */ - PM_CIRCLE() - : radius(0.0), - angle(0.0), - spiral(0.0) - {}; - - /* functions */ - int init(const PM_POSE& start, const PM_POSE& end, - const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn); - int point(double angle, PM_POSE * point); - - /* data */ - PM_CARTESIAN center; - PM_CARTESIAN normal; - PM_CARTESIAN rTan; - PM_CARTESIAN rPerp; - PM_CARTESIAN rHelix; - double radius; - double angle; - double spiral; -}; - -/* overloaded external functions */ - -/* dot */ -extern double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -/* cross */ -extern PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -/* unit */ -extern PM_CARTESIAN unit(const PM_CARTESIAN &v); -extern PM_QUATERNION unit(const PM_QUATERNION &q); -extern PM_ROTATION_VECTOR unit(const PM_ROTATION_VECTOR &r); -extern PM_ROTATION_MATRIX unit(const PM_ROTATION_MATRIX &m); - -/* isNorm */ -extern int isNorm(const PM_CARTESIAN &v); -extern int isNorm(const PM_QUATERNION &q); -extern int isNorm(const PM_ROTATION_VECTOR &r); -extern int isNorm(const PM_ROTATION_MATRIX &m); - -/* mag */ -extern double mag(const PM_CARTESIAN &v); - -/* disp */ -extern double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -/* inv */ -extern PM_CARTESIAN inv(const PM_CARTESIAN &v); -extern PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m); -extern PM_QUATERNION inv(const PM_QUATERNION &q); -extern PM_POSE inv(const PM_POSE &p); -extern PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h); - -/* project */ -extern PM_CARTESIAN proj(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); - -/* overloaded arithmetic functions */ - -/* unary +, - for translation, rotation, pose */ -extern PM_CARTESIAN operator + (const PM_CARTESIAN &v); -extern PM_CARTESIAN operator - (const PM_CARTESIAN &v); -extern PM_QUATERNION operator + (const PM_QUATERNION &q); -extern PM_QUATERNION operator - (const PM_QUATERNION &q); -extern PM_POSE operator + (const PM_POSE &p); -extern PM_POSE operator - (const PM_POSE &p); - -/* compare operators */ -extern int operator == (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); -extern int operator == (const PM_QUATERNION &q1, const PM_QUATERNION &q2); -extern int operator == (const PM_POSE &p1, const PM_POSE &p2); -extern int operator != (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); -extern int operator != (const PM_QUATERNION &q1, const PM_QUATERNION &q2); -extern int operator != (const PM_POSE &p1, const PM_POSE &p2); - -/* translation +, -, scalar *, - */ - -/* v + v */ -extern PM_CARTESIAN operator + (PM_CARTESIAN v1, const PM_CARTESIAN &v2); -/* v - v */ -extern PM_CARTESIAN operator - (PM_CARTESIAN v1, const PM_CARTESIAN &v2); -/* v * s */ -extern PM_CARTESIAN operator *(PM_CARTESIAN v, double s); -/* s * v */ -extern PM_CARTESIAN operator *(double s, PM_CARTESIAN v); -/* v / s */ -extern PM_CARTESIAN operator / (const PM_CARTESIAN &v, double s); - -/* rotation * by scalar, translation, and rotation */ - -/* s * q */ -extern PM_QUATERNION operator *(double s, const PM_QUATERNION &q); -/* q * s */ -extern PM_QUATERNION operator *(const PM_QUATERNION &q, double s); -/* q / s */ -extern PM_QUATERNION operator / (const PM_QUATERNION &q, double s); -/* q * v */ -extern PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v); -/* q * q */ -extern PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2); -/* m * m */ -extern PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, - const PM_ROTATION_MATRIX &m2); - -/* pose operators */ - -/* q * p */ -extern PM_POSE operator *(const PM_QUATERNION &q, const PM_POSE &p); -/* p * p */ -extern PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2); -/* p * v */ -extern PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v); - -#endif /* __cplusplus */ - -/* now comes the C stuff */ +#include "posemath_types.h" #ifdef __cplusplus extern "C" { #endif -/* PmCartesian */ - - typedef struct { - double x, y, z; /* this.x, etc. */ - - } PmCartesian; - -/* PmSpherical */ - - typedef struct { - double theta, phi, r; - - } PmSpherical; - -/* PmCylindrical */ - - typedef struct { - double theta, r, z; - - } PmCylindrical; - -/* PmAxis */ -#ifdef __cplusplus - typedef PM_AXIS PmAxis; -#else - typedef enum { PM_X, PM_Y, PM_Z } PmAxis; -#endif - -/* PmRotationVector */ - - typedef struct { - double s, x, y, z; - - } PmRotationVector; - -/* PmRotationMatrix */ - - typedef struct { - PmCartesian x, y, z; - - } PmRotationMatrix; - -/* PmQuaternion */ - - typedef struct { - double s, x, y, z; /* this.s, etc. */ - - } PmQuaternion; - -/* PmEulerZyz */ - - typedef struct { - double z, y, zp; - - } PmEulerZyz; - -/* PmEulerZyx */ - - typedef struct { - double z, y, x; - - } PmEulerZyx; - -/* PmRpy */ - - typedef struct { - double r, p, y; - - } PmRpy; - -/* PmPose */ - - typedef struct { - PmCartesian tran; - PmQuaternion rot; - - } PmPose; - -/* PmCartLine */ - typedef struct { - PmCartesian start; - PmCartesian end; - PmCartesian uVec; - double tmag; - int tmag_zero; - } PmCartLine; - -/* Homogeneous transform PmHomogeneous */ - - typedef struct { - PmCartesian tran; - PmRotationMatrix rot; - - } PmHomogeneous; - -/* line structure */ - - typedef struct { - PmPose start; /* where motion was started */ - PmPose end; /* where motion is going */ - PmCartesian uVec; /* unit vector from start to end */ - PmQuaternion qVec; /* unit of rotation */ - double tmag; - double rmag; - int tmag_zero; - int rmag_zero; - - } PmLine; - -/* Generalized circle structure */ - - typedef struct { - PmCartesian center; - PmCartesian normal; - PmCartesian rTan; - PmCartesian rPerp; - PmCartesian rHelix; - double radius; - double angle; - double spiral; - - } PmCircle; - -/* some nice constants */ - -#define PM_PI 3.14159265358979323846 -#define PM_PI_2 1.57079632679489661923 -#define PM_PI_4 0.78539816339744830962 -#define PM_2_PI 6.28318530717958647692 - /* quicky macros */ //#define pmClose(a, b, eps) ((fabs((a) - (b)) < (eps)) ? 1 : 0) //#define pmSq(x) ((x)*(x)) -int pmClose(double a, double b, double eps); +int pmClose(double a, double b, double eps); __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; } -#ifdef TO_DEG -#undef TO_DEG -#endif -#define TO_DEG (180./PM_PI) - -#ifdef TO_RAD -#undef TO_RAD -#endif -#define TO_RAD (PM_PI/180.) - -/*! \todo FIXME-- fix these */ - -/* DOUBLE_FUZZ is the smallest double, d, such that (1+d != 1) w/o FPC. - DOUBLECP_FUZZ is the same only with the Floating Point CoProcessor */ - -#define DOUBLE_FUZZ 2.2204460492503131e-16 -#define DOUBLECP_FUZZ 1.0842021724855044e-19 - - -/** - * FIXME sloppily defined constants here. - * These constants are quite large compared to the DOUBLE_FUZZ limitation. They - * seem like an ugly band-aid for floating point problems. - */ - -// FIXME setting this to be an order of magnitude smaller than canon's shortest -// allowed segment. This is still larger than TP's smallest position, so it may -// be silently causing trouble. -// andypugh 5/2/22 This seems to be interpreted to be in config units. -#define CART_FUZZ (1.0e-8) -/* how close a cartesian vector's magnitude must be for it to be considered - a zero vector */ - -#define Q_FUZZ (1.0e-06) -/* how close elements of a Q must be to be equal */ - -#define QS_FUZZ (1.0e-6) -/* how close q.s is to 0 to be 180 deg rotation */ - -#define RS_FUZZ (1.0e-6) -/* how close r.s is for a rotation vector to be considered 0 */ - -#define QSIN_FUZZ (1.0e-6) -/* how close sin(a/2) is to 0 to be zero rotation */ - -#define V_FUZZ (1.0e-8) -/* how close elements of a V must be to be equal */ - -#define SQRT_FUZZ (-1.0e-6) -/* how close to 0 before math_sqrt() is error */ - -#define UNIT_VEC_FUZZ (1.0e-6) -/* how close mag of vec must be to 1.00 */ - -#define UNIT_QUAT_FUZZ (1.0e-6) -/* how close mag of quat must be to 1.00 */ - -#define UNIT_SC_FUZZ (1.0e-6) -/* how close mag of sin, cos must be to 1.00 */ - -#define E_EPSILON (1.0e-6) -/* how close second ZYZ euler angle must be to 0/PI for degeneration */ - -#define SINGULAR_EPSILON (1.0e-6) -/* how close to zero the determinate of a matrix must be for singularity */ - -#define RPY_P_FUZZ (1.0e-6) -/* how close pitch is to zero for RPY to degenerate */ - -#define ZYZ_Y_FUZZ (1.0e-6) -/* how close Y is to zero for ZYZ Euler to degenerate */ - -#define ZYX_Y_FUZZ (1.0e-6) -/* how close Y is to zero for ZYX Euler to degenerate */ - -#define CIRCLE_FUZZ (1.0e-6) -/* Bug fix for the missing circles problem */ - /* debug output printing */ extern void pmPrintError(const char *fmt, ...) __attribute__((format(printf,1,2))); @@ -850,49 +268,14 @@ __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; extern int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point); extern int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end); -/* slicky macros for item-by-item copying between C and C++ structs */ - #ifdef __cplusplus } /* matches extern "C" for C++ */ +#endif -template -void toCart(const A& src, B* dst) {(dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} - -template -void toCyl(const A& src, B* dst) {(dst)->theta = (src).theta; (dst)->r = (src).r; (dst)->z = (src).z;} - -template -void toSph(const A& src, B* dst) {(dst)->theta = (src).theta; (dst)->phi = (src).phi; (dst)->r = (src).r;} - -template -void toQuat(const A& src, B* dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} - -template -void toRot(const A& src, B* dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} - -template -void toMat(const A& src, B* dst) {toCart((src).x, &((dst)->x)); toCart((src).y, &((dst)->y)); toCart((src).z, &((dst)->z));} - -template -void toEulerZyz(const A& src, B* dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->zp = (src).zp;} - -template -void toEulerZyx(const A& src, B* dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->x = (src).x;} - -template -void toRpy(const A& src, B* dst) {(dst)->r = (src).r; (dst)->p = (src).p; (dst)->y = (src).y;} - -template -void toPose(const A& src, B* dst) {toCart((src).tran, &((dst)->tran)); toQuat((src).rot, &((dst)->rot));} - -template -void toHom(const A& src, B* dst) {toCart((src).tran, &((dst)->tran)); toMat((src).rot, &((dst)->rot));} - -template -void toLine(const A& src, B* dst) {toPose((src).start, &((dst)->start)); toPose((src).end, &((dst)->end)); toCart((src).uVec, &((dst)->uVec));} - -template -void toCircle(const A& src, B* dst) {toCart((src).center, &((dst)->center)); toCart((src).normal, &((dst)->normal)); toCart((src).rTan, &((dst)->rTan)); toCart((src).rPerp, &((dst)->rPerp)); toCart((src).rHelix, &((dst)->rHelix)); (dst)->radius = (src).radius; (dst)->angle = (src).angle; (dst)->spiral = (src).spiral;} - +/* The C++ interface lives in posemath.hh. Including it here keeps C++ code + that includes posemath.h working as it did when both were one file. */ +#ifdef __cplusplus +#include "posemath.hh" #endif + #endif /* #ifndef POSEMATH_H */ diff --git a/src/libposemath/posemath.hh b/src/libposemath/posemath.hh new file mode 100644 index 00000000000..e51fadb6a5f --- /dev/null +++ b/src/libposemath/posemath.hh @@ -0,0 +1,433 @@ +/******************************************************************** +* Description: posemath.hh +* The C++ interface of the pose math library: the PM_ classes, the +* operators over them and the templates that copy between the class +* and the C representation of a type. +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: LGPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ + +#ifndef __LINUXCNC_POSEMATH_HH +#define __LINUXCNC_POSEMATH_HH + +#ifndef __cplusplus +#error posemath.hh is the C++ interface; C code wants posemath.h +#endif + +#include "posemath.h" + +/* forward declarations-- conversion ctors will need these */ + +/* translation types */ +struct PM_CARTESIAN; /* Cart */ +struct PM_SPHERICAL; /* Sph */ +struct PM_CYLINDRICAL; /* Cyl */ + +/* rotation types */ +struct PM_ROTATION_VECTOR; /* Rot */ +struct PM_ROTATION_MATRIX; /* Mat */ +struct PM_QUATERNION; /* Quat */ +struct PM_EULER_ZYZ; /* Zyz */ +struct PM_EULER_ZYX; /* Zyx */ +struct PM_RPY; /* Rpy */ + +/* pose types */ +struct PM_POSE; /* Pose */ +struct PM_HOMOGENEOUS; /* Hom */ + +/* PM_CARTESIAN */ + +struct PM_CARTESIAN { + /* ctors/dtors */ + PM_CARTESIAN() { + }; + PM_CARTESIAN(double _x, double _y, double _z); + + PM_CARTESIAN(const PM_CYLINDRICAL & c); /* conversion */ + PM_CARTESIAN(const PM_SPHERICAL & s); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + PM_CARTESIAN & operator += (const PM_CARTESIAN &o); + PM_CARTESIAN & operator -= (const PM_CARTESIAN &o); + + // Scalar operations + PM_CARTESIAN & operator *= (double o); + PM_CARTESIAN & operator /= (double o); + + /* data */ + double x, y, z; /* this.x, etc. */ +}; + +/* PM_SPHERICAL */ + +struct PM_SPHERICAL { + /* ctors/dtors */ + PM_SPHERICAL() { + }; + PM_SPHERICAL(double _theta, double _phi, double _r); + PM_SPHERICAL(const PM_CYLINDRICAL & v); /* conversion */ + PM_SPHERICAL(const PM_CARTESIAN & v); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* data */ + double theta, phi, r; +}; + +/* PM_CYLINDRICAL */ + +struct PM_CYLINDRICAL { + /* ctors/dtors */ + PM_CYLINDRICAL() { + }; + PM_CYLINDRICAL(double _theta, double _r, double _z); + PM_CYLINDRICAL(const PM_CARTESIAN & v); /* conversion */ + PM_CYLINDRICAL(const PM_SPHERICAL & v); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* data */ + double theta, r, z; +}; + +/* PM_ROTATION_VECTOR */ + +struct PM_ROTATION_VECTOR { + /* ctors/dtors */ + PM_ROTATION_VECTOR() { + }; + PM_ROTATION_VECTOR(double _r, double _x, double _y, double _z); + PM_ROTATION_VECTOR(const PM_QUATERNION & q); /* conversion + */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* data */ + double s, x, y, z; +}; + +/* PM_ROTATION_MATRIX */ + +struct PM_ROTATION_MATRIX { + /* ctors/dtors */ + PM_ROTATION_MATRIX() { + }; + PM_ROTATION_MATRIX(double xx, double xy, double xz, + double yx, double yy, double yz, double zx, double zy, double zz); + PM_ROTATION_MATRIX(const PM_CARTESIAN& _x, const PM_CARTESIAN& _y, const PM_CARTESIAN& _z); + PM_ROTATION_MATRIX(const PM_ROTATION_VECTOR & v); /* conversion + */ + PM_ROTATION_MATRIX(const PM_QUATERNION & q); /* conversion + */ + PM_ROTATION_MATRIX(const PM_EULER_ZYZ & zyz); /* conversion + */ + PM_ROTATION_MATRIX(const PM_EULER_ZYX & zyx); /* conversion + */ + PM_ROTATION_MATRIX(const PM_RPY & rpy); /* conversion */ + + /* operators */ + PM_CARTESIAN & operator[](int n); /* this[n] */ + + /* data */ + PM_CARTESIAN x, y, z; +}; + +/* PM_QUATERNION */ +struct PM_QUATERNION { + /* ctors/dtors */ + PM_QUATERNION() { + }; + PM_QUATERNION(double _s, double _x, double _y, double _z); + PM_QUATERNION(const PM_ROTATION_VECTOR & v); /* conversion + */ + PM_QUATERNION(const PM_ROTATION_MATRIX & m); /* conversion + */ + PM_QUATERNION(const PM_EULER_ZYZ & zyz); /* conversion */ + PM_QUATERNION(const PM_EULER_ZYX & zyx); /* conversion */ + PM_QUATERNION(const PM_RPY & rpy); /* conversion */ + PM_QUATERNION(PM_AXIS axis, double angle); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* functions */ + void axisAngleMult(PM_AXIS axis, double angle); + + /* data */ + double s, x, y, z; /* this.s, etc. */ +}; + +/* PM_EULER_ZYZ */ + +struct PM_EULER_ZYZ { + /* ctors/dtors */ + PM_EULER_ZYZ() { + }; + PM_EULER_ZYZ(double _z, double _y, double _zp); + PM_EULER_ZYZ(const PM_QUATERNION & q); /* conversion */ + PM_EULER_ZYZ(const PM_ROTATION_MATRIX & m); /* conversion */ + + /* operators */ + double &operator[] (int n); + + /* data */ + double z, y, zp; +}; + +/* PM_EULER_ZYX */ + +struct PM_EULER_ZYX { + /* ctors/dtors */ + PM_EULER_ZYX() { + }; + PM_EULER_ZYX(double _z, double _y, double _x); + PM_EULER_ZYX(const PM_QUATERNION & q); /* conversion */ + PM_EULER_ZYX(const PM_ROTATION_MATRIX & m); /* conversion */ + + /* operators */ + double &operator[] (int n); + + /* data */ + double z, y, x; +}; + +/* PM_RPY */ + +struct PM_RPY { + /* ctors/dtors */ + PM_RPY() { + }; + PM_RPY(double _r, double _p, double _y); + PM_RPY(const PM_QUATERNION & q); /* conversion */ + PM_RPY(const PM_ROTATION_MATRIX & m); /* conversion */ + + /* operators */ + double &operator[] (int n); + + /* data */ + double r, p, y; +}; + +/* PM_POSE */ + +struct PM_POSE { + /* ctors/dtors */ + PM_POSE() { + }; + PM_POSE(const PM_CARTESIAN& v, const PM_QUATERNION& q); + PM_POSE(double x, double y, double z, + double s, double sx, double sy, double sz); + PM_POSE(const PM_HOMOGENEOUS & h); /* conversion */ + + /* operators */ + double &operator[] (int n); /* this[n] */ + + /* data */ + PM_CARTESIAN tran; + PM_QUATERNION rot; +}; + +/* PM_HOMOGENEOUS */ + +struct PM_HOMOGENEOUS { + /* ctors/dtors */ + PM_HOMOGENEOUS() { + }; + PM_HOMOGENEOUS(const PM_CARTESIAN& v, const PM_ROTATION_MATRIX& m); + PM_HOMOGENEOUS(const PM_POSE & p); /* conversion */ + + /* operators */ + PM_CARTESIAN & operator[](int n); /* column vector */ + + /* data ( [ 0 0 0 1 ] element is manually returned by [] if needed ) */ + PM_CARTESIAN tran; + PM_ROTATION_MATRIX rot; +}; + +/* PM_LINE */ + +struct PM_LINE { + /* ctors/dtors */ + PM_LINE() { + }; + + /* functions */ + int init(const PM_POSE& start, const PM_POSE& end); + int point(double len, PM_POSE * point); + + /* data */ + PM_POSE start; /* where motion was started */ + PM_POSE end; /* where motion is going */ + PM_CARTESIAN uVec; /* unit vector from start to end */ +}; + +/* PM_CIRCLE */ + +struct PM_CIRCLE { + /* ctors/dtors */ + PM_CIRCLE() + : radius(0.0), + angle(0.0), + spiral(0.0) + {}; + + /* functions */ + int init(const PM_POSE& start, const PM_POSE& end, + const PM_CARTESIAN& center, const PM_CARTESIAN& normal, int turn); + int point(double angle, PM_POSE * point); + + /* data */ + PM_CARTESIAN center; + PM_CARTESIAN normal; + PM_CARTESIAN rTan; + PM_CARTESIAN rPerp; + PM_CARTESIAN rHelix; + double radius; + double angle; + double spiral; +}; + +/* overloaded external functions */ + +/* dot */ +extern double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +/* cross */ +extern PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +/* unit */ +extern PM_CARTESIAN unit(const PM_CARTESIAN &v); +extern PM_QUATERNION unit(const PM_QUATERNION &q); +extern PM_ROTATION_VECTOR unit(const PM_ROTATION_VECTOR &r); +extern PM_ROTATION_MATRIX unit(const PM_ROTATION_MATRIX &m); + +/* isNorm */ +extern int isNorm(const PM_CARTESIAN &v); +extern int isNorm(const PM_QUATERNION &q); +extern int isNorm(const PM_ROTATION_VECTOR &r); +extern int isNorm(const PM_ROTATION_MATRIX &m); + +/* mag */ +extern double mag(const PM_CARTESIAN &v); + +/* disp */ +extern double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +/* inv */ +extern PM_CARTESIAN inv(const PM_CARTESIAN &v); +extern PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m); +extern PM_QUATERNION inv(const PM_QUATERNION &q); +extern PM_POSE inv(const PM_POSE &p); +extern PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h); + +/* project */ +extern PM_CARTESIAN proj(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); + +/* overloaded arithmetic functions */ + +/* unary +, - for translation, rotation, pose */ +extern PM_CARTESIAN operator + (const PM_CARTESIAN &v); +extern PM_CARTESIAN operator - (const PM_CARTESIAN &v); +extern PM_QUATERNION operator + (const PM_QUATERNION &q); +extern PM_QUATERNION operator - (const PM_QUATERNION &q); +extern PM_POSE operator + (const PM_POSE &p); +extern PM_POSE operator - (const PM_POSE &p); + +/* compare operators */ +extern int operator == (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); +extern int operator == (const PM_QUATERNION &q1, const PM_QUATERNION &q2); +extern int operator == (const PM_POSE &p1, const PM_POSE &p2); +extern int operator != (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); +extern int operator != (const PM_QUATERNION &q1, const PM_QUATERNION &q2); +extern int operator != (const PM_POSE &p1, const PM_POSE &p2); + +/* translation +, -, scalar *, - */ + +/* v + v */ +extern PM_CARTESIAN operator + (PM_CARTESIAN v1, const PM_CARTESIAN &v2); +/* v - v */ +extern PM_CARTESIAN operator - (PM_CARTESIAN v1, const PM_CARTESIAN &v2); +/* v * s */ +extern PM_CARTESIAN operator *(PM_CARTESIAN v, double s); +/* s * v */ +extern PM_CARTESIAN operator *(double s, PM_CARTESIAN v); +/* v / s */ +extern PM_CARTESIAN operator / (const PM_CARTESIAN &v, double s); + +/* rotation * by scalar, translation, and rotation */ + +/* s * q */ +extern PM_QUATERNION operator *(double s, const PM_QUATERNION &q); +/* q * s */ +extern PM_QUATERNION operator *(const PM_QUATERNION &q, double s); +/* q / s */ +extern PM_QUATERNION operator / (const PM_QUATERNION &q, double s); +/* q * v */ +extern PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v); +/* q * q */ +extern PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2); +/* m * m */ +extern PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, + const PM_ROTATION_MATRIX &m2); + +/* pose operators */ + +/* q * p */ +extern PM_POSE operator *(const PM_QUATERNION &q, const PM_POSE &p); +/* p * p */ +extern PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2); +/* p * v */ +extern PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v); + +/* slicky macros for item-by-item copying between C and C++ structs */ + +template +void toCart(const A& src, B* dst) {(dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} + +template +void toCyl(const A& src, B* dst) {(dst)->theta = (src).theta; (dst)->r = (src).r; (dst)->z = (src).z;} + +template +void toSph(const A& src, B* dst) {(dst)->theta = (src).theta; (dst)->phi = (src).phi; (dst)->r = (src).r;} + +template +void toQuat(const A& src, B* dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} + +template +void toRot(const A& src, B* dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} + +template +void toMat(const A& src, B* dst) {toCart((src).x, &((dst)->x)); toCart((src).y, &((dst)->y)); toCart((src).z, &((dst)->z));} + +template +void toEulerZyz(const A& src, B* dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->zp = (src).zp;} + +template +void toEulerZyx(const A& src, B* dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->x = (src).x;} + +template +void toRpy(const A& src, B* dst) {(dst)->r = (src).r; (dst)->p = (src).p; (dst)->y = (src).y;} + +template +void toPose(const A& src, B* dst) {toCart((src).tran, &((dst)->tran)); toQuat((src).rot, &((dst)->rot));} + +template +void toHom(const A& src, B* dst) {toCart((src).tran, &((dst)->tran)); toMat((src).rot, &((dst)->rot));} + +template +void toLine(const A& src, B* dst) {toPose((src).start, &((dst)->start)); toPose((src).end, &((dst)->end)); toCart((src).uVec, &((dst)->uVec));} + +template +void toCircle(const A& src, B* dst) {toCart((src).center, &((dst)->center)); toCart((src).normal, &((dst)->normal)); toCart((src).rTan, &((dst)->rTan)); toCart((src).rPerp, &((dst)->rPerp)); toCart((src).rHelix, &((dst)->rHelix)); (dst)->radius = (src).radius; (dst)->angle = (src).angle; (dst)->spiral = (src).spiral;} + +#endif /* #ifndef __LINUXCNC_POSEMATH_HH */ diff --git a/src/libposemath/posemath_types.h b/src/libposemath/posemath_types.h new file mode 100644 index 00000000000..afa729c85b2 --- /dev/null +++ b/src/libposemath/posemath_types.h @@ -0,0 +1,221 @@ +/******************************************************************** +* Description: posemath_types.h +* Data types and constants of the pose math library. Included by +* posemath.h; code that wants only the types may include it directly. +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: LGPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ + +#ifndef __LINUXCNC_POSEMATH_TYPES_H +#define __LINUXCNC_POSEMATH_TYPES_H + +/* PmCartesian */ + + typedef struct { + double x, y, z; /* this.x, etc. */ + + } PmCartesian; + +/* PmSpherical */ + + typedef struct { + double theta, phi, r; + + } PmSpherical; + +/* PmCylindrical */ + + typedef struct { + double theta, r, z; + + } PmCylindrical; +/* PmAxis */ + typedef enum PM_AXIS { PM_X, PM_Y, PM_Z } PmAxis; + +/* PmRotationVector */ + + typedef struct { + double s, x, y, z; + + } PmRotationVector; + +/* PmRotationMatrix */ + + typedef struct { + PmCartesian x, y, z; + + } PmRotationMatrix; + +/* PmQuaternion */ + + typedef struct { + double s, x, y, z; /* this.s, etc. */ + + } PmQuaternion; + +/* PmEulerZyz */ + + typedef struct { + double z, y, zp; + + } PmEulerZyz; + +/* PmEulerZyx */ + + typedef struct { + double z, y, x; + + } PmEulerZyx; + +/* PmRpy */ + + typedef struct { + double r, p, y; + + } PmRpy; + +/* PmPose */ + + typedef struct { + PmCartesian tran; + PmQuaternion rot; + + } PmPose; + +/* PmCartLine */ + typedef struct { + PmCartesian start; + PmCartesian end; + PmCartesian uVec; + double tmag; + int tmag_zero; + } PmCartLine; + +/* Homogeneous transform PmHomogeneous */ + + typedef struct { + PmCartesian tran; + PmRotationMatrix rot; + + } PmHomogeneous; + +/* line structure */ + + typedef struct { + PmPose start; /* where motion was started */ + PmPose end; /* where motion is going */ + PmCartesian uVec; /* unit vector from start to end */ + PmQuaternion qVec; /* unit of rotation */ + double tmag; + double rmag; + int tmag_zero; + int rmag_zero; + + } PmLine; + +/* Generalized circle structure */ + + typedef struct { + PmCartesian center; + PmCartesian normal; + PmCartesian rTan; + PmCartesian rPerp; + PmCartesian rHelix; + double radius; + double angle; + double spiral; + + } PmCircle; + +/* some nice constants */ + +#define PM_PI 3.14159265358979323846 +#define PM_PI_2 1.57079632679489661923 +#define PM_PI_4 0.78539816339744830962 +#define PM_2_PI 6.28318530717958647692 + +#ifdef TO_DEG +#undef TO_DEG +#endif +#define TO_DEG (180./PM_PI) + +#ifdef TO_RAD +#undef TO_RAD +#endif +#define TO_RAD (PM_PI/180.) + +/*! \todo FIXME-- fix these */ + +/* DOUBLE_FUZZ is the smallest double, d, such that (1+d != 1) w/o FPC. + DOUBLECP_FUZZ is the same only with the Floating Point CoProcessor */ + +#define DOUBLE_FUZZ 2.2204460492503131e-16 +#define DOUBLECP_FUZZ 1.0842021724855044e-19 + + +/** + * FIXME sloppily defined constants here. + * These constants are quite large compared to the DOUBLE_FUZZ limitation. They + * seem like an ugly band-aid for floating point problems. + */ + +// FIXME setting this to be an order of magnitude smaller than canon's shortest +// allowed segment. This is still larger than TP's smallest position, so it may +// be silently causing trouble. +// andypugh 5/2/22 This seems to be interpreted to be in config units. +#define CART_FUZZ (1.0e-8) +/* how close a cartesian vector's magnitude must be for it to be considered + a zero vector */ + +#define Q_FUZZ (1.0e-06) +/* how close elements of a Q must be to be equal */ + +#define QS_FUZZ (1.0e-6) +/* how close q.s is to 0 to be 180 deg rotation */ + +#define RS_FUZZ (1.0e-6) +/* how close r.s is for a rotation vector to be considered 0 */ + +#define QSIN_FUZZ (1.0e-6) +/* how close sin(a/2) is to 0 to be zero rotation */ + +#define V_FUZZ (1.0e-8) +/* how close elements of a V must be to be equal */ + +#define SQRT_FUZZ (-1.0e-6) +/* how close to 0 before math_sqrt() is error */ + +#define UNIT_VEC_FUZZ (1.0e-6) +/* how close mag of vec must be to 1.00 */ + +#define UNIT_QUAT_FUZZ (1.0e-6) +/* how close mag of quat must be to 1.00 */ + +#define UNIT_SC_FUZZ (1.0e-6) +/* how close mag of sin, cos must be to 1.00 */ + +#define E_EPSILON (1.0e-6) +/* how close second ZYZ euler angle must be to 0/PI for degeneration */ + +#define SINGULAR_EPSILON (1.0e-6) +/* how close to zero the determinate of a matrix must be for singularity */ + +#define RPY_P_FUZZ (1.0e-6) +/* how close pitch is to zero for RPY to degenerate */ + +#define ZYZ_Y_FUZZ (1.0e-6) +/* how close Y is to zero for ZYZ Euler to degenerate */ + +#define ZYX_Y_FUZZ (1.0e-6) +/* how close Y is to zero for ZYX Euler to degenerate */ + +#define CIRCLE_FUZZ (1.0e-6) +/* Bug fix for the missing circles problem */ + +#endif /* #ifndef __LINUXCNC_POSEMATH_TYPES_H */ From 4206e03e641c939171ac88291662ee940c2b4cbb Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 15 Aug 2026 15:23:18 +1000 Subject: [PATCH 3/4] posemath: move EmcPose next to the pose math it is built from EmcPose is a machine pose made of a PmCartesian and six more doubles, and emcpose.c is arithmetic on it. Neither is an NML message, so neither belongs in nml_intf. The split across two headers was historical as well: emcpos.h held the type and emcpose.h the operations, and code that wanted the type included the one that could not stand alone. Fold both into src/libposemath/emcpose.h and move emcpose.c with it, into libposemath rather than liblinuxcnc, which is the only in-tree caller anyway by way of tpmod. emcpos.h stays where it is and includes the new header, so the hundred and twenty odd files that include it are untouched and out of tree code keeps compiling. The declarations gain an extern "C" block. They had none, so a C++ caller would have compiled and then failed to link; there is no such caller today. libposemath.so.0 gains the fifteen emcPose entry points and liblinuxcnc.a loses them. Nothing in userspace calls them. --- src/Makefile | 4 +-- src/emc/nml_intf/Submakefile | 2 -- src/emc/nml_intf/emcpos.h | 24 ++++----------- src/hal/components/tpcomp.comp | 2 +- src/libposemath/Submakefile | 3 +- src/{emc/nml_intf => libposemath}/emcpose.c | 2 +- src/{emc/nml_intf => libposemath}/emcpose.h | 33 +++++++++++++++++++-- 7 files changed, 42 insertions(+), 28 deletions(-) rename src/{emc/nml_intf => libposemath}/emcpose.c (99%) rename src/{emc/nml_intf => libposemath}/emcpose.h (69%) diff --git a/src/Makefile b/src/Makefile index 98d6a8bd7ed..b36312b58dc 100644 --- a/src/Makefile +++ b/src/Makefile @@ -407,8 +407,8 @@ SRCHEADERS := \ emc/ini/inifile.hh \ emc/ini/inifile.h \ emc/nml_intf/emcpos.h \ - emc/nml_intf/emcpose.h \ emc/nml_intf/motion_types.h \ + libposemath/emcpose.h \ libposemath/gomath.h \ libposemath/gotypes.h \ libposemath/posemath.h \ @@ -1294,7 +1294,7 @@ tpmod-objs += emc/tp/tcq.o tpmod-objs += emc/tp/tp.o tpmod-objs += emc/tp/spherical_arc.o tpmod-objs += emc/tp/blendmath.o -tpmod-objs += emc/nml_intf/emcpose.o +tpmod-objs += libposemath/emcpose.o tpmod-objs += libposemath/_posemath.o tpmod-objs += emc/tp/sp_scurve.o tpmod-objs += emc/tp/ruckig_wrapper.o diff --git a/src/emc/nml_intf/Submakefile b/src/emc/nml_intf/Submakefile index 9c250e242dc..d69d6239bf0 100644 --- a/src/emc/nml_intf/Submakefile +++ b/src/emc/nml_intf/Submakefile @@ -3,7 +3,6 @@ LIBEMCSRCS := \ emc/nml_intf/emcglb.c \ emc/nml_intf/modal_state.cc \ emc/nml_intf/emc.cc \ - emc/nml_intf/emcpose.c \ emc/nml_intf/emcargs.cc \ emc/nml_intf/emcops.cc \ emc/nml_intf/canon_position.cc \ @@ -28,7 +27,6 @@ TARGETS += ../lib/liblinuxcnc.a EMCNMLINTFINCS = \ ./emc/nml_intf/emcmotcfg.h \ ./emc/nml_intf/emcpos.h \ - ./emc/nml_intf/emcpose.h \ ./emc/nml_intf/motion_types.h $(patsubst ./emc/nml_intf/%,../include/%,$(EMCNMLINTFINCS)): ../include/%.h: ./emc/nml_intf/%.h diff --git a/src/emc/nml_intf/emcpos.h b/src/emc/nml_intf/emcpos.h index 8249846a766..e952b54d07c 100644 --- a/src/emc/nml_intf/emcpos.h +++ b/src/emc/nml_intf/emcpos.h @@ -1,12 +1,15 @@ /******************************************************************** * Description: emcpos.h * +* EmcPose now lives in emcpose.h, beside the pose math it is built out +* of. This header stays so that code including it keeps working. +* * Derived from a work by Fred Proctor & Will Shackleford * * Author: * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * * Last change: @@ -14,23 +17,6 @@ #ifndef __LINUXCNC_EMCPOS_H #define __LINUXCNC_EMCPOS_H -#include "posemath.h" /* PmCartesian */ - -typedef struct EmcPose { - PmCartesian tran; - double a, b, c; - double u, v, w; -} EmcPose; - -#define ZERO_EMC_POSE(pos) do { \ -(pos).tran.x = 0.0; \ -(pos).tran.y = 0.0; \ -(pos).tran.z = 0.0; \ -(pos).a = 0.0; \ -(pos).b = 0.0; \ -(pos).c = 0.0; \ -(pos).u = 0.0; \ -(pos).v = 0.0; \ -(pos).w = 0.0; } while(0) +#include "emcpose.h" #endif diff --git a/src/hal/components/tpcomp.comp b/src/hal/components/tpcomp.comp index 3e3001277e0..3a1d0a6ec19 100644 --- a/src/hal/components/tpcomp.comp +++ b/src/hal/components/tpcomp.comp @@ -69,7 +69,7 @@ option extra_setup; #include USE_TOPDIR(src/emc/tp/tcq.c) #include USE_TOPDIR(src/emc/tp/spherical_arc.c) #include USE_TOPDIR(src/emc/tp/blendmath.c) -#include USE_TOPDIR(src/emc/nml_intf/emcpose.c) +#include USE_TOPDIR(src/libposemath/emcpose.c) #include USE_TOPDIR(src/libposemath/_posemath.c) #include USE_TOPDIR(src/libposemath/sincos.c) diff --git a/src/libposemath/Submakefile b/src/libposemath/Submakefile index b95497f589f..9fac7576e44 100644 --- a/src/libposemath/Submakefile +++ b/src/libposemath/Submakefile @@ -1,4 +1,4 @@ -POSEMATHSRCS := $(addprefix libposemath/, _posemath.c posemath.cc gomath.c sincos.c) +POSEMATHSRCS := $(addprefix libposemath/, _posemath.c posemath.cc gomath.c sincos.c emcpose.c) $(call TOOBJSDEPS, $(POSEMATHSRCS)) : EXTRAFLAGS=-fPIC USERSRCS += $(POSEMATHSRCS) TARGETS += ../lib/libposemath.so ../lib/libposemath.so.0 @@ -10,6 +10,7 @@ TARGETS += ../lib/libposemath.so ../lib/libposemath.so.0 @$(CXX) $(LDFLAGS) -Wl,-soname,$(notdir $@) -shared -o $@ $^ POSEMATHINCS = \ + ./libposemath/emcpose.h \ ./libposemath/gomath.h \ ./libposemath/gotypes.h \ ./libposemath/posemath.h \ diff --git a/src/emc/nml_intf/emcpose.c b/src/libposemath/emcpose.c similarity index 99% rename from src/emc/nml_intf/emcpose.c rename to src/libposemath/emcpose.c index 5c58799d37b..3fade1f5e2d 100644 --- a/src/emc/nml_intf/emcpose.c +++ b/src/libposemath/emcpose.c @@ -13,7 +13,7 @@ ********************************************************************/ #include "emcpose.h" -#include +#include "posemath.h" #include //#define EMCPOSE_PEDANTIC diff --git a/src/emc/nml_intf/emcpose.h b/src/libposemath/emcpose.h similarity index 69% rename from src/emc/nml_intf/emcpose.h rename to src/libposemath/emcpose.h index 9d39ace01bd..ddb010ca02f 100644 --- a/src/emc/nml_intf/emcpose.h +++ b/src/libposemath/emcpose.h @@ -1,19 +1,44 @@ /******************************************************************** * Description: emcpose.h * +* The EmcPose type and the operations on it. EmcPose is a pose in the +* nine coordinates a machine can have, built out of the pose math types, +* and is not an NML message, which is where it used to live. +* * Derived from a work by Fred Proctor & Will Shackleford * * Author: Robert W. Ellenberg * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * ********************************************************************/ #ifndef __LINUXCNC_EMCPOSE_H #define __LINUXCNC_EMCPOSE_H -#include "emcpos.h" +#include "posemath.h" /* PmCartesian */ + +typedef struct EmcPose { + PmCartesian tran; + double a, b, c; + double u, v, w; +} EmcPose; + +#define ZERO_EMC_POSE(pos) do { \ +(pos).tran.x = 0.0; \ +(pos).tran.y = 0.0; \ +(pos).tran.z = 0.0; \ +(pos).a = 0.0; \ +(pos).b = 0.0; \ +(pos).c = 0.0; \ +(pos).u = 0.0; \ +(pos).v = 0.0; \ +(pos).w = 0.0; } while(0) + +#ifdef __cplusplus +extern "C" { +#endif typedef enum { EMCPOSE_ERR_OK = 0, @@ -48,4 +73,8 @@ int emcPoseMagnitude(EmcPose const * const pose, double * const out); int emcPoseValid(EmcPose const * const pose); +#ifdef __cplusplus +} /* matches extern "C" for C++ */ +#endif + #endif From 1472affefe4c3bfce272d8334f831aba1f52f709 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 15 Aug 2026 15:24:36 +1000 Subject: [PATCH 4/4] posemath: stop exporting gomath, gotypes and sincos The build copies gomath.h, gotypes.h and sincos.h into include/, which makes them part of what LinuxCNC offers to code built against it. Nothing outside libposemath needs them to be: gomath and gotypes are included by two files, both part of genserkins, and sincos.h declares one function, a shim over sin() and cos() that posemath itself calls. Those users include them by path instead, which is what src/-relative quoted includes are for, and the three headers come off the exported list. With the split in place the installed pose math surface is posemath.h and the two headers under it, plus emcpose.h, in one directory. Porting genserkins and scorbot off gomath, so that the N-DOF matrix code can move into posemath and gomath can go, is easier once the symbols are private. scorbot is already off it: scorbot-kins.c includes gotypes.h and uses nothing from it, so that include goes here. gomath.h included rtapi_math.h with quotes, which was right while gomath.h was itself exported and had to carry its siblings to include/. It is a user now, so it takes angle brackets, and it reaches the standard headers through rtapi_math.h rather than around it. gotypes.h wanted float.h for FLT_MAX and DBL_MAX and gomath.h wanted stddef.h for NULL; rtapi_math.h includes float.h and rtapi.h includes stddef.h, in both the kernel and the userspace branch. --- src/Makefile | 3 --- src/emc/kinematics/genserfuncs.c | 4 ++-- src/emc/kinematics/genserkins.h | 4 ++-- src/emc/kinematics/scorbot-kins.c | 1 - src/libposemath/Submakefile | 5 +---- src/libposemath/gomath.h | 4 +--- src/libposemath/gotypes.h | 2 +- 7 files changed, 7 insertions(+), 16 deletions(-) diff --git a/src/Makefile b/src/Makefile index b36312b58dc..14df8fa836c 100644 --- a/src/Makefile +++ b/src/Makefile @@ -409,12 +409,9 @@ SRCHEADERS := \ emc/nml_intf/emcpos.h \ emc/nml_intf/motion_types.h \ libposemath/emcpose.h \ - libposemath/gomath.h \ - libposemath/gotypes.h \ libposemath/posemath.h \ libposemath/posemath.hh \ libposemath/posemath_types.h \ - libposemath/sincos.h \ rtapi/rtapi.h \ rtapi/rtapi_app.h \ rtapi/rtapi_atomic.h \ diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 3d8aa726132..5600ab2be1f 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -38,8 +38,8 @@ #endif #include #include -#include /* go_result, go_integer */ -#include /* go_pose */ +#include "libposemath/gotypes.h" /* go_result, go_integer */ +#include "libposemath/gomath.h" /* go_pose */ #include #include "genserkins.h" /* these decls */ diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index 74c9b5d7ebf..3aa0756fc5a 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -34,8 +34,8 @@ #define GENSERKINS_H #include /* HAL data types */ -#include /* go_result, go_integer */ -#include /* go_pose */ +#include "libposemath/gotypes.h" /* go_result, go_integer */ +#include "libposemath/gomath.h" /* go_pose */ #include /*! diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index 2f4eeec413e..bd8868a063d 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -42,7 +42,6 @@ #include #include #include -#include #include diff --git a/src/libposemath/Submakefile b/src/libposemath/Submakefile index 9fac7576e44..c3d6c1eb8c8 100644 --- a/src/libposemath/Submakefile +++ b/src/libposemath/Submakefile @@ -11,11 +11,8 @@ TARGETS += ../lib/libposemath.so ../lib/libposemath.so.0 POSEMATHINCS = \ ./libposemath/emcpose.h \ - ./libposemath/gomath.h \ - ./libposemath/gotypes.h \ ./libposemath/posemath.h \ - ./libposemath/posemath_types.h \ - ./libposemath/sincos.h + ./libposemath/posemath_types.h POSEMATHCXXINCS = \ ./libposemath/posemath.hh diff --git a/src/libposemath/gomath.h b/src/libposemath/gomath.h index 75f76a02c2c..97d061da470 100644 --- a/src/libposemath/gomath.h +++ b/src/libposemath/gomath.h @@ -19,9 +19,7 @@ #ifndef __LINUXCNC_GO_MATH_H #define __LINUXCNC_GO_MATH_H -#include /* sizeof */ -#include "rtapi_math.h" /* M_PI */ -#include /* FLT,DBL_MIN,MAX,EPSILON */ +#include /* M_PI, NULL, FLT/DBL_MIN,MAX,EPSILON */ #include "gotypes.h" /* go_integer,real */ /*! Returns the square of \a x. */ diff --git a/src/libposemath/gotypes.h b/src/libposemath/gotypes.h index c1567b16765..65a44fe3892 100644 --- a/src/libposemath/gotypes.h +++ b/src/libposemath/gotypes.h @@ -20,7 +20,7 @@ #ifndef __LINUXCNC_GO_TYPES_H #define __LINUXCNC_GO_TYPES_H -#include /* DBL_MAX, FLOAT_MAX */ +#include /* DBL_MAX, FLT_MAX */ /*! GO_RESULT symbols run through a small range of values, on the