Main Page   Class Hierarchy   Alphabetical List   Data Structures   File List   Data Fields   Globals  

posemath.h File Reference

This graph shows which files directly or indirectly include this file:

Included by dependency graph

Go to the source code of this file.

Data Structures

struct  PmCartesian
struct  PmSpherical
struct  PmCylindrical
struct  PmRotationVector
struct  PmRotationMatrix
struct  PmQuaternion
struct  PmEulerZyz
struct  PmEulerZyx
struct  PmRpy
struct  PmPose
struct  PmHomogeneous
struct  PmLine
struct  PmCircle

Defines

#define PM_PI   3.14159265358979323846
#define PM_PI_2   1.57079632679489661923
#define PM_PI_4   0.78539816339744830962
#define PM_2_PI   6.28318530717958647692
#define pmClose(a, b, eps)   ((fabs((a) - (b)) < (eps)) ? 1 : 0)
#define pmSq(x)   ((x)*(x))
#define TO_DEG   (180./PM_PI)
#define TO_RAD   (PM_PI/180.)
#define DOUBLE_FUZZ   2.2204460492503131e-16
#define DOUBLECP_FUZZ   1.0842021724855044e-19
#define CART_FUZZ   (0.000001)
#define Q_FUZZ   (.000001)
#define QS_FUZZ   (.000001)
#define RS_FUZZ   (.000001)
#define QSIN_FUZZ   (.000001)
#define V_FUZZ   (.000001)
#define SQRT_FUZZ   (-.000001)
#define UNIT_VEC_FUZZ   (.000001)
#define UNIT_QUAT_FUZZ   (.000001)
#define UNIT_SC_FUZZ   (.000001)
#define E_EPSILON   (.000001)
#define SINGULAR_EPSILON   (.000001)
#define RPY_P_FUZZ   (0.000001)
#define ZYZ_Y_FUZZ   (0.000001)
#define ZYX_Y_FUZZ   (0.000001)
#define PM_ERR   -1
#define PM_IMPL_ERR   -2
#define PM_NORM_ERR   -3
#define PM_DIV_ERR   -4
#define pmCartNorm(a, b, c, d, e)   bad{a.b.c.d.e}
#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;}

Enumerations

enum  PmAxis { PM_X, PM_Y, PM_Z }

Functions

void pmPrintError (const char *fmt,...)
void pmPerror (const char *fmt)
double pmSqrt (double x)
int pmCartSphConvert (PmCartesian, PmSpherical *)
int pmCartCylConvert (PmCartesian, PmCylindrical *)
int pmSphCartConvert (PmSpherical, PmCartesian *)
int pmSphCylConvert (PmSpherical, PmCylindrical *)
int pmCylCartConvert (PmCylindrical, PmCartesian *)
int pmCylSphConvert (PmCylindrical, PmSpherical *)
int pmAxisAngleQuatConvert (PmAxis, double, PmQuaternion *)
int pmRotQuatConvert (PmRotationVector, PmQuaternion *)
int pmRotMatConvert (PmRotationVector, PmRotationMatrix *)
int pmRotZyzConvert (PmRotationVector, PmEulerZyz *)
int pmRotZyxConvert (PmRotationVector, PmEulerZyx *)
int pmRotRpyConvert (PmRotationVector, PmRpy *)
int pmQuatRotConvert (PmQuaternion, PmRotationVector *)
int pmQuatMatConvert (PmQuaternion, PmRotationMatrix *)
int pmQuatZyzConvert (PmQuaternion, PmEulerZyz *)
int pmQuatZyxConvert (PmQuaternion, PmEulerZyx *)
int pmQuatRpyConvert (PmQuaternion, PmRpy *)
int pmMatRotConvert (PmRotationMatrix, PmRotationVector *)
int pmMatQuatConvert (PmRotationMatrix, PmQuaternion *)
int pmMatZyzConvert (PmRotationMatrix, PmEulerZyz *)
int pmMatZyxConvert (PmRotationMatrix, PmEulerZyx *)
int pmMatRpyConvert (PmRotationMatrix, PmRpy *)
int pmZyzRotConvert (PmEulerZyz, PmRotationVector *)
int pmZyzQuatConvert (PmEulerZyz, PmQuaternion *)
int pmZyzMatConvert (PmEulerZyz, PmRotationMatrix *)
int pmZyzZyxConvert (PmEulerZyz, PmEulerZyx *)
int pmZyzRpyConvert (PmEulerZyz, PmRpy *)
int pmZyxRotConvert (PmEulerZyx, PmRotationVector *)
int pmZyxQuatConvert (PmEulerZyx, PmQuaternion *)
int pmZyxMatConvert (PmEulerZyx, PmRotationMatrix *)
int pmZyxZyzConvert (PmEulerZyx, PmEulerZyz *)
int pmZyxRpyConvert (PmEulerZyx, PmRpy *)
int pmRpyRotConvert (PmRpy, PmRotationVector *)
int pmRpyQuatConvert (PmRpy, PmQuaternion *)
int pmRpyMatConvert (PmRpy, PmRotationMatrix *)
int pmRpyZyzConvert (PmRpy, PmEulerZyz *)
int pmRpyZyxConvert (PmRpy, PmEulerZyx *)
int pmPoseHomConvert (PmPose p, PmHomogeneous *h)
int pmHomPoseConvert (PmHomogeneous h, PmPose *p)
int pmCartCartCompare (PmCartesian, PmCartesian)
int pmCartCartDot (PmCartesian, PmCartesian, double *)
int pmCartCartCross (PmCartesian, PmCartesian, PmCartesian *)
int pmCartMag (PmCartesian, double *)
int pmCartCartDisp (PmCartesian v1, PmCartesian v2, double *d)
int pmCartCartAdd (PmCartesian, PmCartesian, PmCartesian *)
int pmCartCartSub (PmCartesian, PmCartesian, PmCartesian *)
int pmCartScalMult (PmCartesian, double, PmCartesian *)
int pmCartScalDiv (PmCartesian, double, PmCartesian *)
int pmCartNeg (PmCartesian, PmCartesian *)
int pmCartUnit (PmCartesian v, PmCartesian *vout)
int pmCartIsNorm (PmCartesian v)
int pmCartInv (PmCartesian, PmCartesian *)
int pmCartCartProj (PmCartesian, PmCartesian, PmCartesian *)
int pmCartPlaneProj (PmCartesian v, PmCartesian normal, PmCartesian *vout)
int pmQuatQuatCompare (PmQuaternion, PmQuaternion)
int pmQuatMag (PmQuaternion q, double *d)
int pmQuatNorm (PmQuaternion, PmQuaternion *)
int pmQuatInv (PmQuaternion, PmQuaternion *)
int pmQuatIsNorm (PmQuaternion)
int pmQuatScalMult (PmQuaternion q, double s, PmQuaternion *qout)
int pmQuatScalDiv (PmQuaternion q, double s, PmQuaternion *qout)
int pmQuatQuatMult (PmQuaternion, PmQuaternion, PmQuaternion *)
int pmQuatCartMult (PmQuaternion, PmCartesian, PmCartesian *)
int pmQuatAxisAngleMult (PmQuaternion, PmAxis, double, PmQuaternion *)
int pmRotScalMult (PmRotationVector, double, PmRotationVector *)
int pmRotScalDiv (PmRotationVector, double, PmRotationVector *)
int pmRotIsNorm (PmRotationVector)
int pmRotNorm (PmRotationVector, PmRotationVector *)
int pmMatNorm (PmRotationMatrix m, PmRotationMatrix *mout)
int pmMatIsNorm (PmRotationMatrix m)
int pmMatInv (PmRotationMatrix m, PmRotationMatrix *mout)
int pmMatCartMult (PmRotationMatrix m, PmCartesian v, PmCartesian *vout)
int pmMatMatMult (PmRotationMatrix m1, PmRotationMatrix m2, PmRotationMatrix *mout)
int pmPosePoseCompare (PmPose, PmPose)
int pmPoseInv (PmPose p, PmPose *)
int pmPoseCartMult (PmPose, PmCartesian, PmCartesian *)
int pmPosePoseMult (PmPose, PmPose, PmPose *)
int pmHomInv (PmHomogeneous, PmHomogeneous *)
int pmLineInit (PmLine *line, PmPose start, PmPose end)
int pmLinePoint (PmLine *line, double len, PmPose *point)
int pmCircleInit (PmCircle *circle, PmPose start, PmPose end, PmCartesian center, PmCartesian normal, int turn)
int pmCirclePoint (PmCircle *circle, double angle, PmPose *point)

Variables

int pmErrno

Define Documentation

#define PM_PI   3.14159265358979323846
 

Definition at line 692 of file posemath.h.

#define PM_PI_2   1.57079632679489661923
 

Definition at line 693 of file posemath.h.

#define PM_PI_4   0.78539816339744830962
 

Definition at line 694 of file posemath.h.

#define PM_2_PI   6.28318530717958647692
 

Definition at line 695 of file posemath.h.

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

Definition at line 720 of file posemath.h.

Referenced by pmRotQuatConvert().

#define pmSq (  x    )     ((x)*(x))
 

Definition at line 721 of file posemath.h.

Referenced by pmCartCartDisp(), pmCartCylConvert(), pmCartInv(), pmCartIsNorm(), pmCartMag(), pmCartSphConvert(), pmCartUnit(), pmCylSphConvert(), pmMatRpyConvert(), pmMatZyxConvert(), pmMatZyzConvert(), pmQuatIsNorm(), pmQuatMatConvert(), pmQuatNorm(), pmQuatRotConvert(), pmRotIsNorm(), pmRotMatConvert(), and pmRotNorm().

#define TO_DEG   (180./PM_PI)
 

