posemath.h

Source code for an Numeric Cmputer

/* unary +, - for translation, rotation, pose */
extern PM_CARTESIAN operator + (PM_CARTESIAN v);
extern PM_CARTESIAN operator - (PM_CARTESIAN v);
extern PM_QUATERNION operator + (PM_QUATERNION q);
extern PM_QUATERNION operator - (PM_QUATERNION q);
extern PM_POSE operator + (PM_POSE p);
extern PM_POSE operator - (PM_POSE p);

/* compare operators */
extern int operator == (PM_CARTESIAN v1, PM_CARTESIAN v2);
extern int operator == (PM_QUATERNION q1, PM_QUATERNION q2);
extern int operator == (PM_POSE p1, PM_POSE p2);
extern int operator != (PM_CARTESIAN v1, PM_CARTESIAN v2);
extern int operator != (PM_QUATERNION q1, PM_QUATERNION q2);
extern int operator != (PM_POSE p1, PM_POSE p2);

/* translation +, -, scalar *, - */

/* v + v */
extern PM_CARTESIAN operator + (PM_CARTESIAN v1, PM_CARTESIAN v2);
/* v - v */
extern PM_CARTESIAN operator - (PM_CARTESIAN v1, PM_CARTESIAN v2);
/* v * s */
extern PM_CARTESIAN operator *(PM_CARTESIAN v, double s);
/* s * v */
extern PM_CARTESIAN operator *(double s, PM_CARTESIAN v);
/* v / s */
extern PM_CARTESIAN operator / (PM_CARTESIAN v, double s);

/* rotation * by scalar, translation, and rotation */

/* s * q */
extern PM_QUATERNION operator *(double s, PM_QUATERNION q);
/* q * s */
extern PM_QUATERNION operator *(PM_QUATERNION q, double s);
/* q / s */
extern PM_QUATERNION operator / (PM_QUATERNION q, double s);
/* q * v */
extern PM_CARTESIAN operator *(PM_QUATERNION q, PM_CARTESIAN v);
/* q * q */
extern PM_QUATERNION operator *(PM_QUATERNION q1, PM_QUATERNION q2);
/* m * m */
extern PM_ROTATION_MATRIX operator *(PM_ROTATION_MATRIX m1,
    PM_ROTATION_MATRIX m2);

/* pose operators */

/* q * p */
extern PM_POSE operator *(PM_QUATERNION q, PM_POSE p);
/* p * p */
extern PM_POSE operator *(PM_POSE p1, PM_POSE p2);
/* p * v */
extern PM_CARTESIAN operator *(PM_POSE p, PM_CARTESIAN v);

#endif /* __cplusplus */

/* now comes the C stuff */

