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

_posemath.c File Reference

#include <math.h>
#include <float.h>
#include "posemath.h"
#include "sincos.h"

Include dependency graph for _posemath.c:

Include dependency graph

Go to the source code of this file.

Defines

#define IS_FUZZ(a, fuzz)   (fabs(a) < (fuzz) ? 1 : 0)

Functions

double pmSqrt (double x)
int pmCartSphConvert (PmCartesian v, PmSpherical *s)
int pmCartCylConvert (PmCartesian v, PmCylindrical *c)
int pmSphCartConvert (PmSpherical s, PmCartesian *v)
int pmSphCylConvert (PmSpherical s, PmCylindrical *c)
int pmCylCartConvert (PmCylindrical c, PmCartesian *v)
int pmCylSphConvert (PmCylindrical c, PmSpherical *s)
int pmAxisAngleQuatConvert (PmAxis axis, double a, PmQuaternion *q)
int pmRotQuatConvert (PmRotationVector r, PmQuaternion *q)
int pmRotMatConvert (PmRotationVector r, PmRotationMatrix *m)
int pmRotZyzConvert (PmRotationVector r, PmEulerZyz *zyz)
int pmRotZyxConvert (PmRotationVector r, PmEulerZyx *zyx)
int pmRotRpyConvert (PmRotationVector r, PmRpy *rpy)
int pmQuatRotConvert (PmQuaternion q, PmRotationVector *r)
int pmQuatMatConvert (PmQuaternion q, PmRotationMatrix *m)
int pmQuatZyzConvert (PmQuaternion q, PmEulerZyz *zyz)
int pmQuatZyxConvert (PmQuaternion q, PmEulerZyx *zyx)
int pmQuatRpyConvert (PmQuaternion q, PmRpy *rpy)
int pmMatRotConvert (PmRotationMatrix m, PmRotationVector *r)
int pmMatQuatConvert (PmRotationMatrix m, PmQuaternion *q)
int pmMatZyzConvert (PmRotationMatrix m, PmEulerZyz *zyz)
int pmMatZyxConvert (PmRotationMatrix m, PmEulerZyx *zyx)
int pmMatRpyConvert (PmRotationMatrix m, PmRpy *rpy)
int pmZyzRotConvert (PmEulerZyz zyz, PmRotationVector *r)
int pmZyzQuatConvert (PmEulerZyz zyz, PmQuaternion *q)
int pmZyzMatConvert (PmEulerZyz zyz, PmRotationMatrix *m)
int pmZyzRpyConvert (PmEulerZyz zyz, PmRpy *rpy)
int pmZyxRotConvert (PmEulerZyx zyx, PmRotationVector *r)
int pmZyxQuatConvert (PmEulerZyx zyx, PmQuaternion *q)
int pmZyxMatConvert (PmEulerZyx zyx, PmRotationMatrix *m)
int pmZyxZyzConvert (PmEulerZyx zyx, PmEulerZyz *zyz)
int pmZyxRpyConvert (PmEulerZyx zyx, PmRpy *rpy)
int pmRpyRotConvert (PmRpy rpy, PmRotationVector *r)
int pmRpyQuatConvert (PmRpy rpy, PmQuaternion *q)
int pmRpyMatConvert (PmRpy rpy, PmRotationMatrix *m)
int pmRpyZyzConvert (PmRpy rpy, PmEulerZyz *zyz)
int pmRpyZyxConvert (PmRpy rpy, PmEulerZyx *zyx)
int pmPoseHomConvert (PmPose p, PmHomogeneous *h)
int pmHomPoseConvert (PmHomogeneous h, PmPose *p)
int pmCartCartCompare (PmCartesian v1, PmCartesian v2)
int pmCartCartDot (PmCartesian v1, PmCartesian v2, double *d)
int pmCartCartCross (PmCartesian v1, PmCartesian v2, PmCartesian *vout)
int pmCartMag (PmCartesian v, double *d)
int pmCartCartDisp (PmCartesian v1, PmCartesian v2, double *d)
int pmCartCartAdd (PmCartesian v1, PmCartesian v2, PmCartesian *vout)
int pmCartCartSub (PmCartesian v1, PmCartesian v2, PmCartesian *vout)
int pmCartScalMult (PmCartesian v1, double d, PmCartesian *vout)
int pmCartScalDiv (PmCartesian v1, double d, PmCartesian *vout)
int pmCartNeg (PmCartesian v1, PmCartesian *vout)
int pmCartInv (PmCartesian v1, PmCartesian *vout)
int pmCartUnit (PmCartesian v, PmCartesian *vout)
int pmCartIsNorm (PmCartesian v)
int pmCartCartProj (PmCartesian v1, PmCartesian v2, PmCartesian *vout)
int pmCartPlaneProj (PmCartesian v, PmCartesian normal, PmCartesian *vout)
int pmQuatAxisAngleMult (PmQuaternion q, PmAxis axis, double angle, PmQuaternion *pq)
int pmRotScalMult (PmRotationVector r, double s, PmRotationVector *rout)
int pmRotScalDiv (PmRotationVector r, double s, PmRotationVector *rout)
int pmRotIsNorm (PmRotationVector r)
int pmRotNorm (PmRotationVector r, PmRotationVector *rout)
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 pmQuatQuatCompare (PmQuaternion q1, PmQuaternion q2)
int pmQuatMag (PmQuaternion q, double *d)
int pmQuatNorm (PmQuaternion q1, PmQuaternion *qout)
int pmQuatInv (PmQuaternion q1, PmQuaternion *qout)
int pmQuatIsNorm (PmQuaternion q1)
int pmQuatScalMult (PmQuaternion q, double s, PmQuaternion *qout)
int pmQuatScalDiv (PmQuaternion q, double s, PmQuaternion *qout)
int pmQuatQuatMult (PmQuaternion q1, PmQuaternion q2, PmQuaternion *qout)
int pmQuatCartMult (PmQuaternion q1, PmCartesian v2, PmCartesian *vout)
int pmPosePoseCompare (PmPose p1, PmPose p2)
int pmPoseInv (PmPose p1, PmPose *p2)
int pmPoseCartMult (PmPose p1, PmCartesian v2, PmCartesian *vout)
int pmPosePoseMult (PmPose p1, PmPose p2, PmPose *pout)
int pmHomInv (PmHomogeneous h1, PmHomogeneous *h2)
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 = 0