Definition at line 726 of file posemath.h.

#define TO_RAD   (PM_PI/180.)
 

Definition at line 731 of file posemath.h.

#define DOUBLE_FUZZ   2.2204460492503131e-16
 

Definition at line 738 of file posemath.h.

#define DOUBLECP_FUZZ   1.0842021724855044e-19
 

Definition at line 739 of file posemath.h.

#define CART_FUZZ   (0.000001)
 

Definition at line 741 of file posemath.h.

#define Q_FUZZ   (.000001)
 

Definition at line 745 of file posemath.h.

#define QS_FUZZ   (.000001)
 

Definition at line 748 of file posemath.h.

#define RS_FUZZ   (.000001)
 

Definition at line 751 of file posemath.h.

#define QSIN_FUZZ   (.000001)
 

Definition at line 754 of file posemath.h.

#define V_FUZZ   (.000001)
 

Definition at line 757 of file posemath.h.

#define SQRT_FUZZ   (-.000001)
 

Definition at line 760 of file posemath.h.

#define UNIT_VEC_FUZZ   (.000001)
 

Definition at line 763 of file posemath.h.

#define UNIT_QUAT_FUZZ   (.000001)
 

Definition at line 766 of file posemath.h.

#define UNIT_SC_FUZZ   (.000001)
 

Definition at line 769 of file posemath.h.

#define E_EPSILON   (.000001)
 

Definition at line 772 of file posemath.h.

#define SINGULAR_EPSILON   (.000001)
 

Definition at line 775 of file posemath.h.

#define RPY_P_FUZZ   (0.000001)
 

Definition at line 778 of file posemath.h.

#define ZYZ_Y_FUZZ   (0.000001)
 

Definition at line 781 of file posemath.h.

#define ZYX_Y_FUZZ   (0.000001)
 

Definition at line 784 of file posemath.h.

#define PM_ERR   -1
 

Definition at line 793 of file posemath.h.

#define PM_IMPL_ERR   -2
 

Definition at line 794 of file posemath.h.

#define PM_NORM_ERR   -3
 

Definition at line 795 of file posemath.h.

#define PM_DIV_ERR   -4
 

Definition at line 796 of file posemath.h.

#define pmCartNorm (  a,
b,
c,
d,
e    )     bad{a.b.c.d.e}
 

Definition at line 885 of file posemath.h.

Referenced by pmCircleInit(), and pmCirclePoint().

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

Definition at line 955 of file posemath.h.

Referenced by cross(), disp(), dot(), inv(), isNorm(), mag(), operator *(), operator!=(), operator==(), proj(), and unit().

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

Definition at line 957 of file posemath.h.

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

Definition at line 959 of file posemath.h.

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

Definition at line 961 of file posemath.h.

Referenced by inv(), isNorm(), norm(), operator *(), operator!=(), operator-(), operator/(), and operator==().

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

Definition at line 963 of file posemath.h.

Referenced by isNorm(), and norm().

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

Definition at line 965 of file posemath.h.

Referenced by inv(), isNorm(), norm(), and operator *().

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

Definition at line 967 of file posemath.h.

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

Definition at line 969 of file posemath.h.

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

Definition at line 971 of file posemath.h.

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

Definition at line 973 of file posemath.h.

Referenced by inv(), operator *(), operator!=(), operator-(), and operator==().

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

Definition at line 975 of file posemath.h.

Referenced by inv().

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

Definition at line 977 of file posemath.h.

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

Definition at line 979 of file posemath.h.

Enumeration Type Documentation

enum PmAxis
 

Enumeration values:
PM_X 
PM_Y 
PM_Z 

Definition at line 584 of file posemath.h.

00584 {PM_X, PM_Y, PM_Z} PmAxis;

Function Documentation

void pmPrintError (  const char *    fmt,
...   
) 
 

Referenced by operator/(), pmAxisAngleQuatConvert(), pmCartInv(), pmCartScalDiv(), pmCartUnit(), pmCircleInit(), pmCirclePoint(), pmHomInv(), pmMatNorm(), pmMatQuatConvert(), pmPoseCartMult(), pmPoseInv(), pmPosePoseCompare(), pmPosePoseMult(), pmQuatAxisAngleMult(), pmQuatCartMult(), pmQuatInv(), pmQuatMatConvert(), pmQuatNorm(), pmQuatQuatCompare(), pmQuatQuatMult(), pmQuatRotConvert(), pmRotMatConvert(), pmRotNorm(), pmRotQuatConvert(), pmRotScalDiv(), pmRotZyzConvert(), pmRpyZyxConvert(), pmRpyZyzConvert(), pmSqrt(), pmZyxRpyConvert(), pmZyxZyzConvert(), pmZyzRotConvert(), and pmZyzRpyConvert().

void pmPerror (  const char *    fmt ) 
 

double pmSqrt (  double    x ) 
 

Definition at line 125 of file _posemath.c.

Referenced by pmCartCartDisp(), pmCartCylConvert(), pmCartIsNorm(), pmCartMag(), pmCartSphConvert(), pmCartUnit(), pmCylSphConvert(), pmMatQuatConvert(), pmMatZyzConvert(), pmQuatNorm(), pmQuatRotConvert(), pmRotIsNorm(), and pmRotNorm(). 

00126 {
00127   if (x > 0.0)
00128   {
00129     return sqrt(x);
00130   }
00131 
00132   if (x > SQRT_FUZZ)
00133   {
00134     return 0.0;
00135   }
00136 
00137 #ifdef PM_PRINT_ERROR
00138   pmPrintError("sqrt of large negative number\n");
00139 #endif
00140 
00141   return 0.0;
00142 }

int pmCartSphConvert (  PmCartesian    v,
PmSpherical *    s
) 
 

Definition at line 146 of file _posemath.c.

00147 {
00148   double _r;
00149 
00150   s->theta = atan2(v.y, v.x);
00151   s->r = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));
00152   _r = pmSqrt(pmSq(v.x) + pmSq(v.y));
00153   s->phi = atan2(_r, v.z);
00154 
00155   return pmErrno = 0;
00156 }

int pmCartCylConvert (  PmCartesian    v,
PmCylindrical *    c
) 
 

Definition at line 158 of file _posemath.c.

00159 {
00160   c->theta = atan2(v.y, v.x);
00161   c->r = pmSqrt(pmSq(v.x) + pmSq(v.y));
00162   c->z = v.z;
00163 
00164   return pmErrno = 0;
00165 }

int pmSphCartConvert (  PmSpherical    s,
PmCartesian *    v
) 
 

Definition at line 167 of file _posemath.c.

00168 {
00169   double _r;
00170 
00171   _r = s.r * sin(s.phi);
00172   v->z = s.r * cos(s.phi);
00173   v->x = _r * cos(s.theta);
00174   v->y = _r * sin(s.theta);
00175 
00176   return pmErrno = 0;
00177 }

int pmSphCylConvert (  PmSpherical    s,
PmCylindrical *    c
) 
 

Definition at line 179 of file _posemath.c.

00180 {
00181   c->theta = s.theta;
00182   c->r = s.r*cos(s.phi);
00183   c->z = s.r*sin(s.phi);
00184   return pmErrno = 0;
00185 }

int pmCylCartConvert (  PmCylindrical    c,
PmCartesian *    v
) 
 

Definition at line 187 of file _posemath.c.

00188 {
00189   v->x = c.r * cos(c.theta);
00190   v->y = c.r * sin(c.theta);
00191   v->z = c.z;
00192   return pmErrno = 0;
00193 }

int pmCylSphConvert (  PmCylindrical    c,
PmSpherical *    s
) 
 

Definition at line 195 of file _posemath.c.

00196 {
00197   s->theta = c.theta;
00198   s->r = pmSqrt(pmSq(c.r) + pmSq(c.z));
00199   s->phi = atan2(c.z,c.r);
00200   return pmErrno = 0;
00201 }

int pmAxisAngleQuatConvert (  PmAxis    axis,
double    a,
PmQuaternion *    q
) 
 

Definition at line 205 of file _posemath.c.

00206 {
00207   double sh;
00208 
00209   a *= 0.5;
00210   sincos(a, &sh, &(q->s));
00211 
00212   switch (axis)
00213     {
00214     case PM_X:
00215       q->x = sh;
00216       q->y = 0.0;
00217       q->z = 0.0;
00218       break;
00219 
00220     case PM_Y:
00221       q->x = 0.0;
00222       q->y = sh;
00223       q->z = 0.0;
00224       break;
00225 
00226     case PM_Z:
00227       q->x = 0.0;
00228       q->y = 0.0;
00229       q->z = sh;
00230       break;
00231 
00232     default:
00233 #ifdef PM_PRINT_ERROR
00234       pmPrintError("error: bad axis in pmAxisAngleQuatConvert\n");
00235 #endif
00236       return -1;
00237       break;
00238     }
00239 
00240   if( q->s < 0.0 )
00241     {
00242       q->s *= -1.0;
00243       q->x *= -1.0;
00244       q->y *= -1.0;
00245       q->z *= -1.0;
00246     }
00247 
00248   return 0;
00249 }

int pmRotQuatConvert (  PmRotationVector    r,
PmQuaternion *    q
) 
 

Definition at line 251 of file _posemath.c.

Referenced by pmQuatScalDiv(), pmQuatScalMult(), pmRotRpyConvert(), and testcase_pmrpyrot(). 

