posemath.h
1 /******************************************************************** 2 * Description: posemath.h 3 * Declarations for pose math library data types and manipulation 4 * functions. 5 * 6 * Data types comprise various representations of translation and 7 * rotation quantities, and a 'pose' for representing the location 8 * and orientation of a frame in space relative to a base frame. 9 * Translation representations include cartesian, spherical, and 10 * cylindrical coordinates. All of these contain 3 elements. Rotation 11 * representations include rotation vectors, quaternions, rotation 12 * matrices, Euler angles, and roll-pitch-yaw. These contain at least 13 * 3 elements, and may contain more. Only 3 are necessary for the 3 14 * degrees of freedom for either translation or rotation, but some 15 * data representations use more for computational efficiency or 16 * intuition at the expense of storage space. 17 * 18 * Types are abbreviated in function naming with a few letters. 19 * Functions exist for conversion between data types, checking for 20 * consistency, normalization into consistency, extracting features 21 * such as size, and arithmetic operations. 22 * 23 * Names of data representations are in all capitals, prefixed with 24 * 'PM_'. Names of functions are in mixed case, prefixed with 'pm', 25 * with case changes used to indicate new quantities instead of 26 * underscores. Function syntax looks like 27 * int UmQuatRotConvert(PM_QUATERNION, PM_ROTATION_VECTOR *); 28 * 29 * The return value is an error code, 0 for success, or a non-zero 30 * error code for failure, for example: 31 * 32 * #define PM_ERR -1 33 * #define PM_IMPL_ERR -2 34 * 35 * The global variable 'pmErrno' is set to this return value. 36 * 37 * C++ classes are used for data types so that operator overloading can 38 * be used to reduce the programming labor. Using the overloaded operator 39 * version of functions loses the integer error code. The global 40 * variable 'pmErrno' can be queried after these operations. This is not 41 * thread-safe or reentrant. 42 * 43 * C++ names corresponding to the C structures use case mixing instead 44 * of all caps. Thus, a quaternion in C++ is a PmQuaternion. 45 * 46 * The MATH_DEBUG symbol can be defined to include error reporting via 47 * printed errors. 48 * 49 * Native efficient C functions exist for the PM_CARTESIAN, PM_QUATERNION, 50 * and PM_POSE types. Constructors in all the classes have been defined 51 * to convert to/from PM_CARTESIAN and any other translation type, and 52 * to convert to/from PM_QUATERNION and any other rotation type. This means 53 * that if no explicit C functions exist for another type, conversions 54 * to the corresponding native type will occur automatically. If more 55 * efficiency is desired for a particular type, C functions to handle the 56 * operations should be coded and the overloaded C++ functions or operators 57 * should be added. 58 * 59 * 60 * Derived from a work by Fred Proctor & Will Shackleford 61 * 62 * Author: 63 * License: LGPL Version 2 64 * System: Linux 65 * 66 * Copyright (c) 2004 All rights reserved. 67 * 68 * Last change: 69 ********************************************************************/ 70 71 #ifndef POSEMATH_H 72 #define POSEMATH_H 73 74 // #include "config.h" 75 76 #ifdef __cplusplus 77 78 #define USE_CONST 79 #define USE_CCONST 80 #define USE_REF 81 82 #ifdef USE_CCONST 83 #define PM_CCONST const 84 #else 85 #define PM_CCONST 86 #endif 87 88 #ifdef USE_CONST 89 #define PM_CONST const 90 #else 91 #define PM_CONST 92 #endif 93 94 #ifdef USE_REF 95 #define PM_REF & 96 #else 97 #define PM_REF 98 #endif 99 100 #define INCLUDE_POSEMATH_COPY_CONSTRUCTORS 101 102 /* forward declarations-- conversion ctors will need these */ 103 104 /* translation types */ 105 struct PM_CARTESIAN; /* Cart */ 106 struct PM_SPHERICAL; /* Sph */ 107 struct PM_CYLINDRICAL; /* Cyl */ 108 109 /* rotation types */ 110 struct PM_ROTATION_VECTOR; /* Rot */ 111 struct PM_ROTATION_MATRIX; /* Mat */ 112 struct PM_QUATERNION; /* Quat */ 113 struct PM_EULER_ZYZ; /* Zyz */ 114 struct PM_EULER_ZYX; /* Zyx */ 115 struct PM_RPY; /* Rpy */ 116 117 /* pose types */ 118 struct PM_POSE; /* Pose */ 119 struct PM_HOMOGENEOUS; /* Hom */ 120 121 /* PM_CARTESIAN */ 122 123 struct PM_CARTESIAN { 124 /* ctors/dtors */ 125 PM_CARTESIAN() { 126 }; 127 PM_CARTESIAN(double _x, double _y, double _z); 128 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 129 PM_CARTESIAN(PM_CCONST PM_CARTESIAN & cart); // added 7-May-1997 130 // by WPS 131 #endif 132 133 PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c); /* conversion */ 134 PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s); /* conversion */ 135 136 /* operators */ 137 double &operator[] (int n); /* this[n] */ 138 PM_CARTESIAN & operator += (const PM_CARTESIAN &o); 139 PM_CARTESIAN & operator -= (const PM_CARTESIAN &o); 140 141 // Scalar operations 142 PM_CARTESIAN & operator *= (double o); 143 PM_CARTESIAN & operator /= (double o); 144 145 /* data */ 146 double x, y, z; /* this.x, etc. */ 147 }; 148 149 /* PM_SPHERICAL */ 150 151 struct PM_SPHERICAL { 152 /* ctors/dtors */ 153 PM_SPHERICAL() { 154 }; 155 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 156 PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s); 157 #endif 158 PM_SPHERICAL(double _theta, double _phi, double _r); 159 PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF v); /* conversion */ 160 PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v); /* conversion */ 161 162 /* operators */ 163 double &operator[] (int n); /* this[n] */ 164 165 /* data */ 166 double theta, phi, r; 167 }; 168 169 /* PM_CYLINDRICAL */ 170 171 struct PM_CYLINDRICAL { 172 /* ctors/dtors */ 173 PM_CYLINDRICAL() { 174 }; 175 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 176 PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c); 177 #endif 178 PM_CYLINDRICAL(double _theta, double _r, double _z); 179 PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v); /* conversion */ 180 PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF v); /* conversion */ 181 182 /* operators */ 183 double &operator[] (int n); /* this[n] */ 184 185 /* data */ 186 double theta, r, z; 187 }; 188 189 /* PM_ROTATION_VECTOR */ 190 191 struct PM_ROTATION_VECTOR { 192 /* ctors/dtors */ 193 PM_ROTATION_VECTOR() { 194 }; 195 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 196 PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r); 197 #endif 198 PM_ROTATION_VECTOR(double _r, double _x, double _y, double _z); 199 PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q); /* conversion 200 */ 201 202 /* operators */ 203 double &operator[] (int n); /* this[n] */ 204 205 /* data */ 206 double s, x, y, z; 207 }; 208 209 /* PM_ROTATION_MATRIX */ 210 211 struct PM_ROTATION_MATRIX { 212 /* ctors/dtors */ 213 PM_ROTATION_MATRIX() { 214 }; 215 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 216 PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & mat); /* added 217 7-May-1997 218 by WPS */ 219 #endif 220 PM_ROTATION_MATRIX(double xx, double xy, double xz, 221 double yx, double yy, double yz, double zx, double zy, double zz); 222 PM_ROTATION_MATRIX(PM_CARTESIAN _x, PM_CARTESIAN _y, PM_CARTESIAN _z); 223 PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v); /* conversion 224 */ 225 PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q); /* conversion 226 */ 227 PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz); /* conversion 228 */ 229 PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx); /* conversion 230 */ 231 PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy); /* conversion */ 232 233 /* operators */ 234 PM_CARTESIAN & operator[](int n); /* this[n] */ 235 236 /* data */ 237 PM_CARTESIAN x, y, z; 238 }; 239 240 /* PM_QUATERNION */ 241 242 enum PM_AXIS { PM_X, PM_Y, PM_Z }; 243 244 struct PM_QUATERNION { 245 /* ctors/dtors */ 246 PM_QUATERNION() { 247 }; 248 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 249 PM_QUATERNION(PM_CCONST PM_QUATERNION & quat); /* added 7-May-1997 250 by WPS */ 251 #endif 252 PM_QUATERNION(double _s, double _x, double _y, double _z); 253 PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v); /* conversion 254 */ 255 PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion 256 */ 257 PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz); /* conversion */ 258 PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx); /* conversion */ 259 PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy); /* conversion */ 260 PM_QUATERNION(PM_AXIS axis, double angle); /* conversion */ 261 262 /* operators */ 263 double &operator[] (int n); /* this[n] */ 264 265 /* functions */ 266 void axisAngleMult(PM_AXIS axis, double angle); 267 268 /* data */ 269 double s, x, y, z; /* this.s, etc. */ 270 }; 271 272 /* PM_EULER_ZYZ */ 273 274 struct PM_EULER_ZYZ { 275 /* ctors/dtors */ 276 PM_EULER_ZYZ() { 277 }; 278 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 279 PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz); 280 #endif 281 PM_EULER_ZYZ(double _z, double _y, double _zp); 282 PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ 283 PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ 284 285 /* operators */ 286 double &operator[] (int n); 287 288 /* data */ 289 double z, y, zp; 290 }; 291 292 /* PM_EULER_ZYX */ 293 294 struct PM_EULER_ZYX { 295 /* ctors/dtors */ 296 PM_EULER_ZYX() { 297 }; 298 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 299 PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx); 300 #endif 301 PM_EULER_ZYX(double _z, double _y, double _x); 302 PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ 303 PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ 304 305 /* operators */ 306 double &operator[] (int n); 307 308 /* data */ 309 double z, y, x; 310 }; 311 312 /* PM_RPY */ 313 314 struct PM_RPY { 315 /* ctors/dtors */ 316 PM_RPY() { 317 }; 318 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 319 PM_RPY(PM_CCONST PM_RPY PM_REF rpy); /* added 7-May-1997 by WPS */ 320 #endif 321 PM_RPY(double _r, double _p, double _y); 322 PM_RPY(PM_CONST PM_QUATERNION PM_REF q); /* conversion */ 323 PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m); /* conversion */ 324 325 /* operators */ 326 double &operator[] (int n); 327 328 /* data */ 329 double r, p, y; 330 }; 331 332 /* PM_POSE */ 333 334 struct PM_POSE { 335 /* ctors/dtors */ 336 PM_POSE() { 337 }; 338 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 339 PM_POSE(PM_CCONST PM_POSE & p); 340 #endif 341 PM_POSE(PM_CARTESIAN v, PM_QUATERNION q); 342 PM_POSE(double x, double y, double z, 343 double s, double sx, double sy, double sz); 344 PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h); /* conversion */ 345 346 /* operators */ 347 double &operator[] (int n); /* this[n] */ 348 349 /* data */ 350 PM_CARTESIAN tran; 351 PM_QUATERNION rot; 352 }; 353 354 /* PM_HOMOGENEOUS */ 355 356 struct PM_HOMOGENEOUS { 357 /* ctors/dtors */ 358 PM_HOMOGENEOUS() { 359 }; 360 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 361 PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h); 362 #endif 363 PM_HOMOGENEOUS(PM_CARTESIAN v, PM_ROTATION_MATRIX m); 364 PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p); /* conversion */ 365 366 /* operators */ 367 PM_CARTESIAN & operator[](int n); /* column vector */ 368 369 /* data ( [ 0 0 0 1 ] element is manually returned by [] if needed ) */ 370 PM_CARTESIAN tran; 371 PM_ROTATION_MATRIX rot; 372 }; 373 374 /* PM_LINE */ 375 376 struct PM_LINE { 377 /* ctors/dtors */ 378 PM_LINE() { 379 }; 380 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 381 PM_LINE(PM_CCONST PM_LINE &); 382 #endif 383 384 /* functions */ 385 int init(PM_POSE start, PM_POSE end); 386 int point(double len, PM_POSE * point); 387 388 /* data */ 389 PM_POSE start; /* where motion was started */ 390 PM_POSE end; /* where motion is going */ 391 PM_CARTESIAN uVec; /* unit vector from start to end */ 392 }; 393 394 /* PM_CIRCLE */ 395 396 struct PM_CIRCLE { 397 /* ctors/dtors */ 398 PM_CIRCLE() { 399 }; 400 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 401 PM_CIRCLE(PM_CCONST PM_CIRCLE &); 402 #endif 403 404 /* functions */ 405 int init(PM_POSE start, PM_POSE end, 406 PM_CARTESIAN center, PM_CARTESIAN normal, int turn); 407 int point(double angle, PM_POSE * point); 408 409 /* data */ 410 PM_CARTESIAN center; 411 PM_CARTESIAN normal; 412 PM_CARTESIAN rTan; 413 PM_CARTESIAN rPerp; 414 PM_CARTESIAN rHelix; 415 double radius; 416 double angle; 417 double spiral; 418 }; 419 420 /* overloaded external functions */ 421 422 /* dot */ 423 extern double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); 424 425 /* cross */ 426 extern PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); 427 428 #if 0 429 /* norm */ 430 extern PM_CARTESIAN norm(PM_CARTESIAN v); 431 extern PM_QUATERNION norm(PM_QUATERNION q); 432 extern PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r); 433 extern PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m); 434 #endif 435 436 /* unit */ 437 extern PM_CARTESIAN unit(const PM_CARTESIAN &v); 438 extern PM_QUATERNION unit(const PM_QUATERNION &q); 439 extern PM_ROTATION_VECTOR unit(const PM_ROTATION_VECTOR &r); 440 extern PM_ROTATION_MATRIX unit(const PM_ROTATION_MATRIX &m); 441 442 /* isNorm */ 443 extern int isNorm(const PM_CARTESIAN &v); 444 extern int isNorm(const PM_QUATERNION &q); 445 extern int isNorm(const PM_ROTATION_VECTOR &r); 446 extern int isNorm(const PM_ROTATION_MATRIX &m); 447 448 /* mag */ 449 extern double mag(const PM_CARTESIAN &v); 450 451 /* disp */ 452 extern double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); 453 454 /* inv */ 455 extern PM_CARTESIAN inv(const PM_CARTESIAN &v); 456 extern PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m); 457 extern PM_QUATERNION inv(const PM_QUATERNION &q); 458 extern PM_POSE inv(const PM_POSE &p); 459 extern PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h); 460 461 /* project */ 462 extern PM_CARTESIAN proj(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); 463 464 /* overloaded arithmetic functions */ 465 466 /* unary +, - for translation, rotation, pose */ 467 extern PM_CARTESIAN operator + (const PM_CARTESIAN &v); 468 extern PM_CARTESIAN operator - (const PM_CARTESIAN &v); 469 extern PM_QUATERNION operator + (const PM_QUATERNION &q); 470 extern PM_QUATERNION operator - (const PM_QUATERNION &q); 471 extern PM_POSE operator + (const PM_POSE &p); 472 extern PM_POSE operator - (const PM_POSE &p); 473 474 /* compare operators */ 475 extern int operator == (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); 476 extern int operator == (const PM_QUATERNION &q1, const PM_QUATERNION &q2); 477 extern int operator == (const PM_POSE &p1, const PM_POSE &p2); 478 extern int operator != (const PM_CARTESIAN &v1, const PM_CARTESIAN &v2); 479 extern int operator != (const PM_QUATERNION &q1, const PM_QUATERNION &q2); 480 extern int operator != (const PM_POSE &p1, const PM_POSE &p2); 481 482 /* translation +, -, scalar *, - */ 483 484 /* v + v */ 485 extern PM_CARTESIAN operator + (PM_CARTESIAN v1, const PM_CARTESIAN &v2); 486 /* v - v */ 487 extern PM_CARTESIAN operator - (PM_CARTESIAN v1, const PM_CARTESIAN &v2); 488 /* v * s */ 489 extern PM_CARTESIAN operator *(PM_CARTESIAN v, double s); 490 /* s * v */ 491 extern PM_CARTESIAN operator *(double s, PM_CARTESIAN v); 492 /* v / s */ 493 extern PM_CARTESIAN operator / (const PM_CARTESIAN &v, double s); 494 495 /* rotation * by scalar, translation, and rotation */ 496 497 /* s * q */ 498 extern PM_QUATERNION operator *(double s, const PM_QUATERNION &q); 499 /* q * s */ 500 extern PM_QUATERNION operator *(const PM_QUATERNION &q, double s); 501 /* q / s */ 502 extern PM_QUATERNION operator / (const PM_QUATERNION &q, double s); 503 /* q * v */ 504 extern PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v); 505 /* q * q */ 506 extern PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2); 507 /* m * m */ 508 extern PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, 509 const PM_ROTATION_MATRIX &m2); 510 511 /* pose operators */ 512 513 /* q * p */ 514 extern PM_POSE operator *(const PM_QUATERNION &q, const PM_POSE &p); 515 /* p * p */ 516 extern PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2); 517 /* p * v */ 518 extern PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v); 519 520 #endif /* __cplusplus */ 521 522 /* now comes the C stuff */ 523 524 #ifdef __cplusplus 525 extern "C" { 526 #endif 527 528 /* PmCartesian */ 529 530 typedef struct { 531 double x, y, z; /* this.x, etc. */ 532 533 } PmCartesian; 534 535 /* PmSpherical */ 536 537 typedef struct { 538 double theta, phi, r; 539 540 } PmSpherical; 541 542 /* PmCylindrical */ 543 544 typedef struct { 545 double theta, r, z; 546 547 } PmCylindrical; 548 549 /* PmAxis */ 550 #ifdef __cplusplus 551 typedef PM_AXIS PmAxis; 552 #else 553 typedef enum { PM_X, PM_Y, PM_Z } PmAxis; 554 #endif 555 556 /* PmRotationVector */ 557 558 typedef struct { 559 double s, x, y, z; 560 561 } PmRotationVector; 562 563 /* PmRotationMatrix */ 564 565 typedef struct { 566 PmCartesian x, y, z; 567 568 } PmRotationMatrix; 569 570 /* PmQuaternion */ 571 572 typedef struct { 573 double s, x, y, z; /* this.s, etc. */ 574 575 } PmQuaternion; 576 577 /* PmEulerZyz */ 578 579 typedef struct { 580 double z, y, zp; 581 582 } PmEulerZyz; 583 584 /* PmEulerZyx */ 585 586 typedef struct { 587 double z, y, x; 588 589 } PmEulerZyx; 590 591 /* PmRpy */ 592 593 typedef struct { 594 double r, p, y; 595 596 } PmRpy; 597 598 /* PmPose */ 599 600 typedef struct { 601 PmCartesian tran; 602 PmQuaternion rot; 603 604 } PmPose; 605 606 /* PmCartLine */ 607 typedef struct { 608 PmCartesian start; 609 PmCartesian end; 610 PmCartesian uVec; 611 double tmag; 612 int tmag_zero; 613 } PmCartLine; 614 615 /* Homogeneous transform PmHomogeneous */ 616 617 typedef struct { 618 PmCartesian tran; 619 PmRotationMatrix rot; 620 621 } PmHomogeneous; 622 623 /* line structure */ 624 625 typedef struct { 626 PmPose start; /* where motion was started */ 627 PmPose end; /* where motion is going */ 628 PmCartesian uVec; /* unit vector from start to end */ 629 PmQuaternion qVec; /* unit of rotation */ 630 double tmag; 631 double rmag; 632 int tmag_zero; 633 int rmag_zero; 634 635 } PmLine; 636 637 /* Generalized circle structure */ 638 639 typedef struct { 640 PmCartesian center; 641 PmCartesian normal; 642 PmCartesian rTan; 643 PmCartesian rPerp; 644 PmCartesian rHelix; 645 double radius; 646 double angle; 647 double spiral; 648 649 } PmCircle; 650 651 /* 652 shorthand types for normal use-- don't define PM_LOOSE_NAMESPACE if these 653 names are used by other headers you need to include and you don't want 654 these shorthand versions 655 */ 656 657 /* some nice constants */ 658 659 #define PM_PI 3.14159265358979323846 660 #define PM_PI_2 1.57079632679489661923 661 #define PM_PI_4 0.78539816339744830962 662 #define PM_2_PI 6.28318530717958647692 663 664 #ifdef PM_LOOSE_NAMESPACE 665 666 typedef PmCartesian VECTOR; 667 typedef PmSpherical SPHERICAL; 668 typedef PmCylindrical CYLINDRICAL; 669 typedef PmQuaternion QUATERNION; 670 typedef PmRotationMatrix MATRIX; 671 typedef PmEulerZyz ZYZ; 672 typedef PmEulerZyx ZYX; 673 typedef PmRpy RPY; 674 typedef PmPose POSE; 675 typedef PmHomogeneous HX; 676 typedef PmCircle CIRCLE; 677 typedef PmLine LINE; 678 679 #define PI PM_PI 680 #define PI_2 PM_PI_2 681 #define PI_4 PM_PI_4 682 #define TWO_PI PM_2_PI /* 2_PI invalid macro name */ 683 #endif 684 685 /* quicky macros */ 686 687 #define pmClose(a, b, eps) ((fabs((a) - (b)) < (eps)) ? 1 : 0) 688 #define pmSq(x) ((x)*(x)) 689 690 #ifdef TO_DEG 691 #undef TO_DEG 692 #endif 693 #define TO_DEG (180./PM_PI) 694 695 #ifdef TO_RAD 696 #undef TO_RAD 697 #endif 698 #define TO_RAD (PM_PI/180.) 699 700 /*! \todo FIXME-- fix these */ 701 702 /* DOUBLE_FUZZ is the smallest double, d, such that (1+d != 1) w/o FPC. 703 DOUBLECP_FUZZ is the same only with the Floating Point CoProcessor */ 704 705 #define DOUBLE_FUZZ 2.2204460492503131e-16 706 #define DOUBLECP_FUZZ 1.0842021724855044e-19 707 708 709 /** 710 * FIXME sloppily defined constants here. 711 * These constants are quite large compared to the DOUBLE_FUZZ limitation. They 712 * seem like an ugly band-aid for floating point problems. 713 */ 714 715 // FIXME setting this to be an order of magnitude smaller than canon's shortest 716 // allowed segment. This is still larger than TP's smallest position, so it may 717 // be silently causing trouble. 718 #define CART_FUZZ (1.0e-8) 719 /* how close a cartesian vector's magnitude must be for it to be considered 720 a zero vector */ 721 722 #define Q_FUZZ (1.0e-06) 723 /* how close elements of a Q must be to be equal */ 724 725 #define QS_FUZZ (1.0e-6) 726 /* how close q.s is to 0 to be 180 deg rotation */ 727 728 #define RS_FUZZ (1.0e-6) 729 /* how close r.s is for a rotation vector to be considered 0 */ 730 731 #define QSIN_FUZZ (1.0e-6) 732 /* how close sin(a/2) is to 0 to be zero rotat */ 733 734 #define V_FUZZ (1.0e-8) 735 /* how close elements of a V must be to be equal */ 736 737 #define SQRT_FUZZ (-1.0e-6) 738 /* how close to 0 before math_sqrt() is error */ 739 740 #define UNIT_VEC_FUZZ (1.0e-6) 741 /* how close mag of vec must be to 1.00 */ 742 743 #define UNIT_QUAT_FUZZ (1.0e-6) 744 /* how close mag of quat must be to 1.00 */ 745 746 #define UNIT_SC_FUZZ (1.0e-6) 747 /* how close mag of sin, cos must be to 1.00 */ 748 749 #define E_EPSILON (1.0e-6) 750 /* how close second ZYZ euler angle must be to 0/PI for degeneration */ 751 752 #define SINGULAR_EPSILON (1.0e-6) 753 /* how close to zero the determinate of a matrix must be for singularity */ 754 755 #define RPY_P_FUZZ (1.0e-6) 756 /* how close pitch is to zero for RPY to degenerate */ 757 758 #define ZYZ_Y_FUZZ (1.0e-6) 759 /* how close Y is to zero for ZYZ Euler to degenerate */ 760 761 #define ZYX_Y_FUZZ (1.0e-6) 762 /* how close Y is to zero for ZYX Euler to degenerate */ 763 764 #define CIRCLE_FUZZ (1.0e-6) 765 /* Bug fix for the missing circles problem */ 766 767 /* debug output printing */ 768 extern void pmPrintError(const char *fmt, ...) __attribute__((format(printf,1,2))); 769 770 /* global error number and errors */ 771 extern int pmErrno; 772 extern void pmPerror(const char *fmt); 773 #define PM_ERR -1 /* unspecified error */ 774 #define PM_IMPL_ERR -2 /* not implemented */ 775 #define PM_NORM_ERR -3 /* arg should have been norm */ 776 #define PM_DIV_ERR -4 /* divide by zero error */ 777 778 /* Scalar functions */ 779 780 extern double pmSqrt(double x); 781 782 /* Translation rep conversion functions */ 783 784 extern int pmCartSphConvert(PmCartesian const * const, PmSpherical * const); 785 extern int pmCartCylConvert(PmCartesian const * const, PmCylindrical * const); 786 extern int pmSphCartConvert(PmSpherical const * const, PmCartesian * const); 787 extern int pmSphCylConvert(PmSpherical const * const, PmCylindrical * const); 788 extern int pmCylCartConvert(PmCylindrical const * const, PmCartesian * const); 789 extern int pmCylSphConvert(PmCylindrical const * const, PmSpherical * const); 790 791 /* Rotation rep conversion functions */ 792 793 extern int pmAxisAngleQuatConvert(PmAxis, double, PmQuaternion * const); 794 795 extern int pmRotQuatConvert(PmRotationVector const * const, PmQuaternion * const); 796 extern int pmRotMatConvert(PmRotationVector const * const, PmRotationMatrix * const); 797 extern int pmRotZyzConvert(PmRotationVector const * const, PmEulerZyz * const); 798 extern int pmRotZyxConvert(PmRotationVector const * const, PmEulerZyx * const); 799 extern int pmRotRpyConvert(PmRotationVector const * const, PmRpy * const); 800 801 extern int pmQuatRotConvert(PmQuaternion const * const, PmRotationVector * const); 802 extern int pmQuatMatConvert(PmQuaternion const * const, PmRotationMatrix * const); 803 extern int pmQuatZyzConvert(PmQuaternion const * const, PmEulerZyz * const); 804 extern int pmQuatZyxConvert(PmQuaternion const * const, PmEulerZyx * const); 805 extern int pmQuatRpyConvert(PmQuaternion const * const, PmRpy * const); 806 807 extern int pmMatRotConvert(PmRotationMatrix const * const, PmRotationVector * const); 808 extern int pmMatQuatConvert(PmRotationMatrix const * const, PmQuaternion * const); 809 extern int pmMatZyzConvert(PmRotationMatrix const * const, PmEulerZyz * const); 810 extern int pmMatZyxConvert(PmRotationMatrix const * const, PmEulerZyx * const); 811 extern int pmMatRpyConvert(PmRotationMatrix const * const, PmRpy * const); 812 813 extern int pmZyzRotConvert(PmEulerZyz const * const, PmRotationVector * const); 814 extern int pmZyzQuatConvert(PmEulerZyz const * const, PmQuaternion * const); 815 extern int pmZyzMatConvert(PmEulerZyz const * const, PmRotationMatrix * const); 816 extern int pmZyzZyxConvert(PmEulerZyz const * const, PmEulerZyx * const); 817 extern int pmZyzRpyConvert(PmEulerZyz const * const, PmRpy * const); 818 819 extern int pmZyxRotConvert(PmEulerZyx const * const, PmRotationVector * const); 820 extern int pmZyxQuatConvert(PmEulerZyx const * const, PmQuaternion * const); 821 extern int pmZyxMatConvert(PmEulerZyx const * const, PmRotationMatrix * const); 822 extern int pmZyxZyzConvert(PmEulerZyx const * const, PmEulerZyz * const); 823 extern int pmZyxRpyConvert(PmEulerZyx const * const, PmRpy * const); 824 825 extern int pmRpyRotConvert(PmRpy const * const, PmRotationVector * const); 826 extern int pmRpyQuatConvert(PmRpy const * const, PmQuaternion * const); 827 extern int pmRpyMatConvert(PmRpy const * const, PmRotationMatrix * const); 828 extern int pmRpyZyzConvert(PmRpy const * const, PmEulerZyz * const); 829 extern int pmRpyZyxConvert(PmRpy const * const, PmEulerZyx * const); 830 831 /* Combined rep conversion functions */ 832 833 extern int pmPoseHomConvert(PmPose const * const, PmHomogeneous* const); 834 835 extern int pmHomPoseConvert(PmHomogeneous const * const, PmPose * const); 836 837 /* Arithmetic functions 838 839 Note: currently, only functions for PmCartesian, PmQuaternion, and 840 PmPose are supported directly. The type conversion functions 841 will be used implicitly when applying arithmetic function 842 to other types. This will be slower and less accurate. Explicit 843 functions can be added incrementally. 844 */ 845 846 /* translation functions */ 847 848 /* NOTE: only Cartesian type supported in C now */ 849 850 extern int pmCartCartCompare(PmCartesian const * const, PmCartesian const * const); 851 extern int pmCartCartDot(PmCartesian const * const, PmCartesian const * const, double * const); 852 extern int pmCartCartCross(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); 853 extern int pmCartCartMult(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); 854 extern int pmCartCartDiv(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); 855 extern int pmCartInfNorm(PmCartesian const * v, double * out); 856 extern int pmCartMag(PmCartesian const * const, double * const); 857 extern int pmCartMagSq(PmCartesian const * const, double * const); 858 extern int pmCartCartDisp(PmCartesian const * const v1, PmCartesian const * const v2, double *d); 859 extern int pmCartCartAdd(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); 860 extern int pmCartCartSub(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); 861 extern int pmCartScalMult(PmCartesian const * const, double, PmCartesian * const); 862 extern int pmCartScalDiv(PmCartesian const * const, double, PmCartesian * const); 863 extern int pmCartNeg(PmCartesian const * const, PmCartesian * const); 864 extern int pmCartUnit(PmCartesian const * const, PmCartesian * const); 865 extern int pmCartAbs(PmCartesian const * const, PmCartesian * const); 866 // Equivalent of compound operators like +=, -=, etc. Basically, these functions work directly on the first PmCartesian 867 extern int pmCartCartAddEq(PmCartesian * const, PmCartesian const * const); 868 extern int pmCartCartSubEq(PmCartesian * const, PmCartesian const * const); 869 extern int pmCartScalMultEq(PmCartesian * const, double); 870 extern int pmCartScalDivEq(PmCartesian * const, double); 871 extern int pmCartUnitEq(PmCartesian * const); 872 extern int pmCartNegEq(PmCartesian * const); 873 /*! \todo Another #if 0 */ 874 #if 0 875 extern int pmCartNorm(PmCartesian const * const v, PmCartesian * const vout); 876 #else 877 // Hopefully guaranteed to cause a compile error when used. 878 #define pmCartNorm(a,b,c,d,e) bad{a.b.c.d.e} 879 #endif 880 881 extern int pmCartIsNorm(PmCartesian const * const v); 882 extern int pmCartInv(PmCartesian const * const, PmCartesian * const); 883 extern int pmCartInvEq(PmCartesian * const); 884 extern int pmCartCartProj(PmCartesian const * const, PmCartesian const * const, PmCartesian * const); 885 extern int pmCartPlaneProj(PmCartesian const * const v, PmCartesian const * const normal, 886 PmCartesian * vout); 887 888 /* rotation functions */ 889 890 /* quaternion functions */ 891 892 extern int pmQuatQuatCompare(PmQuaternion const * const, PmQuaternion const * const); 893 extern int pmQuatMag(PmQuaternion const * const q, double *d); 894 extern int pmQuatNorm(PmQuaternion const * const, PmQuaternion * const); 895 extern int pmQuatInv(PmQuaternion const * const, PmQuaternion * const); 896 extern int pmQuatIsNorm(PmQuaternion const * const); 897 extern int pmQuatScalMult(PmQuaternion const * const q, double s, PmQuaternion * const qout); 898 extern int pmQuatScalDiv(PmQuaternion const * const q, double s, PmQuaternion * const qout); 899 extern int pmQuatQuatMult(PmQuaternion const * const, PmQuaternion const * const, PmQuaternion * const); 900 extern int pmQuatCartMult(PmQuaternion const * const, PmCartesian const * const, PmCartesian * const); 901 extern int pmQuatAxisAngleMult(PmQuaternion const * const, PmAxis, double, 902 PmQuaternion *); 903 904 /* rotation vector functions */ 905 906 extern int pmRotScalMult(PmRotationVector const * const, double, PmRotationVector * const); 907 extern int pmRotScalDiv(PmRotationVector const * const, double, PmRotationVector * const); 908 extern int pmRotIsNorm(PmRotationVector const * const); 909 extern int pmRotNorm(PmRotationVector const * const, PmRotationVector * const); 910 911 /* rotation matrix functions */ 912 913 /* | m.x.x m.y.x m.z.x | */ 914 /* M = | m.x.y m.y.y m.z.y | */ 915 /* | m.x.z m.y.z m.z.z | */ 916 917 extern int pmMatNorm(PmRotationMatrix const * const m, PmRotationMatrix * const mout); 918 extern int pmMatIsNorm(PmRotationMatrix const * const m); 919 extern int pmMatInv(PmRotationMatrix const * const m, PmRotationMatrix * const mout); 920 extern int pmMatCartMult(PmRotationMatrix const * const m, PmCartesian const * const v, 921 PmCartesian * const vout); 922 extern int pmMatMatMult(PmRotationMatrix const * const m1, PmRotationMatrix const * const m2, 923 PmRotationMatrix * const mout); 924 925 /* pose functions*/ 926 927 extern int pmPosePoseCompare(PmPose const * const, PmPose const * const); 928 extern int pmPoseInv(PmPose const * const p, PmPose * const); 929 extern int pmPoseCartMult(PmPose const * const, PmCartesian const * const, PmCartesian * const); 930 extern int pmPosePoseMult(PmPose const * const, PmPose const * const, PmPose * const); 931 932 /* homogeneous functions */ 933 extern int pmHomInv(PmHomogeneous const * const, PmHomogeneous * const); 934 935 /* line functions */ 936 937 extern int pmLineInit(PmLine * const line, PmPose const * const start, PmPose const * const end); 938 extern int pmLinePoint(PmLine const * const line, double len, PmPose * const point); 939 940 /* pure cartesian line functions */ 941 extern int pmCartLineInit(PmCartLine * const line, PmCartesian const * const start, PmCartesian const * const end); 942 extern int pmCartLinePoint(PmCartLine const * const line, double len, PmCartesian * const point); 943 extern int pmCartLineStretch(PmCartLine * const line, double new_len, int from_end); 944 945 /* circle functions */ 946 947 extern int pmCircleInit(PmCircle * const circle, 948 PmCartesian const * const start, PmCartesian const * const end, 949 PmCartesian const * const center, PmCartesian const * const normal, int turn); 950 951 extern int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point); 952 extern int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end); 953 954 /* slicky macros for item-by-item copying between C and C++ structs */ 955 956 #define toCart(src,dst) {(dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} 957 958 #define toCyl(src,dst) {(dst)->theta = (src).theta; (dst)->r = (src).r; (dst)->z = (src).z;} 959 960 #define toSph(src,dst) {(dst)->theta = (src).theta; (dst)->phi = (src).phi; (dst)->r = (src).r;} 961 962 #define toQuat(src,dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} 963 964 #define toRot(src,dst) {(dst)->s = (src).s; (dst)->x = (src).x; (dst)->y = (src).y; (dst)->z = (src).z;} 965 966 #define toMat(src,dst) {toCart((src).x, &((dst)->x)); toCart((src).y, &((dst)->y)); toCart((src).z, &((dst)->z));} 967 968 #define toEulerZyz(src,dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->zp = (src).zp;} 969 970 #define toEulerZyx(src,dst) {(dst)->z = (src).z; (dst)->y = (src).y; (dst)->x = (src).x;} 971 972 #define toRpy(src,dst) {(dst)->r = (src).r; (dst)->p = (src).p; (dst)->y = (src).y;} 973 974 #define toPose(src,dst) {toCart((src).tran, &((dst)->tran)); toQuat((src).rot, &((dst)->rot));} 975 976 #define toHom(src,dst) {toCart((src).tran, &((dst)->tran)); toMat((src).rot, &((dst)->rot));} 977 978 #define toLine(src,dst) {toPose((src).start, &((dst)->start)); toPose((src).end, &((dst)->end)); toCart((src).uVec, &((dst)->uVec));} 979 980 #define toCircle(src,dst) {toCart((src).center, &((dst)->center)); toCart((src).normal, &((dst)->normal)); toCart((src).rTan, &((dst)->rTan)); toCart((src).rPerp, &((dst)->rPerp)); toCart((src).rHelix, &((dst)->rHelix)); (dst)->radius = (src).radius; (dst)->angle = (src).angle; (dst)->spiral = (src).spiral;} 981 982 #ifdef __cplusplus 983 } /* matches extern "C" for C++ */ 984 #endif 985 #endif /* #ifndef POSEMATH_H */