posemath.h

Source code for an Numeric Cmputer

/********************************************************************
* Description: posemath.h
*   Declarations for pose math library data types and manipulation
*   functions.
*
*   Data types comprise various representations of translation and
*   rotation quantities, and a 'pose' for representing the location
*   and orientation of a frame in space relative to a base frame.
*   Translation representations include cartesian, spherical, and
*   cylindrical coordinates. All of these contain 3 elements. Rotation
*   representations include rotation vectors, quaternions, rotation
*   matrices, Euler angles, and roll-pitch-yaw. These contain at least
*   3 elements, and may contain more. Only 3 are necessary for the 3
*   degrees of freedom for either translation or rotation, but some
*   data representations use more for computational efficiency or
*   intuition at the expense of storage space.
*
*   Types are abbreviated in function naming with a few letters.
*   Functions exist for conversion between data types, checking for
*   consistency, normalization into consistency, extracting features
*   such as size, and arithmetic operations.
*
*   Names of data representations are in all capitals, prefixed with
*   'PM_'. Names of functions are in mixed case, prefixed with 'pm',
*   with case changes used to indicate new quantities instead of
*   underscores. Function syntax looks like
*    int UmQuatRotConvert(PM_QUATERNION, PM_ROTATION_VECTOR *);
*
*   The return value is an error code, 0 for success, or a non-zero
*   error code for failure, for example:
*
*    #define PM_ERR -1
*    #define PM_IMPL_ERR -2
*
*   The global variable 'pmErrno' is set to this return value.
*
*   C++ classes are used for data types so that operator overloading can
*   be used to reduce the programming labor. Using the overloaded operator
*   version of functions loses the integer error code. The global
*   variable 'pmErrno' can be queried after these operations. This is not
*   thread-safe or reentrant.
*
*   C++ names corresponding to the C structures use case mixing instead
*   of all caps. Thus, a quaternion in C++ is a PmQuaternion.
*
*   The MATH_DEBUG symbol can be defined to include error reporting via
*   printed errors.
*
*   Native efficient C functions exist for the PM_CARTESIAN, PM_QUATERNION,
*   and PM_POSE types. Constructors in all the classes have been defined
*   to convert to/from PM_CARTESIAN and any other translation type, and
*   to convert to/from PM_QUATERNION and any other rotation type. This means
*   that if no explicit C functions exist for another type, conversions
*   to the corresponding native type will occur automatically. If more
*   efficiency is desired for a particular type, C functions to handle the
*   operations should be coded and the overloaded C++ functions or operators
*   should be added.
*
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: LGPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change: 
* $Revision: 1.10 $
* $Author: paul_c $
* $Date: 2005/08/08 13:03:34 $
********************************************************************/

#ifndef POSEMATH_H
#define POSEMATH_H

#include "config.h"

#ifdef RTAPI
#include <linux/types.h>
#ifdef __attribute_used__
#undef __attribute_used__
#endif
#ifdef __attribute_pure__
#undef __attribute_pure__
#endif
#endif
#include <sys/cdefs.h>
#include <float.h>
#include <math.h>

#ifdef __cplusplus

#define USE_CONST
#define USE_CCONST
#define USE_REF

#ifdef USE_CCONST
#define PM_CCONST const
#else
#define PM_CCONST
#endif

#ifdef USE_CONST
#define PM_CONST const
#else
#define PM_CONST
#endif

#ifdef USE_REF
#define PM_REF  &
#else
#define PM_REF
#endif

#define INCLUDE_POSEMATH_COPY_CONSTRUCTORS

/* forward declarations-- conversion ctors will need these */

/* translation types */
struct PM_CARTESIAN;		/* Cart */
struct PM_SPHERICAL;		/* Sph */
struct PM_CYLINDRICAL;		/* Cyl */

/* rotation types */
struct PM_ROTATION_VECTOR;	/* Rot */
struct PM_ROTATION_MATRIX;	/* Mat */
struct PM_QUATERNION;		/* Quat */
struct PM_EULER_ZYZ;		/* Zyz */
struct PM_EULER_ZYX;		/* Zyx */
struct PM_RPY;			/* Rpy */

/* pose types */
struct PM_POSE;			/* Pose */
struct PM_HOMOGENEOUS;		/* Hom */

/* PM_CARTESIAN */

struct PM_CARTESIAN {
    /* ctors/dtors */
    PM_CARTESIAN() {
    };
    PM_CARTESIAN(double _x, double _y, double _z);
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_CARTESIAN(PM_CCONST PM_CARTESIAN & cart);	// added 7-May-1997
    // by WPS
#endif

    PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c);	/* conversion */
    PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s);	/* conversion */

    /* operators */
    double &operator[] (int n);	/* this[n] */
    PM_CARTESIAN operator = (PM_CARTESIAN v);	/* this = v */

    /* data */
    double x, y, z;		/* this.x, etc. */
};

/* PM_SPHERICAL */

struct PM_SPHERICAL {
    /* ctors/dtors */
    PM_SPHERICAL() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s);
#endif
    PM_SPHERICAL(double _theta, double _phi, double _r);
    PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF v);	/* conversion */
    PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v);	/* conversion */

    /* operators */
    double &operator[] (int n);	/* this[n] */
    PM_SPHERICAL operator = (PM_SPHERICAL s);	/* this = s */

    /* data */
    double theta, phi, r;
};

/* PM_CYLINDRICAL */

struct PM_CYLINDRICAL {
    /* ctors/dtors */
    PM_CYLINDRICAL() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c);
#endif
    PM_CYLINDRICAL(double _theta, double _r, double _z);
    PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v);	/* conversion */
    PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF v);	/* conversion */

    /* operators */
    double &operator[] (int n);	/* this[n] */
    PM_CYLINDRICAL operator = (PM_CYLINDRICAL c);	/* this = c */

    /* data */
    double theta, r, z;
};

/* PM_ROTATION_VECTOR */

struct PM_ROTATION_VECTOR {
    /* ctors/dtors */
    PM_ROTATION_VECTOR() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r);
#endif
    PM_ROTATION_VECTOR(double _r, double _x, double _y, double _z);
    PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q);	/* conversion 
								 */

    /* operators */
    double &operator[] (int n);	/* this[n] */
    PM_ROTATION_VECTOR operator = (PM_ROTATION_VECTOR r);	/* this = r */

    /* data */
    double s, x, y, z;
};

/* PM_ROTATION_MATRIX */

struct PM_ROTATION_MATRIX {
    /* ctors/dtors */
    PM_ROTATION_MATRIX() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & mat);	/* added
								   7-May-1997 
								   by WPS */
#endif
    PM_ROTATION_MATRIX(double xx, double xy, double xz,
	double yx, double yy, double yz, double zx, double zy, double zz);
    PM_ROTATION_MATRIX(PM_CARTESIAN _x, PM_CARTESIAN _y, PM_CARTESIAN _z);
    PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v);	/* conversion 
								 */
    PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q);	/* conversion 
								 */
    PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz);	/* conversion 
								 */
    PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx);	/* conversion 
								 */
    PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy);	/* conversion */

    /* operators */
    PM_CARTESIAN & operator[](int n);	/* this[n] */
    PM_ROTATION_MATRIX operator = (PM_ROTATION_MATRIX m);	/* this = m */

    /* data */
    PM_CARTESIAN x, y, z;
};

/* PM_QUATERNION */

enum PM_AXIS { PM_X, PM_Y, PM_Z };

struct PM_QUATERNION {
    /* ctors/dtors */
    PM_QUATERNION() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_QUATERNION(PM_CCONST PM_QUATERNION & quat);	/* added 7-May-1997
							   by WPS */
#endif
    PM_QUATERNION(double _s, double _x, double _y, double _z);
    PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v);	/* conversion 
								 */
    PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m);	/* conversion 
								 */
    PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz);	/* conversion */
    PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx);	/* conversion */
    PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy);	/* conversion */
    PM_QUATERNION(PM_AXIS axis, double angle);	/* conversion */

    /* operators */
    double &operator[] (int n);	/* this[n] */
    PM_QUATERNION operator = (PM_QUATERNION q);	/* this = q */

    /* functions */
    void axisAngleMult(PM_AXIS axis, double angle);

    /* data */
    double s, x, y, z;		/* this.s, etc. */
};

/* PM_EULER_ZYZ */

struct PM_EULER_ZYZ {
    /* ctors/dtors */
    PM_EULER_ZYZ() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz);
#endif
    PM_EULER_ZYZ(double _z, double _y, double _zp);
    PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q);	/* conversion */
    PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m);	/* conversion */

    /* operators */
    double &operator[] (int n);
    PM_EULER_ZYZ operator = (PM_EULER_ZYZ zyz);

    /* data */
    double z, y, zp;
};

/* PM_EULER_ZYX */

struct PM_EULER_ZYX {
    /* ctors/dtors */
    PM_EULER_ZYX() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx);
#endif
    PM_EULER_ZYX(double _z, double _y, double _x);
    PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q);	/* conversion */
    PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m);	/* conversion */

    /* operators */
    double &operator[] (int n);
    PM_EULER_ZYX operator = (PM_EULER_ZYX zyx);

    /* data */
    double z, y, x;
};