00252 {
00253   double sh;
00254 
00255 #ifdef PM_DEBUG
00256   /* make sure r is normalized */
00257   if (0 != pmRotNorm(r, &r))
00258   {
00259 #ifdef PM_PRINT_ERROR
00260     pmPrintError("error: pmRotQuatConvert rotation vector not normalized\n");
00261 #endif
00262     return pmErrno = PM_NORM_ERR;
00263   }
00264 #endif
00265 
00266   if (pmClose(r.s, 0.0, QS_FUZZ))
00267   {
00268     q->s = 1.0;
00269     q->x = q->y = q->z = 0.0;
00270 
00271     return pmErrno = 0;
00272   }
00273 
00274   sincos(r.s / 2.0, &sh, &(q->s));
00275 
00276   if (q->s >= 0.0)
00277   {
00278     q->x = r.x * sh;
00279     q->y = r.y * sh;
00280     q->z = r.z * sh;
00281   }
00282   else
00283   {
00284     q->s *= -1;
00285     q->x = -r.x * sh;
00286     q->y = -r.y * sh;
00287     q->z = -r.z * sh;
00288   }
00289 
00290   return pmErrno = 0;
00291 }

int pmRotMatConvert (  PmRotationVector    r,
PmRotationMatrix *    m
) 
 

Definition at line 293 of file _posemath.c.

Referenced by pmRotZyxConvert(). 

00294 {
00295   double s, c, omc;
00296 
00297 #ifdef PM_DEBUG
00298   if (! pmRotIsNorm(r))
00299   {
00300 #ifdef PM_PRINT_ERROR
00301     pmPrintError("Bad vector in pmRotMatConvert\n");
00302 #endif
00303     return pmErrno = PM_NORM_ERR;
00304   }
00305 #endif
00306 
00307   sincos(r.s, &s, &c);
00308 
00309   /* from space book */
00310   m->x.x = c + pmSq(r.x) * (omc = 1 - c);       /* omc = One Minus Cos */
00311   m->y.x = -r.z * s + r.x * r.y * omc;
00312   m->z.x = r.y * s + r.x * r.z * omc;
00313 
00314   m->x.y = r.z * s + r.y * r.x * omc;
00315   m->y.y = c + pmSq(r.y) * omc;
00316   m->z.y = -r.x * s + r.y * r.z * omc;
00317 
00318   m->x.z = -r.y * s + r.z * r.x * omc;
00319   m->y.z = r.x * s + r.z * r.y * omc;
00320   m->z.z = c + pmSq(r.z) * omc;
00321 
00322   return pmErrno = 0;
00323 }

int pmRotZyzConvert (  PmRotationVector    r,
PmEulerZyz *    zyz
) 
 

Definition at line 325 of file _posemath.c.

00326 {
00327 #ifdef PM_DEBUG
00328 #ifdef PM_PRINT_ERROR
00329   pmPrintError("error: pmRotZyzConvert not implemented\n");
00330 #endif
00331   return pmErrno = PM_IMPL_ERR;
00332 #else
00333   return PM_IMPL_ERR;
00334 #endif
00335 }

int pmRotZyxConvert (  PmRotationVector    r,
PmEulerZyx *    zyx
) 
 

Definition at line 337 of file _posemath.c.

Referenced by fullcircle_test(). 

00338 {
00339   PmRotationMatrix m;
00340   int r1, r2;
00341 
00342   r1 = pmRotMatConvert(r, &m);
00343   r2 = pmMatZyxConvert(m, zyx);
00344 
00345   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00346 }

int pmRotRpyConvert (  PmRotationVector    r,
PmRpy *    rpy
) 
 

Definition at line 348 of file _posemath.c.

Referenced by testcase_pmrpyrot(). 

00349 {
00350   PmQuaternion q;
00351   int r1,r2;
00352   r1 = 0;
00353   r2 = 0;
00354   q.s = q.x = q.y = q.z = 0.0;
00355 
00356   r1 = pmRotQuatConvert(r,&q);
00357   r2 = pmQuatRpyConvert(q,rpy);
00358 
00359   return r1 || r2 ?  pmErrno : 0;
00360 }

int pmQuatRotConvert (  PmQuaternion    q,
PmRotationVector *    r
) 
 

Definition at line 362 of file _posemath.c.

Referenced by pmMatRotConvert(), pmQuatMag(), pmQuatScalDiv(), pmQuatScalMult(), pmRpyRotConvert(), and testcase_pmrpyrot(). 

00363 {
00364   double sh;
00365 
00366 #ifdef PM_DEBUG
00367   if (! pmQuatIsNorm(q))
00368   {
00369 #ifdef PM_PRINT_ERROR
00370     pmPrintError("Bad quaternion in pmQuatRotConvert\n");
00371 #endif
00372     return pmErrno = PM_NORM_ERR;
00373   }
00374 #endif
00375   if(r == 0)
00376     {
00377       return (pmErrno = PM_ERR);
00378     }
00379 
00380   sh = pmSqrt(pmSq(q.x) + pmSq(q.y) + pmSq(q.z));
00381 
00382   if (sh > QSIN_FUZZ)
00383   {
00384     r->s = 2.0 * atan2(sh, q.s);
00385     r->x = q.x / sh;
00386     r->y = q.y / sh;
00387     r->z = q.z / sh;
00388   }
00389   else
00390   {
00391     r->s = 0.0;
00392     r->x = 0.0;
00393     r->y = 0.0;
00394     r->z = 0.0;
00395   }
00396 
00397   return pmErrno = 0;
00398 }

int pmQuatMatConvert (  PmQuaternion    q,
PmRotationMatrix *    m
) 
 

Definition at line 400 of file _posemath.c.

Referenced by _pmSprintf(), pmPoseHomConvert(), pmQuatRpyConvert(), pmQuatZyxConvert(), and pmQuatZyzConvert(). 

00401 {
00402 #ifdef PM_DEBUG
00403   if (! pmQuatIsNorm(q))
00404   {
00405 #ifdef PM_PRINT_ERROR
00406     pmPrintError("Bad quaternion in pmQuatMatConvert\n");
00407 #endif
00408     return pmErrno = PM_NORM_ERR;
00409   }
00410 #endif
00411 
00412   /* from space book where e1=q.x e2=q.y e3=q.z e4=q.s */
00413   m->x.x = 1 - 2 * (pmSq(q.y) + pmSq(q.z));
00414   m->y.x = 2 * (q.x * q.y - q.z * q.s);
00415   m->z.x = 2 * (q.z * q.x + q.y * q.s);
00416 
00417   m->x.y = 2 * (q.x * q.y + q.z * q.s);
00418   m->y.y = 1 - 2 * (pmSq(q.z) + pmSq(q.x));
00419   m->z.y = 2 * (q.y * q.z - q.x * q.s);
00420 
00421   m->x.z = 2 * (q.z * q.x - q.y * q.s);
00422   m->y.z = 2 * (q.y * q.z + q.x * q.s);
00423   m->z.z = 1 - 2 * (pmSq(q.x) + pmSq(q.y));
00424 
00425   return pmErrno = 0;
00426 }

int pmQuatZyzConvert (  PmQuaternion    q,
PmEulerZyz *    zyz
) 
 

Definition at line 428 of file _posemath.c.

00429 {
00430   PmRotationMatrix m;
00431   int r1, r2;
00432 
00433   /* FIXME-- need direct equations */
00434   r1 = pmQuatMatConvert(q, &m);
00435   r2 = pmMatZyzConvert(m, zyz);
00436 
00437   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00438 }

int pmQuatZyxConvert (  PmQuaternion    q,
PmEulerZyx *    zyx
) 
 

Definition at line 440 of file _posemath.c.

00441 {
00442   PmRotationMatrix m;
00443   int r1, r2;
00444 
00445   /* FIXME-- need direct equations */
00446   r1 = pmQuatMatConvert(q, &m);
00447   r2 = pmMatZyxConvert(m, zyx);
00448 
00449   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00450 }

int pmQuatRpyConvert (  PmQuaternion    q,
PmRpy *    rpy
) 
 

Definition at line 452 of file _posemath.c.

Referenced by pmRotRpyConvert(), and testcase_pmrpyrot(). 

00453 {
00454   PmRotationMatrix m;
00455   int r1, r2;
00456 
00457   /* FIXME-- need direct equations */
00458   r1 = pmQuatMatConvert(q, &m);
00459   r2 = pmMatRpyConvert(m, rpy);
00460 
00461   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00462 }

int pmMatRotConvert (  PmRotationMatrix    m,
PmRotationVector *    r
) 
 

Definition at line 464 of file _posemath.c.

Referenced by pmZyxRotConvert(). 

00465 {
00466   PmQuaternion q;
00467   int r1, r2;
00468 
00469   /* FIXME-- need direct equations */
00470   r1 = pmMatQuatConvert(m,&q);
00471   r2 = pmQuatRotConvert(q, r);
00472 
00473   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00474 }

int pmMatQuatConvert (  PmRotationMatrix    m,
PmQuaternion *    q
) 
 

Definition at line 477 of file _posemath.c.

Referenced by pmHomPoseConvert(), pmMatRotConvert(), pmRpyQuatConvert(), pmZyxQuatConvert(), and pmZyzQuatConvert(). 

