posemath.cc

Source code for an Numeric Cmputer

/********************************************************************
* Description: posemath.cc
*    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: 
* $Revision: 1.6 $
* $Author: paul_c $
* $Date: 2005/06/13 14:38:50 $
********************************************************************/

#include "posemath.h"

#ifdef PM_PRINT_ERROR
#define PM_DEBUG		// need debug with printing
#endif

// place to reference arrays when bounds are exceeded
static double noElement = 0.0;
static PM_CARTESIAN *noCart = 0;


// PM_CARTESIAN class

PM_CARTESIAN::PM_CARTESIAN(double _x, double _y, double _z)
{
    x = _x;
    y = _y;
    z = _z;
}

PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c)
{
    PmCylindrical cyl;
    PmCartesian cart;

    toCyl(c, &cyl);
    pmCylCartConvert(cyl, &cart);
    toCart(cart, this);
}

PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s)
{
    PmSpherical sph;
    PmCartesian cart;

    toSph(s, &sph);
    pmSphCartConvert(sph, &cart);
    toCart(cart, this);
}

double &PM_CARTESIAN::operator [] (int n) {
    switch (n) {
    case 0:
	return x;
    case 1:
	return y;
    case 2:
	return z;
    default:
	return noElement;	// need to return a double &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_CARTESIAN::PM_CARTESIAN(PM_CCONST PM_CARTESIAN & v)
{
    x = v.x;
    y = v.y;
    z = v.z;
}
#endif

PM_CARTESIAN PM_CARTESIAN::operator =(PM_CARTESIAN v)
{
    x = v.x;
    y = v.y;
    z = v.z;

    return v;
}

// PM_SPHERICAL

PM_SPHERICAL::PM_SPHERICAL(double _theta, double _phi, double _r)
{
    theta = _theta;
    phi = _phi;
    r = _r;
}

PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v)
{
    PmCartesian cart;
    PmSpherical sph;

    toCart(v, &cart);
    pmCartSphConvert(cart, &sph);
    toSph(sph, this);
}

PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF c)
{
    PmCylindrical cyl;
    PmSpherical sph;

    toCyl(c, &cyl);
    pmCylSphConvert(cyl, &sph);
    toSph(sph, this);
}

double &PM_SPHERICAL::operator [] (int n) {
    switch (n) {
    case 0:
	return theta;
    case 1:
	return phi;
    case 2:
	return r;
    default:
	return noElement;	// need to return a double &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_SPHERICAL::PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s)
{
    theta = s.theta;
    phi = s.phi;
    r = s.r;
}
#endif

PM_SPHERICAL PM_SPHERICAL::operator =(PM_SPHERICAL s)
{
    theta = s.theta;
    phi = s.phi;
    r = s.r;

    return s;
}

// PM_CYLINDRICAL

PM_CYLINDRICAL::PM_CYLINDRICAL(double _theta, double _r, double _z)
{
    theta = _theta;
    r = _r;
    z = _z;
}

PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v)
{
    PmCartesian cart;
    PmCylindrical cyl;

    toCart(v, &cart);
    pmCartCylConvert(cart, &cyl);
    toCyl(cyl, this);
}

PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF s)
{
    PmSpherical sph;
    PmCylindrical cyl;

    toSph(s, &sph);
    pmSphCylConvert(sph, &cyl);
    toCyl(cyl, this);
}

double &PM_CYLINDRICAL::operator [] (int n) {
    switch (n) {
    case 0:
	return theta;
    case 1:
	return r;
    case 2:
	return z;
    default:
	return noElement;	// need to return a double &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c)
{
    theta = c.theta;
    r = c.r;
    z = c.z;
}
#endif

PM_CYLINDRICAL PM_CYLINDRICAL::operator =(PM_CYLINDRICAL c)
{
    theta = c.theta;
    r = c.r;
    z = c.z;

    return c;
}

// PM_ROTATION_VECTOR

PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(double _s, double _x,
    double _y, double _z)
{
    PmRotationVector rv;

    rv.s = _s;
    rv.x = _x;
    rv.y = _y;
    rv.z = _z;

    pmRotNorm(rv, &rv);
    toRot(rv, this);
}

PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q)
{
    PmQuaternion quat;
    PmRotationVector rv;

    toQuat(q, &quat);
    pmQuatRotConvert(quat, &rv);
    toRot(rv, this);
}

double &PM_ROTATION_VECTOR::operator [] (int n) {
    switch (n) {
    case 0:
	return s;
    case 1:
	return x;
    case 2:
	return y;
    case 3:
	return z;
    default:
	return noElement;	// need to return a double &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r)
{
    s = r.s;
    x = r.x;
    y = r.y;
    z = r.z;
}
#endif

PM_ROTATION_VECTOR PM_ROTATION_VECTOR::operator =(PM_ROTATION_VECTOR r)
{
    s = r.s;
    x = r.x;
    y = r.y;
    z = r.z;

    return r;
}

// PM_ROTATION_MATRIX class

// ctors/dtors

PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(double xx, double xy, double xz,
    double yx, double yy, double yz, double zx, double zy, double zz)
{
    x.x = xx;
    x.y = xy;
    x.z = xz;

    y.x = yx;
    y.y = yy;
    y.z = yz;

    z.x = zx;
    z.y = zy;
    z.z = zz;

    /*! \todo FIXME-- need a matrix orthonormalization function pmMatNorm() */
}

PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CARTESIAN _x, PM_CARTESIAN _y,
    PM_CARTESIAN _z)
{
    x = _x;
    y = _y;
    z = _z;
}

PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v)
{
    PmRotationVector rv;
    PmRotationMatrix mat;

    toRot(v, &rv);
    pmRotMatConvert(rv, &mat);
    toMat(mat, this);
}

PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q)
{
    PmQuaternion quat;
    PmRotationMatrix mat;

    toQuat(q, &quat);
    pmQuatMatConvert(quat, &mat);
    toMat(mat, this);
}

PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy)
{
    PmRpy _rpy;
    PmRotationMatrix mat;

    toRpy(rpy, &_rpy);
    pmRpyMatConvert(_rpy, &mat);
    toMat(mat, this);
}

PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz)
{
    PmEulerZyz _zyz;
    PmRotationMatrix mat;

    toEulerZyz(zyz, &_zyz);
    pmZyzMatConvert(_zyz, &mat);
    toMat(mat, this);
}

PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx)
{
    PmEulerZyx _zyx;
    PmRotationMatrix mat;

    toEulerZyx(zyx, &_zyx);
    pmZyxMatConvert(_zyx, &mat);
    toMat(mat, this);
}

// operators

PM_CARTESIAN & PM_ROTATION_MATRIX::operator [](int n) {
    switch (n) {
    case 0:
	return x;
    case 1:
	return y;
    case 2:
	return z;
    default:
	if (0 == noCart) {
	    noCart = new PM_CARTESIAN(0.0, 0.0, 0.0);
	}
	return (*noCart);	// need to return a PM_CARTESIAN &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & m)
{
    x = m.x;
    y = m.y;
    z = m.z;
}
#endif

PM_ROTATION_MATRIX PM_ROTATION_MATRIX::operator =(PM_ROTATION_MATRIX m)
{
    x = m.x;
    y = m.y;
    z = m.z;

    return m;
}

// PM_QUATERNION class

PM_QUATERNION::PM_QUATERNION(double _s, double _x, double _y, double _z)
{
    PmQuaternion quat;

    quat.s = _s;
    quat.x = _x;
    quat.y = _y;
    quat.z = _z;

    pmQuatNorm(quat, &quat);

    s = quat.s;
    x = quat.x;
    y = quat.y;
    z = quat.z;
}

PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v)
{
    PmRotationVector rv;
    PmQuaternion quat;

    toRot(v, &rv);
    pmRotQuatConvert(rv, &quat);
    toQuat(quat, this);
}

PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m)
{
    PmRotationMatrix mat;
    PmQuaternion quat;

    toMat(m, &mat);
    pmMatQuatConvert(mat, &quat);
    toQuat(quat, this);
}

PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz)
{
    PmEulerZyz _zyz;
    PmQuaternion quat;

    toEulerZyz(zyz, &_zyz);
    pmZyzQuatConvert(_zyz, &quat);
    toQuat(quat, this);
}

PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx)
{
    PmEulerZyx _zyx;
    PmQuaternion quat;

    toEulerZyx(zyx, &_zyx);
    pmZyxQuatConvert(_zyx, &quat);
    toQuat(quat, this);
}

PM_QUATERNION::PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy)
{
    PmRpy _rpy;
    PmQuaternion quat;

    toRpy(rpy, &_rpy);
    pmRpyQuatConvert(_rpy, &quat);
    toQuat(quat, this);
}

PM_QUATERNION::PM_QUATERNION(PM_AXIS _axis, double _angle)
{
    PmQuaternion quat;

    pmAxisAngleQuatConvert((PmAxis) _axis, _angle, &quat);
    toQuat(quat, this);
}

void PM_QUATERNION::axisAngleMult(PM_AXIS _axis, double _angle)
{
    PmQuaternion quat;

    toQuat((*this), &quat);
    pmQuatAxisAngleMult(quat, (PmAxis) _axis, _angle, &quat);
    toQuat(quat, this);
}

double &PM_QUATERNION::operator [] (int n) {
    switch (n) {
    case 0:
	return s;
    case 1:
	return x;
    case 2:
	return y;
    case 3:
	return z;
    default:
	return noElement;	// need to return a double &
    }
}

PM_QUATERNION PM_QUATERNION::operator =(PM_QUATERNION q)
{
    s = q.s;
    x = q.x;
    y = q.y;
    z = q.z;

    return q;
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_QUATERNION::PM_QUATERNION(PM_CCONST PM_QUATERNION & q)
{
    s = q.s;
    x = q.x;
    y = q.y;
    z = q.z;
}
#endif

// PM_EULER_ZYZ class

PM_EULER_ZYZ::PM_EULER_ZYZ(double _z, double _y, double _zp)
{
    z = _z;
    y = _y;
    zp = _zp;
}

PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q)
{
    PmQuaternion quat;
    PmEulerZyz zyz;

    toQuat(q, &quat);
    pmQuatZyzConvert(quat, &zyz);
    toEulerZyz(zyz, this);
}

PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m)
{
    PmRotationMatrix mat;
    PmEulerZyz zyz;

    toMat(m, &mat);
    pmMatZyzConvert(mat, &zyz);
    toEulerZyz(zyz, this);
}

double &PM_EULER_ZYZ::operator [] (int n) {
    switch (n) {
    case 0:
	return z;
    case 1:
	return y;
    case 2:
	return zp;
    default:
	return noElement;	// need to return a double &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz)
{
    z = zyz.z;
    y = zyz.y;
    zp = zyz.zp;
}
#endif

PM_EULER_ZYZ PM_EULER_ZYZ::operator =(PM_EULER_ZYZ zyz)
{
    z = zyz.z;
    y = zyz.y;
    zp = zyz.zp;

    return zyz;
}

// PM_EULER_ZYX class

PM_EULER_ZYX::PM_EULER_ZYX(double _z, double _y, double _x)
{
    z = _z;
    y = _y;
    x = _x;
}

PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q)
{
    PmQuaternion quat;
    PmEulerZyx zyx;

    toQuat(q, &quat);
    pmQuatZyxConvert(quat, &zyx);
    toEulerZyx(zyx, this);
}

PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m)
{
    PmRotationMatrix mat;
    PmEulerZyx zyx;

    toMat(m, &mat);
    pmMatZyxConvert(mat, &zyx);
    toEulerZyx(zyx, this);
}

double &PM_EULER_ZYX::operator [] (int n) {
    switch (n) {
    case 0:
	return z;
    case 1:
	return y;
    case 2:
	return x;
    default:
	return noElement;	// need to return a double &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_EULER_ZYX::PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx)
{
    z = zyx.z;
    y = zyx.y;
    x = zyx.x;
}
#endif

PM_EULER_ZYX PM_EULER_ZYX::operator =(PM_EULER_ZYX zyx)
{
    z = zyx.z;
    y = zyx.y;
    x = zyx.x;

    return zyx;
}

// PM_RPY class

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_RPY::PM_RPY(PM_CCONST PM_RPY & rpy)
{
    r = rpy.r;
    p = rpy.p;
    y = rpy.y;
}
#endif

PM_RPY::PM_RPY(double _r, double _p, double _y)
{
    r = _r;
    p = _p;
    y = _y;
}

PM_RPY::PM_RPY(PM_CONST PM_QUATERNION PM_REF q)
{
    PmQuaternion quat;
    PmRpy rpy;

    toQuat(q, &quat);
    pmQuatRpyConvert(quat, &rpy);
    toRpy(rpy, this);
}

PM_RPY::PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m)
{
    PmRotationMatrix mat;
    PmRpy rpy;

    toMat(m, &mat);
    pmMatRpyConvert(mat, &rpy);
    toRpy(rpy, this);
}

double &PM_RPY::operator [] (int n) {
    switch (n) {
    case 0:
	return r;
    case 1:
	return p;
    case 2:
	return y;
    default:
	return noElement;	// need to return a double &
    }
}

PM_RPY PM_RPY::operator =(PM_RPY rpy)
{
    r = rpy.r;
    p = rpy.p;
    y = rpy.y;

    return rpy;
}

// PM_POSE class

PM_POSE::PM_POSE(PM_CARTESIAN v, PM_QUATERNION q)
{
    tran.x = v.x;
    tran.y = v.y;
    tran.z = v.z;
    rot.s = q.s;
    rot.x = q.x;
    rot.y = q.y;
    rot.z = q.z;
}

PM_POSE::PM_POSE(double x, double y, double z,
    double s, double sx, double sy, double sz)
{
    tran.x = x;
    tran.y = y;
    tran.z = z;
    rot.s = s;
    rot.x = sx;
    rot.y = sy;
    rot.z = sz;
}

PM_POSE::PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h)
{
    PmHomogeneous hom;
    PmPose pose;

    toHom(h, &hom);
    pmHomPoseConvert(hom, &pose);
    toPose(pose, this);
}

double &PM_POSE::operator [] (int n) {
    switch (n) {
    case 0:
	return tran.x;
    case 1:
	return tran.y;
    case 2:
	return tran.z;
    case 3:
	return rot.s;
    case 4:
	return rot.x;
    case 5:
	return rot.y;
    case 6:
	return rot.z;
    default:
	return noElement;	// need to return a double &
    }
}

#ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS
PM_POSE::PM_POSE(PM_CCONST PM_POSE & p)
{
    tran = p.tran;
    rot = p.rot;
}
#endif

PM_POSE PM_POSE::operator =(PM_POSE p)
{
    tran = p.tran;
    rot = p.rot;