/* PM_RPY */

struct PM_RPY {
    /* ctors/dtors */
    PM_RPY() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_RPY(PM_CCONST PM_RPY & rpy);	/* added 7-May-1997 by WPS */
#endif
    PM_RPY(double _r, double _p, double _y);
    PM_RPY(PM_CONST PM_QUATERNION PM_REF q);	/* conversion */
    PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m);	/* conversion */

    /* operators */
    double &operator[] (int n);
    PM_RPY operator = (PM_RPY rpy);

    /* data */
    double r, p, y;
};

/* PM_POSE */

struct PM_POSE {
    /* ctors/dtors */
    PM_POSE() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_POSE(PM_CCONST PM_POSE & p);
#endif
    PM_POSE(PM_CARTESIAN v, PM_QUATERNION q);
    PM_POSE(double x, double y, double z,
	double s, double sx, double sy, double sz);
    PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h);	/* conversion */

    /* operators */
    double &operator[] (int n);	/* this[n] */
    PM_POSE operator = (PM_POSE p);	/* this = p */

    /* data */
    PM_CARTESIAN tran;
    PM_QUATERNION rot;
};

/* PM_HOMOGENEOUS */

struct PM_HOMOGENEOUS {
    /* ctors/dtors */
    PM_HOMOGENEOUS() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h);
#endif
    PM_HOMOGENEOUS(PM_CARTESIAN v, PM_ROTATION_MATRIX m);
    PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p);	/* conversion */

    /* operators */
    PM_CARTESIAN & operator[](int n);	/* column vector */
    PM_HOMOGENEOUS operator = (PM_HOMOGENEOUS h);

    /* data ( [ 0 0 0 1 ] element is manually returned by [] if needed ) */
    PM_CARTESIAN tran;
    PM_ROTATION_MATRIX rot;
};

/* PM_LINE */

struct PM_LINE {
    /* ctors/dtors */
    PM_LINE() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_LINE(PM_CCONST PM_LINE &);
#endif

    /* functions */
    int init(PM_POSE start, PM_POSE end);
    int point(double len, PM_POSE * point);

    /* data */
    PM_POSE start;		/* where motion was started */
    PM_POSE end;		/* where motion is going */
    PM_CARTESIAN uVec;		/* unit vector from start to end */
};

/* PM_CIRCLE */

struct PM_CIRCLE {
    /* ctors/dtors */
    PM_CIRCLE() {
    };
#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
    PM_CIRCLE(PM_CCONST PM_CIRCLE &);
#endif

    /* functions */
    int init(PM_POSE start, PM_POSE end,
	PM_CARTESIAN center, PM_CARTESIAN normal, int turn);
    int point(double angle, PM_POSE * point);

    /* data */
    PM_CARTESIAN center;
    PM_CARTESIAN normal;
    PM_CARTESIAN rTan;
    PM_CARTESIAN rPerp;
    PM_CARTESIAN rHelix;
    double radius;
    double angle;
    double spiral;
};

/* overloaded external functions */

/* dot */
extern double dot(PM_CARTESIAN v1, PM_CARTESIAN v2);

/* cross */
extern PM_CARTESIAN cross(PM_CARTESIAN v1, PM_CARTESIAN v2);

/* norm */
extern PM_CARTESIAN norm(PM_CARTESIAN v);
extern PM_QUATERNION norm(PM_QUATERNION q);
extern PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r);
extern PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m);

/* unit */
extern PM_CARTESIAN unit(PM_CARTESIAN v);
extern PM_QUATERNION unit(PM_QUATERNION q);
extern PM_ROTATION_VECTOR unit(PM_ROTATION_VECTOR r);
extern PM_ROTATION_MATRIX unit(PM_ROTATION_MATRIX m);

/* isNorm */
extern int isNorm(PM_CARTESIAN v);
extern int isNorm(PM_QUATERNION q);
extern int isNorm(PM_ROTATION_VECTOR r);
extern int isNorm(PM_ROTATION_MATRIX m);

/* mag */
extern double mag(PM_CARTESIAN v);

/* disp */
extern double disp(PM_CARTESIAN v1, PM_CARTESIAN v2);

/* inv */
extern PM_CARTESIAN inv(PM_CARTESIAN v);
extern PM_ROTATION_MATRIX inv(PM_ROTATION_MATRIX m);
extern PM_QUATERNION inv(PM_QUATERNION q);
extern PM_POSE inv(PM_POSE p);
extern PM_HOMOGENEOUS inv(PM_HOMOGENEOUS h);

/* project */
extern PM_CARTESIAN proj(PM_CARTESIAN v1, PM_CARTESIAN v2);

/* overloaded arithmetic functions */