00478 {
00479   /*
00480      from Stephe's "space" book
00481      e1 = (c32 - c23) / 4*e4
00482      e2 = (c13 - c31) / 4*e4
00483      e3 = (c21 - c12) / 4*e4
00484      e4 = sqrt(1 + c11 + c22 + c33) / 2
00485 
00486      if e4 == 0
00487      e1 = sqrt(1 + c11 - c33 - c22) / 2
00488      e2 = sqrt(1 + c22 - c33 - c11) / 2
00489      e3 = sqrt(1 + c33 - c11 - c22) / 2
00490      to determine whether to take the positive or negative sqrt value
00491      since e4 == 0 indicates a 180* rotation then (0 x y z) = (0 -x -y -z).
00492      Thus some generallities can be used:
00493      1) find which of e1, e2, or e3 has the largest magnitude and leave it pos.
00494      2) if e1 is largest then
00495      if c21 < 0 then take the negative for e2
00496      if c31 < 0 then take the negative for e3
00497      3) else if e2 is largest then
00498      if c21 < 0 then take the negative for e1
00499      if c32 < 0 then take the negative for e3
00500      4) else if e3 is larget then
00501      if c31 < 0 then take the negative for e1
00502      if c32 < 0 then take the negative for e2
00503 
00504      Note: c21 in the space book is m.x.y in this C code
00505      */
00506 
00507   double a;
00508 
00509 #ifdef PM_DEBUG
00510   if (! pmMatIsNorm(m))
00511   {
00512 #ifdef PM_PRINT_ERROR
00513     pmPrintError("Bad matrix in pmMatQuatConvert\n");
00514 #endif
00515     return pmErrno = PM_NORM_ERR;
00516   }
00517 #endif
00518 
00519   q->s = 0.5 * pmSqrt(1.0 + m.x.x + m.y.y + m.z.z);
00520 
00521   if (fabs(q->s) > QS_FUZZ)
00522   {
00523     q->x = (m.y.z - m.z.y) / (a = 4 * q->s);
00524     q->y = (m.z.x - m.x.z) / a;
00525     q->z = (m.x.y - m.y.x) / a;
00526   }
00527   else
00528   {
00529     q->s = 0;
00530     q->x = pmSqrt(1.0 + m.x.x - m.y.y - m.z.z) / 2.0;
00531     q->y = pmSqrt(1.0 + m.y.y - m.x.x - m.z.z) / 2.0;
00532     q->z = pmSqrt(1.0 + m.z.z - m.y.y - m.x.x) / 2.0;
00533 
00534     if (q->x > q->y && q->x > q->z)
00535     {
00536       if (m.x.y < 0.0)
00537       {
00538         q->y *= -1;
00539       }
00540       if (m.x.z < 0.0)
00541       {
00542         q->z *= -1;
00543       }
00544     }
00545     else if (q->y > q->z)
00546     {
00547       if (m.x.y < 0.0)
00548       {
00549         q->x *= -1;
00550       }
00551       if (m.y.z < 0.0)
00552       {
00553         q->z *= -1;
00554       }
00555     }
00556     else
00557     {
00558       if (m.x.z < 0.0)
00559       {
00560         q->x *= -1;
00561       }
00562       if (m.y.z < 0.0)
00563       {
00564         q->y *= -1;
00565       }
00566     }
00567   }
00568 
00569   return pmQuatNorm(*q, q);
00570 }

int pmMatZyzConvert (  PmRotationMatrix    m,
PmEulerZyz *    zyz
) 
 

Definition at line 572 of file _posemath.c.

Referenced by pmQuatZyzConvert(). 

00573 {
00574   zyz->y = atan2(pmSqrt(pmSq(m.x.z) + pmSq(m.y.z)), m.z.z);
00575 
00576   if (fabs(zyz->y) < ZYZ_Y_FUZZ)
00577   {
00578     zyz->z = 0.0;
00579     zyz->y = 0.0;               /* force Y to 0 */
00580     zyz->zp = atan2(-m.y.x, m.x.x);
00581   }
00582   else if (fabs(zyz->y - PM_PI) < ZYZ_Y_FUZZ)
00583   {
00584     zyz->z = 0.0;
00585     zyz->y = PM_PI;             /* force Y to 180 */
00586     zyz->zp = atan2(m.y.x, -m.x.x);
00587   }
00588   else
00589   {
00590     zyz->z = atan2(m.z.y, m.z.x);
00591     zyz->zp = atan2(m.y.z, -m.x.z);
00592   }
00593 
00594   return pmErrno = 0;
00595 }

int pmMatZyxConvert (  PmRotationMatrix    m,
PmEulerZyx *    zyx
) 
 

Definition at line 597 of file _posemath.c.

Referenced by pmQuatZyxConvert(), and pmRotZyxConvert(). 

00598 {
00599   zyx->y = atan2(-m.x.z, pmSqrt(pmSq(m.x.x) + pmSq(m.x.y)));
00600 
00601   if (fabs(zyx->y - PM_PI_2) < ZYX_Y_FUZZ)
00602   {
00603     zyx->z = 0.0;
00604     zyx->y = PM_PI_2;           /* force it */
00605     zyx->x = atan2(m.y.x, m.y.y);
00606   }
00607   else if (fabs(zyx->y + PM_PI_2) < ZYX_Y_FUZZ)
00608   {
00609     zyx->z = 0.0;
00610     zyx->y = -PM_PI_2;          /* force it */
00611     zyx->x = -atan2(m.y.z, m.y.y);
00612   }
00613   else
00614   {
00615     zyx->z = atan2(m.x.y, m.x.x);
00616     zyx->x = atan2(m.y.z, m.z.z);
00617   }
00618 
00619   return pmErrno = 0;
00620 }

int pmMatRpyConvert (  PmRotationMatrix    m,
PmRpy *    rpy
) 
 

Definition at line 622 of file _posemath.c.

Referenced by pmQuatRpyConvert(). 

00623 {
00624   rpy->p = atan2(-m.x.z, pmSqrt(pmSq(m.x.x) + pmSq(m.x.y)));
00625 
00626   if (fabs(rpy->p -PM_PI_2) < RPY_P_FUZZ)
00627   {
00628     rpy->r = atan2(m.y.x, m.y.y);
00629     rpy->p =PM_PI_2;            /* force it */
00630     rpy->y = 0.0;
00631   }
00632   else if (fabs(rpy->p +PM_PI_2) < RPY_P_FUZZ)
00633   {
00634     rpy->r = -atan2(m.y.z, m.y.y);
00635     rpy->p = -PM_PI_2;          /* force it */
00636     rpy->y = 0.0;
00637   }
00638   else
00639   {
00640     rpy->r = atan2(m.y.z, m.z.z);
00641     rpy->y = atan2(m.x.y, m.x.x);
00642   }
00643 
00644   return pmErrno = 0;
00645 }

int pmZyzRotConvert (  PmEulerZyz    zyz,
PmRotationVector *    r
) 
 

Definition at line 647 of file _posemath.c.

00648 {
00649 #ifdef PM_PRINT_ERROR
00650   pmPrintError("error: pmZyzRotConvert not implemented\n");
00651 #endif
00652   return pmErrno = PM_IMPL_ERR;
00653 }

int pmZyzQuatConvert (  PmEulerZyz    zyz,
PmQuaternion *    q
) 
 

Definition at line 655 of file _posemath.c.

00656 {
00657   PmRotationMatrix m;
00658   int r1, r2;
00659 
00660   /* FIXME-- need direct equations */
00661   r1 = pmZyzMatConvert(zyz, &m);
00662   r2 = pmMatQuatConvert(m, q);
00663 
00664   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00665 }

int pmZyzMatConvert (  PmEulerZyz    zyz,
PmRotationMatrix *    m
) 
 

Definition at line 667 of file _posemath.c.

Referenced by pmZyzQuatConvert(). 

00668 {
00669   double sa, sb, sg;
00670   double ca, cb, cg;
00671 
00672   sa = sin(zyz.z);
00673   sb = sin(zyz.y);
00674   sg = sin(zyz.zp);
00675 
00676   ca = cos(zyz.z);
00677   cb = cos(zyz.y);
00678   cg = cos(zyz.zp);
00679 
00680   m->x.x = ca * cb * cg - sa * sg;
00681   m->y.x = -ca * cb * sg - sa * cg;
00682   m->z.x = ca * sb;
00683 
00684   m->x.y = sa * cb * cg + ca * sg;
00685   m->y.y = -sa * cb * sg + ca * cg;
00686   m->z.y = sa * sb;
00687 
00688   m->x.z = -sb * cg;
00689   m->y.z = sb * sg;
00690   m->z.z = cb;
00691 
00692   return pmErrno = 0;
00693 }

int pmZyzZyxConvert (  PmEulerZyz   ,
PmEulerZyx *   
) 
 

int pmZyzRpyConvert (  PmEulerZyz    zyz,
PmRpy *    rpy
) 
 

Definition at line 695 of file _posemath.c.

00696 {
00697 #ifdef PM_PRINT_ERROR
00698   pmPrintError("error: pmZyzRpyConvert not implemented\n");
00699 #endif
00700   return pmErrno = PM_IMPL_ERR;
00701 }

int pmZyxRotConvert (  PmEulerZyx    zyx,
PmRotationVector *    r
) 
 

Definition at line 703 of file _posemath.c.

Referenced by main(). 

00704 {
00705   PmRotationMatrix m;
00706   int r1, r2;
00707 
00708   /* FIXME-- need direct equations */
00709   r1 = pmZyxMatConvert(zyx, &m);
00710   r2 = pmMatRotConvert(m, r);
00711 
00712   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00713 }

int pmZyxQuatConvert (  PmEulerZyx    zyx,
PmQuaternion *    q
) 
 

Definition at line 716 of file _posemath.c.

00717 {
00718   PmRotationMatrix m;
00719   int r1, r2;
00720 
00721   /* FIXME-- need direct equations */
00722   r1 = pmZyxMatConvert(zyx, &m);
00723   r2 = pmMatQuatConvert(m, q);
00724 
00725   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00726 }

int pmZyxMatConvert (  PmEulerZyx    zyx,
PmRotationMatrix *    m
) 
 

Definition at line 728 of file _posemath.c.

Referenced by pmZyxQuatConvert(), and pmZyxRotConvert(). 

