gomath.h
1 /******************************************************************** 2 * Description: gomath.h 3 * Library file with various functions for working with matrices 4 * 5 * Derived from a work by Fred Proctor, 6 * changed to work with emc2 and HAL 7 * 8 * Adapting Author: Alex Joni 9 * License: LGPL Version 2 10 * System: Linux 11 * 12 ******************************************************************* 13 Similar to posemath, but using different functions. 14 15 TODO: 16 * find the new functions, add them to posemath, convert the rest 17 */ 18 19 #ifndef GO_MATH_H 20 #define GO_MATH_H 21 22 #include <stddef.h> /* sizeof */ 23 #include "rtapi_math.h" /* M_PI */ 24 #include <float.h> /* FLT,DBL_MIN,MAX,EPSILON */ 25 #include "gotypes.h" /* go_integer,real */ 26 27 /*! Returns the square of \a x. */ 28 #define go_sq(x) ((x)*(x)) 29 /*! Returns the cube of \a x. */ 30 #define go_cub(x) ((x)*(x)*(x)) 31 /*! Returns \a x to the fourth power. */ 32 #define go_qua(x) ((x)*(x)*(x)*(x)) 33 34 /*! Returns the sine and cosine of \a x (in radians) in \a s and \a c, 35 respectively. Implemented as a single call when supported, to speed 36 up the calculation of the two values. */ 37 extern void go_sincos(go_real x, go_real * s, go_real * c); 38 39 /*! Returns the cube root of \a x. */ 40 extern go_real go_cbrt(go_real x); 41 42 #ifdef M_PI 43 /*! The value of Pi. */ 44 #define GO_PI M_PI 45 #else 46 /*! The value of Pi. */ 47 #define GO_PI 3.14159265358979323846 48 #endif 49 /*! The value of twice Pi. */ 50 #define GO_2_PI (2.0*GO_PI) 51 52 #ifdef M_PI_2 53 /*! The value of half of Pi. */ 54 #define GO_PI_2 M_PI_2 55 #else 56 /*! The value of half of Pi. */ 57 #define GO_PI_2 1.57079632679489661923 58 #endif 59 60 #ifdef M_PI_4 61 /*! The value of one-fourth of Pi. */ 62 #define GO_PI_4 M_PI_4 63 #else 64 /*! The value of one-fourth of Pi. */ 65 #define GO_PI_4 0.78539816339744830962 66 #endif 67 68 /*! Returns \a rad in radians as its value in degrees. */ 69 #define GO_TO_DEG(rad) ((rad)*57.295779513082323) 70 /*! Returns \a deg in degrees as its value in radians. */ 71 #define GO_TO_RAD(deg) ((deg)*0.0174532925199432952) 72 73 /*! How close translational quantities must be to be equal. */ 74 #define GO_TRAN_CLOSE(x,y) (fabs((x)-(y)) < GO_REAL_EPSILON) 75 /*! How small a translational quantity must be to be zero. */ 76 #define GO_TRAN_SMALL(x) (fabs(x) < GO_REAL_EPSILON) 77 78 /*! How close rotational quantities must be to be equal. */ 79 #define GO_ROT_CLOSE(x,y) (fabs((x)-(y)) < GO_REAL_EPSILON) 80 /*! How small a rotational quantity must be to be zero. */ 81 #define GO_ROT_SMALL(x) (fabs(x) < GO_REAL_EPSILON) 82 83 /*! How close general quantities must be to be equal. Use this when 84 you have something other than translational or rotational quantities, 85 otherwise use one of \a GO_TRAN,ROT_CLOSE. */ 86 #define GO_CLOSE(x,y) (fabs((x)-(y)) < GO_REAL_EPSILON) 87 /*! How small a general quantity must be to be zero. Use this when 88 you have something other than a translational or rotational quantity, 89 otherwise use one of \a GO_TRAN,ROT_SMALL. */ 90 #define GO_SMALL(x) (fabs(x) < GO_REAL_EPSILON) 91 92 /*! A point or vector in Cartesian coordinates. */ 93 typedef struct { 94 go_real x; 95 go_real y; 96 go_real z; 97 } go_cart; 98 99 /*! A point or vector in spherical coordinates, with \a phi as 100 the angle down from the zenith, not up from the XY plane. */ 101 typedef struct { 102 go_real theta; 103 go_real phi; 104 go_real r; 105 } go_sph; 106 107 /*! A point or vector in cylindrical coordinates. */ 108 typedef struct { 109 go_real theta; 110 go_real r; 111 go_real z; 112 } go_cyl; 113 114 /*! A rotation vector, whose direction points along the axis of positive 115 rotation, and whose magnitude is the amount of rotation around this 116 axis, in radians. */ 117 typedef struct { 118 go_real x; 119 go_real y; 120 go_real z; 121 } go_rvec; 122 123 /* | m.x.x m.y.x m.z.x | */ 124 /* go_mat m = | m.x.y m.y.y m.z.y | */ 125 /* | m.x.z m.y.z m.z.z | */ 126 127 /*! A rotation matrix. */ 128 typedef struct { 129 go_cart x; /*!< X unit vector */ 130 go_cart y; /*!< Y unit vector */ 131 go_cart z; /*!< Z unit vector */ 132 } go_mat; 133 134 /*! A quaternion. \a s is the cosine of the half angle of rotation, 135 and the \a xyz elements comprise the vector that points in the direction 136 of positive rotation and whose magnitude is the sine of the half 137 angle of rotation. */ 138 typedef struct { 139 go_real s; 140 go_real x; 141 go_real y; 142 go_real z; 143 } go_quat; 144 145 /*! ZYZ Euler angles. \a z is the amount of the first rotation around the 146 Z axis. \a y is the amount of the second rotation around the new \a Y 147 axis. \a zp is the amount of the third rotation around the new \a Z axis. */ 148 typedef struct { 149 go_real z; 150 go_real y; 151 go_real zp; 152 } go_zyz; 153 154 /*! ZYX Euler angles. \a z is the amount of the first rotation around the 155 Z axis. \a y is the amount of the second rotation around the new \a Y 156 axis. \a x is the amount of the third rotation around the new \a X axis. */ 157 typedef struct { 158 go_real z; 159 go_real y; 160 go_real x; 161 } go_zyx; 162 163 /*! Roll-pitch-yaw angles. \a r is the amount of the first rotation 164 (roll) around the X axis. \a p is the amount of the second rotation 165 (pitch) around the original Y axis. \a y is the amount of the third 166 rotation (yaw) around the original Z axis. */ 167 typedef struct { 168 go_real r; 169 go_real p; 170 go_real y; 171 } go_rpy; 172 173 /*! 174 A \a go_pose represents the Cartesian position vector and quaternion 175 orientation of a frame. 176 */ 177 typedef struct { 178 go_cart tran; 179 go_quat rot; 180 } go_pose; 181 182 /*! 183 A \a go_screw represents the linear- and angular velocity vectors 184 of a frame. \a v is the Cartesian linear velocity vector. 185 \a w is the Cartesian angular velocity vector, the instantaneous 186 vector about which the frame is rotating. 187 */ 188 typedef struct { 189 go_cart v; 190 go_cart w; 191 } go_screw; 192 193 /*! Convenience function that returns a \a go_pose given individual 194 elements. */ 195 extern go_pose 196 go_pose_this(go_real x, go_real y, go_real z, 197 go_real rs, go_real rx, go_real ry, go_real rz); 198 199 /*! Returns the zero vector. */ 200 extern go_cart 201 go_cart_zero(void); 202 203 /*! Returns the identity (zero) quaternion, i.e., no rotation. */ 204 extern go_quat 205 go_quat_identity(void); 206 207 /*! Returns the identity pose, no translation or rotation. */ 208 extern go_pose 209 go_pose_identity(void); 210 211 typedef struct { 212 go_cart tran; 213 go_mat rot; 214 } go_hom; 215 216 /* lines, planes and related functions */ 217 218 /*! 219 Lines are represented in point-direction form 220 (point p, direction v) as 221 (x - px)/vx = (y - py)/vy = (z - pz)vz 222 */ 223 typedef struct { 224 go_cart point; 225 go_cart direction; /* always a unit vector */ 226 } go_line; 227 228 /*! 229 Given a plane as Ax + By + Cz + D = 0, the normal vector 230 \a normal is the Cartesian vector (A,B,C), and the number 231 \a d is the value D. 232 233 Planes have a handedness, given by the direction of the normal 234 vector, so two planes that appear coincident may be different by the 235 direction of their anti-parallel normal vectors. 236 */ 237 typedef struct { 238 go_cart normal; 239 go_real d; 240 } go_plane; 241 242 /*! Fills in \a line given \a point and \a direction. Returns GO_RESULT_OK 243 if \a direction is non-zero, otherwise GO_RESULT_ERROR. */ 244 extern int go_line_from_point_direction(const go_cart * point, const go_cart * direction, go_line * line); 245 246 /*! Fills in \a line given two points. Returns GO_RESULT_OK if the points 247 are different, otherwise GO_RESULT_ERROR. */ 248 extern int go_line_from_points(const go_cart * point1, const go_cart * point2, go_line * line); 249 250 /*! Fill in \a line with the intersection of the two planes. Returns GO_RESULT_OK if the planes are not parallel, otherwise GO_RESULT_ERROR. */ 251 extern int go_line_from_planes(const go_plane * plane1, const go_plane * plane2, go_line * line); 252 253 /*! Returns non-zero if the lines are the same, otherwise zero. */ 254 extern go_flag go_line_line_compare(const go_line * line1, const go_line * line2); 255 256 /*! Fills in \a point with the point located distance \a d along \a line */ 257 extern int go_line_evaluate(const go_line * line, go_real d, go_cart * point); 258 259 /*! Fills in \a distance with the distance from \a point to \a line */ 260 extern int go_point_line_distance(const go_cart * point, const go_line * line, go_real * distance); 261 262 /*! Fills in \a pout with the nearest point on \a line to \a point */ 263 extern int go_point_line_proj(const go_cart * point, const go_line * line, go_cart * pout); 264 265 /*! Fills in \a proj with the projection of \a point onto \a plane */ 266 extern int go_point_plane_proj(const go_cart * point, const go_plane * plane, go_cart * proj); 267 268 /*! Fills in \a proj with the projection of \a line onto \a plane */ 269 extern int go_line_plane_proj(const go_line * line, const go_plane * plane, go_line * proj); 270 271 /*! Fills in \a plane give a \a point on the plane and the normal \a direction. */ 272 extern int go_plane_from_point_normal(const go_cart * point, const go_cart * normal, go_plane * plane); 273 274 /*! Fills in \a plane given the A, B, C and D values in the canonical 275 form Ax + By + Cz + D = 0. Returns GO_RESULT_OK 276 if not all of A, B and C are zero, otherwise GO_RESULT_ERROR. */ 277 extern int go_plane_from_abcd(go_real A, go_real B, go_real C, go_real D, go_plane * plane); 278 279 /*! Fills in \a plane given three points. Returns GO_RESULT_OK 280 if the points are distinct, otherwise GO_RESULT_ERROR. */ 281 extern int go_plane_from_points(const go_cart * point1, const go_cart * point2, const go_cart * point3, go_plane * plane); 282 283 /*! Fills in \a plane given a \a point on the plane and a \a line 284 in the plane. Returns GO_RESULT_OK if the point is not on the line, 285 GO_RESULT_ERROR otherwise. */ 286 extern int go_plane_from_point_line(const go_cart * point, const go_line * line, go_plane * plane); 287 288 /*! Returns non-zero if the planes are coincident and have the same 289 normal direction, otherwise zero. */ 290 extern go_flag go_plane_plane_compare(const go_plane * plane1, const go_plane * plane2); 291 292 /*! Fills in the \a distance from the \a point to the \a plane. */ 293 extern int go_point_plane_distance(const go_cart * point, const go_plane * plane, go_real * distance); 294 295 /*! Fills in \a point with the point located distances \a u and \a v along 296 some othogonal planar coordinate system in \a plane */ 297 extern int go_plane_evaluate(const go_plane * plane, go_real u, go_real v, go_cart * point); 298 299 /*! Fills in \a point with the intersection point of 300 \a line with \a plane, and \a distance with the distance along the 301 line to the intersection point. Returns GO_RESULT_ERROR if the line 302 is parallel to the plane and not lying in the plane, otherwise 303 GO_RESULT_OK. */ 304 extern int go_line_plane_intersect(const go_line * line, const go_plane * plane, go_cart * point, go_real * distance); 305 306 /* 307 struct arguments are passed to functions as const pointers since the 308 speed is at least as fast for all but structs of one or two elements. 309 */ 310 311 /* translation rep conversion functions */ 312 313 extern int go_cart_sph_convert(const go_cart *, go_sph *); 314 extern int go_cart_cyl_convert(const go_cart *, go_cyl *); 315 extern int go_sph_cart_convert(const go_sph *, go_cart *); 316 extern int go_sph_cyl_convert(const go_sph *, go_cyl *); 317 extern int go_cyl_cart_convert(const go_cyl *, go_cart *); 318 extern int go_cyl_sph_convert(const go_cyl *, go_sph *); 319 320 /* rotation rep conversion functions */ 321 322 extern int go_rvec_quat_convert(const go_rvec *, go_quat *); 323 extern int go_rvec_mat_convert(const go_rvec *, go_mat *); 324 extern int go_rvec_zyz_convert(const go_rvec *, go_zyz *); 325 extern int go_rvec_zyx_convert(const go_rvec *, go_zyx *); 326 extern int go_rvec_rpy_convert(const go_rvec *, go_rpy *); 327 328 extern int go_quat_rvec_convert(const go_quat *, go_rvec *); 329 extern int go_quat_mat_convert(const go_quat *, go_mat *); 330 extern int go_quat_zyz_convert(const go_quat *, go_zyz *); 331 extern int go_quat_zyx_convert(const go_quat *, go_zyx *); 332 extern int go_quat_rpy_convert(const go_quat *, go_rpy *); 333 334 extern int go_mat_rvec_convert(const go_mat *, go_rvec *); 335 extern int go_mat_quat_convert(const go_mat *, go_quat *); 336 extern int go_mat_zyz_convert(const go_mat *, go_zyz *); 337 extern int go_mat_zyx_convert(const go_mat *, go_zyx *); 338 extern int go_mat_rpy_convert(const go_mat *, go_rpy *); 339 340 extern int go_zyz_rvec_convert(const go_zyz *, go_rvec *); 341 extern int go_zyz_quat_convert(const go_zyz *, go_quat *); 342 extern int go_zyz_mat_convert(const go_zyz *, go_mat *); 343 extern int go_zyz_zyx_convert(const go_zyz *, go_zyx *); 344 extern int go_zyz_rpy_convert(const go_zyz *, go_rpy *); 345 346 extern int go_zyx_rvec_convert(const go_zyx *, go_rvec *); 347 extern int go_zyx_quat_convert(const go_zyx *, go_quat *); 348 extern int go_zyx_mat_convert(const go_zyx *, go_mat *); 349 extern int go_zyx_zyz_convert(const go_zyx *, go_zyz *); 350 extern int go_zyx_rpy_convert(const go_zyx *, go_rpy *); 351 352 extern int go_rpy_rvec_convert(const go_rpy *, go_rvec *); 353 extern int go_rpy_quat_convert(const go_rpy *, go_quat *); 354 extern int go_rpy_mat_convert(const go_rpy *, go_mat *); 355 extern int go_rpy_zyz_convert(const go_rpy *, go_zyz *); 356 extern int go_rpy_zyx_convert(const go_rpy *, go_zyx *); 357 358 /* combined rep conversion functions */ 359 360 extern int go_pose_hom_convert(const go_pose *, go_hom *); 361 extern int go_hom_pose_convert(const go_hom *, go_pose *); 362 363 /* misc conversion functions */ 364 /*! 365 go_cart_rvec_convert and go_rvec_cart_convert convert between 366 Cartesian vectors and rotation vectors. The conversion is trivial 367 but keeps types distinct. 368 */ 369 extern int go_cart_rvec_convert(const go_cart * cart, go_rvec * rvec); 370 extern int go_rvec_cart_convert(const go_rvec * rvec, go_cart * cart); 371 372 /* translation functions, that work only with the preferred 373 go_cart type. Other types must be converted to go_cart 374 to use these, e.g., there's no go_sph_cyl_compare() */ 375 376 extern go_flag go_cart_cart_compare(const go_cart *, const go_cart *); 377 extern int go_cart_cart_dot(const go_cart *, const go_cart *, 378 go_real *); 379 extern int go_cart_cart_cross(const go_cart *, const go_cart *, 380 go_cart *); 381 extern int go_cart_mag(const go_cart *, go_real *); 382 extern int go_cart_magsq(const go_cart *, go_real *); 383 extern go_flag go_cart_cart_par(const go_cart *, const go_cart *); 384 extern go_flag go_cart_cart_perp(const go_cart *, const go_cart *); 385 386 /*! Places the Cartesian displacement between two vectors \a v1 and 387 \a v2 in \a disp, returning \a GO_RESULT_OK. */ 388 extern int go_cart_cart_disp(const go_cart * v1, 389 const go_cart * v2, 390 go_real * disp); 391 392 extern int go_cart_cart_add(const go_cart *, const go_cart *, 393 go_cart *); 394 extern int go_cart_cart_sub(const go_cart *, const go_cart *, 395 go_cart *); 396 extern int go_cart_scale_mult(const go_cart *, go_real, go_cart *); 397 extern int go_cart_neg(const go_cart *, go_cart *); 398 extern int go_cart_unit(const go_cart *, go_cart *); 399 400 /*! 401 Given two non-zero vectors \a v1 and \a v2, fill in \a quat with 402 the minimum rotation that brings \a v1 to \a v2. 403 */ 404 extern int go_cart_cart_rot(const go_cart * v1, 405 const go_cart * v2, 406 go_quat * quat); 407 408 /*! 409 Project vector \a v1 onto \a v2, with the resulting vector placed 410 into \a vout. Returns GO_RESULT_OK if it can be done, otherwise 411 something else. 412 */ 413 extern int go_cart_cart_proj(const go_cart * v1, const go_cart * v2, 414 go_cart * vout); 415 extern int go_cart_plane_proj(const go_cart *, const go_cart *, 416 go_cart *); 417 extern int go_cart_cart_angle(const go_cart *, const go_cart *, 418 go_real *); 419 /*! 420 go_cart_normal finds one of the infinite vectors perpendicular 421 to \a v, putting the result in \a vout. 422 */ 423 extern int go_cart_normal(const go_cart * v, go_cart * vout); 424 extern int go_cart_centroid(const go_cart * varray, 425 go_integer num, 426 go_cart * centroid); 427 extern int go_cart_centroidize(const go_cart * vinarray, 428 go_integer num, 429 go_cart * centroid, 430 go_cart * voutarray); 431 extern int go_cart_cart_pose(const go_cart *, const go_cart *, 432 go_cart *, go_cart *, 433 go_integer, go_pose *); 434 435 /*! 436 Returns the Cartesian point \a p whose distances from three other points 437 \a c1, \a c2 and \a c3 are \a l1, \a l2 and \a l3, respectively. In 438 general there are 0, 1 or two points possible. If no point is possible, 439 this returns GO_RESULT_ERROR, otherwise the points are returned in \a 440 p1 and \a p2, which may be the same point. 441 */ 442 443 int go_cart_trilaterate(const go_cart * c1, 444 const go_cart * c2, 445 const go_cart * c3, 446 go_real l1, 447 go_real l2, 448 go_real l3, 449 go_cart * p1, 450 go_cart * p2); 451 452 /* quat functions */ 453 454 extern go_flag go_quat_quat_compare(const go_quat *, const go_quat *); 455 extern int go_quat_mag(const go_quat *, go_real *); 456 457 /*! 458 go_quat_unit takes a quaternion rotation \a q and converts it into 459 a unit rotation about the same axis, \a qout. 460 */ 461 extern int go_quat_unit(const go_quat * q, go_quat * qout); 462 extern int go_quat_norm(const go_quat *, go_quat *); 463 extern int go_quat_inv(const go_quat *, go_quat *); 464 extern go_flag go_quat_is_norm(const go_quat *); 465 extern int go_quat_scale_mult(const go_quat *, go_real, go_quat *); 466 extern int go_quat_quat_mult(const go_quat *, const go_quat *, 467 go_quat *); 468 extern int go_quat_cart_mult(const go_quat *, const go_cart *, 469 go_cart *); 470 471 /* rotation vector functions */ 472 473 extern go_flag go_rvec_rvec_compare(const go_rvec * r1, const go_rvec * r2); 474 extern int go_rvec_scale_mult(const go_rvec *, go_real, go_rvec *); 475 476 /* rotation matrix functions */ 477 478 /* | m.x.x m.y.x m.z.x | */ 479 /* M = | m.x.y m.y.y m.z.y | */ 480 /* | m.x.z m.y.z m.z.z | */ 481 482 /*! 483 Normalizes rotation matrix \a m so that all columns are mutually 484 perpendicular unit vectors, placing the result in \a mout. 485 */ 486 extern int go_mat_norm(const go_mat * m, go_mat * mout); 487 488 extern go_flag go_mat_is_norm(const go_mat *); 489 extern int go_mat_inv(const go_mat *, go_mat *); 490 extern int go_mat_cart_mult(const go_mat *, const go_cart *, go_cart *); 491 extern int go_mat_mat_mult(const go_mat *, const go_mat *, go_mat *); 492 493 /* pose functions*/ 494 495 extern go_flag go_pose_pose_compare(const go_pose *, const go_pose *); 496 extern int go_pose_inv(const go_pose *, go_pose *); 497 extern int go_pose_cart_mult(const go_pose *, const go_cart *, go_cart *); 498 extern int go_pose_pose_mult(const go_pose *, const go_pose *, go_pose *); 499 extern int go_pose_scale_mult(const go_pose *, go_real, go_pose *); 500 501 /*! Given two times \a t1 and \a t2, and associated poses \a p1 and \a 502 p2, interpolates (or extrapolates) to find pose \a p3 at time \a 503 t3. The result is stored in \a p3. Returns GO_RESULT_OK if \a t1 and 504 \a t2 are distinct and \a p1 and \a p2 are valid poses, otherwise it 505 can't interpolate and returns a relevant error. */ 506 extern int 507 go_pose_pose_interp(go_real t1, 508 const go_pose * p1, 509 go_real t2, 510 const go_pose * p2, 511 go_real t3, 512 go_pose * p3); 513 514 /* homogeneous transform functions */ 515 516 extern int go_hom_inv(const go_hom *, go_hom *); 517 518 /* screw functions */ 519 520 /*! Given \a pose transformation from frame A to B, and a screw \a screw 521 in frame A, transform the screw into frame B and place in \a out. */ 522 extern int go_pose_screw_mult(const go_pose * pose, const go_screw * screw, go_screw * out); 523 524 /* declarations for general MxN matrices */ 525 526 /*! 527 Declare a matrix variable \a m with \a rows rows and \a cols columns. 528 Allocates \a rows X \a columns of space in \a mspace. 529 */ 530 531 typedef go_real go_vector; 532 533 typedef struct { 534 go_integer rows; 535 go_integer cols; 536 go_real ** el; 537 go_real ** elcpy; 538 go_real * v; 539 go_integer * index; 540 } go_matrix; 541 542 #define GO_MATRIX_DECLARE(M,Mspace,_rows,_cols) \ 543 go_matrix M = {0, 0, 0, 0, 0, 0}; \ 544 struct { \ 545 go_real * el[_rows]; \ 546 go_real * elcpy[_rows]; \ 547 go_real stg[_rows][_cols]; \ 548 go_real stgcpy[_rows][_cols]; \ 549 go_real v[_rows]; \ 550 go_integer index[_rows]; \ 551 } Mspace 552 553 #define go_matrix_init(M,Mspace,_rows,_cols) \ 554 M.el = Mspace.el; \ 555 M.elcpy = Mspace.elcpy; \ 556 for (M.rows = 0; M.rows < (_rows); M.rows++) { \ 557 M.el[M.rows] = Mspace.stg[M.rows]; \ 558 M.elcpy[M.rows] = Mspace.stgcpy[M.rows]; \ 559 } \ 560 M.rows = (_rows); \ 561 M.cols = (_cols); \ 562 M.v = Mspace.v; \ 563 M.index = Mspace.index 564 565 extern go_real 566 go_get_singular_epsilon(void); 567 568 extern int 569 go_set_singular_epsilon(go_real epsilon); 570 571 extern int 572 ludcmp(go_real ** a, 573 go_real * scratchrow, 574 go_integer n, 575 go_integer * indx, 576 go_real * d); 577 578 extern int 579 lubksb(go_real ** a, 580 go_integer n, 581 go_integer * indx, 582 go_real * b); 583 584 /* MxN matrix, Mx1 vector functions */ 585 586 extern int 587 go_cart_vector_convert(const go_cart * c, 588 go_vector * v); 589 extern int 590 go_vector_cart_convert(const go_real * v, 591 go_cart * c); 592 593 extern int 594 go_quat_matrix_convert(const go_quat * quat, 595 go_matrix * matrix); 596 597 extern int 598 go_mat_matrix_convert(const go_mat * mat, 599 go_matrix * matrix); 600 601 extern int 602 go_matrix_matrix_add(const go_matrix * a, 603 const go_matrix * b, 604 go_matrix * apb); 605 606 extern int 607 go_matrix_matrix_copy(const go_matrix * src, 608 go_matrix * dst); 609 610 extern int 611 go_matrix_matrix_mult(const go_matrix * a, 612 const go_matrix * b, 613 go_matrix * ab); 614 615 extern int 616 go_matrix_vector_mult(const go_matrix * a, 617 const go_vector * v, 618 go_vector * av); 619 620 /*! 621 The matrix-vector cross product is a matrix of the same dimension, 622 whose columns are the column-wise cross products of the matrix 623 and the vector. The matrices must be 3xN, the vector 3x1. 624 */ 625 extern int 626 go_matrix_vector_cross(const go_matrix * a, 627 const go_vector * v, 628 go_matrix * axv); 629 630 extern int 631 go_matrix_transpose(const go_matrix * a, 632 go_matrix * at); 633 634 extern int 635 go_matrix_inv(const go_matrix * a, 636 go_matrix * ainv); 637 638 /* Square matrix functions, where matN is an NxN matrix, and vecN 639 is an Nx1 vector */ 640 641 /* Optimized 3x3 functions */ 642 extern int go_mat3_inv(const go_real a[3][3], 643 go_real ainv[3][3]); 644 extern int go_mat3_mat3_mult(const go_real a[3][3], 645 const go_real b[3][3], 646 go_real axb[3][3]); 647 extern int go_mat3_vec3_mult(const go_real a[3][3], 648 const go_real v[3], 649 go_real axv[3]); 650 651 /* Optimized 4x4 functions */ 652 extern int go_mat4_inv(const go_real a[4][4], 653 go_real ainv[4][4]); 654 extern int go_mat4_mat4_mult(const go_real a[4][4], 655 const go_real b[4][4], 656 go_real axb[4][4]); 657 extern int go_mat4_vec4_mult(const go_real a[4][4], 658 const go_real v[4], 659 go_real axv[4]); 660 661 /*! 662 Given a 6x6 matrix \a a, computes the inverse and returns it in 663 \a ainv. Leaves \a a untouched. Returns GO_RESULT_OK if there is an 664 inverse, else GO_RESULT_SINGULAR if the matrix is singular. 665 */ 666 extern int go_mat6_inv(const go_real a[6][6], 667 go_real ainv[6][6]); 668 669 /*! 670 Given two 6x6 matrices \a a and \a b, multiplies them and returns 671 the result in \a axb. Leaves \a a and \a b untouched. 672 Returns GO_RESULT_OK. 673 */ 674 extern int go_mat6_mat6_mult(const go_real a[6][6], 675 const go_real b[6][6], 676 go_real axb[6][6]); 677 678 /*! 679 Given a 6x6 matrix \a a and a 6x1 vector \a v, multiplies them 680 and returns the result in \a axv. Leaves \a a and \a v untouched. 681 Returns GO_RESULT_OK. 682 */ 683 extern int go_mat6_vec6_mult(const go_real a[6][6], 684 const go_real v[6], 685 go_real axv[6]); 686 687 /* Denavit-Hartenberg to pose conversions */ 688 689 /* 690 The link frams is assumed to be 691 692 | i-1 693 | T 694 | i 695 696 that is, elements of the link frame expressed wrt the 697 previous link frame. 698 */ 699 700 /*! 701 These DH parameters follow the convention in John J. Craig, 702 _Introduction to Robotics: Mechanics and Control_. 703 */ 704 typedef struct { 705 go_real a; /*< a[i-1] */ 706 go_real alpha; /*< alpha[i-1] */ 707 /* either d or theta are the free variable, depending on quantity */ 708 go_real d; /*< d[i] */ 709 go_real theta; /*< theta[i] */ 710 } go_dh; 711 712 /*! 713 PK parameters are used for parallel kinematic mechanisms, and 714 represent the Cartesian positions of the ends of the link in the 715 stationary base frame and the moving platform frame. Currently this 716 only supports prismatic links. 717 */ 718 typedef struct { 719 go_cart base; /*< position of fixed end in base frame */ 720 go_cart platform; /*< position of moving end in platform frame */ 721 go_real d; /*< the length of the link */ 722 } go_pk; 723 724 /*! 725 PP parameters represent the pose of the link with respect to the 726 previous link. Revolute joints rotate about the Z axis, prismatic 727 joints slide along the Z axis. 728 */ 729 typedef struct { 730 go_pose pose; /*< the pose of the link wrt to the previous link */ 731 } go_pp; 732 733 /*! Types of link parameter representations */ 734 enum { 735 GO_LINK_DH = 1, /*< for Denavit-Hartenberg params */ 736 GO_LINK_PK, /*< for parallel kinematics */ 737 GO_LINK_PP /*< for serial kinematics */ 738 }; 739 740 #define go_link_to_string(L) \ 741 (L) == GO_LINK_DH ? "DH" : \ 742 (L) == GO_LINK_PK ? "PK" : \ 743 (L) == GO_LINK_PP ? "PP" : "None" 744 745 /*! 746 This is the generic link structure for PKM sliding/cable links and 747 serial revolute/prismatic links. 748 */ 749 typedef struct { 750 union { 751 go_dh dh; /*< if you have DH params and don't want to convert to PP */ 752 go_pk pk; /*< if you have a parallel machine, e.g., hexapod or robot crane */ 753 go_pp pp; /*< if you have a serial machine, e.g., an industrial robot */ 754 } u; 755 go_flag type; /*< one of GO_LINK_DH,PK,PP */ 756 go_flag quantity; /*< one of GO_QUANTITY_LENGTH,ANGLE */ 757 } go_link; 758 759 /*! 760 Converts DH parameters in \a dh to their pose equivalent, stored 761 in \a pose. 762 */ 763 extern int go_dh_pose_convert(const go_dh * dh, go_pose * pose); 764 765 /*! 766 Converts \a pose to the equivalent DH parameters, stored in \a dh. 767 Warning! Conversion from these DH parameters back to a pose via \a 768 go_dh_pose_convert will NOT in general result in the same 769 pose. Poses have 6 degrees of freedom, DH parameters have 4, and 770 conversion to DH parameters loses some information. The source of 771 this information loss is the convention imposed on DH parameters for 772 choice of X-Y-Z axis directions. With poses, there is no such 773 convention, and poses are thus freer than DH parameters. 774 */ 775 extern int go_pose_dh_convert(const go_pose * pose, go_dh * dh); 776 777 /*! 778 Fixes the link in \a link to its value when the joint 779 variable is \a joint, storing the result in \a linkout. 780 */ 781 extern int go_link_joint_set(const go_link * link, go_real joint, go_link * linkout); 782 783 /*! 784 Takes the link description of the device in \a links, and the number 785 of these in \a num, and builds the pose of the device and stores in 786 \a pose. \a links should have the value of the free link parameter 787 filled in with the current joint value, e.g., with a prior call to 788 go_link_joint_set. 789 */ 790 extern int go_link_pose_build(const go_link * links, go_integer num, go_pose * pose); 791 792 typedef struct { 793 go_real re; 794 go_real im; 795 } go_complex; 796 797 extern go_complex go_complex_add(go_complex z1, go_complex z2); 798 extern go_complex go_complex_sub(go_complex z1, go_complex z2); 799 extern go_complex go_complex_mult(go_complex z1, go_complex z2); 800 extern go_complex go_complex_div(go_complex z1, go_complex z2, int * result); 801 extern go_complex go_complex_scale(go_complex z, go_real scale); 802 extern go_real go_complex_mag(go_complex z); 803 extern go_real go_complex_arg(go_complex z); 804 extern void go_complex_sqrt(go_complex z, go_complex * z1, go_complex * z2); 805 extern void go_complex_cbrt(go_complex z, go_complex * z1, go_complex * z2, go_complex * z3); 806 807 typedef struct { 808 /* leading coefficient is 1, x^2 + ax + b = 0 */ 809 go_real a; 810 go_real b; 811 } go_quadratic; 812 813 typedef struct { 814 /* leading coefficient is 1, x^3 + ax^2 + bx + c = 0 */ 815 go_real a; 816 go_real b; 817 go_real c; 818 } go_cubic; 819 820 typedef struct { 821 /* leading coefficient is 1, x^4 + ax^3 + bx^2 + cx + d = 0 */ 822 go_real a; 823 go_real b; 824 go_real c; 825 go_real d; 826 } go_quartic; 827 828 extern int go_quadratic_solve(const go_quadratic * quad, 829 go_complex * z1, 830 go_complex * z2); 831 832 extern int go_cubic_solve(const go_cubic * cub, 833 go_complex * z1, 834 go_complex * z2, 835 go_complex * z3); 836 837 extern int go_quartic_solve(const go_quartic * quart, 838 go_complex * z1, 839 go_complex * z2, 840 go_complex * z3, 841 go_complex * z4); 842 843 extern int go_tridiag_reduce(go_real ** a, 844 go_integer n, 845 go_real * d, 846 go_real * e); 847 848 extern int go_tridiag_ql(go_real * d, 849 go_real * e, 850 go_integer n, 851 go_real ** z); 852 853 #endif /* GO_MATH_H */ 854