#ifdef __cplusplus
extern "C" {
#endif

/* PmCartesian */

    typedef struct {
	double x, y, z;		/* this.x, etc. */

    } PmCartesian;

/* PmSpherical */

    typedef struct {
	double theta, phi, r;

    } PmSpherical;

/* PmCylindrical */

    typedef struct {
	double theta, r, z;

    } PmCylindrical;

/* PmAxis */
#ifdef __cplusplus
    typedef PM_AXIS PmAxis;
#else
    typedef enum { PM_X, PM_Y, PM_Z } PmAxis;
#endif

/* PmRotationVector */

    typedef struct {
	double s, x, y, z;

    } PmRotationVector;

/* PmRotationMatrix */

    typedef struct {
	PmCartesian x, y, z;

    } PmRotationMatrix;

/* PmQuaternion */

    typedef struct {
	double s, x, y, z;	/* this.s, etc. */

    } PmQuaternion;

/* PmEulerZyz */

    typedef struct {
	double z, y, zp;

    } PmEulerZyz;

/* PmEulerZyx */

    typedef struct {
	double z, y, x;

    } PmEulerZyx;

/* PmRpy */

    typedef struct {
	double r, p, y;

    } PmRpy;

/* PmPose */

    typedef struct {
	PmCartesian tran;
	PmQuaternion rot;

    } PmPose;

/* Homogeneous transform PmHomogeneous */

    typedef struct {
	PmCartesian tran;
	PmRotationMatrix rot;

    } PmHomogeneous;

/* line structure */

    typedef struct {
	PmPose start;		/* where motion was started */
	PmPose end;		/* where motion is going */
	PmCartesian uVec;	/* unit vector from start to end */
	PmQuaternion qVec;	/* unit of rotation */
	double tmag;
	double rmag;
	int tmag_is_greater_than_rmag;
	int tmag_zero;
	int rmag_zero;

    } PmLine;

/* Generalized circle structure */

    typedef struct {
	PmCartesian center;
	PmCartesian normal;
	PmCartesian rTan;
	PmCartesian rPerp;
	PmCartesian rHelix;
	double radius;
	double angle;
	double spiral;

    } PmCircle;

/*
   shorthand types for normal use-- don't define PM_LOOSE_NAMESPACE if these
   names are used by other headers you need to include and you don't want
   these shorthand versions
*/

/* some nice constants */

#define PM_PI      3.14159265358979323846
#define PM_PI_2    1.57079632679489661923
#define PM_PI_4    0.78539816339744830962
#define PM_2_PI    6.28318530717958647692

#ifdef PM_LOOSE_NAMESPACE

    typedef PmCartesian VECTOR;
    typedef PmSpherical SPHERICAL;
    typedef PmCylindrical CYLINDRICAL;
    typedef PmQuaternion QUATERNION;
    typedef PmRotationMatrix MATRIX;
    typedef PmEulerZyz ZYZ;
    typedef PmEulerZyx ZYX;
    typedef PmRpy RPY;
    typedef PmPose POSE;
    typedef PmHomogeneous HX;
    typedef PmCircle CIRCLE;
    typedef PmLine LINE;

#define PI                PM_PI
#define PI_2              PM_PI_2
#define PI_4              PM_PI_4
#define TWO_PI            PM_2_PI	/* 2_PI invalid macro name */
#endif

/* quicky macros */

#define pmClose(a, b, eps) ((fabs((a) - (b)) < (eps)) ? 1 : 0)
#define pmSq(x) ((x)*(x))

#ifdef TO_DEG
#undef TO_DEG
#endif
#define TO_DEG (180./PM_PI)

#ifdef TO_RAD
#undef TO_RAD
#endif
#define TO_RAD (PM_PI/180.)

/*! \todo FIXME-- fix these */

/* DOUBLE_FUZZ is the smallest double, d, such that (1+d != 1) w/o FPC.
   DOUBLECP_FUZZ is the same only with the Floating Point CoProcessor */

#define DOUBLE_FUZZ 2.2204460492503131e-16
#define DOUBLECP_FUZZ 1.0842021724855044e-19

#define CART_FUZZ (0.000001)
/* how close a cartesian vector's magnitude must be for it to be considered
   a zero vector */

#define Q_FUZZ (.000001)
/* how close elements of a Q must be to be equal */

#define QS_FUZZ (.000001)
/* how close q.s is to 0 to be 180 deg rotation */

#define RS_FUZZ (.000001)
/* how close r.s is for a rotation vector to be considered 0 */

#define QSIN_FUZZ (.000001)
/* how close sin(a/2) is to 0 to be zero rotat */

#define V_FUZZ (.000001)
/* how close elements of a V must be to be equal */

#define SQRT_FUZZ (-.000001)
/* how close to 0 before math_sqrt() is error */

#define UNIT_VEC_FUZZ (.000001)
/* how close mag of vec must be to 1.00 */

#define UNIT_QUAT_FUZZ (.000001)
/* how close mag of quat must be to 1.00 */

#define UNIT_SC_FUZZ (.000001)
/* how close mag of sin, cos must be to 1.00 */

#define E_EPSILON (.000001)
/* how close second ZYZ euler angle must be to 0/PI for degeneration */

#define SINGULAR_EPSILON (.000001)
/* how close to zero the determinate of a matrix must be for singularity */

#define RPY_P_FUZZ (0.000001)
/* how close pitch is to zero for RPY to degenerate */

#define ZYZ_Y_FUZZ (0.000001)
/* how close Y is to zero for ZYZ Euler to degenerate */

#define ZYX_Y_FUZZ (0.000001)
/* how close Y is to zero for ZYX Euler to degenerate */

#define CIRCLE_FUZZ (0.000001)
/* Bug fix for the missing circles problem */

/* debug output printing */
    extern void pmPrintError(const char *fmt, ...);

/* global error number and errors */
    extern int pmErrno;
    extern void pmPerror(const char *fmt);
#define PM_ERR             -1	/* unspecified error */
#define PM_IMPL_ERR        -2	/* not implemented */
#define PM_NORM_ERR        -3	/* arg should have been norm */
#define PM_DIV_ERR         -4	/* divide by zero error */

/* Scalar functions */

    extern double pmSqrt(double x);

/* Translation rep conversion functions */

    extern int pmCartSphConvert(PmCartesian, PmSpherical *);
    extern int pmCartCylConvert(PmCartesian, PmCylindrical *);
    extern int pmSphCartConvert(PmSpherical, PmCartesian *);
    extern int pmSphCylConvert(PmSpherical, PmCylindrical *);
    extern int pmCylCartConvert(PmCylindrical, PmCartesian *);
    extern int pmCylSphConvert(PmCylindrical, PmSpherical *);

/* Rotation rep conversion functions */

    extern int pmAxisAngleQuatConvert(PmAxis, double, PmQuaternion *);

    extern int pmRotQuatConvert(PmRotationVector, PmQuaternion *);
    extern int pmRotMatConvert(PmRotationVector, PmRotationMatrix *);
    extern int pmRotZyzConvert(PmRotationVector, PmEulerZyz *);
    extern int pmRotZyxConvert(PmRotationVector, PmEulerZyx *);
    extern int pmRotRpyConvert(PmRotationVector, PmRpy *);

    extern int pmQuatRotConvert(PmQuaternion, PmRotationVector *);
    extern int pmQuatMatConvert(PmQuaternion, PmRotationMatrix *);
    extern int pmQuatZyzConvert(PmQuaternion, PmEulerZyz *);
    extern int pmQuatZyxConvert(PmQuaternion, PmEulerZyx *);
    extern int pmQuatRpyConvert(PmQuaternion, PmRpy *);

    extern int pmMatRotConvert(PmRotationMatrix, PmRotationVector *);
    extern int pmMatQuatConvert(PmRotationMatrix, PmQuaternion *);
    extern int pmMatZyzConvert(PmRotationMatrix, PmEulerZyz *);
    extern int pmMatZyxConvert(PmRotationMatrix, PmEulerZyx *);
    extern int pmMatRpyConvert(PmRotationMatrix, PmRpy *);

    extern int pmZyzRotConvert(PmEulerZyz, PmRotationVector *);
    extern int pmZyzQuatConvert(PmEulerZyz, PmQuaternion *);
    extern int pmZyzMatConvert(PmEulerZyz, PmRotationMatrix *);
    extern int pmZyzZyxConvert(PmEulerZyz, PmEulerZyx *);
    extern int pmZyzRpyConvert(PmEulerZyz, PmRpy *);

    extern int pmZyxRotConvert(PmEulerZyx, PmRotationVector *);
    extern int pmZyxQuatConvert(PmEulerZyx, PmQuaternion *);
    extern int pmZyxMatConvert(PmEulerZyx, PmRotationMatrix *);
    extern int pmZyxZyzConvert(PmEulerZyx, PmEulerZyz *);
    extern int pmZyxRpyConvert(PmEulerZyx, PmRpy *);

    extern int pmRpyRotConvert(PmRpy, PmRotationVector *);
    extern int pmRpyQuatConvert(PmRpy, PmQuaternion *);
    extern int pmRpyMatConvert(PmRpy, PmRotationMatrix *);
    extern int pmRpyZyzConvert(PmRpy, PmEulerZyz *);
    extern int pmRpyZyxConvert(PmRpy, PmEulerZyx *);

/* Combined rep conversion functions */

    extern int pmPoseHomConvert(PmPose p, PmHomogeneous * h);

    extern int pmHomPoseConvert(PmHomogeneous h, PmPose * p);

/* Arithmetic functions

   Note: currently, only functions for PmCartesian, PmQuaternion, and
   PmPose are supported directly. The type conversion functions
   will be used implicitly when applying arithmetic function
   to other types. This will be slower and less accurate. Explicit
   functions can be added incrementally.
*/

/* translation functions */

/* NOTE:  only Cartesian type supported in C now */

    extern int pmCartCartCompare(PmCartesian, PmCartesian);
    extern int pmCartCartDot(PmCartesian, PmCartesian, double *);
    extern int pmCartCartCross(PmCartesian, PmCartesian, PmCartesian *);
    extern int pmCartMag(PmCartesian, double *);
    extern int pmCartCartDisp(PmCartesian v1, PmCartesian v2, double *d);
    extern int pmCartCartAdd(PmCartesian, PmCartesian, PmCartesian *);
    extern int pmCartCartSub(PmCartesian, PmCartesian, PmCartesian *);
    extern int pmCartScalMult(PmCartesian, double, PmCartesian *);
    extern int pmCartScalDiv(PmCartesian, double, PmCartesian *);
    extern int pmCartNeg(PmCartesian, PmCartesian *);
    extern int pmCartUnit(PmCartesian v, PmCartesian * vout);
/*! \todo Another #if 0 */
#if 0
    extern int pmCartNorm(PmCartesian v, PmCartesian * vout);
#else
// Hopefully guaranteed to cause a compile error when used.
#define pmCartNorm(a,b,c,d,e)  bad{a.b.c.d.e}
#endif

    extern int pmCartIsNorm(PmCartesian v);
    extern int pmCartInv(PmCartesian, PmCartesian *);
    extern int pmCartCartProj(PmCartesian, PmCartesian, PmCartesian *);
    extern int pmCartPlaneProj(PmCartesian v, PmCartesian normal,
	PmCartesian * vout);

/* rotation functions */

/* quaternion functions */

    extern int pmQuatQuatCompare(PmQuaternion, PmQuaternion);
    extern int pmQuatMag(PmQuaternion q, double *d);
    extern int pmQuatNorm(PmQuaternion, PmQuaternion *);
    extern int pmQuatInv(PmQuaternion, PmQuaternion *);
    extern int pmQuatIsNorm(PmQuaternion);
    extern int pmQuatScalMult(PmQuaternion q, double s, PmQuaternion * qout);
    extern int pmQuatScalDiv(PmQuaternion q, double s, PmQuaternion * qout);
    extern int pmQuatQuatMult(PmQuaternion, PmQuaternion, PmQuaternion *);
    extern int pmQuatCartMult(PmQuaternion, PmCartesian, PmCartesian *);
    extern int pmQuatAxisAngleMult(PmQuaternion, PmAxis, double,
	PmQuaternion *);

/* rotation vector functions */

    extern int pmRotScalMult(PmRotationVector, double, PmRotationVector *);
    extern int pmRotScalDiv(PmRotationVector, double, PmRotationVector *);
    extern int pmRotIsNorm(PmRotationVector);
    extern int pmRotNorm(PmRotationVector, PmRotationVector *);

/* 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  |   */

    extern int pmMatNorm(PmRotationMatrix m, PmRotationMatrix * mout);
    extern int pmMatIsNorm(PmRotationMatrix m);
    extern int pmMatInv(PmRotationMatrix m, PmRotationMatrix * mout);
    extern int pmMatCartMult(PmRotationMatrix m, PmCartesian v,
	PmCartesian * vout);
    extern int pmMatMatMult(PmRotationMatrix m1, PmRotationMatrix m2,
	PmRotationMatrix * mout);

/* pose functions*/

    extern int pmPosePoseCompare(PmPose, PmPose);
    extern int pmPoseInv(PmPose p, PmPose *);
    extern int pmPoseCartMult(PmPose, PmCartesian, PmCartesian *);
    extern int pmPosePoseMult(PmPose, PmPose, PmPose *);

/* homogeneous functions */
    extern int pmHomInv(PmHomogeneous, PmHomogeneous *);

/* line functions */

    extern int pmLineInit(PmLine * line, PmPose start, PmPose end);
    extern int pmLinePoint(PmLine * line, double len, PmPose * point);

/* circle functions */

    extern int pmCircleInit(PmCircle * circle,
	PmPose start, PmPose end,
	PmCartesian center, PmCartesian normal, int turn);
    extern int pmCirclePoint(PmCircle * circle, double angle, PmPose * point);

/* slicky macros for item-by-item copying between C and C++ structs */

#define toCart(src,dst) {(dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;}

#define toCyl(src,dst) {(dst)->theta = (src).theta; (dst)->r = (src).r; (dst)->z = (src).z;}

#define toSph(src,dst) {(dst)->theta = (src).theta; (dst)->phi = (src).phi; (dst)->r = (src).r;}

#define toQuat(src,dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;}

#define toRot(src,dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;}

#define toMat(src,dst) {toCart((src).x, &((dst)->x)); toCart((src).y, &((dst)->y)); toCart((src).z, &((dst)->z));}

#define toEulerZyz(src,dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->zp = (src).zp;}

#define toEulerZyx(src,dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->x = (src).x;}

#define toRpy(src,dst) {(dst)->r = (src).r; (dst)->p = (src).p; (dst)->y = (src).y;}

#define toPose(src,dst) {toCart((src).tran, &((dst)->tran)); toQuat((src).rot, &((dst)->rot));}

#define toHom(src,dst) {toCart((src).tran, &((dst)->tran)); toMat((src).rot, &((dst)->rot));}

#define toLine(src,dst) {toPose((src).start, &((dst)->start)); toPose((src).end, &((dst)->end)); toCart((src).uVec, &((dst)->uVec));}

#define toCircle(src,dst) {toCart((src).center, &((dst)->center)); toCart((src).normal, &((dst)->normal)); toCart((src).rTan, &((dst)->rTan)); toCart((src).rPerp, &((dst)->rPerp)); toCart((src).rHelix, &((dst)->rHelix)); (dst)->radius = (src).radius; (dst)->angle = (src).angle; (dst)->spiral = (src).spiral;}

#ifdef __cplusplus
}				/* matches extern "C" for C++ */
#endif
#endif				/* #ifndef POSEMATH_H */