00729 {
00730   double sa, sb, sg;
00731   double ca, cb, cg;
00732 
00733   sa = sin(zyx.z);
00734   sb = sin(zyx.y);
00735   sg = sin(zyx.x);
00736 
00737   ca = cos(zyx.z);
00738   cb = cos(zyx.y);
00739   cg = cos(zyx.x);
00740 
00741   m->x.x = ca * cb;
00742   m->y.x = ca * sb * sg - sa * cg;
00743   m->z.x = ca * sb * cg + sa * sg;
00744 
00745   m->x.y = sa * cb;
00746   m->y.y = sa * sb * sg + ca * cg;
00747   m->z.y = sa * sb * cg - ca * sg;
00748 
00749   m->x.z = -sb;
00750   m->y.z = cb * sg;
00751   m->z.z = cb * cg;
00752 
00753   return pmErrno = 0;
00754 }

int pmZyxZyzConvert (  PmEulerZyx    zyx,
PmEulerZyz *    zyz
) 
 

Definition at line 756 of file _posemath.c.

00757 {
00758 #ifdef PM_PRINT_ERROR
00759   pmPrintError("error: pmZyxZyzConvert not implemented\n");
00760 #endif
00761   return pmErrno = PM_IMPL_ERR;
00762 }

int pmZyxRpyConvert (  PmEulerZyx    zyx,
PmRpy *    rpy
) 
 

Definition at line 764 of file _posemath.c.

00765 {
00766 #ifdef PM_PRINT_ERROR
00767   pmPrintError("error: pmZyxRpyConvert not implemented\n");
00768 #endif
00769   return pmErrno = PM_IMPL_ERR;
00770 }

int pmRpyRotConvert (  PmRpy    rpy,
PmRotationVector *    r
) 
 

Definition at line 772 of file _posemath.c.

Referenced by testcase_pmrpyrot(). 

00773 {
00774   PmQuaternion q;
00775   int r1,r2;
00776   r1 = 0;
00777   r2 = 0;
00778   q.s = q.x = q.y = q.z = 0.0;
00779   r->s = r->x = r->y = r->z = 0.0;
00780 
00781   r1 = pmRpyQuatConvert(rpy,&q);
00782   r2 = pmQuatRotConvert(q,r);
00783 
00784   return r1 || r2 ? pmErrno : 0;
00785 }

int pmRpyQuatConvert (  PmRpy    rpy,
PmQuaternion *    q
) 
 

Definition at line 787 of file _posemath.c.

Referenced by pmRpyRotConvert(), and testcase_pmrpyrot(). 

00788 {
00789   PmRotationMatrix m;
00790   int r1, r2;
00791 
00792   /* FIXME-- need direct equations */
00793   r1 = pmRpyMatConvert(rpy, &m);
00794   r2 = pmMatQuatConvert(m, q);
00795 
00796   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
00797 }

int pmRpyMatConvert (  PmRpy    rpy,
PmRotationMatrix *    m
) 
 

Definition at line 799 of file _posemath.c.

Referenced by pmRpyQuatConvert(). 

00800 {
00801   double sa, sb, sg;
00802   double ca, cb, cg;
00803 
00804   sa = sin(rpy.y);
00805   sb = sin(rpy.p);
00806   sg = sin(rpy.r);
00807 
00808   ca = cos(rpy.y);
00809   cb = cos(rpy.p);
00810   cg = cos(rpy.r);
00811 
00812   m->x.x = ca * cb;
00813   m->y.x = ca * sb * sg - sa * cg;
00814   m->z.x = ca * sb * cg + sa * sg;
00815 
00816   m->x.y = sa * cb;
00817   m->y.y = sa * sb * sg + ca * cg;
00818   m->z.y = sa * sb * cg - ca * sg;
00819 
00820   m->x.z = -sb;
00821   m->y.z = cb * sg;
00822   m->z.z = cb * cg;
00823 
00824   return pmErrno = 0;
00825 }

int pmRpyZyzConvert (  PmRpy    rpy,
PmEulerZyz *    zyz
) 
 

Definition at line 827 of file _posemath.c.

00828 {
00829 #ifdef PM_PRINT_ERROR
00830   pmPrintError("error: pmRpyZyzConvert not implemented\n");
00831 #endif
00832   return pmErrno = PM_IMPL_ERR;
00833 }

int pmRpyZyxConvert (  PmRpy    rpy,
PmEulerZyx *    zyx
) 
 

Definition at line 835 of file _posemath.c.

00836 {
00837 #ifdef PM_PRINT_ERROR
00838   pmPrintError("error: pmRpyZyxConvert not implemented\n");
00839 #endif
00840   return pmErrno = PM_IMPL_ERR;
00841 }

int pmPoseHomConvert (  PmPose    p,
PmHomogeneous *    h
) 
 

Definition at line 843 of file _posemath.c.

00844 {
00845   int r1;
00846 
00847   h->tran = p.tran;
00848   r1 = pmQuatMatConvert(p.rot, &h->rot);
00849 
00850   return pmErrno = r1;
00851 }

int pmHomPoseConvert (  PmHomogeneous    h,
PmPose *    p
) 
 

Definition at line 853 of file _posemath.c.

00854 {
00855   int r1;
00856 
00857   p->tran = h.tran;
00858   r1 = pmMatQuatConvert(h.rot, &p->rot);
00859 
00860   return pmErrno = r1;
00861 }

int pmCartCartCompare (  PmCartesian    v1,
PmCartesian    v2
) 
 

Definition at line 865 of file _posemath.c.

Referenced by operator!=(), and operator==(). 

00866 {
00867   if (fabs(v1.x-v2.x) >= V_FUZZ ||
00868       fabs(v1.y-v2.y) >= V_FUZZ ||
00869       fabs(v1.z-v2.z) >= V_FUZZ)
00870   {
00871     return 0;
00872   }
00873 
00874   return 1;
00875 }

int pmCartCartDot (  PmCartesian    v1,
PmCartesian    v2,
double *    d
) 
 

Definition at line 877 of file _posemath.c.

Referenced by dot(), pmCartCartProj(), and pmCircleInit(). 

00878 {
00879   *d = v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
00880 
00881   return pmErrno = 0;
00882 }

int pmCartCartCross (  PmCartesian    v1,
PmCartesian    v2,
PmCartesian *    vout
) 
 

Definition at line 884 of file _posemath.c.

Referenced by cross(), pmCircleInit(), and pmMatIsNorm(). 

00885 {
00886   vout->x = v1.y * v2.z - v1.z * v2.y;
00887   vout->y = v1.z * v2.x - v1.x * v2.z;
00888   vout->z = v1.x * v2.y - v1.y * v2.x;
00889 
00890   return pmErrno = 0;
00891 }

int pmCartMag (  PmCartesian    v,
double *    d
) 
 

Definition at line 893 of file _posemath.c.

Referenced by mag(), pmCircleInit(), and pmLineInit(). 

00894 {
00895   *d = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));
00896 
00897   return pmErrno = 0;
00898 }

int pmCartCartDisp (  PmCartesian    v1,
PmCartesian    v2,
double *    d
) 
 

Definition at line 900 of file _posemath.c.

Referenced by disp(), and pmCircleInit(). 

00901 {
00902   *d = pmSqrt(pmSq(v2.x - v1.x) + pmSq(v2.y - v1.y) + pmSq(v2.z - v1.z));
00903 
00904   return pmErrno = 0;
00905 }

int pmCartCartAdd (  PmCartesian    v1,
PmCartesian    v2,
PmCartesian *    vout
) 
 

Definition at line 907 of file _posemath.c.

Referenced by pmCircleInit(), pmCirclePoint(), pmLinePoint(), pmPoseCartMult(), and pmPosePoseMult(). 

00908 {
00909   vout->x = v1.x + v2.x;
00910   vout->y = v1.y + v2.y;
00911   vout->z = v1.z + v2.z;
00912 
00913   return pmErrno = 0;
00914 }

int pmCartCartSub (  PmCartesian    v1,
PmCartesian    v2,
PmCartesian *    vout
) 
 

Definition at line 916 of file _posemath.c.

Referenced by pmCartPlaneProj(), pmCircleInit(), and pmLineInit(). 

00917 {
00918   vout->x = v1.x - v2.x;
00919   vout->y = v1.y - v2.y;
00920   vout->z = v1.z - v2.z;
00921 
00922   return pmErrno = 0;
00923 }

int pmCartScalMult (  PmCartesian    v1,
double    d,
PmCartesian *    vout
) 
 

Definition at line 925 of file _posemath.c.

Referenced by pmCartCartProj(), pmCircleInit(), pmCirclePoint(), and pmLinePoint(). 

00926 {
00927   vout->x = v1.x * d;
00928   vout->y = v1.y * d;
00929   vout->z = v1.z * d;
00930 
00931   return pmErrno = 0;
00932 }

int pmCartScalDiv (  PmCartesian    v1,
double    d,
PmCartesian *    vout
) 
 

Definition at line 934 of file _posemath.c.

00935 {
00936   if (d == 0.0)
00937   {
00938 #ifdef PM_PRINT_ERROR
00939     pmPrintError("Divide by 0 in pmCartScalDiv\n");
00940 #endif
00941     vout->x = DBL_MAX;
00942     vout->y = DBL_MAX;
00943     vout->z = DBL_MAX;
00944 
00945     return pmErrno = PM_DIV_ERR;
00946   }
00947 
00948   vout->x = v1.x / d;
00949   vout->y = v1.y / d;
00950   vout->z = v1.z / d;
00951 
00952   return pmErrno = 0;
00953 }

int pmCartNeg (  PmCartesian    v1,
PmCartesian *    vout
) 
 

Definition at line 955 of file _posemath.c.

00956 {
00957   vout->x = -v1.x;
00958   vout->y = -v1.y;
00959   vout->z = -v1.z;
00960 
00961   return pmErrno = 0;
00962 }

