/src/libnml/posemath/_posemath.c
C | 1740 lines | 1285 code | 342 blank | 113 comment | 164 complexity | 29d70c9cb35ce7686124ae4f7c902763 MD5 | raw file
- /********************************************************************
- * Description: _posemath.c
- * C definitions for pose math library data types and manipulation
- * functions.
- *
- * Derived from a work by Fred Proctor & Will Shackleford
- *
- * Author:
- * License: LGPL Version 2
- * System: Linux
- *
- * Copyright (c) 2004 All rights reserved.
- *
- * Last change:
- ********************************************************************/
- #if defined(PM_PRINT_ERROR) && defined(rtai)
- #undef PM_PRINT_ERROR
- #endif
- #if defined(PM_DEBUG) && defined(rtai)
- #undef PM_DEBUG
- #endif
- #ifdef PM_PRINT_ERROR
- #define PM_DEBUG /* have to have debug with printing */
- #include <stdio.h>
- #include <stdarg.h>
- #endif
- #include "posemath.h"
- #include "rtapi_math.h"
- #include <float.h>
- #include "sincos.h"
- /* global error number */
- int pmErrno = 0;
- #ifdef PM_PRINT_ERROR
- void pmPrintError(const char *fmt, ...)
- {
- va_list args;
- va_start(args, fmt);
- vfprintf(stderr, fmt, args);
- va_end(args);
- }
- /* error printing function */
- void pmPerror(const char *s)
- {
- char *pmErrnoString;
- switch (pmErrno) {
- case 0:
- /* no error */
- return;
- case PM_ERR:
- pmErrnoString = "unspecified error";
- break;
- case PM_IMPL_ERR:
- pmErrnoString = "not implemented";
- break;
- case PM_NORM_ERR:
- pmErrnoString = "expected normalized value";
- break;
- case PM_DIV_ERR:
- pmErrnoString = "divide by zero";
- break;
- default:
- pmErrnoString = "unassigned error";
- break;
- }
- if (s != 0 && s[0] != 0) {
- fprintf(stderr, "%s: %s\n", s, pmErrnoString);
- } else {
- fprintf(stderr, "%s\n", pmErrnoString);
- }
- }
- #endif /* PM_PRINT_ERROR */
- /* fuzz checker */
- #define IS_FUZZ(a,fuzz) (fabs(a) < (fuzz) ? 1 : 0)
- /* Pose Math Basis Functions */
- /* Scalar functions */
- double pmSqrt(double x)
- {
- if (x > 0.0) {
- return sqrt(x);
- }
- if (x > SQRT_FUZZ) {
- return 0.0;
- }
- #ifdef PM_PRINT_ERROR
- pmPrintError("sqrt of large negative number\n");
- #endif
- return 0.0;
- }
- /* Translation rep conversion functions */
- int pmCartSphConvert(PmCartesian v, PmSpherical * s)
- {
- double _r;
- s->theta = atan2(v.y, v.x);
- s->r = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));
- _r = pmSqrt(pmSq(v.x) + pmSq(v.y));
- s->phi = atan2(_r, v.z);
- return pmErrno = 0;
- }
- int pmCartCylConvert(PmCartesian v, PmCylindrical * c)
- {
- c->theta = atan2(v.y, v.x);
- c->r = pmSqrt(pmSq(v.x) + pmSq(v.y));
- c->z = v.z;
- return pmErrno = 0;
- }
- int pmSphCartConvert(PmSpherical s, PmCartesian * v)
- {
- double _r;
- _r = s.r * sin(s.phi);
- v->z = s.r * cos(s.phi);
- v->x = _r * cos(s.theta);
- v->y = _r * sin(s.theta);
- return pmErrno = 0;
- }
- int pmSphCylConvert(PmSpherical s, PmCylindrical * c)
- {
- c->theta = s.theta;
- c->r = s.r * cos(s.phi);
- c->z = s.r * sin(s.phi);
- return pmErrno = 0;
- }
- int pmCylCartConvert(PmCylindrical c, PmCartesian * v)
- {
- v->x = c.r * cos(c.theta);
- v->y = c.r * sin(c.theta);
- v->z = c.z;
- return pmErrno = 0;
- }
- int pmCylSphConvert(PmCylindrical c, PmSpherical * s)
- {
- s->theta = c.theta;
- s->r = pmSqrt(pmSq(c.r) + pmSq(c.z));
- s->phi = atan2(c.z, c.r);
- return pmErrno = 0;
- }
- /* Rotation rep conversion functions */
- int pmAxisAngleQuatConvert(PmAxis axis, double a, PmQuaternion * q)
- {
- double sh;
- a *= 0.5;
- sincos(a, &sh, &(q->s));
- switch (axis) {
- case PM_X:
- q->x = sh;
- q->y = 0.0;
- q->z = 0.0;
- break;
- case PM_Y:
- q->x = 0.0;
- q->y = sh;
- q->z = 0.0;
- break;
- case PM_Z:
- q->x = 0.0;
- q->y = 0.0;
- q->z = sh;
- break;
- default:
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: bad axis in pmAxisAngleQuatConvert\n");
- #endif
- return -1;
- }
- if (q->s < 0.0) {
- q->s *= -1.0;
- q->x *= -1.0;
- q->y *= -1.0;
- q->z *= -1.0;
- }
- return 0;
- }
- int pmRotQuatConvert(PmRotationVector r, PmQuaternion * q)
- {
- double sh;
- #ifdef PM_DEBUG
- /* make sure r is normalized */
- if (0 != pmRotNorm(r, &r)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError
- ("error: pmRotQuatConvert rotation vector not normalized\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- if (pmClose(r.s, 0.0, QS_FUZZ)) {
- q->s = 1.0;
- q->x = q->y = q->z = 0.0;
- return pmErrno = 0;
- }
- sincos(r.s / 2.0, &sh, &(q->s));
- if (q->s >= 0.0) {
- q->x = r.x * sh;
- q->y = r.y * sh;
- q->z = r.z * sh;
- } else {
- q->s *= -1;
- q->x = -r.x * sh;
- q->y = -r.y * sh;
- q->z = -r.z * sh;
- }
- return pmErrno = 0;
- }
- int pmRotMatConvert(PmRotationVector r, PmRotationMatrix * m)
- {
- double s, c, omc;
- #ifdef PM_DEBUG
- if (!pmRotIsNorm(r)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad vector in pmRotMatConvert\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- sincos(r.s, &s, &c);
- /* from space book */
- m->x.x = c + pmSq(r.x) * (omc = 1 - c); /* omc = One Minus Cos */
- m->y.x = -r.z * s + r.x * r.y * omc;
- m->z.x = r.y * s + r.x * r.z * omc;
- m->x.y = r.z * s + r.y * r.x * omc;
- m->y.y = c + pmSq(r.y) * omc;
- m->z.y = -r.x * s + r.y * r.z * omc;
- m->x.z = -r.y * s + r.z * r.x * omc;
- m->y.z = r.x * s + r.z * r.y * omc;
- m->z.z = c + pmSq(r.z) * omc;
- return pmErrno = 0;
- }
- int pmRotZyzConvert(PmRotationVector r, PmEulerZyz * zyz)
- {
- #ifdef PM_DEBUG
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmRotZyzConvert not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- #else
- return PM_IMPL_ERR;
- #endif
- }
- int pmRotZyxConvert(PmRotationVector r, PmEulerZyx * zyx)
- {
- PmRotationMatrix m;
- int r1, r2;
- r1 = pmRotMatConvert(r, &m);
- r2 = pmMatZyxConvert(m, zyx);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmRotRpyConvert(PmRotationVector r, PmRpy * rpy)
- {
- PmQuaternion q;
- int r1, r2;
- q.s = q.x = q.y = q.z = 0.0;
- r1 = pmRotQuatConvert(r, &q);
- r2 = pmQuatRpyConvert(q, rpy);
- return r1 || r2 ? pmErrno : 0;
- }
- int pmQuatRotConvert(PmQuaternion q, PmRotationVector * r)
- {
- double sh;
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(q)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmQuatRotConvert\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- if (r == 0) {
- return (pmErrno = PM_ERR);
- }
- sh = pmSqrt(pmSq(q.x) + pmSq(q.y) + pmSq(q.z));
- if (sh > QSIN_FUZZ) {
- r->s = 2.0 * atan2(sh, q.s);
- r->x = q.x / sh;
- r->y = q.y / sh;
- r->z = q.z / sh;
- } else {
- r->s = 0.0;
- r->x = 0.0;
- r->y = 0.0;
- r->z = 0.0;
- }
- return pmErrno = 0;
- }
- int pmQuatMatConvert(PmQuaternion q, PmRotationMatrix * m)
- {
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(q)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmQuatMatConvert\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- /* from space book where e1=q.x e2=q.y e3=q.z e4=q.s */
- m->x.x = 1 - 2 * (pmSq(q.y) + pmSq(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 * (pmSq(q.z) + pmSq(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 * (pmSq(q.x) + pmSq(q.y));
- return pmErrno = 0;
- }
- int pmQuatZyzConvert(PmQuaternion q, PmEulerZyz * zyz)
- {
- PmRotationMatrix m;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmQuatMatConvert(q, &m);
- r2 = pmMatZyzConvert(m, zyz);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmQuatZyxConvert(PmQuaternion q, PmEulerZyx * zyx)
- {
- PmRotationMatrix m;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmQuatMatConvert(q, &m);
- r2 = pmMatZyxConvert(m, zyx);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmQuatRpyConvert(PmQuaternion q, PmRpy * rpy)
- {
- PmRotationMatrix m;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmQuatMatConvert(q, &m);
- r2 = pmMatRpyConvert(m, rpy);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmMatRotConvert(PmRotationMatrix m, PmRotationVector * r)
- {
- PmQuaternion q;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmMatQuatConvert(m, &q);
- r2 = pmQuatRotConvert(q, r);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmMatQuatConvert(PmRotationMatrix m, PmQuaternion * q)
- {
- /*
- from Stephe's "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 generallities
- 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 larget 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 */
- double a;
- #ifdef PM_DEBUG
- if (!pmMatIsNorm(m)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad matrix in pmMatQuatConvert\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- q->s = 0.5 * pmSqrt(1.0 + m.x.x + m.y.y + m.z.z);
- if (fabs(q->s) > QS_FUZZ) {
- 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;
- } else {
- q->s = 0;
- q->x = pmSqrt(1.0 + m.x.x - m.y.y - m.z.z) / 2.0;
- q->y = pmSqrt(1.0 + m.y.y - m.x.x - m.z.z) / 2.0;
- q->z = pmSqrt(1.0 + m.z.z - m.y.y - m.x.x) / 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;
- }
- }
- }
- return pmQuatNorm(*q, q);
- }
- int pmMatZyzConvert(PmRotationMatrix m, PmEulerZyz * zyz)
- {
- zyz->y = atan2(pmSqrt(pmSq(m.x.z) + pmSq(m.y.z)), m.z.z);
- if (fabs(zyz->y) < ZYZ_Y_FUZZ) {
- zyz->z = 0.0;
- zyz->y = 0.0; /* force Y to 0 */
- zyz->zp = atan2(-m.y.x, m.x.x);
- } else if (fabs(zyz->y - PM_PI) < ZYZ_Y_FUZZ) {
- zyz->z = 0.0;
- zyz->y = PM_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 pmErrno = 0;
- }
- int pmMatZyxConvert(PmRotationMatrix m, PmEulerZyx * zyx)
- {
- zyx->y = atan2(-m.x.z, pmSqrt(pmSq(m.x.x) + pmSq(m.x.y)));
- if (fabs(zyx->y - (2 * PM_PI)) < ZYX_Y_FUZZ) {
- zyx->z = 0.0;
- zyx->y = (2 * PM_PI); /* force it */
- zyx->x = atan2(m.y.x, m.y.y);
- } else if (fabs(zyx->y + (2 * PM_PI)) < ZYX_Y_FUZZ) {
- zyx->z = 0.0;
- zyx->y = -(2 * PM_PI); /* 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 pmErrno = 0;
- }
- int pmMatRpyConvert(PmRotationMatrix m, PmRpy * rpy)
- {
- rpy->p = atan2(-m.x.z, pmSqrt(pmSq(m.x.x) + pmSq(m.x.y)));
- if (fabs(rpy->p - (2 * PM_PI)) < RPY_P_FUZZ) {
- rpy->r = atan2(m.y.x, m.y.y);
- rpy->p = (2 * PM_PI); /* force it */
- rpy->y = 0.0;
- } else if (fabs(rpy->p + (2 * PM_PI)) < RPY_P_FUZZ) {
- rpy->r = -atan2(m.y.z, m.y.y);
- rpy->p = -(2 * PM_PI); /* force it */
- rpy->y = 0.0;
- } else {
- rpy->r = atan2(m.y.z, m.z.z);
- rpy->y = atan2(m.x.y, m.x.x);
- }
- return pmErrno = 0;
- }
- int pmZyzRotConvert(PmEulerZyz zyz, PmRotationVector * r)
- {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmZyzRotConvert not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- }
- int pmZyzQuatConvert(PmEulerZyz zyz, PmQuaternion * q)
- {
- PmRotationMatrix m;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmZyzMatConvert(zyz, &m);
- r2 = pmMatQuatConvert(m, q);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmZyzMatConvert(PmEulerZyz zyz, PmRotationMatrix * m)
- {
- double sa, sb, sg;
- double 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 pmErrno = 0;
- }
- int pmZyzRpyConvert(PmEulerZyz zyz, PmRpy * rpy)
- {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmZyzRpyConvert not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- }
- int pmZyxRotConvert(PmEulerZyx zyx, PmRotationVector * r)
- {
- PmRotationMatrix m;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmZyxMatConvert(zyx, &m);
- r2 = pmMatRotConvert(m, r);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmZyxQuatConvert(PmEulerZyx zyx, PmQuaternion * q)
- {
- PmRotationMatrix m;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmZyxMatConvert(zyx, &m);
- r2 = pmMatQuatConvert(m, q);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmZyxMatConvert(PmEulerZyx zyx, PmRotationMatrix * m)
- {
- double sa, sb, sg;
- double 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 pmErrno = 0;
- }
- int pmZyxZyzConvert(PmEulerZyx zyx, PmEulerZyz * zyz)
- {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmZyxZyzConvert not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- }
- int pmZyxRpyConvert(PmEulerZyx zyx, PmRpy * rpy)
- {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmZyxRpyConvert not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- }
- int pmRpyRotConvert(PmRpy rpy, PmRotationVector * r)
- {
- PmQuaternion q;
- int r1, r2;
- q.s = q.x = q.y = q.z = 0.0;
- r->s = r->x = r->y = r->z = 0.0;
- r1 = pmRpyQuatConvert(rpy, &q);
- r2 = pmQuatRotConvert(q, r);
- return r1 || r2 ? pmErrno : 0;
- }
- int pmRpyQuatConvert(PmRpy rpy, PmQuaternion * q)
- {
- PmRotationMatrix m;
- int r1, r2;
- /*! \todo FIXME-- need direct equations */
- r1 = pmRpyMatConvert(rpy, &m);
- r2 = pmMatQuatConvert(m, q);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- int pmRpyMatConvert(PmRpy rpy, PmRotationMatrix * m)
- {
- double sa, sb, sg;
- double 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 pmErrno = 0;
- }
- int pmRpyZyzConvert(PmRpy rpy, PmEulerZyz * zyz)
- {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmRpyZyzConvert not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- }
- int pmRpyZyxConvert(PmRpy rpy, PmEulerZyx * zyx)
- {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmRpyZyxConvert not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- }
- int pmPoseHomConvert(PmPose p, PmHomogeneous * h)
- {
- int r1;
- h->tran = p.tran;
- r1 = pmQuatMatConvert(p.rot, &h->rot);
- return pmErrno = r1;
- }
- int pmHomPoseConvert(PmHomogeneous h, PmPose * p)
- {
- int r1;
- p->tran = h.tran;
- r1 = pmMatQuatConvert(h.rot, &p->rot);
- return pmErrno = r1;
- }
- /* PmCartesian functions */
- int pmCartCartCompare(PmCartesian v1, PmCartesian v2)
- {
- if (fabs(v1.x - v2.x) >= V_FUZZ ||
- fabs(v1.y - v2.y) >= V_FUZZ || fabs(v1.z - v2.z) >= V_FUZZ) {
- return 0;
- }
- return 1;
- }
- int pmCartCartDot(PmCartesian v1, PmCartesian v2, double *d)
- {
- *d = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
- return pmErrno = 0;
- }
- int pmCartCartCross(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
- {
- vout->x = v1.y * v2.z - v1.z * v2.y;
- vout->y = v1.z * v2.x - v1.x * v2.z;
- vout->z = v1.x * v2.y - v1.y * v2.x;
- return pmErrno = 0;
- }
- int pmCartMag(PmCartesian v, double *d)
- {
- *d = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));
- return pmErrno = 0;
- }
- int pmCartCartDisp(PmCartesian v1, PmCartesian v2, double *d)
- {
- *d = pmSqrt(pmSq(v2.x - v1.x) + pmSq(v2.y - v1.y) + pmSq(v2.z - v1.z));
- return pmErrno = 0;
- }
- int pmCartCartAdd(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
- {
- vout->x = v1.x + v2.x;
- vout->y = v1.y + v2.y;
- vout->z = v1.z + v2.z;
- return pmErrno = 0;
- }
- int pmCartCartSub(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
- {
- vout->x = v1.x - v2.x;
- vout->y = v1.y - v2.y;
- vout->z = v1.z - v2.z;
- return pmErrno = 0;
- }
- int pmCartScalMult(PmCartesian v1, double d, PmCartesian * vout)
- {
- vout->x = v1.x * d;
- vout->y = v1.y * d;
- vout->z = v1.z * d;
- return pmErrno = 0;
- }
- int pmCartScalDiv(PmCartesian v1, double d, PmCartesian * vout)
- {
- if (d == 0.0) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Divide by 0 in pmCartScalDiv\n");
- #endif
- vout->x = DBL_MAX;
- vout->y = DBL_MAX;
- vout->z = DBL_MAX;
- return pmErrno = PM_DIV_ERR;
- }
- vout->x = v1.x / d;
- vout->y = v1.y / d;
- vout->z = v1.z / d;
- return pmErrno = 0;
- }
- int pmCartNeg(PmCartesian v1, PmCartesian * vout)
- {
- vout->x = -v1.x;
- vout->y = -v1.y;
- vout->z = -v1.z;
- return pmErrno = 0;
- }
- int pmCartInv(PmCartesian v1, PmCartesian * vout)
- {
- double size_sq = pmSq(v1.x) + pmSq(v1.y) + pmSq(v1.z);
- if (size_sq == 0.0) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Zero vector in pmCartInv\n");
- #endif
- vout->x = DBL_MAX;
- vout->y = DBL_MAX;
- vout->z = DBL_MAX;
- return pmErrno = PM_NORM_ERR;
- }
- vout->x = v1.x / size_sq;
- vout->y = v1.y / size_sq;
- vout->z = v1.z / size_sq;
- return pmErrno = 0;
- }
- // This used to be called pmCartNorm.
- int pmCartUnit(PmCartesian v, PmCartesian * vout)
- {
- double size = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));
- if (size == 0.0) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Zero vector in pmCartUnit\n");
- #endif
- vout->x = DBL_MAX;
- vout->y = DBL_MAX;
- vout->z = DBL_MAX;
- return pmErrno = PM_NORM_ERR;
- }
- vout->x = v.x / size;
- vout->y = v.y / size;
- vout->z = v.z / size;
- return pmErrno = 0;
- }
- /*! \todo This is if 0'd out so we can find all the pmCartNorm calls that should
- be renamed pmCartUnit.
- Later we'll put this back. */
- #if 0
- int pmCartNorm(PmCartesian v, PmCartesian * vout)
- {
- vout->x = v.x;
- vout->y = v.y;
- vout->z = v.z;
- return pmErrno = 0;
- }
- #endif
- int pmCartIsNorm(PmCartesian v)
- {
- return pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z)) - 1.0 <
- UNIT_VEC_FUZZ ? 1 : 0;
- }
- int pmCartCartProj(PmCartesian v1, PmCartesian v2, PmCartesian * vout)
- {
- int r1, r2, r3;
- double d;
- r1 = pmCartUnit(v2, &v2);
- r2 = pmCartCartDot(v1, v2, &d);
- r3 = pmCartScalMult(v2, d, vout);
- return pmErrno = r1 || r2 || r3 ? PM_NORM_ERR : 0;
- }
- int pmCartPlaneProj(PmCartesian v, PmCartesian normal, PmCartesian * vout)
- {
- int r1, r2;
- PmCartesian par;
- r1 = pmCartCartProj(v, normal, &par);
- r2 = pmCartCartSub(v, par, vout);
- return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
- }
- /* angle-axis functions */
- int pmQuatAxisAngleMult(PmQuaternion q, PmAxis axis, double angle,
- PmQuaternion * pq)
- {
- double sh, ch;
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(q)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: non-unit quaternion in pmQuatAxisAngleMult\n");
- #endif
- return -1;
- }
- #endif
- angle *= 0.5;
- sincos(angle, &sh, &ch);
- switch (axis) {
- case PM_X:
- pq->s = ch * q.s - sh * q.x;
- pq->x = ch * q.x + sh * q.s;
- pq->y = ch * q.y + sh * q.z;
- pq->z = ch * q.z - sh * q.y;
- break;
- case PM_Y:
- pq->s = ch * q.s - sh * q.y;
- pq->x = ch * q.x - sh * q.z;
- pq->y = ch * q.y + sh * q.s;
- pq->z = ch * q.z + sh * q.x;
- break;
- case PM_Z:
- pq->s = ch * q.s - sh * q.z;
- pq->x = ch * q.x + sh * q.y;
- pq->y = ch * q.y - sh * q.x;
- pq->z = ch * q.z + sh * q.s;
- break;
- default:
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: bad axis in pmQuatAxisAngleMult\n");
- #endif
- return -1;
- }
- if (pq->s < 0.0) {
- pq->s *= -1.0;
- pq->x *= -1.0;
- pq->y *= -1.0;
- pq->z *= -1.0;
- }
- return 0;
- }
- /* PmRotationVector functions */
- int pmRotScalMult(PmRotationVector r, double s, PmRotationVector * rout)
- {
- rout->s = r.s * s;
- rout->x = r.x;
- rout->y = r.y;
- rout->z = r.z;
- return pmErrno = 0;
- }
- int pmRotScalDiv(PmRotationVector r, double s, PmRotationVector * rout)
- {
- if (s == 0.0) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Divide by zero in pmRotScalDiv\n");
- #endif
- rout->s = DBL_MAX;
- rout->x = r.x;
- rout->y = r.y;
- rout->z = r.z;
- return pmErrno = PM_NORM_ERR;
- }
- rout->s = r.s / s;
- rout->x = r.x;
- rout->y = r.y;
- rout->z = r.z;
- return pmErrno = 0;
- }
- int pmRotIsNorm(PmRotationVector r)
- {
- if (fabs(r.s) < RS_FUZZ ||
- fabs(pmSqrt(pmSq(r.x) + pmSq(r.y) + pmSq(r.z))) - 1.0 < UNIT_VEC_FUZZ)
- {
- return 1;
- }
- return 0;
- }
- int pmRotNorm(PmRotationVector r, PmRotationVector * rout)
- {
- double size;
- size = pmSqrt(pmSq(r.x) + pmSq(r.y) + pmSq(r.z));
- if (fabs(r.s) < RS_FUZZ) {
- rout->s = 0.0;
- rout->x = 0.0;
- rout->y = 0.0;
- rout->z = 0.0;
- return pmErrno = 0;
- }
- if (size == 0.0) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmRotNorm size is zero\n");
- #endif
- rout->s = 0.0;
- rout->x = 0.0;
- rout->y = 0.0;
- rout->z = 0.0;
- return pmErrno = PM_NORM_ERR;
- }
- rout->s = r.s;
- rout->x = r.x / size;
- rout->y = r.y / size;
- rout->z = r.z / size;
- return pmErrno = 0;
- }
- /* PmRotationMatrix functions */
- int pmMatNorm(PmRotationMatrix m, PmRotationMatrix * mout)
- {
- /*! \todo FIXME */
- *mout = m;
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmMatNorm not implemented\n");
- #endif
- return pmErrno = PM_IMPL_ERR;
- }
- int pmMatIsNorm(PmRotationMatrix m)
- {
- PmCartesian u;
- pmCartCartCross(m.x, m.y, &u);
- return (pmCartIsNorm(m.x) &&
- pmCartIsNorm(m.y) && pmCartIsNorm(m.z) && pmCartCartCompare(u, m.z));
- }
- int pmMatInv(PmRotationMatrix m, PmRotationMatrix * mout)
- {
- /* inverse of a rotation matrix is the transpose */
- mout->x.x = m.x.x;
- mout->x.y = m.y.x;
- mout->x.z = m.z.x;
- mout->y.x = m.x.y;
- mout->y.y = m.y.y;
- mout->y.z = m.z.y;
- mout->z.x = m.x.z;
- mout->z.y = m.y.z;
- mout->z.z = m.z.z;
- return pmErrno = 0;
- }
- int pmMatCartMult(PmRotationMatrix m, PmCartesian v, PmCartesian * vout)
- {
- vout->x = m.x.x * v.x + m.y.x * v.y + m.z.x * v.z;
- vout->y = m.x.y * v.x + m.y.y * v.y + m.z.y * v.z;
- vout->z = m.x.z * v.x + m.y.z * v.y + m.z.z * v.z;
- return pmErrno = 0;
- }
- int pmMatMatMult(PmRotationMatrix m1, PmRotationMatrix m2,
- PmRotationMatrix * mout)
- {
- mout->x.x = m1.x.x * m2.x.x + m1.y.x * m2.x.y + m1.z.x * m2.x.z;
- mout->x.y = m1.x.y * m2.x.x + m1.y.y * m2.x.y + m1.z.y * m2.x.z;
- mout->x.z = m1.x.z * m2.x.x + m1.y.z * m2.x.y + m1.z.z * m2.x.z;
- mout->y.x = m1.x.x * m2.y.x + m1.y.x * m2.y.y + m1.z.x * m2.y.z;
- mout->y.y = m1.x.y * m2.y.x + m1.y.y * m2.y.y + m1.z.y * m2.y.z;
- mout->y.z = m1.x.z * m2.y.x + m1.y.z * m2.y.y + m1.z.z * m2.y.z;
- mout->z.x = m1.x.x * m2.z.x + m1.y.x * m2.z.y + m1.z.x * m2.z.z;
- mout->z.y = m1.x.y * m2.z.x + m1.y.y * m2.z.y + m1.z.y * m2.z.z;
- mout->z.z = m1.x.z * m2.z.x + m1.y.z * m2.z.y + m1.z.z * m2.z.z;
- return pmErrno = 0;
- }
- /* PmQuaternion functions */
- int pmQuatQuatCompare(PmQuaternion q1, PmQuaternion q2)
- {
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmQuatQuatCompare\n");
- #endif
- }
- #endif
- if (fabs(q1.s - q2.s) < Q_FUZZ &&
- fabs(q1.x - q2.x) < Q_FUZZ &&
- fabs(q1.y - q2.y) < Q_FUZZ && fabs(q1.z - q2.z) < Q_FUZZ) {
- return 1;
- }
- /* note (0, x, y, z) = (0, -x, -y, -z) */
- if (fabs(q1.s) >= QS_FUZZ ||
- fabs(q1.x + q2.x) >= Q_FUZZ ||
- fabs(q1.y + q2.y) >= Q_FUZZ || fabs(q1.z + q2.z) >= Q_FUZZ) {
- return 0;
- }
- return 1;
- }
- int pmQuatMag(PmQuaternion q, double *d)
- {
- PmRotationVector r;
- int r1;
- if (0 == d) {
- return (pmErrno = PM_ERR);
- }
- r1 = pmQuatRotConvert(q, &r);
- *d = r.s;
- return pmErrno = r1;
- }
- int pmQuatNorm(PmQuaternion q1, PmQuaternion * qout)
- {
- double size = pmSqrt(pmSq(q1.s) + pmSq(q1.x) + pmSq(q1.y) + pmSq(q1.z));
- if (size == 0.0) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmQuatNorm\n");
- #endif
- qout->s = 1;
- qout->x = 0;
- qout->y = 0;
- qout->z = 0;
- return pmErrno = PM_NORM_ERR;
- }
- if (q1.s >= 0.0) {
- qout->s = q1.s / size;
- qout->x = q1.x / size;
- qout->y = q1.y / size;
- qout->z = q1.z / size;
- return pmErrno = 0;
- } else {
- qout->s = -q1.s / size;
- qout->x = -q1.x / size;
- qout->y = -q1.y / size;
- qout->z = -q1.z / size;
- return pmErrno = 0;
- }
- }
- int pmQuatInv(PmQuaternion q1, PmQuaternion * qout)
- {
- if (qout == 0) {
- return pmErrno = PM_ERR;
- }
- qout->s = q1.s;
- qout->x = -q1.x;
- qout->y = -q1.y;
- qout->z = -q1.z;
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(q1)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmQuatInv\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- return pmErrno = 0;
- }
- int pmQuatIsNorm(PmQuaternion q1)
- {
- return (fabs(pmSq(q1.s) + pmSq(q1.x) + pmSq(q1.y) + pmSq(q1.z) - 1.0) <
- UNIT_QUAT_FUZZ);
- }
- int pmQuatScalMult(PmQuaternion q, double s, PmQuaternion * qout)
- {
- /*! \todo FIXME-- need a native version; this goes through a rotation vector */
- PmRotationVector r;
- int r1, r2, r3;
- r1 = pmQuatRotConvert(q, &r);
- r2 = pmRotScalMult(r, s, &r);
- r3 = pmRotQuatConvert(r, qout);
- return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
- }
- int pmQuatScalDiv(PmQuaternion q, double s, PmQuaternion * qout)
- {
- /*! \todo FIXME-- need a native version; this goes through a rotation vector */
- PmRotationVector r;
- int r1, r2, r3;
- r1 = pmQuatRotConvert(q, &r);
- r2 = pmRotScalDiv(r, s, &r);
- r3 = pmRotQuatConvert(r, qout);
- return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
- }
- int pmQuatQuatMult(PmQuaternion q1, PmQuaternion q2, PmQuaternion * qout)
- {
- if (qout == 0) {
- return pmErrno = PM_ERR;
- }
- qout->s = q1.s * q2.s - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
- if (qout->s >= 0.0) {
- qout->x = q1.s * q2.x + q1.x * q2.s + q1.y * q2.z - q1.z * q2.y;
- qout->y = q1.s * q2.y - q1.x * q2.z + q1.y * q2.s + q1.z * q2.x;
- qout->z = q1.s * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.s;
- } else {
- qout->s *= -1;
- qout->x = -q1.s * q2.x - q1.x * q2.s - q1.y * q2.z + q1.z * q2.y;
- qout->y = -q1.s * q2.y + q1.x * q2.z - q1.y * q2.s - q1.z * q2.x;
- qout->z = -q1.s * q2.z - q1.x * q2.y + q1.y * q2.x - q1.z * q2.s;
- }
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmQuatQuatMult\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- return pmErrno = 0;
- }
- int pmQuatCartMult(PmQuaternion q1, PmCartesian v2, PmCartesian * vout)
- {
- PmCartesian c;
- 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.0 * (q1.s * c.x + q1.y * c.z - q1.z * c.y);
- vout->y = v2.y + 2.0 * (q1.s * c.y + q1.z * c.x - q1.x * c.z);
- vout->z = v2.z + 2.0 * (q1.s * c.z + q1.x * c.y - q1.y * c.x);
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(q1)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmQuatCartMult\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- return pmErrno = 0;
- }
- /* PmPose functions*/
- int pmPosePoseCompare(PmPose p1, PmPose p2)
- {
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(p1.rot) || !pmQuatIsNorm(p2.rot)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmPosePoseCompare\n");
- #endif
- }
- #endif
- return pmErrno = (pmQuatQuatCompare(p1.rot, p2.rot) &&
- pmCartCartCompare(p1.tran, p2.tran));
- }
- int pmPoseInv(PmPose p1, PmPose * p2)
- {
- int r1, r2;
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(p1.rot)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmPoseInv\n");
- #endif
- }
- #endif
- r1 = pmQuatInv(p1.rot, &p2->rot);
- r2 = pmQuatCartMult(p2->rot, p1.tran, &p2->tran);
- p2->tran.x *= -1.0;
- p2->tran.y *= -1.0;
- p2->tran.z *= -1.0;
- return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
- }
- int pmPoseCartMult(PmPose p1, PmCartesian v2, PmCartesian * vout)
- {
- int r1, r2;
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(p1.rot)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmPoseCartMult\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- r1 = pmQuatCartMult(p1.rot, v2, vout);
- r2 = pmCartCartAdd(p1.tran, *vout, vout);
- return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
- }
- int pmPosePoseMult(PmPose p1, PmPose p2, PmPose * pout)
- {
- int r1, r2, r3;
- #ifdef PM_DEBUG
- if (!pmQuatIsNorm(p1.rot) || !pmQuatIsNorm(p2.rot)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad quaternion in pmPosePoseMult\n");
- #endif
- return pmErrno = PM_NORM_ERR;
- }
- #endif
- r1 = pmQuatCartMult(p1.rot, p2.tran, &pout->tran);
- r2 = pmCartCartAdd(p1.tran, pout->tran, &pout->tran);
- r3 = pmQuatQuatMult(p1.rot, p2.rot, &pout->rot);
- return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
- }
- /* homogeneous transform functions */
- int pmHomInv(PmHomogeneous h1, PmHomogeneous * h2)
- {
- int r1, r2;
- #ifdef PM_DEBUG
- if (!pmMatIsNorm(h1.rot)) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("Bad rotation matrix in pmHomInv\n");
- #endif
- }
- #endif
- r1 = pmMatInv(h1.rot, &h2->rot);
- r2 = pmMatCartMult(h2->rot, h1.tran, &h2->tran);
- h2->tran.x *= -1.0;
- h2->tran.y *= -1.0;
- h2->tran.z *= -1.0;
- return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
- }
- /* line functions */
- int pmLineInit(PmLine * line, PmPose start, PmPose end)
- {
- int r1 = 0, r2 = 0, r3 = 0, r4 = 0, r5 = 0;
- double tmag = 0.0;
- double rmag = 0.0;
- PmQuaternion startQuatInverse;
- if (0 == line) {
- return (pmErrno = PM_ERR);
- }
- r3 = pmQuatInv(start.rot, &startQuatInverse);
- if (r3) {
- return r3;
- }
- r4 = pmQuatQuatMult(startQuatInverse, end.rot, &line->qVec);
- if (r4) {
- return r4;
- }
- pmQuatMag(line->qVec, &rmag);
- if (rmag > Q_FUZZ) {
- r5 = pmQuatScalMult(line->qVec, 1 / rmag, &(line->qVec));
- if (r5) {
- return r5;
- }
- }
- line->start = start;
- line->end = end;
- r1 = pmCartCartSub(end.tran, start.tran, &line->uVec);
- if (r1) {
- return r1;
- }
- pmCartMag(line->uVec, &tmag);
- if (IS_FUZZ(tmag, CART_FUZZ)) {
- line->uVec.x = 1.0;
- line->uVec.y = 0.0;
- line->uVec.z = 0.0;
- } else {
- r2 = pmCartUnit(line->uVec, &line->uVec);
- }
- line->tmag = tmag;
- line->rmag = rmag;
- line->tmag_zero = (line->tmag <= CART_FUZZ);
- line->rmag_zero = (line->rmag <= Q_FUZZ);
- /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
- return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : 0;
- }
- int pmLinePoint(PmLine * line, double len, PmPose * point)
- {
- int r1 = 0, r2 = 0, r3 = 0, r4 = 0;
- if (line->tmag_zero) {
- point->tran = line->end.tran;
- } else {
- /* return start + len * uVec */
- r1 = pmCartScalMult(line->uVec, len, &point->tran);
- r2 = pmCartCartAdd(line->start.tran, point->tran, &point->tran);
- }
- if (line->rmag_zero) {
- point->rot = line->end.rot;
- } else {
- if (line->tmag_zero) {
- r3 = pmQuatScalMult(line->qVec, len, &point->rot);
- } else {
- r3 = pmQuatScalMult(line->qVec, len * line->rmag / line->tmag,
- &point->rot);
- }
- r4 = pmQuatQuatMult(line->start.rot, point->rot, &point->rot);
- }
- return pmErrno = (r1 || r2 || r3 || r4) ? PM_NORM_ERR : 0;
- }
- /* circle functions */
- /*
- pmCircleInit() takes the defining parameters of a generalized circle
- and sticks them in the structure. It also computes the radius and vectors
- in the plane that are useful for other functions and that don't need
- to be recomputed every time.
- Note that the end can be placed arbitrarily, resulting in a combination of
- spiral and helical motion. There is an overconstraint between the start,
- center, and normal vector: the center vector and start vector are assumed
- to be in the plane defined by the normal vector. If this is not true, then
- it will be made true by moving the center vector onto the plane.
- */
- int pmCircleInit(PmCircle * circle,
- PmPose start, PmPose end,
- PmCartesian center, PmCartesian normal, int turn)
- {
- double dot;
- PmCartesian rEnd;
- PmCartesian v;
- double d;
- int r1;
- #ifdef PM_DEBUG
- if (0 == circle) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmCircleInit cirle pointer is null\n");
- #endif
- return pmErrno = PM_ERR;
- }
- #endif
- /* adjust center */
- pmCartCartSub(start.tran, center, &v);
- r1 = pmCartCartProj(v, normal, &v);
- if (PM_NORM_ERR == r1) {
- /* bad normal vector-- abort */
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmCircleInit normal vector is 0\n");
- #endif
- return -1;
- }
- pmCartCartAdd(v, center, &circle->center);
- /* normalize and redirect normal vector based on turns. If turn is less
- than 0, point normal vector in other direction and make turn positive,
- -1 -> 0, -2 -> 1, etc. */
- pmCartUnit(normal, &circle->normal);
- if (turn < 0) {
- turn = -1 - turn;
- pmCartScalMult(circle->normal, -1.0, &circle->normal);
- }
- /* radius */
- pmCartCartDisp(start.tran, circle->center, &circle->radius);
- /* vector in plane of circle from center to start, magnitude radius */
- pmCartCartSub(start.tran, circle->center, &circle->rTan);
- /* vector in plane of circle perpendicular to rTan, magnitude radius */
- pmCartCartCross(circle->normal, circle->rTan, &circle->rPerp);
- /* do rHelix, rEnd */
- pmCartCartSub(end.tran, circle->center, &circle->rHelix);
- pmCartPlaneProj(circle->rHelix, circle->normal, &rEnd);
- pmCartMag(rEnd, &circle->spiral);
- circle->spiral -= circle->radius;
- pmCartCartSub(circle->rHelix, rEnd, &circle->rHelix);
- pmCartUnit(rEnd, &rEnd);
- /* unit vector in plane of circle perpendicular to rEnd */
- /* utvOut: unit tangent vector outward */
- pmCartCartCross(circle->normal, rEnd, &circle->utvOut);
- /* utvIn: unit tangent vector inward */
- pmCartUnit(circle->rPerp, &circle->utvIn);
- pmCartScalMult(rEnd, circle->radius, &rEnd);
- /* Patch for error spiral end same as spiral center */
- pmCartMag(rEnd, &d);
- if (d == 0.0) {
- pmCartScalMult(circle->normal, DOUBLE_FUZZ, &v);
- pmCartCartAdd(rEnd, v, &rEnd);
- }
- /* end patch 03-mar-1999 Dirk Maij */
- /* angle */
- pmCartCartDot(circle->rTan, rEnd, &dot);
- dot = dot / (circle->radius * circle->radius);
- if (dot > 1.0) {
- circle->angle = 0.0;
- } else if (dot < -1.0) {
- circle->angle = PM_PI;
- } else {
- circle->angle = acos(dot);
- }
- /* now angle is in range 0..PI . Check if cross is antiparallel to
- normal. If so, true angle is between PI..2PI. Need to subtract from
- 2PI. */
- pmCartCartCross(circle->rTan, rEnd, &v);
- pmCartCartDot(v, circle->normal, &d);
- if (d < 0.0) {
- circle->angle = PM_2_PI - circle->angle;
- }
- if (circle->angle > -(CIRCLE_FUZZ) && circle->angle < (CIRCLE_FUZZ)) {
- circle->angle = PM_2_PI;
- }
- /* now add more angle for multi turns */
- if (turn > 0) {
- circle->angle += turn * 2.0 * PM_PI;
- }
- /* if 0'ed out while not debugging*/
- #if 0
- printf("\n\n");
- printf("pmCircleInit:\n");
- printf(" \t start : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- start.tran.x, start.tran.y, start.tran.z);
- printf(" \t end : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- end.tran.x, end.tran.y, end.tran.z);
- printf(" \t center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- center.x, center.y, center.z);
- printf(" \t normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- normal.x, normal.y, normal.z);
- printf(" \t rEnd : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- rEnd.x, rEnd.y, rEnd.z);
- printf(" \t turn=%d\n", turn);
- printf(" \t dot=%9.9f\n", dot);
- printf(" \t d=%9.9f\n", d);
- printf(" \t circle \t{angle=%9.9f, radius=%9.9f, spiral=%9.9f}\n",
- circle->angle, circle->radius, circle->spiral);
- printf(" \t circle->normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- circle->normal.x, circle->normal.y, circle->normal.z);
- printf(" \t circle->center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- circle->center.x, circle->center.y, circle->center.z);
- printf(" \t circle->rTan : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- circle->rTan.x, circle->rTan.y, circle->rTan.z);
- printf(" \t circle->rPerp : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- circle->rPerp.x, circle->rPerp.y, circle->rPerp.z);
- printf(" \t circle->rHelix : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
- circle->rHelix.x, circle->rHelix.y, circle->rHelix.z);
- printf("\n\n");
- #endif
- return pmErrno = 0;
- }
- /*
- pmCirclePoint() returns the point at the given angle along
- the circle. If the circle is a helix or spiral or combination, the
- point will include interpolation off the actual circle.
- */
- int pmCirclePoint(PmCircle * circle, double angle, PmPose * point)
- {
- PmCartesian par, perp;
- double scale;
- #ifdef PM_DEBUG
- if (0 == circle || 0 == point) {
- #ifdef PM_PRINT_ERROR
- pmPrintError
- ("error: pmCirclePoint circle or point pointer is null\n");
- #endif
- return pmErrno = PM_ERR;
- }
- #endif
- /* compute components rel to center */
- pmCartScalMult(circle->rTan, cos(angle), &par);
- pmCartScalMult(circle->rPerp, sin(angle), &perp);
- /* add to get radius vector rel to center */
- pmCartCartAdd(par, perp, &point->tran);
- /* get scale for spiral, helix interpolation */
- if (circle->angle == 0.0) {
- #ifdef PM_PRINT_ERROR
- pmPrintError("error: pmCirclePoint angle is zero\n");
- #endif
- return pmErrno = PM_DIV_ERR;
- }
- scale = angle / circle->angle;
- /* add scaled vector in radial dir for spiral */
- pmCartUnit(point->tran, &par);
- pmCartScalMult(par, scale * circle->spiral, &par);
- pmCartCartAdd(point->tran, par, &point->tran);
- /* add scaled vector in helix dir */
- pmCartScalMult(circle->rHelix, scale, &perp);
- pmCartCartAdd(point->tran, perp, &point->tran);
- /* add to center vector for final result */
- pmCartCartAdd(circle->center, point->tran, &point->tran);
- return pmErrno = 0;
- }