Define Documentation

#define IS_FUZZ (  a,
fuzz    )     (fabs(a) < (fuzz) ? 1 : 0)
 

Definition at line 119 of file _posemath.c.

Referenced by pmLineInit().

Function Documentation

double pmSqrt (  double    x ) 
 

Definition at line 125 of file _posemath.c.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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 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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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 pmCartInv (  PmCartesian    v1,
PmCartesian *    vout
) 
 

Definition at line 964 of file _posemath.c.

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 pmCartUnit (  PmCartesian    v,
PmCartesian *    vout
) 
 

Definition at line 990 of file _posemath.c.

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.

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

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

Definition at line 1037 of file _posemath.c.

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.

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 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.

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.

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.

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.

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.

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.

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.

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.

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.

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 pmQuatQuatCompare (  PmQuaternion    q1,
PmQuaternion    q2
) 
 

Definition at line 1280 of file _posemath.c.

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.

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.

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.

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.

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.

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.

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.

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 pmPosePoseCompare (  PmPose    p1,
PmPose    p2
) 
 

Definition at line 1486 of file _posemath.c.

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.

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.

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.

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.

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
) 
 

Definition at line 1705 of file _posemath.c.

01709 {
01710   double dot;
01711   PmCartesian rEnd;
01712   PmCartesian v;
01713   double d;
01714   int r1;
01715 
01716 #ifdef PM_DEBUG
01717   if (0 == circle)
01718     {
01719 #ifdef PM_PRINT_ERROR
01720       pmPrintError("error: pmCircleInit cirle pointer is null\n");
01721 #endif
01722       return pmErrno = PM_ERR;
01723     }
01724 #endif
01725 
01726   /* adjust center */
01727   pmCartCartSub(start.tran, center, &v);
01728   r1 = pmCartCartProj(v, normal, &v);
01729   if (PM_NORM_ERR == r1)
01730     {
01731       /* bad normal vector-- abort */
01732 #ifdef PM_PRINT_ERROR
01733       pmPrintError("error: pmCircleInit normal vector is 0\n");
01734 #endif
01735       return -1;
01736     }
01737   pmCartCartAdd(v, center, &circle->center);
01738 
01739   /* normalize and redirect normal vector based on turns. If turn is
01740      less than 0, point normal vector in other direction and make
01741      turn positive, -1 -> 0, -2 -> 1, etc. */
01742   pmCartUnit(normal, &circle->normal);
01743   if (turn < 0)
01744     {
01745       turn = -1 - turn;
01746       pmCartScalMult(circle->normal, -1.0, &circle->normal);
01747     }
01748 
01749   /* radius */
01750   pmCartCartDisp(start.tran, circle->center, &circle->radius);
01751 
01752   /* vector in plane of circle from center to start, magnitude radius */
01753   pmCartCartSub(start.tran, circle->center, &circle->rTan);
01754   /* vector in plane of circle perpendicular to rTan, magnitude radius */
01755   pmCartCartCross(circle->normal, circle->rTan, &circle->rPerp);
01756 
01757   /* do rHelix, rEnd */
01758   pmCartCartSub(end.tran, circle->center, &circle->rHelix);
01759   pmCartPlaneProj(circle->rHelix, circle->normal, &rEnd);
01760   pmCartMag(rEnd, &circle->spiral);
01761   circle->spiral -= circle->radius;
01762   pmCartCartSub(circle->rHelix, rEnd, &circle->rHelix);
01763   pmCartUnit(rEnd, &rEnd);
01764   pmCartScalMult(rEnd, circle->radius, &rEnd);
01765 
01766  /* Patch for error spiral end same as spiral center */
01767   pmCartMag(rEnd, &d);
01768   if(d == 0.0)
01769     {
01770       pmCartScalMult(circle->normal, DOUBLE_FUZZ, &v);
01771       pmCartCartAdd(rEnd, v, &rEnd);
01772     }
01773   /* end patch 03-mar-1999 Dirk Maij*/
01774   
01775 
01776   /* angle */
01777   pmCartCartDot(circle->rTan, rEnd, &dot);
01778   dot = dot / (circle->radius * circle->radius);
01779   if (dot > 1.0)
01780     {
01781       circle->angle = 0.0;
01782     }
01783   else if (dot < -1.0)
01784     {
01785       circle->angle = PM_PI;
01786     }
01787   else
01788     {
01789       circle->angle = acos(dot);
01790     }
01791   /* now angle is in range 0..PI . Check if cross is antiparallel to
01792      normal. If so, true angle is between PI..2PI. Need to subtract
01793      from 2PI. */
01794   pmCartCartCross(circle->rTan, rEnd, &v);
01795   pmCartCartDot(v, circle->normal, &d);
01796   if (d < 0.0)
01797     {
01798       circle->angle = PM_2_PI - circle->angle;
01799     }
01800 
01801   if (circle->angle == 0.0)
01802   {
01803      circle->angle = PM_2_PI; 
01804   }
01805 
01806   /* now add more angle for multi turns */
01807   if (turn > 0)
01808     {
01809       circle->angle += turn * 2.0 * PM_PI;
01810     }
01811 
01812   return pmErrno = 0;
01813 }

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