int pmCartUnit (  PmCartesian    v,
PmCartesian *    vout
) 
 

Definition at line 990 of file _posemath.c.

Referenced by pmCartCartProj(), pmCircleInit(), pmCirclePoint(), pmLineInit(), and unit(). 

00991 {
00992   double size = pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z));
00993 
00994   if (size == 0.0)
00995   {
00996 #ifdef PM_PRINT_ERROR
00997     pmPrintError("Zero vector in pmCartUnit\n");
00998 #endif
00999 
01000     vout->x = DBL_MAX;
01001     vout->y = DBL_MAX;
01002     vout->z = DBL_MAX;
01003 
01004     return pmErrno = PM_NORM_ERR;
01005   }
01006 
01007   vout->x = v.x / size;
01008   vout->y = v.y / size;
01009   vout->z = v.z / size;
01010 
01011   return pmErrno = 0;
01012 }

int pmCartIsNorm (  PmCartesian    v ) 
 

Definition at line 1031 of file _posemath.c.

Referenced by isNorm(), and pmMatIsNorm(). 

01032 {
01033   return pmSqrt(pmSq(v.x) + pmSq(v.y) + pmSq(v.z)) - 1.0 <
01034     UNIT_VEC_FUZZ ? 1 : 0;
01035 }

int pmCartInv (  PmCartesian    v1,
PmCartesian *    vout
) 
 

Definition at line 964 of file _posemath.c.

Referenced by inv(). 

00965 {
00966   double size_sq = pmSq(v1.x) + pmSq(v1.y) + pmSq(v1.z);
00967 
00968   if (size_sq == 0.0)
00969   {
00970 #ifdef PM_PRINT_ERROR
00971     pmPrintError("Zero vector in pmCartInv\n");
00972 #endif
00973 
00974     vout->x = DBL_MAX;
00975     vout->y = DBL_MAX;
00976     vout->z = DBL_MAX;
00977 
00978     return pmErrno = PM_NORM_ERR;
00979   }
00980 
00981   vout->x = v1.x / size_sq;
00982   vout->y = v1.y / size_sq;
00983   vout->z = v1.z / size_sq;
00984 
00985   return pmErrno = 0;
00986 }

int pmCartCartProj (  PmCartesian    v1,
PmCartesian    v2,
PmCartesian *    vout
) 
 

Definition at line 1037 of file _posemath.c.

Referenced by pmCartPlaneProj(), pmCircleInit(), and proj(). 

01039 {
01040   int r1, r2, r3;
01041   double d;
01042 
01043   r1 = pmCartUnit(v2, &v2);
01044   r2 = pmCartCartDot(v1, v2, &d);
01045   r3 = pmCartScalMult(v2, d, vout);
01046 
01047   return pmErrno = r1 || r2 || r3 ? PM_NORM_ERR : 0;
01048 }

int pmCartPlaneProj (  PmCartesian    v,
PmCartesian    normal,
PmCartesian *    vout
) 
 

Definition at line 1050 of file _posemath.c.

Referenced by pmCircleInit(). 

01051 {
01052   int r1, r2;
01053   PmCartesian par;
01054 
01055   r1 = pmCartCartProj(v, normal, &par);
01056   r2 = pmCartCartSub(v, par, vout);
01057 
01058   return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
01059 }

int pmQuatQuatCompare (  PmQuaternion    q1,
PmQuaternion    q2
) 
 

Definition at line 1280 of file _posemath.c.

Referenced by operator!=(), operator==(), and pmPosePoseCompare(). 

01281 {
01282 #ifdef PM_DEBUG
01283   if (! pmQuatIsNorm(q1) ||
01284       ! pmQuatIsNorm(q2))
01285   {
01286 #ifdef PM_PRINT_ERROR
01287     pmPrintError("Bad quaternion in pmQuatQuatCompare\n");
01288 #endif
01289   }
01290 #endif
01291 
01292   if (fabs(q1.s-q2.s) < Q_FUZZ &&
01293       fabs(q1.x-q2.x) < Q_FUZZ &&
01294       fabs(q1.y-q2.y) < Q_FUZZ &&
01295       fabs(q1.z-q2.z) < Q_FUZZ)
01296   {
01297     return 1;
01298   }
01299 
01300   /* note (0, x, y, z) = (0, -x, -y, -z) */
01301   if (fabs(q1.s) >= QS_FUZZ ||
01302       fabs(q1.x + q2.x) >= Q_FUZZ ||
01303       fabs(q1.y + q2.y) >= Q_FUZZ ||
01304       fabs(q1.z + q2.z) >= Q_FUZZ)
01305   {
01306     return 0;
01307   }
01308 
01309   return 1;
01310 }

int pmQuatMag (  PmQuaternion    q,
double *    d
) 
 

Definition at line 1312 of file _posemath.c.

Referenced by pmLineInit(). 

01313 {
01314   PmRotationVector r;
01315   int r1;
01316 
01317   if(0 == d)
01318     {
01319       return (pmErrno = PM_ERR);
01320     }
01321 
01322   r1 = pmQuatRotConvert(q, &r);
01323   *d = r.s;
01324 
01325   return pmErrno = r1;
01326 }

int pmQuatNorm (  PmQuaternion    q1,
PmQuaternion *    qout
) 
 

Definition at line 1328 of file _posemath.c.

Referenced by norm(), operator/(), pmMatQuatConvert(), and test_math_printf(). 

01329 {
01330   double size = pmSqrt(pmSq(q1.s) + pmSq(q1.x) + pmSq(q1.y) + pmSq(q1.z));
01331 
01332   if (size == 0.0)
01333   {
01334 #ifdef PM_PRINT_ERROR
01335     pmPrintError("Bad quaternion in pmQuatNorm\n");
01336 #endif
01337     qout->s = 1;
01338     qout->x = 0;
01339     qout->y = 0;
01340     qout->z = 0;
01341 
01342     return pmErrno = PM_NORM_ERR;
01343   }
01344 
01345   if (q1.s >= 0.0)
01346   {
01347     qout->s = q1.s / size;
01348     qout->x = q1.x / size;
01349     qout->y = q1.y / size;
01350     qout->z = q1.z / size;
01351 
01352     return pmErrno = 0;
01353   }
01354   else
01355   {
01356     qout->s = -q1.s / size;
01357     qout->x = -q1.x / size;
01358     qout->y = -q1.y / size;
01359     qout->z = -q1.z / size;
01360 
01361     return pmErrno = 0;
01362   }
01363 }

int pmQuatInv (  PmQuaternion    q1,
PmQuaternion *    qout
) 
 

Definition at line 1365 of file _posemath.c.

Referenced by inv(), operator-(), pmLineInit(), and pmPoseInv(). 

01366 {
01367   if(qout == 0)
01368     {
01369       return pmErrno = PM_ERR;
01370     }
01371 
01372   qout->s = q1.s;
01373   qout->x = - q1.x;
01374   qout->y = - q1.y;
01375   qout->z = - q1.z;
01376 
01377 #ifdef PM_DEBUG
01378   if (! pmQuatIsNorm(q1))
01379   {
01380 #ifdef PM_PRINT_ERROR
01381     pmPrintError("Bad quaternion in pmQuatInv\n");
01382 #endif
01383     return pmErrno = PM_NORM_ERR;
01384   }
01385 #endif
01386 
01387   return pmErrno = 0;
01388 }

int pmQuatIsNorm (  PmQuaternion    q1 ) 
 

Definition at line 1390 of file _posemath.c.

Referenced by isNorm(), pmPoseCartMult(), pmPoseInv(), pmPosePoseCompare(), pmPosePoseMult(), pmQuatAxisAngleMult(), pmQuatCartMult(), pmQuatInv(), pmQuatMatConvert(), pmQuatQuatCompare(), pmQuatQuatMult(), and pmQuatRotConvert(). 

01391 {
01392   return (fabs(pmSq(q1.s) + pmSq(q1.x) + pmSq(q1.y) + pmSq(q1.z) - 1.0) <
01393           UNIT_QUAT_FUZZ);
01394 }

int pmQuatScalMult (  PmQuaternion    q,
double    s,
PmQuaternion *    qout
) 
 

Definition at line 1396 of file _posemath.c.

Referenced by operator *(), operator/(), pmLineInit(), and pmLinePoint(). 

01397 {
01398   /* FIXME-- need a native version; this goes through a rotation vector */
01399   PmRotationVector r;
01400   int r1, r2, r3;
01401 
01402   r1 = pmQuatRotConvert(q, &r);
01403   r2 = pmRotScalMult(r, s, &r);
01404   r3 = pmRotQuatConvert(r, qout);
01405 
01406   return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
01407 }

int pmQuatScalDiv (  PmQuaternion    q,
double    s,
PmQuaternion *    qout
) 
 

Definition at line 1409 of file _posemath.c.

01410 {
01411   /* FIXME-- need a native version; this goes through a rotation vector */
01412   PmRotationVector r;
01413   int r1, r2, r3;
01414 
01415   r1 = pmQuatRotConvert(q, &r);
01416   r2 = pmRotScalDiv(r, s, &r);
01417   r3 = pmRotQuatConvert(r, qout);
01418 
01419   return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
01420 }

int pmQuatQuatMult (  PmQuaternion    q1,
PmQuaternion    q2,
PmQuaternion *    qout
) 
 

Definition at line 1422 of file _posemath.c.

Referenced by operator *(), pmLineInit(), pmLinePoint(), and pmPosePoseMult(). 

