posemath.cc
1 /******************************************************************** 2 * Description: posemath.cc 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 #include "posemath.h" 18 19 #ifdef PM_PRINT_ERROR 20 #define PM_DEBUG // need debug with printing 21 #endif 22 23 // place to reference arrays when bounds are exceeded 24 static double noElement = 0.0; 25 static PM_CARTESIAN *noCart = 0; 26 27 28 // PM_CARTESIAN class 29 30 PM_CARTESIAN::PM_CARTESIAN(double _x, double _y, double _z) 31 { 32 x = _x; 33 y = _y; 34 z = _z; 35 } 36 37 PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_CYLINDRICAL PM_REF c) 38 { 39 PmCylindrical cyl; 40 PmCartesian cart; 41 42 toCyl(c, &cyl); 43 pmCylCartConvert(&cyl, &cart); 44 toCart(cart, this); 45 } 46 47 PM_CARTESIAN::PM_CARTESIAN(PM_CONST PM_SPHERICAL PM_REF s) 48 { 49 PmSpherical sph; 50 PmCartesian cart; 51 52 toSph(s, &sph); 53 pmSphCartConvert(&sph, &cart); 54 toCart(cart, this); 55 } 56 57 double &PM_CARTESIAN::operator [] (int n) { 58 switch (n) { 59 case 0: 60 return x; 61 case 1: 62 return y; 63 case 2: 64 return z; 65 default: 66 return noElement; // need to return a double & 67 } 68 } 69 70 PM_CARTESIAN & PM_CARTESIAN::operator -= (const PM_CARTESIAN &o) { 71 x-=o.x; 72 y-=o.y; 73 z-=o.z; 74 return *this; 75 } 76 PM_CARTESIAN & PM_CARTESIAN::operator += (const PM_CARTESIAN &o) { 77 x+=o.x; 78 y+=o.y; 79 z+=o.z; 80 return *this; 81 } 82 /* 83 const PM_CARTESIAN PM_CARTESIAN::operator+(const PM_CARTESIAN &o) const { 84 PM_CARTESIAN result = *this; 85 result += o; 86 return result; 87 } 88 89 const PM_CARTESIAN PM_CARTESIAN::operator-(const PM_CARTESIAN &o) const { 90 PM_CARTESIAN result = *this; 91 result -= o; 92 return result; 93 } 94 */ 95 96 PM_CARTESIAN & PM_CARTESIAN::operator *= (double o) 97 { 98 x*=o; 99 y*=o; 100 z*=o; 101 return *this; 102 103 } 104 105 PM_CARTESIAN & PM_CARTESIAN::operator /= (double o) 106 { 107 x/=o; 108 y/=o; 109 z/=o; 110 return *this; 111 } 112 113 114 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 115 PM_CARTESIAN::PM_CARTESIAN(PM_CCONST PM_CARTESIAN & v) 116 { 117 x = v.x; 118 y = v.y; 119 z = v.z; 120 } 121 #endif 122 123 // PM_SPHERICAL 124 125 PM_SPHERICAL::PM_SPHERICAL(double _theta, double _phi, double _r) 126 { 127 theta = _theta; 128 phi = _phi; 129 r = _r; 130 } 131 132 PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CARTESIAN PM_REF v) 133 { 134 PmCartesian cart; 135 PmSpherical sph; 136 137 toCart(v, &cart); 138 pmCartSphConvert(&cart, &sph); 139 toSph(sph, this); 140 } 141 142 PM_SPHERICAL::PM_SPHERICAL(PM_CONST PM_CYLINDRICAL PM_REF c) 143 { 144 PmCylindrical cyl; 145 PmSpherical sph; 146 147 toCyl(c, &cyl); 148 pmCylSphConvert(&cyl, &sph); 149 toSph(sph, this); 150 } 151 152 double &PM_SPHERICAL::operator [] (int n) { 153 switch (n) { 154 case 0: 155 return theta; 156 case 1: 157 return phi; 158 case 2: 159 return r; 160 default: 161 return noElement; // need to return a double & 162 } 163 } 164 165 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 166 PM_SPHERICAL::PM_SPHERICAL(PM_CCONST PM_SPHERICAL & s) 167 { 168 theta = s.theta; 169 phi = s.phi; 170 r = s.r; 171 } 172 #endif 173 // PM_CYLINDRICAL 174 175 PM_CYLINDRICAL::PM_CYLINDRICAL(double _theta, double _r, double _z) 176 { 177 theta = _theta; 178 r = _r; 179 z = _z; 180 } 181 182 PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_CARTESIAN PM_REF v) 183 { 184 PmCartesian cart; 185 PmCylindrical cyl; 186 187 toCart(v, &cart); 188 pmCartCylConvert(&cart, &cyl); 189 toCyl(cyl, this); 190 } 191 192 PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CONST PM_SPHERICAL PM_REF s) 193 { 194 PmSpherical sph; 195 PmCylindrical cyl; 196 197 toSph(s, &sph); 198 pmSphCylConvert(&sph, &cyl); 199 toCyl(cyl, this); 200 } 201 202 double &PM_CYLINDRICAL::operator [] (int n) { 203 switch (n) { 204 case 0: 205 return theta; 206 case 1: 207 return r; 208 case 2: 209 return z; 210 default: 211 return noElement; // need to return a double & 212 } 213 } 214 215 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 216 PM_CYLINDRICAL::PM_CYLINDRICAL(PM_CCONST PM_CYLINDRICAL & c) 217 { 218 theta = c.theta; 219 r = c.r; 220 z = c.z; 221 } 222 #endif 223 // PM_ROTATION_VECTOR 224 225 PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(double _s, double _x, 226 double _y, double _z) 227 { 228 PmRotationVector rv; 229 230 rv.s = _s; 231 rv.x = _x; 232 rv.y = _y; 233 rv.z = _z; 234 235 pmRotNorm(&rv, &rv); 236 toRot(rv, this); 237 } 238 239 PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CONST PM_QUATERNION PM_REF q) 240 { 241 PmQuaternion quat; 242 PmRotationVector rv; 243 244 toQuat(q, &quat); 245 pmQuatRotConvert(&quat, &rv); 246 toRot(rv, this); 247 } 248 249 double &PM_ROTATION_VECTOR::operator [] (int n) { 250 switch (n) { 251 case 0: 252 return s; 253 case 1: 254 return x; 255 case 2: 256 return y; 257 case 3: 258 return z; 259 default: 260 return noElement; // need to return a double & 261 } 262 } 263 264 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 265 PM_ROTATION_VECTOR::PM_ROTATION_VECTOR(PM_CCONST PM_ROTATION_VECTOR & r) 266 { 267 s = r.s; 268 x = r.x; 269 y = r.y; 270 z = r.z; 271 } 272 #endif 273 // PM_ROTATION_MATRIX class 274 275 // ctors/dtors 276 277 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(double xx, double xy, double xz, 278 double yx, double yy, double yz, double zx, double zy, double zz) 279 { 280 x.x = xx; 281 x.y = xy; 282 x.z = xz; 283 284 y.x = yx; 285 y.y = yy; 286 y.z = yz; 287 288 z.x = zx; 289 z.y = zy; 290 z.z = zz; 291 292 /*! \todo FIXME-- need a matrix orthonormalization function pmMatNorm() */ 293 } 294 295 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CARTESIAN _x, PM_CARTESIAN _y, 296 PM_CARTESIAN _z) 297 { 298 x = _x; 299 y = _y; 300 z = _z; 301 } 302 303 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_ROTATION_VECTOR PM_REF v) 304 { 305 PmRotationVector rv; 306 PmRotationMatrix mat; 307 308 toRot(v, &rv); 309 pmRotMatConvert(&rv, &mat); 310 toMat(mat, this); 311 } 312 313 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_QUATERNION PM_REF q) 314 { 315 PmQuaternion quat; 316 PmRotationMatrix mat; 317 318 toQuat(q, &quat); 319 pmQuatMatConvert(&quat, &mat); 320 toMat(mat, this); 321 } 322 323 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_RPY PM_REF rpy) 324 { 325 PmRpy _rpy; 326 PmRotationMatrix mat; 327 328 toRpy(rpy, &_rpy); 329 pmRpyMatConvert(&_rpy, &mat); 330 toMat(mat, this); 331 } 332 333 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYZ PM_REF zyz) 334 { 335 PmEulerZyz _zyz; 336 PmRotationMatrix mat; 337 338 toEulerZyz(zyz, &_zyz); 339 pmZyzMatConvert(&_zyz, &mat); 340 toMat(mat, this); 341 } 342 343 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CONST PM_EULER_ZYX PM_REF zyx) 344 { 345 PmEulerZyx _zyx; 346 PmRotationMatrix mat; 347 348 toEulerZyx(zyx, &_zyx); 349 pmZyxMatConvert(&_zyx, &mat); 350 toMat(mat, this); 351 } 352 353 // operators 354 355 PM_CARTESIAN & PM_ROTATION_MATRIX::operator [](int n) { 356 switch (n) { 357 case 0: 358 return x; 359 case 1: 360 return y; 361 case 2: 362 return z; 363 default: 364 if (0 == noCart) { 365 noCart = new PM_CARTESIAN(0.0, 0.0, 0.0); 366 } 367 return (*noCart); // need to return a PM_CARTESIAN & 368 } 369 } 370 371 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 372 PM_ROTATION_MATRIX::PM_ROTATION_MATRIX(PM_CCONST PM_ROTATION_MATRIX & m) 373 { 374 x = m.x; 375 y = m.y; 376 z = m.z; 377 } 378 #endif 379 // PM_QUATERNION class 380 381 PM_QUATERNION::PM_QUATERNION(double _s, double _x, double _y, double _z) 382 { 383 PmQuaternion quat; 384 385 quat.s = _s; 386 quat.x = _x; 387 quat.y = _y; 388 quat.z = _z; 389 390 pmQuatNorm(&quat, &quat); 391 392 s = quat.s; 393 x = quat.x; 394 y = quat.y; 395 z = quat.z; 396 } 397 398 PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_VECTOR PM_REF v) 399 { 400 PmRotationVector rv; 401 PmQuaternion quat; 402 403 toRot(v, &rv); 404 pmRotQuatConvert(&rv, &quat); 405 toQuat(quat, this); 406 } 407 408 PM_QUATERNION::PM_QUATERNION(PM_CONST PM_ROTATION_MATRIX PM_REF m) 409 { 410 PmRotationMatrix mat; 411 PmQuaternion quat; 412 413 toMat(m, &mat); 414 pmMatQuatConvert(&mat, &quat); 415 toQuat(quat, this); 416 } 417 418 PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYZ PM_REF zyz) 419 { 420 PmEulerZyz _zyz; 421 PmQuaternion quat; 422 423 toEulerZyz(zyz, &_zyz); 424 pmZyzQuatConvert(&_zyz, &quat); 425 toQuat(quat, this); 426 } 427 428 PM_QUATERNION::PM_QUATERNION(PM_CONST PM_EULER_ZYX PM_REF zyx) 429 { 430 PmEulerZyx _zyx; 431 PmQuaternion quat; 432 433 toEulerZyx(zyx, &_zyx); 434 pmZyxQuatConvert(&_zyx, &quat); 435 toQuat(quat, this); 436 } 437 438 PM_QUATERNION::PM_QUATERNION(PM_CONST PM_RPY PM_REF rpy) 439 { 440 PmRpy _rpy; 441 PmQuaternion quat; 442 443 toRpy(rpy, &_rpy); 444 pmRpyQuatConvert(&_rpy, &quat); 445 toQuat(quat, this); 446 } 447 448 PM_QUATERNION::PM_QUATERNION(PM_AXIS _axis, double _angle) 449 { 450 PmQuaternion quat; 451 452 pmAxisAngleQuatConvert((PmAxis) _axis, _angle, &quat); 453 toQuat(quat, this); 454 } 455 456 void PM_QUATERNION::axisAngleMult(PM_AXIS _axis, double _angle) 457 { 458 PmQuaternion quat; 459 460 toQuat((*this), &quat); 461 pmQuatAxisAngleMult(&quat, (PmAxis) _axis, _angle, &quat); 462 toQuat(quat, this); 463 } 464 465 double &PM_QUATERNION::operator [] (int n) { 466 switch (n) { 467 case 0: 468 return s; 469 case 1: 470 return x; 471 case 2: 472 return y; 473 case 3: 474 return z; 475 default: 476 return noElement; // need to return a double & 477 } 478 } 479 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 480 PM_QUATERNION::PM_QUATERNION(PM_CCONST PM_QUATERNION & q) 481 { 482 s = q.s; 483 x = q.x; 484 y = q.y; 485 z = q.z; 486 } 487 #endif 488 489 // PM_EULER_ZYZ class 490 491 PM_EULER_ZYZ::PM_EULER_ZYZ(double _z, double _y, double _zp) 492 { 493 z = _z; 494 y = _y; 495 zp = _zp; 496 } 497 498 PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_QUATERNION PM_REF q) 499 { 500 PmQuaternion quat; 501 PmEulerZyz zyz; 502 503 toQuat(q, &quat); 504 pmQuatZyzConvert(&quat, &zyz); 505 toEulerZyz(zyz, this); 506 } 507 508 PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CONST PM_ROTATION_MATRIX PM_REF m) 509 { 510 PmRotationMatrix mat; 511 PmEulerZyz zyz; 512 513 toMat(m, &mat); 514 pmMatZyzConvert(&mat, &zyz); 515 toEulerZyz(zyz, this); 516 } 517 518 double &PM_EULER_ZYZ::operator [] (int n) { 519 switch (n) { 520 case 0: 521 return z; 522 case 1: 523 return y; 524 case 2: 525 return zp; 526 default: 527 return noElement; // need to return a double & 528 } 529 } 530 531 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 532 PM_EULER_ZYZ::PM_EULER_ZYZ(PM_CCONST PM_EULER_ZYZ & zyz) 533 { 534 z = zyz.z; 535 y = zyz.y; 536 zp = zyz.zp; 537 } 538 #endif 539 540 // PM_EULER_ZYX class 541 542 PM_EULER_ZYX::PM_EULER_ZYX(double _z, double _y, double _x) 543 { 544 z = _z; 545 y = _y; 546 x = _x; 547 } 548 549 PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_QUATERNION PM_REF q) 550 { 551 PmQuaternion quat; 552 PmEulerZyx zyx; 553 554 toQuat(q, &quat); 555 pmQuatZyxConvert(&quat, &zyx); 556 toEulerZyx(zyx, this); 557 } 558 559 PM_EULER_ZYX::PM_EULER_ZYX(PM_CONST PM_ROTATION_MATRIX PM_REF m) 560 { 561 PmRotationMatrix mat; 562 PmEulerZyx zyx; 563 564 toMat(m, &mat); 565 pmMatZyxConvert(&mat, &zyx); 566 toEulerZyx(zyx, this); 567 } 568 569 double &PM_EULER_ZYX::operator [] (int n) { 570 switch (n) { 571 case 0: 572 return z; 573 case 1: 574 return y; 575 case 2: 576 return x; 577 default: 578 return noElement; // need to return a double & 579 } 580 } 581 582 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 583 PM_EULER_ZYX::PM_EULER_ZYX(PM_CCONST PM_EULER_ZYX & zyx) 584 { 585 z = zyx.z; 586 y = zyx.y; 587 x = zyx.x; 588 } 589 #endif 590 // PM_RPY class 591 592 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 593 PM_RPY::PM_RPY(PM_CCONST PM_RPY & rpy) 594 { 595 r = rpy.r; 596 p = rpy.p; 597 y = rpy.y; 598 } 599 #endif 600 601 PM_RPY::PM_RPY(double _r, double _p, double _y) 602 { 603 r = _r; 604 p = _p; 605 y = _y; 606 } 607 608 PM_RPY::PM_RPY(PM_CONST PM_QUATERNION PM_REF q) 609 { 610 PmQuaternion quat; 611 PmRpy rpy; 612 613 toQuat(q, &quat); 614 pmQuatRpyConvert(&quat, &rpy); 615 toRpy(rpy, this); 616 } 617 618 PM_RPY::PM_RPY(PM_CONST PM_ROTATION_MATRIX PM_REF m) 619 { 620 PmRotationMatrix mat; 621 PmRpy rpy; 622 623 toMat(m, &mat); 624 pmMatRpyConvert(&mat, &rpy); 625 toRpy(rpy, this); 626 } 627 628 double &PM_RPY::operator [] (int n) { 629 switch (n) { 630 case 0: 631 return r; 632 case 1: 633 return p; 634 case 2: 635 return y; 636 default: 637 return noElement; // need to return a double & 638 } 639 } 640 641 // PM_POSE class 642 643 PM_POSE::PM_POSE(PM_CARTESIAN v, PM_QUATERNION q) 644 { 645 tran.x = v.x; 646 tran.y = v.y; 647 tran.z = v.z; 648 rot.s = q.s; 649 rot.x = q.x; 650 rot.y = q.y; 651 rot.z = q.z; 652 } 653 654 PM_POSE::PM_POSE(double x, double y, double z, 655 double s, double sx, double sy, double sz) 656 { 657 tran.x = x; 658 tran.y = y; 659 tran.z = z; 660 rot.s = s; 661 rot.x = sx; 662 rot.y = sy; 663 rot.z = sz; 664 } 665 666 PM_POSE::PM_POSE(PM_CONST PM_HOMOGENEOUS PM_REF h) 667 { 668 PmHomogeneous hom; 669 PmPose pose; 670 671 toHom(h, &hom); 672 pmHomPoseConvert(&hom, &pose); 673 toPose(pose, this); 674 } 675 676 double &PM_POSE::operator [] (int n) { 677 switch (n) { 678 case 0: 679 return tran.x; 680 case 1: 681 return tran.y; 682 case 2: 683 return tran.z; 684 case 3: 685 return rot.s; 686 case 4: 687 return rot.x; 688 case 5: 689 return rot.y; 690 case 6: 691 return rot.z; 692 default: 693 return noElement; // need to return a double & 694 } 695 } 696 697 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 698 PM_POSE::PM_POSE(PM_CCONST PM_POSE & p) 699 { 700 tran = p.tran; 701 rot = p.rot; 702 } 703 #endif 704 // PM_HOMOGENEOUS class 705 706 PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CARTESIAN v, PM_ROTATION_MATRIX m) 707 { 708 tran = v; 709 rot = m; 710 } 711 712 PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CONST PM_POSE PM_REF p) 713 { 714 PmPose pose; 715 PmHomogeneous hom; 716 717 toPose(p, &pose); 718 pmPoseHomConvert(&pose, &hom); 719 toHom(hom, this); 720 } 721 722 PM_CARTESIAN & PM_HOMOGENEOUS::operator [](int n) { 723 // if it is a rotation vector, stuff 0 as default bottom 724 // if it is a translation vector, stuff 1 as default bottom 725 726 switch (n) { 727 case 0: 728 noElement = 0.0; 729 return rot.x; 730 case 1: 731 noElement = 0.0; 732 return rot.y; 733 case 2: 734 noElement = 0.0; 735 return rot.z; 736 case 3: 737 noElement = 1.0; 738 return tran; 739 default: 740 if (0 == noCart) { 741 noCart = new PM_CARTESIAN(0.0, 0.0, 0.0); 742 } 743 return (*noCart); // need to return a PM_CARTESIAN & 744 } 745 } 746 747 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 748 PM_HOMOGENEOUS::PM_HOMOGENEOUS(PM_CCONST PM_HOMOGENEOUS & h) 749 { 750 tran = h.tran; 751 rot = h.rot; 752 } 753 #endif 754 755 // PM_LINE class 756 757 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 758 PM_LINE::PM_LINE(PM_CCONST PM_LINE & l) 759 { 760 start = l.start; 761 end = l.end; 762 uVec = l.uVec; 763 } 764 #endif 765 766 int PM_LINE::init(PM_POSE start, PM_POSE end) 767 { 768 PmLine _line; 769 PmPose _start, _end; 770 int retval; 771 772 toPose(start, &_start); 773 toPose(end, &_end); 774 775 retval = pmLineInit(&_line, &_start, &_end); 776 777 toLine(_line, this); 778 779 return retval; 780 } 781 782 int PM_LINE::point(double len, PM_POSE * point) 783 { 784 PmLine _line; 785 PmPose _point; 786 int retval; 787 788 toLine(*this, &_line); 789 790 retval = pmLinePoint(&_line, len, &_point); 791 792 toPose(_point, point); 793 794 return retval; 795 } 796 797 // PM_CIRCLE class 798 799 #ifdef INCLUDE_POSEMATH_COPY_CONSTRUCTORS 800 PM_CIRCLE::PM_CIRCLE(PM_CCONST PM_CIRCLE & c) 801 { 802 center = c.center; 803 normal = c.normal; 804 rTan = c.rTan; 805 rPerp = c.rPerp; 806 rHelix = c.rHelix; 807 radius = c.radius; 808 angle = c.angle; 809 spiral = c.spiral; 810 } 811 #endif 812 813 int PM_CIRCLE::init(PM_POSE start, PM_POSE end, 814 PM_CARTESIAN center, PM_CARTESIAN normal, int turn) 815 { 816 PmCircle _circle; 817 PmPose _start, _end; 818 PmCartesian _center, _normal; 819 int retval; 820 821 toPose(start, &_start); 822 toPose(end, &_end); 823 toCart(center, &_center); 824 toCart(normal, &_normal); 825 826 retval = pmCircleInit(&_circle, &_start.tran, &_end.tran, &_center, &_normal, turn); 827 828 toCircle(_circle, this); 829 830 return retval; 831 } 832 833 int PM_CIRCLE::point(double angle, PM_POSE * point) 834 { 835 PmCircle _circle; 836 PmPose _point; 837 int retval; 838 839 toCircle(*this, &_circle); 840 841 retval = pmCirclePoint(&_circle, angle, &_point.tran); 842 843 toPose(_point, point); 844 845 return retval; 846 } 847 848 // overloaded external functions 849 850 // dot 851 852 double dot(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) 853 { 854 double d; 855 PmCartesian _v1, _v2; 856 857 toCart(v1, &_v1); 858 toCart(v2, &_v2); 859 860 pmCartCartDot(&_v1, &_v2, &d); 861 862 return d; 863 } 864 865 // cross 866 867 PM_CARTESIAN cross(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) 868 { 869 PM_CARTESIAN ret; 870 PmCartesian _v1, _v2, _v3; 871 872 toCart(v1, &_v1); 873 toCart(v2, &_v2); 874 875 pmCartCartCross(&_v1, &_v2, &_v3); 876 877 toCart(_v3, &ret); 878 879 return ret; 880 } 881 882 //unit 883 884 PM_CARTESIAN unit(const PM_CARTESIAN &v) 885 { 886 PM_CARTESIAN vout; 887 PmCartesian _v; 888 889 toCart(v, &_v); 890 891 pmCartUnitEq(&_v); 892 893 toCart(_v, &vout); 894 895 return vout; 896 } 897 898 /*! \todo Another #if 0 */ 899 #if 0 900 901 PM_CARTESIAN norm(PM_CARTESIAN v) 902 { 903 PM_CARTESIAN vout; 904 PmCartesian _v; 905 906 toCart(v, &_v); 907 908 pmCartNorm(&_v, &_v); 909 910 toCart(_v, &vout); 911 912 return vout; 913 } 914 915 PM_QUATERNION norm(PM_QUATERNION q) 916 { 917 PM_QUATERNION qout; 918 PmQuaternion _q; 919 920 toQuat(q, &_q); 921 pmQuatNorm(_q, &_q); 922 923 toQuat(_q, &qout); 924 925 return qout; 926 } 927 928 PM_ROTATION_VECTOR norm(PM_ROTATION_VECTOR r) 929 { 930 PM_ROTATION_VECTOR rout; 931 PmRotationVector _r; 932 933 toRot(r, &_r); 934 935 pmRotNorm(_r, &_r); 936 937 toRot(_r, &rout); 938 939 return rout; 940 } 941 942 PM_ROTATION_MATRIX norm(PM_ROTATION_MATRIX m) 943 { 944 PM_ROTATION_MATRIX mout; 945 PmRotationMatrix _m; 946 947 toMat(m, &_m); 948 949 pmMatNorm(_m, &_m); 950 951 toMat(_m, &mout); 952 953 return mout; 954 } 955 #endif 956 957 // isNorm 958 959 int isNorm(PM_CARTESIAN v) 960 { 961 PmCartesian _v; 962 963 toCart(v, &_v); 964 965 return pmCartIsNorm(&_v); 966 } 967 968 int isNorm(PM_QUATERNION q) 969 { 970 PmQuaternion _q; 971 972 toQuat(q, &_q); 973 974 return pmQuatIsNorm(&_q); 975 } 976 977 int isNorm(PM_ROTATION_VECTOR r) 978 { 979 PmRotationVector _r; 980 981 toRot(r, &_r); 982 983 return pmRotIsNorm(&_r); 984 } 985 986 int isNorm(PM_ROTATION_MATRIX m) 987 { 988 PmRotationMatrix _m; 989 990 toMat(m, &_m); 991 992 return pmMatIsNorm(&_m); 993 } 994 995 // mag 996 997 double mag(const PM_CARTESIAN &v) 998 { 999 double ret; 1000 PmCartesian _v; 1001 1002 toCart(v, &_v); 1003 1004 pmCartMag(&_v, &ret); 1005 1006 return ret; 1007 } 1008 1009 // disp 1010 1011 double disp(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) 1012 { 1013 double ret; 1014 PmCartesian _v1, _v2; 1015 1016 toCart(v1, &_v1); 1017 toCart(v2, &_v2); 1018 1019 pmCartCartDisp(&_v1, &_v2, &ret); 1020 1021 return ret; 1022 } 1023 1024 // inv 1025 1026 PM_CARTESIAN inv(const PM_CARTESIAN &v) 1027 { 1028 PM_CARTESIAN ret; 1029 PmCartesian _v; 1030 1031 toCart(v, &_v); 1032 1033 pmCartInv(&_v, &_v); 1034 1035 toCart(_v, &ret); 1036 1037 return ret; 1038 } 1039 1040 PM_ROTATION_MATRIX inv(const PM_ROTATION_MATRIX &m) 1041 { 1042 PM_ROTATION_MATRIX ret; 1043 PmRotationMatrix _m; 1044 1045 toMat(m, &_m); 1046 1047 pmMatInv(&_m, &_m); 1048 1049 toMat(_m, &ret); 1050 1051 return ret; 1052 } 1053 1054 PM_QUATERNION inv(const PM_QUATERNION &q) 1055 { 1056 PM_QUATERNION ret; 1057 PmQuaternion _q; 1058 1059 toQuat(q, &_q); 1060 1061 pmQuatInv(&_q, &_q); 1062 1063 toQuat(_q, &ret); 1064 1065 return ret; 1066 } 1067 1068 PM_POSE inv(const PM_POSE &p) 1069 { 1070 PM_POSE ret; 1071 PmPose _p; 1072 1073 toPose(p, &_p); 1074 1075 pmPoseInv(&_p, &_p); 1076 1077 toPose(_p, &ret); 1078 1079 return ret; 1080 } 1081 1082 PM_HOMOGENEOUS inv(const PM_HOMOGENEOUS &h) 1083 { 1084 PM_HOMOGENEOUS ret; 1085 PmHomogeneous _h; 1086 1087 toHom(h, &_h); 1088 1089 pmHomInv(&_h, &_h); 1090 1091 toHom(_h, &ret); 1092 1093 return ret; 1094 } 1095 1096 // project 1097 1098 PM_CARTESIAN proj(const PM_CARTESIAN &v1, PM_CARTESIAN &v2) 1099 { 1100 PM_CARTESIAN ret; 1101 PmCartesian _v1, _v2; 1102 1103 toCart(v1, &_v1); 1104 toCart(v2, &_v2); 1105 1106 pmCartCartProj(&_v1, &_v2, &_v1); 1107 1108 toCart(_v1, &ret); 1109 1110 return ret; 1111 } 1112 1113 // overloaded arithmetic operators 1114 1115 PM_CARTESIAN operator +(const PM_CARTESIAN &v) 1116 { 1117 return v; 1118 } 1119 1120 PM_CARTESIAN operator -(const PM_CARTESIAN &v) 1121 { 1122 PM_CARTESIAN ret; 1123 1124 ret.x = -v.x; 1125 ret.y = -v.y; 1126 ret.z = -v.z; 1127 1128 return ret; 1129 } 1130 1131 PM_QUATERNION operator +(const PM_QUATERNION &q) 1132 { 1133 return q; 1134 } 1135 1136 PM_QUATERNION operator -(const PM_QUATERNION &q) 1137 { 1138 PM_QUATERNION ret; 1139 PmQuaternion _q; 1140 1141 toQuat(q, &_q); 1142 1143 pmQuatInv(&_q, &_q); 1144 1145 toQuat(_q, &ret); 1146 1147 return ret; 1148 } 1149 1150 PM_POSE operator +(const PM_POSE &p) 1151 { 1152 return p; 1153 } 1154 1155 PM_POSE operator -(const PM_POSE &p) 1156 { 1157 PM_POSE ret; 1158 PmPose _p; 1159 1160 toPose(p, &_p); 1161 1162 pmPoseInv(&_p, &_p); 1163 1164 toPose(_p, &ret); 1165 1166 return ret; 1167 } 1168 1169 int operator ==(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) 1170 { 1171 PmCartesian _v1, _v2; 1172 1173 toCart(v1, &_v1); 1174 toCart(v2, &_v2); 1175 1176 return pmCartCartCompare(&_v1, &_v2); 1177 } 1178 1179 int operator ==(const PM_QUATERNION &q1, PM_QUATERNION &q2) 1180 { 1181 PmQuaternion _q1, _q2; 1182 1183 toQuat(q1, &_q1); 1184 toQuat(q2, &_q2); 1185 1186 return pmQuatQuatCompare(&_q1, &_q2); 1187 } 1188 1189 int operator ==(const PM_POSE &p1, const PM_POSE &p2) 1190 { 1191 PmPose _p1, _p2; 1192 1193 toPose(p1, &_p1); 1194 toPose(p2, &_p2); 1195 1196 return pmPosePoseCompare(&_p1, &_p2); 1197 } 1198 1199 int operator !=(const PM_CARTESIAN &v1, const PM_CARTESIAN &v2) 1200 { 1201 PmCartesian _v1, _v2; 1202 1203 toCart(v1, &_v1); 1204 toCart(v2, &_v2); 1205 1206 return !pmCartCartCompare(&_v1, &_v2); 1207 } 1208 1209 int operator !=(const PM_QUATERNION &q1, const PM_QUATERNION &q2) 1210 { 1211 PmQuaternion _q1, _q2; 1212 1213 toQuat(q1, &_q1); 1214 toQuat(q2, &_q2); 1215 1216 return !pmQuatQuatCompare(&_q1, &_q2); 1217 } 1218 1219 int operator !=(const PM_POSE &p1, const PM_POSE &p2) 1220 { 1221 PmPose _p1, _p2; 1222 1223 toPose(p1, &_p1); 1224 toPose(p2, &_p2); 1225 1226 return !pmPosePoseCompare(&_p1, &_p2); 1227 } 1228 1229 PM_CARTESIAN operator +(PM_CARTESIAN v1, const PM_CARTESIAN &v2) 1230 { 1231 v1 += v2; 1232 return v1; 1233 } 1234 1235 PM_CARTESIAN operator -(PM_CARTESIAN v1, const PM_CARTESIAN &v2) 1236 { 1237 v1 -= v2; 1238 return v1; 1239 } 1240 1241 PM_CARTESIAN operator *(PM_CARTESIAN v, double s) 1242 { 1243 v *= s; 1244 return v; 1245 } 1246 1247 PM_CARTESIAN operator *(double s, PM_CARTESIAN v) 1248 { 1249 v *= s; 1250 return v; 1251 } 1252 1253 PM_CARTESIAN operator /(const PM_CARTESIAN &v, double s) 1254 { 1255 PM_CARTESIAN ret; 1256 1257 #ifdef PM_DEBUG 1258 if (s == 0.0) { 1259 #ifdef PM_PRINT_ERROR 1260 pmPrintError("PM_CARTESIAN::operator / : divide by 0\n"); 1261 #endif 1262 pmErrno = PM_DIV_ERR; 1263 return ret; 1264 } 1265 #endif 1266 1267 ret.x = v.x / s; 1268 ret.y = v.y / s; 1269 ret.z = v.z / s; 1270 1271 return ret; 1272 } 1273 1274 PM_QUATERNION operator *(double s, const PM_QUATERNION &q) 1275 { 1276 PM_QUATERNION qout; 1277 PmQuaternion _q; 1278 1279 toQuat(q, &_q); 1280 1281 pmQuatScalMult(&_q, s, &_q); 1282 1283 toQuat(_q, &qout); 1284 1285 return qout; 1286 } 1287 1288 PM_QUATERNION operator *(const PM_QUATERNION &q, double s) 1289 { 1290 PM_QUATERNION qout; 1291 PmQuaternion _q; 1292 1293 toQuat(q, &_q); 1294 1295 pmQuatScalMult(&_q, s, &_q); 1296 1297 toQuat(_q, &qout); 1298 1299 return qout; 1300 } 1301 1302 PM_QUATERNION operator /(const PM_QUATERNION &q, double s) 1303 { 1304 PM_QUATERNION qout; 1305 PmQuaternion _q; 1306 1307 toQuat(q, &_q); 1308 1309 #ifdef PM_DEBUG 1310 if (s == 0.0) { 1311 #ifdef PM_PRINT_ERROR 1312 pmPrintError("Divide by 0 in operator /\n"); 1313 #endif 1314 pmErrno = PM_NORM_ERR; 1315 1316 /*! \todo Another #if 0 */ 1317 #if 0 1318 // g++/gcc versions 2.8.x and 2.9.x 1319 // will complain that the call to PM_QUATERNION(PM_QUATERNION) is 1320 // ambigous. (2.7.x and some others allow it) 1321 return qout = 1322 PM_QUATERNION((double) 0, (double) 0, (double) 0, (double) 0); 1323 #else 1324 1325 PmQuaternion quat; 1326 1327 quat.s = 0; 1328 quat.x = 0; 1329 quat.y = 0; 1330 quat.z = 0; 1331 1332 pmQuatNorm(&quat, &quat); 1333 1334 qout.s = quat.s; 1335 qout.x = quat.x; 1336 qout.y = quat.y; 1337 qout.z = quat.z; 1338 return qout; 1339 #endif 1340 1341 } 1342 #endif 1343 1344 pmQuatScalMult(&_q, 1.0 / s, &_q); 1345 toQuat(_q, &qout); 1346 1347 pmErrno = 0; 1348 return qout; 1349 } 1350 1351 PM_CARTESIAN operator *(const PM_QUATERNION &q, const PM_CARTESIAN &v) 1352 { 1353 PM_CARTESIAN vout; 1354 PmQuaternion _q; 1355 PmCartesian _v; 1356 1357 toQuat(q, &_q); 1358 toCart(v, &_v); 1359 1360 pmQuatCartMult(&_q, &_v, &_v); 1361 1362 toCart(_v, &vout); 1363 1364 return vout; 1365 } 1366 1367 PM_QUATERNION operator *(const PM_QUATERNION &q1, const PM_QUATERNION &q2) 1368 { 1369 PM_QUATERNION ret; 1370 PmQuaternion _q1, _q2; 1371 1372 toQuat(q1, &_q1); 1373 toQuat(q2, &_q2); 1374 1375 pmQuatQuatMult(&_q1, &_q2, &_q1); 1376 1377 toQuat(_q1, &ret); 1378 1379 return ret; 1380 } 1381 1382 PM_ROTATION_MATRIX operator *(const PM_ROTATION_MATRIX &m1, const PM_ROTATION_MATRIX &m2) 1383 { 1384 PM_ROTATION_MATRIX ret; 1385 PmRotationMatrix _m1, _m2; 1386 1387 toMat(m1, &_m1); 1388 toMat(m2, &_m2); 1389 1390 pmMatMatMult(&_m1, &_m2, &_m1); 1391 1392 toMat(_m1, &ret); 1393 1394 return ret; 1395 } 1396 1397 PM_POSE operator *(const PM_POSE &p1, const PM_POSE &p2) 1398 { 1399 PM_POSE ret; 1400 PmPose _p1, _p2; 1401 1402 toPose(p1, &_p1); 1403 toPose(p2, &_p2); 1404 1405 pmPosePoseMult(&_p1, &_p2, &_p1); 1406 1407 toPose(_p1, &ret); 1408 1409 return ret; 1410 } 1411 1412 PM_CARTESIAN operator *(const PM_POSE &p, const PM_CARTESIAN &v) 1413 { 1414 PM_CARTESIAN ret; 1415 PmPose _p; 1416 PmCartesian _v; 1417 1418 toPose(p, &_p); 1419 toCart(v, &_v); 1420 1421 pmPoseCartMult(&_p, &_v, &_v); 1422 1423 toCart(_v, &ret); 1424 1425 return ret; 1426 }