1  /********************************************************************
   2  * Description: _posemath.c
   3  *    C definitions for pose math library data types and manipulation
   4  *    functions.
   5  *
   6  *   Derived from a work by Fred Proctor & Will Shackleford
   7  *
   8  * Author:
   9  * License: LGPL Version 2
  10  * System: Linux
  11  *    
  12  * Copyright (c) 2004 All rights reserved.
  13  *
  14  * Last change: 
  15  ********************************************************************/
  16  
  17  #if defined(PM_PRINT_ERROR) && defined(rtai)
  18  #undef PM_PRINT_ERROR
  19  #endif
  20  
  21  #if defined(PM_DEBUG) && defined(rtai)
  22  #undef PM_DEBUG
  23  #endif
  24  
  25  #ifdef PM_PRINT_ERROR
  26  #define PM_DEBUG		/* have to have debug with printing */
  27  #include <stdio.h>
  28  #include <stdarg.h>
  29  #endif
  30  #include "posemath.h"
  31  
  32  #include "rtapi_math.h"
  33  #include <float.h>
  34  
  35  #include "sincos.h"
  36  
  37  /* global error number */
  38  int pmErrno = 0;
  39  
  40  #ifdef PM_PRINT_ERROR
  41  
  42  void pmPrintError(const char *fmt, ...)
  43  {
  44      va_list args;
  45  
  46      va_start(args, fmt);
  47      vfprintf(stderr, fmt, args);
  48      va_end(args);
  49  }
  50  
  51  /* error printing function */
  52  void pmPerror(const char *s)
  53  {
  54      char *pmErrnoString;
  55  
  56      switch (pmErrno) {
  57      case 0:
  58  	/* no error */
  59  	return;
  60  
  61      case PM_ERR:
  62  	pmErrnoString = "unspecified error";
  63  	break;
  64  
  65      case PM_IMPL_ERR:
  66  	pmErrnoString = "not implemented";
  67  	break;
  68  
  69      case PM_NORM_ERR:
  70  	pmErrnoString = "expected normalized value";
  71  	break;
  72  
  73      case PM_DIV_ERR:
  74  	pmErrnoString = "divide by zero";
  75  	break;
  76  
  77      default:
  78  	pmErrnoString = "unassigned error";
  79  	break;
  80      }
  81  
  82      if (s != 0 && s[0] != 0) {
  83  	fprintf(stderr, "%s: %s\n", s, pmErrnoString);
  84      } else {
  85  	fprintf(stderr, "%s\n", pmErrnoString);
  86      }
  87  }
  88  
  89  #endif /* PM_PRINT_ERROR */
  90  
  91  /* fuzz checker */
  92  #define IS_FUZZ(a,fuzz) (fabs(a) < (fuzz) ? 1 : 0)
  93  
  94  /* Pose Math Basis Functions */
  95  
  96  /* Scalar functions */
  97  
  98  double pmSqrt(double x)
  99  {
 100      if (x > 0.0) {
 101  	return sqrt(x);
 102      }
 103  
 104      if (x > SQRT_FUZZ) {
 105  	return 0.0;
 106      }
 107  #ifdef PM_PRINT_ERROR
 108      pmPrintError("sqrt of large negative number\n");
 109  #endif
 110  
 111      return 0.0;
 112  }
 113  
 114  /* Translation rep conversion functions */
 115  
 116  int pmCartSphConvert(PmCartesian const * const v, PmSpherical * const s)
 117  {
 118      double _r;
 119  
 120      s->theta = atan2(v->y, v->x);
 121      s->r = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z));
 122      _r = pmSqrt(pmSq(v->x) + pmSq(v->y));
 123      s->phi = atan2(_r, v->z);
 124  
 125      return pmErrno = 0;
 126  }
 127  
 128  int pmCartCylConvert(PmCartesian const * const v, PmCylindrical * const c)
 129  {
 130      c->theta = atan2(v->y, v->x);
 131      c->r = pmSqrt(pmSq(v->x) + pmSq(v->y));
 132      c->z = v->z;
 133  
 134      return pmErrno = 0;
 135  }
 136  
 137  int pmSphCartConvert(PmSpherical const * const s, PmCartesian * const v)
 138  {
 139      double _r;
 140  
 141      _r = s->r * sin(s->phi);
 142      v->z = s->r * cos(s->phi);
 143      v->x = _r * cos(s->theta);
 144      v->y = _r * sin(s->theta);
 145  
 146      return pmErrno = 0;
 147  }
 148  
 149  int pmSphCylConvert(PmSpherical const * const s, PmCylindrical * const c)
 150  {
 151      c->theta = s->theta;
 152      c->r = s->r * cos(s->phi);
 153      c->z = s->r * sin(s->phi);
 154      return pmErrno = 0;
 155  }
 156  
 157  int pmCylCartConvert(PmCylindrical const * const c, PmCartesian * const v)
 158  {
 159      v->x = c->r * cos(c->theta);
 160      v->y = c->r * sin(c->theta);
 161      v->z = c->z;
 162      return pmErrno = 0;
 163  }
 164  
 165  int pmCylSphConvert(PmCylindrical const * const c, PmSpherical * const s)
 166  {
 167      s->theta = c->theta;
 168      s->r = pmSqrt(pmSq(c->r) + pmSq(c->z));
 169      s->phi = atan2(c->z, c->r);
 170      return pmErrno = 0;
 171  }
 172  
 173  /* Rotation rep conversion functions */
 174  
 175  int pmAxisAngleQuatConvert(PmAxis axis, double a, PmQuaternion * const q)
 176  {
 177      double sh;
 178  
 179      a *= 0.5;
 180      sincos(a, &sh, &(q->s));
 181  
 182      switch (axis) {
 183      case PM_X:
 184  	q->x = sh;
 185  	q->y = 0.0;
 186  	q->z = 0.0;
 187  	break;
 188  
 189      case PM_Y:
 190  	q->x = 0.0;
 191  	q->y = sh;
 192  	q->z = 0.0;
 193  	break;
 194  
 195      case PM_Z:
 196  	q->x = 0.0;
 197  	q->y = 0.0;
 198  	q->z = sh;
 199  	break;
 200  
 201      default:
 202  #ifdef PM_PRINT_ERROR
 203  	pmPrintError("error: bad axis in pmAxisAngleQuatConvert\n");
 204  #endif
 205  	return -1;
 206      }
 207  
 208      if (q->s < 0.0) {
 209  	q->s *= -1.0;
 210  	q->x *= -1.0;
 211  	q->y *= -1.0;
 212  	q->z *= -1.0;
 213      }
 214  
 215      return 0;
 216  }
 217  
 218  int pmRotQuatConvert(PmRotationVector const * const r, PmQuaternion * const q)
 219  {
 220      double sh;
 221  
 222  #ifdef PM_DEBUG
 223      /* make sure r is normalized */
 224      //FIXME breaks const promise
 225      PmRotationVector r_raw = *r;
 226      if (0 != pmRotNorm(&r_raw, r)) {
 227  #ifdef PM_PRINT_ERROR
 228  	pmPrintError
 229  	    ("error: pmRotQuatConvert rotation vector not normalized\n");
 230  #endif
 231  	return pmErrno = PM_NORM_ERR;
 232      }
 233  #endif
 234  
 235      if (pmClose(r->s, 0.0, QS_FUZZ)) {
 236  	q->s = 1.0;
 237  	q->x = q->y = q->z = 0.0;
 238  
 239  	return pmErrno = 0;
 240      }
 241  
 242      sincos(r->s / 2.0, &sh, &(q->s));
 243  
 244      if (q->s >= 0.0) {
 245  	q->x = r->x * sh;
 246  	q->y = r->y * sh;
 247  	q->z = r->z * sh;
 248      } else {
 249  	q->s *= -1;
 250  	q->x = -r->x * sh;
 251  	q->y = -r->y * sh;
 252  	q->z = -r->z * sh;
 253      }
 254  
 255      return pmErrno = 0;
 256  }
 257  
 258  int pmRotMatConvert(PmRotationVector const * const r, PmRotationMatrix * const m)
 259  {
 260      double s, c, omc;
 261  
 262  #ifdef PM_DEBUG
 263      if (!pmRotIsNorm(r)) {
 264  #ifdef PM_PRINT_ERROR
 265  	pmPrintError("Bad vector in pmRotMatConvert\n");
 266  #endif
 267  	return pmErrno = PM_NORM_ERR;
 268      }
 269  #endif
 270  
 271      sincos(r->s, &s, &c);
 272  
 273      /* from space book */
 274      m->x.x = c + pmSq(r->x) * (omc = 1 - c);	/* omc = One Minus Cos */
 275      m->y.x = -r->z * s + r->x * r->y * omc;
 276      m->z.x = r->y * s + r->x * r->z * omc;
 277  
 278      m->x.y = r->z * s + r->y * r->x * omc;
 279      m->y.y = c + pmSq(r->y) * omc;
 280      m->z.y = -r->x * s + r->y * r->z * omc;
 281  
 282      m->x.z = -r->y * s + r->z * r->x * omc;
 283      m->y.z = r->x * s + r->z * r->y * omc;
 284      m->z.z = c + pmSq(r->z) * omc;
 285  
 286      return pmErrno = 0;
 287  }
 288  
 289  int pmRotZyzConvert(PmRotationVector const * const r, PmEulerZyz * const zyz)
 290  {
 291  #ifdef PM_DEBUG
 292  #ifdef PM_PRINT_ERROR
 293      pmPrintError("error: pmRotZyzConvert not implemented\n");
 294  #endif
 295      return pmErrno = PM_IMPL_ERR;
 296  #else
 297      return PM_IMPL_ERR;
 298  #endif
 299  }
 300  
 301  int pmRotZyxConvert(PmRotationVector const * const r, PmEulerZyx * const zyx)
 302  {
 303      PmRotationMatrix m;
 304      int r1, r2;
 305  
 306      r1 = pmRotMatConvert(r, &m);
 307      r2 = pmMatZyxConvert(&m, zyx);
 308  
 309      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 310  }
 311  
 312  int pmRotRpyConvert(PmRotationVector const * const r, PmRpy * const rpy)
 313  {
 314      PmQuaternion q;
 315      int r1, r2;
 316  
 317      q.s = q.x = q.y = q.z = 0.0;
 318  
 319      r1 = pmRotQuatConvert(r, &q);
 320      r2 = pmQuatRpyConvert(&q, rpy);
 321  
 322      return r1 || r2 ? pmErrno : 0;
 323  }
 324  
 325  int pmQuatRotConvert(PmQuaternion const * const q, PmRotationVector * const r)
 326  {
 327      double sh;
 328  
 329  #ifdef PM_DEBUG
 330      if (!pmQuatIsNorm(q)) {
 331  #ifdef PM_PRINT_ERROR
 332  	pmPrintError("Bad quaternion in pmQuatRotConvert\n");
 333  #endif
 334  	return pmErrno = PM_NORM_ERR;
 335      }
 336  #endif
 337      if (r == 0) {
 338  	return (pmErrno = PM_ERR);
 339      }
 340  
 341      sh = pmSqrt(pmSq(q->x) + pmSq(q->y) + pmSq(q->z));
 342  
 343      if (sh > QSIN_FUZZ) {
 344  	r->s = 2.0 * atan2(sh, q->s);
 345  	r->x = q->x / sh;
 346  	r->y = q->y / sh;
 347  	r->z = q->z / sh;
 348      } else {
 349  	r->s = 0.0;
 350  	r->x = 0.0;
 351  	r->y = 0.0;
 352  	r->z = 0.0;
 353      }
 354  
 355      return pmErrno = 0;
 356  }
 357  
 358  int pmQuatMatConvert(PmQuaternion const * const q, PmRotationMatrix * const m)
 359  {
 360  #ifdef PM_DEBUG
 361      if (!pmQuatIsNorm(q)) {
 362  #ifdef PM_PRINT_ERROR
 363  	pmPrintError("Bad quaternion in pmQuatMatConvert\n");
 364  #endif
 365  	return pmErrno = PM_NORM_ERR;
 366      }
 367  #endif
 368  
 369      /* from space book where e1=q->x e2=q->y e3=q->z e4=q->s */
 370      m->x.x = 1.0 - 2.0 * (pmSq(q->y) + pmSq(q->z));
 371      m->y.x = 2.0 * (q->x * q->y - q->z * q->s);
 372      m->z.x = 2.0 * (q->z * q->x + q->y * q->s);
 373  
 374      m->x.y = 2.0 * (q->x * q->y + q->z * q->s);
 375      m->y.y = 1.0 - 2.0 * (pmSq(q->z) + pmSq(q->x));
 376      m->z.y = 2.0 * (q->y * q->z - q->x * q->s);
 377  
 378      m->x.z = 2.0 * (q->z * q->x - q->y * q->s);
 379      m->y.z = 2.0 * (q->y * q->z + q->x * q->s);
 380      m->z.z = 1.0 - 2.0 * (pmSq(q->x) + pmSq(q->y));
 381  
 382      return pmErrno = 0;
 383  }
 384  
 385  int pmQuatZyzConvert(PmQuaternion const * const q, PmEulerZyz * const zyz)
 386  {
 387      PmRotationMatrix m;
 388      int r1, r2;
 389  
 390      /*! \todo FIXME-- need direct equations */
 391      r1 = pmQuatMatConvert(q, &m);
 392      r2 = pmMatZyzConvert(&m, zyz);
 393  
 394      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 395  }
 396  
 397  int pmQuatZyxConvert(PmQuaternion const * const q, PmEulerZyx * const zyx)
 398  {
 399      PmRotationMatrix m;
 400      int r1, r2;
 401  
 402      /*! \todo FIXME-- need direct equations */
 403      r1 = pmQuatMatConvert(q, &m);
 404      r2 = pmMatZyxConvert(&m, zyx);
 405  
 406      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 407  }
 408  
 409  int pmQuatRpyConvert(PmQuaternion const * const q, PmRpy * const rpy)
 410  {
 411      PmRotationMatrix m;
 412      int r1, r2;
 413  
 414      /*! \todo FIXME-- need direct equations */
 415      r1 = pmQuatMatConvert(q, &m);
 416      r2 = pmMatRpyConvert(&m, rpy);
 417  
 418      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 419  }
 420  
 421  int pmMatRotConvert(PmRotationMatrix const * const m, PmRotationVector * const r)
 422  {
 423      PmQuaternion q;
 424      int r1, r2;
 425  
 426      /*! \todo FIXME-- need direct equations */
 427      r1 = pmMatQuatConvert(m, &q);
 428      r2 = pmQuatRotConvert(&q, r);
 429  
 430      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 431  }
 432  
 433  int pmMatQuatConvert(PmRotationMatrix const * const m, PmQuaternion * const q)
 434  {
 435      /* 
 436         from Stephe's "space" book e1 = (c32 - c23) / 4*e4 e2 = (c13 - c31) /
 437         4*e4 e3 = (c21 - c12) / 4*e4 e4 = sqrt(1 + c11 + c22 + c33) / 2
 438  
 439         if e4 == 0 e1 = sqrt(1 + c11 - c33 - c22) / 2 e2 = sqrt(1 + c22 - c33
 440         - c11) / 2 e3 = sqrt(1 + c33 - c11 - c22) / 2 to determine whether to
 441         take the positive or negative sqrt value since e4 == 0 indicates a
 442         180* rotation then (0 x y z) = (0 -x -y -z). Thus some generallities
 443         can be used: 1) find which of e1, e2, or e3 has the largest magnitude
 444         and leave it pos. 2) if e1 is largest then if c21 < 0 then take the
 445         negative for e2 if c31 < 0 then take the negative for e3 3) else if e2 
 446         is largest then if c21 < 0 then take the negative for e1 if c32 < 0
 447         then take the negative for e3 4) else if e3 is larget then if c31 < 0
 448         then take the negative for e1 if c32 < 0 then take the negative for e2
 449  
 450         Note: c21 in the space book is m->x.y in this C code */
 451  
 452      double a;
 453  
 454  #ifdef PM_DEBUG
 455      if (!pmMatIsNorm(m)) {
 456  #ifdef PM_PRINT_ERROR
 457  	pmPrintError("Bad matrix in pmMatQuatConvert\n");
 458  #endif
 459  	return pmErrno = PM_NORM_ERR;
 460      }
 461  #endif
 462  
 463      q->s = 0.5 * pmSqrt(1.0 + m->x.x + m->y.y + m->z.z);
 464  
 465      if (fabs(q->s) > QS_FUZZ) {
 466  	q->x = (m->y.z - m->z.y) / (a = 4 * q->s);
 467  	q->y = (m->z.x - m->x.z) / a;
 468  	q->z = (m->x.y - m->y.x) / a;
 469      } else {
 470  	q->s = 0;
 471  	q->x = pmSqrt(1.0 + m->x.x - m->y.y - m->z.z) / 2.0;
 472  	q->y = pmSqrt(1.0 + m->y.y - m->x.x - m->z.z) / 2.0;
 473  	q->z = pmSqrt(1.0 + m->z.z - m->y.y - m->x.x) / 2.0;
 474  
 475  	if (q->x > q->y && q->x > q->z) {
 476  	    if (m->x.y < 0.0) {
 477  		q->y *= -1;
 478  	    }
 479  	    if (m->x.z < 0.0) {
 480  		q->z *= -1;
 481  	    }
 482  	} else if (q->y > q->z) {
 483  	    if (m->x.y < 0.0) {
 484  		q->x *= -1;
 485  	    }
 486  	    if (m->y.z < 0.0) {
 487  		q->z *= -1;
 488  	    }
 489  	} else {
 490  	    if (m->x.z < 0.0) {
 491  		q->x *= -1;
 492  	    }
 493  	    if (m->y.z < 0.0) {
 494  		q->y *= -1;
 495  	    }
 496  	}
 497      }
 498  
 499      return pmQuatNorm(q, q);
 500  }
 501  
 502  int pmMatZyzConvert(PmRotationMatrix const * const m, PmEulerZyz * const zyz)
 503  {
 504      zyz->y = atan2(pmSqrt(pmSq(m->x.z) + pmSq(m->y.z)), m->z.z);
 505  
 506      if (fabs(zyz->y) < ZYZ_Y_FUZZ) {
 507  	zyz->z = 0.0;
 508  	zyz->y = 0.0;		/* force Y to 0 */
 509  	zyz->zp = atan2(-m->y.x, m->x.x);
 510      } else if (fabs(zyz->y - PM_PI) < ZYZ_Y_FUZZ) {
 511  	zyz->z = 0.0;
 512  	zyz->y = PM_PI;		/* force Y to 180 */
 513  	zyz->zp = atan2(m->y.x, -m->x.x);
 514      } else {
 515  	zyz->z = atan2(m->z.y, m->z.x);
 516  	zyz->zp = atan2(m->y.z, -m->x.z);
 517      }
 518  
 519      return pmErrno = 0;
 520  }
 521  
 522  int pmMatZyxConvert(PmRotationMatrix const * const m, PmEulerZyx * const zyx)
 523  {
 524      zyx->y = atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y)));
 525  
 526      if (fabs(zyx->y - PM_PI_2) < ZYX_Y_FUZZ) {
 527  	zyx->z = 0.0;
 528  	zyx->y = PM_PI_2;	/* force it */
 529  	zyx->x = atan2(m->y.x, m->y.y);
 530      } else if (fabs(zyx->y + PM_PI_2) < ZYX_Y_FUZZ) {
 531  	zyx->z = 0.0;
 532  	zyx->y = -PM_PI_2;	/* force it */
 533  	zyx->x = -atan2(m->y.z, m->y.y);
 534      } else {
 535  	zyx->z = atan2(m->x.y, m->x.x);
 536  	zyx->x = atan2(m->y.z, m->z.z);
 537      }
 538  
 539      return pmErrno = 0;
 540  }
 541  
 542  int pmMatRpyConvert(PmRotationMatrix const * const m, PmRpy * const rpy)
 543  {
 544      rpy->p = atan2(-m->x.z, pmSqrt(pmSq(m->x.x) + pmSq(m->x.y)));
 545  
 546      if (fabs(rpy->p - PM_PI_2) < RPY_P_FUZZ) {
 547  	rpy->r = atan2(m->y.x, m->y.y);
 548  	rpy->p = PM_PI_2;	/* force it */
 549  	rpy->y = 0.0;
 550      } else if (fabs(rpy->p + PM_PI_2) < RPY_P_FUZZ) {
 551  	rpy->r = -atan2(m->y.x, m->y.y);
 552  	rpy->p = -PM_PI_2;	/* force it */
 553  	rpy->y = 0.0;
 554      } else {
 555  	rpy->r = atan2(m->y.z, m->z.z);
 556  	rpy->y = atan2(m->x.y, m->x.x);
 557      }
 558  
 559      return pmErrno = 0;
 560  }
 561  
 562  int pmZyzRotConvert(PmEulerZyz const * const zyz, PmRotationVector * const r)
 563  {
 564  #ifdef PM_PRINT_ERROR
 565      pmPrintError("error: pmZyzRotConvert not implemented\n");
 566  #endif
 567      return pmErrno = PM_IMPL_ERR;
 568  }
 569  
 570  int pmZyzQuatConvert(PmEulerZyz const * const zyz, PmQuaternion * const q)
 571  {
 572      PmRotationMatrix m;
 573      int r1, r2;
 574  
 575      /*! \todo FIXME-- need direct equations */
 576      r1 = pmZyzMatConvert(zyz, &m);
 577      r2 = pmMatQuatConvert(&m, q);
 578  
 579      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 580  }
 581  
 582  int pmZyzMatConvert(PmEulerZyz  const * const zyz, PmRotationMatrix * const m)
 583  {
 584      double sa, sb, sg;
 585      double ca, cb, cg;
 586  
 587      sa = sin(zyz->z);
 588      sb = sin(zyz->y);
 589      sg = sin(zyz->zp);
 590  
 591      ca = cos(zyz->z);
 592      cb = cos(zyz->y);
 593      cg = cos(zyz->zp);
 594  
 595      m->x.x = ca * cb * cg - sa * sg;
 596      m->y.x = -ca * cb * sg - sa * cg;
 597      m->z.x = ca * sb;
 598  
 599      m->x.y = sa * cb * cg + ca * sg;
 600      m->y.y = -sa * cb * sg + ca * cg;
 601      m->z.y = sa * sb;
 602  
 603      m->x.z = -sb * cg;
 604      m->y.z = sb * sg;
 605      m->z.z = cb;
 606  
 607      return pmErrno = 0;
 608  }
 609  
 610  int pmZyzRpyConvert(PmEulerZyz  const * const zyz, PmRpy * const rpy)
 611  {
 612  #ifdef PM_PRINT_ERROR
 613      pmPrintError("error: pmZyzRpyConvert not implemented\n");
 614  #endif
 615      return pmErrno = PM_IMPL_ERR;
 616  }
 617  
 618  int pmZyxRotConvert(PmEulerZyx  const * const zyx, PmRotationVector * const r)
 619  {
 620      PmRotationMatrix m;
 621      int r1, r2;
 622  
 623      /*! \todo FIXME-- need direct equations */
 624      r1 = pmZyxMatConvert(zyx, &m);
 625      r2 = pmMatRotConvert(&m, r);
 626  
 627      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 628  }
 629  
 630  int pmZyxQuatConvert(PmEulerZyx  const * const zyx, PmQuaternion * const q)
 631  {
 632      PmRotationMatrix m;
 633      int r1, r2;
 634  
 635      /*! \todo FIXME-- need direct equations */
 636      r1 = pmZyxMatConvert(zyx, &m);
 637      r2 = pmMatQuatConvert(&m, q);
 638  
 639      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 640  }
 641  
 642  int pmZyxMatConvert(PmEulerZyx  const * const zyx, PmRotationMatrix * const m)
 643  {
 644      double sa, sb, sg;
 645      double ca, cb, cg;
 646  
 647      sa = sin(zyx->z);
 648      sb = sin(zyx->y);
 649      sg = sin(zyx->x);
 650  
 651      ca = cos(zyx->z);
 652      cb = cos(zyx->y);
 653      cg = cos(zyx->x);
 654  
 655      m->x.x = ca * cb;
 656      m->y.x = ca * sb * sg - sa * cg;
 657      m->z.x = ca * sb * cg + sa * sg;
 658  
 659      m->x.y = sa * cb;
 660      m->y.y = sa * sb * sg + ca * cg;
 661      m->z.y = sa * sb * cg - ca * sg;
 662  
 663      m->x.z = -sb;
 664      m->y.z = cb * sg;
 665      m->z.z = cb * cg;
 666  
 667      return pmErrno = 0;
 668  }
 669  
 670  int pmZyxZyzConvert(PmEulerZyx  const * const zyx, PmEulerZyz * const zyz)
 671  {
 672  #ifdef PM_PRINT_ERROR
 673      pmPrintError("error: pmZyxZyzConvert not implemented\n");
 674  #endif
 675      return pmErrno = PM_IMPL_ERR;
 676  }
 677  
 678  int pmZyxRpyConvert(PmEulerZyx  const * const zyx, PmRpy * const rpy)
 679  {
 680  #ifdef PM_PRINT_ERROR
 681      pmPrintError("error: pmZyxRpyConvert not implemented\n");
 682  #endif
 683      return pmErrno = PM_IMPL_ERR;
 684  }
 685  
 686  int pmRpyRotConvert(PmRpy  const * const rpy, PmRotationVector * const r)
 687  {
 688      PmQuaternion q;
 689      int r1, r2;
 690  
 691      q.s = q.x = q.y = q.z = 0.0;
 692      r->s = r->x = r->y = r->z = 0.0;
 693  
 694      r1 = pmRpyQuatConvert(rpy, &q);
 695      r2 = pmQuatRotConvert(&q, r);
 696  
 697      return r1 || r2 ? pmErrno : 0;
 698  }
 699  
 700  int pmRpyQuatConvert(PmRpy  const * const rpy, PmQuaternion * const q)
 701  {
 702      PmRotationMatrix m;
 703      int r1, r2;
 704  
 705      /*! \todo FIXME-- need direct equations */
 706      r1 = pmRpyMatConvert(rpy, &m);
 707      r2 = pmMatQuatConvert(&m, q);
 708  
 709      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
 710  }
 711  
 712  int pmRpyMatConvert(PmRpy  const * const rpy, PmRotationMatrix * const m)
 713  {
 714      double sa, sb, sg;
 715      double ca, cb, cg;
 716  
 717      sa = sin(rpy->y);
 718      sb = sin(rpy->p);
 719      sg = sin(rpy->r);
 720  
 721      ca = cos(rpy->y);
 722      cb = cos(rpy->p);
 723      cg = cos(rpy->r);
 724  
 725      m->x.x = ca * cb;
 726      m->y.x = ca * sb * sg - sa * cg;
 727      m->z.x = ca * sb * cg + sa * sg;
 728  
 729      m->x.y = sa * cb;
 730      m->y.y = sa * sb * sg + ca * cg;
 731      m->z.y = sa * sb * cg - ca * sg;
 732  
 733      m->x.z = -sb;
 734      m->y.z = cb * sg;
 735      m->z.z = cb * cg;
 736  
 737      return pmErrno = 0;
 738  }
 739  
 740  int pmRpyZyzConvert(PmRpy  const * const rpy, PmEulerZyz * const zyz)
 741  {
 742  #ifdef PM_PRINT_ERROR
 743      pmPrintError("error: pmRpyZyzConvert not implemented\n");
 744  #endif
 745      return pmErrno = PM_IMPL_ERR;
 746  }
 747  
 748  int pmRpyZyxConvert(PmRpy  const * const rpy, PmEulerZyx * const zyx)
 749  {
 750  #ifdef PM_PRINT_ERROR
 751      pmPrintError("error: pmRpyZyxConvert not implemented\n");
 752  #endif
 753      return pmErrno = PM_IMPL_ERR;
 754  }
 755  
 756  int pmPoseHomConvert(PmPose const * const p, PmHomogeneous * const h)
 757  {
 758      int r1;
 759  
 760      h->tran = p->tran;
 761      r1 = pmQuatMatConvert(&p->rot, &h->rot);
 762  
 763      return pmErrno = r1;
 764  }
 765  
 766  int pmHomPoseConvert(PmHomogeneous const * const h, PmPose * const p)
 767  {
 768      int r1;
 769  
 770      p->tran = h->tran;
 771      r1 = pmMatQuatConvert(&h->rot, &p->rot);
 772  
 773      return pmErrno = r1;
 774  }
 775  
 776  /* PmCartesian functions */
 777  
 778  int pmCartCartCompare(PmCartesian const * const v1, PmCartesian const * const v2)
 779  {
 780      if (fabs(v1->x - v2->x) >= V_FUZZ ||
 781  	fabs(v1->y - v2->y) >= V_FUZZ || fabs(v1->z - v2->z) >= V_FUZZ) {
 782  	return 0;
 783      }
 784  
 785      return 1;
 786  }
 787  
 788  int pmCartCartDot(PmCartesian const * const v1, PmCartesian const * const v2, double *d)
 789  {
 790      *d = v1->x * v2->x + v1->y * v2->y + v1->z * v2->z;
 791  
 792      return pmErrno = 0;
 793  }
 794  
 795  int pmCartCartMult(PmCartesian const * const v1, PmCartesian const * const v2,
 796          PmCartesian * const out)
 797  {
 798      out->x = v1->x * v2->x;
 799      out->y = v1->y * v2->y;
 800      out->z = v1->z * v2->z;
 801  
 802      return pmErrno = 0;
 803  }
 804  
 805  int pmCartCartDiv(PmCartesian const * const v1, PmCartesian const * const v2,
 806          PmCartesian * const out)
 807  {
 808      out->x = v1->x / v2->x;
 809      out->y = v1->y / v2->y;
 810      out->z = v1->z / v2->z;
 811  
 812      return pmErrno = 0;
 813  }
 814  
 815  int pmCartCartCross(PmCartesian const * const v1, PmCartesian const * const v2,
 816          PmCartesian * const vout)
 817  {
 818      if (vout == v1 || vout == v2) {
 819          return pmErrno = PM_IMPL_ERR;
 820      }
 821      vout->x = v1->y * v2->z - v1->z * v2->y;
 822      vout->y = v1->z * v2->x - v1->x * v2->z;
 823      vout->z = v1->x * v2->y - v1->y * v2->x;
 824  
 825      return pmErrno = 0;
 826  }
 827  
 828  int pmCartInfNorm(PmCartesian const * v, double * out)
 829  {
 830      *out = fmax(fabs(v->x),fmax(fabs(v->y),fabs(v->z)));
 831      return pmErrno = 0;
 832  }
 833  
 834  int pmCartMag(PmCartesian const * const v, double *d)
 835  {
 836      *d = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z));
 837  
 838      return pmErrno = 0;
 839  }
 840  
 841  /** Find square of magnitude of a vector (useful for some calculations to save a sqrt).*/
 842  int pmCartMagSq(PmCartesian const * const v, double *d)
 843  {
 844      *d = pmSq(v->x) + pmSq(v->y) + pmSq(v->z);
 845  
 846      return pmErrno = 0;
 847  }
 848  
 849  int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2,
 850          double *d)
 851  {
 852      *d = pmSqrt(pmSq(v2->x - v1->x) + pmSq(v2->y - v1->y) + pmSq(v2->z - v1->z));
 853  
 854      return pmErrno = 0;
 855  }
 856  
 857  int pmCartCartAdd(PmCartesian const * const v1, PmCartesian const * const v2,
 858          PmCartesian * const vout)
 859  {
 860      vout->x = v1->x + v2->x;
 861      vout->y = v1->y + v2->y;
 862      vout->z = v1->z + v2->z;
 863  
 864      return pmErrno = 0;
 865  }
 866  
 867  int pmCartCartSub(PmCartesian const * const v1, PmCartesian const * const v2,
 868          PmCartesian * const vout)
 869  {
 870      vout->x = v1->x - v2->x;
 871      vout->y = v1->y - v2->y;
 872      vout->z = v1->z - v2->z;
 873  
 874      return pmErrno = 0;
 875  }
 876  
 877  int pmCartScalMult(PmCartesian const * const v1, double d, PmCartesian * const vout)
 878  {
 879      if (v1 != vout) {
 880          *vout = *v1;
 881      }
 882      return pmCartScalMultEq(vout, d);
 883  }
 884  
 885  int pmCartScalDiv(PmCartesian const * const v1, double d, PmCartesian * const vout)
 886  {
 887      if (v1 != vout) {
 888          *vout = *v1;
 889      }
 890      return pmCartScalDivEq(vout, d);
 891  }
 892  
 893  int pmCartNeg(PmCartesian const * const v1, PmCartesian * const vout)
 894  {
 895      if (v1 != vout) {
 896          *vout = *v1;
 897      }
 898  
 899      return pmCartNegEq(vout);
 900  }
 901  
 902  int pmCartNegEq(PmCartesian * const v1)
 903  {
 904      v1->x = -v1->x;
 905      v1->y = -v1->y;
 906      v1->z = -v1->z;
 907  
 908      return pmErrno = 0;
 909  }
 910  
 911  int pmCartInv(PmCartesian const * const v1, PmCartesian * const vout)
 912  {
 913      if (v1 != vout) {
 914          *vout = *v1;
 915      }
 916  
 917      return pmCartInvEq(vout);
 918  }
 919  
 920  int pmCartInvEq(PmCartesian * const v)
 921  {
 922      double size_sq;
 923      pmCartMagSq(v,&size_sq);
 924  
 925      if (size_sq == 0.0) {
 926  #ifdef PM_PRINT_ERROR
 927          pmPrintError(&"Zero vector in pmCartInv\n");
 928  #endif
 929          return pmErrno = PM_NORM_ERR;
 930      }
 931  
 932      v->x /= size_sq;
 933      v->y /= size_sq;
 934      v->z /= size_sq;
 935  
 936      return pmErrno = 0;
 937  }
 938  
 939  // This used to be called pmCartNorm.
 940  
 941  int pmCartUnit(PmCartesian const * const v, PmCartesian * const vout)
 942  {
 943      if (vout != v) {
 944          *vout = *v;
 945      }
 946      return pmCartUnitEq(vout);
 947  }
 948  
 949  int pmCartAbs(PmCartesian const * const v, PmCartesian * const vout)
 950  {
 951  
 952      vout->x = fabs(v->x);
 953      vout->y = fabs(v->y);
 954      vout->z = fabs(v->z);
 955  
 956      return pmErrno = 0;
 957  }
 958  
 959  /* Compound assign operator equivalent functions. These are to prevent issues with passing the same variable as both input (const) and output */
 960  
 961  int pmCartCartAddEq(PmCartesian * const v, PmCartesian const * const v_add)
 962  {
 963      v->x += v_add->x;
 964      v->y += v_add->y;
 965      v->z += v_add->z;
 966  
 967      return pmErrno = 0;
 968  }
 969  
 970  int pmCartCartSubEq(PmCartesian * const v, PmCartesian const * const v_sub)
 971  {
 972      v->x -= v_sub->x;
 973      v->y -= v_sub->y;
 974      v->z -= v_sub->z;
 975  
 976      return pmErrno = 0;
 977  }
 978  
 979  int pmCartScalMultEq(PmCartesian * const v, double d)
 980  {
 981  
 982      v->x *= d;
 983      v->y *= d;
 984      v->z *= d;
 985  
 986      return pmErrno = 0;
 987  }
 988  
 989  int pmCartScalDivEq(PmCartesian * const v, double d)
 990  {
 991  
 992      if (d == 0.0) {
 993  #ifdef PM_PRINT_ERROR
 994          pmPrintError(&"Divide by 0 in pmCartScalDiv\n");
 995  #endif
 996  
 997          return pmErrno = PM_DIV_ERR;
 998      }
 999  
1000      v->x /= d;
1001      v->y /= d;
1002      v->z /= d;
1003  
1004      return pmErrno = 0;
1005  }
1006  
1007  int pmCartUnitEq(PmCartesian * const v)
1008  {
1009      double size = pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z));
1010  
1011      if (size == 0.0) {
1012  #ifdef PM_PRINT_ERROR
1013          pmPrintError("Zero vector in pmCartUnit\n");
1014  #endif
1015          return pmErrno = PM_NORM_ERR;
1016      }
1017  
1018      v->x /= size;
1019      v->y /= size;
1020      v->z /= size;
1021  
1022      return pmErrno = 0;
1023  }
1024  
1025  /*! \todo This is if 0'd out so we can find all the pmCartNorm calls that should
1026   be renamed pmCartUnit. 
1027   Later we'll put this back. */
1028  #if 0
1029  
1030  int pmCartNorm(PmCartesian const * const v, PmCartesian * const vout)
1031  {
1032  
1033      vout->x = v->x;
1034      vout->y = v->y;
1035      vout->z = v->z;
1036  
1037      return pmErrno = 0;
1038  }
1039  #endif
1040  
1041  int pmCartIsNorm(PmCartesian const * const v)
1042  {
1043      return pmSqrt(pmSq(v->x) + pmSq(v->y) + pmSq(v->z)) - 1.0 < UNIT_VEC_FUZZ ? 1 : 0;
1044  }
1045  
1046  int pmCartCartProj(PmCartesian const * const v1, PmCartesian const * const v2, PmCartesian * const vout)
1047  {
1048      int r1, r2;
1049      int r3=1;
1050      double d12;
1051      double d22;
1052      
1053      r1 = pmCartCartDot(v1, v2, &d12);
1054      r2 = pmCartCartDot(v2, v2, &d22);
1055      if (!(r1 || r1)){
1056          r3 = pmCartScalMult(v2, d12/d22, vout);
1057      }
1058  
1059      return pmErrno = r1 || r2 || r3 ? PM_NORM_ERR : 0;
1060  }
1061  
1062  int pmCartPlaneProj(PmCartesian const * const v, PmCartesian const * const normal, PmCartesian * const vout)
1063  {
1064      int r1, r2;
1065      PmCartesian par;
1066  
1067      r1 = pmCartCartProj(v, normal, &par);
1068      r2 = pmCartCartSub(v, &par, vout);
1069  
1070      return pmErrno = r1 || r2 ? PM_NORM_ERR : 0;
1071  }
1072  
1073  /* angle-axis functions */
1074  
1075  int pmQuatAxisAngleMult(PmQuaternion const * const q, PmAxis axis, double angle,
1076      PmQuaternion * const pq)
1077  {
1078      double sh, ch;
1079  
1080  #ifdef PM_DEBUG
1081      if (!pmQuatIsNorm(q)) {
1082  #ifdef PM_PRINT_ERROR
1083  	pmPrintError("error: non-unit quaternion in pmQuatAxisAngleMult\n");
1084  #endif
1085  	return -1;
1086      }
1087  #endif
1088  
1089      angle *= 0.5;
1090      sincos(angle, &sh, &ch);
1091  
1092      switch (axis) {
1093      case PM_X:
1094  	pq->s = ch * q->s - sh * q->x;
1095  	pq->x = ch * q->x + sh * q->s;
1096  	pq->y = ch * q->y + sh * q->z;
1097  	pq->z = ch * q->z - sh * q->y;
1098  	break;
1099  
1100      case PM_Y:
1101  	pq->s = ch * q->s - sh * q->y;
1102  	pq->x = ch * q->x - sh * q->z;
1103  	pq->y = ch * q->y + sh * q->s;
1104  	pq->z = ch * q->z + sh * q->x;
1105  	break;
1106  
1107      case PM_Z:
1108  	pq->s = ch * q->s - sh * q->z;
1109  	pq->x = ch * q->x + sh * q->y;
1110  	pq->y = ch * q->y - sh * q->x;
1111  	pq->z = ch * q->z + sh * q->s;
1112  	break;
1113  
1114      default:
1115  #ifdef PM_PRINT_ERROR
1116  	pmPrintError("error: bad axis in pmQuatAxisAngleMult\n");
1117  #endif
1118  	return -1;
1119      }
1120  
1121      if (pq->s < 0.0) {
1122  	pq->s *= -1.0;
1123  	pq->x *= -1.0;
1124  	pq->y *= -1.0;
1125  	pq->z *= -1.0;
1126      }
1127  
1128      return 0;
1129  }
1130  
1131  /* PmRotationVector functions */
1132  
1133  int pmRotScalMult(PmRotationVector const * const r, double s, PmRotationVector * const rout)
1134  {
1135      rout->s = r->s * s;
1136      rout->x = r->x;
1137      rout->y = r->y;
1138      rout->z = r->z;
1139  
1140      return pmErrno = 0;
1141  }
1142  
1143  int pmRotScalDiv(PmRotationVector const * const r, double s, PmRotationVector * const rout)
1144  {
1145      if (s == 0.0) {
1146  #ifdef PM_PRINT_ERROR
1147  	pmPrintError("Divide by zero in pmRotScalDiv\n");
1148  #endif
1149  
1150  	rout->s = DBL_MAX;
1151  	rout->x = r->x;
1152  	rout->y = r->y;
1153  	rout->z = r->z;
1154  
1155  	return pmErrno = PM_NORM_ERR;
1156      }
1157  
1158      rout->s = r->s / s;
1159      rout->x = r->x;
1160      rout->y = r->y;
1161      rout->z = r->z;
1162  
1163      return pmErrno = 0;
1164  }
1165  
1166  int pmRotIsNorm(PmRotationVector const * const r)
1167  {
1168      if (fabs(r->s) < RS_FUZZ ||
1169  	fabs(pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z))) - 1.0 < UNIT_VEC_FUZZ)
1170      {
1171  	return 1;
1172      }
1173  
1174      return 0;
1175  }
1176  
1177  int pmRotNorm(PmRotationVector const * const r, PmRotationVector * const rout)
1178  {
1179      double size;
1180  
1181      size = pmSqrt(pmSq(r->x) + pmSq(r->y) + pmSq(r->z));
1182  
1183      if (fabs(r->s) < RS_FUZZ) {
1184  	rout->s = 0.0;
1185  	rout->x = 0.0;
1186  	rout->y = 0.0;
1187  	rout->z = 0.0;
1188  
1189  	return pmErrno = 0;
1190      }
1191  
1192      if (size == 0.0) {
1193  #ifdef PM_PRINT_ERROR
1194  	pmPrintError("error: pmRotNorm size is zero\n");
1195  #endif
1196  
1197  	rout->s = 0.0;
1198  	rout->x = 0.0;
1199  	rout->y = 0.0;
1200  	rout->z = 0.0;
1201  
1202  	return pmErrno = PM_NORM_ERR;
1203      }
1204  
1205      rout->s = r->s;
1206      rout->x = r->x / size;
1207      rout->y = r->y / size;
1208      rout->z = r->z / size;
1209  
1210      return pmErrno = 0;
1211  }
1212  
1213  /* PmRotationMatrix functions */
1214  
1215  int pmMatNorm(PmRotationMatrix const * const m, PmRotationMatrix * const mout)
1216  {
1217      /*! \todo FIXME */
1218      *mout = *m;
1219  
1220  #ifdef PM_PRINT_ERROR
1221      pmPrintError("error: pmMatNorm not implemented\n");
1222  #endif
1223      return pmErrno = PM_IMPL_ERR;
1224  }
1225  
1226  int pmMatIsNorm(PmRotationMatrix const * const m)
1227  {
1228      PmCartesian u;
1229  
1230      pmCartCartCross(&m->x, &m->y, &u);
1231  
1232      return (pmCartIsNorm(&m->x) && pmCartIsNorm(&m->y) && pmCartIsNorm(&m->z) && pmCartCartCompare(&u, &m->z));
1233  }
1234  
1235  int pmMatInv(PmRotationMatrix const * const m, PmRotationMatrix * const mout)
1236  {
1237      /* inverse of a rotation matrix is the transpose */
1238  
1239      mout->x.x = m->x.x;
1240      mout->x.y = m->y.x;
1241      mout->x.z = m->z.x;
1242  
1243      mout->y.x = m->x.y;
1244      mout->y.y = m->y.y;
1245      mout->y.z = m->z.y;
1246  
1247      mout->z.x = m->x.z;
1248      mout->z.y = m->y.z;
1249      mout->z.z = m->z.z;
1250  
1251      return pmErrno = 0;
1252  }
1253  
1254  int pmMatCartMult(PmRotationMatrix const * const m, PmCartesian const * const v, PmCartesian * const vout)
1255  {
1256      vout->x = m->x.x * v->x + m->y.x * v->y + m->z.x * v->z;
1257      vout->y = m->x.y * v->x + m->y.y * v->y + m->z.y * v->z;
1258      vout->z = m->x.z * v->x + m->y.z * v->y + m->z.z * v->z;
1259  
1260      return pmErrno = 0;
1261  }
1262  
1263  int pmMatMatMult(PmRotationMatrix const * const m1, PmRotationMatrix const * const m2,
1264      PmRotationMatrix * const mout)
1265  {
1266      mout->x.x = m1->x.x * m2->x.x + m1->y.x * m2->x.y + m1->z.x * m2->x.z;
1267      mout->x.y = m1->x.y * m2->x.x + m1->y.y * m2->x.y + m1->z.y * m2->x.z;
1268      mout->x.z = m1->x.z * m2->x.x + m1->y.z * m2->x.y + m1->z.z * m2->x.z;
1269  
1270      mout->y.x = m1->x.x * m2->y.x + m1->y.x * m2->y.y + m1->z.x * m2->y.z;
1271      mout->y.y = m1->x.y * m2->y.x + m1->y.y * m2->y.y + m1->z.y * m2->y.z;
1272      mout->y.z = m1->x.z * m2->y.x + m1->y.z * m2->y.y + m1->z.z * m2->y.z;
1273  
1274      mout->z.x = m1->x.x * m2->z.x + m1->y.x * m2->z.y + m1->z.x * m2->z.z;
1275      mout->z.y = m1->x.y * m2->z.x + m1->y.y * m2->z.y + m1->z.y * m2->z.z;
1276      mout->z.z = m1->x.z * m2->z.x + m1->y.z * m2->z.y + m1->z.z * m2->z.z;
1277  
1278      return pmErrno = 0;
1279  }
1280  
1281  /* PmQuaternion functions */
1282  
1283  int pmQuatQuatCompare(PmQuaternion const * const q1, PmQuaternion const * const q2)
1284  {
1285  #ifdef PM_DEBUG
1286      if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) {
1287  #ifdef PM_PRINT_ERROR
1288  	pmPrintError("Bad quaternion in pmQuatQuatCompare\n");
1289  #endif
1290      }
1291  #endif
1292  
1293      if (fabs(q1->s - q2->s) < Q_FUZZ &&
1294  	fabs(q1->x - q2->x) < Q_FUZZ &&
1295  	fabs(q1->y - q2->y) < Q_FUZZ && fabs(q1->z - q2->z) < Q_FUZZ) {
1296  	return 1;
1297      }
1298  
1299      /* note (0, x, y, z) = (0, -x, -y, -z) */
1300      if (fabs(q1->s) >= QS_FUZZ ||
1301  	fabs(q1->x + q2->x) >= Q_FUZZ ||
1302  	fabs(q1->y + q2->y) >= Q_FUZZ || fabs(q1->z + q2->z) >= Q_FUZZ) {
1303  	return 0;
1304      }
1305  
1306      return 1;
1307  }
1308  
1309  int pmQuatMag(PmQuaternion const * const q, double *d)
1310  {
1311      PmRotationVector r;
1312      int r1;
1313  
1314      if (0 == d) {
1315  	return (pmErrno = PM_ERR);
1316      }
1317  
1318      r1 = pmQuatRotConvert(q, &r);
1319      *d = r.s;
1320  
1321      return pmErrno = r1;
1322  }
1323  
1324  int pmQuatNorm(PmQuaternion const * const q1, PmQuaternion * const qout)
1325  {
1326      double size = pmSqrt(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z));
1327  
1328      if (size == 0.0) {
1329  #ifdef PM_PRINT_ERROR
1330  	pmPrintError("Bad quaternion in pmQuatNorm\n");
1331  #endif
1332  	qout->s = 1;
1333  	qout->x = 0;
1334  	qout->y = 0;
1335  	qout->z = 0;
1336  
1337  	return pmErrno = PM_NORM_ERR;
1338      }
1339  
1340      if (q1->s >= 0.0) {
1341  	qout->s = q1->s / size;
1342  	qout->x = q1->x / size;
1343  	qout->y = q1->y / size;
1344  	qout->z = q1->z / size;
1345  
1346  	return pmErrno = 0;
1347      } else {
1348  	qout->s = -q1->s / size;
1349  	qout->x = -q1->x / size;
1350  	qout->y = -q1->y / size;
1351  	qout->z = -q1->z / size;
1352  
1353  	return pmErrno = 0;
1354      }
1355  }
1356  
1357  int pmQuatInv(PmQuaternion const * const q1, PmQuaternion * const qout)
1358  {
1359      if (qout == 0) {
1360  	return pmErrno = PM_ERR;
1361      }
1362  
1363      qout->s = q1->s;
1364      qout->x = -q1->x;
1365      qout->y = -q1->y;
1366      qout->z = -q1->z;
1367  
1368  #ifdef PM_DEBUG
1369      if (!pmQuatIsNorm(q1)) {
1370  #ifdef PM_PRINT_ERROR
1371  	pmPrintError("Bad quaternion in pmQuatInv\n");
1372  #endif
1373  	return pmErrno = PM_NORM_ERR;
1374      }
1375  #endif
1376  
1377      return pmErrno = 0;
1378  }
1379  
1380  int pmQuatIsNorm(PmQuaternion const * const q1)
1381  {
1382      return (fabs(pmSq(q1->s) + pmSq(q1->x) + pmSq(q1->y) + pmSq(q1->z) - 1.0) <
1383  	UNIT_QUAT_FUZZ);
1384  }
1385  
1386  int pmQuatScalMult(PmQuaternion const * const q, double s, PmQuaternion * const qout)
1387  {
1388      /*! \todo FIXME-- need a native version; this goes through a rotation vector */
1389      PmRotationVector r;
1390      int r1, r2, r3;
1391  
1392      r1 = pmQuatRotConvert(q, &r);
1393      r2 = pmRotScalMult(&r, s, &r);
1394      r3 = pmRotQuatConvert(&r, qout);
1395  
1396      return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
1397  }
1398  
1399  int pmQuatScalDiv(PmQuaternion const * const q, double s, PmQuaternion * const qout)
1400  {
1401      /*! \todo FIXME-- need a native version; this goes through a rotation vector */
1402      PmRotationVector r;
1403      int r1, r2, r3;
1404  
1405      r1 = pmQuatRotConvert(q, &r);
1406      r2 = pmRotScalDiv(&r, s, &r);
1407      r3 = pmRotQuatConvert(&r, qout);
1408  
1409      return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
1410  }
1411  
1412  int pmQuatQuatMult(PmQuaternion const * const q1, PmQuaternion const * const q2, PmQuaternion * const qout)
1413  {
1414      if (qout == 0) {
1415  	return pmErrno = PM_ERR;
1416      }
1417  
1418      qout->s = q1->s * q2->s - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;
1419  
1420      if (qout->s >= 0.0) {
1421  	qout->x = q1->s * q2->x + q1->x * q2->s + q1->y * q2->z - q1->z * q2->y;
1422  	qout->y = q1->s * q2->y - q1->x * q2->z + q1->y * q2->s + q1->z * q2->x;
1423  	qout->z = q1->s * q2->z + q1->x * q2->y - q1->y * q2->x + q1->z * q2->s;
1424      } else {
1425  	qout->s *= -1;
1426  	qout->x = -q1->s * q2->x - q1->x * q2->s - q1->y * q2->z + q1->z * q2->y;
1427  	qout->y = -q1->s * q2->y + q1->x * q2->z - q1->y * q2->s - q1->z * q2->x;
1428  	qout->z = -q1->s * q2->z - q1->x * q2->y + q1->y * q2->x - q1->z * q2->s;
1429      }
1430  
1431  #ifdef PM_DEBUG
1432      if (!pmQuatIsNorm(q1) || !pmQuatIsNorm(q2)) {
1433  #ifdef PM_PRINT_ERROR
1434  	pmPrintError("Bad quaternion in pmQuatQuatMult\n");
1435  #endif
1436  	return pmErrno = PM_NORM_ERR;
1437      }
1438  #endif
1439  
1440      return pmErrno = 0;
1441  }
1442  
1443  int pmQuatCartMult(PmQuaternion const * const q1, PmCartesian const * const v2, PmCartesian * const vout)
1444  {
1445      PmCartesian c;
1446  
1447      c.x = q1->y * v2->z - q1->z * v2->y;
1448      c.y = q1->z * v2->x - q1->x * v2->z;
1449      c.z = q1->x * v2->y - q1->y * v2->x;
1450  
1451      vout->x = v2->x + 2.0 * (q1->s * c.x + q1->y * c.z - q1->z * c.y);
1452      vout->y = v2->y + 2.0 * (q1->s * c.y + q1->z * c.x - q1->x * c.z);
1453      vout->z = v2->z + 2.0 * (q1->s * c.z + q1->x * c.y - q1->y * c.x);
1454  
1455  #ifdef PM_DEBUG
1456      if (!pmQuatIsNorm(q1)) {
1457  #ifdef PM_PRINT_ERROR
1458  	pmPrintError(&"Bad quaternion in pmQuatCartMult\n");
1459  #endif
1460  	return pmErrno = PM_NORM_ERR;
1461      }
1462  #endif
1463  
1464      return pmErrno = 0;
1465  }
1466  
1467  /* PmPose functions*/
1468  
1469  int pmPosePoseCompare(PmPose const * const p1, PmPose const * const p2)
1470  {
1471  #ifdef PM_DEBUG
1472      if (!pmQuatIsNorm(&p1->rot) || !pmQuatIsNorm(&p2->rot)) {
1473  #ifdef PM_PRINT_ERROR
1474  	pmPrintError("Bad quaternion in pmPosePoseCompare\n");
1475  #endif
1476      }
1477  #endif
1478  
1479      return pmErrno = (pmQuatQuatCompare(&p1->rot, &p2->rot) && pmCartCartCompare(&p1->tran, &p2->tran));
1480  }
1481  
1482  int pmPoseInv(PmPose const * const p1, PmPose * const p2)
1483  {
1484      int r1, r2;
1485  
1486  #ifdef PM_DEBUG
1487      if (!pmQuatIsNorm(&p1->rot)) {
1488  #ifdef PM_PRINT_ERROR
1489  	pmPrintError("Bad quaternion in pmPoseInv\n");
1490  #endif
1491      }
1492  #endif
1493  
1494      r1 = pmQuatInv(&p1->rot, &p2->rot);
1495      r2 = pmQuatCartMult(&p2->rot, &p1->tran, &p2->tran);
1496  
1497      p2->tran.x *= -1.0;
1498      p2->tran.y *= -1.0;
1499      p2->tran.z *= -1.0;
1500  
1501      return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
1502  }
1503  
1504  int pmPoseCartMult(PmPose const * const p1, PmCartesian const * const v2, PmCartesian * const vout)
1505  {
1506      int r1, r2;
1507  
1508  #ifdef PM_DEBUG
1509      if (!pmQuatIsNorm(&p1->rot)) {
1510  #ifdef PM_PRINT_ERROR
1511  	pmPrintError(&"Bad quaternion in pmPoseCartMult\n");
1512  #endif
1513  	return pmErrno = PM_NORM_ERR;
1514      }
1515  #endif
1516  
1517      r1 = pmQuatCartMult(&p1->rot, v2, vout);
1518      r2 = pmCartCartAdd(&p1->tran, vout, vout);
1519  
1520      return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
1521  }
1522  
1523  int pmPosePoseMult(PmPose const * const p1, PmPose const * const p2, PmPose * const pout)
1524  {
1525      int r1, r2, r3;
1526  
1527  #ifdef PM_DEBUG
1528      if (!pmQuatIsNorm(&p1->rot) || !pmQuatIsNorm(&p2->rot)) {
1529  #ifdef PM_PRINT_ERROR
1530  	pmPrintError("Bad quaternion in pmPosePoseMult\n");
1531  #endif
1532  	return pmErrno = PM_NORM_ERR;
1533      }
1534  #endif
1535  
1536      r1 = pmQuatCartMult(&p1->rot, &p2->tran, &pout->tran);
1537      r2 = pmCartCartAdd(&p1->tran, &pout->tran, &pout->tran);
1538      r3 = pmQuatQuatMult(&p1->rot, &p2->rot, &pout->rot);
1539  
1540      return pmErrno = (r1 || r2 || r3) ? PM_NORM_ERR : 0;
1541  }
1542  
1543  /* homogeneous transform functions */
1544  
1545  int pmHomInv(PmHomogeneous const * const h1, PmHomogeneous * const h2)
1546  {
1547      int r1, r2;
1548  
1549  #ifdef PM_DEBUG
1550      if (!pmMatIsNorm(&h1->rot)) {
1551  #ifdef PM_PRINT_ERROR
1552  	pmPrintError("Bad rotation matrix in pmHomInv\n");
1553  #endif
1554      }
1555  #endif
1556  
1557      r1 = pmMatInv(&h1->rot, &h2->rot);
1558      r2 = pmMatCartMult(&h2->rot, &h1->tran, &h2->tran);
1559  
1560      h2->tran.x *= -1.0;
1561      h2->tran.y *= -1.0;
1562      h2->tran.z *= -1.0;
1563  
1564      return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
1565  }
1566  
1567  /* line functions */
1568  
1569  int pmLineInit(PmLine * const line, PmPose const * const start, PmPose const * const end)
1570  {
1571      int r1 = 0, r2 = 0, r3 = 0, r4 = 0, r5 = 0;
1572      double tmag = 0.0;
1573      double rmag = 0.0;
1574      PmQuaternion startQuatInverse;
1575  
1576      if (0 == line) {
1577          return (pmErrno = PM_ERR);
1578      }
1579  
1580      r3 = pmQuatInv(&start->rot, &startQuatInverse);
1581      if (r3) {
1582          return r3;
1583      }
1584  
1585      r4 = pmQuatQuatMult(&startQuatInverse, &end->rot, &line->qVec);
1586      if (r4) {
1587          return r4;
1588      }
1589  
1590      pmQuatMag(&line->qVec, &rmag);
1591      if (rmag > Q_FUZZ) {
1592          r5 = pmQuatScalMult(&line->qVec, 1 / rmag, &(line->qVec));
1593          if (r5) {
1594              return r5;
1595          }
1596      }
1597  
1598      line->start = *start;
1599      line->end = *end;
1600      r1 = pmCartCartSub(&end->tran, &start->tran, &line->uVec);
1601      if (r1) {
1602          return r1;
1603      }
1604  
1605      pmCartMag(&line->uVec, &tmag);
1606      if (IS_FUZZ(tmag, CART_FUZZ)) {
1607          line->uVec.x = 1.0;
1608          line->uVec.y = 0.0;
1609          line->uVec.z = 0.0;
1610      } else {
1611          r2 = pmCartUnit(&line->uVec, &line->uVec);
1612      }
1613      line->tmag = tmag;
1614      line->rmag = rmag;
1615      line->tmag_zero = (line->tmag <= CART_FUZZ);
1616      line->rmag_zero = (line->rmag <= Q_FUZZ);
1617  
1618      /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
1619      return pmErrno = (r1 || r2 || r3 || r4 || r5) ? PM_NORM_ERR : 0;
1620  }
1621  
1622  int pmLinePoint(PmLine const * const line, double len, PmPose * const point)
1623  {
1624      int r1 = 0, r2 = 0, r3 = 0, r4 = 0;
1625  
1626      if (line->tmag_zero) {
1627  	point->tran = line->end.tran;
1628      } else {
1629  	/* return start + len * uVec */
1630  	r1 = pmCartScalMult(&line->uVec, len, &point->tran);
1631  	r2 = pmCartCartAdd(&line->start.tran, &point->tran, &point->tran);
1632      }
1633  
1634      if (line->rmag_zero) {
1635  	point->rot = line->end.rot;
1636      } else {
1637  	if (line->tmag_zero) {
1638  	    r3 = pmQuatScalMult(&line->qVec, len, &point->rot);
1639  	} else {
1640  	    r3 = pmQuatScalMult(&line->qVec, len * line->rmag / line->tmag,
1641  		&point->rot);
1642  	}
1643  	r4 = pmQuatQuatMult(&line->start.rot, &point->rot, &point->rot);
1644      }
1645  
1646      return pmErrno = (r1 || r2 || r3 || r4) ? PM_NORM_ERR : 0;
1647  }
1648  
1649  
1650  /* pure cartesian line functions */
1651  
1652  int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmCartesian const * const end)
1653  {
1654      int r1 = 0, r2 = 0;
1655  
1656      if (0 == line) {
1657          return (pmErrno = PM_ERR);
1658      }
1659  
1660      line->start = *start;
1661      line->end = *end;
1662      r1 = pmCartCartSub(end, start, &line->uVec);
1663      if (r1) {
1664          return r1;
1665      }
1666  
1667      pmCartMag(&line->uVec, &line->tmag);
1668      // NOTE: use the same criteria for "zero" length vectors as used by canon
1669      double max_xyz=0;
1670      pmCartInfNorm(&line->uVec, &max_xyz);
1671  
1672      if (IS_FUZZ(max_xyz, CART_FUZZ)) {
1673          line->uVec.x = 1.0;
1674          line->uVec.y = 0.0;
1675          line->uVec.z = 0.0;
1676          line->tmag_zero = 1;
1677      } else {
1678          r2 = pmCartUnitEq(&line->uVec);
1679          line->tmag_zero = 0;
1680      }
1681  
1682      /* return PM_NORM_ERR if uVec has been set to 1, 0, 0 */
1683      return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
1684  }
1685  
1686  int pmCartLinePoint(PmCartLine const * const line, double len, PmCartesian * const point)
1687  {
1688      int r1 = 0, r2 = 0;
1689  
1690      if (line->tmag_zero) {
1691          *point = line->end;
1692      } else {
1693          /* return start + len * uVec */
1694          r1 = pmCartScalMult(&line->uVec, len, point);
1695          r2 = pmCartCartAdd(&line->start, point, point);
1696      }
1697  
1698      return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
1699  }
1700  
1701  
1702  int pmCartLineStretch(PmCartLine * const line, double new_len, int from_end)
1703  {
1704      int r1 = 0, r2 = 0;
1705  
1706      if (!line || line->tmag_zero || new_len <= DOUBLE_FUZZ) {
1707          return PM_ERR;
1708      }
1709  
1710      if (from_end) {
1711          // Store the new relative position from end in the start point
1712          r1 = pmCartScalMult(&line->uVec, -new_len, &line->start);
1713          // Offset the new start point by the current end point
1714          r2 = pmCartCartAddEq(&line->start, &line->end);
1715      } else {
1716          // Store the new relative position from start in the end point:
1717          r1 = pmCartScalMult(&line->uVec, new_len, &line->end);
1718          // Offset the new end point by the current start point
1719          r2 = pmCartCartAdd(&line->start, &line->end, &line->end);
1720      }
1721      line->tmag = new_len;
1722  
1723      return pmErrno = (r1 || r2) ? PM_NORM_ERR : 0;
1724  }
1725  
1726  /* circle functions */
1727  
1728  /*
1729    pmCircleInit() takes the defining parameters of a generalized circle
1730    and sticks them in the structure. It also computes the radius and vectors
1731    in the plane that are useful for other functions and that don't need
1732    to be recomputed every time.
1733  
1734    Note that the end can be placed arbitrarily, resulting in a combination of
1735    spiral and helical motion. There is an overconstraint between the start,
1736    center, and normal vector: the center vector and start vector are assumed
1737    to be in the plane defined by the normal vector. If this is not true, then
1738    it will be made true by moving the center vector onto the plane.
1739    */
1740  int pmCircleInit(PmCircle * const circle,
1741          PmCartesian const * const start, PmCartesian const * const end,
1742          PmCartesian const * const center, PmCartesian const * const normal, int turn)
1743  {
1744      double dot;
1745      PmCartesian rEnd;
1746      PmCartesian v;
1747      double d;
1748      int r1;
1749  
1750  #ifdef PM_DEBUG
1751      if (0 == circle) {
1752  #ifdef PM_PRINT_ERROR
1753          pmPrintError("error: pmCircleInit cirle pointer is null\n");
1754  #endif
1755          return pmErrno = PM_ERR;
1756      }
1757  #endif
1758  
1759      /* adjust center */
1760      pmCartCartSub(start, center, &v);
1761      r1 = pmCartCartProj(&v, normal, &v);
1762      if (PM_NORM_ERR == r1) {
1763          /* bad normal vector-- abort */
1764  #ifdef PM_PRINT_ERROR
1765          pmPrintError("error: pmCircleInit normal vector is 0\n");
1766  #endif
1767          return -1;
1768      }
1769      pmCartCartAdd(&v, center, &circle->center);
1770  
1771      /* normalize and redirect normal vector based on turns. If turn is less
1772         than 0, point normal vector in other direction and make turn positive, 
1773         -1 -> 0, -2 -> 1, etc. */
1774      pmCartUnit(normal, &circle->normal);
1775      if (turn < 0) {
1776          turn = -1 - turn;
1777          pmCartScalMult(&circle->normal, -1.0, &circle->normal);
1778      }
1779  
1780      /* radius */
1781      pmCartCartDisp(start, &circle->center, &circle->radius);
1782  
1783      /* vector in plane of circle from center to start, magnitude radius */
1784      pmCartCartSub(start, &circle->center, &circle->rTan);
1785      /* vector in plane of circle perpendicular to rTan, magnitude radius */
1786      pmCartCartCross(&circle->normal, &circle->rTan, &circle->rPerp);
1787  
1788      /* do rHelix, rEnd */
1789      pmCartCartSub(end, &circle->center, &circle->rHelix);
1790      pmCartPlaneProj(&circle->rHelix, &circle->normal, &rEnd);
1791      pmCartMag(&rEnd, &circle->spiral);
1792      circle->spiral -= circle->radius;
1793      pmCartCartSub(&circle->rHelix, &rEnd, &circle->rHelix);
1794      pmCartUnit(&rEnd, &rEnd);
1795      pmCartScalMult(&rEnd, circle->radius, &rEnd);
1796  
1797      /* Patch for error spiral end same as spiral center */
1798      pmCartMag(&rEnd, &d);
1799      if (d == 0.0) {
1800          pmCartScalMult(&circle->normal, DOUBLE_FUZZ, &v);
1801          pmCartCartAdd(&rEnd, &v, &rEnd);
1802      }
1803      /* end patch 03-mar-1999 Dirk Maij */
1804  
1805      /* angle */
1806      pmCartCartDot(&circle->rTan, &rEnd, &dot);
1807      dot = dot / (circle->radius * circle->radius);
1808      if (dot > 1.0) {
1809          circle->angle = 0.0;
1810      } else if (dot < -1.0) {
1811          circle->angle = PM_PI;
1812      } else {
1813          circle->angle = acos(dot);
1814      }
1815      /* now angle is in range 0..PI . Check if cross is antiparallel to
1816         normal. If so, true angle is between PI..2PI. Need to subtract from
1817         2PI. */
1818      pmCartCartCross(&circle->rTan, &rEnd, &v);
1819      pmCartCartDot(&v, &circle->normal, &d);
1820      if (d < 0.0) {
1821          circle->angle = PM_2_PI - circle->angle;
1822      }
1823  
1824      if (circle->angle > -(CIRCLE_FUZZ) && circle->angle < (CIRCLE_FUZZ)) {
1825          circle->angle = PM_2_PI;
1826      }
1827  
1828      /* now add more angle for multi turns */
1829      if (turn > 0) {
1830          circle->angle += turn * 2.0 * PM_PI;
1831      }
1832  
1833      //Default to invalid
1834  /* if 0'ed out while not debugging*/
1835  #if 0
1836      printf("\n\n");
1837      printf("pmCircleInit:\n");
1838      printf(" \t start  : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1839  	start->x, start->y, start->z);
1840      printf(" \t end    : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1841  	end->x, end->y, end->z);
1842      printf(" \t center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1843  	center->x, center->y, center->z);
1844      printf(" \t normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1845  	normal->x, normal->y, normal->z);
1846      printf(" \t rEnd   : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1847  	rEnd.x, rEnd.y, rEnd.z);
1848      printf(" \t turn=%d\n", turn);
1849      printf(" \t dot=%9.9f\n", dot);
1850      printf(" \t d=%9.9f\n", d);
1851      printf(" \t circle  \t{angle=%9.9f, radius=%9.9f, spiral=%9.9f}\n",
1852  	circle->angle, circle->radius, circle->spiral);
1853      printf(" \t circle->normal : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1854  	circle->normal.x, circle->normal.y, circle->normal.z);
1855      printf(" \t circle->center : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1856  	circle->center.x, circle->center.y, circle->center.z);
1857      printf(" \t circle->rTan : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1858  	circle->rTan.x, circle->rTan.y, circle->rTan.z);
1859      printf(" \t circle->rPerp : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1860  	circle->rPerp.x, circle->rPerp.y, circle->rPerp.z);
1861      printf(" \t circle->rHelix : \t{x=%9.9f, y=%9.9f, z=%9.9f}\n",
1862              circle->rHelix.x, circle->rHelix.y, circle->rHelix.z);
1863      printf("\n\n");
1864  #endif
1865  
1866      return pmErrno = 0;
1867  }
1868  
1869  
1870  /*
1871    pmCirclePoint() returns the point at the given angle along
1872    the circle. If the circle is a helix or spiral or combination, the
1873    point will include interpolation off the actual circle.
1874    */
1875  int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point)
1876  {
1877      PmCartesian par, perp;
1878      double scale;
1879  
1880  #ifdef PM_DEBUG
1881      if (0 == circle || 0 == point) {
1882  #ifdef PM_PRINT_ERROR
1883  	pmPrintError
1884  	    ("error: pmCirclePoint circle or point pointer is null\n");
1885  #endif
1886  	return pmErrno = PM_ERR;
1887      }
1888  #endif
1889  
1890      /* compute components rel to center */
1891      pmCartScalMult(&circle->rTan, cos(angle), &par);
1892      pmCartScalMult(&circle->rPerp, sin(angle), &perp);
1893  
1894      /* add to get radius vector rel to center */
1895      pmCartCartAdd(&par, &perp, point);
1896  
1897      /* get scale for spiral, helix interpolation */
1898      if (circle->angle == 0.0) {
1899  #ifdef PM_PRINT_ERROR
1900  	pmPrintError("error: pmCirclePoint angle is zero\n");
1901  #endif
1902  	return pmErrno = PM_DIV_ERR;
1903      }
1904      scale = angle / circle->angle;
1905  
1906      /* add scaled vector in radial dir for spiral */
1907      pmCartUnit(point, &par);
1908      pmCartScalMult(&par, scale * circle->spiral, &par);
1909      pmCartCartAdd(point, &par, point);
1910  
1911      /* add scaled vector in helix dir */
1912      pmCartScalMult(&circle->rHelix, scale, &perp);
1913      pmCartCartAdd(point, &perp, point);
1914  
1915      /* add to center vector for final result */
1916      pmCartCartAdd(&circle->center, point, point);
1917  
1918      return pmErrno = 0;
1919  }
1920  
1921  int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end)
1922  {
1923      if (!circ || new_angle <= DOUBLE_FUZZ) {
1924          return PM_ERR;
1925      }
1926  
1927      double mag = 0;
1928      pmCartMagSq(&circ->rHelix, &mag);
1929      if ( mag > 1e-6 ) {
1930          //Can't handle helices
1931          return PM_ERR;
1932      }
1933      //TODO handle spiral?
1934      if (from_end) {
1935          //Not implemented yet, way more reprocessing...
1936          PmCartesian new_start;
1937          double start_angle = circ->angle - new_angle;
1938          pmCirclePoint(circ, start_angle, &new_start);
1939          pmCartCartSub(&new_start, &circ->center, &circ->rTan);
1940          pmCartCartCross(&circ->normal, &circ->rTan, &circ->rPerp);
1941          pmCartMag(&circ->rTan, &circ->radius);
1942      } 
1943      //Reduce the spiral proportionally
1944      circ->spiral *= (new_angle / circ->angle);
1945      // Easy to grow / shrink from start
1946      circ->angle = new_angle;
1947  
1948      return 0;
1949  }