01423 {
01424   if(qout == 0)
01425     {
01426       return pmErrno = PM_ERR;
01427     }
01428 
01429   qout->s = q1.s * q2.s - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
01430 
01431   if (qout->s >= 0.0)
01432   {
01433     qout->x = q1.s * q2.x + q1.x * q2.s + q1.y * q2.z - q1.z * q2.y;
01434     qout->y = q1.s * q2.y - q1.x * q2.z + q1.y * q2.s + q1.z * q2.x;
01435     qout->z = q1.s * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.s;
01436   }
01437   else
01438   {
01439     qout->s *= -1;
01440     qout->x = - q1.s * q2.x - q1.x * q2.s - q1.y * q2.z + q1.z * q2.y;
01441     qout->y = - q1.s * q2.y + q1.x * q2.z - q1.y * q2.s - q1.z * q2.x;
01442     qout->z = - q1.s * q2.z - q1.x * q2.y + q1.y * q2.x - q1.z * q2.s;
01443   }
01444 
01445 #ifdef PM_DEBUG
01446   if (! pmQuatIsNorm(q1) ||
01447       ! pmQuatIsNorm(q2))
01448   {
01449 #ifdef PM_PRINT_ERROR
01450     pmPrintError("Bad quaternion in pmQuatQuatMult\n");
01451 #endif
01452     return pmErrno = PM_NORM_ERR;
01453   }
01454 #endif
01455 
01456   return pmErrno = 0;
01457 }

int pmQuatCartMult (  PmQuaternion    q1,
PmCartesian    v2,
PmCartesian *    vout
) 
 

Definition at line 1459 of file _posemath.c.

Referenced by operator *(), pmPoseCartMult(), pmPoseInv(), and pmPosePoseMult(). 

01460 {
01461   PmCartesian c;
01462 
01463   c.x = q1.y * v2.z  -  q1.z * v2.y;
01464   c.y = q1.z * v2.x  -  q1.x * v2.z;
01465   c.z = q1.x * v2.y  -  q1.y * v2.x;
01466 
01467   vout->x = v2.x + 2.0 * (q1.s * c.x + q1.y * c.z - q1.z * c.y);
01468   vout->y = v2.y + 2.0 * (q1.s * c.y + q1.z * c.x - q1.x * c.z);
01469   vout->z = v2.z + 2.0 * (q1.s * c.z + q1.x * c.y - q1.y * c.x);
01470 
01471 #ifdef PM_DEBUG
01472   if (! pmQuatIsNorm(q1))
01473   {
01474 #ifdef PM_PRINT_ERROR
01475     pmPrintError("Bad quaternion in pmQuatCartMult\n");
01476 #endif
01477     return pmErrno = PM_NORM_ERR;
01478   }
01479 #endif
01480 
01481   return pmErrno = 0;
01482 }

int pmQuatAxisAngleMult (  PmQuaternion    q,
PmAxis    axis,
double    angle,
PmQuaternion *    pq
) 
 

Definition at line 1063 of file _posemath.c.

01064 {
01065   double sh, ch;
01066 
01067 #ifdef PM_DEBUG
01068   if (! pmQuatIsNorm(q))
01069     {
01070 #ifdef PM_PRINT_ERROR
01071       pmPrintError("error: non-unit quaternion in pmQuatAxisAngleMult\n");
01072 #endif
01073       return -1;
01074     }
01075 #endif
01076 
01077   angle *= 0.5;
01078   sincos(angle, &sh, &ch);
01079 
01080   switch (axis)
01081     {
01082     case PM_X:
01083       pq->s = ch * q.s - sh * q.x;
01084       pq->x = ch * q.x + sh * q.s;
01085       pq->y = ch * q.y + sh * q.z;
01086       pq->z = ch * q.z - sh * q.y;
01087       break;
01088 
01089     case PM_Y:
01090       pq->s = ch * q.s - sh * q.y;
01091       pq->x = ch * q.x - sh * q.z;
01092       pq->y = ch * q.y + sh * q.s;
01093       pq->z = ch * q.z + sh * q.x;
01094       break;
01095 
01096     case PM_Z:
01097       pq->s = ch * q.s - sh * q.z;
01098       pq->x = ch * q.x + sh * q.y;
01099       pq->y = ch * q.y - sh * q.x;
01100       pq->z = ch * q.z + sh * q.s;
01101       break;
01102 
01103     default:
01104 #ifdef PM_PRINT_ERROR
01105       pmPrintError("error: bad axis in pmQuatAxisAngleMult\n");
01106 #endif
01107       return -1;
01108       break;
01109     }
01110 
01111   if (pq->s < 0.0)
01112     {
01113       pq->s *= -1.0;
01114       pq->x *= -1.0;
01115       pq->y *= -1.0;
01116       pq->z *= -1.0;
01117     }
01118 
01119   return 0;
01120 }

int pmRotScalMult (  PmRotationVector    r,
double    s,
PmRotationVector *    rout
) 
 

Definition at line 1124 of file _posemath.c.

Referenced by fullcircle_test(), and pmQuatScalMult(). 

01125 {
01126   rout->s = r.s * s;
01127   rout->x = r.x;
01128   rout->y = r.y;
01129   rout->z = r.z;
01130 
01131   return pmErrno = 0;
01132 }

int pmRotScalDiv (  PmRotationVector    r,
double    s,
PmRotationVector *    rout
) 
 

Definition at line 1134 of file _posemath.c.

Referenced by pmQuatScalDiv(). 

01135 {
01136   if (s == 0.0)
01137   {
01138 #ifdef PM_PRINT_ERROR
01139     pmPrintError("Divide by zero in pmRotScalDiv\n");
01140 #endif
01141 
01142     rout->s = DBL_MAX;
01143     rout->x = r.x;
01144     rout->y = r.y;
01145     rout->z = r.z;
01146 
01147     return pmErrno = PM_NORM_ERR;
01148   }
01149 
01150   rout->s = r.s / s;
01151   rout->x = r.x;
01152   rout->y = r.y;
01153   rout->z = r.z;
01154 
01155   return pmErrno = 0;
01156 }

int pmRotIsNorm (  PmRotationVector    r ) 
 

Definition at line 1158 of file _posemath.c.

Referenced by isNorm(), and pmRotMatConvert(). 

01159 {
01160   if (fabs(r.s) < RS_FUZZ ||
01161       fabs(pmSqrt(pmSq(r.x) + pmSq(r.y) + pmSq(r.z))) - 1.0 < UNIT_VEC_FUZZ)
01162   {
01163     return 1;
01164   }
01165 
01166   return 0;
01167 }

int pmRotNorm (  PmRotationVector    r,
PmRotationVector *    rout
) 
 

Definition at line 1169 of file _posemath.c.

Referenced by norm(), and pmRotQuatConvert(). 

01170 {
01171   double size;
01172 
01173   size = pmSqrt(pmSq(r.x) + pmSq(r.y) + pmSq(r.z));
01174 
01175   if (fabs(r.s) < RS_FUZZ)
01176   {
01177     rout->s = 0.0;
01178     rout->x = 0.0;
01179     rout->y = 0.0;
01180     rout->z = 0.0;
01181 
01182     return pmErrno = 0;
01183   }
01184 
01185   if (size == 0.0)
01186   {
01187 #ifdef PM_PRINT_ERROR
01188     pmPrintError("error: pmRotNorm size is zero\n");
01189 #endif
01190 
01191     rout->s = 0.0;
01192     rout->x = 0.0;
01193     rout->y = 0.0;
01194     rout->z = 0.0;
01195 
01196     return pmErrno = PM_NORM_ERR;
01197   }
01198 
01199   rout->s = r.s;
01200   rout->x = r.x / size;
01201   rout->y = r.y / size;
01202   rout->z = r.z / size;
01203 
01204   return pmErrno = 0;
01205 }

int pmMatNorm (  PmRotationMatrix    m,
PmRotationMatrix *    mout
) 
 

Definition at line 1209 of file _posemath.c.

Referenced by norm(). 

01210 {
01211   /* FIXME */
01212   *mout = m;
01213 
01214 #ifdef PM_PRINT_ERROR
01215   pmPrintError("error: pmMatNorm not implemented\n");
01216 #endif
01217   return pmErrno = PM_IMPL_ERR;
01218 }

int pmMatIsNorm (  PmRotationMatrix    m ) 
 

Definition at line 1220 of file _posemath.c.

Referenced by isNorm(), pmHomInv(), and pmMatQuatConvert(). 

01221 {
01222   PmCartesian u;
01223 
01224   pmCartCartCross(m.x, m.y, &u);
01225 
01226   return (pmCartIsNorm(m.x) &&
01227           pmCartIsNorm(m.y) &&
01228           pmCartIsNorm(m.z) &&
01229           pmCartCartCompare(u, m.z));
01230 }

int pmMatInv (  PmRotationMatrix    m,
PmRotationMatrix *    mout
) 
 

Definition at line 1232 of file _posemath.c.

Referenced by inv(), and pmHomInv(). 

01233 {
01234   /* inverse of a rotation matrix is the transpose */
01235 
01236   mout->x.x = m.x.x;
01237   mout->x.y = m.y.x;
01238   mout->x.z = m.z.x;
01239 
01240   mout->y.x = m.x.y;
01241   mout->y.y = m.y.y;
01242   mout->y.z = m.z.y;
01243 
01244   mout->z.x = m.x.z;
01245   mout->z.y = m.y.z;
01246   mout->z.z = m.z.z;
01247 
01248   return pmErrno = 0;
01249 }

int pmMatCartMult (  PmRotationMatrix    m,
PmCartesian    v,
PmCartesian *    vout
) 
 

Definition at line 1251 of file _posemath.c.

Referenced by pmHomInv(). 

