From 51b74e30346580fc6c3dce462fe338d87795a046 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 13 Oct 2026 00:01:16 +1100 Subject: [PATCH 1/4] genserkins: use posemath instead of gomath genserkins was the only user of gomath. The pose, quaternion and RPY work now goes through posemath, which has the same routines. What posemath has no counterpart for stays with genser as static functions in genserfuncs.c: the small matrices of the Jacobian and their LU inverse, the Denavit-Hartenberg link pose, and the quaternion to rotation vector conversion, since pmQuatRotConvert() zeroes a rotation under 1e-6 and the inverse iterates to 1e-7. pmPosePoseMult() writes the result's translation before it reads the first operand's, so products whose result is an operand go through a copy. The thresholds are gomath's double ones, 1e-7, so the results do not move: over a grid of 592 poses the forward agrees to 1e-12, and the inverse returns the same joints and the same result at every pose except those with the wrist exactly straight, where its Jacobian is singular and the iteration may settle on another of the equivalent wrist solutions. At a pitch of minus a quarter turn the RPY conversion now takes the branch posemath has for it; gomath tested plus a quarter turn twice (#389). genser_kin_jac_inv() and genser_kin_jac_fwd() had no callers and go, with the declarations of functions genserkins.h named but nothing defined. --- src/Makefile | 2 +- src/emc/kinematics/genserfuncs.c | 621 +++++++++++++++++++------------ src/emc/kinematics/genserkins.c | 5 - src/emc/kinematics/genserkins.h | 88 +++-- src/emc/kinematics/ugenserkins.c | 8 +- 5 files changed, 439 insertions(+), 285 deletions(-) diff --git a/src/Makefile b/src/Makefile index df19b0c3dc7..e1a02502425 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1116,7 +1116,7 @@ genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o genserkins-objs := emc/kinematics/genserkins.o genserkins-objs += emc/kinematics/genserfuncs.o -genserkins-objs += libposemath/gomath.o +genserkins-objs += libposemath/_posemath.o genserkins-objs += $(MATHSTUB) genserkins-objs += emc/kinematics/kins_util.o genserkins-objs += emc/kinematics/switchkins.o diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 02703735b2a..c5fe246faa3 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -39,8 +39,7 @@ #include #include #include -#include "libposemath/gotypes.h" /* go_result, go_integer */ -#include "libposemath/gomath.h" /* go_pose */ +#include #include #include "genserkins.h" /* these decls */ @@ -61,7 +60,7 @@ static struct haldata { hal_real_t d[GENSER_MAX_JOINTS]; hal_sint_t unrotate[GENSER_MAX_JOINTS]; genser_struct *kins; - go_pose *pos; // used in various functions, we malloc it + PmPose *pos; // used in various functions, we malloc it // only once in genserKinematicsSetup() } *haldata = NULL; @@ -74,6 +73,293 @@ double j[GENSER_MAX_JOINTS]; #error GENSER_MAX_JOINTS must be at least 6; fix genserkins.h #endif +/* below this a joint increment or a rotation counts as zero */ +#define GENSER_SMALL 1.0e-7 +/* below this a pivot leaves a matrix singular */ +#define GENSER_SINGULAR_EPSILON 1.0e-15 + +static void matrix_init(genser_matrix *m, int rows, int cols) +{ + memset(m, 0, sizeof(*m)); + m->rows = rows; + m->cols = cols; +} + +/* the products and sums go through a copy, so the result may be either + operand */ +static void matrix_matrix_mult(const genser_matrix *a, const genser_matrix *b, + genser_matrix *ab) +{ + genser_matrix out; + int row, col, i; + + matrix_init(&out, a->rows, b->cols); + for (row = 0; row < a->rows; row++) { + for (col = 0; col < b->cols; col++) { + for (i = 0; i < a->cols; i++) { + out.el[row][col] += a->el[row][i] * b->el[i][col]; + } + } + } + *ab = out; +} + +static void matrix_matrix_add(const genser_matrix *a, const genser_matrix *b, + genser_matrix *apb) +{ + genser_matrix out; + int row, col; + + matrix_init(&out, a->rows, a->cols); + for (row = 0; row < a->rows; row++) { + for (col = 0; col < a->cols; col++) { + out.el[row][col] = a->el[row][col] + b->el[row][col]; + } + } + *apb = out; +} + +static void matrix_vector_mult(const genser_matrix *a, const double *v, + double *av) +{ + double out[GENSER_MAX_JOINTS]; + int row, i; + + for (row = 0; row < a->rows; row++) { + out[row] = 0; + for (i = 0; i < a->cols; i++) { + out[row] += a->el[row][i] * v[i]; + } + } + for (row = 0; row < a->rows; row++) { + av[row] = out[row]; + } +} + +/* the cross product of each column of the 3 by N matrix a with v */ +static void matrix_vector_cross(const genser_matrix *a, const PmCartesian *v, + genser_matrix *axv) +{ + genser_matrix out; + PmCartesian c, cross; + int col; + + matrix_init(&out, 3, a->cols); + for (col = 0; col < a->cols; col++) { + c.x = a->el[0][col]; + c.y = a->el[1][col]; + c.z = a->el[2][col]; + pmCartCartCross(&c, v, &cross); + out.el[0][col] = cross.x; + out.el[1][col] = cross.y; + out.el[2][col] = cross.z; + } + *axv = out; +} + +static void matrix_transpose(const genser_matrix *a, genser_matrix *at) +{ + genser_matrix out; + int row, col; + + matrix_init(&out, a->cols, a->rows); + for (row = 0; row < a->rows; row++) { + for (col = 0; col < a->cols; col++) { + out.el[col][row] = a->el[row][col]; + } + } + *at = out; +} + +static void quat_matrix_convert(const PmQuaternion *q, genser_matrix *m) +{ + PmRotationMatrix r; + + pmQuatMatConvert(q, &r); + matrix_init(m, 3, 3); + m->el[0][0] = r.x.x; m->el[0][1] = r.y.x; m->el[0][2] = r.z.x; + m->el[1][0] = r.x.y; m->el[1][1] = r.y.y; m->el[1][2] = r.z.y; + m->el[2][0] = r.x.z; m->el[2][1] = r.y.z; m->el[2][2] = r.z.z; +} + +/* LU decomposition of the n by n matrix a in place, with partial + pivoting; the row permutation goes to indx */ +static int ludcmp(double a[GENSER_MAX_JOINTS][GENSER_MAX_JOINTS], int n, int *indx) +{ + double scale[GENSER_MAX_JOINTS]; + double big, dum, sum, temp; + int i, imax, j, k; + + for (i = 0; i < n; i++) { + big = 0.0; + for (j = 0; j < n; j++) { + if ((temp = fabs(a[i][j])) > big) { big = temp; } + } + if (big < GENSER_SINGULAR_EPSILON) { return GENSER_SINGULAR; } + scale[i] = 1.0 / big; + } + for (j = 0; j < n; j++) { + for (i = 0; i < j; i++) { + sum = a[i][j]; + for (k = 0; k < i; k++) { sum -= a[i][k] * a[k][j]; } + a[i][j] = sum; + } + big = 0.0; + imax = 0; + for (i = j; i < n; i++) { + sum = a[i][j]; + for (k = 0; k < j; k++) { sum -= a[i][k] * a[k][j]; } + a[i][j] = sum; + if ((dum = scale[i] * fabs(sum)) >= big) { + big = dum; + imax = i; + } + } + if (j != imax) { + for (k = 0; k < n; k++) { + dum = a[imax][k]; + a[imax][k] = a[j][k]; + a[j][k] = dum; + } + scale[imax] = scale[j]; + } + indx[j] = imax; + if (fabs(a[j][j]) < GENSER_SINGULAR_EPSILON) { return GENSER_SINGULAR; } + if (j != n - 1) { + dum = 1.0 / a[j][j]; + for (i = j + 1; i < n; i++) { a[i][j] *= dum; } + } + } + return GENSER_OK; +} + +/* solve a x = b for the decomposition ludcmp() left in a, b in place */ +static int lubksb(double a[GENSER_MAX_JOINTS][GENSER_MAX_JOINTS], int n, + const int *indx, double *b) +{ + double sum; + int i, ii = -1, ip, j; + + for (i = 0; i < n; i++) { + ip = indx[i]; + sum = b[ip]; + b[ip] = b[i]; + if (ii != -1) { + for (j = ii; j <= i - 1; j++) { sum -= a[i][j] * b[j]; } + } else if (sum) { + ii = i; + } + b[i] = sum; + } + for (i = n - 1; i >= 0; i--) { + sum = b[i]; + for (j = i + 1; j < n; j++) { sum -= a[i][j] * b[j]; } + if (fabs(a[i][i]) < GENSER_SINGULAR_EPSILON) { return GENSER_SINGULAR; } + b[i] = sum / a[i][i]; + } + return GENSER_OK; +} + +/* the inverse of the square matrix m */ +static int matrix_inv(const genser_matrix *m, genser_matrix *minv) +{ + double lu[GENSER_MAX_JOINTS][GENSER_MAX_JOINTS]; + double v[GENSER_MAX_JOINTS]; + int indx[GENSER_MAX_JOINTS]; + genser_matrix out; + int n = m->rows; + int row, col, retval; + + memcpy(lu, m->el, sizeof(lu)); + retval = ludcmp(lu, n, indx); + if (GENSER_OK != retval) { return retval; } + + /* backsubstitute a column with a 1 in it to get the inverse */ + matrix_init(&out, n, n); + for (col = 0; col < n; col++) { + memset(v, 0, sizeof(v)); + v[col] = 1.0; + retval = lubksb(lu, n, indx, v); + if (GENSER_OK != retval) { return retval; } + for (row = 0; row < n; row++) { out.el[row][col] = v[row]; } + } + *minv = out; + return GENSER_OK; +} + +/* pmPosePoseMult() writes the result's translation before it reads + the first operand's, so the result goes through a copy */ +static void pose_pose_mult(const PmPose *p1, const PmPose *p2, PmPose *pout) +{ + PmPose out; + + pmPosePoseMult(p1, p2, &out); + *pout = out; +} + +/* the rotation as a vector along its axis, as long as the angle. + pmQuatRotConvert() zeroes a rotation under QSIN_FUZZ, which would stop + the inverse short of the increment it converges to. */ +static void quat_rvec_convert(const PmQuaternion *q, PmCartesian *r) +{ + double sh = sqrt(q->x * q->x + q->y * q->y + q->z * q->z); + double mag; + + if (sh < GENSER_SMALL) { + r->x = 0; + r->y = 0; + r->z = 0; + } else { + mag = 2 * atan2(sh, q->s) / sh; + r->x = mag * q->x; + r->y = mag * q->y; + r->z = mag * q->z; + } +} + +static void dh_pose_convert(const genser_dh *dh, PmPose *p) +{ + PmHomogeneous h; + double sth = sin(dh->theta), cth = cos(dh->theta); + double sal = sin(dh->alpha), cal = cos(dh->alpha); + + h.rot.x.x = cth; h.rot.y.x = -sth; h.rot.z.x = 0.0; + h.rot.x.y = sth * cal; h.rot.y.y = cth * cal; h.rot.z.y = -sal; + h.rot.x.z = sth * sal; h.rot.y.z = cth * sal; h.rot.z.z = cal; + + h.tran.x = dh->a; + h.tran.y = -sal * dh->d; + h.tran.z = cal * dh->d; + + pmHomPoseConvert(&h, p); +} + +/* the link with its joint at the given value */ +static void link_joint_set(const genser_link *link, double joint, + genser_link *linkout) +{ + *linkout = *link; + if (GENSER_QUANTITY_LENGTH == link->quantity) { + linkout->dh.d = joint; + } else { + linkout->dh.theta = joint; + } +} + +/* the pose of the last link in the base frame */ +static void link_pose_build(const genser_link *links, int num, PmPose *pose) +{ + PmPose p; + int link; + + pose->tran.x = 0; pose->tran.y = 0; pose->tran.z = 0; + pose->rot.s = 1; pose->rot.x = 0; pose->rot.y = 0; pose->rot.z = 0; + for (link = 0; link < num; link++) { + dh_pose_convert(&links[link].dh, &p); + pose_pose_mult(pose, &p, pose); + } +} + static int genser_hal_inited = 0; int genser_kin_init(void) { @@ -84,19 +370,18 @@ int genser_kin_init(void) { /* init them all and make them revolute joints */ /* FIXME: should allow LINEAR joints based on HAL param too */ for (t = 0; t < GENSER_MAX_JOINTS; t++) { - genser->links[t].u.dh.a = hal_get_real(haldata->a[t]); - genser->links[t].u.dh.alpha = hal_get_real(haldata->alpha[t]); - genser->links[t].u.dh.d = hal_get_real(haldata->d[t]); - genser->links[t].u.dh.theta = 0; - genser->links[t].type = GO_LINK_DH; - genser->links[t].quantity = GO_QUANTITY_ANGLE; + genser->links[t].dh.a = hal_get_real(haldata->a[t]); + genser->links[t].dh.alpha = hal_get_real(haldata->alpha[t]); + genser->links[t].dh.d = hal_get_real(haldata->d[t]); + genser->links[t].dh.theta = 0; + genser->links[t].quantity = GENSER_QUANTITY_ANGLE; } /* set a select few to make it PUMA-like */ // FIXME-AJ: make a hal pin, also set number of joints based on it genser->link_num = 6; - return GO_RESULT_OK; + return GENSER_OK; } /* compute the forward jacobian function: @@ -106,78 +391,49 @@ int genser_kin_init(void) { It is analytically possible to calculate the inverse of the jacobian (sometimes only the pseudoinverse) and to use that for the inverse kinematics. */ -int compute_jfwd(go_link * link_params, +int compute_jfwd(const genser_link * link_params, int link_number, - go_matrix * Jfwd, - go_pose * T_L_0) + genser_matrix * Jfwd, + PmPose * T_L_0) { - GO_MATRIX_DECLARE(Jv, Jvstg, 3, GENSER_MAX_JOINTS); - GO_MATRIX_DECLARE(Jw, Jwstg, 3, GENSER_MAX_JOINTS); - GO_MATRIX_DECLARE(R_i_ip1, R_i_ip1stg, 3, 3); - GO_MATRIX_DECLARE(scratch, scratchstg, 3, GENSER_MAX_JOINTS); - GO_MATRIX_DECLARE(R_inv, R_invstg, 3, 3); - go_pose pose; - go_quat quat; - go_vector P_ip1_i[3]; + genser_matrix Jv, Jw, R_i_ip1, scratch, R_inv; + PmPose pose; + PmQuaternion quat; int row, col; - /* init matrices to possibly smaller size */ - go_matrix_init(Jv, Jvstg, 3, link_number); - go_matrix_init(Jw, Jwstg, 3, link_number); - go_matrix_init(R_i_ip1, R_i_ip1stg, 3, 3); - go_matrix_init(scratch, scratchstg, 3, link_number); - go_matrix_init(R_inv, R_invstg, 3, 3); + matrix_init(&Jv, 3, link_number); + matrix_init(&Jw, 3, link_number); - Jv.el[0][0] = 0, Jv.el[1][0] = 0, Jv.el[2][0] = (GO_QUANTITY_LENGTH == link_params[0].quantity ? 1 : 0); - Jw.el[0][0] = 0, Jw.el[1][0] = 0, Jw.el[2][0] = (GO_QUANTITY_ANGLE == link_params[0].quantity ? 1 : 0); + Jv.el[2][0] = (GENSER_QUANTITY_LENGTH == link_params[0].quantity ? 1 : 0); + Jw.el[2][0] = (GENSER_QUANTITY_ANGLE == link_params[0].quantity ? 1 : 0); /* initialize inverse rotational transform */ - if (GO_LINK_DH == link_params[0].type) { - go_dh_pose_convert(&link_params[0].u.dh, &pose); - } else if (GO_LINK_PP == link_params[0].type) { - pose = link_params[0].u.pp.pose; - } else { - return GO_RESULT_IMPL_ERROR; - } + dh_pose_convert(&link_params[0].dh, &pose); *T_L_0 = pose; for (col = 1; col < link_number; col++) { /* T_ip1_i */ - if (GO_LINK_DH == link_params[col].type) { - go_dh_pose_convert(&link_params[col].u.dh, &pose); - } else if (GO_LINK_PP == link_params[col].type) { - pose = link_params[col].u.pp.pose; - } else { - return GO_RESULT_IMPL_ERROR; - } + dh_pose_convert(&link_params[col].dh, &pose); - go_cart_vector_convert(&pose.tran, P_ip1_i); - go_quat_inv(&pose.rot, &quat); - go_quat_matrix_convert(&quat, &R_i_ip1); + pmQuatInv(&pose.rot, &quat); + quat_matrix_convert(&quat, &R_i_ip1); /* Jv */ - go_matrix_vector_cross(&Jw, P_ip1_i, &scratch); - go_matrix_matrix_add(&Jv, &scratch, &scratch); - go_matrix_matrix_mult(&R_i_ip1, &scratch, &Jv); - Jv.el[0][col] = 0, Jv.el[1][col] = 0, Jv.el[2][col] = (GO_QUANTITY_LENGTH == link_params[col].quantity ? 1 : 0); + matrix_vector_cross(&Jw, &pose.tran, &scratch); + matrix_matrix_add(&Jv, &scratch, &scratch); + matrix_matrix_mult(&R_i_ip1, &scratch, &Jv); + Jv.el[0][col] = 0, Jv.el[1][col] = 0, Jv.el[2][col] = (GENSER_QUANTITY_LENGTH == link_params[col].quantity ? 1 : 0); /* Jw */ - go_matrix_matrix_mult(&R_i_ip1, &Jw, &Jw); - Jw.el[0][col] = 0, Jw.el[1][col] = 0, Jw.el[2][col] = (GO_QUANTITY_ANGLE == link_params[col].quantity ? 1 : 0); - if (GO_LINK_DH == link_params[col].type) { - go_dh_pose_convert(&link_params[col].u.dh, &pose); - } else if (GO_LINK_PP == link_params[col].type) { - pose = link_params[col].u.pp.pose; - } else { - return GO_RESULT_IMPL_ERROR; - } - go_pose_pose_mult(T_L_0, &pose, T_L_0); + matrix_matrix_mult(&R_i_ip1, &Jw, &Jw); + Jw.el[0][col] = 0, Jw.el[1][col] = 0, Jw.el[2][col] = (GENSER_QUANTITY_ANGLE == link_params[col].quantity ? 1 : 0); + pose_pose_mult(T_L_0, &pose, T_L_0); } /* rotate back into {0} frame */ - go_quat_matrix_convert(&T_L_0->rot, &R_inv); - go_matrix_matrix_mult(&R_inv, &Jv, &Jv); - go_matrix_matrix_mult(&R_inv, &Jw, &Jw); + quat_matrix_convert(&T_L_0->rot, &R_inv); + matrix_matrix_mult(&R_inv, &Jv, &Jv); + matrix_matrix_mult(&R_inv, &Jw, &Jw); /* put Jv atop Jw in J */ for (row = 0; row < 6; row++) { @@ -190,128 +446,45 @@ int compute_jfwd(go_link * link_params, } } - return GO_RESULT_OK; + return GENSER_OK; } /* compute the inverse of the jacobian matrix */ -int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv) +int compute_jinv(const genser_matrix * Jfwd, genser_matrix * Jinv) { int retval; - GO_MATRIX_DECLARE(JT, JTstg, GENSER_MAX_JOINTS, 6); + genser_matrix JT; /* compute inverse, or pseudo-inverse */ if (Jfwd->rows == Jfwd->cols) { - retval = go_matrix_inv(Jfwd, Jinv); - if (GO_RESULT_OK != retval) + retval = matrix_inv(Jfwd, Jinv); + if (GENSER_OK != retval) return retval; } else if (Jfwd->rows < Jfwd->cols) { /* underdetermined, optimize on smallest sum of square of speeds */ /* JT(JJT)inv */ - GO_MATRIX_DECLARE(JJT, JJTstg, 6, 6); - - go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); - go_matrix_init(JJT, JJTstg, Jfwd->rows, Jfwd->rows); - go_matrix_transpose(Jfwd, &JT); - go_matrix_matrix_mult(Jfwd, &JT, &JJT); - retval = go_matrix_inv(&JJT, &JJT); - if (GO_RESULT_OK != retval) + genser_matrix JJT; + + matrix_transpose(Jfwd, &JT); + matrix_matrix_mult(Jfwd, &JT, &JJT); + retval = matrix_inv(&JJT, &JJT); + if (GENSER_OK != retval) return retval; - go_matrix_matrix_mult(&JT, &JJT, Jinv); + matrix_matrix_mult(&JT, &JJT, Jinv); } else { /* overdetermined, do least-squares best fit */ /* (JTJ)invJT */ - GO_MATRIX_DECLARE(JTJ, JTJstg, GENSER_MAX_JOINTS, GENSER_MAX_JOINTS); - - go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); - go_matrix_init(JTJ, JTJstg, Jfwd->cols, Jfwd->cols); - go_matrix_transpose(Jfwd, &JT); - go_matrix_matrix_mult(&JT, Jfwd, &JTJ); - retval = go_matrix_inv(&JTJ, &JTJ); - if (GO_RESULT_OK != retval) - return retval; - go_matrix_matrix_mult(&JTJ, &JT, Jinv); - } - - return GO_RESULT_OK; -} - -int genser_kin_jac_inv(void *kins, - const go_pose * pos, - const go_screw * vel, const go_real * joints, go_real * jointvels) -{ - (void)pos; - genser_struct *genser = (genser_struct *) kins; - GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); - GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); - go_pose T_L_0; - go_link linkout[GENSER_MAX_JOINTS] = {}; - go_real vw[6]; - int link; - int retval; - - go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); - go_matrix_init(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + genser_matrix JTJ; - for (link = 0; link < genser->link_num; link++) { - retval = - go_link_joint_set(&genser->links[link], joints[link], - &linkout[link]); - if (GO_RESULT_OK != retval) + matrix_transpose(Jfwd, &JT); + matrix_matrix_mult(&JT, Jfwd, &JTJ); + retval = matrix_inv(&JTJ, &JTJ); + if (GENSER_OK != retval) return retval; + matrix_matrix_mult(&JTJ, &JT, Jinv); } - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) - return retval; - retval = compute_jinv(&Jfwd, &Jinv); - if (GO_RESULT_OK != retval) - return retval; - - vw[0] = vel->v.x; - vw[1] = vel->v.y; - vw[2] = vel->v.z; - vw[3] = vel->w.x; - vw[4] = vel->w.y; - vw[5] = vel->w.z; - return go_matrix_vector_mult(&Jinv, vw, jointvels); -} - -int genser_kin_jac_fwd(void *kins, - const go_real * joints, - const go_real * jointvels, const go_pose * pos, go_screw * vel) -{ - (void)pos; - genser_struct *genser = (genser_struct *) kins; - GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); - go_pose T_L_0; - go_link linkout[GENSER_MAX_JOINTS] = {}; - go_real vw[6]; - int link; - int retval; - - go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); - - for (link = 0; link < genser->link_num; link++) { - retval = - go_link_joint_set(&genser->links[link], joints[link], - &linkout[link]); - if (GO_RESULT_OK != retval) - return retval; - } - - retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) - return retval; - - go_matrix_vector_mult(&Jfwd, jointvels, vw); - vel->v.x = vw[0]; - vel->v.y = vw[1]; - vel->v.z = vw[2]; - vel->w.x = vw[3]; - vel->w.y = vw[4]; - vel->w.z = vw[5]; - - return GO_RESULT_OK; + return GENSER_OK; } /* The Jacobian in the terms of kinematics.h: joints in degrees per pose @@ -320,7 +493,7 @@ int genser_kin_jac_fwd(void *kins, compute_jinv() gives the geometric inverse Jacobian, radians of joint per unit of base-frame twist. A pose word rate is not a twist: the roll, pitch and yaw rates reach the angular velocity through E, the matrix of - the axes each one turns about, for the RPY convention of go_rpy_mat_convert, + the axes each one turns about, for the RPY convention of pmRpyMatConvert, R = Rz(yaw) Ry(pitch) Rx(roll). So dq/dp = unrotate . deg . Jinv . blockdiag(I, E . rad) @@ -334,11 +507,10 @@ int genserKinematicsJacobian(const double *joint, { (void)iflags; genser_struct *genser = KINS_PTR; - GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); - GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); - go_pose T_L_0; - go_link linkout[GENSER_MAX_JOINTS] = {}; - go_real jest[GENSER_MAX_JOINTS]; + genser_matrix Jfwd, Jinv; + PmPose T_L_0; + genser_link linkout[GENSER_MAX_JOINTS] = {}; + double jest[GENSER_MAX_JOINTS]; double E[3][3]; double sb, cb, sc, cc; int link, i, a, m, retval; @@ -363,19 +535,17 @@ int genserKinematicsJacobian(const double *joint, jest[link] -= unrotate * jest[link-1]; } - go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); - go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); + matrix_init(&Jfwd, 6, genser->link_num); + matrix_init(&Jinv, genser->link_num, 6); for (link = 0; link < genser->link_num; link++) { - retval = go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); - if (GO_RESULT_OK != retval) - return -1; + link_joint_set(&genser->links[link], jest[link], &linkout[link]); } retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) + if (GENSER_OK != retval) return -1; retval = compute_jinv(&Jfwd, &Jinv); - if (GO_RESULT_OK != retval) + if (GENSER_OK != retval) return -1; // singular: no finite joint rate follows the pose // E columns: the roll axis carried by pitch and yaw, the pitch axis @@ -427,9 +597,9 @@ int genserKinematicsForward(const double *joint, (void)fflags; (void)iflags; - go_pose *pos; - go_rpy rpy; - go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints + PmPose *pos; + PmRpy rpy; + double jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints int ret = 0; int i, changed=0; if (!genser_hal_inited) { @@ -440,7 +610,7 @@ int genserKinematicsForward(const double *joint, for (i=0; i< 6; i++) { // FIXME - debug hack - if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1; + if (fabs(j[i] - joint[i]) >= GENSER_SMALL) changed = 1; // convert to radians to pass to genser_kin_fwd jcopy[i] = joint[i] * PM_PI / 180; rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[i]); @@ -462,7 +632,7 @@ int genserKinematicsForward(const double *joint, rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &pos->rot); + pmRpyQuatConvert(&rpy, &pos->rot); pos->tran.x = world->tran.x; pos->tran.y = world->tran.y; pos->tran.z = world->tran.z; @@ -479,9 +649,7 @@ int genserKinematicsForward(const double *joint, return ret; // AJ: convert back to emc2 coords - ret = go_quat_rpy_convert(&pos->rot, &rpy); - if (ret < 0) - return ret; + pmQuatRpyConvert(&pos->rot, &rpy); world->tran.x = pos->tran.x; world->tran.y = pos->tran.y; world->tran.z = pos->tran.z; @@ -495,27 +663,21 @@ int genserKinematicsForward(const double *joint, return 0; } -int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) +int genser_kin_fwd(void *kins, const double * joints, PmPose * pos) { genser_struct *genser = kins; - go_link linkout[GENSER_MAX_JOINTS] = {}; - + genser_link linkout[GENSER_MAX_JOINTS] = {}; int link; - int retval; genser_kin_init(); for (link = 0; link < genser->link_num; link++) { - retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); - if (GO_RESULT_OK != retval) - return retval; + link_joint_set(&genser->links[link], joints[link], &linkout[link]); } - retval = go_link_pose_build(linkout, genser->link_num, pos); - if (GO_RESULT_OK != retval) - return retval; + link_pose_build(linkout, genser->link_num, pos); - return GO_RESULT_OK; + return GENSER_OK; } int genserKinematicsInverse(const EmcPose * world, @@ -527,17 +689,16 @@ int genserKinematicsInverse(const EmcPose * world, (void)fflags; genser_struct *genser = KINS_PTR; - GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); - GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); - go_pose T_L_0; - go_real dvw[6]; - go_real jest[GENSER_MAX_JOINTS]; - go_real dj[GENSER_MAX_JOINTS]; - go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose - go_rpy rpy; - go_rvec rvec; - go_cart cart; - go_link linkout[GENSER_MAX_JOINTS]; + genser_matrix Jfwd, Jinv; + PmPose T_L_0; + double dvw[6]; + double jest[GENSER_MAX_JOINTS]; + double dj[GENSER_MAX_JOINTS]; + PmPose pest, pestinv, Tdelta; // pos = converted pose from EmcPose + PmRpy rpy; + PmCartesian rvec; + PmCartesian cart; + genser_link linkout[GENSER_MAX_JOINTS]; int link; int smalls; int retval; @@ -561,13 +722,13 @@ int genserKinematicsInverse(const EmcPose * world, rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &haldata->pos->rot); + pmRpyQuatConvert(&rpy, &haldata->pos->rot); haldata->pos->tran.x = world->tran.x; haldata->pos->tran.y = world->tran.y; haldata->pos->tran.z = world->tran.z; - go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); - go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); + matrix_init(&Jfwd, 6, genser->link_num); + matrix_init(&Jinv, genser->link_num, 6); /* jest[] is a copy of joints[], which is the joint estimate */ for (link = 0; link < genser->link_num; link++) { @@ -581,16 +742,16 @@ int genserKinematicsInverse(const EmcPose * world, hal_set_ui32(haldata->last_iterations, genser->iterations); /* update the Jacobians */ for (link = 0; link < genser->link_num; link++) { - go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); + link_joint_set(&genser->links[link], jest[link], &linkout[link]); } retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); - if (GO_RESULT_OK != retval) { + if (GENSER_OK != retval) { rtapi_print("ERR kI - compute_jfwd (joints: %f %f %f %f %f %f), (iterations=%d)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); return retval; } retval = compute_jinv(&Jfwd, &Jinv); - if (GO_RESULT_OK != retval) { + if (GENSER_OK != retval) { rtapi_print("ERR kI - compute_jinv (joints: %f %f %f %f %f %f), (iterations=%d)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); return retval; @@ -600,7 +761,7 @@ int genserKinematicsInverse(const EmcPose * world, genser_kin_fwd(KINS_PTR, jest, &pest); //printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]); /* pestinv is its inverse */ - go_pose_inv(&pest, &pestinv); + pmPoseInv(&pest, &pestinv); /* Tdelta is the incremental pose from pest to pos, such that @@ -612,7 +773,7 @@ int genserKinematicsInverse(const EmcPose * world, .Tdelta = pestinv * pos L 0 L */ - go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); + pose_pose_mult(&pestinv, haldata->pos, &Tdelta); /* We need Tdelta in 0 frame, not pest frame, so rotate it @@ -621,24 +782,21 @@ int genserKinematicsInverse(const EmcPose * world, */ /* first rotate the translation differential */ - go_quat_cart_mult(&pest.rot, &Tdelta.tran, &cart); + pmQuatCartMult(&pest.rot, &Tdelta.tran, &cart); dvw[0] = cart.x; dvw[1] = cart.y; dvw[2] = cart.z; /* to rotate the rotation differential, convert it to a velocity screw and rotate that */ - go_quat_rvec_convert(&Tdelta.rot, &rvec); - cart.x = rvec.x; - cart.y = rvec.y; - cart.z = rvec.z; - go_quat_cart_mult(&pest.rot, &cart, &cart); + quat_rvec_convert(&Tdelta.rot, &rvec); + pmQuatCartMult(&pest.rot, &rvec, &cart); dvw[3] = cart.x; dvw[4] = cart.y; dvw[5] = cart.z; /* push the Cartesian velocity vector through the inverse Jacobian */ - go_matrix_vector_mult(&Jinv, dvw, dj); + matrix_vector_mult(&Jinv, dvw, dj); //pass through 678 as uvw if (total_joints > 6) joints[6] = world->u; @@ -647,13 +805,8 @@ int genserKinematicsInverse(const EmcPose * world, /* check for small joint increments, if so we're done */ for (link = 0, smalls = 0; link < genser->link_num; link++) { - if (GO_QUANTITY_LENGTH == linkout[link].quantity) { - if (GO_TRAN_SMALL(dj[link])) + if (fabs(dj[link]) < GENSER_SMALL) smalls++; - } else { - if (GO_ROT_SMALL(dj[link])) - smalls++; - } } if (smalls == genser->link_num) { /* converged, copy jest[] out */ @@ -668,7 +821,7 @@ int genserKinematicsInverse(const EmcPose * world, // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); //rtapi_print("OKkineInverse: %.2f %.2f %.2f %.2f %.2f %.2f)\n", // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - return GO_RESULT_OK; + return GENSER_OK; } /* else keep iterating */ for (link = 0; link < genser->link_num; link++) { @@ -678,7 +831,7 @@ int genserKinematicsInverse(const EmcPose * world, rtapi_print("ERRkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - return GO_RESULT_ERROR; + return GENSER_ERROR; } /* @@ -694,9 +847,9 @@ int genser_kin_inv_iterations(genser_struct * genser) int genser_kin_inv_set_max_iterations(int i) { - if (i <= 0) return GO_RESULT_ERROR; + if (i <= 0) return GENSER_ERROR; hal_set_ui32(haldata->max_iterations, i); - return GO_RESULT_OK; + return GENSER_OK; } int genser_kin_inv_get_max_iterations() @@ -742,7 +895,7 @@ int genserKinematicsSetup(const int comp_id, 0, "%s.last-iterations",kp->halprefix); KINS_PTR = hal_malloc(sizeof(genser_struct)); - haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose)); + haldata->pos = (PmPose *) hal_malloc(sizeof(PmPose)); if (KINS_PTR == NULL) {goto error;} if (haldata->pos == NULL) {goto error;} res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->max_iterations, diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index be71f33ffc5..c8b2eb655d5 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -12,7 +12,6 @@ TEST: switchable kinematics: identity or genserkins 1) genser kinematics provided by genserfuncs.c (shared with genserkins.c 2) uses same pin names as genserkins -3) tesing mm configs: increase GO_REAL_EPSILON from 1e-7 to 1e-6 NOTE: a) requires *exactly* 6 joints @@ -44,10 +43,6 @@ frame-larger-than: #include "genserkins.h" #include -//-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) -#undef GO_REAL_EPSILON -#define GO_REAL_EPSILON (1e-6) - //********************************************************************* diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index b74b826d2ec..4000962e09b 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -34,8 +34,7 @@ #define GENSERKINS_H #include /* HAL data types */ -#include "libposemath/gotypes.h" /* go_result, go_integer */ -#include "libposemath/gomath.h" /* go_pose */ +#include /* PmPose */ #include /*! @@ -46,7 +45,7 @@ #define GENSER_DEFAULT_MAX_ITERATIONS 100 -#define PI_2 GO_PI_2 +#define PI_2 PM_PI_2 /* default DH parameters, these should be ok for a puma - at least according to Craig */ #define DEFAULT_A1 0 @@ -73,47 +72,54 @@ #define DEFAULT_ALPHA6 -PI_2 #define DEFAULT_D6 0 +/* results; nonzero is a failure */ +enum { + GENSER_OK = 0, + GENSER_ERROR, /* the inverse did not converge */ + GENSER_SINGULAR /* a Jacobian has no inverse */ +}; + +/* what a joint moves: the link offset d or the joint angle theta */ +enum { + GENSER_QUANTITY_LENGTH = 1, + GENSER_QUANTITY_ANGLE +}; + +/* + Denavit-Hartenberg parameters of a link, as John J Craig defines + them in "Introduction to Robotics: Mechanics and Control". +*/ +typedef struct { + double a; /* a[i-1] */ + double alpha; /* alpha[i-1] */ + double d; /* d[i] */ + double theta; /* theta[i] */ +} genser_dh; + typedef struct { - go_link links[GENSER_MAX_JOINTS]; /*!< The link description of the device. */ + genser_dh dh; + int quantity; /* GENSER_QUANTITY_LENGTH or GENSER_QUANTITY_ANGLE */ +} genser_link; + +/* a matrix of up to GENSER_MAX_JOINTS rows and columns, the Jacobians + are 6 by the joint count */ +typedef struct { + int rows; + int cols; + double el[GENSER_MAX_JOINTS][GENSER_MAX_JOINTS]; +} genser_matrix; + +typedef struct { + genser_link links[GENSER_MAX_JOINTS]; /*!< The link description of the device. */ int link_num; /*!< How many are actually present. */ unsigned iterations; /*!< How many iterations were actually used to compute the inverse kinematics. */ } genser_struct; -extern int genser_kin_size(void); - extern int genser_kin_init(void); -extern const char * genser_kin_get_name(void); - -extern int genser_kin_num_joints(void * kins); - extern int genser_kin_fwd(void * kins, - const go_real *joint, - go_pose * world); - -extern int genser_kin_inv(void * kins, - const go_pose * world, - go_real *joint); - -extern int genser_kin_set_parameters(void * kins, go_link * params, int num); - -extern int genser_kin_get_parameters(void * kins, go_link * params, int num); - -extern int genser_kin_jac_inv(void * kins, - const go_pose * pos, - const go_screw * vel, - const go_real * joints, - go_real * jointvels); - - -extern int genser_kin_jac_fwd(void * kins, - const go_real * joints, - const go_real * jointvels, - const go_pose * pos, - go_screw * vel); - -extern int genser_kin_fwd_interations(genser_struct * genser); - + const double *joint, + PmPose * world); /* Extras, not callable using go_kin_ wrapper but if you know you have @@ -134,13 +140,13 @@ extern int genser_kin_inv_set_max_iterations(int i); compute inverse kinematics functions */ extern int genser_kin_inv_get_max_iterations(void); -extern int compute_jfwd(go_link * link_params, +extern int compute_jfwd(const genser_link * link_params, int link_number, - go_matrix * Jfwd, - go_pose * T_L_0); + genser_matrix * Jfwd, + PmPose * T_L_0); -extern int compute_jinv(go_matrix * Jfwd, - go_matrix * Jinv); +extern int compute_jinv(const genser_matrix * Jfwd, + genser_matrix * Jinv); extern int genserKinematicsJacobian(const double *joint, const EmcPose *world, diff --git a/src/emc/kinematics/ugenserkins.c b/src/emc/kinematics/ugenserkins.c index 1d80e5ae70c..859efb37f32 100644 --- a/src/emc/kinematics/ugenserkins.c +++ b/src/emc/kinematics/ugenserkins.c @@ -127,7 +127,7 @@ joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); fprintf(stderr,"gki2:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); if (0 != retval) { - printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + printf("inv kins error %d\n", retval); } } else { retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); @@ -224,7 +224,7 @@ fprintf(stderr,"gki1:\n"); printf("%f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { - printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + printf("inv kins error %d\n", retval); } else { retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); @@ -262,7 +262,7 @@ fprintf(stderr,"gki1:\n"); jointvels[2], jointvels[3], jointvels[4], jointvels[5]); if (0 != retval) { - printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + printf("inv kins error %d\n", retval); } } } @@ -281,7 +281,7 @@ fprintf(stderr,"gki1:\n"); printf("j0--j5: %f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { - printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + printf("inv kins error %d\n", retval); } } } From 2798bd27f1a6d7ccfc46025e9b14cc1e0bf68a10 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 13 Oct 2026 00:01:16 +1100 Subject: [PATCH 2/4] libposemath: remove gomath With genserkins on posemath nothing uses gomath. Its headers were never exported, so no module outside the tree could include them. --- src/emc/kinematics/userkfuncs.c | 2 +- src/libposemath/Submakefile | 2 +- src/libposemath/gomath.c | 3749 ------------------------------- src/libposemath/gomath.h | 852 ------- src/libposemath/gotypes.h | 169 -- 5 files changed, 2 insertions(+), 4772 deletions(-) delete mode 100644 src/libposemath/gomath.c delete mode 100644 src/libposemath/gomath.h delete mode 100644 src/libposemath/gotypes.h diff --git a/src/emc/kinematics/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 81aa4c2d942..4cdab2f4f5a 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -21,7 +21,7 @@ #include // Add for kins based on genserkins: -// #include "genserkins.h" //includes gomath,hal +// #include "genserkins.h" //includes posemath,hal //********************************************************************** // static local variables and functions go here diff --git a/src/libposemath/Submakefile b/src/libposemath/Submakefile index 48aa5f64370..8fd132254cf 100644 --- a/src/libposemath/Submakefile +++ b/src/libposemath/Submakefile @@ -1,4 +1,4 @@ -POSEMATHSRCS := $(addprefix libposemath/, _posemath.c posemath.cc gomath.c emcpose.c) +POSEMATHSRCS := $(addprefix libposemath/, _posemath.c posemath.cc emcpose.c) $(call TOOBJSDEPS, $(POSEMATHSRCS)) : EXTRAFLAGS=-fPIC USERSRCS += $(POSEMATHSRCS) TARGETS += ../lib/libposemath.so ../lib/libposemath.so.0 diff --git a/src/libposemath/gomath.c b/src/libposemath/gomath.c deleted file mode 100644 index 028bca152b4..00000000000 --- a/src/libposemath/gomath.c +++ /dev/null @@ -1,3749 +0,0 @@ -/******************************************************************** -* Description: gomath.c -* Library file with various functions for working with matrices -* -* Derived from a work by Fred Proctor, -* changed to work with emc2 and HAL -* -* Adapting Author: Alex Joni -* License: LGPL Version 2 -* System: Linux -* -******************************************************************* - Similar to posemath, but using different functions. - - TODO: - * find the new functions, add them to posemath, convert the rest -*/ - -/* for debugging */ -extern int printf(const char * fmt, ...); -#include /* NULL */ -#include -#include /* memset */ -#include - - -#ifdef HAVE_CONFIG_H -#include "config.h" -#endif -#include "gotypes.h" /* GoResult ,Real, etc */ -#include "gomath.h" /* these decls, GoCartesian, etc. */ - -go_real go_cbrt(go_real x) -{ - if (x < 0.0) { - return (go_real) -pow((double) -x, 1.0/3.0); - } - return (go_real) pow((double) x, 1.0/3.0); -} - - -/* Translation rep conversion functions */ - -int go_cart_sph_convert(const go_cart * v, go_sph * s) -{ - go_real r; - - s->theta = atan2(v->y, v->x); - r = sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)); - s->r = r; - if (GO_TRAN_SMALL(r)) { - s->phi = 0; - } else { - s->phi = acos(v->z / r); - } - - return GO_RESULT_OK; -} - -int go_cart_cyl_convert(const go_cart * v, go_cyl * c) -{ - c->theta = atan2(v->y, v->x); - c->r = sqrt(go_sq(v->x) + go_sq(v->y)); - c->z = v->z; - - return GO_RESULT_OK; -} - -int go_sph_cart_convert(const go_sph * s, go_cart * v) -{ - go_real sth, cth, sph, cph; - - sth = sin(s->theta); - cth = cos(s->theta); - sph = sin(s->phi); - cph = cos(s->phi); - - v->x = s->r * cth * sph; - v->y = s->r * sth * sph; - v->z = s->r * cph; - - return GO_RESULT_OK; -} - -int go_sph_cyl_convert(const go_sph * s, go_cyl * c) -{ - go_real sph, cph; - - sph = sin(s->phi); - cph = cos(s->phi); - - c->theta = s->theta; - c->r = s->r * sph; - c->z = s->r * cph; - - return GO_RESULT_OK; -} - -int go_cyl_cart_convert(const go_cyl * c, go_cart * v) -{ - v->x = c->r * cos(c->theta); - v->y = c->r * sin(c->theta); - v->z = c->z; - - return GO_RESULT_OK; -} - -int go_cyl_sph_convert(const go_cyl * c, go_sph * s) -{ - s->theta = c->theta; - s->r = sqrt(go_sq(c->r) + go_sq(c->z)); - if (GO_TRAN_SMALL(s->r)) { - s->phi = 0.0; - } else { - s->phi = acos(c->z / s->r); - } - - return GO_RESULT_OK; -} - -/* rotation rep conversion functions */ - -int go_rvec_quat_convert(const go_rvec * r, go_quat * q) -{ - go_cart vec; - go_cart uvec; - go_real mag; - go_real sh; - - vec.x = r->x; - vec.y = r->y; - vec.z = r->z; - - if (GO_RESULT_OK != go_cart_unit(&vec, &uvec)) { - /* a zero vector */ - q->s = 1; - q->x = q->y = q->z = 0; - return GO_RESULT_OK; - } - - (void) go_cart_mag(&vec, &mag); - - sh = sin(0.5 * mag); - q->s = cos(0.5 * mag); - - if (q->s >= 0) { - q->x = uvec.x * sh; - q->y = uvec.y * sh; - q->z = uvec.z * sh; - } else { - q->s = -q->s; - q->x = -uvec.x * sh; - q->y = -uvec.y * sh; - q->z = -uvec.z * sh; - } - - return GO_RESULT_OK; -} - -int go_rvec_mat_convert(const go_rvec * r, go_mat * m) -{ - go_cart vec; - go_cart uvec; - go_real s, c, omc; - go_real mag; - - vec.x = r->x; - vec.y = r->y; - vec.z = r->z; - - if (GO_RESULT_OK != go_cart_unit(&vec, &uvec)) { - /* a zero vector */ - m->x.x = 1, m->y.x = 0, m->z.x = 0; - m->x.y = 0, m->y.y = 1, m->z.y = 0; - m->x.z = 0, m->y.z = 0, m->z.z = 1; - return GO_RESULT_OK; - } - - (void) go_cart_mag(&vec, &mag); - - s = sin(mag); - c = cos(mag); - omc = 1 - c; - - m->x.x = c + go_sq(uvec.x) * omc; - m->y.x = -uvec.z * s + uvec.x * uvec.y * omc; - m->z.x = uvec.y * s + uvec.x * uvec.z * omc; - - m->x.y = uvec.z * s + uvec.y * uvec.x * omc; - m->y.y = c + go_sq(uvec.y) * omc; - m->z.y = -uvec.x * s + uvec.y * uvec.z * omc; - - m->x.z = -uvec.y * s + uvec.z * uvec.x * omc; - m->y.z = uvec.x * s + uvec.z * uvec.y * omc; - m->z.z = c + go_sq(uvec.z) * omc; - - return GO_RESULT_OK; -} - -int go_rvec_zyz_convert(const go_rvec * rvec, go_zyz * zyz) -{ - go_mat mat; - int retval; - - retval = go_rvec_mat_convert(rvec, &mat); - if (GO_RESULT_OK != retval) return retval; - - return go_mat_zyz_convert(&mat, zyz); -} - -int go_rvec_zyx_convert(const go_rvec * rvec, go_zyx * zyx) -{ - go_mat mat; - int retval; - - retval = go_rvec_mat_convert(rvec, &mat); - if (GO_RESULT_OK != retval) return retval; - - return go_mat_zyx_convert(&mat, zyx); -} - -int go_rvec_rpy_convert(const go_rvec * r, go_rpy * rpy) -{ - go_quat q; - int retval; - - retval = go_rvec_quat_convert(r, &q); - if (GO_RESULT_OK != retval) return retval; - - return go_quat_rpy_convert(&q, rpy); -} - -int go_quat_rvec_convert(const go_quat * q, go_rvec * r) -{ - go_real sh; - go_real mag; - - sh = sqrt(go_sq(q->x) + go_sq(q->y) + go_sq(q->z)); - - if (GO_ROT_SMALL(sh)) { - r->x = 0; - r->y = 0; - r->z = 0; - } else { - mag = 2 * atan2(sh, q->s) / sh; - r->x = mag * q->x; - r->y = mag * q->y; - r->z = mag * q->z; - } - - return GO_RESULT_OK; -} - -int go_quat_mat_convert(const go_quat * q, go_mat * m) -{ - /* from space book where e1=q->x e2=q->y e3=q->z e4=q->s */ - m->x.x = 1 - 2 * (go_sq(q->y) + go_sq(q->z)); - m->y.x = 2 * (q->x * q->y - q->z * q->s); - m->z.x = 2 * (q->z * q->x + q->y * q->s); - - m->x.y = 2 * (q->x * q->y + q->z * q->s); - m->y.y = 1 - 2 * (go_sq(q->z) + go_sq(q->x)); - m->z.y = 2 * (q->y * q->z - q->x * q->s); - - m->x.z = 2 * (q->z * q->x - q->y * q->s); - m->y.z = 2 * (q->y * q->z + q->x * q->s); - m->z.z = 1 - 2 * (go_sq(q->x) + go_sq(q->y)); - - return GO_RESULT_OK; -} - -int go_quat_zyz_convert(const go_quat * q, go_zyz * zyz) -{ - go_mat m; - int retval; - - retval = go_quat_mat_convert(q, &m); - if (GO_RESULT_OK != retval) return retval; - return go_mat_zyz_convert(&m, zyz); -} - -int go_quat_zyx_convert(const go_quat * q, go_zyx * zyx) -{ - go_mat m; - int retval; - - retval = go_quat_mat_convert(q, &m); - if (GO_RESULT_OK != retval) return retval; - return go_mat_zyx_convert(&m, zyx); -} - -int go_quat_rpy_convert(const go_quat * q, go_rpy * rpy) -{ - go_mat m; - int retval; - - retval = go_quat_mat_convert(q, &m); - if (GO_RESULT_OK != retval) return retval; - return go_mat_rpy_convert(&m, rpy); -} - -int go_mat_rvec_convert(const go_mat * m, go_rvec * r) -{ - go_quat q; - int retval; - - retval = go_mat_quat_convert(m, &q); - if (GO_RESULT_OK != retval) return retval; - - return go_quat_rvec_convert(&q, r); -} - -/* - from space book: - - e1 = (c32 - c23) / 4*e4 - e2 = (c13 - c31) / 4*e4 - e3 = (c21 - c12) / 4*e4 - e4 = sqrt(1 + c11 + c22 + c33) / 2 - - if e4 == 0 - e1 = sqrt(1 + c11 - c33 - c22) / 2 - e2 = sqrt(1 + c22 - c33 - c11) / 2 - e3 = sqrt(1 + c33 - c11 - c22) / 2 - - to determine whether to take the positive or negative sqrt value - since e4 == 0 indicates a 180* rotation then (0 x y z) = (0 -x -y -z). - Thus some generalities can be used: - 1) find which of e1, e2, or e3 has the largest magnitude and leave it pos - 2) if e1 is largest then - if c21 < 0 then take the negative for e2 - if c31 < 0 then take the negative for e3 - 3) else if e2 is largest then - if c21 < 0 then take the negative for e1 - if c32 < 0 then take the negative for e3 - 4) else if e3 is larger then - if c31 < 0 then take the negative for e1 - if c32 < 0 then take the negative for e2 - - Note: c21 in the space book is m.x.y in this C code -*/ -int go_mat_quat_convert(const go_mat * m, go_quat * q) -{ - go_real discr; - go_real a; - - if (! go_mat_is_norm(m)) { - return GO_RESULT_NORM_ERROR; - } - - discr = 1.0 + m->x.x + m->y.y + m->z.z; - if (discr < 0.0) discr = 0.0; /* give sqrt some slack for tiny negs */ - - q->s = 0.5 * sqrt(discr); - - if (GO_ROT_SMALL(q->s)) { - q->s = 0; - discr = 1.0 + m->x.x - m->y.y - m->z.z; - if (discr < 0.0) discr = 0.0; - q->x = sqrt(discr) / 2.0; - discr = 1.0 + m->y.y - m->x.x - m->z.z; - if (discr < 0.0) discr = 0.0; - q->y = sqrt(discr) / 2.0; - discr = 1.0 + m->z.z - m->y.y - m->x.x; - if (discr < 0.0) discr = 0.0; - q->z = sqrt(discr) / 2.0; - - if (q->x > q->y && q->x > q->z) { - if (m->x.y < 0.0) { - q->y *= -1; - } - if (m->x.z < 0.0) { - q->z *= -1; - } - } else if (q->y > q->z) { - if (m->x.y < 0.0) { - q->x *= -1; - } - if (m->y.z < 0.0) { - q->z *= -1; - } - } else { - if (m->x.z < 0.0) { - q->x *= -1; - } - if (m->y.z < 0.0) { - q->y *= -1; - } - } - } else { - q->x = (m->y.z - m->z.y) / (a = 4 * q->s); - q->y = (m->z.x - m->x.z) / a; - q->z = (m->x.y - m->y.x) / a; - } - - return go_quat_norm(q, q); -} - -int go_mat_zyz_convert(const go_mat * m, go_zyz * zyz) -{ - zyz->y = atan2(sqrt(go_sq(m->x.z) + go_sq(m->y.z)), m->z.z); - - if (GO_ROT_SMALL(zyz->y)) { - zyz->z = 0; - zyz->y = 0; /* force Y to 0 */ - zyz->zp = atan2(-m->y.x, m->x.x); - } else if (GO_ROT_CLOSE(zyz->y, GO_PI)) { - zyz->z = 0; - zyz->y = GO_PI; /* force Y to 180 */ - zyz->zp = atan2(m->y.x, -m->x.x); - } else { - zyz->z = atan2(m->z.y, m->z.x); - zyz->zp = atan2(m->y.z, -m->x.z); - } - - return GO_RESULT_OK; -} - -int go_mat_zyx_convert(const go_mat * m, go_zyx * zyx) -{ - zyx->y = atan2(-m->x.z, sqrt(go_sq(m->x.x) + go_sq(m->x.y))); - - if (GO_ROT_CLOSE(zyx->y, GO_PI_2)) { - zyx->z = 0; - zyx->y = GO_PI_2; /* force it */ - zyx->x = atan2(m->y.x, m->y.y); - } else if (GO_ROT_CLOSE(zyx->y, GO_PI_2)) { - zyx->z = 0; - zyx->y = -GO_PI_2; /* force it */ - zyx->x = -atan2(m->y.z, m->y.y); - } else { - zyx->z = atan2(m->x.y, m->x.x); - zyx->x = atan2(m->y.z, m->z.z); - } - - return GO_RESULT_OK; -} - -int go_mat_rpy_convert(const go_mat * m, go_rpy * rpy) -{ - rpy->p = atan2(-m->x.z, sqrt(go_sq(m->x.x) + go_sq(m->x.y))); - - if (GO_ROT_CLOSE(rpy->p, GO_PI_2)) { - rpy->r = atan2(m->y.x, m->y.y); - rpy->p = GO_PI_2; /* force it */ - rpy->y = 0; - } else if (GO_ROT_CLOSE(rpy->p, GO_PI_2)) { - rpy->r = -atan2(m->y.z, m->y.y); - rpy->p = -GO_PI_2; /* force it */ - rpy->y = 0; - } else { - rpy->r = atan2(m->y.z, m->z.z); - rpy->y = atan2(m->x.y, m->x.x); - } - - return GO_RESULT_OK; -} - -int go_zyz_rvec_convert(const go_zyz * zyz, go_rvec * r) -{ - go_mat m; - int retval; - - retval = go_zyz_mat_convert(zyz, &m); - if (GO_RESULT_OK != retval) return retval; - return go_mat_rvec_convert(&m, r); -} - -int go_zyz_quat_convert(const go_zyz * zyz, go_quat * q) -{ - go_mat m; - int retval; - - retval = go_zyz_mat_convert(zyz, &m); - if (GO_RESULT_OK != retval) return retval; - return go_mat_quat_convert(&m, q); -} - -int go_zyz_mat_convert(const go_zyz * zyz, go_mat * m) -{ - go_real sa, sb, sg; - go_real ca, cb, cg; - - sa = sin(zyz->z); - sb = sin(zyz->y); - sg = sin(zyz->zp); - - ca = cos(zyz->z); - cb = cos(zyz->y); - cg = cos(zyz->zp); - - m->x.x = ca * cb * cg - sa * sg; - m->y.x = -ca * cb * sg - sa * cg; - m->z.x = ca * sb; - - m->x.y = sa * cb * cg + ca * sg; - m->y.y = -sa * cb * sg + ca * cg; - m->z.y = sa * sb; - - m->x.z = -sb * cg; - m->y.z = sb * sg; - m->z.z = cb; - - return GO_RESULT_OK; -} - -int go_zyz_zyx_convert(const go_zyz * zyz, go_zyx * zyx) -{ - go_mat mat; - int retval; - - retval = go_zyz_mat_convert(zyz, &mat); - if (GO_RESULT_OK != retval) return retval; - return go_mat_zyx_convert(&mat, zyx); -} - -int go_zyz_rpy_convert(const go_zyz * zyz, go_rpy * rpy) -{ - go_mat mat; - int retval; - - retval = go_zyz_mat_convert(zyz, &mat); - if (GO_RESULT_OK != retval) return retval; - - return go_mat_rpy_convert(&mat, rpy); -} - -int go_zyx_rvec_convert(const go_zyx * zyx, go_rvec * r) -{ - go_mat mat; - int retval; - - retval = go_zyx_mat_convert(zyx, &mat); - if (GO_RESULT_OK != retval) return retval; - - return go_mat_rvec_convert(&mat, r); -} - - -int go_zyx_quat_convert(const go_zyx * zyx, go_quat * q) -{ - go_mat mat; - int retval; - - retval = go_zyx_mat_convert(zyx, &mat); - if (GO_RESULT_OK != retval) return retval; - - return go_mat_quat_convert(&mat, q); -} - -int go_zyx_mat_convert(const go_zyx * zyx, go_mat * m) -{ - go_real sa, sb, sg; - go_real ca, cb, cg; - - sa = sin(zyx->z); - sb = sin(zyx->y); - sg = sin(zyx->x); - - ca = cos(zyx->z); - cb = cos(zyx->y); - cg = cos(zyx->x); - - m->x.x = ca * cb; - m->y.x = ca * sb * sg - sa * cg; - m->z.x = ca * sb * cg + sa * sg; - - m->x.y = sa * cb; - m->y.y = sa * sb * sg + ca * cg; - m->z.y = sa * sb * cg - ca * sg; - - m->x.z = -sb; - m->y.z = cb * sg; - m->z.z = cb * cg; - - return GO_RESULT_OK; -} - -int go_zyx_zyz_convert(const go_zyx * zyx, go_zyz * zyz) -{ - go_mat mat; - int retval; - - retval = go_zyx_mat_convert(zyx, &mat); - if (GO_RESULT_OK != retval) return retval; - return go_mat_zyz_convert(&mat, zyz); -} - -int go_zyx_rpy_convert(const go_zyx * zyx, go_rpy * rpy) -{ - go_mat mat; - int retval; - - retval = go_zyx_mat_convert(zyx, &mat); - if (GO_RESULT_OK != retval) return retval; - return go_mat_rpy_convert(&mat, rpy); -} - -int go_rpy_rvec_convert(const go_rpy * rpy, go_rvec * rvec) -{ - go_quat quat; - int retval; - - retval = go_rpy_quat_convert(rpy, &quat); - if (GO_RESULT_OK != retval) return retval; - return go_quat_rvec_convert(&quat, rvec); -} - -int go_rpy_quat_convert(const go_rpy * rpy, go_quat * quat) -{ - go_mat mat; - int retval; - - retval = go_rpy_mat_convert(rpy, &mat); - if (GO_RESULT_OK != retval) return retval; - return go_mat_quat_convert(&mat, quat); -} - -int go_rpy_mat_convert(const go_rpy * rpy, go_mat * m) -{ - go_real sa, sb, sg; - go_real ca, cb, cg; - - sa = sin(rpy->y); - sb = sin(rpy->p); - sg = sin(rpy->r); - - ca = cos(rpy->y); - cb = cos(rpy->p); - cg = cos(rpy->r); - - m->x.x = ca * cb; - m->y.x = ca * sb * sg - sa * cg; - m->z.x = ca * sb * cg + sa * sg; - - m->x.y = sa * cb; - m->y.y = sa * sb * sg + ca * cg; - m->z.y = sa * sb * cg - ca * sg; - - m->x.z = -sb; - m->y.z = cb * sg; - m->z.z = cb * cg; - - return GO_RESULT_OK; -} - -int go_rpy_zyz_convert(const go_rpy * rpy, go_zyz * zyz) -{ - go_mat mat; - int retval; - - retval = go_rpy_mat_convert(rpy, &mat); - if (GO_RESULT_OK != retval) return retval; - return go_mat_zyz_convert(&mat, zyz); -} - -int go_rpy_zyx_convert(const go_rpy * rpy, go_zyx * zyx) -{ - go_mat mat; - int retval; - - retval = go_rpy_mat_convert(rpy, &mat); - if (GO_RESULT_OK != retval) return retval; - return go_mat_zyx_convert(&mat, zyx); -} - -go_pose go_pose_this(go_real x, go_real y, go_real z, - go_real rs, go_real rx, go_real ry, go_real rz) -{ - go_pose pose; - - pose.tran.x = x, pose.tran.y = y, pose.tran.z = z; - pose.rot.s = rs, pose.rot.x = rx, pose.rot.y = ry, pose.rot.z = rz; - - return pose; -} - -go_cart go_cart_zero(void) -{ - go_cart cart; - - cart.x = 0, cart.y = 0, cart.z = 0; - - return cart; -} - -go_quat go_quat_identity(void) -{ - go_quat quat; - - quat.s = 1, quat.x = 0, quat.y = 0, quat.z = 0; - - return quat; -} - -go_pose go_pose_identity(void) -{ - go_pose pose; - - pose.tran.x = 0, pose.tran.y = 0, pose.tran.z = 0; - pose.rot.s = 1, pose.rot.x = 0, pose.rot.y = 0, pose.rot.z = 0; - - return pose; -} - -int go_pose_hom_convert(const go_pose * p, go_hom * h) -{ - h->tran = p->tran; - - return go_quat_mat_convert(&p->rot, &h->rot); -} - -int go_hom_pose_convert(const go_hom * h, go_pose * p) -{ - p->tran = h->tran; - - return go_mat_quat_convert(&h->rot, &p->rot); -} - -int go_cart_rvec_convert(const go_cart * cart, go_rvec * rvec) -{ - rvec->x = cart->x; - rvec->y = cart->y; - rvec->z = cart->z; - - return GO_RESULT_OK; -} - -int go_rvec_cart_convert(const go_rvec * rvec, go_cart * cart) -{ - cart->x = rvec->x; - cart->y = rvec->y; - cart->z = rvec->z; - - return GO_RESULT_OK; -} - -/* go_cart functions */ - -go_flag go_cart_cart_compare(const go_cart * v1, const go_cart * v2) -{ - if (GO_TRAN_CLOSE(v1->x, v2->x) && - GO_TRAN_CLOSE(v1->y, v2->y) && - GO_TRAN_CLOSE(v1->z, v2->z)) { - return 1; - } - - return 0; -} - -int go_cart_cart_dot(const go_cart * v1, const go_cart * v2, - go_real * d) -{ - *d = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z; - - return GO_RESULT_OK; -} - -int go_cart_cart_cross(const go_cart * v1, const go_cart * v2, - go_cart * vout) -{ - go_cart cp1, cp2; - - cp1 = *v1; - cp2 = *v2; - - vout->x = cp1.y * cp2.z - cp1.z * cp2.y; - vout->y = cp1.z * cp2.x - cp1.x * cp2.z; - vout->z = cp1.x * cp2.y - cp1.y * cp2.x; - - return GO_RESULT_OK; -} - -int go_cart_mag(const go_cart * v, go_real * d) -{ - *d = sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)); - - return GO_RESULT_OK; -} - -int go_cart_magsq(const go_cart * v, go_real * d) -{ - *d = go_sq(v->x) + go_sq(v->y) + go_sq(v->z); - - return GO_RESULT_OK; -} - -go_flag go_cart_cart_par(const go_cart *v1, const go_cart *v2) -{ - go_real dot; - go_real magsq1, magsq2; - - /* - parallel if v1 dot v2 = mag v1 * mag v2, or more efficiently, - sq(v1 dot v2) = magsq v1 * magsq v2 - */ - - go_cart_cart_dot(v1, v2, &dot); - go_cart_magsq(v1, &magsq1); - go_cart_magsq(v2, &magsq2); - - return GO_TRAN_CLOSE(dot*dot, magsq1*magsq2); -} - -go_flag go_cart_cart_perp(const go_cart *v1, const go_cart *v2) -{ - go_real dot; - - /* perpendicular if v1 dot v2 is 0 */ - - go_cart_cart_dot(v1, v2, &dot); - - return GO_TRAN_SMALL(dot); -} - -int go_cart_cart_disp(const go_cart * v1, const go_cart * v2, - go_real * d) -{ - *d = sqrt(go_sq(v2->x - v1->x) + - go_sq(v2->y - v1->y) + - go_sq(v2->z - v1->z)); - - return GO_RESULT_OK; -} - -int go_cart_cart_add(const go_cart * v1, const go_cart * v2, - go_cart * vout) -{ - vout->x = v1->x + v2->x; - vout->y = v1->y + v2->y; - vout->z = v1->z + v2->z; - - return GO_RESULT_OK; -} - -int go_cart_cart_sub(const go_cart * v1, const go_cart * v2, - go_cart * vout) -{ - vout->x = v1->x - v2->x; - vout->y = v1->y - v2->y; - vout->z = v1->z - v2->z; - - return GO_RESULT_OK; -} - -int go_cart_scale_mult(const go_cart * v1, go_real d, go_cart * vout) -{ - vout->x = v1->x * d; - vout->y = v1->y * d; - vout->z = v1->z * d; - - return GO_RESULT_OK; -} - -int go_cart_neg(const go_cart * v1, go_cart * vout) -{ - vout->x = -v1->x; - vout->y = -v1->y; - vout->z = -v1->z; - - return GO_RESULT_OK; -} - -int go_cart_unit(const go_cart * v, go_cart * vout) -{ - go_real size = sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)); - - if (GO_TRAN_SMALL(size)) { - vout->x = GO_INF; - vout->y = GO_INF; - vout->z = GO_INF; - - return GO_RESULT_NORM_ERROR; - } - - size = 1.0 / size; - - vout->x = v->x * size; - vout->y = v->y * size; - vout->z = v->z * size; - - return GO_RESULT_OK; -} - -int go_cart_is_norm(const go_cart * v) -{ - return GO_TRAN_CLOSE(sqrt(go_sq(v->x) + go_sq(v->y) + go_sq(v->z)), 1); -} - -int go_cart_cart_rot(const go_cart * v1, - const go_cart * v2, - go_quat * quat) -{ - int retval; - go_cart u1; - go_cart u2; - go_cart cross; - go_real mag; - go_real dot; - go_real th; - go_rvec rvec; - - /* unitize the input vectors */ - retval = go_cart_unit(v1, &u1); - if (GO_RESULT_OK != retval) return retval; - retval = go_cart_unit(v2, &u2); - if (GO_RESULT_OK != retval) return retval; - /* and cross the unit vectors to get the mutual normal */ - (void) go_cart_cart_cross(&u1, &u2, &cross); - /* the magnitude of the mutual normal is sin theta */ - (void) go_cart_mag(&cross, &mag); - th = asin(mag); - /* dot them to get parallel (1) or antiparallel (-1) */ - (void) go_cart_cart_dot(&u1, &u2, &dot); - /* make the cross a unit vector so we can theta-ize it */ - retval = go_cart_unit(&cross, &cross); - - /* handle aligned vectors */ - if (GO_RESULT_OK != retval) { - /* v1 and v2 are aligned, so th will be zero */ - if (dot > 0.0) { /* would be -1 or 1 */ - /* parallel */ - quat->s = 1.0; - quat->x = quat->y = quat->z = 0.0; - return GO_RESULT_OK; - } - /* else antiparallel - set u2 to be normal to u1, - rotate about it PI rads */ - retval = go_cart_normal(&u1, &u2); - if (GO_RESULT_OK != retval) return retval; - rvec.x = GO_PI * u2.x; - rvec.y = GO_PI * u2.y; - rvec.z = GO_PI * u2.z; - return go_rvec_quat_convert(&rvec, quat); - } - - /* else not aligned */ - if (dot < 0.0) { - th = GO_PI - th; - } - rvec.x = th * cross.x; - rvec.y = th * cross.y; - rvec.z = th * cross.z; - return go_rvec_quat_convert(&rvec, quat); -} - -int go_cart_cart_proj(const go_cart * v1, const go_cart * v2, - go_cart * vout) -{ - go_cart v2u; - go_real d; - int retval; - - retval = go_cart_unit(v2, &v2u); - if (GO_RESULT_OK != retval) return retval; - - retval = go_cart_cart_dot(v1, &v2u, &d); - if (GO_RESULT_OK != retval) return retval; - - return go_cart_scale_mult(&v2u, d, vout); -} - -int go_cart_plane_proj(const go_cart * v, const go_cart * normal, - go_cart * vout) -{ - go_cart par; - int retval; - - retval = go_cart_cart_proj(v, normal, &par); - if (GO_RESULT_OK != retval) return retval; - - return go_cart_cart_sub(v, &par, vout); -} - -int go_cart_cart_angle(const go_cart * v1, const go_cart * v2, - go_real * a) -{ - go_real dot, m1, m2; - - go_cart_cart_dot(v1, v2, &dot); - go_cart_mag(v1, &m1); - go_cart_mag(v2, &m2); - - if (m1 <= 0. || m2 <= 0.) { - return GO_RESULT_DIV_ERROR; - } - - dot = dot / (m1 * m2); - if (dot > 1.) - dot = 1.; - else if (dot < -1.) - dot = -1.; - - *a = acos(dot); - - return GO_RESULT_OK; -} - -int go_cart_normal(const go_cart * v, go_cart * vout) -{ - go_cart cart; - go_real min, ymin; - - /* pick the X, Y or Z vector that is most perpendicular to 'v' */ - cart.x = 1, cart.y = 0, cart.z = 0; /* start with X */ - min = fabs(v->x); - ymin = fabs(v->y); - if (ymin < min) { - min = ymin; /* Y is more perp */ - cart.x = 0, cart.y = 1, cart.z = 0; - } - if (fabs(v->z) < min) { - cart.x = 0, cart.y = 0, cart.z = 1; - } - - /* cross the most perp axis vector with 'v' to get a real perp */ - go_cart_cart_cross(v, &cart, &cart); - - /* unitize it into 'vout' and return */ - return go_cart_unit(&cart, vout); -} - -int go_cart_centroid(const go_cart * varray, - go_integer num, - go_cart * centroid) -{ - go_integer i; - - if (num < 1) return GO_RESULT_BAD_ARGS; - - centroid->x = varray[0].x; - centroid->y = varray[0].y; - centroid->z = varray[0].z; - - for (i = 1; i < num; i++) { - centroid->x += varray[i].x; - centroid->y += varray[i].y; - centroid->z += varray[i].z; - } - - return go_cart_scale_mult(centroid, 1.0/((go_real) num), centroid); -} - -int go_cart_centroidize(const go_cart * vinarray, - go_integer num, - go_cart * centroid, - go_cart * voutarray) -{ - go_integer i; - int retval; - - retval = go_cart_centroid(vinarray, num, centroid); - if (GO_RESULT_OK != retval) return retval; - - for (i = 0; i < num; i++) { - (void) go_cart_cart_sub(&vinarray[i], centroid, &voutarray[i]); - } - - return GO_RESULT_OK; -} - -/* - Another Solution of the Cubic Equation Here is another method of - solving a cubic polynomial equation submitted independently by Paul - A. Torres and Robert A. Warren. It is based on the idea of "completing - the cube," by arranging matters so that three of the four terms are - three of the four terms of a perfect cube. - - Start with the cubic equation - - x3 + ax2 + bx + c = 0. - - If a2 - 3b = 0, then the first three terms are the first three terms - of a perfect cube, namely (x+a/3)3. Then you can "complete the cube" - by subtracting c from both sides and adding the missing term of the - cube a3/27 to both sides. Recalling that b = a2/3, you get - - x3 + ax2 + bx + c = 0, - x3 + ax2 + (a2/3)x + c = 0, - x3 + ax2 + (a2/3)x = -c, - x3 + ax2 + (a2/3)x + a3/27 = a3/27 - c, - (x+a/3)3 = a3/27 - c. - - By taking the cube root of the left side and the three cube roots of - the right side, you get - - x = -a/3 + (a3/27 - c)1/3, - x = -a/3 + (a3/27 - c)1/3(-1+sqrt[-3])/2, - x = -a/3 + (a3/27 - c)1/3(-1-sqrt[-3])/2. - - These are the roots of the cubic equation that were sought. - - If a2 - 3b is nonzero, then proceed as follows. Set x = y + z, where y - is an indeterminate and z is a function of a, b, and c, which will be - found below. Then - - (y+z)3 + a(y+z)2 + b(y+z) + c = 0, - y3 + (3z+a)y2 + (3z2+2az+b)y + (z3+az2+bz+c) = 0, - y3 + dy2 + ey + f = 0, - - where - - d = 3z + a, - e = 3z2 + 2az + b, - f = z3 + az2 + bz + c. - - The first three terms of this equation in y will be those of a perfect - cube if and only if d2 - 3e is zero, which happens if and only if a2 - - 3b = 0, which cannot happen in this case, so we seemingly haven't - gained anything. However, the last three terms of this equation in y - will be those of a perfect cube if and only if e2 = 3df, that is if - and only if - - (3z2+2az+b)2 = 3(3z+a)(z3+az2+bz+c), - (a2-3b)z2 + (ab-9c)z + (b2-3ac) = 0, - gz2 + hz + i = 0, - - where - - g = a2 - 3b, - h = ab - 9c, - i = b2 - 3ac. - - Since a2 - 3b is nonzero, g is nonzero, and we have a true quadratic - equation, called the resolvent quadratic. Now we pick z to be a root - of this quadratic equation. If the GCD of z2 + (h/g)z + (i/g) and z3 + - az2 + bz + c as polynomials in z is not 1, then any root of the GCD is - also a root of the original cubic equation in x. Once you have at - least one root, the problem of finding the other roots is reduced to - solving a quadratic or linear equation. If the GCD is 1, then neither - value of z can make f = 0, so we can assume henceforth that f is - nonzero. Either root z of the quadratic will do, but we must choose - one of them. We arbitrarily pick the one with a plus sign in front of - the radical, - - z = (-h+sqrt[h2-4gi])/(2g), - = (9c-ab+sqrt[81c2-54abc+12a3c+12b3-3a2b2])/(2[a2-3b]). - - Set z equal to this value in the equation for y, and divide it by f on - both sides. Then the last three terms of the cubic in y are those of a - perfect cube, namely (ey/[3f]+1)3, so we can complete the cube to - solve it. We do this by subtracting y3/f from both sides, then adding - the missing term of the cubic, (ey/[3f])3 to both sides, obtaining - - (ey/[3f]+1)3 = ([e/(3f)]3-1/f)y3, - - ey/(3f) + 1 = y([e/(3f)]3-1/f)1/3, - ey/(3f) + 1 = y([e/(3f)]3-1/f)1/3(-1+sqrt[-3])/2, - ey/(3f) + 1 = y([e/(3f)]3-1/f)1/3(-1-sqrt[-3])/2, - - y = 3f/[-e+(e3-27f2)1/3], - y = 3f/[-e+(e3-27f2)1/3(-1+sqrt[-3])/2], - y = 3f/[-e+(e3-27f2)1/3(-1-sqrt[-3])/2]. - - Now you have the values of y. Add z to each to get the values of x: - - x = z + 3f/[(e3-27f2)1/3-e], - x = z + 3f/[(e3-27f2)1/3(-1+sqrt[-3])/2-e], - x = z + 3f/[(e3-27f2)1/3(-1-sqrt[-3])/2-e]. - - These are the roots of the cubic equation that were sought. - - Example: Solve x3 + 6x2 + 9x + 6 = 0. - - a = 6, b = 9, c = 6. - - Then - - d = 3z + 6, e = 3z2 + 12z + 9, f = z3 + 6z2 + 9z + 6. - - Then - - g = a2 - 3b = 36 - 27 = 9, - h = ab - 9c = 54 - 54 = 0, - i = b2 - 3ac = 81 - 108 = -27, - - and the resolvent quadratic is - - 9z2 - 27 = 0, - z2 - 3 = 0, - z = sqrt(3). - - Then - - d = 6 + 3 sqrt(3), - e = 18 + 12 sqrt(3), - f = 24 + 12 sqrt(3), - - and the cubic in y is - - y3 + (6+3 sqrt[3])y2 + (18+12 sqrt[3])y + (24+12 sqrt[3]) = 0. - - Then one root is - - y = 3f/[(e3-27f2)1/3-e], - = 3(24+12 sqrt[3])/([18+12 sqrt(3)]3-27[24+12 sqrt(3)]2)1/3-18-12 sqrt[3]), - = (12+6 sqrt[3])/([9+6 sqrt(3)]1/3-3-2 sqrt[3]), - = (6+4 sqrt[3])/([2+sqrt(3)]1/3-2-sqrt[3]). - - After a lot of simplification, you get - - y = -2 - sqrt(3) - (2-sqrt[3])1/3 - (2+sqrt[3])1/3, - x = -2 - (2-sqrt[3])1/3 - (2+sqrt[3])1/3 - - (and two other roots). -*/ - -go_complex go_complex_add(go_complex z1, go_complex z2) -{ - go_complex sum; - - sum.re = z1.re + z2.re; - sum.im = z1.im + z2.im; - - return sum; -} - -go_complex go_complex_sub(go_complex z1, go_complex z2) -{ - go_complex diff; - - diff.re = z1.re - z2.re; - diff.im = z1.im - z2.im; - - return diff; -} - -go_complex go_complex_mult(go_complex z1, go_complex z2) -{ - go_complex prod; - - prod.re = (z1.re * z2.re) - (z1.im * z2.im); - prod.im = (z1.re * z2.im) + (z1.im * z2.re); - - return prod; -} - -go_complex go_complex_div(go_complex z1, go_complex z2, int * result) -{ - go_complex z2c = {0, 0}; - go_real denom; - - denom = go_sq(z2.re) + go_sq(z2.im); - if (denom < GO_REAL_EPSILON) { - *result = GO_RESULT_DIV_ERROR; - return z2c; - } - - z2c.re = z2.re, z2c.im = -z2.im; /* complex conjugate */ - *result = GO_RESULT_OK; - return go_complex_scale(go_complex_mult(z1, z2c), 1.0 / denom); -} - -go_complex go_complex_scale(go_complex z, go_real scale) -{ - go_complex s; - - s.re = scale * z.re; - s.im = scale * z.im; - - return s; -} - -go_real go_complex_mag(go_complex z) -{ - return sqrt(go_sq(z.re) + go_sq(z.im)); -} - -go_real go_complex_arg(go_complex z) -{ - return atan2(z.im, z.re); -} - -/* - go_complex_sqrt() and go_complex_cbrt() are structured so that if - the roots of complex conjugates are taken, the first roots z1 will - be complex conjugates. This is due to atan2 returning args in the - range -PI to PI. - */ - -void go_complex_sqrt(go_complex z, go_complex * z1, go_complex * z2) -{ - go_real r, th; - - r = sqrt(go_complex_mag(z)); - th = 0.5 * go_complex_arg(z); - - z1->re = r * cos(th); - z1->im = r * sin(th); - if (NULL != z2) { - z2->re = -z1->re; - z2->im = -z1->im; - } - - return; -} - -void go_complex_cbrt(go_complex z, go_complex * z1, go_complex * z2, go_complex * z3) -{ - go_real r, th; - - r = go_cbrt(go_complex_mag(z)); - th = 1.0/3.0 * go_complex_arg(z); - - z1->re = r * cos(th); - z1->im = r * sin(th); - if (NULL != z2) { - z2->re = r * cos(th + 2.0/3.0 * GO_PI); - z2->im = r * sin(th + 2.0/3.0 * GO_PI); - } - if (NULL != z3) { - z3->re = r * cos(th + 4.0/3.0 * GO_PI); - z3->im = r * sin(th + 4.0/3.0 * GO_PI); - } - - return; -} - -int go_quadratic_solve(const go_quadratic * quad, - go_complex * z1, - go_complex * z2) -{ - go_real discr; - - /* check if b is 0 and we can reduce order */ - if (GO_SMALL(quad->b)) { - z2->re = z2->im = 0.0; - z1->re = -quad->a; - z1->im = 0.0; - return GO_RESULT_OK; - } - - discr = go_sq(quad->a) - 4.0 * quad->b; - if (discr < 0.0) { - discr = sqrt(-discr); - z1->re = z2->re = -0.5 * quad->a; - z1->im = 0.5 * discr; - z2->im = -0.5 * discr; - } else { - discr = sqrt(discr); - z1->re = 0.5 * (-quad->a + discr); - z2->re = 0.5 * (-quad->a - discr); - z1->im = z2->im = 0.0; - } - - return GO_RESULT_OK; -} - -/* - References on solving the cubic: - - "Solving Cubic and Quartic Polynomials," - www.karlscalculus.org/pdf/cubicQuartic.pdf - - "Cubic Equations in One Formula," - http://mathforum.org/library/drmath/view/52668.html -*/ -int go_cubic_solve(const go_cubic * cub, - go_complex * z1, - go_complex * z2, - go_complex * z3) -{ - go_quadratic quad; - go_real a, b; - go_complex z, A, B, I = {0.0, 1.0}; - go_complex u1, u2, u3; - - /* check if c is 0, and we have a root of 0 and can reduce order - to a quadratic */ - if (GO_SMALL(cub->c)) { - z3->re = z3->im = 0.0; - quad.a = cub->a; - quad.b = cub->b; - return go_quadratic_solve(&quad, z1, z2); - } - - a = cub->b - 1.0/3.0 * go_sq(cub->a); - b = cub->c - 1.0/3.0 * cub->a * cub->b + 2.0/27.0 * go_cub(cub->a); - z.re = 0.25 * go_sq(b) + 1.0/27.0 * go_cub(a), z.im = 0.0; - go_complex_sqrt(z, &A, &B); - - /* since z is real, the pair of sqrt(z) is either pure real or - pure complex */ - A.re -= 0.5 * b; - B.re -= 0.5 * b; - /* subtracting b/2 keeps them pure real and in general unequal, or - makes them complex conjugates */ - - /* Now we need to take the cube root of each. If they're pure real, - we can take the scalar cube root. */ - if (GO_SMALL(A.im)) { - A.re = go_cbrt(A.re), A.im = 0.0; - B.re = go_cbrt(B.re), B.im = 0.0; - } else { - go_complex_cbrt(A, &A, NULL, NULL); - B.re = A.re, B.im = -A.im; /* B and A must be complex conjugates */ - } - u1 = go_complex_add(A, B); - u2 = u3 = go_complex_scale(u1, -0.5); - z = go_complex_scale(go_complex_sub(A, B), sqrt(0.75)); - z = go_complex_mult(I, z); - u2 = go_complex_add(u2, z); - u3 = go_complex_sub(u3, z); - - z1->re = u1.re - 1.0/3.0 * cub->a, z1->im = u1.im; - z2->re = u2.re - 1.0/3.0 * cub->a, z2->im = u2.im; - z3->re = u3.re - 1.0/3.0 * cub->a, z3->im = u3.im; - - return GO_RESULT_OK; -} - -/* - http://mathforum.org/dr.math/faq/faq.cubic.equations.html - - Solving the quartic with no third-order term: - - The quartic in y must factor into two quadratics with real - coefficients, since any complex roots must occur in conjugate - pairs. Write - - y4 + e y2 + f y + g = (y2 + h y + j) (y2 - h y + g/j) - - Since f and g aren't zero, neither j nor h is zero either. Without - loss of generality, we can assume h > 0 (otherwise swap the two - factors). Then, equating coefficients of y2 and y, - - e = g/j + j - h2, f = h (g/j - j). - - so - - g/j + j = e + h2, g/j - j = f/h. - - Adding and subtracting these two equations, - - 2 g/j = e + h2 + f/h, 2 j = e + h2 - f/h. - - Multiplying these together, - - 4 g = e2 + 2 e h2 + h4 - f2/h2. - - Rearranging, - - h6 + 2 e h4 + (e2-4 g) h2 - f2 = 0. - - This is a cubic equation in h2, with known coefficients, since we know - e, f, and g. We can use the solution for cubic equations shown above - to find a positive real value of h2, whose existence is guaranteed by - Descartes' Rule of Signs, and then take its positive square root. This - value of h will give a value of j from 2 j = e + h2 - f/h. Once you - know h and j, you know the quadratic factors of the quartic in - y. Notice that you do not need all the roots h2 of the cubic, and that - any positive real one will do. Further notice that h2 = 4 z from the - previous section. - - Once you have factored the quartic into two quadratics, finishing the - finding of the roots is simple, using the Quadratic Formula. Once the - roots y are found, the corresponding x's are gotten from x = y - a/4. -*/ - -int go_quartic_solve(const go_quartic * quart, - go_complex * z1, - go_complex * z2, - go_complex * z3, - go_complex * z4) -{ - go_cubic cub; - go_quadratic quad; - go_real a2, a3, a4; - go_real e, f, g; - go_complex p, q, r, fc, a4c; - int retval; - - /* - There are several ways to solve quartic equations. They all start by - dividing the equation by the leading coefficient, to make it of the - form - - x4 + a x3 + b x2 + c x + d = 0. - - There is an easy special case if d = 0, in which case you can factor - the quartic into x times a cubic. The roots then are 0 and the roots - of that cubic. (To solve the cubic equation, see above.) - */ - if (GO_SMALL(quart->d)) { - /* set one root to be 0 and solve for reduced cubic */ - z4->re = z4->im = 0.0; - cub.a = quart->a; - cub.b = quart->b; - cub.c = quart->c; - return go_cubic_solve(&cub, z1, z2, z3); - } - - /* - First eliminate the third-order term. Substitute x = y - a/4, - expand, and simplify, to get - - y4 + e y2 + f y + g = 0 - - where - - e = b - 3 a2/8, - f = c + a3/8 - a b/2, - g = d - 3 a4/256 + a2 b/16 - a c/4. - */ - a2 = go_sq(quart->a); - e = quart->b - 3.0/8.0 * a2; - a3 = a2 * quart->a; - f = quart->c + 1.0/8.0 * a3 - 0.5 * (quart->a * quart->b); - a4 = a2 * a2; - g = quart->d - 3.0/256.0 * a4 + 1.0/16.0 * a2 * quart->b - 0.25 * (quart->a * quart->c); - /* - At this point, there are two useful special cases. - - 1. If g = 0, then, again, you can factor the quartic into y times - a cubic. The roots of the original equation are then x = -a/4 - and the roots of that cubic with a/4 subtracted from each. - - 2. If f = 0, then the quartic in y is actually a quadratic - equation in the variable y2. Solve this using your favorite - method, and then take the two square roots of each of the - solutions for y2 to find the four values of y which - work. Subtract a/4 from each to get the four roots x. - - Henceforth we can assume that d, f, and g are all nonzero. - */ - /* Case 1 */ - if (GO_SMALL(g)) { - a4 = -0.25 * quart->a; /* reuse a4 as -a/4, save a var */ - z4->re = a4; - z4->im = 0.0; - cub.a = 0.0; - cub.b = e; - cub.c = f; - retval = go_cubic_solve(&cub, z1, z2, z3); - if (GO_RESULT_OK != retval) return retval; - z1->re += a4; - z2->re += a4; - z3->re += a4; - return GO_RESULT_OK; - } - /* Case 2 */ - if (GO_SMALL(f)) { - a4 = -0.25 * quart->a; /* reuse a4 as -a/4, save a var */ - quad.a = e; - quad.b = g; - retval = go_quadratic_solve(&quad, z1, z3); - if (GO_RESULT_OK != retval) return retval; - go_complex_sqrt(*z1, z1, z2); - go_complex_sqrt(*z3, z3, z4); - z1->re += a4; - z2->re += a4; - z3->re += a4; - z4->re += a4; - return GO_RESULT_OK; - } - - /* - Here is one way to proceed from this point. The related auxiliary - cubic equation - - z3 + (e/2) z2 + ((e2-4 g)/16) z - f2/64 = 0 - - has three roots. Find those roots by solving this cubic (again, - see above). None of these is zero because f isn't zero. Let p and - q be the square roots of two of those roots (any choices of roots - and signs will work), and set - - r = -f/(8 p q). - - Then p2, q2, and r2 are the three roots of the above cubic. More - important is the fact that the four roots of the original quartic - are - - x = p + q + r - a/4, - x = p - q - r - a/4, - x = -p + q - r - a/4, and - x = -p - q + r - a/4. - - This solution was discovered by Leonhard Euler (1707-1783). - */ - cub.a = 0.5 * e; - cub.b = 1.0/16.0 * (go_sq(e) - 4.0 * g); - cub.c = -1.0/64.0 * go_sq(f); - retval = go_cubic_solve(&cub, z1, z2, z3); - if (GO_RESULT_OK != retval) return retval; - go_complex_sqrt(*z1, &p, NULL); - go_complex_sqrt(*z2, &q, NULL); - fc.re = f, fc.im = 0.0; /* fc is complex of scalar f */ - r = go_complex_div(fc, go_complex_scale(go_complex_mult(p, q), -8.0), &retval); - if (GO_RESULT_OK != retval) return retval; - - a4c.re = 0.25 * quart->a, a4c.im = 0.0; /* a/4 as a complex */ - *z1 = go_complex_sub(go_complex_add(go_complex_add(p, q), r), a4c); - *z2 = go_complex_sub(go_complex_sub(go_complex_sub(p, q), r), a4c); - *z3 = go_complex_sub(go_complex_sub(go_complex_sub(q, p), r), a4c); - *z4 = go_complex_sub(go_complex_sub(go_complex_sub(r, p), q), a4c); - - return GO_RESULT_OK; -} - -#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) -#define MAG2(a,b) sqrt((a)*(a)+(b)*(b)) - -/*! - This computes the Householder reduction to tridiagonal form of a - real symmetric matrix \a a. \a a will contain the orthogonal matrix - that effects the transformation. \a d will contain the diagonal - elements, and \a e will contain the off-diagonal elements. This is - used by go_tridiag_ql to find the eigenvalues and eigenvectors of a - real symmetric matrix. - */ -int go_tridiag_reduce(go_real ** a, /*< the real symmetric matrix, - overwritten with the diagonalizing - matrix */ - go_integer n, /*< how many rows and columns */ - go_real * d, /*< the diagonal elements will be - stored here */ - go_real * e) /*< the off-diagonal elements - will be stored here, starting - at index 1 */ -{ - go_integer l,k,j,i; - go_real scale,hh,h,g,f; - - for (i=n-1;i>0;i--) { - l=i-1; - h=scale=0.0; - if (l > 0) { - for (k=0;k<=l;k++) - scale += fabs(a[i][k]); - if (scale == 0.0) - e[i]=a[i][l]; - else { - for (k=0;k<=l;k++) { - a[i][k] /= scale; - h += a[i][k]*a[i][k]; - } - f=a[i][l]; - g=(f >= 0.0 ? -sqrt(h) : sqrt(h)); - e[i]=scale*g; - h -= f*g; - a[i][l]=f-g; - f=0.0; - for (j=0;j<=l;j++) { - a[j][i]=a[i][j]/h; - g=0.0; - for (k=0;k<=j;k++) - g += a[j][k]*a[i][k]; - for (k=j+1;k<=l;k++) - g += a[k][j]*a[i][k]; - e[j]=g/h; - f += e[j]*a[i][j]; - } - hh=f/(h+h); - for (j=0;j<=l;j++) { - f=a[i][j]; - e[j]=g=e[j]-hh*f; - for (k=0;k<=j;k++) - a[j][k] -= (f*e[k]+g*a[i][k]); - } - } - } else - e[i]=a[i][l]; - d[i]=h; - } - d[0]=0.0; - e[0]=0.0; - for (i=0;i=l;i--) { - f=s*e[i]; - b=c*e[i]; - e[i+1]=(r=MAG2(f,g)); - if (r == 0.0) { - d[i+1] -= p; - e[m]=0.0; - break; - } - s=f/r; - c=g/r; - g=d[i+1]-p; - r=(d[i]-g)*s+2.0*c*b; - d[i+1]=g+(p=s*r); - g=c*r-b; - for (k=0;k= l) continue; - d[l] -= p; - e[l]=g; - e[m]=0.0; - } - } while (m != l); - } - - return GO_RESULT_OK; -} - -int go_cart_cart_pose(const go_cart * v1, const go_cart * v2, - go_cart * v1c, go_cart * v2c, - go_integer num, go_pose * p) -{ - go_integer t; - go_cart c1, c2; - go_real Sxx, Sxy, Sxz; - go_real Syx, Syy, Syz; - go_real Szx, Szy, Szz; - GO_MATRIX_DECLARE(N, Nspace, 4, 4); - go_real d[4], e[4]; - go_real eigenval; - int retval; - - memset(&Nspace,0,sizeof(Nspace)); - - Sxx = Sxy = Sxz = 0.0; - Syx = Syy = Syz = 0.0; - Szx = Szy = Szz = 0.0; - - go_matrix_init(N, Nspace, 4, 4); - - retval = go_cart_centroidize(v1, num, &c1, v1c); - if (GO_RESULT_OK != retval) return retval; - retval = go_cart_centroidize(v2, num, &c2, v2c); - if (GO_RESULT_OK != retval) return retval; - - for (t = 0; t < num; t++) { - Sxx += v1c[t].x * v2c[t].x; - Sxy += v1c[t].x * v2c[t].y; - Sxz += v1c[t].x * v2c[t].z; - - Syx += v1c[t].y * v2c[t].x; - Syy += v1c[t].y * v2c[t].y; - Syz += v1c[t].y * v2c[t].z; - - Szx += v1c[t].z * v2c[t].x; - Szy += v1c[t].z * v2c[t].y; - Szz += v1c[t].z * v2c[t].z; - } - - N.el[0][0] = Sxx + Syy + Szz; - N.el[0][1] = N.el[1][0] = Syz - Szy; - N.el[0][2] = N.el[2][0] = Szx - Sxz; - N.el[0][3] = N.el[3][0] = Sxy - Syx; - - N.el[1][1] = Sxx - Syy - Szz; - N.el[1][2] = N.el[2][1] = Sxy + Syx; - N.el[1][3] = N.el[3][1] = Szx + Sxz; - - N.el[2][2] = -Sxx + Syy - Szz; - N.el[2][3] = N.el[3][2] = Syz + Szy; - - N.el[3][3] = -Sxx -Syy + Szz; - - /* compute eigenvectors */ - retval = go_tridiag_reduce(N.el, 4, d, e); - if (GO_RESULT_OK != retval) return retval; - retval = go_tridiag_ql(d, e, 4, N.el); - if (GO_RESULT_OK != retval) return retval; - - /* pick eigenvector associated with most positive eigenvalue */ - eigenval = d[0], t = 0; - if (d[1] > eigenval) eigenval = d[1], t = 1; - if (d[2] > eigenval) eigenval = d[2], t = 2; - if (d[3] > eigenval) eigenval = d[3], t = 3; - /* now t is the column with the eigenvector we want as our quaternion */ - p->rot.s = N.el[0][t]; - p->rot.x = N.el[1][t]; - p->rot.y = N.el[2][t]; - p->rot.z = N.el[3][t]; - retval = go_quat_norm(&p->rot, &p->rot); - if (GO_RESULT_OK != retval) return retval; - - /* rotate left centroid, subtract right from left to get translation */ - (void) go_quat_cart_mult(&p->rot, &c1, &c1); - (void) go_cart_cart_sub(&c2, &c1, &p->tran); - - return GO_RESULT_OK; -} - -static int trilaterate(go_real x2, go_real x3, go_real y3, - go_real l1, go_real l2, go_real l3, - go_cart * p) -{ - go_real discr; - - p->x = 0.5 * (x2 - (go_sq(l2) - go_sq(l1))/x2); - - p->y = - (go_sq(l1) - 2.0 * p->x * x3 + go_sq(x3) + - go_sq(y3) - go_sq(l3)) / (2.0 * y3); - - discr = go_sq(l1) - go_sq(p->x) - go_sq(p->y); - if (discr < -GO_REAL_EPSILON) { - return GO_RESULT_DOMAIN_ERROR; - } - if (discr < 0.0) discr = 0.0; /* make slightly negative numbers 0 */ - - /* return z positive, caller can make negative if they want the - point below the base */ - p->z = sqrt(discr); - - return GO_RESULT_OK; -} - -int go_cart_trilaterate(const go_cart * c1, - const go_cart * c2, - const go_cart * c3, - go_real l1, - go_real l2, - go_real l3, - go_cart * out1, - go_cart * out2) -{ - go_pose P_to_B, B_to_P; - go_mat mat; - go_cart diff, proj; - go_cart p1, p2, p3, out1_in_P, out2_in_P; - int retval; - - /* transform the points from their original base frame {B} into an - easier frame {P}, with c1 at the {P}, origin, c2 along the {P} x - axis, and all three in the {P} xy plane */ - - /* first get the transform */ - P_to_B.tran = *c1; - go_cart_cart_sub(c2, c1, &diff); - retval = go_cart_unit(&diff, &mat.x); - if (GO_RESULT_OK != retval) return retval; - go_cart_cart_sub(c3, c1, &diff); - retval = go_cart_cart_proj(&diff, &mat.x, &proj); - if (GO_RESULT_OK != retval) return retval; - go_cart_cart_sub(&diff, &proj, &diff); - retval = go_cart_unit(&diff, &mat.y); - if (GO_RESULT_OK != retval) return retval; - go_cart_cart_cross(&mat.x, &mat.y, &mat.z); - retval = go_mat_quat_convert(&mat, &P_to_B.rot); - if (GO_RESULT_OK != retval) return retval; - retval = go_pose_inv(&P_to_B, &B_to_P); - if (GO_RESULT_OK != retval) return retval; - /* now transform them */ - go_pose_cart_mult(&B_to_P, c1, &p1); - go_pose_cart_mult(&B_to_P, c2, &p2); - go_pose_cart_mult(&B_to_P, c3, &p3); - - /* trilaterate to get the point in {P} */ - retval = trilaterate(p2.x, p3.x, p3.y, l1, l2, l3, &out1_in_P); - if (GO_RESULT_OK != retval) return retval; - out2_in_P.x = out1_in_P.x; - out2_in_P.y = out1_in_P.y; - out2_in_P.z = -out1_in_P.z; /* the other one has a minus z */ - go_pose_cart_mult(&P_to_B, &out1_in_P, out1); - go_pose_cart_mult(&P_to_B, &out2_in_P, out2); - - return GO_RESULT_OK; -} - -/* go_rvec functions */ - -go_flag go_rvec_rvec_compare(const go_rvec * r1, const go_rvec * r2) -{ - if (GO_ROT_CLOSE(r1->x, r2->x) && - GO_ROT_CLOSE(r1->y, r2->y) && - GO_ROT_CLOSE(r1->z, r2->z)) { - return 1; - } - - return 0; -} - -int go_rvec_scale_mult(const go_rvec * r, go_real s, go_rvec * rout) -{ - rout->x = r->x * s; - rout->y = r->y * s; - rout->z = r->z * s; - - return GO_RESULT_OK; -} - -/* go_mat functions */ - -int go_mat_norm(const go_mat * mat, go_mat * mout) -{ - go_cart yprojx; - int retval; - - /* unitize the X vector, which we must be able to do */ - retval = go_cart_unit(&mat->x, &mout->x); - if (GO_RESULT_OK != retval) return retval; - - /* project Y onto X, subtract from Y to get Y perp to X, and unitize */ - retval = go_cart_cart_proj(&mat->y, &mout->x, &yprojx); - if (GO_RESULT_OK != retval) return retval; - go_cart_cart_sub(&mat->y, &yprojx, &mout->y); - retval = go_cart_unit(&mout->y, &mout->y); - if (GO_RESULT_OK != retval) return retval; - - /* cross the new X and Y to get Z */ - return go_cart_cart_cross(&mout->x, &mout->y, &mout->z); -} - -go_flag go_mat_is_norm(const go_mat * m) -{ - go_cart u; - - go_cart_cart_cross(&m->x, &m->y, &u); - -#define COL_IS_UNIT(r) GO_TRAN_CLOSE(go_sq((r).x) + go_sq((r).y) + go_sq((r).z), 1) - return (COL_IS_UNIT(m->x) && - COL_IS_UNIT(m->y) && - COL_IS_UNIT(m->z) && - go_cart_cart_compare(&u, &m->z)); -#undef COL_IS_UNIT -} - -int go_mat_inv(const go_mat * m, go_mat * mout) -{ - go_real cp; - - /* inverse of a rotation matrix is the transpose */ - - mout->x.x = m->x.x; - mout->y.y = m->y.y; - mout->z.z = m->z.z; - - cp = m->x.y; - mout->x.y = m->y.x; - mout->y.x = cp; - - cp = m->x.z; - mout->x.z = m->z.x; - mout->z.x = cp; - - cp = m->y.z; - mout->y.z = m->z.y; - mout->z.y = cp; - - return GO_RESULT_OK; -} - -int go_mat_cart_mult(const go_mat * m, const go_cart * v, - go_cart * vout) -{ - go_cart cp; - - cp = *v; - - vout->x = m->x.x * cp.x + m->y.x * cp.y + m->z.x * cp.z; - vout->y = m->x.y * cp.x + m->y.y * cp.y + m->z.y * cp.z; - vout->z = m->x.z * cp.x + m->y.z * cp.y + m->z.z * cp.z; - - return GO_RESULT_OK; -} - -int go_mat_mat_mult(const go_mat * m1, const go_mat * m2, go_mat * mout) -{ - go_mat cp1, cp2; - - cp1 = *m1; - cp2 = *m2; - - mout->x.x = cp1.x.x * cp2.x.x + cp1.y.x * cp2.x.y + cp1.z.x * cp2.x.z; - mout->x.y = cp1.x.y * cp2.x.x + cp1.y.y * cp2.x.y + cp1.z.y * cp2.x.z; - mout->x.z = cp1.x.z * cp2.x.x + cp1.y.z * cp2.x.y + cp1.z.z * cp2.x.z; - - mout->y.x = cp1.x.x * cp2.y.x + cp1.y.x * cp2.y.y + cp1.z.x * cp2.y.z; - mout->y.y = cp1.x.y * cp2.y.x + cp1.y.y * cp2.y.y + cp1.z.y * cp2.y.z; - mout->y.z = cp1.x.z * cp2.y.x + cp1.y.z * cp2.y.y + cp1.z.z * cp2.y.z; - - mout->z.x = cp1.x.x * cp2.z.x + cp1.y.x * cp2.z.y + cp1.z.x * cp2.z.z; - mout->z.y = cp1.x.y * cp2.z.x + cp1.y.y * cp2.z.y + cp1.z.y * cp2.z.z; - mout->z.z = cp1.x.z * cp2.z.x + cp1.y.z * cp2.z.y + cp1.z.z * cp2.z.z; - - return GO_RESULT_OK; -} - -/* go_quat functions */ - -go_flag go_quat_quat_compare(const go_quat * q1, const go_quat * q2) -{ - if (GO_ROT_CLOSE(q1->s, q2->s) && - GO_TRAN_CLOSE(q1->x, q2->x) && - GO_TRAN_CLOSE(q1->y, q2->y) && - GO_TRAN_CLOSE(q1->z, q2->z)) { - return 1; - } - - /* note (0, x, y, z) = (0, -x, -y, -z) */ - if (! GO_ROT_SMALL(q1->s) || - GO_TRAN_CLOSE(q1->x, -q2->x) || - GO_TRAN_CLOSE(q1->y, -q2->y) || - GO_TRAN_CLOSE(q1->z, -q2->z)) { - return 0; - } - - return 1; -} - -int go_quat_mag(const go_quat * quat, go_real * d) -{ - go_real sh; - - sh = sqrt(go_sq(quat->x) + go_sq(quat->y) + go_sq(quat->z)); - - *d = 2 * atan2(sh, quat->s); - - return GO_RESULT_OK; -} - -int go_quat_unit(const go_quat * q1, go_quat * qout) -{ - go_real d; - - (void) go_quat_mag(q1, &d); - if (GO_SMALL(d)) return GO_RESULT_ERROR; - d = 1.0 / d; - - return go_quat_scale_mult(q1, d, qout); -} - -int go_quat_norm(const go_quat * q1, go_quat * qout) -{ - go_real size; - - size = sqrt(go_sq(q1->s) + go_sq(q1->x) + go_sq(q1->y) + go_sq(q1->z)); - - if (GO_ROT_SMALL(size)) { - qout->s = 1; - qout->x = 0; - qout->y = 0; - qout->z = 0; - return GO_RESULT_NORM_ERROR; - } - size = 1.0 / size; - - if (q1->s >= 0) { - qout->s = q1->s * size; - qout->x = q1->x * size; - qout->y = q1->y * size; - qout->z = q1->z * size; - - return GO_RESULT_OK; - } else { - qout->s = -q1->s * size; - qout->x = -q1->x * size; - qout->y = -q1->y * size; - qout->z = -q1->z * size; - - return GO_RESULT_OK; - } -} - -int go_quat_inv(const go_quat * q1, go_quat * qout) -{ - qout->s = q1->s; - qout->x = -q1->x; - qout->y = -q1->y; - qout->z = -q1->z; - - if (!go_quat_is_norm(q1)) { - return GO_RESULT_NORM_ERROR; - } - - return GO_RESULT_OK; -} - -go_flag go_quat_is_norm(const go_quat * q1) -{ - return GO_TRAN_CLOSE(go_sq(q1->s) + - go_sq(q1->x) + - go_sq(q1->y) + - go_sq(q1->z), 1); -} - -int go_quat_scale_mult(const go_quat * q, go_real s, go_quat * qout) -{ - go_real sh; /* sine of half angle */ - go_real ha; /* half angle */ - go_real scale; /* new sh / old sh */ - - sh = sqrt(go_sq(q->x) + go_sq(q->y) + go_sq(q->z)); - if (GO_SMALL(sh)) { - /* zero rotation-- leave it alone */ - *qout = *q; - return GO_RESULT_OK; - } - - ha = atan2(sh, q->s); - ha *= s; - scale = sin(ha) / sh; - - qout->s = cos(ha); - qout->x = scale * q->x; - qout->y = scale * q->y; - qout->z = scale * q->z; - - return GO_RESULT_OK; -} - -int go_quat_quat_mult(const go_quat * q1, const go_quat * q2, - go_quat * qout) -{ - go_quat cp1, cp2; - - if (!go_quat_is_norm(q1) || !go_quat_is_norm(q2)) { - return GO_RESULT_NORM_ERROR; - } - - cp1 = *q1; - cp2 = *q2; - - qout->s = cp1.s * cp2.s - cp1.x * cp2.x - cp1.y * cp2.y - cp1.z * cp2.z; - - if (qout->s >= 0) { - qout->x = cp1.s * cp2.x + cp1.x * cp2.s + cp1.y * cp2.z - cp1.z * cp2.y; - qout->y = cp1.s * cp2.y - cp1.x * cp2.z + cp1.y * cp2.s + cp1.z * cp2.x; - qout->z = cp1.s * cp2.z + cp1.x * cp2.y - cp1.y * cp2.x + cp1.z * cp2.s; - } else { - qout->s = -qout->s; - qout->x = -cp1.s * cp2.x - cp1.x * cp2.s - cp1.y * cp2.z + cp1.z * cp2.y; - qout->y = -cp1.s * cp2.y + cp1.x * cp2.z - cp1.y * cp2.s - cp1.z * cp2.x; - qout->z = -cp1.s * cp2.z - cp1.x * cp2.y + cp1.y * cp2.x - cp1.z * cp2.s; - } - - return GO_RESULT_OK; -} - -int go_quat_cart_mult(const go_quat * q1, const go_cart * v2, - go_cart * vout) -{ - go_cart c; - - if (!go_quat_is_norm(q1)) { - return GO_RESULT_NORM_ERROR; - } - - c.x = q1->y * v2->z - q1->z * v2->y; - c.y = q1->z * v2->x - q1->x * v2->z; - c.z = q1->x * v2->y - q1->y * v2->x; - - vout->x = v2->x + 2 * (q1->s * c.x + q1->y * c.z - q1->z * c.y); - vout->y = v2->y + 2 * (q1->s * c.y + q1->z * c.x - q1->x * c.z); - vout->z = v2->z + 2 * (q1->s * c.z + q1->x * c.y - q1->y * c.x); - - return GO_RESULT_OK; -} - -/* go_pose functions*/ - -go_flag go_pose_pose_compare(const go_pose * p1, const go_pose * p2) -{ - return (go_quat_quat_compare(&p1->rot, &p2->rot) && - go_cart_cart_compare(&p1->tran, &p2->tran)); -} - -int go_pose_inv(const go_pose * p1, go_pose * p2) -{ - int retval; - - retval = go_quat_inv(&p1->rot, &p2->rot); - if (GO_RESULT_OK != retval) return retval; - - retval = go_quat_cart_mult(&p2->rot, &p1->tran, &p2->tran); - if (GO_RESULT_OK != retval) return retval; - - p2->tran.x = -p2->tran.x; - p2->tran.y = -p2->tran.y; - p2->tran.z = -p2->tran.z; - - return GO_RESULT_OK; -} - -int go_pose_cart_mult(const go_pose * p1, const go_cart * v2, go_cart * vout) -{ - int retval; - - /* first rotate the vector */ - retval = go_quat_cart_mult(&p1->rot, v2, vout); - if (GO_RESULT_OK != retval) return retval; - - /* then translate it */ - return go_cart_cart_add(&p1->tran, vout, vout); -} - -int go_pose_pose_mult(const go_pose * p1, const go_pose * p2, go_pose * pout) -{ - go_pose out; - int retval; - - retval = go_quat_cart_mult(&p1->rot, &p2->tran, &out.tran); - if (GO_RESULT_OK != retval) return retval; - - retval = go_cart_cart_add(&p1->tran, &out.tran, &out.tran); - if (GO_RESULT_OK != retval) return retval; - - retval = go_quat_quat_mult(&p1->rot, &p2->rot, &out.rot); - - *pout = out; - - return retval; -} - -int go_pose_scale_mult(const go_pose * p1, go_real s, go_pose * pout) -{ - (void) go_cart_scale_mult(&p1->tran, s, &pout->tran); - return go_quat_scale_mult(&p1->rot, s, &pout->rot); -} - -int go_pose_pose_interp(go_real t1, const go_pose * p1, go_real t2, const go_pose * p2, go_real t3, go_pose * p3) -{ - go_pose pdiff; - int retval; - - if (GO_CLOSE(t1, t2)) return GO_RESULT_ERROR; - - retval = go_pose_inv(p1, &pdiff); - if (GO_RESULT_OK != retval) return retval; - - (void) go_pose_pose_mult(&pdiff, p2, &pdiff); - (void) go_pose_scale_mult(&pdiff, (t3 - t1) / (t2 - t1), &pdiff); - return go_pose_pose_mult(p1, &pdiff, p3); -} - -/* homogeneous transform functions */ - -int go_hom_inv(const go_hom * h1, go_hom * h2) -{ - int retval; - - retval = go_mat_inv(&h1->rot, &h2->rot); - if (GO_RESULT_OK != retval) return retval; - - retval = go_mat_cart_mult(&h2->rot, &h1->tran, &h2->tran); - if (GO_RESULT_OK != retval) return retval; - - h2->tran.x = -h2->tran.x; - h2->tran.y = -h2->tran.y; - h2->tran.z = -h2->tran.z; - - return GO_RESULT_OK; -} - -int go_pose_screw_mult(const go_pose * pose, const go_screw * screw, go_screw * out) -{ - go_pose poseinv; - go_cart wxp; - go_cart v; - int retval; - - /* - B |B . B | - .T = | R . P | - A |A . A| - - B B A A A - .v = R ( v + w X P ) - . A B - - B B A - .w = R w - . A - */ - - retval = go_pose_inv(pose, &poseinv); - if (GO_RESULT_OK != retval) return retval; - - go_cart_cart_cross(&screw->w, &poseinv.tran, &wxp); - go_cart_cart_add(&screw->v, &wxp, &v); - go_quat_cart_mult(&pose->rot, &v, &out->v); - go_quat_cart_mult(&pose->rot, &screw->w, &out->w); - - return GO_RESULT_OK; -} - -/* line and plane functions */ - -int go_line_from_poGO_RESULT_direction(const go_cart * point, const go_cart * direction, go_line * line) -{ - int retval; - - retval = go_cart_unit(direction, &line->direction); - if (GO_RESULT_OK != retval) return GO_RESULT_DIV_ERROR; - - line->point = *point; - - return GO_RESULT_OK; -} - -int go_line_from_points(const go_cart * point1, const go_cart * point2, go_line * line) -{ - go_cart direction; - int retval; - - (void) go_cart_cart_sub(point2, point1, &direction); - retval = go_cart_unit(&direction, &line->direction); - if (GO_RESULT_OK != retval) return GO_RESULT_DIV_ERROR; - - line->point = *point1; - - return GO_RESULT_OK; -} - -int go_line_from_planes(const go_plane * plane1, const go_plane * plane2, go_line * line) -{ - enum {X, Y, Z} which; - go_real max, ymax, denominv; - - (void) go_cart_cart_cross(&plane1->normal, &plane2->normal, &line->direction); - if (GO_RESULT_OK != go_cart_unit(&line->direction, &line->direction)) { - /* planes are parallel */ - return GO_RESULT_ERROR; - } - - /* pick the cross product component with the largest magnitude, - which will give the most robust calculations for the point - on the line */ - max = fabs(line->direction.x); - which = X; - ymax = fabs(line->direction.y); - if (ymax > max) { - max = ymax; - which = Y; - } - if (fabs(line->direction.z) > max) { - which = Z; - } - - switch (which) { - case X: - denominv = 1.0/(plane1->normal.y*plane2->normal.z - - plane2->normal.y*plane1->normal.z); - line->point.y = (plane1->normal.z*plane2->d - - plane2->normal.z*plane1->d)*denominv; - line->point.z = (plane2->normal.y*plane1->d - - plane1->normal.y*plane2->d)*denominv; - line->point.x = 0; - break; - case Y: - denominv = 1.0/(plane1->normal.z*plane2->normal.x - - plane2->normal.z*plane1->normal.x); - line->point.z = (plane1->normal.x*plane2->d - - plane2->normal.x*plane1->d)*denominv; - line->point.x = (plane2->normal.z*plane1->d - - plane1->normal.z*plane2->d)*denominv; - line->point.y = 0; - break; - default: - denominv = 1.0/(plane1->normal.x*plane2->normal.y - - plane2->normal.x*plane1->normal.y); - line->point.x = (plane1->normal.y*plane2->d - - plane2->normal.y*plane1->d)*denominv; - line->point.y = (plane2->normal.x*plane1->d - - plane1->normal.x*plane2->d)*denominv; - line->point.z = 0; - break; - } - - return GO_RESULT_OK; -} - -go_flag go_line_line_compare(const go_line * line1, const go_line * line2) -{ - go_cart diff; - go_real dot; - - /* check that directions are parallel */ - go_cart_cart_dot(&line1->direction, &line2->direction, &dot); - if (! GO_CLOSE(dot, 1)) return 0; - - /* check that points are along the direction */ - (void) go_cart_cart_sub(&line1->point, &line2->point, &diff); - go_cart_cart_dot(&line1->direction, &diff, &dot); - if (! GO_CLOSE(dot, 1)) return 0; - - return 1; -} - -int go_line_evaluate(const go_line * line, go_real d, go_cart * point) -{ - go_cart v; - - (void) go_cart_scale_mult(&line->direction, d, &v); - (void) go_cart_cart_add(&line->point, &v, point); - - return GO_RESULT_OK; -} - -int go_poGO_RESULT_line_distance(const go_cart * point, const go_line * line, go_real * distance) -{ - *distance = sqrt( - go_sq(line->direction.z * (point->y - line->point.y) - - line->direction.y * (point->z - line->point.z)) + - go_sq(line->direction.x * (point->z - line->point.z) - - line->direction.z * (point->x - line->point.x)) + - go_sq(line->direction.y * (point->x - line->point.x) - - line->direction.x * (point->y - line->point.y))); - - return GO_RESULT_OK; -} - -int go_poGO_RESULT_line_proj(const go_cart * point, const go_line * line, go_cart * pout) -{ - go_cart vp; - int retval; - - go_cart_cart_sub(point, &line->point, &vp); - retval = go_cart_cart_proj(&vp, &line->direction, &vp); - if (GO_RESULT_OK != retval) return retval; - go_cart_cart_add(&line->point, &vp, pout); - - return GO_RESULT_OK; -} - -int go_poGO_RESULT_plane_proj(const go_cart * point, const go_plane * plane, go_cart * proj) -{ - go_real denom; - go_real k; - - denom = - go_sq(plane->normal.x) + - go_sq(plane->normal.y) + - go_sq(plane->normal.z); - - if (GO_TRAN_SMALL(denom)) return GO_RESULT_DIV_ERROR; - - k = -(plane->normal.x * point->x + - plane->normal.y * point->y + - plane->normal.z * point->z + - plane->d)/denom; - - proj->x = point->x + k*plane->normal.x; - proj->y = point->y + k*plane->normal.y; - proj->z = point->z + k*plane->normal.z; - - return GO_RESULT_OK; -} - -int go_line_plane_proj(const go_line * line, const go_plane * plane, go_line * proj) -{ - int retval; - - retval = go_cart_plane_proj(&line->direction, &plane->normal, &proj->direction); - if (GO_RESULT_OK != retval) return retval; - - return go_poGO_RESULT_plane_proj(&line->point, plane, &proj->point); -} - -int go_plane_from_poGO_RESULT_normal(const go_cart * point, const go_cart * normal, go_plane * plane) -{ - if (GO_RESULT_OK != go_cart_unit(normal, &plane->normal)) return GO_RESULT_ERROR; - /* D = -(Ax + By + Cz) */ - plane->d = -(plane->normal.x * point->x + - plane->normal.y * point->y + - plane->normal.z * point->z); - - return GO_RESULT_OK; -} - -int go_plane_from_abcd(go_real A, go_real B, go_real C, go_real D, go_plane * plane) -{ - go_real mag; - - /* get magnitude of normal vector, actually mag squared */ - mag = go_sq(A) + go_sq(B) + go_sq(C); - if (GO_TRAN_SMALL(mag)) { - return GO_RESULT_DIV_ERROR; - } - mag = 1.0 / sqrt(mag); /* now it's the mag multiplier */ - - plane->normal.x = A * mag; - plane->normal.y = B * mag; - plane->normal.z = C * mag; - plane->d = D * mag; - - return GO_RESULT_OK; -} - -int go_plane_from_points(const go_cart * point1, const go_cart * point2, const go_cart * point3, go_plane * plane) -{ - go_cart v12, v23; - - /* cross vectors from 1-2, 2-3 to get normal */ - go_cart_cart_sub(point2, point1, &v12); - go_cart_cart_sub(point3, point2, &v23); - go_cart_cart_cross(&v12, &v23, &plane->normal); - if (GO_RESULT_OK != go_cart_unit(&plane->normal, &plane->normal)) return GO_RESULT_ERROR; - /* D = -(Ax + By + Cz) */ - plane->d = -(plane->normal.x * point1->x + - plane->normal.y * point1->y + - plane->normal.z * point1->z); - - return GO_RESULT_OK; -} - -int go_plane_from_poGO_RESULT_line(const go_cart * point, const go_line * line, go_plane * plane) -{ - go_cart p0, p1; - - (void) go_line_evaluate(line, 0, &p0); - (void) go_line_evaluate(line, 1, &p1); - - return go_plane_from_points(&p0, &p1, point, plane); -} - -/* for planes to be the same, they must have the same normal vector - and same 'd', so the comparison is simple */ -go_flag go_plane_plane_compare(const go_plane * plane1, const go_plane * plane2) -{ - if (! go_cart_cart_compare(&plane1->normal, &plane2->normal)) return 0; - return GO_CLOSE(plane1->d, plane2->d); -} - -int go_poGO_RESULT_plane_distance(const go_cart * point, const go_plane * plane, go_real * distance) -{ - *distance = plane->normal.x * point->x + - plane->normal.y * point->y + - plane->normal.z * point->z + - plane->d; - - return GO_RESULT_OK; -} - -int go_plane_evaluate(const go_plane * plane, go_real u, go_real v, go_cart * point) -{ - go_cart v1, v2; /* orthogonal vectors in plane */ - go_cart p; /* point in plane closest to origin */ - - if (GO_RESULT_OK != go_cart_normal(&plane->normal, &v1)) return GO_RESULT_ERROR; - - (void) go_cart_cart_cross(&plane->normal, &v1, &v2); - (void) go_cart_scale_mult(&v1, u, &v1); - (void) go_cart_scale_mult(&v2, v, &v2); - - /* with point = k * normal, - a*ka * y*ky + z*kz + d = 0, - k*(1) + d = 0, - k = -d - */ - p.x = -plane->normal.x * plane->d; - p.y = -plane->normal.y * plane->d; - p.z = -plane->normal.z * plane->d; - - (void) go_cart_cart_add(&p, &v1, &p); - (void) go_cart_cart_add(&p, &v2, point); - - return GO_RESULT_OK; -} - -/* - To get the intersect point of the line and plane, set them equal and - solve for the line parameter d, then plug into the parameterization - to get the point: - - A(px + d*vx) + B(py + d*vy) + C(pz + d*vz) + D = 0 - - _ -(A*px + B*py + C*pz + D) - d --------------------- - _ A*vx + B*vy + C*vz - - Then plug d into px + d*vx, ... to get intersect x, y and z. -*/ -int go_line_plane_intersect(const go_line * line, const go_plane * plane, go_cart * point, go_real * distance) -{ - go_real num, denom; - - /* first check for line parallel to plane */ - (void) go_cart_cart_dot(&plane->normal, &line->direction, &denom); - if (GO_SMALL(denom)) return GO_RESULT_ERROR; - - /* check for point lying in plane */ - (void) go_cart_cart_dot(&plane->normal, &line->point, &num); - num += plane->d; - if (GO_SMALL(num)) { - *point = line->point; - /* distance probably isn't exactly zero, so compute it */ - return go_poGO_RESULT_plane_distance(&line->point, plane, distance); - } - - /* else plug in d to get intersect point */ - *distance = -num / denom; - return go_line_evaluate(line, *distance, point); -} - -/* general vector and matrix functions */ - -/* - Adapted from ludcmp routine in Numerical Recipes in C, but with - indices starting at 0 and no heap allocation. - - Given a matrix a[0..n-1][0..n-1], this routine replaces it by the LU - decomposition of a rowwise permutation of itself. a and n are - input. a is changed and output. indx[0..n-1] is an output vector - that records the row permutation effected by the partial pivoting; d - is output as +/-1 depending on whether the number of row - interchanges was even or odd, respectively. This routine is used in - combination with lubksb to solve linear equations or invert a - matrix. - - Warning! The matrix 'a' can't be declared as go_real a[n][n]. It - needs to be an array of go_real pointers. See e.g. go_mat6_inv() for - how to set up the 'a' matrix. -*/ - -static go_real go_singular_epsilon = 1.0e-15; - -go_real go_get_singular_epsilon(void) -{ - return go_singular_epsilon; -} - -int go_set_singular_epsilon(go_real epsilon) -{ - if (epsilon <= 0.0) return GO_RESULT_ERROR; - - go_singular_epsilon = epsilon; - - return GO_RESULT_OK; -} - -int ludcmp(go_real ** a, - go_real * scratchrow, - go_integer n, - go_integer * indx, - go_real * d) -{ - go_integer i, imax, j, k; - go_real big, dum, sum, temp; - - *d = 1.0; - for (i = 0; i < n; i++) { - big = 0.0; - for (j = 0; j < n; j++) - if ((temp = fabs(a[i][j])) > big) - big = temp; - if (big < go_singular_epsilon) - return GO_RESULT_SINGULAR; - scratchrow[i] = 1.0 / big; - } - for (j = 0; j < n; j++) { - for (i = 0; i < j; i++) { - sum = a[i][j]; - for (k = 0; k < i; k++) - sum -= a[i][k] * a[k][j]; - a[i][j] = sum; - } - big = 0.0; - imax = 0; - for (i = j; i < n; i++) { - sum = a[i][j]; - for (k = 0; k < j; k++) - sum -= a[i][k] * a[k][j]; - a[i][j] = sum; - if ((dum = scratchrow[i] * fabs(sum)) >= big) { - big = dum; - imax = i; - } - } - if (j != imax) { - for (k = 0; k < n; k++) { - dum = a[imax][k]; - a[imax][k] = a[j][k]; - a[j][k] = dum; - } - *d = -(*d); - scratchrow[imax] = scratchrow[j]; - } - indx[j] = imax; - if (fabs(a[j][j]) < go_singular_epsilon) - return GO_RESULT_SINGULAR; - if (j != n - 1) { - dum = 1.0 / (a[j][j]); - for (i = j + 1; i < n; i++) - a[i][j] *= dum; - } - } - - return GO_RESULT_OK; -} - -/* - Solves the set of n linear equations A·X = B. Here a[0..n-1][0..n-1] - is input, not as the matrix A but rather as its LU decomposition, - determined by the routine ludcmp. indx[0..n-1] is input as the - permutation vector returned by ludcmp. b[0..n-1] is input as the - right-hand side vector B, and returns with the solution vector X. a, - n, and indx are not modified by this routine and can be left in - place for successive calls with different right-hand sides b. This - routine takes into account the possibility that b will begin with - many zero elements, so it is efficient for use in matrix inversion. - - Warning! The matrix 'a' can't be declared as go_real a[n][n]. It - needs to be an array of go_real pointers. See e.g. go_mat6_inv() for - how to set up the 'a' matrix. -*/ - -int lubksb(go_real ** a, - go_integer n, - go_integer * indx, - go_real * b) -{ - go_integer i, ii = -1, ip, j; - go_real sum; - - for (i = 0; i < n; i++) { - ip = indx[i]; - sum = b[ip]; - b[ip] = b[i]; - if (ii != -1) - for (j = ii; j <= i - 1; j++) - sum -= a[i][j] * b[j]; - else if (sum) - ii = i; - b[i] = sum; - } - for (i = n - 1; i >= 0; i--) { - sum = b[i]; - for (j = i + 1; j < n; j++) - sum -= a[i][j] * b[j]; - if (fabs(a[i][i]) < go_singular_epsilon) - return GO_RESULT_SINGULAR; - b[i] = sum / a[i][i]; - } - - return GO_RESULT_OK; -} - -int go_cart_vector_convert(const go_cart * c, - go_real * v) -{ - v[0] = c->x, v[1] = c->y, v[2] = c->z; - - return GO_RESULT_OK; -} - -int go_vector_cart_convert(const go_real * v, - go_cart * c) -{ - c->x = v[0], c->y = v[1], c->z = v[2]; - - return GO_RESULT_OK; -} - -int go_quat_matrix_convert(const go_quat * quat, - go_matrix * matrix) -{ - go_mat mat; - int retval; - - /* check for an initialized matrix */ - if (NULL == matrix->el[0]) return GO_RESULT_ERROR; - /* check for a 3x3 matrix */ - if (matrix->rows != 3 || matrix->cols != 3) return GO_RESULT_ERROR; - - retval = go_quat_mat_convert(quat, &mat); - if (GO_RESULT_OK != retval) return retval; - - return go_mat_matrix_convert(&mat, matrix); -} - -/* | m.x.x m.y.x m.z.x | */ -/* go_mat m = | m.x.y m.y.y m.z.y | */ -/* | m.x.z m.y.z m.z.z | */ -/* go_matrix mout[row][col] */ - -int go_mat_matrix_convert(const go_mat * mat, - go_matrix * matrix) -{ - /* check for an initialized matrix */ - if (NULL == matrix->el[0]) return GO_RESULT_ERROR; - /* check for a 3x3 matrix */ - if (matrix->rows != 3 || matrix->cols != 3) return GO_RESULT_ERROR; - - matrix->el[0][0] = mat->x.x, matrix->el[0][1] = mat->y.x, matrix->el[0][2] = mat->z.x; - matrix->el[1][0] = mat->x.y, matrix->el[1][1] = mat->y.y, matrix->el[1][2] = mat->z.y; - matrix->el[2][0] = mat->x.z, matrix->el[2][1] = mat->y.z, matrix->el[2][2] = mat->z.z; - - return GO_RESULT_OK; -} - -int go_matrix_matrix_add(const go_matrix * a, - const go_matrix * b, - go_matrix * apb) -{ - go_integer row, col; - - /* check for an initialized matrix */ - if (NULL == a->el[0] || NULL == b->el[0] || NULL == apb->el[0]) return GO_RESULT_ERROR; - /* check for matching rows and cols */ - if (a->rows != b->rows || a->cols != b->cols || - b->rows != apb->rows || b->cols != apb->cols) return GO_RESULT_ERROR; - - for (row = 0; row < a->rows; row++) { - for (col = 0; col < a->cols; col++) { - apb->el[row][col] = a->el[row][col] + b->el[row][col]; - } - } - - return GO_RESULT_OK; -} - -int go_matrix_matrix_copy(const go_matrix * src, - go_matrix * dst) -{ - go_integer row, col; - - /* check for an initialized matrix */ - if (NULL == src->el[0] || NULL == dst->el[0]) return GO_RESULT_ERROR; - - /* check for matching rows and cols */ - if (src->rows != dst->rows || src->cols != dst->cols) return GO_RESULT_ERROR; - - for (row = 0; row < src->rows; row++) { - for (col = 0; col < src->cols; col++) { - dst->el[row][col] = src->el[row][col]; - } - } - - return GO_RESULT_OK; -} - -int go_matrix_matrix_mult(const go_matrix * a, - const go_matrix * b, - go_matrix * ab) -{ - go_real ** ptrin; - go_real ** ptrout; - go_integer row, col, i; - - /* check for an initialized matrix */ - if (NULL == a->el[0] || NULL == b->el[0] || NULL == ab->el[0]) return GO_RESULT_ERROR; - /* check for consistent rows and cols */ - if (a->cols != b->rows || - a->rows != ab->rows || - b->cols != ab->cols) return GO_RESULT_ERROR; - - if (ab == a) { - /* destructive multiply, use a's copy space and copy back */ - ptrin = a->elcpy; - ptrout = a->el; - } else if (ab == b) { - ptrin = b->elcpy; - ptrout = b->el; - } else { - ptrin = ab->el; - ptrout = NULL; - } - - for (row = 0; row < a->rows; row++) { - for (col = 0; col < b->cols; col++) { - ptrin[row][col] = 0; - for (i = 0; i < a->cols; i++) { - ptrin[row][col] += a->el[row][i] * b->el[i][col]; - } - } - } - - if (NULL != ptrout) { - for (row = 0; row < ab->rows; row++) { - for (col = 0; col < ab->cols; col++) { - ptrout[row][col] = ptrin[row][col]; - } - } - } - - return GO_RESULT_OK; -} - -int go_matrix_vector_mult(const go_matrix * a, - const go_vector * v, - go_vector * axv) -{ - go_vector * ptrin; - go_vector * ptrout; - go_integer row, i; - - /* check for an initialized matrix */ - if (NULL == a->el[0]) return GO_RESULT_ERROR; - - if (axv == v) { - ptrin = a->elcpy[0]; - ptrout = axv; - } else { - ptrin = axv; - ptrout = NULL; - } - - for (row = 0; row < a->rows; row++) { - ptrin[row] = 0; - for (i = 0; i < a->cols; i++) { - ptrin[row] += a->el[row][i] * v[i]; - } - } - - if (ptrout != NULL) { - for (row = 0; row < a->rows; row++) { - ptrout[row] = ptrin[row]; - } - } - - return GO_RESULT_OK; -} - -/* - The matrix-vector cross product is a matrix of the same dimension, - whose columns are the column-wise cross products of the matrix - and the vector. The matrices must be 3xN, the vector 3x1. -*/ -int go_matrix_vector_cross(const go_matrix * a, - const go_vector * v, - go_matrix * axv) -{ - go_real ** ptrin; - go_real ** ptrout; - go_cart vc; /* 'v' */ - go_cart ac; /* a column of the 'a' matrix */ - go_cart cross; - go_integer row, col; - - /* check for an initialized matrix */ - if (NULL == a->el[0] || NULL == axv->el[0]) return GO_RESULT_ERROR; - /* check for consistent rows and cols */ - if (a->rows != 3 || - axv->rows != 3 || - a->cols != axv->cols) return GO_RESULT_ERROR; - - if (axv == a) { - /* destructive multiply, use a's copy space and copy back */ - ptrin = a->elcpy; - ptrout = a->el; - } else { - ptrin = axv->el; - ptrout = NULL; - } - - /* get 'v' as a cartesian type */ - vc.x = v[0], vc.y = v[1], vc.z = v[2]; - - for (col = 0; col < a->cols; col++) { - /* pick off the col'th column as a cartesian type */ - ac.x = a->el[0][col], ac.y = a->el[1][col], ac.z = a->el[2][col]; - /* cross it with v */ - go_cart_cart_cross(&ac, &vc, &cross); - /* make it the col'th column of axv[] */ - ptrin[0][col] = cross.x, ptrin[1][col] = cross.y, ptrin[2][col] = cross.z; - } - - if (ptrout != NULL) { - for (row = 0; row < a->rows; row++) { - for (col = 0; col < a->cols; col++) { - ptrout[row][col] = ptrin[row][col]; - } - } - } - - return GO_RESULT_OK; -} - -int go_matrix_transpose(const go_matrix * a, - go_matrix * at) -{ - go_real ** ptrin; - go_real ** ptrout; - go_integer row, col; - - /* check for fixed matrix */ - if (NULL == a->el[0] || NULL == at->el[0]) return GO_RESULT_ERROR; - - if (at == a) { - ptrin = a->elcpy; - ptrout = a->el; - } else { - ptrin = at->el; - ptrout = NULL; - } - - for (row = 0; row < a->rows; row++) { - for (col = 0; col < a->cols; col++) { - ptrin[col][row] = a->el[row][col]; - } - } - - if (ptrout != NULL) { - for (row = 0; row < a->rows; row++) { - for (col = 0; col < a->cols; col++) { - ptrout[row][col] = ptrin[row][col]; - } - } - } - - return GO_RESULT_OK; -} - -int go_matrix_inv(const go_matrix * m, /* M x N */ - go_matrix * minv) /* N x M */ -{ - go_real d; - go_integer N, row, col; - int retval; - - /* check for fixed matrix */ - if (NULL == m->el[0] || NULL == minv->el[0]) return GO_RESULT_ERROR; - - N = m->rows; - - /* copy of m since ludcmp destroys input matrix */ - for (row = 0; row < N; row++) { - for (col = 0; col < N; col++) { - m->elcpy[row][col] = m->el[row][col]; - } - } - - /* convert the copy to its LU decomposition */ - retval = ludcmp(m->elcpy, m->v, N, m->index, &d); - if (GO_RESULT_OK != retval) return retval; - - /* backsubstitute a column with a 1 in it to get the inverse */ - for (col = 0; col < N; col++) { - for (row = 0; row < N; row++) { - m->v[row] = 0.0; - } - m->v[col] = 1.0; - retval = lubksb(m->elcpy, N, m->index, m->v); - if (GO_RESULT_OK != retval) return retval; - for (row = 0; row < N; row++) { - minv->el[row][col] = m->v[row]; - } - } - - return GO_RESULT_OK; -} - -extern int go_mat3_inv(const go_real a[3][3], - go_real ainv[3][3]) -{ - go_real a11a22; - go_real a11a23; - go_real a11a32; - go_real a11a33; - - go_real a12a33; - go_real a12a23; - go_real a12a21; - go_real a12a31; - - go_real a13a21; - go_real a13a22; - go_real a13a31; - go_real a13a32; - - go_real a21a32; - go_real a21a33; - - go_real a22a31; - go_real a22a33; - - go_real a23a31; - go_real a23a32; - - go_real denom; - - a11a22 = a[0][0]*a[1][1]; - a11a23 = a[0][0]*a[1][2]; - a11a32 = a[0][0]*a[2][1]; - a11a33 = a[0][0]*a[2][2]; - - a12a21 = a[0][1]*a[1][0]; - a12a23 = a[0][1]*a[1][2]; - a12a31 = a[0][1]*a[2][0]; - a12a33 = a[0][1]*a[2][2]; - - a13a21 = a[0][2]*a[1][0]; - a13a22 = a[0][2]*a[1][1]; - a13a31 = a[0][2]*a[2][0]; - a13a32 = a[0][2]*a[2][1]; - - a21a32 = a[1][0]*a[2][1]; - a21a33 = a[1][0]*a[2][2]; - - a22a31 = a[1][1]*a[2][0]; - a22a33 = a[1][1]*a[2][2]; - - a23a31 = a[1][2]*a[2][0]; - a23a32 = a[1][2]*a[2][1]; - - denom = - + a11a22*a[2][2] - - a11a23*a[2][1] - - a12a33*a[1][0] - + a13a21*a[2][1] - - a13a22*a[2][0] - + a12a23*a[2][0]; - - if (GO_SMALL(denom)) { - return GO_RESULT_SINGULAR; - } - - denom = 1.0/denom; - - ainv[0][0] = (a22a33 - a23a32)*denom; - ainv[0][1] = (a13a32 - a12a33)*denom; - ainv[0][2] = (a12a23 - a13a22)*denom; - - ainv[1][0] = (a23a31 - a21a33)*denom; - ainv[1][1] = (a11a33 - a13a31)*denom; - ainv[1][2] = (a13a21 - a11a23)*denom; - - ainv[2][0] = (a21a32 - a22a31)*denom; - ainv[2][1] = (a12a31 - a11a32)*denom; - ainv[2][2] = (a11a22 - a12a21)*denom; - - return GO_RESULT_OK; -} - -int go_mat3_mat3_mult(const go_real a[3][3], - const go_real b[3][3], - go_real axb[3][3]) -{ - go_real work[3][3]; - go_integer i, j, k; - - for (i = 0; i < 3; i++) { - for (j = 0; j < 3; j++) { - work[i][j] = 0; - for (k = 0; k < 3; k++) { - work[i][j] += a[i][k] * b[k][j]; - } - } - } - - for (i = 0; i < 3; i++) - for (j = 0; j < 3; j++) - axb[i][j] = work[i][j]; - - return GO_RESULT_OK; -} - -int go_mat3_vec3_mult(const go_real a[3][3], - const go_real v[3], - go_real axv[3]) -{ - go_real work[3]; - go_integer i, j; - - for (i = 0; i < 3; i++) { - work[i] = 0; - for (j = 0; j < 3; j++) { - work[i] += a[i][j] * v[j]; - } - } - - for (i = 0; i < 3; i++) - axv[i] = work[i]; - - return GO_RESULT_OK; -} - -extern int go_mat4_inv(const go_real a[4][4], - go_real ainv[4][4]) -{ - go_real work[4][4]; - go_real denom; - - go_real a11a22; - go_real a11a23; - go_real a11a24; - go_real a11a33; - go_real a11a32; - go_real a11a34; - - go_real a12a21; - go_real a12a23; - go_real a12a24; - go_real a12a31; - go_real a12a33; - go_real a12a34; - - go_real a13a21; - go_real a13a22; - go_real a13a24; - go_real a13a31; - go_real a13a32; - go_real a13a34; - - go_real a14a21; - go_real a14a22; - go_real a14a23; - go_real a14a31; - go_real a14a32; - go_real a14a33; - - go_real a21a32; - go_real a21a33; - go_real a21a34; - - go_real a22a31; - go_real a22a33; - go_real a22a34; - - go_real a23a31; - go_real a23a32; - go_real a23a34; - - go_real a24a31; - go_real a24a32; - go_real a24a33; - - go_real a11a22a33; - go_real a11a22a34; - go_real a11a23a32; - go_real a11a23a34; - go_real a11a24a32; - go_real a11a24a33; - go_real a12a21a33; - go_real a12a21a34; - go_real a12a23a31; - go_real a12a23a34; - go_real a12a24a31; - go_real a12a24a33; - go_real a13a21a32; - go_real a13a21a34; - go_real a13a22a31; - go_real a13a22a34; - go_real a13a24a31; - go_real a13a24a32; - go_real a14a21a32; - go_real a14a21a33; - go_real a14a22a31; - go_real a14a22a33; - go_real a14a23a31; - go_real a14a23a32; - - go_integer row, col; - - a11a22 = a[0][0]*a[1][1]; - a11a23 = a[0][0]*a[1][2]; - a11a24 = a[0][0]*a[1][3]; - a11a33 = a[0][0]*a[2][2]; - a11a32 = a[0][0]*a[2][1]; - a11a34 = a[0][0]*a[2][3]; - - a12a21 = a[0][1]*a[1][0]; - a12a23 = a[0][1]*a[1][2]; - a12a24 = a[0][1]*a[1][3]; - a12a31 = a[0][1]*a[2][0]; - a12a33 = a[0][1]*a[2][2]; - a12a34 = a[0][1]*a[2][3]; - - a13a21 = a[0][2]*a[1][0]; - a13a22 = a[0][2]*a[1][1]; - a13a24 = a[0][2]*a[1][3]; - a13a31 = a[0][2]*a[2][0]; - a13a32 = a[0][2]*a[2][1]; - a13a34 = a[0][2]*a[2][3]; - - a14a21 = a[0][3]*a[1][0]; - a14a22 = a[0][3]*a[1][1]; - a14a23 = a[0][3]*a[1][2]; - a14a31 = a[0][3]*a[2][0]; - a14a32 = a[0][3]*a[2][1]; - a14a33 = a[0][3]*a[2][2]; - - a21a32 = a[1][0]*a[2][1]; - a21a33 = a[1][0]*a[2][2]; - a21a34 = a[1][0]*a[2][3]; - - a22a31 = a[1][1]*a[2][0]; - a22a33 = a[1][1]*a[2][2]; - a22a34 = a[1][1]*a[2][3]; - - a23a31 = a[1][2]*a[2][0]; - a23a32 = a[1][2]*a[2][1]; - a23a34 = a[1][2]*a[2][3]; - - a24a31 = a[1][3]*a[2][0]; - a24a32 = a[1][3]*a[2][1]; - a24a33 = a[1][3]*a[2][2]; - - a11a22a33 = a11a22*a[2][2]; - a11a22a34 = a11a22*a[2][3]; - a11a23a32 = a11a23*a[2][1]; - a11a23a34 = a11a23*a[2][3]; - a11a24a32 = a11a24*a[2][1]; - a11a24a33 = a11a24*a[2][2]; - a12a21a33 = a12a21*a[2][2]; - a12a21a34 = a12a21*a[2][3]; - a12a23a31 = a12a23*a[2][0]; - a12a23a34 = a12a23*a[2][3]; - a12a24a31 = a12a24*a[2][0]; - a12a24a33 = a12a24*a[2][2]; - a13a21a32 = a13a21*a[2][1]; - a13a21a34 = a13a21*a[2][3]; - a13a22a31 = a13a22*a[2][0]; - a13a22a34 = a13a22*a[2][3]; - a13a24a31 = a13a24*a[2][0]; - a13a24a32 = a13a24*a[2][1]; - a14a21a32 = a14a21*a[2][1]; - a14a21a33 = a14a21*a[2][2]; - a14a22a31 = a14a22*a[2][0]; - a14a22a33 = a14a22*a[2][2]; - a14a23a31 = a14a23*a[2][0]; - a14a23a32 = a14a23*a[2][1]; - - denom = - + a12a24a33*a[3][0] - + a13a24a31*a[3][1] - - a12a21a33*a[3][3] - - a13a24a32*a[3][0] - - a14a21a32*a[3][2] - - a12a24a31*a[3][2] - + a14a23a32*a[3][0] - + a13a21a32*a[3][3] - + a14a21a33*a[3][1] - + a12a23a31*a[3][3] - - a14a23a31*a[3][1] - + a12a21a34*a[3][2] - - a12a23a34*a[3][0] - - a13a21a34*a[3][1] - - a13a22a31*a[3][3] - + a14a22a31*a[3][2] - + a13a22a34*a[3][0] - - a14a22a33*a[3][0] - + a11a22a33*a[3][3] - - a11a22a34*a[3][2] - + a11a24a32*a[3][2] - - a11a23a32*a[3][3] - + a11a23a34*a[3][1] - - a11a24a33*a[3][1]; - - if (GO_SMALL(denom)) { - return GO_RESULT_SINGULAR; - } - - denom = 1.0/denom; - - work[0][0] = (+ a22a33*a[3][3] - - a22a34*a[3][2] - + a24a32*a[3][2] - - a23a32*a[3][3] - + a23a34*a[3][1] - - a24a33*a[3][1])*denom; - work[0][1] = (- a12a33*a[3][3] - + a12a34*a[3][2] - - a13a34*a[3][1] - + a13a32*a[3][3] - - a14a32*a[3][2] - + a14a33*a[3][1])*denom; - work[0][2] = (+ a12a23*a[3][3] - - a12a24*a[3][2] - - a13a22*a[3][3] - + a13a24*a[3][1] - + a14a22*a[3][2] - - a14a23*a[3][1])*denom; - work[0][3] = (- a12a23a34 - + a12a24a33 - + a13a22a34 - - a13a24a32 - - a14a22a33 - + a14a23a32)*denom; - - work[1][0] = (+ a23a31*a[3][3] - - a23a34*a[3][0] - - a21a33*a[3][3] - + a21a34*a[3][2] - + a24a33*a[3][0] - - a24a31*a[3][2])*denom; - work[1][1] = (+ a11a33*a[3][3] - - a11a34*a[3][2] - - a13a31*a[3][3] - + a14a31*a[3][2] - + a13a34*a[3][0] - - a14a33*a[3][0])*denom; - work[1][2] = (- a11a23*a[3][3] - + a11a24*a[3][2] - + a13a21*a[3][3] - + a14a23*a[3][0] - - a14a21*a[3][2] - - a13a24*a[3][0])*denom; - work[1][3] = (+ a11a23a34 - - a11a24a33 - - a13a21a34 - + a14a21a33 - - a14a23a31 - + a13a24a31)*denom; - - work[2][0] = (+ a22a34*a[3][0] - - a22a31*a[3][3] - - a24a32*a[3][0] - + a21a32*a[3][3] - - a21a34*a[3][1] - + a24a31*a[3][1])*denom; - work[2][1] = (+ a11a34*a[3][1] - - a11a32*a[3][3] - - a12a34*a[3][0] - - a14a31*a[3][1] - + a12a31*a[3][3] - + a14a32*a[3][0])*denom; - work[2][2] = (- a11a24*a[3][1] - - a14a22*a[3][0] - + a11a22*a[3][3] - + a12a24*a[3][0] - + a14a21*a[3][1] - - a12a21*a[3][3])*denom; - work[2][3] = (- a11a22a34 - + a11a24a32 - + a14a22a31 - - a12a24a31 - + a12a21a34 - - a14a21a32)*denom; - - work[3][0] = (+ a21a33*a[3][1] - + a23a32*a[3][0] - - a22a33*a[3][0] - - a23a31*a[3][1] - + a22a31*a[3][2] - - a21a32*a[3][2])*denom; - work[3][1] = (+ a12a33*a[3][0] - - a13a32*a[3][0] - - a12a31*a[3][2] - + a13a31*a[3][1] - + a11a32*a[3][2] - - a11a33*a[3][1])*denom; - work[3][2] = (- a12a23*a[3][0] - + a12a21*a[3][2] - - a13a21*a[3][1] - + a13a22*a[3][0] - - a11a22*a[3][2] - + a11a23*a[3][1])*denom; - work[3][3] = (+ a13a21a32 - + a12a23a31 - - a12a21a33 - - a13a22a31 - + a11a22a33 - - a11a23a32)*denom; - - for (row = 0; row < 4; row++) - for (col = 0; col < 4; col++) - ainv[row][col] = work[row][col]; - - return GO_RESULT_OK; -} - -int go_mat4_mat4_mult(const go_real a[4][4], - const go_real b[4][4], - go_real axb[4][4]) -{ - go_real work[4][4]; - go_integer i, j, k; - - for (i = 0; i < 4; i++) { - for (j = 0; j < 4; j++) { - work[i][j] = 0; - for (k = 0; k < 4; k++) { - work[i][j] += a[i][k] * b[k][j]; - } - } - } - - for (i = 0; i < 4; i++) - for (j = 0; j < 4; j++) - axb[i][j] = work[i][j]; - - return GO_RESULT_OK; -} - -int go_mat4_vec4_mult(const go_real a[4][4], - const go_real v[4], - go_real axv[4]) -{ - go_real work[4]; - go_integer i, j; - - for (i = 0; i < 4; i++) { - work[i] = 0; - for (j = 0; j < 4; j++) { - work[i] += a[i][j] * v[j]; - } - } - - for (i = 0; i < 4; i++) - axv[i] = work[i]; - - return GO_RESULT_OK; -} - -int go_mat6_inv(const go_real a[6][6], - go_real ainv[6][6]) -{ - go_real cpy[6][6]; - go_real *cpyptr[6]; - go_real scratchrow[6]; - go_real v[6]; - go_real d; - go_integer index[6]; - go_integer row, col; - int retval; - - /* create a copy of m[][] in cpy[][], since ludcmp destroys input matrix */ - for (row = 0; row < 6; row++) { - for (col = 0; col < 6; col++) { - cpy[row][col] = a[row][col]; - } - /* set up the go_real pointer array that ludcmp likes */ - cpyptr[row] = cpy[row]; - } - - /* convert the copy to its LU decomposition */ - retval = ludcmp(cpyptr, scratchrow, 6, index, &d); - if (GO_RESULT_OK != retval) return retval; - - /* backsubstitute a column with a 1 in it to get the inverse */ - for (col = 0; col < 6; col++) { - for (row = 0; row < 6; row++) { - v[row] = 0.0; - } - v[col] = 1.0; - retval = lubksb(cpyptr, 6, index, v); - if (GO_RESULT_OK != retval) return retval; - for (row = 0; row < 6; row++) { - ainv[row][col] = v[row]; - } - } - - return GO_RESULT_OK; -} - -int go_mat6_mat6_mult(const go_real a[6][6], - const go_real b[6][6], - go_real axb[6][6]) -{ - go_real work[6][6]; - go_integer i, j, k; - - for (i = 0; i < 6; i++) { - for (j = 0; j < 6; j++) { - work[i][j] = 0; - for (k = 0; k < 6; k++) { - work[i][j] += a[i][k] * b[k][j]; - } - } - } - - for (i = 0; i < 6; i++) - for (j = 0; j < 6; j++) - axb[i][j] = work[i][j]; - - return GO_RESULT_OK; -} - -int go_mat6_vec6_mult(const go_real a[6][6], - const go_real v[6], - go_real axv[6]) -{ - go_real work[6]; - go_integer i, j; - - for (i = 0; i < 6; i++) { - work[i] = 0; - for (j = 0; j < 6; j++) { - work[i] += a[i][j] * v[j]; - } - } - - for (i = 0; i < 6; i++) - axv[i] = work[i]; - - return GO_RESULT_OK; -} - -/* recall: */ -/* | m.x.x m.y.x m.z.x | */ -/* M = | m.x.y m.y.y m.z.y | */ -/* | m.x.z m.y.z m.z.z | */ - -int go_dh_pose_convert(const go_dh * dh, go_pose * p) -{ - go_hom h; - go_real sth, cth; /* sin, cos theta[i] */ - go_real sal, cal; /* sin, cos alpha[i-1] */ - - sth = sin(dh->theta); - cth = cos(dh->theta); - sal = sin(dh->alpha); - cal = cos(dh->alpha); - - h.rot.x.x = cth, h.rot.y.x = -sth, h.rot.z.x = 0.0; - h.rot.x.y = sth*cal, h.rot.y.y = cth*cal, h.rot.z.y = -sal; - h.rot.x.z = sth*sal, h.rot.y.z = cth*sal, h.rot.z.z = cal; - - h.tran.x = dh->a; - h.tran.y = -sal*dh->d; - h.tran.z = cal*dh->d; - - return go_hom_pose_convert(&h, p); -} - -int go_pose_dh_convert(const go_pose * ph, go_dh * dh) -{ - go_hom h; - - go_pose_hom_convert(ph, &h); - - dh->a = h.tran.x; - dh->alpha = -atan2(h.rot.z.y, h.rot.z.z); - dh->theta = -atan2(h.rot.y.x, h.rot.x.x); - if (GO_ROT_SMALL(dh->alpha)) { - dh->d = h.tran.z / cos(dh->alpha); - } else { - dh->d = -h.tran.y / sin(dh->alpha); - } - - return GO_RESULT_OK; -} - -int go_link_joint_set(const go_link * link, go_real joint, go_link * linkout) -{ - go_pose pose; - go_rvec rvec; - int retval; - - linkout->type = link->type; - linkout->quantity = link->quantity; - - if (GO_LINK_DH == link->type) { - linkout->u.dh.a = link->u.dh.a; - linkout->u.dh.alpha = link->u.dh.alpha; - if (GO_QUANTITY_LENGTH == link->quantity) { - linkout->u.dh.d = joint; - linkout->u.dh.theta = link->u.dh.theta; - } else { - linkout->u.dh.d = link->u.dh.d; - linkout->u.dh.theta = joint; - } - return GO_RESULT_OK; - } - - if (GO_LINK_PP == link->type) { - pose = go_pose_identity(); - if (GO_QUANTITY_LENGTH == link->quantity) { - pose.tran.z = joint; - return go_pose_pose_mult(&link->u.pp.pose, &pose, &linkout->u.pp.pose); - } - /* else revolute */ - rvec.x = 0, rvec.y = 0, rvec.z = joint; /* rot(Z,joint) */ - retval = go_rvec_quat_convert(&rvec, &pose.rot); - if (GO_RESULT_OK != retval) return retval; - return go_pose_pose_mult(&link->u.pp.pose, &pose, &linkout->u.pp.pose); - } - - if (GO_LINK_PK == link->type) { - /* - Our PK type is always a length joint, so link->quantity must - be GO_QUANTITY_LENGTH. One day we may be able to handle revolute - parallel joints, but we can't now. Let's fail if we ever get - a revolute joint. - FIXME-- add revolute joints to PKMs. - */ - if (GO_QUANTITY_LENGTH != link->quantity) { - return GO_RESULT_IMPL_ERROR; - } - /* else we're a prismatic joint */ - linkout->u.pk.base = link->u.pk.base; - linkout->u.pk.platform = link->u.pk.platform; - linkout->u.pk.d = joint; - return GO_RESULT_OK; - } - - /* else not a recognized link type */ - return GO_RESULT_ERROR; -} - -/* this only works for serial-link manipulators */ -int go_link_pose_build(const go_link * link_params, go_integer num, go_pose * pose) -{ - go_pose p; - go_integer link; - - *pose = go_pose_identity(); - - for (link = 0; link < num; link++) { - if (GO_LINK_DH == link_params[link].type) { - go_dh_pose_convert(&link_params[link].u.dh, &p); - go_pose_pose_mult(pose, &p, pose); - } else if (GO_LINK_PP == link_params[link].type) { - go_pose_pose_mult(pose, &link_params[link].u.pp.pose, pose); - } else { - return GO_RESULT_ERROR; - } - } - - return GO_RESULT_OK; -} diff --git a/src/libposemath/gomath.h b/src/libposemath/gomath.h deleted file mode 100644 index 97d061da470..00000000000 --- a/src/libposemath/gomath.h +++ /dev/null @@ -1,852 +0,0 @@ -/******************************************************************** -* Description: gomath.h -* Library file with various functions for working with matrices -* -* Derived from a work by Fred Proctor, -* changed to work with emc2 and HAL -* -* Adapting Author: Alex Joni -* License: LGPL Version 2 -* System: Linux -* -******************************************************************* - Similar to posemath, but using different functions. - - TODO: - * find the new functions, add them to posemath, convert the rest -*/ - -#ifndef __LINUXCNC_GO_MATH_H -#define __LINUXCNC_GO_MATH_H - -#include /* M_PI, NULL, FLT/DBL_MIN,MAX,EPSILON */ -#include "gotypes.h" /* go_integer,real */ - -/*! Returns the square of \a x. */ -#define go_sq(x) ((x)*(x)) -/*! Returns the cube of \a x. */ -#define go_cub(x) ((x)*(x)*(x)) -/*! Returns \a x to the fourth power. */ -#define go_qua(x) ((x)*(x)*(x)*(x)) - -/*! Returns the sine and cosine of \a x (in radians) in \a s and \a c, - respectively. Implemented as a single call when supported, to speed - up the calculation of the two values. */ -extern void go_sincos(go_real x, go_real * s, go_real * c); - -/*! Returns the cube root of \a x. */ -extern go_real go_cbrt(go_real x); - -#ifdef M_PI -/*! The value of Pi. */ -#define GO_PI M_PI -#else -/*! The value of Pi. */ -#define GO_PI 3.14159265358979323846 -#endif -/*! The value of twice Pi. */ -#define GO_2_PI (2.0*GO_PI) - -#ifdef M_PI_2 -/*! The value of half of Pi. */ -#define GO_PI_2 M_PI_2 -#else -/*! The value of half of Pi. */ -#define GO_PI_2 1.57079632679489661923 -#endif - -#ifdef M_PI_4 -/*! The value of one-fourth of Pi. */ -#define GO_PI_4 M_PI_4 -#else -/*! The value of one-fourth of Pi. */ -#define GO_PI_4 0.78539816339744830962 -#endif - -/*! Returns \a rad in radians as its value in degrees. */ -#define GO_TO_DEG(rad) ((rad)*57.295779513082323) -/*! Returns \a deg in degrees as its value in radians. */ -#define GO_TO_RAD(deg) ((deg)*0.0174532925199432952) - -/*! How close translational quantities must be to be equal. */ -#define GO_TRAN_CLOSE(x,y) (fabs((x)-(y)) < GO_REAL_EPSILON) -/*! How small a translational quantity must be to be zero. */ -#define GO_TRAN_SMALL(x) (fabs(x) < GO_REAL_EPSILON) - -/*! How close rotational quantities must be to be equal. */ -#define GO_ROT_CLOSE(x,y) (fabs((x)-(y)) < GO_REAL_EPSILON) -/*! How small a rotational quantity must be to be zero. */ -#define GO_ROT_SMALL(x) (fabs(x) < GO_REAL_EPSILON) - -/*! How close general quantities must be to be equal. Use this when - you have something other than translational or rotational quantities, - otherwise use one of \a GO_TRAN,ROT_CLOSE. */ -#define GO_CLOSE(x,y) (fabs((x)-(y)) < GO_REAL_EPSILON) -/*! How small a general quantity must be to be zero. Use this when - you have something other than a translational or rotational quantity, - otherwise use one of \a GO_TRAN,ROT_SMALL. */ -#define GO_SMALL(x) (fabs(x) < GO_REAL_EPSILON) - -/*! A point or vector in Cartesian coordinates. */ -typedef struct { - go_real x; - go_real y; - go_real z; -} go_cart; - -/*! A point or vector in spherical coordinates, with \a phi as - the angle down from the zenith, not up from the XY plane. */ -typedef struct { - go_real theta; - go_real phi; - go_real r; -} go_sph; - -/*! A point or vector in cylindrical coordinates. */ -typedef struct { - go_real theta; - go_real r; - go_real z; -} go_cyl; - -/*! A rotation vector, whose direction points along the axis of positive - rotation, and whose magnitude is the amount of rotation around this - axis, in radians. */ -typedef struct { - go_real x; - go_real y; - go_real z; -} go_rvec; - -/* | m.x.x m.y.x m.z.x | */ -/* go_mat m = | m.x.y m.y.y m.z.y | */ -/* | m.x.z m.y.z m.z.z | */ - -/*! A rotation matrix. */ -typedef struct { - go_cart x; /*!< X unit vector */ - go_cart y; /*!< Y unit vector */ - go_cart z; /*!< Z unit vector */ -} go_mat; - -/*! A quaternion. \a s is the cosine of the half angle of rotation, - and the \a xyz elements comprise the vector that points in the direction - of positive rotation and whose magnitude is the sine of the half - angle of rotation. */ -typedef struct { - go_real s; - go_real x; - go_real y; - go_real z; -} go_quat; - -/*! ZYZ Euler angles. \a z is the amount of the first rotation around the - Z axis. \a y is the amount of the second rotation around the new \a Y - axis. \a zp is the amount of the third rotation around the new \a Z axis. */ -typedef struct { - go_real z; - go_real y; - go_real zp; -} go_zyz; - -/*! ZYX Euler angles. \a z is the amount of the first rotation around the - Z axis. \a y is the amount of the second rotation around the new \a Y - axis. \a x is the amount of the third rotation around the new \a X axis. */ -typedef struct { - go_real z; - go_real y; - go_real x; -} go_zyx; - -/*! Roll-pitch-yaw angles. \a r is the amount of the first rotation - (roll) around the X axis. \a p is the amount of the second rotation - (pitch) around the original Y axis. \a y is the amount of the third - rotation (yaw) around the original Z axis. */ -typedef struct { - go_real r; - go_real p; - go_real y; -} go_rpy; - -/*! - A \a go_pose represents the Cartesian position vector and quaternion - orientation of a frame. -*/ -typedef struct { - go_cart tran; - go_quat rot; -} go_pose; - -/*! - A \a go_screw represents the linear- and angular velocity vectors - of a frame. \a v is the Cartesian linear velocity vector. - \a w is the Cartesian angular velocity vector, the instantaneous - vector about which the frame is rotating. -*/ -typedef struct { - go_cart v; - go_cart w; -} go_screw; - -/*! Convenience function that returns a \a go_pose given individual - elements. */ -extern go_pose -go_pose_this(go_real x, go_real y, go_real z, - go_real rs, go_real rx, go_real ry, go_real rz); - -/*! Returns the zero vector. */ -extern go_cart -go_cart_zero(void); - -/*! Returns the identity (zero) quaternion, i.e., no rotation. */ -extern go_quat -go_quat_identity(void); - -/*! Returns the identity pose, no translation or rotation. */ -extern go_pose -go_pose_identity(void); - -typedef struct { - go_cart tran; - go_mat rot; -} go_hom; - -/* lines, planes and related functions */ - -/*! - Lines are represented in point-direction form - (point p, direction v) as - (x - px)/vx = (y - py)/vy = (z - pz)vz -*/ -typedef struct { - go_cart point; - go_cart direction; /* always a unit vector */ -} go_line; - -/*! - Given a plane as Ax + By + Cz + D = 0, the normal vector - \a normal is the Cartesian vector (A,B,C), and the number - \a d is the value D. - - Planes have a handedness, given by the direction of the normal - vector, so two planes that appear coincident may be different by the - direction of their anti-parallel normal vectors. -*/ -typedef struct { - go_cart normal; - go_real d; -} go_plane; - -/*! Fills in \a line given \a point and \a direction. Returns GO_RESULT_OK - if \a direction is non-zero, otherwise GO_RESULT_ERROR. */ -extern int go_line_from_point_direction(const go_cart * point, const go_cart * direction, go_line * line); - -/*! Fills in \a line given two points. Returns GO_RESULT_OK if the points - are different, otherwise GO_RESULT_ERROR. */ -extern int go_line_from_points(const go_cart * point1, const go_cart * point2, go_line * line); - -/*! Fill in \a line with the intersection of the two planes. Returns GO_RESULT_OK if the planes are not parallel, otherwise GO_RESULT_ERROR. */ -extern int go_line_from_planes(const go_plane * plane1, const go_plane * plane2, go_line * line); - -/*! Returns non-zero if the lines are the same, otherwise zero. */ -extern go_flag go_line_line_compare(const go_line * line1, const go_line * line2); - -/*! Fills in \a point with the point located distance \a d along \a line */ -extern int go_line_evaluate(const go_line * line, go_real d, go_cart * point); - -/*! Fills in \a distance with the distance from \a point to \a line */ -extern int go_point_line_distance(const go_cart * point, const go_line * line, go_real * distance); - -/*! Fills in \a pout with the nearest point on \a line to \a point */ -extern int go_point_line_proj(const go_cart * point, const go_line * line, go_cart * pout); - -/*! Fills in \a proj with the projection of \a point onto \a plane */ -extern int go_point_plane_proj(const go_cart * point, const go_plane * plane, go_cart * proj); - -/*! Fills in \a proj with the projection of \a line onto \a plane */ -extern int go_line_plane_proj(const go_line * line, const go_plane * plane, go_line * proj); - -/*! Fills in \a plane give a \a point on the plane and the normal \a direction. */ -extern int go_plane_from_point_normal(const go_cart * point, const go_cart * normal, go_plane * plane); - -/*! Fills in \a plane given the A, B, C and D values in the canonical - form Ax + By + Cz + D = 0. Returns GO_RESULT_OK - if not all of A, B and C are zero, otherwise GO_RESULT_ERROR. */ -extern int go_plane_from_abcd(go_real A, go_real B, go_real C, go_real D, go_plane * plane); - -/*! Fills in \a plane given three points. Returns GO_RESULT_OK - if the points are distinct, otherwise GO_RESULT_ERROR. */ -extern int go_plane_from_points(const go_cart * point1, const go_cart * point2, const go_cart * point3, go_plane * plane); - -/*! Fills in \a plane given a \a point on the plane and a \a line - in the plane. Returns GO_RESULT_OK if the point is not on the line, - GO_RESULT_ERROR otherwise. */ -extern int go_plane_from_point_line(const go_cart * point, const go_line * line, go_plane * plane); - -/*! Returns non-zero if the planes are coincident and have the same - normal direction, otherwise zero. */ -extern go_flag go_plane_plane_compare(const go_plane * plane1, const go_plane * plane2); - -/*! Fills in the \a distance from the \a point to the \a plane. */ -extern int go_point_plane_distance(const go_cart * point, const go_plane * plane, go_real * distance); - -/*! Fills in \a point with the point located distances \a u and \a v along - some orthogonal planar coordinate system in \a plane */ -extern int go_plane_evaluate(const go_plane * plane, go_real u, go_real v, go_cart * point); - -/*! Fills in \a point with the intersection point of - \a line with \a plane, and \a distance with the distance along the - line to the intersection point. Returns GO_RESULT_ERROR if the line - is parallel to the plane and not lying in the plane, otherwise - GO_RESULT_OK. */ -extern int go_line_plane_intersect(const go_line * line, const go_plane * plane, go_cart * point, go_real * distance); - -/* - struct arguments are passed to functions as const pointers since the - speed is at least as fast for all but structs of one or two elements. -*/ - -/* translation rep conversion functions */ - -extern int go_cart_sph_convert(const go_cart *, go_sph *); -extern int go_cart_cyl_convert(const go_cart *, go_cyl *); -extern int go_sph_cart_convert(const go_sph *, go_cart *); -extern int go_sph_cyl_convert(const go_sph *, go_cyl *); -extern int go_cyl_cart_convert(const go_cyl *, go_cart *); -extern int go_cyl_sph_convert(const go_cyl *, go_sph *); - -/* rotation rep conversion functions */ - -extern int go_rvec_quat_convert(const go_rvec *, go_quat *); -extern int go_rvec_mat_convert(const go_rvec *, go_mat *); -extern int go_rvec_zyz_convert(const go_rvec *, go_zyz *); -extern int go_rvec_zyx_convert(const go_rvec *, go_zyx *); -extern int go_rvec_rpy_convert(const go_rvec *, go_rpy *); - -extern int go_quat_rvec_convert(const go_quat *, go_rvec *); -extern int go_quat_mat_convert(const go_quat *, go_mat *); -extern int go_quat_zyz_convert(const go_quat *, go_zyz *); -extern int go_quat_zyx_convert(const go_quat *, go_zyx *); -extern int go_quat_rpy_convert(const go_quat *, go_rpy *); - -extern int go_mat_rvec_convert(const go_mat *, go_rvec *); -extern int go_mat_quat_convert(const go_mat *, go_quat *); -extern int go_mat_zyz_convert(const go_mat *, go_zyz *); -extern int go_mat_zyx_convert(const go_mat *, go_zyx *); -extern int go_mat_rpy_convert(const go_mat *, go_rpy *); - -extern int go_zyz_rvec_convert(const go_zyz *, go_rvec *); -extern int go_zyz_quat_convert(const go_zyz *, go_quat *); -extern int go_zyz_mat_convert(const go_zyz *, go_mat *); -extern int go_zyz_zyx_convert(const go_zyz *, go_zyx *); -extern int go_zyz_rpy_convert(const go_zyz *, go_rpy *); - -extern int go_zyx_rvec_convert(const go_zyx *, go_rvec *); -extern int go_zyx_quat_convert(const go_zyx *, go_quat *); -extern int go_zyx_mat_convert(const go_zyx *, go_mat *); -extern int go_zyx_zyz_convert(const go_zyx *, go_zyz *); -extern int go_zyx_rpy_convert(const go_zyx *, go_rpy *); - -extern int go_rpy_rvec_convert(const go_rpy *, go_rvec *); -extern int go_rpy_quat_convert(const go_rpy *, go_quat *); -extern int go_rpy_mat_convert(const go_rpy *, go_mat *); -extern int go_rpy_zyz_convert(const go_rpy *, go_zyz *); -extern int go_rpy_zyx_convert(const go_rpy *, go_zyx *); - -/* combined rep conversion functions */ - -extern int go_pose_hom_convert(const go_pose *, go_hom *); -extern int go_hom_pose_convert(const go_hom *, go_pose *); - -/* misc conversion functions */ -/*! - go_cart_rvec_convert and go_rvec_cart_convert convert between - Cartesian vectors and rotation vectors. The conversion is trivial - but keeps types distinct. -*/ -extern int go_cart_rvec_convert(const go_cart * cart, go_rvec * rvec); -extern int go_rvec_cart_convert(const go_rvec * rvec, go_cart * cart); - -/* translation functions, that work only with the preferred - go_cart type. Other types must be converted to go_cart - to use these, e.g., there's no go_sph_cyl_compare() */ - -extern go_flag go_cart_cart_compare(const go_cart *, const go_cart *); -extern int go_cart_cart_dot(const go_cart *, const go_cart *, - go_real *); -extern int go_cart_cart_cross(const go_cart *, const go_cart *, - go_cart *); -extern int go_cart_mag(const go_cart *, go_real *); -extern int go_cart_magsq(const go_cart *, go_real *); -extern go_flag go_cart_cart_par(const go_cart *, const go_cart *); -extern go_flag go_cart_cart_perp(const go_cart *, const go_cart *); - -/*! Places the Cartesian displacement between two vectors \a v1 and - \a v2 in \a disp, returning \a GO_RESULT_OK. */ -extern int go_cart_cart_disp(const go_cart * v1, - const go_cart * v2, - go_real * disp); - -extern int go_cart_cart_add(const go_cart *, const go_cart *, - go_cart *); -extern int go_cart_cart_sub(const go_cart *, const go_cart *, - go_cart *); -extern int go_cart_scale_mult(const go_cart *, go_real, go_cart *); -extern int go_cart_neg(const go_cart *, go_cart *); -extern int go_cart_unit(const go_cart *, go_cart *); - -/*! - Given two non-zero vectors \a v1 and \a v2, fill in \a quat with - the minimum rotation that brings \a v1 to \a v2. - */ -extern int go_cart_cart_rot(const go_cart * v1, - const go_cart * v2, - go_quat * quat); - -/*! - Project vector \a v1 onto \a v2, with the resulting vector placed - into \a vout. Returns GO_RESULT_OK if it can be done, otherwise - something else. -*/ -extern int go_cart_cart_proj(const go_cart * v1, const go_cart * v2, - go_cart * vout); -extern int go_cart_plane_proj(const go_cart *, const go_cart *, - go_cart *); -extern int go_cart_cart_angle(const go_cart *, const go_cart *, - go_real *); -/*! - go_cart_normal finds one of the infinite vectors perpendicular - to \a v, putting the result in \a vout. - */ -extern int go_cart_normal(const go_cart * v, go_cart * vout); -extern int go_cart_centroid(const go_cart * varray, - go_integer num, - go_cart * centroid); -extern int go_cart_centroidize(const go_cart * vinarray, - go_integer num, - go_cart * centroid, - go_cart * voutarray); -extern int go_cart_cart_pose(const go_cart *, const go_cart *, - go_cart *, go_cart *, - go_integer, go_pose *); - -/*! - Returns the Cartesian point \a p whose distances from three other points - \a c1, \a c2 and \a c3 are \a l1, \a l2 and \a l3, respectively. In - general there are 0, 1 or two points possible. If no point is possible, - this returns GO_RESULT_ERROR, otherwise the points are returned in \a - p1 and \a p2, which may be the same point. -*/ - -int go_cart_trilaterate(const go_cart * c1, - const go_cart * c2, - const go_cart * c3, - go_real l1, - go_real l2, - go_real l3, - go_cart * p1, - go_cart * p2); - -/* quat functions */ - -extern go_flag go_quat_quat_compare(const go_quat *, const go_quat *); -extern int go_quat_mag(const go_quat *, go_real *); - -/*! - go_quat_unit takes a quaternion rotation \a q and converts it into - a unit rotation about the same axis, \a qout. -*/ -extern int go_quat_unit(const go_quat * q, go_quat * qout); -extern int go_quat_norm(const go_quat *, go_quat *); -extern int go_quat_inv(const go_quat *, go_quat *); -extern go_flag go_quat_is_norm(const go_quat *); -extern int go_quat_scale_mult(const go_quat *, go_real, go_quat *); -extern int go_quat_quat_mult(const go_quat *, const go_quat *, - go_quat *); -extern int go_quat_cart_mult(const go_quat *, const go_cart *, - go_cart *); - -/* rotation vector functions */ - -extern go_flag go_rvec_rvec_compare(const go_rvec * r1, const go_rvec * r2); -extern int go_rvec_scale_mult(const go_rvec *, go_real, go_rvec *); - -/* rotation matrix functions */ - -/* | m.x.x m.y.x m.z.x | */ -/* M = | m.x.y m.y.y m.z.y | */ -/* | m.x.z m.y.z m.z.z | */ - -/*! - Normalizes rotation matrix \a m so that all columns are mutually - perpendicular unit vectors, placing the result in \a mout. -*/ -extern int go_mat_norm(const go_mat * m, go_mat * mout); - -extern go_flag go_mat_is_norm(const go_mat *); -extern int go_mat_inv(const go_mat *, go_mat *); -extern int go_mat_cart_mult(const go_mat *, const go_cart *, go_cart *); -extern int go_mat_mat_mult(const go_mat *, const go_mat *, go_mat *); - -/* pose functions*/ - -extern go_flag go_pose_pose_compare(const go_pose *, const go_pose *); -extern int go_pose_inv(const go_pose *, go_pose *); -extern int go_pose_cart_mult(const go_pose *, const go_cart *, go_cart *); -extern int go_pose_pose_mult(const go_pose *, const go_pose *, go_pose *); -extern int go_pose_scale_mult(const go_pose *, go_real, go_pose *); - -/*! Given two times \a t1 and \a t2, and associated poses \a p1 and \a - p2, interpolates (or extrapolates) to find pose \a p3 at time \a - t3. The result is stored in \a p3. Returns GO_RESULT_OK if \a t1 and - \a t2 are distinct and \a p1 and \a p2 are valid poses, otherwise it - can't interpolate and returns a relevant error. */ -extern int -go_pose_pose_interp(go_real t1, - const go_pose * p1, - go_real t2, - const go_pose * p2, - go_real t3, - go_pose * p3); - -/* homogeneous transform functions */ - -extern int go_hom_inv(const go_hom *, go_hom *); - -/* screw functions */ - -/*! Given \a pose transformation from frame A to B, and a screw \a screw - in frame A, transform the screw into frame B and place in \a out. */ -extern int go_pose_screw_mult(const go_pose * pose, const go_screw * screw, go_screw * out); - -/* declarations for general MxN matrices */ - -/*! - Declare a matrix variable \a m with \a rows rows and \a cols columns. - Allocates \a rows X \a columns of space in \a mspace. -*/ - -typedef go_real go_vector; - -typedef struct { - go_integer rows; - go_integer cols; - go_real ** el; - go_real ** elcpy; - go_real * v; - go_integer * index; -} go_matrix; - -#define GO_MATRIX_DECLARE(M,Mspace,_rows,_cols) \ -go_matrix M = {0, 0, NULL, NULL, NULL, NULL}; \ -struct { \ - go_real * el[_rows]; \ - go_real * elcpy[_rows]; \ - go_real stg[_rows][_cols]; \ - go_real stgcpy[_rows][_cols]; \ - go_real v[_rows]; \ - go_integer index[_rows]; \ -} Mspace - -#define go_matrix_init(M,Mspace,_rows,_cols) \ -M.el = Mspace.el; \ -M.elcpy = Mspace.elcpy; \ -for (M.rows = 0; M.rows < (_rows); M.rows++) { \ - M.el[M.rows] = Mspace.stg[M.rows]; \ - M.elcpy[M.rows] = Mspace.stgcpy[M.rows]; \ -} \ -M.rows = (_rows); \ -M.cols = (_cols); \ -M.v = Mspace.v; \ -M.index = Mspace.index - -extern go_real -go_get_singular_epsilon(void); - -extern int -go_set_singular_epsilon(go_real epsilon); - -extern int -ludcmp(go_real ** a, - go_real * scratchrow, - go_integer n, - go_integer * indx, - go_real * d); - -extern int -lubksb(go_real ** a, - go_integer n, - go_integer * indx, - go_real * b); - -/* MxN matrix, Mx1 vector functions */ - -extern int -go_cart_vector_convert(const go_cart * c, - go_vector * v); -extern int -go_vector_cart_convert(const go_real * v, - go_cart * c); - -extern int -go_quat_matrix_convert(const go_quat * quat, - go_matrix * matrix); - -extern int -go_mat_matrix_convert(const go_mat * mat, - go_matrix * matrix); - -extern int -go_matrix_matrix_add(const go_matrix * a, - const go_matrix * b, - go_matrix * apb); - -extern int -go_matrix_matrix_copy(const go_matrix * src, - go_matrix * dst); - -extern int -go_matrix_matrix_mult(const go_matrix * a, - const go_matrix * b, - go_matrix * ab); - -extern int -go_matrix_vector_mult(const go_matrix * a, - const go_vector * v, - go_vector * av); - -/*! - The matrix-vector cross product is a matrix of the same dimension, - whose columns are the column-wise cross products of the matrix - and the vector. The matrices must be 3xN, the vector 3x1. -*/ -extern int -go_matrix_vector_cross(const go_matrix * a, - const go_vector * v, - go_matrix * axv); - -extern int -go_matrix_transpose(const go_matrix * a, - go_matrix * at); - -extern int -go_matrix_inv(const go_matrix * a, - go_matrix * ainv); - -/* Square matrix functions, where matN is an NxN matrix, and vecN - is an Nx1 vector */ - -/* Optimized 3x3 functions */ -extern int go_mat3_inv(const go_real a[3][3], - go_real ainv[3][3]); -extern int go_mat3_mat3_mult(const go_real a[3][3], - const go_real b[3][3], - go_real axb[3][3]); -extern int go_mat3_vec3_mult(const go_real a[3][3], - const go_real v[3], - go_real axv[3]); - -/* Optimized 4x4 functions */ -extern int go_mat4_inv(const go_real a[4][4], - go_real ainv[4][4]); -extern int go_mat4_mat4_mult(const go_real a[4][4], - const go_real b[4][4], - go_real axb[4][4]); -extern int go_mat4_vec4_mult(const go_real a[4][4], - const go_real v[4], - go_real axv[4]); - -/*! - Given a 6x6 matrix \a a, computes the inverse and returns it in - \a ainv. Leaves \a a untouched. Returns GO_RESULT_OK if there is an - inverse, else GO_RESULT_SINGULAR if the matrix is singular. -*/ -extern int go_mat6_inv(const go_real a[6][6], - go_real ainv[6][6]); - -/*! - Given two 6x6 matrices \a a and \a b, multiplies them and returns - the result in \a axb. Leaves \a a and \a b untouched. - Returns GO_RESULT_OK. -*/ -extern int go_mat6_mat6_mult(const go_real a[6][6], - const go_real b[6][6], - go_real axb[6][6]); - -/*! - Given a 6x6 matrix \a a and a 6x1 vector \a v, multiplies them - and returns the result in \a axv. Leaves \a a and \a v untouched. - Returns GO_RESULT_OK. -*/ -extern int go_mat6_vec6_mult(const go_real a[6][6], - const go_real v[6], - go_real axv[6]); - -/* Denavit-Hartenberg to pose conversions */ - -/* - The link frame is assumed to be - - | i-1 - | T - | i - - that is, elements of the link frame expressed wrt the - previous link frame. -*/ - -/*! - These DH parameters follow the convention in John J. Craig, - _Introduction to Robotics: Mechanics and Control_. -*/ -typedef struct { - go_real a; /*< a[i-1] */ - go_real alpha; /*< alpha[i-1] */ - /* either d or theta are the free variable, depending on quantity */ - go_real d; /*< d[i] */ - go_real theta; /*< theta[i] */ -} go_dh; - -/*! - PK parameters are used for parallel kinematic mechanisms, and - represent the Cartesian positions of the ends of the link in the - stationary base frame and the moving platform frame. Currently this - only supports prismatic links. - */ -typedef struct { - go_cart base; /*< position of fixed end in base frame */ - go_cart platform; /*< position of moving end in platform frame */ - go_real d; /*< the length of the link */ -} go_pk; - -/*! - PP parameters represent the pose of the link with respect to the - previous link. Revolute joints rotate about the Z axis, prismatic - joints slide along the Z axis. - */ -typedef struct { - go_pose pose; /*< the pose of the link wrt to the previous link */ -} go_pp; - -/*! Types of link parameter representations */ -enum { - GO_LINK_DH = 1, /*< for Denavit-Hartenberg params */ - GO_LINK_PK, /*< for parallel kinematics */ - GO_LINK_PP /*< for serial kinematics */ -}; - -#define go_link_to_string(L) \ -(L) == GO_LINK_DH ? "DH" : \ -(L) == GO_LINK_PK ? "PK" : \ -(L) == GO_LINK_PP ? "PP" : "None" - -/*! - This is the generic link structure for PKM sliding/cable links and - serial revolute/prismatic links. - */ -typedef struct { - union { - go_dh dh; /*< if you have DH params and don't want to convert to PP */ - go_pk pk; /*< if you have a parallel machine, e.g., hexapod or robot crane */ - go_pp pp; /*< if you have a serial machine, e.g., an industrial robot */ - } u; - go_flag type; /*< one of GO_LINK_DH,PK,PP */ - go_flag quantity; /*< one of GO_QUANTITY_LENGTH,ANGLE */ -} go_link; - -/*! - Converts DH parameters in \a dh to their pose equivalent, stored - in \a pose. - */ -extern int go_dh_pose_convert(const go_dh * dh, go_pose * pose); - -/*! - Converts \a pose to the equivalent DH parameters, stored in \a dh. - Warning! Conversion from these DH parameters back to a pose via \a - go_dh_pose_convert will NOT in general result in the same - pose. Poses have 6 degrees of freedom, DH parameters have 4, and - conversion to DH parameters loses some information. The source of - this information loss is the convention imposed on DH parameters for - choice of X-Y-Z axis directions. With poses, there is no such - convention, and poses are thus freer than DH parameters. - */ -extern int go_pose_dh_convert(const go_pose * pose, go_dh * dh); - -/*! - Fixes the link in \a link to its value when the joint - variable is \a joint, storing the result in \a linkout. -*/ -extern int go_link_joint_set(const go_link * link, go_real joint, go_link * linkout); - -/*! - Takes the link description of the device in \a links, and the number - of these in \a num, and builds the pose of the device and stores in - \a pose. \a links should have the value of the free link parameter - filled in with the current joint value, e.g., with a prior call to - go_link_joint_set. -*/ -extern int go_link_pose_build(const go_link * links, go_integer num, go_pose * pose); - -typedef struct { - go_real re; - go_real im; -} go_complex; - -extern go_complex go_complex_add(go_complex z1, go_complex z2); -extern go_complex go_complex_sub(go_complex z1, go_complex z2); -extern go_complex go_complex_mult(go_complex z1, go_complex z2); -extern go_complex go_complex_div(go_complex z1, go_complex z2, int * result); -extern go_complex go_complex_scale(go_complex z, go_real scale); -extern go_real go_complex_mag(go_complex z); -extern go_real go_complex_arg(go_complex z); -extern void go_complex_sqrt(go_complex z, go_complex * z1, go_complex * z2); -extern void go_complex_cbrt(go_complex z, go_complex * z1, go_complex * z2, go_complex * z3); - -typedef struct { - /* leading coefficient is 1, x^2 + ax + b = 0 */ - go_real a; - go_real b; -} go_quadratic; - -typedef struct { - /* leading coefficient is 1, x^3 + ax^2 + bx + c = 0 */ - go_real a; - go_real b; - go_real c; -} go_cubic; - -typedef struct { - /* leading coefficient is 1, x^4 + ax^3 + bx^2 + cx + d = 0 */ - go_real a; - go_real b; - go_real c; - go_real d; -} go_quartic; - -extern int go_quadratic_solve(const go_quadratic * quad, - go_complex * z1, - go_complex * z2); - -extern int go_cubic_solve(const go_cubic * cub, - go_complex * z1, - go_complex * z2, - go_complex * z3); - -extern int go_quartic_solve(const go_quartic * quart, - go_complex * z1, - go_complex * z2, - go_complex * z3, - go_complex * z4); - -extern int go_tridiag_reduce(go_real ** a, - go_integer n, - go_real * d, - go_real * e); - -extern int go_tridiag_ql(go_real * d, - go_real * e, - go_integer n, - go_real ** z); - -#endif /* GO_MATH_H */ - diff --git a/src/libposemath/gotypes.h b/src/libposemath/gotypes.h deleted file mode 100644 index 65a44fe3892..00000000000 --- a/src/libposemath/gotypes.h +++ /dev/null @@ -1,169 +0,0 @@ -/******************************************************************** -* Description: gotypes.h -* Library file with various functions for working with matrices -* -* Derived from a work by Fred Proctor, -* changed to work with emc2 and HAL -* -* Adapting Author: Alex Joni -* License: LGPL Version 2 -* System: Linux -* -******************************************************************* - Similar to posemath, but using different functions. - - TODO: - * find the new functions, add them to posemath, convert the rest -*/ - - -#ifndef __LINUXCNC_GO_TYPES_H -#define __LINUXCNC_GO_TYPES_H - -#include /* DBL_MAX, FLT_MAX */ - -/*! - GO_RESULT symbols run through a small range of values, on the - order of tens, suitable for a byte. GO_RESULT_OK is zero for easy - detection of error conditions, e.g., if (result) { handle error } -*/ -enum { - GO_RESULT_OK = 0, - GO_RESULT_IGNORED, /* action can't be done, ignored */ - GO_RESULT_BAD_ARGS, /* arguments bad, e.g., null pointer */ - GO_RESULT_RANGE_ERROR, /* supplied range value out of bounds */ - GO_RESULT_DOMAIN_ERROR, /* resulting domain out of bounds */ - GO_RESULT_ERROR, /* action can't be done, a problem */ - GO_RESULT_IMPL_ERROR, /* function not implemented */ - GO_RESULT_NORM_ERROR, /* a value is expected to be normalized */ - GO_RESULT_DIV_ERROR, /* divide by zero */ - GO_RESULT_SINGULAR, /* a matrix is singular */ - GO_RESULT_NO_SPACE, /* no space for append operation */ - GO_RESULT_EMPTY, /* data structure is empty */ - GO_RESULT_BUG /* a bug in Go, e.g., unknown case */ -}; - -#define go_result_to_string(r) \ -(r) == GO_RESULT_OK ? "Ok" : \ -(r) == GO_RESULT_IGNORED ? "Ignored" : \ -(r) == GO_RESULT_BAD_ARGS ? "Bad Args" : \ -(r) == GO_RESULT_RANGE_ERROR ? "Range Error" : \ -(r) == GO_RESULT_DOMAIN_ERROR ? "Domain Error" : \ -(r) == GO_RESULT_ERROR ? "General Error" : \ -(r) == GO_RESULT_IMPL_ERROR ? "Implementation Error" : \ -(r) == GO_RESULT_NORM_ERROR ? "Norm Error" : \ -(r) == GO_RESULT_DIV_ERROR ? "Div Error" : \ -(r) == GO_RESULT_SINGULAR ? "Singular" : \ -(r) == GO_RESULT_NO_SPACE ? "No Space" : \ -(r) == GO_RESULT_EMPTY ? "Empty" : \ -(r) == GO_RESULT_BUG ? "Bug" : "?" - -/*! - Joints are characterized by the quantities they affect, such as - length for linear joints and angle for rotary joints. -*/ -enum { - GO_QUANTITY_NONE = 0, - GO_QUANTITY_LENGTH, - GO_QUANTITY_ANGLE -}; - -#define go_quantity_to_string(q) \ -(q) == GO_QUANTITY_LENGTH ? "Length" : \ -(q) == GO_QUANTITY_ANGLE ? "Angle" : "None" - -/* go_real: float, long double, default double; GO_INF is defined as - the associated max value from float.h */ - -/* - In IEEE floating point, - - FLT_MIN = 1.175494e-38, FLT_EPSILON 1.192093e-07 - DBL_MIN = 2.225074e-308, DBL_EPSILON 2.220446e-16 -*/ - -#if defined(GO_REAL_FLOAT) -typedef float go_real; -#define GO_REAL go_real_float -extern int go_real_float; -#define GO_REAL_MIN FLT_MIN -#define GO_REAL_MAX FLT_MAX -#define GO_REAL_EPSILON (1.0e-4) -#define GO_INF FLT_MAX - -#elif defined(GO_REAL_LONG_DOUBLE) -typedef long double go_real; -#define GO_REAL go_real_long_double -extern int go_real_long_double; -#define GO_REAL_MIN DBL_MIN -#define GO_REAL_MAX DBL_MAX -#define GO_REAL_EPSILON (1.0e-10) -#define GO_INF DBL_MAX - -#else -#define GO_REAL_DOUBLE -typedef double go_real; -#define GO_REAL go_real_double -extern int go_real_double; -#define GO_REAL_MIN DBL_MIN -#define GO_REAL_MAX DBL_MAX -#define GO_REAL_EPSILON (1.0e-7) -#define GO_INF DBL_MAX -#endif - -/* go_integer: short, long, long long, default int */ - -#if defined(GO_INTEGER_SHORT) -typedef short int go_integer; -#define GO_INTEGER go_integer_short -extern int go_integer_short; -#if defined(SHRT_MAX) -#define GO_INTEGER_MAX SHRT_MAX -#endif - -#elif defined(GO_INTEGER_LONG) -typedef long int go_integer; -#define GO_INTEGER go_integer_long -extern int go_integer_long; -#if defined(LONG_MAX) -#define GO_INTEGER_MAX LONG_MAX -#endif - -#elif defined(GO_INTEGER_LONG_LONG) -typedef long long int go_integer; -#define GO_INTEGER go_integer_long_long -extern int go_integer_long_long; -#if defined(LONG_MAX) -#define GO_INTEGER_MAX LONG_MAX -#endif - -#else -#define GO_INTEGER_INT -typedef int go_integer; -#define GO_INTEGER go_integer_int -extern int go_integer_int; -#if defined(INT_MAX) -#define GO_INTEGER_MAX INT_MAX -#endif -#endif - -/* go_flag: unsigned short, unsigned int, default unsigned char */ - -#if defined(GO_FLAG_USHORT) -typedef unsigned short go_flag; -#define GO_FLAG go_flag_ushort -extern int go_flag_ushort; - -#elif defined(GO_FLAG_UINT) -typedef unsigned int go_flag; -#define GO_FLAG go_flag_uint -extern int go_flag_uint; - -#else -#define GO_FLAG_UCHAR -typedef unsigned char go_flag; -#define GO_FLAG go_flag_uchar -extern int go_flag_uchar; -#endif - -#endif /* GO_TYPES_H */ From 1b3a7fab1ec2e5264a8a58bf617d989834ee7862 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 13 Oct 2026 00:01:16 +1100 Subject: [PATCH 3/4] posemath: take the parallel checks over from blendmath pmCartCartParallel() and pmCartCartAntiParallel() are vector helpers with nothing of the planner in them, so they go with the rest of the PmCartesian functions. The debug JSON output they wrote goes with the move, since posemath has no planner debug output. --- include/posemath.h | 2 ++ src/emc/tp/blendmath.c | 49 ------------------------------------- src/emc/tp/blendmath.h | 8 ------ src/libposemath/_posemath.c | 37 ++++++++++++++++++++++++++++ 4 files changed, 39 insertions(+), 57 deletions(-) diff --git a/include/posemath.h b/include/posemath.h index 51ea880eb59..a2ef710e1a4 100644 --- a/include/posemath.h +++ b/include/posemath.h @@ -178,6 +178,8 @@ __attribute__((always_inline)) static inline double pmSq(double x) { return x*x; extern int pmCartMag(PmCartesian const * const, double * const); extern int pmCartMagSq(PmCartesian const * const, double * const); extern int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2, double *d); + extern int pmCartCartParallel(PmCartesian const * const u1, PmCartesian const * const u2, double tol); + extern int pmCartCartAntiParallel(PmCartesian const * const u1, PmCartesian const * const u2, double tol); extern int pmCartCartAdd(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); extern int pmCartCartSub(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); extern int pmCartScalMult(PmCartesian const * const, double, PmCartesian * const); diff --git a/src/emc/tp/blendmath.c b/src/emc/tp/blendmath.c index da14de3f0ef..3493890d34d 100644 --- a/src/emc/tp/blendmath.c +++ b/src/emc/tp/blendmath.c @@ -321,55 +321,6 @@ int checkTangentAngle(PmCircle const * const circ, SphericalArc const * const ar } -/** - * Checks if two UNIT vectors are parallel to the given angle tolerance (in radians). - * @warning tol depends on the small angle approximation and will not be - * accurate for angles larger than about 10 deg. This function is meant for - * small tolerances! - */ -int pmCartCartParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol) -{ - double d_diff; - { - PmCartesian u_diff; - pmCartCartSub(u1, u2, &u_diff); - pmCartMagSq(&u_diff, &d_diff); - } - - tp_debug_json_start(pmCartCartParallel); - tp_debug_json_double(d_diff); - tp_debug_json_end(); - - return d_diff < tol; -} - -/** - * Checks if two UNIT vectors are anti-parallel to the given angle tolerance (in radians). - * @warning tol depends on the small angle approximation and will not be - * accurate for angles larger than about 10 deg. This function is meant for - * small tolerances! - */ -int pmCartCartAntiParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol) -{ - double d_sum; - { - PmCartesian u_sum; - pmCartCartAdd(u1, u2, &u_sum); - pmCartMagSq(&u_sum, &d_sum); - } - - tp_debug_json_start(pmCartCartAntiParallel); - tp_debug_json_double(d_sum); - tp_debug_json_end(); - - return d_sum < tol; -} - - /** * Check if two cartesian vectors are parallel or anti-parallel * The input tolerance specifies what the maximum angle between the diff --git a/src/emc/tp/blendmath.h b/src/emc/tp/blendmath.h index f89efb70d2c..899e7863bfe 100644 --- a/src/emc/tp/blendmath.h +++ b/src/emc/tp/blendmath.h @@ -155,14 +155,6 @@ int findAccelScale(PmCartesian const * const acc, int pmUnitCartsColinear(PmCartesian const * const u1, PmCartesian const * const u2); -int pmCartCartParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol); - -int pmCartCartAntiParallel(PmCartesian const * const u1, - PmCartesian const * const u2, - double tol); - int pmCircLineCoplanar(PmCircle const * const circ, PmCartLine const * const line, double tol); diff --git a/src/libposemath/_posemath.c b/src/libposemath/_posemath.c index 2dc47db84a5..d2f7381cf36 100644 --- a/src/libposemath/_posemath.c +++ b/src/libposemath/_posemath.c @@ -890,6 +890,43 @@ int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2, return pmErrno = PM_OK; } +/** + * Checks if two UNIT vectors are parallel to the given tolerance, the square + * of the angle between them in radians. + * @warning tol depends on the small angle approximation and will not be + * accurate for angles larger than about 10 deg. This function is meant for + * small tolerances! + */ +int pmCartCartParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) +{ + double d_diff; + PmCartesian u_diff; + + pmCartCartSub(u1, u2, &u_diff); + pmCartMagSq(&u_diff, &d_diff); + + return d_diff < tol; +} + +/** + * Checks if two UNIT vectors are anti-parallel to the given tolerance, as + * pmCartCartParallel(). + */ +int pmCartCartAntiParallel(PmCartesian const * const u1, + PmCartesian const * const u2, + double tol) +{ + double d_sum; + PmCartesian u_sum; + + pmCartCartAdd(u1, u2, &u_sum); + pmCartMagSq(&u_sum, &d_sum); + + return d_sum < tol; +} + int pmCartCartAdd(PmCartesian const * const v1, PmCartesian const * const v2, PmCartesian * const vout) { From 3fdd1a8abc68fcdc3af08308cf35b60473237c25 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 13 Oct 2026 00:01:16 +1100 Subject: [PATCH 4/4] tests: check the posemath parallel tests as a realtime component tests/blendmath compiled blendmath.c into a userspace program with -DULAPI. Since blendmath.c exports pmCircleEffectiveMinRadius() (#4643), EXPORT_SYMBOL is undefined there, which GCC 13 lets through as an implicit int and GCC 14 rejects. The two functions it checks are posemath's now, so it becomes tests/posemath-parallel: a component loaded after tpmod, which exports them, the way tests/kins-jacobian checks the kinematics modules. A failed check fails the load. The checks themselves are unchanged. --- src/emc/tp/tpmod.c | 6 ++ tests/blendmath/checkresult | 2 - tests/blendmath/skip | 5 -- tests/blendmath/test.sh | 17 ---- tests/blendmath/test_blendmath.c | 93 --------------------- tests/posemath-parallel/checkresult | 2 + tests/posemath-parallel/parallelcheck.c | 102 ++++++++++++++++++++++++ tests/posemath-parallel/skip | 4 + tests/posemath-parallel/test.sh | 11 +++ 9 files changed, 125 insertions(+), 117 deletions(-) delete mode 100755 tests/blendmath/checkresult delete mode 100755 tests/blendmath/skip delete mode 100755 tests/blendmath/test.sh delete mode 100644 tests/blendmath/test_blendmath.c create mode 100755 tests/posemath-parallel/checkresult create mode 100644 tests/posemath-parallel/parallelcheck.c create mode 100755 tests/posemath-parallel/skip create mode 100755 tests/posemath-parallel/test.sh diff --git a/src/emc/tp/tpmod.c b/src/emc/tp/tpmod.c index 7058cdc6760..ff579e6b910 100644 --- a/src/emc/tp/tpmod.c +++ b/src/emc/tp/tpmod.c @@ -9,8 +9,14 @@ #include #include #include +#include MODULE_LICENSE("GPL"); +// posemath is linked into each module that uses it. These two are +// exported so a component loaded after tpmod can check them. +EXPORT_SYMBOL(pmCartCartParallel); +EXPORT_SYMBOL(pmCartCartAntiParallel); + #define TPMOD_DEBUG #undef TPMOD_DEBUG diff --git a/tests/blendmath/checkresult b/tests/blendmath/checkresult deleted file mode 100755 index 92099410a82..00000000000 --- a/tests/blendmath/checkresult +++ /dev/null @@ -1,2 +0,0 @@ -#!/bin/sh -grep -q "all blendmath checks passed" "$1" && ! grep -q "FAIL" "$1" diff --git a/tests/blendmath/skip b/tests/blendmath/skip deleted file mode 100755 index 3527d670020..00000000000 --- a/tests/blendmath/skip +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/sh -# This test compiles blendmath.c and internal TP headers from the source -# tree, which is only available in run-in-place builds. Skip when testing -# installed packages. -[ -z "$SYSTEM_BUILD" ] diff --git a/tests/blendmath/test.sh b/tests/blendmath/test.sh deleted file mode 100755 index 92f9b003661..00000000000 --- a/tests/blendmath/test.sh +++ /dev/null @@ -1,17 +0,0 @@ -#!/bin/bash -set -e - -# RIP layout: $HEADERS is $TOPDIR/include -TOPDIR=$(dirname "$HEADERS") - -# blendmath.c references the rest of the TP (tc.c, spherical_arc.c, -# sp_scurve.c, motion globals). Only the tested functions are used here, so -# build with function sections and let the linker garbage-collect the rest -# instead of linking the whole trajectory planner. -gcc -O2 -Wall -ffunction-sections -fdata-sections -DULAPI -DUNIT_TEST \ - -I"$HEADERS" -I"$TOPDIR/src" -I"$TOPDIR/src/emc" \ - -o test_blendmath test_blendmath.c "$TOPDIR/src/emc/tp/blendmath.c" \ - -L"$LIBDIR" -Wl,-rpath,"$LIBDIR" -Wl,--gc-sections -lposemath -lm - -./test_blendmath -rm -f test_blendmath diff --git a/tests/blendmath/test_blendmath.c b/tests/blendmath/test_blendmath.c deleted file mode 100644 index 51f86b6ecde..00000000000 --- a/tests/blendmath/test_blendmath.c +++ /dev/null @@ -1,93 +0,0 @@ -/* - * Unit tests for blendmath utility functions. - * - * Salvaged from the retired meson-only unit_tests/tp/test_blendmath.c - * (originally by Robert W. Ellenberg) and converted from the greatest - * framework to plain C so it can build and run as a runtest. - */ - -#include -#include -#include - -#include -#include -#include "tp/blendmath.h" -#include "tp/tp_types.h" - -/* kludge: satisfy rtapi_print_msg references without an rtapi app */ -void rtapi_print_msg(msg_level_t level, const char *fmt, ...) -{ - va_list args; - - (void)level; - va_start(args, fmt); - vprintf(fmt, args); - va_end(args); -} - -static int failures = 0; - -#define CHECK(cond) do { \ - if (!(cond)) { \ - printf("FAIL %s:%d: %s\n", __FILE__, __LINE__, #cond); \ - failures++; \ - } \ -} while (0) - -#define CHECK_FALSE(cond) CHECK(!(cond)) - -static void test_pmCartCartParallel_numerical(void) -{ - PmCartesian u0 = {1, 0, 0}; - PmCartesian u_close = {cos(TP_ANGLE_EPSILON), sin(TP_ANGLE_EPSILON), 0}; - int k; - - CHECK(pmCartCartParallel(&u0, &u0, TP_ANGLE_EPSILON_SQ)); - CHECK_FALSE(pmCartCartParallel(&u0, &u_close, 0.0)); - - /* Test that the tolerance makes sense */ - CHECK_FALSE(pmCartCartParallel(&u0, &u_close, 0.5 * TP_ANGLE_EPSILON_SQ)); - CHECK(pmCartCartParallel(&u0, &u_close, 1.5 * TP_ANGLE_EPSILON_SQ)); - - /* Try a bunch of other angles including anti-parallel */ - for (k = 1; k <= 7; ++k) { - PmCartesian u_far = {cos(PM_PI_4 * k), sin(PM_PI_4 * k), 0}; - CHECK_FALSE(pmCartCartParallel(&u0, &u_far, TP_ANGLE_EPSILON_SQ)); - } -} - -static void test_pmCartCartAntiParallel_numerical(void) -{ - PmCartesian u0 = {1, 0, 0}; - PmCartesian u_close = {-cos(TP_ANGLE_EPSILON), sin(TP_ANGLE_EPSILON), 0}; - PmCartesian u_opposite; - int k; - - pmCartScalMult(&u0, -1.0, &u_opposite); - CHECK(pmCartCartAntiParallel(&u0, &u_opposite, TP_ANGLE_EPSILON_SQ)); - CHECK_FALSE(pmCartCartAntiParallel(&u0, &u_close, 0.0)); - - /* Test that the tolerance makes sense */ - CHECK_FALSE(pmCartCartAntiParallel(&u0, &u_close, 0.5 * TP_ANGLE_EPSILON_SQ)); - CHECK(pmCartCartAntiParallel(&u0, &u_close, 1.5 * TP_ANGLE_EPSILON_SQ)); - - /* Try a bunch of other angles including anti-parallel */ - for (k = 1; k <= 7; ++k) { - PmCartesian u_far = {-cos(PM_PI_4 * k), sin(PM_PI_4 * k), 0}; - CHECK_FALSE(pmCartCartAntiParallel(&u0, &u_far, TP_ANGLE_EPSILON_SQ)); - } -} - -int main(void) -{ - test_pmCartCartParallel_numerical(); - test_pmCartCartAntiParallel_numerical(); - - if (failures) { - printf("%d check(s) FAILED\n", failures); - return 1; - } - printf("all blendmath checks passed\n"); - return 0; -} diff --git a/tests/posemath-parallel/checkresult b/tests/posemath-parallel/checkresult new file mode 100755 index 00000000000..254762c99c9 --- /dev/null +++ b/tests/posemath-parallel/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +grep -q "all parallel checks passed" "$1" && ! grep -q "FAIL" "$1" diff --git a/tests/posemath-parallel/parallelcheck.c b/tests/posemath-parallel/parallelcheck.c new file mode 100644 index 00000000000..6aa323a76e8 --- /dev/null +++ b/tests/posemath-parallel/parallelcheck.c @@ -0,0 +1,102 @@ +/* + * Checks of the posemath parallel and anti-parallel vector tests. + * + * Salvaged from the retired meson-only unit_tests/tp/test_blendmath.c + * (originally by Robert W. Ellenberg). Loaded after tpmod, which exports + * the two functions; a failed check fails the load, and a failed load + * fails the test. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + */ + +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("posemath parallel check"); + +/* the tolerance the planner's blends use, ANGLE_EPSILON */ +#define ANGLE_EPSILON 1e-6 +#define ANGLE_EPSILON_SQ (ANGLE_EPSILON * ANGLE_EPSILON) + +static int comp_id = -1; +static int failures = 0; + +#define CHECK(cond) do { \ + if (!(cond)) { \ + rtapi_print_msg(RTAPI_MSG_ERR, "parallelcheck: FAIL line %d: %s\n", \ + __LINE__, #cond); \ + failures++; \ + } \ +} while (0) + +#define CHECK_FALSE(cond) CHECK(!(cond)) + +static void test_pmCartCartParallel_numerical(void) +{ + PmCartesian u0 = {1, 0, 0}; + PmCartesian u_close = {cos(ANGLE_EPSILON), sin(ANGLE_EPSILON), 0}; + int k; + + CHECK(pmCartCartParallel(&u0, &u0, ANGLE_EPSILON_SQ)); + CHECK_FALSE(pmCartCartParallel(&u0, &u_close, 0.0)); + + /* Test that the tolerance makes sense */ + CHECK_FALSE(pmCartCartParallel(&u0, &u_close, 0.5 * ANGLE_EPSILON_SQ)); + CHECK(pmCartCartParallel(&u0, &u_close, 1.5 * ANGLE_EPSILON_SQ)); + + /* Try a bunch of other angles including anti-parallel */ + for (k = 1; k <= 7; ++k) { + PmCartesian u_far = {cos(PM_PI_4 * k), sin(PM_PI_4 * k), 0}; + CHECK_FALSE(pmCartCartParallel(&u0, &u_far, ANGLE_EPSILON_SQ)); + } +} + +static void test_pmCartCartAntiParallel_numerical(void) +{ + PmCartesian u0 = {1, 0, 0}; + PmCartesian u_close = {-cos(ANGLE_EPSILON), sin(ANGLE_EPSILON), 0}; + PmCartesian u_opposite = {-1, 0, 0}; + int k; + + CHECK(pmCartCartAntiParallel(&u0, &u_opposite, ANGLE_EPSILON_SQ)); + CHECK_FALSE(pmCartCartAntiParallel(&u0, &u_close, 0.0)); + + /* Test that the tolerance makes sense */ + CHECK_FALSE(pmCartCartAntiParallel(&u0, &u_close, 0.5 * ANGLE_EPSILON_SQ)); + CHECK(pmCartCartAntiParallel(&u0, &u_close, 1.5 * ANGLE_EPSILON_SQ)); + + /* Try a bunch of other angles including anti-parallel */ + for (k = 1; k <= 7; ++k) { + PmCartesian u_far = {-cos(PM_PI_4 * k), sin(PM_PI_4 * k), 0}; + CHECK_FALSE(pmCartCartAntiParallel(&u0, &u_far, ANGLE_EPSILON_SQ)); + } +} + +int rtapi_app_main(void) +{ + comp_id = hal_init("parallelcheck"); + if (comp_id < 0) { return comp_id; } + + test_pmCartCartParallel_numerical(); + test_pmCartCartAntiParallel_numerical(); + + if (failures) { + rtapi_print_msg(RTAPI_MSG_ERR, "parallelcheck: %d check(s) failed\n", + failures); + hal_exit(comp_id); + return -ERANGE; + } + + rtapi_print("parallelcheck: all parallel checks passed\n"); + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/posemath-parallel/skip b/tests/posemath-parallel/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/posemath-parallel/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/posemath-parallel/test.sh b/tests/posemath-parallel/test.sh new file mode 100755 index 00000000000..6b1c2a0cb81 --- /dev/null +++ b/tests/posemath-parallel/test.sh @@ -0,0 +1,11 @@ +#!/bin/bash +set -e + +halcompile --install parallelcheck.c >/dev/null + +# tpmod exports the two checks; posemath is linked into each module that +# uses it rather than loaded as a module of its own +hal=$(mktemp --suffix=.hal) +printf 'loadrt tpmod\nloadrt parallelcheck\n' > "$hal" +halrun -f "$hal" +rm -f "$hal"