_posemath.c

Source code for an Numeric Cmputer

/********************************************************************
* 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: 
* $Revision: 1.10 $
* $Author: jmkasunich $
* $Date: 2005/08/06 17:56:56 $
********************************************************************/

#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 "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;
	break;
    }

    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;
    r1 = 0;
    r2 = 0;
    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 - M_PIl) < ZYZ_Y_FUZZ) {
	zyz->z = 0.0;
	zyz->y = M_PIl;		/* 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 * M_PIl)) < ZYX_Y_FUZZ) {
	zyx->z = 0.0;
	zyx->y = (2 * M_PIl);	/* force it */
	zyx->x = atan2(m.y.x, m.y.y);
    } else if (fabs(zyx->y + (2 * M_PIl)) < ZYX_Y_FUZZ) {
	zyx->z = 0.0;
	zyx->y = -(2 * M_PIl);	/* 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 * M_PIl)) < RPY_P_FUZZ) {
	rpy->r = atan2(m.y.x, m.y.y);
	rpy->p = (2 * M_PIl);	/* force it */
	rpy->y = 0.0;
    } else if (fabs(rpy->p + (2 * M_PIl)) < RPY_P_FUZZ) {
	rpy->r = -atan2(m.y.z, m.y.y);
	rpy->p = -(2 * M_PIl);	/* 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;
}