01252 {
01253   vout->x = m.x.x * v.x + m.y.x * v.y + m.z.x * v.z;
01254   vout->y = m.x.y * v.x + m.y.y * v.y + m.z.y * v.z;
01255   vout->z = m.x.z * v.x + m.y.z * v.y + m.z.z * v.z;
01256 
01257   return pmErrno = 0;
01258 }

int pmMatMatMult (  PmRotationMatrix    m1,
PmRotationMatrix    m2,
PmRotationMatrix *    mout
) 
 

Definition at line 1260 of file _posemath.c.

Referenced by operator *(). 

01262 {
01263   mout->x.x = m1.x.x * m2.x.x + m1.y.x * m2.x.y + m1.z.x * m2.x.z;
01264   mout->x.y = m1.x.y * m2.x.x + m1.y.y * m2.x.y + m1.z.y * m2.x.z;
01265   mout->x.z = m1.x.z * m2.x.x + m1.y.z * m2.x.y + m1.z.z * m2.x.z;
01266 
01267   mout->y.x = m1.x.x * m2.y.x + m1.y.x * m2.y.y + m1.z.x * m2.y.z;
01268   mout->y.y = m1.x.y * m2.y.x + m1.y.y * m2.y.y + m1.z.y * m2.y.z;
01269   mout->y.z = m1.x.z * m2.y.x + m1.y.z * m2.y.y + m1.z.z * m2.y.z;
01270 
01271   mout->z.x = m1.x.x * m2.z.x + m1.y.x * m2.z.y + m1.z.x * m2.z.z;
01272   mout->z.y = m1.x.y * m2.z.x + m1.y.y * m2.z.y + m1.z.y * m2.z.z;
01273   mout->z.z = m1.x.z * m2.z.x + m1.y.z * m2.z.y + m1.z.z * m2.z.z;
01274 
01275   return pmErrno = 0;
01276 }

int pmPosePoseCompare (  PmPose    p1,
PmPose    p2
) 
 

Definition at line 1486 of file _posemath.c.

Referenced by operator!=(), and operator==(). 

01487 {
01488 #ifdef PM_DEBUG
01489   if (! pmQuatIsNorm(p1.rot) ||
01490       ! pmQuatIsNorm(p2.rot))
01491   {
01492 #ifdef PM_PRINT_ERROR
01493     pmPrintError("Bad quaternion in pmPosePoseCompare\n");
01494 #endif
01495   }
01496 #endif
01497 
01498   return pmErrno = (pmQuatQuatCompare(p1.rot, p2.rot) &&
01499           pmCartCartCompare(p1.tran, p2.tran));
01500 }

int pmPoseInv (  PmPose    p,
PmPose *    p2
) 
 

Definition at line 1502 of file _posemath.c.

Referenced by inv(), and operator-(). 

01503 {
01504   int r1, r2;
01505 
01506 #ifdef PM_DEBUG
01507   if (! pmQuatIsNorm(p1.rot))
01508   {
01509 #ifdef PM_PRINT_ERROR
01510     pmPrintError("Bad quaternion in pmPoseInv\n");
01511 #endif
01512   }
01513 #endif
01514 
01515   r1 = pmQuatInv(p1.rot, &p2->rot);
01516   r2 = pmQuatCartMult(p2->rot, p1.tran, &p2->tran);
01517 
01518   p2->tran.x *= -1.0;
01519   p2->tran.y *= -1.0;
01520   p2->tran.z *= -1.0;
01521 
01522   return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
01523 }

int pmPoseCartMult (  PmPose    p1,
PmCartesian    v2,
PmCartesian *    vout
) 
 

Definition at line 1525 of file _posemath.c.

Referenced by operator *(). 

01526 {
01527   int r1, r2;
01528 
01529 #ifdef PM_DEBUG
01530   if (! pmQuatIsNorm(p1.rot))
01531   {
01532 #ifdef PM_PRINT_ERROR
01533     pmPrintError("Bad quaternion in pmPoseCartMult\n");
01534 #endif
01535     return pmErrno = PM_NORM_ERR;
01536   }
01537 #endif
01538 
01539   r1 = pmQuatCartMult(p1.rot, v2, vout);
01540   r2 = pmCartCartAdd(p1.tran, *vout, vout);
01541 
01542   return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
01543 }

int pmPosePoseMult (  PmPose    p1,
PmPose    p2,
PmPose *    pout
) 
 

Definition at line 1545 of file _posemath.c.

Referenced by operator *(). 

01546 {
01547   int r1, r2, r3;
01548 
01549 #ifdef PM_DEBUG
01550   if (! pmQuatIsNorm(p1.rot) ||
01551       ! pmQuatIsNorm(p2.rot))
01552   {
01553 #ifdef PM_PRINT_ERROR
01554     pmPrintError("Bad quaternion in pmPosePoseMult\n");
01555 #endif
01556     return pmErrno = PM_NORM_ERR;
01557   }
01558 #endif
01559 
01560   r1 = pmQuatCartMult(p1.rot, p2.tran, &pout->tran);
01561   r2 = pmCartCartAdd(p1.tran, pout->tran, &pout->tran);
01562   r3 = pmQuatQuatMult(p1.rot, p2.rot, &pout->rot);
01563 
01564   return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
01565 }

int pmHomInv (  PmHomogeneous    h1,
PmHomogeneous *    h2
) 
 

Definition at line 1569 of file _posemath.c.

Referenced by inv(). 

01570 {
01571   int r1, r2;
01572 
01573 #ifdef PM_DEBUG
01574   if (! pmMatIsNorm(h1.rot))
01575   {
01576 #ifdef PM_PRINT_ERROR
01577     pmPrintError("Bad rotation matrix in pmHomInv\n");
01578 #endif
01579   }
01580 #endif
01581 
01582   r1 = pmMatInv(h1.rot, &h2->rot);
01583   r2 = pmMatCartMult(h2->rot, h1.tran, &h2->tran);
01584 
01585   h2->tran.x *= -1.0;
01586   h2->tran.y *= -1.0;
01587   h2->tran.z *= -1.0;
01588 
01589   return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
01590 }

int pmLineInit (  PmLine *    line,
PmPose    start,
PmPose    end
) 
 

Definition at line 1594 of file _posemath.c.

01595 {
01596   int r1 = 0, r2 = 0, r3 = 0, r4 = 0, r5 = 0;
01597   double tmag = 0.0;
01598   double rmag = 0.0;
01599   PmQuaternion startQuatInverse;
01600 
01601   if(0 == line)
01602     {
01603       return (pmErrno = PM_ERR);
01604     }
01605 
01606   r3 = pmQuatInv(start.rot,&startQuatInverse);
01607   if(r3) 
01608     {
01609       return r3;
01610     }
01611 
01612   r4 = pmQuatQuatMult(startQuatInverse,end.rot,&line->qVec);
01613   if(r4)
01614     {
01615       return r4;
01616     }
01617 
01618   pmQuatMag(line->qVec,&rmag);
01619   if (rmag > Q_FUZZ) 
01620     {
01621       r5 = pmQuatScalMult(line->qVec,1/rmag,&(line->qVec));
01622       if(r5)
01623         {
01624           return r5;
01625         }
01626     }
01627 
01628   line->start = start;
01629   line->end = end;
01630   r1 = pmCartCartSub(end.tran, start.tran, &line->uVec);
01631   if(r1)
01632     {
01633       return r1;
01634     }
01635 
01636   pmCartMag(line->uVec, &tmag);
01637   if (IS_FUZZ(tmag, CART_FUZZ))
01638     {
01639       line->uVec.x = 1.0;
01640       line->uVec.y = 0.0;
01641       line->uVec.z = 0.0;
01642     }
01643   else
01644     {
01645       r2 = pmCartUnit(line->uVec, &line->uVec);
01646     }
01647   line->tmag = tmag;
01648   line->rmag = rmag;
01649   line->tmag_zero = (line->tmag <= CART_FUZZ);
01650   line->rmag_zero = (line->rmag <= Q_FUZZ);
01651 
01652   /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
01653   return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : 0;
01654 }

int pmLinePoint (  PmLine *    line,
double    len,
PmPose *    point
) 
 

Definition at line 1656 of file _posemath.c.

01657 {
01658   int r1=0, r2 =0,r3 = 0,r4=0;
01659 
01660   if(line->tmag_zero)
01661     {
01662       point->tran = line->end.tran;
01663     }      
01664   else
01665     {
01666       /* return start + len * uVec */
01667       r1 = pmCartScalMult(line->uVec, len, &point->tran);
01668       r2 = pmCartCartAdd(line->start.tran, point->tran, &point->tran);
01669     }
01670 
01671   if(line->rmag_zero)
01672     {
01673       point->rot = line->end.rot;
01674     }
01675   else
01676     {
01677       if(line->tmag_zero)
01678         {
01679           r3 = pmQuatScalMult(line->qVec, len, &point->rot);
01680         }
01681       else
01682         {
01683           r3 = pmQuatScalMult(line->qVec, len*line->rmag/line->tmag, &point->rot);
01684         }
01685       r4 = pmQuatQuatMult(line->start.rot,point->rot,&point->rot);
01686     }
01687 
01688   return pmErrno = (r1 || r2 || r3 || r4) ? PM_NORM_ERR : 0;
01689 }

int pmCircleInit (  PmCircle *    circle,
PmPose    start,
PmPose    end,
PmCartesian    center,
PmCartesian    normal,
int    turn
) 
 

Referenced by main().

int pmCirclePoint (  PmCircle *    circle,
double    angle,
PmPose *    point
) 
 

Referenced by main().

Variable Documentation

int pmErrno
 

Definition at line 791 of file posemath.h.

Generated on Sun Dec 2 15:57:30 2001 for rcslib by doxygen1.2.11.1 written by Dimitri van Heesch, © 1997-2001