Definition at line 1820 of file _posemath.c.

01821 {
01822   PmCartesian par, perp;
01823   double scale;
01824 
01825 #ifdef PM_DEBUG
01826   if (0 == circle ||
01827       0 == point)
01828     {
01829 #ifdef PM_PRINT_ERROR
01830       pmPrintError("error: pmCirclePoint circle or point pointer is null\n");
01831 #endif
01832       return pmErrno = PM_ERR;
01833     }
01834 #endif
01835 
01836   /* compute components rel to center */
01837   pmCartScalMult(circle->rTan, cos(angle), &par);
01838   pmCartScalMult(circle->rPerp, sin(angle), &perp);
01839 
01840   /* add to get radius vector rel to center */
01841   pmCartCartAdd(par, perp, &point->tran);
01842 
01843   /* get scale for spiral, helix interpolation */
01844   if (circle->angle == 0.0)
01845     {
01846 #ifdef PM_PRINT_ERROR
01847       pmPrintError("error: pmCirclePoint angle is zero\n");
01848 #endif
01849       return pmErrno = PM_DIV_ERR;
01850     }
01851   scale = angle / circle->angle;
01852 
01853   /* add scaled vector in radial dir for spiral */
01854   pmCartUnit(point->tran, &par);
01855   pmCartScalMult(par, scale * circle->spiral, &par);
01856   pmCartCartAdd(point->tran, par, &point->tran);
01857 
01858   /* add scaled vector in helix dir */
01859   pmCartScalMult(circle->rHelix, scale, &perp);
01860   pmCartCartAdd(point->tran, perp, &point->tran);
01861 
01862   /* add to center vector for final result */
01863   pmCartCartAdd(circle->center, point->tran, &point->tran);
01864 
01865   return pmErrno = 0;
01866 }

Variable Documentation

int pmErrno = 0
 

Definition at line 60 of file _posemath.c.

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