vmath.h

Go to the documentation of this file.
00001 /*                         V M A T H . H
00002  * BRL-CAD
00003  *
00004  * Copyright (c) 2004-2012 United States Government as represented by
00005  * the U.S. Army Research Laboratory.
00006  *
00007  * This library is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU Lesser General Public License
00009  * version 2.1 as published by the Free Software Foundation.
00010  *
00011  * This library is distributed in the hope that it will be useful, but
00012  * WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Lesser General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Lesser General Public
00017  * License along with this file; see the file named COPYING for more
00018  * information.
00019  */
00020 /** @addtogroup mat */
00021 /** @{ */
00022 /** @file vmath.h
00023  *
00024  * @brief vector/matrix math
00025  *
00026  * This header file defines many commonly used 3D vector math macros,
00027  * and operates on vect_t, point_t, mat_t, and quat_t objects.
00028  *
00029  * Note that while many people in the computer graphics field use
00030  * post-multiplication with row vectors (ie, vector * matrix * matrix
00031  * ...) the BRL-CAD system uses the more traditional representation
00032  * of column vectors (ie, ... matrix * matrix * vector).  (The
00033  * matrices in these two representations are the transposes of each
00034  * other). Therefore, when transforming a vector by a matrix,
00035  * pre-multiplication is used, ie:
00036  *
00037  * view_vec = model2view_mat * model_vec
00038  *
00039  * Furthermore, additional transformations are multiplied on the left,
00040  * ie:
00041  *
00042  <tt> @code
00043  * vec'  =  T1 * vec
00044  * vec'' =  T2 * T1 * vec  =  T2 * vec'
00045  @endcode </tt>
00046  *
00047  * The most notable implication of this is the location of the "delta"
00048  * (translation) values in the matrix, ie:
00049  *
00050  <tt> @code
00051  * x'   (R0  R1  R2 Dx) x
00052  * y' = (R4  R5  R6 Dy) * y
00053  * z'   (R8  R9 R10 Dz) z
00054  * w'   (0   0   0  1/s) w
00055  @endcode </tt>
00056  *
00057  * Note -
00058  *@n  vect_t objects are 3-tuples
00059  *@n hvect_t objects are 4-tuples
00060  *
00061  * Most of these macros require that the result be in separate
00062  * storage, distinct from the input parameters, except where noted.
00063  *
00064  * When writing macros like this, it is very important that any
00065  * variables which are declared within code blocks inside a macro
00066  * start with an underscore.  This (hopefully) minimizes any name
00067  * conflicts with user-provided parameters.  For example:
00068  *
00069  * { register double _f; stuff; }
00070  *
00071  */
00072 
00073 #ifndef __VMATH_H__
00074 #define __VMATH_H__
00075 
00076 #include "common.h"
00077 
00078 /* for sqrt(), sin(), cos(), rint(), etc */
00079 #include <math.h>
00080 
00081 /* for floating point tolerances and other math constants */
00082 #include <float.h>
00083 
00084 /* for fastf_t */
00085 #include "bu.h"
00086 
00087 
00088 __BEGIN_DECLS
00089 
00090 #ifndef M_
00091 #  define M_            XXX /**< */
00092 #endif
00093 
00094 #ifndef M_1_PI
00095 #  define M_1_PI        0.31830988618379067153776752675 /**< 1/pi */
00096 #endif
00097 #ifndef M_2_PI
00098 #  define M_2_PI        0.63661977236758134307553505349 /**< 2/pi */
00099 #endif
00100 #ifndef M_2_SQRTPI
00101 #  define M_2_SQRTPI    1.12837916709551257389615890312 /**< 2/sqrt(pi) */
00102 #endif
00103 #ifndef M_E
00104 #  define M_E           2.71828182845904523536028747135 /**< e */
00105 #endif
00106 #ifndef M_EULER
00107 #  define M_EULER       0.57721566490153286060651209008 /**< Euler's constant */
00108 #endif
00109 #ifndef M_LOG2E
00110 #  define M_LOG2E       1.44269504088896340735992468100 /**< log_2(e) */
00111 #endif
00112 #ifndef M_LOG10E
00113 #  define M_LOG10E      0.43429448190325182765112891892 /**< log_10(e) */
00114 #endif
00115 #ifndef M_LN10
00116 #  define M_LN10        2.30258509299404568401799145468 /**< log_e(10) */
00117 #endif
00118 #ifndef M_LN2
00119 #  define M_LN2         0.69314718055994530941723212146 /**< log_e(2) */
00120 #endif
00121 #ifndef M_LNPI
00122 #  define M_LNPI        1.14472988584940017414342735135 /**< log_e(pi) */
00123 #endif
00124 #ifndef M_PI
00125 #  define M_PI          3.14159265358979323846264338328 /**< pi */
00126 #endif
00127 #ifndef M_PI_2
00128 #  define M_PI_2        1.57079632679489661923132169164 /**< pi/2 */
00129 #endif
00130 #ifndef M_PI_4
00131 #  define M_PI_4        0.78539816339744830966156608458 /**< pi/4 */
00132 #endif
00133 #ifndef M_SQRT1_2
00134 #  define M_SQRT1_2     0.70710678118654752440084436210 /**< sqrt(1/2) */
00135 #endif
00136 #ifndef M_SQRT2
00137 #  define M_SQRT2       1.41421356237309504880168872421 /**< sqrt(2) */
00138 #endif
00139 #ifndef M_SQRT3
00140 #  define M_SQRT3       1.73205080756887729352744634151 /**< sqrt(3) */
00141 #endif
00142 #ifndef M_SQRTPI
00143 #  define M_SQRTPI      1.77245385090551602729816748334 /**< sqrt(pi) */
00144 #endif
00145 
00146 #ifndef DEG2RAD
00147 #  define DEG2RAD       0.017453292519943295769236907684 /**< pi/180 */
00148 #endif
00149 #ifndef RAD2DEG
00150 #  define RAD2DEG       57.295779513082320876798154814105 /**< 180/pi */
00151 #endif
00152 
00153 
00154 /* minimum computation tolerances */
00155 #ifdef vax
00156 #  define VDIVIDE_TOL           (1.0e-10)
00157 #  define VUNITIZE_TOL          (1.0e-7)
00158 #else
00159 #  ifdef DBL_EPSILON
00160 #    define VDIVIDE_TOL         (DBL_EPSILON)
00161 #  else
00162 #    define VDIVIDE_TOL         (1.0e-20)
00163 #  endif
00164 #  ifdef FLT_EPSILON
00165 #    define VUNITIZE_TOL        (FLT_EPSILON)
00166 #  else
00167 #    define VUNITIZE_TOL        (1.0e-15)
00168 #  endif
00169 #endif
00170 
00171 /** @brief # of fastf_t's per vect2d_t */
00172 #define ELEMENTS_PER_VECT2D     2
00173 
00174 /** @brief # of fastf_t's per point2d_t */
00175 #define ELEMENTS_PER_POINT2D    2
00176 
00177 /** @brief # of fastf_t's per vect_t */
00178 #define ELEMENTS_PER_VECT       3
00179 
00180 /** @brief # of fastf_t's per point_t */
00181 #define ELEMENTS_PER_POINT      3
00182 
00183 /** @brief # of fastf_t's per hvect_t (homogeneous vector) */
00184 #define ELEMENTS_PER_HVECT      4
00185 
00186 /** @brief # of fastf_t's per hpt_t (homogeneous point) */
00187 #define ELEMENTS_PER_HPOINT     4
00188 
00189 /** @brief # of fastf_t's per plane_t */
00190 #define ELEMENTS_PER_PLANE      4
00191 
00192 /** @brief # of fastf_t's per mat_t */
00193 #define ELEMENTS_PER_MAT        (ELEMENTS_PER_PLANE*ELEMENTS_PER_PLANE)
00194 
00195 /*
00196  * Types for matrixes and vectors.
00197  */
00198 
00199 /** @brief 2-tuple vector */
00200 typedef fastf_t vect2d_t[ELEMENTS_PER_VECT2D];
00201 
00202 /** @brief pointer to a 2-tuple vector */
00203 typedef fastf_t *vect2dp_t;
00204 
00205 /** @brief 2-tuple point */
00206 typedef fastf_t point2d_t[ELEMENTS_PER_POINT2D];
00207 
00208 /** @brief pointer to a 2-tuple point */
00209 typedef fastf_t *point2dp_t;
00210 
00211 /** @brief 3-tuple vector */
00212 typedef fastf_t vect_t[ELEMENTS_PER_VECT];
00213 
00214 /** @brief pointer to a 3-tuple vector */
00215 typedef fastf_t *vectp_t;
00216 
00217 /** @brief 3-tuple point */
00218 typedef fastf_t point_t[ELEMENTS_PER_POINT];
00219 
00220 /** @brief pointer to a 3-tuple point */
00221 typedef fastf_t *pointp_t;
00222 
00223 /** @brief 4-tuple vector */
00224 typedef fastf_t hvect_t[ELEMENTS_PER_HVECT];
00225 
00226 /** @brief 4-element quaternion */
00227 typedef hvect_t quat_t;
00228 
00229 /** @brief 4-tuple point */
00230 typedef fastf_t hpoint_t[ELEMENTS_PER_HPOINT];
00231 
00232 /** @brief 4x4 matrix */
00233 typedef fastf_t mat_t[ELEMENTS_PER_MAT];
00234 
00235 /** @brief pointer to a 4x4 matrix */
00236 typedef fastf_t *matp_t;
00237 
00238 /** Vector component names for homogeneous (4-tuple) vectors */
00239 typedef enum bn_vector_component_ {
00240     X = 0,
00241     Y = 1,
00242     Z = 2,
00243     H = 3,
00244     W = H
00245 } bn_vector_component;
00246 
00247 /**
00248  * Locations of deltas (MD*) and scaling values (MS*) in a 4x4
00249  * Homogenous Transform matrix
00250  */
00251 typedef enum bn_matrix_component_ {
00252     MSX = 0,
00253     MDX = 3,
00254     MSY = 5,
00255     MDY = 7,
00256     MSZ = 10,
00257     MDZ = 11,
00258     MSA = 15
00259 } bn_matrix_component;
00260 
00261 /**
00262  * @brief Definition of a plane equation
00263  *
00264  * A plane is defined by a unit-length outward pointing normal vector
00265  * (N), and the perpendicular (shortest) distance from the origin to
00266  * the plane (in element N[W]).
00267  *
00268  * The plane consists of all points P=(x, y, z) such that
00269  *@n VDOT(P, N) - N[W] == 0
00270  *@n that is,
00271  *@n N[X]*x + N[Y]*y + N[Z]*z - N[W] == 0
00272  *
00273  * The inside of the halfspace bounded by the plane
00274  * consists of all points P such that
00275  *@n VDOT(P, N) - N[W] <= 0
00276  *
00277  * A ray with direction D is classified w.r.t. the plane by
00278  *
00279  *@n VDOT(D, N) < 0 ray enters halfspace defined by plane
00280  *@n VDOT(D, N) == 0 ray is parallel to plane
00281  *@n VDOT(D, N) > 0 ray exits halfspace defined by plane
00282  */
00283 typedef fastf_t plane_t[ELEMENTS_PER_PLANE];
00284 
00285 
00286 /**
00287  * Return truthfully whether a value is within a specified epsilon
00288  * distance from zero.
00289  */
00290 #define NEAR_ZERO(val, epsilon) (((val) > -epsilon) && ((val) < epsilon))
00291 
00292 /**
00293  * Return truthfully whether all elements of a given vector are within
00294  * a specified epsilon distance from zero.
00295  */
00296 #define VNEAR_ZERO(v, tol) \
00297         (NEAR_ZERO(v[X], tol) \
00298          && NEAR_ZERO(v[Y], tol) \
00299          && NEAR_ZERO(v[Z], tol))
00300 
00301 /**
00302  * Test for all elements of `v' being smaller than `tol'.
00303  * Version for degree 2 vectors.
00304  */
00305 #define V2NEAR_ZERO(v, tol) (NEAR_ZERO(v[X], tol) && NEAR_ZERO(v[Y], tol))
00306 
00307 /**
00308  * Test for all elements of `v' being smaller than `tol'.
00309  * Version for degree 2 vectors.
00310  */
00311 #define HNEAR_ZERO(v, tol) \
00312     (NEAR_ZERO(v[X], tol) \
00313      && NEAR_ZERO(v[Y], tol) \
00314      && NEAR_ZERO(v[Z], tol) \
00315      && NEAR_ZERO(h[W], tol))
00316 
00317 
00318 /**
00319  * Return truthfully whether a value is within a minimum
00320  * representation tolerance from zero.
00321  *
00322  * Use not recommended due to compilation-variant tolerance.
00323  */
00324 #define ZERO(_a) NEAR_ZERO((_a), SMALL_FASTF)
00325 
00326 /**
00327  * Return truthfully whether a vector is within a minimum
00328  * representation tolerance from zero.
00329  *
00330  * Use not recommended due to compilation-variant tolerance.
00331  */
00332 #define VZERO(_a) VNEAR_ZERO((_a), SMALL_FASTF)
00333 
00334 /**
00335  * Return truthfully whether a 2d vector is within a minimum
00336  * representation tolerance from zero.
00337  *
00338  * Use not recommended due to compilation-variant tolerance.
00339  */
00340 #define V2ZERO(_a) V2NEAR_ZERO((_a), SMALL_FASTF)
00341 
00342 /**
00343  * Return truthfully whether a homogenized 4-element vector is within
00344  * a minimum representation tolerance from zero.
00345  *
00346  * Use not recommended due to compilation-variant tolerance.
00347  */
00348 #define HZERO(_a) HNEAR_ZERO((_a), SMALL_FASTF)
00349 
00350 
00351 /**
00352  * Return truthfully whether two values are within a specified epsilon
00353  * distance from each other.
00354  */
00355 #define NEAR_EQUAL(_a, _b, _tol) NEAR_ZERO((_a) - (_b), (_tol))
00356 
00357 /**
00358  * Return truthfully whether two 3D vectors are approximately equal,
00359  * within a specified absolute tolerance.
00360  */
00361 #define VNEAR_EQUAL(_a, _b, _tol) \
00362     (NEAR_EQUAL((_a)[X], (_b)[X], (_tol)) \
00363      && NEAR_EQUAL((_a)[Y], (_b)[Y], (_tol)) \
00364      && NEAR_EQUAL((_a)[Z], (_b)[Z], (_tol)))
00365 
00366 /**
00367  * Return truthfully whether two 2D vectors are approximately equal,
00368  * within a specified absolute tolerance.
00369  */
00370 #define V2NEAR_EQUAL(a, b, tol) \
00371     (NEAR_EQUAL((a)[X], (b)[X], tol) \
00372      && NEAR_EQUAL((a)[Y], (b)[Y], tol))
00373 
00374 /**
00375  * Return truthfully whether two 4D vectors are approximately equal,
00376  * within a specified absolute tolerance.
00377  */
00378 #define HNEAR_EQUAL(_a, _b, _tol) \
00379     (NEAR_EQUAL((_a)[X], (_b)[X], (_tol)) \
00380      && NEAR_EQUAL((_a)[Y], (_b)[Y], (_tol)) \
00381      && NEAR_EQUAL((_a)[Z], (_b)[Z], (_tol)) \
00382      && NEAR_EQUAL((_a)[W], (_b)[W], (_tol)))
00383 
00384 /**
00385  * Return truthfully whether two values are within a minimum
00386  * representation tolerance from each other.
00387  *
00388  * Use not recommended due to compilation-variant tolerance.
00389  */
00390 #define EQUAL(_a, _b) NEAR_EQUAL((_a), (_b), SMALL_FASTF)
00391 
00392 
00393 /**
00394  * Return truthfully whether two vectors are equal within a minimum
00395  * representation tolerance.
00396  *
00397  * Use not recommended due to compilation-variant tolerance.
00398  */
00399 #define VEQUAL(_a, _b) VNEAR_EQUAL((_a), (_b), SMALL_FASTF)
00400 
00401 /**
00402  * @brief Compare two vectors for EXACT equality.  Use carefully. 
00403  * Version for degree 2 vectors.  FIXME: no such thing as exact.
00404  */
00405 #define V2EQUAL(a, b)   ((a)[X]==(b)[X] && (a)[Y]==(b)[Y])
00406 
00407 /**
00408  * @brief Compare two vectors for EXACT equality.  Use carefully. 
00409  * Version for degree 4 vectors.   FIXME: no such thing as exact.
00410  */
00411 #define HEQUAL(a, b)    ((a)[X]==(b)[X] && (a)[Y]==(b)[Y] && (a)[Z]==(b)[Z] && (a)[W]==(b)[W])
00412 
00413 
00414 /**
00415  * clamp a value to a low/high number.
00416  */
00417 #define CLAMP(_v, _l, _h) if ((_v) < (_l)) _v = _l; else if ((_v) > (_h)) _v = _h
00418 
00419 
00420 /** @brief Compute distance from a point to a plane. */
00421 #define DIST_PT_PLANE(_pt, _pl) (VDOT(_pt, _pl) - (_pl)[W])
00422 
00423 /** @brief Compute distance between two points. */
00424 #define DIST_PT_PT_SQ(_a, _b) \
00425         ((_a)[X]-(_b)[X])*((_a)[X]-(_b)[X]) + \
00426         ((_a)[Y]-(_b)[Y])*((_a)[Y]-(_b)[Y]) + \
00427         ((_a)[Z]-(_b)[Z])*((_a)[Z]-(_b)[Z])
00428 #define DIST_PT_PT(_a, _b) sqrt(DIST_PT_PT_SQ(_a, _b))
00429 
00430 /** @brief set translation values of 4x4 matrix with x, y, z values. */
00431 #define MAT_DELTAS(_m, _x, _y, _z) { \
00432         (_m)[MDX] = (_x); \
00433         (_m)[MDY] = (_y); \
00434         (_m)[MDZ] = (_z); \
00435 }
00436 
00437 /** @brief set translation values of 4x4 matrix from a vector. */
00438 #define MAT_DELTAS_VEC(_m, _v)  \
00439         MAT_DELTAS(_m, (_v)[X], (_v)[Y], (_v)[Z])
00440 
00441 /**
00442  * @brief set translation values of 4x4 matrix from a reversed
00443  * vector.
00444  */
00445 #define MAT_DELTAS_VEC_NEG(_m, _v)      \
00446         MAT_DELTAS(_m, -(_v)[X], -(_v)[Y], -(_v)[Z])
00447 
00448 /** @brief get translation values of 4x4 matrix to a vector. */
00449 #define MAT_DELTAS_GET(_v, _m) { \
00450         (_v)[X] = (_m)[MDX]; \
00451         (_v)[Y] = (_m)[MDY]; \
00452         (_v)[Z] = (_m)[MDZ]; \
00453 }
00454 
00455 /**
00456  * @brief get translation values of 4x4 matrix to a vector,
00457  * reversed.
00458  */
00459 #define MAT_DELTAS_GET_NEG(_v, _m) { \
00460         (_v)[X] = -(_m)[MDX]; \
00461         (_v)[Y] = -(_m)[MDY]; \
00462         (_v)[Z] = -(_m)[MDZ]; \
00463 }
00464 
00465 /**
00466  * @brief increment translation elements in a 4x4 matrix with x, y, z
00467  * values.
00468  */
00469 #define MAT_DELTAS_ADD(_m, _x, _y, _z) { \
00470         (_m)[MDX] += (_x); \
00471         (_m)[MDY] += (_y); \
00472         (_m)[MDZ] += (_z); \
00473 }
00474 
00475 /**
00476  * @brief increment translation elements in a 4x4 matrix from a
00477  * vector.
00478  */
00479 #define MAT_DELTAS_ADD_VEC(_m, _v) { \
00480         (_m)[MDX] += (_v)[X]; \
00481         (_m)[MDY] += (_v)[Y]; \
00482         (_m)[MDZ] += (_v)[Z]; \
00483 }
00484 
00485 /**
00486  * @brief decrement translation elements in a 4x4 matrix with x, y, z
00487  * values.
00488  */
00489 #define MAT_DELTAS_SUB(_m, _x, _y, _z) { \
00490         (_m)[MDX] -= (_x); \
00491         (_m)[MDY] -= (_y); \
00492         (_m)[MDZ] -= (_z); \
00493 }
00494 
00495 /**
00496  * @brief decrement translation elements in a 4x4 matrix from a
00497  * vector.
00498  */
00499 #define MAT_DELTAS_SUB_VEC(_m, _v) { \
00500         (_m)[MDX] -= (_v)[X]; \
00501         (_m)[MDY] -= (_v)[Y]; \
00502         (_m)[MDZ] -= (_v)[Z]; \
00503 }
00504 
00505 /**
00506  * @brief decrement translation elements in a 4x4 matrix with x, y, z
00507  * values.
00508  */
00509 #define MAT_DELTAS_MUL(_m, _x, _y, _z) { \
00510         (_m)[MDX] *= (_x); \
00511         (_m)[MDY] *= (_y); \
00512         (_m)[MDZ] *= (_z); \
00513 }
00514 
00515 /**
00516  * @brief decrement translation elements in a 4x4 matrix from a
00517  * vector.
00518  */
00519 #define MAT_DELTAS_MUL_VEC(_m, _v) { \
00520         (_m)[MDX] *= (_v)[X]; \
00521         (_m)[MDY] *= (_v)[Y]; \
00522         (_m)[MDZ] *= (_v)[Z]; \
00523 }
00524 
00525 /** @brief set scale of 4x4 matrix from xyz. */
00526 #define MAT_SCALE(_m, _x, _y, _z) { \
00527         (_m)[MSX] = _x; \
00528         (_m)[MSY] = _y; \
00529         (_m)[MSZ] = _z; \
00530 }
00531 
00532 /** @brief set scale of 4x4 matrix from vector. */
00533 #define MAT_SCALE_VEC(_m, _v) { \
00534         (_m)[MSX] = (_v)[X]; \
00535         (_m)[MSY] = (_v)[Y]; \
00536         (_m)[MSZ] = (_v)[Z]; \
00537 }
00538 
00539 /** @brief set uniform scale of 4x4 matrix from scalar. */
00540 #define MAT_SCALE_ALL(_m, _s) (_m)[MSA] = (_s)
00541 
00542 /** @brief add to scaling elements in a 4x4 matrix from xyz. */
00543 #define MAT_SCALE_ADD(_m, _x, _y, _z) { \
00544         (_m)[MSX] += _x; \
00545         (_m)[MSY] += _y; \
00546         (_m)[MSZ] += _z; \
00547 }
00548 
00549 /** @brief add to scaling elements in a 4x4 matrix from vector. */
00550 #define MAT_SCALE_ADD_VEC(_m, _v) { \
00551         (_m)[MSX] += (_v)[X]; \
00552         (_m)[MSY] += (_v)[Y]; \
00553         (_m)[MSZ] += (_v)[Z]; \
00554 }
00555 
00556 /** @brief subtract from scaling elements in a 4x4 matrix from xyz. */
00557 #define MAT_SCALE_SUB(_m, _x, _y, _z) { \
00558         (_m)[MSX] -= _x; \
00559         (_m)[MSY] -= _y; \
00560         (_m)[MSZ] -= _z; \
00561 }
00562 
00563 /**
00564  * @brief subtract from scaling elements in a 4x4 matrix from
00565  * vector.
00566  */
00567 #define MAT_SCALE_SUB_VEC(_m, _v) { \
00568         (_m)[MSX] -= (_v)[X]; \
00569         (_m)[MSY] -= (_v)[Y]; \
00570         (_m)[MSZ] -= (_v)[Z]; \
00571 }
00572 
00573 /** @brief multipy scaling elements in a 4x4 matrix from xyz. */
00574 #define MAT_SCALE_MUL(_m, _x, _y, _z) { \
00575         (_m)[MSX] *= _x; \
00576         (_m)[MSY] *= _y; \
00577         (_m)[MSZ] *= _z; \
00578 }
00579 
00580 /** @brief multiply scaling elements in a 4x4 matrix from vector. */
00581 #define MAT_SCALE_MUL_VEC(_m, _v) { \
00582         (_m)[MSX] *= (_v)[X]; \
00583         (_m)[MSY] *= (_v)[Y]; \
00584         (_m)[MSZ] *= (_v)[Z]; \
00585 }
00586 
00587 
00588 /**
00589  * In following are macro versions of librt/mat.c functions for when
00590  * speed really matters.
00591  */
00592 
00593 
00594 /** @brief Zero a matrix. */
00595 #define MAT_ZERO(m) { \
00596         (m)[0] = (m)[1] = (m)[2] = (m)[3] = \
00597         (m)[4] = (m)[5] = (m)[6] = (m)[7] = \
00598         (m)[8] = (m)[9] = (m)[10] = (m)[11] = \
00599         (m)[12] = (m)[13] = (m)[14] = (m)[15] = 0.0; \
00600 }
00601 
00602 /** @brief Set matrix to identity. */
00603 #define MAT_IDN(m) { \
00604         (m)[1] = (m)[2] = (m)[3] = (m)[4] = \
00605         (m)[6] = (m)[7] = (m)[8] = (m)[9] = \
00606         (m)[11] = (m)[12] = (m)[13] = (m)[14] = 0.0; \
00607         (m)[0] = (m)[5] = (m)[10] = (m)[15] = 1.0; \
00608 }
00609 
00610 /** @brief set t to the transpose of matrix m */
00611 #define MAT_TRANSPOSE(t, m) { \
00612         (t)[0] = (m)[0]; \
00613         (t)[4] = (m)[1]; \
00614         (t)[8] = (m)[2]; \
00615         (t)[12] = (m)[3]; \
00616         (t)[1] = (m)[4]; \
00617         (t)[5] = (m)[5]; \
00618         (t)[9] = (m)[6]; \
00619         (t)[13] = (m)[7]; \
00620         (t)[2] = (m)[8]; \
00621         (t)[6] = (m)[9]; \
00622         (t)[10] = (m)[10]; \
00623         (t)[14] = (m)[11]; \
00624         (t)[3] = (m)[12]; \
00625         (t)[7] = (m)[13]; \
00626         (t)[11] = (m)[14]; \
00627         (t)[15] = (m)[15]; \
00628 }
00629 
00630 /** @brief Copy a matrix. */
00631 #define MAT_COPY(d, s) { \
00632         (d)[0] = (s)[0]; \
00633         (d)[1] = (s)[1]; \
00634         (d)[2] = (s)[2]; \
00635         (d)[3] = (s)[3]; \
00636         (d)[4] = (s)[4]; \
00637         (d)[5] = (s)[5]; \
00638         (d)[6] = (s)[6]; \
00639         (d)[7] = (s)[7]; \
00640         (d)[8] = (s)[8]; \
00641         (d)[9] = (s)[9]; \
00642         (d)[10] = (s)[10]; \
00643         (d)[11] = (s)[11]; \
00644         (d)[12] = (s)[12]; \
00645         (d)[13] = (s)[13]; \
00646         (d)[14] = (s)[14]; \
00647         (d)[15] = (s)[15]; \
00648 }
00649 
00650 /** @brief Set 3D vector at `a' to have coordinates `b', `c', and `d'. */
00651 #define VSET(a, b, c, d) { \
00652         (a)[X] = (b); \
00653         (a)[Y] = (c); \
00654         (a)[Z] = (d); \
00655 }
00656 
00657 /** @brief Set 2D vector at `a' to have coordinates `b' and `c'. */
00658 #define V2SET(a, b, c) { \
00659         (a)[X] = (b); \
00660         (a)[Y] = (c); \
00661 }
00662 
00663 /** @brief Set 4D vector at `a' to homogenous coordinates `b', `c', `d', and `e'. */
00664 #define HSET(a, b, c, d, e) { \
00665         (a)[X] = (b); \
00666         (a)[Y] = (c); \
00667         (a)[Z] = (d); \
00668         (a)[H] = (e); \
00669 }
00670 
00671 
00672 /** @brief Set all elements of 3D vector to same scalar value. */
00673 #define VSETALL(a, s) { \
00674         (a)[X] = (a)[Y] = (a)[Z] = (s); \
00675 }
00676 
00677 /** @brief Set 2D vector elements to same scalar value. */
00678 #define V2SETALL(a, s) { \
00679         (a)[X] = (a)[Y] = (s); \
00680 }
00681 
00682 /** @brief Set 4D vector elements to same scalar value. */
00683 #define HSETALL(a, s) { \
00684         (a)[X] = (a)[Y] = (a)[Z] = (a)[H] = (s); \
00685 }
00686 
00687 
00688 /** @brief Set all elements of N-vector to same scalar value. */
00689 #define VSETALLN(v, s, n) { \
00690         register int _j; \
00691         for (_j=0; _j<n; _j++) v[_j]=(s); \
00692 }
00693 
00694 
00695 /** @brief Transfer 3D vector at `b' to vector at `a'. */
00696 #define VMOVE(a, b) { \
00697         (a)[X] = (b)[X]; \
00698         (a)[Y] = (b)[Y]; \
00699         (a)[Z] = (b)[Z]; \
00700 }
00701 
00702 /** @brief Move a 2D vector. */
00703 #define V2MOVE(a, b) { \
00704         (a)[X] = (b)[X]; \
00705         (a)[Y] = (b)[Y]; \
00706 }
00707 
00708 /** @brief Move a homogeneous 4-tuple. */
00709 #define HMOVE(a, b) { \
00710         (a)[X] = (b)[X]; \
00711         (a)[Y] = (b)[Y]; \
00712         (a)[Z] = (b)[Z]; \
00713         (a)[W] = (b)[W]; \
00714 }
00715 
00716 /** @brief Transfer vector of length `n' at `b' to vector at `a'. */
00717 #define VMOVEN(a, b, n) { \
00718         register int _vmove; \
00719         for (_vmove = 0; _vmove < (n); _vmove++) { \
00720                 (a)[_vmove] = (b)[_vmove]; \
00721         } \
00722 }
00723 
00724 
00725 /** @brief Reverse the direction of 3D vector `b' and store it in `a'. */
00726 #define VREVERSE(a, b) { \
00727         (a)[X] = -(b)[X]; \
00728         (a)[Y] = -(b)[Y]; \
00729         (a)[Z] = -(b)[Z]; \
00730 }
00731 
00732 /** @brief Reverse the direction of 2D vector `b' and store it in `a'. */
00733 #define V2REVERSE(a, b) { \
00734         (a)[X] = -(b)[X]; \
00735         (a)[Y] = -(b)[Y]; \
00736 }
00737 
00738 /**
00739  * @brief Same as VREVERSE, but for a 4-tuple.  Also useful on plane_t
00740  * objects.
00741  */
00742 #define HREVERSE(a, b) { \
00743         (a)[X] = -(b)[X]; \
00744         (a)[Y] = -(b)[Y]; \
00745         (a)[Z] = -(b)[Z]; \
00746         (a)[W] = -(b)[W]; \
00747 }
00748 
00749 /** @brief Add 3D vectors at `b' and `c', store result at `a'. */
00750 #define VADD2(a, b, c) { \
00751         (a)[X] = (b)[X] + (c)[X]; \
00752         (a)[Y] = (b)[Y] + (c)[Y]; \
00753         (a)[Z] = (b)[Z] + (c)[Z]; \
00754 }
00755 
00756 /** @brief Add 2D vectors at `b' and `c', store result at `a'. */
00757 #define V2ADD2(a, b, c) { \
00758         (a)[X] = (b)[X] + (c)[X]; \
00759         (a)[Y] = (b)[Y] + (c)[Y]; \
00760 }
00761 
00762 /** @brief Add 4D vectors at `b' and `c', store result at `a'. */
00763 #define HADD2(a, b, c) { \
00764         (a)[X] = (b)[X] + (c)[X]; \
00765         (a)[Y] = (b)[Y] + (c)[Y]; \
00766         (a)[Z] = (b)[Z] + (c)[Z]; \
00767         (a)[W] = (b)[W] + (c)[W]; \
00768 }
00769 
00770 /**
00771  * @brief Add vectors of length `n' at `b' and `c', store result at
00772  * `a'.
00773  */
00774 #define VADD2N(a, b, c, n) { \
00775         register int _vadd2; \
00776         for (_vadd2 = 0; _vadd2 < (n); _vadd2++) { \
00777                 (a)[_vadd2] = (b)[_vadd2] + (c)[_vadd2]; \
00778         } \
00779 }
00780 
00781 
00782 /**
00783  * @brief Subtract 3D vector at `c' from vector at `b', store result at
00784  * `a'.
00785  */
00786 #define VSUB2(a, b, c) { \
00787         (a)[X] = (b)[X] - (c)[X]; \
00788         (a)[Y] = (b)[Y] - (c)[Y]; \
00789         (a)[Z] = (b)[Z] - (c)[Z]; \
00790 }
00791 
00792 /**
00793  * @brief Subtract 2D vector at `c' from vector at `b', store result at
00794  * `a'.
00795  */
00796 #define V2SUB2(a, b, c) { \
00797         (a)[X] = (b)[X] - (c)[X]; \
00798         (a)[Y] = (b)[Y] - (c)[Y]; \
00799 }
00800 
00801 /**
00802  * @brief Subtract 4D vector at `c' from vector at `b', store result at
00803  * `a'.
00804  */
00805 #define HSUB2(a, b, c) { \
00806         (a)[X] = (b)[X] - (c)[X]; \
00807         (a)[Y] = (b)[Y] - (c)[Y]; \
00808         (a)[Z] = (b)[Z] - (c)[Z]; \
00809         (a)[W] = (b)[W] - (c)[W]; \
00810 }
00811 
00812 /**
00813  * @brief Subtract `n' length vector at `c' from vector at `b', store
00814  * result at `a'.
00815  */
00816 #define VSUB2N(a, b, c, n) { \
00817         register int _vsub2; \
00818         for (_vsub2 = 0; _vsub2 < (n); _vsub2++) { \
00819                 (a)[_vsub2] = (b)[_vsub2] - (c)[_vsub2]; \
00820         } \
00821 }
00822 
00823 
00824 /** @brief 3D Vectors:  A = B - C - D */
00825 #define VSUB3(a, b, c, d) { \
00826         (a)[X] = (b)[X] - (c)[X] - (d)[X]; \
00827         (a)[Y] = (b)[Y] - (c)[Y] - (d)[Y]; \
00828         (a)[Z] = (b)[Z] - (c)[Z] - (d)[Z]; \
00829 }
00830 
00831 /** @brief 2D Vectors:  A = B - C - D */
00832 #define V2SUB3(a, b, c, d) { \
00833         (a)[X] = (b)[X] - (c)[X] - (d)[X]; \
00834         (a)[Y] = (b)[Y] - (c)[Y] - (d)[Y]; \
00835 }
00836 
00837 /** @brief 4D Vectors:  A = B - C - D */
00838 #define HSUB3(a, b, c, d) { \
00839         (a)[X] = (b)[X] - (c)[X] - (d)[X]; \
00840         (a)[Y] = (b)[Y] - (c)[Y] - (d)[Y]; \
00841         (a)[Z] = (b)[Z] - (c)[Z] - (d)[Z]; \
00842         (a)[W] = (b)[W] - (c)[W] - (d)[W]; \
00843 }
00844 
00845 /** @brief Vectors:  A = B - C - D for vectors of length `n'. */
00846 #define VSUB3N(a, b, c, d, n) { \
00847         register int _vsub3; \
00848         for (_vsub3 = 0; _vsub3 < (n); _vsub3++) { \
00849                 (a)[_vsub3] = (b)[_vsub3] - (c)[_vsub3] - (d)[_vsub3]; \
00850         } \
00851 }
00852 
00853 
00854 /** @brief Add 3 3D vectors at `b', `c', and `d', store result at `a'. */
00855 #define VADD3(a, b, c, d) { \
00856         (a)[X] = (b)[X] + (c)[X] + (d)[X]; \
00857         (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y]; \
00858         (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z]; \
00859 }
00860 
00861 /** @brief Add 3 2D vectors at `b', `c', and `d', store result at `a'. */
00862 #define V2ADD3(a, b, c, d) { \
00863         (a)[X] = (b)[X] + (c)[X] + (d)[X]; \
00864         (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y]; \
00865 }
00866 
00867 /** @brief Add 3 4D vectors at `b', `c', and `d', store result at `a'. */
00868 #define HADD3(a, b, c, d) { \
00869         (a)[X] = (b)[X] + (c)[X] + (d)[X]; \
00870         (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y]; \
00871         (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z]; \
00872         (a)[W] = (b)[W] + (c)[W] + (d)[W]; \
00873 }
00874 
00875 /**
00876  * @brief Add 3 vectors of length `n' at `b', `c', and `d', store
00877  * result at `a'.
00878  */
00879 #define VADD3N(a, b, c, d, n) { \
00880         register int _vadd3; \
00881         for (_vadd3 = 0; _vadd3 < (n); _vadd3++) { \
00882                 (a)[_vadd3] = (b)[_vadd3] + (c)[_vadd3] + (d)[_vadd3]; \
00883         } \
00884 }
00885 
00886 
00887 /**
00888  * @brief Add 4 vectors at `b', `c', `d', and `e', store result at
00889  * `a'.
00890  */
00891 #define VADD4(a, b, c, d, e) { \
00892         (a)[X] = (b)[X] + (c)[X] + (d)[X] + (e)[X]; \
00893         (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y] + (e)[Y]; \
00894         (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z] + (e)[Z]; \
00895 }
00896 
00897 /**
00898  * @brief Add 4 2D vectors at `b', `c', `d', and `e', store result at
00899  * `a'.
00900  */
00901 #define V2ADD4(a, b, c, d, e) { \
00902         (a)[X] = (b)[X] + (c)[X] + (d)[X] + (e)[X]; \
00903         (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y] + (e)[Y]; \
00904 }
00905 
00906 /**
00907  * @brief Add 4 4D vectors at `b', `c', `d', and `e', store result at
00908  * `a'.
00909  */
00910 #define HADD4(a, b, c, d, e) { \
00911         (a)[X] = (b)[X] + (c)[X] + (d)[X] + (e)[X]; \
00912         (a)[Y] = (b)[Y] + (c)[Y] + (d)[Y] + (e)[Y]; \
00913         (a)[Z] = (b)[Z] + (c)[Z] + (d)[Z] + (e)[Z]; \
00914         (a)[W] = (b)[W] + (c)[W] + (d)[W] + (e)[W]; \
00915 }
00916 
00917 /**
00918  * @brief Add 4 `n' length vectors at `b', `c', `d', and `e', store
00919  * result at `a'.
00920  */
00921 #define VADD4N(a, b, c, d, e, n) { \
00922         register int _vadd4; \
00923         for (_vadd4 = 0; _vadd4 < (n); _vadd4++) { \
00924                 (a)[_vadd4] = (b)[_vadd4] + (c)[_vadd4] + (d)[_vadd4] + (e)[_vadd4]; \
00925         } \
00926 }
00927 
00928 
00929 /** @brief Scale 3D vector at `b' by scalar `c', store result at `a'. */
00930 #define VSCALE(a, b, c) { \
00931         (a)[X] = (b)[X] * (c); \
00932         (a)[Y] = (b)[Y] * (c); \
00933         (a)[Z] = (b)[Z] * (c); \
00934 }
00935 
00936 /** @brief Scale 2D vector at `b' by scalar `c', store result at `a'. */
00937 #define V2SCALE(a, b, c) { \
00938         (a)[X] = (b)[X] * (c); \
00939         (a)[Y] = (b)[Y] * (c); \
00940 }
00941 
00942 /** @brief Scale 4D vector at `b' by scalar `c', store result at `a'. */
00943 #define HSCALE(a, b, c) { \
00944         (a)[X] = (b)[X] * (c); \
00945         (a)[Y] = (b)[Y] * (c); \
00946         (a)[Z] = (b)[Z] * (c); \
00947         (a)[W] = (b)[W] * (c); \
00948 }
00949 
00950 /**
00951  * @brief Scale vector of length `n' at `b' by scalar `c', store
00952  * result at `a'
00953  */
00954 #define VSCALEN(a, b, c, n) { \
00955         register int _vscale; \
00956         for (_vscale = 0; _vscale < (n); _vscale++) { \
00957                 (a)[_vscale] = (b)[_vscale] * (c); \
00958         } \
00959 }
00960 
00961 /** @brief Normalize vector `a' to be a unit vector. */
00962 #define VUNITIZE(a) { \
00963         register double _f = MAGSQ(a); \
00964         if (! NEAR_EQUAL(_f, 1.0, VUNITIZE_TOL)) { \
00965                 _f = sqrt(_f); \
00966                 if (_f < VDIVIDE_TOL) { \
00967                         VSETALL((a), 0.0); \
00968                 } else { \
00969                         _f = 1.0/_f; \
00970                         (a)[X] *= _f; (a)[Y] *= _f; (a)[Z] *= _f; \
00971                 } \
00972         } \
00973 }
00974 
00975 /** @brief If vector magnitude is too small, return an error code. */
00976 #define VUNITIZE_RET(a, ret) { \
00977         register double _f; \
00978         _f = MAGNITUDE(a); \
00979         if (_f < VDIVIDE_TOL) return ret; \
00980         _f = 1.0/_f; \
00981         (a)[X] *= _f; (a)[Y] *= _f; (a)[Z] *= _f; \
00982 }
00983 
00984 /**
00985  * @brief Find the sum of two points, and scale the result.  Often
00986  * used to find the midpoint.
00987  */
00988 #define VADD2SCALE(o, a, b, s) { \
00989                         (o)[X] = ((a)[X] + (b)[X]) * (s); \
00990                         (o)[Y] = ((a)[Y] + (b)[Y]) * (s); \
00991                         (o)[Z] = ((a)[Z] + (b)[Z]) * (s); \
00992 }
00993 
00994 #define VADD2SCALEN(o, a, b, n) { \
00995         register int _vadd2scale; \
00996         for (_vadd2scale = 0; \
00997         _vadd2scale < (n); \
00998         _vadd2scale++) { \
00999                 (o)[_vadd2scale] = ((a)[_vadd2scale] + (b)[_vadd2scale]) * (s); \
01000         } \
01001 }
01002 
01003 /**
01004  * @brief Find the difference between two points, and scale result.
01005  * Often used to compute bounding sphere radius given rpp points.
01006  */
01007 #define VSUB2SCALE(o, a, b, s) { \
01008                         (o)[X] = ((a)[X] - (b)[X]) * (s); \
01009                         (o)[Y] = ((a)[Y] - (b)[Y]) * (s); \
01010                         (o)[Z] = ((a)[Z] - (b)[Z]) * (s); \
01011 }
01012 
01013 #define VSUB2SCALEN(o, a, b, n) { \
01014         register int _vsub2scale; \
01015         for (_vsub2scale = 0; \
01016         _vsub2scale < (n); \
01017         _vsub2scale++) { \
01018                 (o)[_vsub2scale] = ((a)[_vsub2scale] - (b)[_vsub2scale]) * (s); \
01019         } \
01020 }
01021 
01022 
01023 /** @brief Combine together several vectors, scaled by a scalar. */
01024 #define VCOMB3(o, a, b, c, d, e, f) { \
01025         (o)[X] = (a) * (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01026         (o)[Y] = (a) * (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01027         (o)[Z] = (a) * (b)[Z] + (c) * (d)[Z] + (e) * (f)[Z]; \
01028 }
01029 
01030 #define VCOMB3N(o, a, b, c, d, e, f, n) { \
01031         register int _vcomb3; \
01032         for (_vcomb3 = 0; \
01033         _vcomb3 < (n); \
01034         _vcomb3++) { \
01035                 (o)[_vcomb3] = (a) * (b)[_vcomb3] + (c) * (d)[_vcomb3] + (e) * (f)[_vcomb3]; \
01036         } \
01037 }
01038 
01039 #define VCOMB2(o, a, b, c, d) { \
01040         (o)[X] = (a) * (b)[X] + (c) * (d)[X]; \
01041         (o)[Y] = (a) * (b)[Y] + (c) * (d)[Y]; \
01042         (o)[Z] = (a) * (b)[Z] + (c) * (d)[Z]; \
01043 }
01044 
01045 #define VCOMB2N(o, a, b, c, d, n) { \
01046         register int _vcomb2; \
01047         for (_vcomb2 = 0; \
01048         _vcomb2 < (n); \
01049         _vcomb2++) { \
01050                 (o)[_vcomb2] = (a) * (b)[_vcomb2] + (c) * (d)[_vcomb2]; \
01051         } \
01052 }
01053 
01054 #define VJOIN4(a, b, c, d, e, f, g, h, i, j) { \
01055         (a)[X] = (b)[X] + (c)*(d)[X] + (e)*(f)[X] + (g)*(h)[X] + (i)*(j)[X]; \
01056         (a)[Y] = (b)[Y] + (c)*(d)[Y] + (e)*(f)[Y] + (g)*(h)[Y] + (i)*(j)[Y]; \
01057         (a)[Z] = (b)[Z] + (c)*(d)[Z] + (e)*(f)[Z] + (g)*(h)[Z] + (i)*(j)[Z]; \
01058 }
01059 
01060 #define VJOIN3(a, b, c, d, e, f, g, h) { \
01061         (a)[X] = (b)[X] + (c)*(d)[X] + (e)*(f)[X] + (g)*(h)[X]; \
01062         (a)[Y] = (b)[Y] + (c)*(d)[Y] + (e)*(f)[Y] + (g)*(h)[Y]; \
01063         (a)[Z] = (b)[Z] + (c)*(d)[Z] + (e)*(f)[Z] + (g)*(h)[Z]; \
01064 }
01065 
01066 
01067 /**
01068  * @brief Compose 3D vector at `a' of:
01069  * Vector at `b' plus
01070  * scalar `c' times vector at `d' plus
01071  * scalar `e' times vector at `f'
01072  */
01073 #define VJOIN2(a, b, c, d, e, f) { \
01074         (a)[X] = (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01075         (a)[Y] = (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01076         (a)[Z] = (b)[Z] + (c) * (d)[Z] + (e) * (f)[Z]; \
01077 }
01078 
01079 /**
01080  * @brief Compose 2D vector at `a' of:
01081  * Vector at `b' plus
01082  * scalar `c' times vector at `d' plus
01083  * scalar `e' times vector at `f'
01084  */
01085 #define V2JOIN2(a, b, c, d, e, f) { \
01086         (a)[X] = (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01087         (a)[Y] = (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01088 }
01089 
01090 /**
01091  * @brief Compose 4D vector at `a' of:
01092  * Vector at `b' plus
01093  * scalar `c' times vector at `d' plus
01094  * scalar `e' times vector at `f'
01095  */
01096 #define HJOIN2(a, b, c, d, e, f) { \
01097         (a)[X] = (b)[X] + (c) * (d)[X] + (e) * (f)[X]; \
01098         (a)[Y] = (b)[Y] + (c) * (d)[Y] + (e) * (f)[Y]; \
01099         (a)[Z] = (b)[Z] + (c) * (d)[Z] + (e) * (f)[Z]; \
01100         (a)[W] = (b)[W] + (c) * (d)[W] + (e) * (f)[W]; \
01101 }
01102 
01103 #define VJOIN2N(a, b, c, d, e, f, n) { \
01104         register int _vjoin2; \
01105         for (_vjoin2 = 0; \
01106         _vjoin2 < (n); \
01107         _vjoin2++) { \
01108                 (a)[_vjoin2] = (b)[_vjoin2] + (c) * (d)[_vjoin2] + (e) * (f)[_vjoin2]; \
01109         } \
01110 }
01111 
01112 
01113 #define VJOIN1(a, b, c, d) { \
01114         (a)[X] = (b)[X] + (c) * (d)[X]; \
01115         (a)[Y] = (b)[Y] + (c) * (d)[Y]; \
01116         (a)[Z] = (b)[Z] + (c) * (d)[Z]; \
01117 }
01118 
01119 #define V2JOIN1(a, b, c, d) { \
01120         (a)[X] = (b)[X] + (c) * (d)[X]; \
01121         (a)[Y] = (b)[Y] + (c) * (d)[Y]; \
01122 }
01123 
01124 #define HJOIN1(a, b, c, d) { \
01125         (a)[X] = (b)[X] + (c) * (d)[X]; \
01126         (a)[Y] = (b)[Y] + (c) * (d)[Y]; \
01127         (a)[Z] = (b)[Z] + (c) * (d)[Z]; \
01128         (a)[W] = (b)[W] + (c) * (d)[W]; \
01129 }
01130 
01131 #define VJOIN1N(a, b, c, d, n) { \
01132         register int _vjoin1; \
01133         for (_vjoin1 = 0; \
01134         _vjoin1 < (n); \
01135         _vjoin1++) { \
01136                 (a)[_vjoin1] = (b)[_vjoin1] + (c) * (d)[_vjoin1]; \
01137         } \
01138 }
01139 
01140 
01141 /**
01142  * @brief Blend into vector `a'
01143  * scalar `b' times vector at `c' plus
01144  * scalar `d' times vector at `e'
01145  */
01146 #define VBLEND2(a, b, c, d, e) { \
01147         (a)[X] = (b) * (c)[X] + (d) * (e)[X]; \
01148         (a)[Y] = (b) * (c)[Y] + (d) * (e)[Y]; \
01149         (a)[Z] = (b) * (c)[Z] + (d) * (e)[Z]; \
01150 }
01151 
01152 #define VBLEND2N(a, b, c, d, e, n) { \
01153         register int _vblend2; \
01154         for (_vblend2 = 0; \
01155         _vblend2 < (n); \
01156         _vblend2++) { \
01157                 (a)[_vblend2] = (b) * (c)[_vblend2] + (d) * (e)[_vblend2]; \
01158         } \
01159 }
01160 
01161 
01162 /**
01163  * @brief Project vector `a' onto `b'
01164  * vector `c' is the component of `a' parallel to `b'
01165  *     "    `d' "   "     "      "   "  orthogonal "   "
01166  */
01167 #define VPROJECT(a, b, c, d) { \
01168     VSCALE(c, b, VDOT(a, b) / VDOT(b, b)); \
01169     VSUB2(d, a, c); \
01170 }
01171 
01172 /** @brief Return scalar magnitude squared of vector at `a' */
01173 #define MAGSQ(a)        ((a)[X]*(a)[X] + (a)[Y]*(a)[Y] + (a)[Z]*(a)[Z])
01174 #define MAG2SQ(a)       ((a)[X]*(a)[X] + (a)[Y]*(a)[Y])
01175 
01176 /** @brief Return scalar magnitude of vector at `a' */
01177 #define MAGNITUDE(a) sqrt(MAGSQ(a))
01178 
01179 /**
01180  * @brief Store cross product of vectors at `b' and `c' in vector at
01181  * `a'.  Note that the "right hand rule" applies: If closing your
01182  * right hand goes from `b' to `c', then your thumb points in the
01183  * direction of the cross product.
01184  *
01185  * If the angle from `b' to `c' goes clockwise, then the result vector
01186  * points "into" the plane (inward normal).  Example: b=(0, 1, 0),
01187  * c=(1, 0, 0), then bXc=(0, 0, -1).
01188  *
01189  * If the angle from `b' to `c' goes counter-clockwise, then the
01190  * result vector points "out" of the plane.  This outward pointing
01191  * normal is the BRL-CAD convention.
01192  */
01193 #define VCROSS(a, b, c) { \
01194         (a)[X] = (b)[Y] * (c)[Z] - (b)[Z] * (c)[Y]; \
01195         (a)[Y] = (b)[Z] * (c)[X] - (b)[X] * (c)[Z]; \
01196         (a)[Z] = (b)[X] * (c)[Y] - (b)[Y] * (c)[X]; \
01197 }
01198 
01199 /** @brief Compute dot product of vectors at `a' and `b'. */
01200 #define VDOT(a, b)      ((a)[X]*(b)[X] + (a)[Y]*(b)[Y] + (a)[Z]*(b)[Z])
01201 
01202 #define V2DOT(a, b)     ((a)[X]*(b)[X] + (a)[Y]*(b)[Y])
01203 
01204 #define HDOT(a, b)      ((a)[X]*(b)[X] + (a)[Y]*(b)[Y] + (a)[Z]*(b)[Z] + (a)[W]*(b)[W])
01205 
01206 
01207 /**
01208  * @brief Subtract two points to make a vector, dot with another
01209  * vector.
01210  */
01211 #define VSUB2DOT(_pt2, _pt, _vec)       (\
01212         ((_pt2)[X] - (_pt)[X]) * (_vec)[X] + \
01213         ((_pt2)[Y] - (_pt)[Y]) * (_vec)[Y] + \
01214         ((_pt2)[Z] - (_pt)[Z]) * (_vec)[Z])
01215 
01216 /**
01217  * @brief Turn a vector into comma-separated list of elements, for
01218  * subroutine args.
01219  */
01220 #define V2ARGS(a)       (a)[X], (a)[Y]
01221 #define V3ARGS(a)       (a)[X], (a)[Y], (a)[Z]
01222 #define V4ARGS(a)       (a)[X], (a)[Y], (a)[Z], (a)[W]
01223 
01224 /**
01225  * if a value is within computation tolerance of an integer, clamp the
01226  * value to that integer.
01227  *
01228  * NOTE: should use VDIVIDE_TOL here, but cannot yet until floats are
01229  * replaced universally with fastf_t's since their epsilon is
01230  * considerably less than that of a double.
01231  */
01232 #define INTCLAMP(_a) (NEAR_EQUAL((_a), rint(_a), VUNITIZE_TOL) ? (double)(long)rint(_a) : (_a))
01233 
01234 /** @brief integer clamped versions of the previous arg macros. */
01235 #define V2INTCLAMPARGS(a) INTCLAMP((a)[X]), INTCLAMP((a)[Y])
01236 /** @brief integer clamped versions of the previous arg macros. */
01237 #define V3INTCLAMPARGS(a) INTCLAMP((a)[X]), INTCLAMP((a)[Y]), INTCLAMP((a)[Z])
01238 /** @brief integer clamped versions of the previous arg macros. */
01239 #define V4INTCLAMPARGS(a) INTCLAMP((a)[X]), INTCLAMP((a)[Y]), INTCLAMP((a)[Z]), INTCLAMP((a)[W])
01240 
01241 /** @brief Print vector name and components on stderr. */
01242 #define V2PRINT(a, b)   \
01243         (void)fprintf(stderr, "%s (%g, %g)\n", a, V2ARGS(b));
01244 #define VPRINT(a, b)    \
01245         (void)fprintf(stderr, "%s (%g, %g, %g)\n", a, V3ARGS(b));
01246 #define HPRINT(a, b)    \
01247         (void)fprintf(stderr, "%s (%g, %g, %g, %g)\n", a, V4ARGS(b));
01248 
01249 /**
01250  * @brief Included below are integer clamped versions of the previous
01251  * print macros.
01252  */
01253 
01254 #define V2INTCLAMPPRINT(a, b)   \
01255         (void)fprintf(stderr, "%s (%g, %g)\n", a, V2INTCLAMPARGS(b));
01256 #define VINTCLAMPPRINT(a, b)    \
01257         (void)fprintf(stderr, "%s (%g, %g, %g)\n", a, V3INTCLAMPARGS(b));
01258 #define HINTCLAMPPRINT(a, b)    \
01259         (void)fprintf(stderr, "%s (%g, %g, %g, %g)\n", a, V4INTCLAMPARGS(b));
01260 
01261 #ifdef __cplusplus
01262 #define CPP_V3PRINT(_os, _title, _p)    (_os) << (_title) << "=(" << \
01263         (_p)[X] << ", " << (_p)[Y] << ")\n";
01264 #define CPP_VPRINT(_os, _title, _p)     (_os) << (_title) << "=(" << \
01265         (_p)[X] << ", " << (_p)[Y] << ", " << (_p)[Z] << ")\n";
01266 #define CPP_HPRINT(_os, _title, _p)     (_os) << (_title) << "=(" << \
01267         (_p)[X] << ", " << (_p)[Y] << ", " << (_p)[Z] << ", " << (_p)[W]<< ")\n";
01268 #endif
01269 
01270 /** @brief Vector element multiplication.  Really: diagonal matrix X vect. */
01271 #define VELMUL(a, b, c) { \
01272         (a)[X] = (b)[X] * (c)[X]; \
01273         (a)[Y] = (b)[Y] * (c)[Y]; \
01274         (a)[Z] = (b)[Z] * (c)[Z]; \
01275 }
01276 
01277 #define VELMUL3(a, b, c, d) { \
01278         (a)[X] = (b)[X] * (c)[X] * (d)[X]; \
01279         (a)[Y] = (b)[Y] * (c)[Y] * (d)[Y]; \
01280         (a)[Z] = (b)[Z] * (c)[Z] * (d)[Z]; \
01281 }
01282 
01283 /** @brief Similar to VELMUL. */
01284 #define VELDIV(a, b, c) { \
01285         (a)[0] = (b)[0] / (c)[0]; \
01286         (a)[1] = (b)[1] / (c)[1]; \
01287         (a)[2] = (b)[2] / (c)[2]; \
01288 }
01289 
01290 /**
01291  * @brief Given a direction vector, compute the inverses of each element.
01292  * When division by zero would have occured, mark inverse as INFINITY.
01293  */
01294 #define VINVDIR(_inv, _dir) { \
01295         if ((_dir)[X] < -SQRT_SMALL_FASTF || (_dir)[X] > SQRT_SMALL_FASTF) { \
01296                 (_inv)[X]=1.0/(_dir)[X]; \
01297         } else { \
01298                 (_dir)[X] = 0.0; \
01299                 (_inv)[X] = INFINITY; \
01300         } \
01301         if ((_dir)[Y] < -SQRT_SMALL_FASTF || (_dir)[Y] > SQRT_SMALL_FASTF) { \
01302                 (_inv)[Y]=1.0/(_dir)[Y]; \
01303         } else { \
01304                 (_dir)[Y] = 0.0; \
01305                 (_inv)[Y] = INFINITY; \
01306         } \
01307         if ((_dir)[Z] < -SQRT_SMALL_FASTF || (_dir)[Z] > SQRT_SMALL_FASTF) { \
01308                 (_inv)[Z]=1.0/(_dir)[Z]; \
01309         } else { \
01310                 (_dir)[Z] = 0.0; \
01311                 (_inv)[Z] = INFINITY; \
01312         } \
01313     }
01314 
01315 /**
01316  * @brief Apply the 3x3 part of a mat_t to a 3-tuple.  This rotates a
01317  * vector without scaling it (changing its length).
01318  */
01319 #define MAT3X3VEC(o, mat, vec) { \
01320         (o)[X] = (mat)[X]*(vec)[X]+(mat)[Y]*(vec)[Y] + (mat)[ 2]*(vec)[Z]; \
01321         (o)[Y] = (mat)[4]*(vec)[X]+(mat)[5]*(vec)[Y] + (mat)[ 6]*(vec)[Z]; \
01322         (o)[Z] = (mat)[8]*(vec)[X]+(mat)[9]*(vec)[Y] + (mat)[10]*(vec)[Z]; \
01323 }
01324 
01325 /** @brief Multiply a 3-tuple by the 3x3 part of a mat_t. */
01326 #define VEC3X3MAT(o, i, m) { \
01327         (o)[X] = (i)[X]*(m)[X] + (i)[Y]*(m)[4] + (i)[Z]*(m)[8]; \
01328         (o)[Y] = (i)[X]*(m)[1] + (i)[Y]*(m)[5] + (i)[Z]*(m)[9]; \
01329         (o)[Z] = (i)[X]*(m)[2] + (i)[Y]*(m)[6] + (i)[Z]*(m)[10]; \
01330 }
01331 
01332 /** @brief Apply the 3x3 part of a mat_t to a 2-tuple (Z part=0). */
01333 #define MAT3X2VEC(o, mat, vec) { \
01334         (o)[X] = (mat)[0]*(vec)[X] + (mat)[Y]*(vec)[Y]; \
01335         (o)[Y] = (mat)[4]*(vec)[X] + (mat)[5]*(vec)[Y]; \
01336         (o)[Z] = (mat)[8]*(vec)[X] + (mat)[9]*(vec)[Y]; \
01337 }
01338 
01339 /** @brief Multiply a 2-tuple (Z=0) by the 3x3 part of a mat_t. */
01340 #define VEC2X3MAT(o, i, m) { \
01341         (o)[X] = (i)[X]*(m)[0] + (i)[Y]*(m)[4]; \
01342         (o)[Y] = (i)[X]*(m)[1] + (i)[Y]*(m)[5]; \
01343         (o)[Z] = (i)[X]*(m)[2] + (i)[Y]*(m)[6]; \
01344 }
01345 
01346 /**
01347  * @brief Apply a 4x4 matrix to a 3-tuple which is an absolute Point
01348  * in space.  Output and input points should be separate arrays.
01349  */
01350 #define MAT4X3PNT(o, m, i) { \
01351         register double _f; \
01352         _f = 1.0/((m)[12]*(i)[X] + (m)[13]*(i)[Y] + (m)[14]*(i)[Z] + (m)[15]); \
01353         (o)[X]=((m)[0]*(i)[X] + (m)[1]*(i)[Y] + (m)[ 2]*(i)[Z] + (m)[3]) * _f; \
01354         (o)[Y]=((m)[4]*(i)[X] + (m)[5]*(i)[Y] + (m)[ 6]*(i)[Z] + (m)[7]) * _f; \
01355         (o)[Z]=((m)[8]*(i)[X] + (m)[9]*(i)[Y] + (m)[10]*(i)[Z] + (m)[11])* _f; \
01356 }
01357 
01358 /**
01359  * @brief Multiply an Absolute 3-Point by a full 4x4 matrix.  Output
01360  * and input points should be separate arrays.
01361  */
01362 #define PNT3X4MAT(o, i, m) { \
01363         register double _f; \
01364         _f = 1.0/((i)[X]*(m)[3] + (i)[Y]*(m)[7] + (i)[Z]*(m)[11] + (m)[15]); \
01365         (o)[X]=((i)[X]*(m)[0] + (i)[Y]*(m)[4] + (i)[Z]*(m)[8] + (m)[12]) * _f; \
01366         (o)[Y]=((i)[X]*(m)[1] + (i)[Y]*(m)[5] + (i)[Z]*(m)[9] + (m)[13]) * _f; \
01367         (o)[Z]=((i)[X]*(m)[2] + (i)[Y]*(m)[6] + (i)[Z]*(m)[10] + (m)[14])* _f; \
01368 }
01369 
01370 /**
01371  * @brief Multiply an Absolute hvect_t 4-Point by a full 4x4 matrix.
01372  * Output and input points should be separate arrays.
01373  */
01374 #define MAT4X4PNT(o, m, i) { \
01375         (o)[X]=(m)[ 0]*(i)[X] + (m)[ 1]*(i)[Y] + (m)[ 2]*(i)[Z] + (m)[ 3]*(i)[H]; \
01376         (o)[Y]=(m)[ 4]*(i)[X] + (m)[ 5]*(i)[Y] + (m)[ 6]*(i)[Z] + (m)[ 7]*(i)[H]; \
01377         (o)[Z]=(m)[ 8]*(i)[X] + (m)[ 9]*(i)[Y] + (m)[10]*(i)[Z] + (m)[11]*(i)[H]; \
01378         (o)[H]=(m)[12]*(i)[X] + (m)[13]*(i)[Y] + (m)[14]*(i)[Z] + (m)[15]*(i)[H]; \
01379 }
01380 
01381 /**
01382  * @brief Apply a 4x4 matrix to a 3-tuple which is a relative Vector
01383  * in space. This macro can scale the length of the vector if [15] !=
01384  * 1.0.  Output and input vectors should be separate arrays.
01385  */
01386 #define MAT4X3VEC(o, m, i) { \
01387         register double _f; \
01388         _f = 1.0/((m)[15]); \
01389         (o)[X] = ((m)[0]*(i)[X] + (m)[1]*(i)[Y] + (m)[ 2]*(i)[Z]) * _f; \
01390         (o)[Y] = ((m)[4]*(i)[X] + (m)[5]*(i)[Y] + (m)[ 6]*(i)[Z]) * _f; \
01391         (o)[Z] = ((m)[8]*(i)[X] + (m)[9]*(i)[Y] + (m)[10]*(i)[Z]) * _f; \
01392 }
01393 
01394 #define MAT4XSCALOR(o, m, i) { \
01395         (o) = (i) / (m)[15]; \
01396 }
01397 
01398 /**
01399  * @brief Multiply a Relative 3-Vector by most of a 4x4 matrix.
01400  * Output and input vectors should be separate arrays.
01401  */
01402 #define VEC3X4MAT(o, i, m) { \
01403         register double _f; \
01404         _f = 1.0/((m)[15]); \
01405         (o)[X] = ((i)[X]*(m)[0] + (i)[Y]*(m)[4] + (i)[Z]*(m)[8]) * _f; \
01406         (o)[Y] = ((i)[X]*(m)[1] + (i)[Y]*(m)[5] + (i)[Z]*(m)[9]) * _f; \
01407         (o)[Z] = ((i)[X]*(m)[2] + (i)[Y]*(m)[6] + (i)[Z]*(m)[10]) * _f; \
01408 }
01409 
01410 /** @brief Multiply a Relative 2-Vector by most of a 4x4 matrix. */
01411 #define VEC2X4MAT(o, i, m) { \
01412         register double _f; \
01413         _f = 1.0/((m)[15]); \
01414         (o)[X] = ((i)[X]*(m)[0] + (i)[Y]*(m)[4]) * _f; \
01415         (o)[Y] = ((i)[X]*(m)[1] + (i)[Y]*(m)[5]) * _f; \
01416         (o)[Z] = ((i)[X]*(m)[2] + (i)[Y]*(m)[6]) * _f; \
01417 }
01418 
01419 /** @brief Test a vector for non-unit length. */
01420 #define BN_VEC_NON_UNIT_LEN(_vec)       \
01421         (fabs(MAGSQ(_vec)) < 0.0001 || fabs(fabs(MAGSQ(_vec))-1) > 0.0001)
01422 
01423 /**
01424  * @brief Included below are macros to update min and max X, Y, Z
01425  * values to contain a point
01426  */
01427 
01428 #define V_MIN(r, s) if ((r) > (s)) r = (s)
01429 
01430 #define V_MAX(r, s) if ((r) < (s)) r = (s)
01431 
01432 #define VMIN(r, s) { \
01433         V_MIN((r)[X], (s)[X]); V_MIN((r)[Y], (s)[Y]); V_MIN((r)[Z], (s)[Z]); \
01434 }
01435 
01436 #define VMAX(r, s) { \
01437         V_MAX((r)[X], (s)[X]); V_MAX((r)[Y], (s)[Y]); V_MAX((r)[Z], (s)[Z]); \
01438 }
01439 
01440 #define VMINMAX(min, max, pt) { \
01441         VMIN((min), (pt)); VMAX((max), (pt)); \
01442 }
01443 
01444 /**
01445  * @brief Divide out homogeneous parameter from hvect_t, creating
01446  * vect_t.
01447  */
01448 #define HDIVIDE(a, b) { \
01449         (a)[X] = (b)[X] / (b)[H]; \
01450         (a)[Y] = (b)[Y] / (b)[H]; \
01451         (a)[Z] = (b)[Z] / (b)[H]; \
01452 }
01453 
01454 /**
01455  * @brief Some 2-D versions of the 3-D macros given above.
01456  *
01457  * A better naming convention is V2MOVE() rather than VMOVE_2D().
01458  *
01459  * DEPRECATED: These xxx_2D names are slated to go away, use the
01460  * others.
01461  *
01462  * THESE ARE ALL DEPRECATED.
01463  */
01464 #define VADD2_2D(a, b, c) V2ADD2(a, b, c)
01465 #define VSUB2_2D(a, b, c) V2SUB2(a, b, c)
01466 #define MAGSQ_2D(a) MAG2SQ(a)
01467 #define VDOT_2D(a, b) V2DOT(a, b)
01468 #define VMOVE_2D(a, b) V2MOVE(a, b)
01469 #define VSCALE_2D(a, b, c) V2SCALE(a, b, c)
01470 #define VJOIN1_2D(a, b, c, d) V2JOIN1(a, b, c, d)
01471 
01472 
01473 /**
01474  * @brief Quaternion math definitions.
01475  *
01476  * Note that the [W] component will be put in the last (i.e., third)
01477  * place rather than the first [X] (i.e., [0]) place, so that the X,
01478  * Y, and Z elements will be compatible with vectors.  Only
01479  * QUAT_FROM_VROT macros depend on component locations, however.
01480  */
01481 
01482 /**
01483  * @brief Create Quaternion from Vector and Rotation about vector.
01484  *
01485  * To produce a quaternion representing a rotation by PI radians about
01486  * X-axis:
01487  *
01488  * VSET(axis, 1, 0, 0);
01489  * QUAT_FROM_VROT(quat, M_PI, axis);
01490  * or
01491  * QUAT_FROM_ROT(quat, M_PI, 1.0, 0.0, 0.0, 0.0);
01492  *
01493  * Alternatively, in degrees:
01494  * QUAT_FROM_ROT_DEG(quat, 180.0, 1.0, 0.0, 0.0, 0.0);
01495  */
01496 #define QUAT_FROM_ROT(q, r, x, y, z) { \
01497         register fastf_t _rot = (r) * 0.5; \
01498         QSET(q, x, y, z, cos(_rot)); \
01499         VUNITIZE(q); \
01500         _rot = sin(_rot); /* _rot is really just a temp variable now */ \
01501         VSCALE(q, q, _rot); \
01502 }
01503 
01504 #define QUAT_FROM_VROT(q, r, v) { \
01505         register fastf_t _rot = (r) * 0.5; \
01506         VMOVE(q, v); \
01507         VUNITIZE(q); \
01508         (q)[W] = cos(_rot); \
01509         _rot = sin(_rot); /* _rot is really just a temp variable now */ \
01510         VSCALE(q, q, _rot); \
01511 }
01512 
01513 #define QUAT_FROM_VROT_DEG(q, r, v) \
01514         QUAT_FROM_VROT(q, ((r)*(M_PI/180.0)), v)
01515 
01516 #define QUAT_FROM_ROT_DEG(q, r, x, y, z) \
01517         QUAT_FROM_ROT(q, ((r)*(M_PI/180.0)), x, y, z)
01518 
01519 
01520 /**
01521  * @brief Set quaternion at `a' to have coordinates `b', `c', `d', and
01522  * `e'.
01523  */
01524 #define QSET(a, b, c, d, e) { \
01525         (a)[X] = (b); \
01526         (a)[Y] = (c); \
01527         (a)[Z] = (d); \
01528         (a)[W] = (e); \
01529 }
01530 
01531 /** @brief Transfer quaternion at `b' to quaternion at `a'. */
01532 #define QMOVE(a, b) { \
01533         (a)[X] = (b)[X]; \
01534         (a)[Y] = (b)[Y]; \
01535         (a)[Z] = (b)[Z]; \
01536         (a)[W] = (b)[W]; \
01537 }
01538 
01539 /** @brief Add quaternions at `b' and `c', store result at `a'. */
01540 #define QADD2(a, b, c) { \
01541         (a)[X] = (b)[X] + (c)[X]; \
01542         (a)[Y] = (b)[Y] + (c)[Y]; \
01543         (a)[Z] = (b)[Z] + (c)[Z]; \
01544         (a)[W] = (b)[W] + (c)[W]; \
01545 }
01546 
01547 /**
01548  * @brief Subtract quaternion at `c' from quaternion at `b', store
01549  * result at `a'.
01550  */
01551 #define QSUB2(a, b, c) { \
01552         (a)[X] = (b)[X] - (c)[X]; \
01553         (a)[Y] = (b)[Y] - (c)[Y]; \
01554         (a)[Z] = (b)[Z] - (c)[Z]; \
01555         (a)[W] = (b)[W] - (c)[W]; \
01556 }
01557 
01558 /**
01559  * @brief Scale quaternion at `b' by scalar `c', store result at
01560  * `a'.
01561  */
01562 #define QSCALE(a, b, c) { \
01563         (a)[X] = (b)[X] * (c); \
01564         (a)[Y] = (b)[Y] * (c); \
01565         (a)[Z] = (b)[Z] * (c); \
01566         (a)[W] = (b)[W] * (c); \
01567 }
01568 
01569 /** @brief Normalize quaternion 'a' to be a unit quaternion. */
01570 #define QUNITIZE(a) { \
01571         register double _f; \
01572         _f = QMAGNITUDE(a); \
01573         if (_f < VDIVIDE_TOL) _f = 0.0; else _f = 1.0/_f; \
01574         (a)[X] *= _f; (a)[Y] *= _f; (a)[Z] *= _f; (a)[W] *= _f; \
01575 }
01576 
01577 /** @brief Return scalar magnitude squared of quaternion at `a'. */
01578 #define QMAGSQ(a) \
01579         ((a)[X]*(a)[X] + (a)[Y]*(a)[Y] \
01580         + (a)[Z]*(a)[Z] + (a)[W]*(a)[W])
01581 
01582 /** @brief Return scalar magnitude of quaternion at `a'. */
01583 #define QMAGNITUDE(a) sqrt(QMAGSQ(a))
01584 
01585 /** @brief Compute dot product of quaternions at `a' and `b'. */
01586 #define QDOT(a, b) \
01587         ((a)[X]*(b)[X] + (a)[Y]*(b)[Y] \
01588         + (a)[Z]*(b)[Z] + (a)[W]*(b)[W])
01589 
01590 /**
01591  * @brief Compute quaternion product a = b * c
01592  *
01593  * a[W] = b[W]*c[W] - VDOT(b, c);
01594  * VCROSS(temp, b, c);
01595  * VJOIN2(a, temp, b[W], c, c[W], b);
01596  */
01597 #define QMUL(a, b, c) { \
01598         (a)[W] = (b)[W]*(c)[W] - (b)[X]*(c)[X] - (b)[Y]*(c)[Y] - (b)[Z]*(c)[Z]; \
01599         (a)[X] = (b)[W]*(c)[X] + (b)[X]*(c)[W] + (b)[Y]*(c)[Z] - (b)[Z]*(c)[Y]; \
01600         (a)[Y] = (b)[W]*(c)[Y] + (b)[Y]*(c)[W] + (b)[Z]*(c)[X] - (b)[X]*(c)[Z]; \
01601         (a)[Z] = (b)[W]*(c)[Z] + (b)[Z]*(c)[W] + (b)[X]*(c)[Y] - (b)[Y]*(c)[X]; \
01602 }
01603 
01604 /** @brief Conjugate quaternion */
01605 #define QCONJUGATE(a, b) { \
01606         (a)[X] = -(b)[X]; \
01607         (a)[Y] = -(b)[Y]; \
01608         (a)[Z] = -(b)[Z]; \
01609         (a)[W] =  (b)[W]; \
01610 }
01611 
01612 /** @brief Multiplicative inverse quaternion */
01613 #define QINVERSE(a, b) { \
01614         register double _f = QMAGSQ(b); \
01615         if (_f < VDIVIDE_TOL) _f = 0.0; else _f = 1.0/_f; \
01616         (a)[X] = -(b)[X] * _f; \
01617         (a)[Y] = -(b)[Y] * _f; \
01618         (a)[Z] = -(b)[Z] * _f; \
01619         (a)[W] =  (b)[W] * _f; \
01620 }
01621 
01622 /**
01623  * @brief Blend into quaternion `a'
01624  *
01625  * scalar `b' times quaternion at `c' plus
01626  * scalar `d' times quaternion at `e'
01627  */
01628 #define QBLEND2(a, b, c, d, e) { \
01629         (a)[X] = (b) * (c)[X] + (d) * (e)[X]; \
01630         (a)[Y] = (b) * (c)[Y] + (d) * (e)[Y]; \
01631         (a)[Z] = (b) * (c)[Z] + (d) * (e)[Z]; \
01632         (a)[W] = (b) * (c)[W] + (d) * (e)[W]; \
01633 }
01634 
01635 /**
01636  * Macros for dealing with 3-D "extents", aka bounding boxes, that are
01637  * represented as axis-aligned right parallelpipeds (RPPs).  This is
01638  * stored as two points: a min point, and a max point.  RPP 1 is
01639  * defined by lo1, hi1, RPP 2 by lo2, hi2.
01640  */
01641 
01642 /**
01643  * Compare two bounding boxes and return true if they are disjoint.
01644  */
01645 #define V3RPP_DISJOINT(_l1, _h1, _l2, _h2) \
01646       ((_l1)[X] > (_h2)[X] || (_l1)[Y] > (_h2)[Y] || (_l1)[Z] > (_h2)[Z] || \
01647         (_l2)[X] > (_h1)[X] || (_l2)[Y] > (_h1)[Y] || (_l2)[Z] > (_h1)[Z])
01648 
01649 /**
01650  * Compare two bounding boxes and return true if they are disjoint
01651  * by at least distance tolerance.
01652  */
01653 #define V3RPP_DISJOINT_TOL(_l1, _h1, _l2, _h2, _t) \
01654       (((_l1)[X] > (_h2)[X] + (_t) && \
01655         (_l1)[Y] > (_h2)[Y] + (_t) && \
01656         (_l1)[Z] > (_h2)[Z] + (_t)) || \
01657        ((_l2)[X] > (_h1)[X] + (_t) && \
01658         (_l2)[Y] > (_h1)[Y] + (_t) && \
01659         (_l2)[Z] > (_h1)[Z] + (_t))) 
01660 
01661 /** Compare two bounding boxes and return true If they overlap. */
01662 #define V3RPP_OVERLAP(_l1, _h1, _l2, _h2) \
01663     (! ((_l1)[X] > (_h2)[X] || (_l1)[Y] > (_h2)[Y] || (_l1)[Z] > (_h2)[Z] || \
01664         (_l2)[X] > (_h1)[X] || (_l2)[Y] > (_h1)[Y] || (_l2)[Z] > (_h1)[Z]))
01665 
01666 /**
01667  * @brief If two extents overlap within distance tolerance, return
01668  * true.
01669  */
01670 #define V3RPP_OVERLAP_TOL(_l1, _h1, _l2, _h2, _t) \
01671     (! ((_l1)[X] > (_h2)[X] + (_t) || \
01672         (_l1)[Y] > (_h2)[Y] + (_t) || \
01673         (_l1)[Z] > (_h2)[Z] + (_t) || \
01674         (_l2)[X] > (_h1)[X] + (_t) || \
01675         (_l2)[Y] > (_h1)[Y] + (_t) || \
01676         (_l2)[Z] > (_h1)[Z] + (_t)))
01677 
01678 /**
01679  * @brief Is the point within or on the boundary of the RPP?
01680  *
01681  * FIXME: should not be using >= <=, '=' case is unreliable
01682  */
01683 #define V3PT_IN_RPP(_pt, _lo, _hi)      (\
01684         (_pt)[X] >= (_lo)[X] && (_pt)[X] <= (_hi)[X] && \
01685         (_pt)[Y] >= (_lo)[Y] && (_pt)[Y] <= (_hi)[Y] && \
01686         (_pt)[Z] >= (_lo)[Z] && (_pt)[Z] <= (_hi)[Z])
01687 
01688 /**
01689  * @brief Within the distance tolerance, is the point within the RPP?
01690  *
01691  * FIXME: should not be using >= <=, '=' case is unreliable
01692  */
01693 #define V3PT_IN_RPP_TOL(_pt, _lo, _hi, _t)      (\
01694         (_pt)[X] >= (_lo)[X]-(_t) && (_pt)[X] <= (_hi)[X]+(_t) && \
01695         (_pt)[Y] >= (_lo)[Y]-(_t) && (_pt)[Y] <= (_hi)[Y]+(_t) && \
01696         (_pt)[Z] >= (_lo)[Z]-(_t) && (_pt)[Z] <= (_hi)[Z]+(_t))
01697 
01698 /**
01699  * @brief Is the point outside the RPP by at least the distance tolerance?
01700  * This will not return true if the point is on the RPP.
01701  */
01702 #define V3PT_OUT_RPP_TOL(_pt, _lo, _hi, _t)      (\
01703         (_pt)[X] < (_lo)[X]-(_t) || (_pt)[X] > (_hi)[X]+(_t) || \
01704         (_pt)[Y] < (_lo)[Y]-(_t) || (_pt)[Y] > (_hi)[Y]+(_t) || \
01705         (_pt)[Z] < (_lo)[Z]-(_t) || (_pt)[Z] > (_hi)[Z]+(_t))
01706 
01707 /**
01708  * @brief Determine if one bounding box is within another.  Also
01709  * returns true if the boxes are the same.
01710  *
01711  * FIXME: should not be using >= <=, '=' case is unreliable
01712  */
01713 #define V3RPP1_IN_RPP2(_lo1, _hi1, _lo2, _hi2)  (\
01714         (_lo1)[X] >= (_lo2)[X] && (_hi1)[X] <= (_hi2)[X] && \
01715         (_lo1)[Y] >= (_lo2)[Y] && (_hi1)[Y] <= (_hi2)[Y] && \
01716         (_lo1)[Z] >= (_lo2)[Z] && (_hi1)[Z] <= (_hi2)[Z])
01717 
01718 /** Convert an azimuth/elevation to a direction vector. */
01719 #define V3DIR_FROM_AZEL(_d, _a, _e) { \
01720         register fastf_t _c_e = cos(_e); \
01721         (_d)[X] = cos(_a) * _c_e; \
01722         (_d)[Y] = sin(_a) * _c_e; \
01723         (_d)[Z] = sin(_e); \
01724 }
01725 
01726 /** Convert a direction vector to azimuth/elevation (in radians). */
01727 #define AZEL_FROM_V3DIR(_a, _e, _d) { \
01728         (_a) = ((NEAR_ZERO((_d)[X], SMALL_FASTF)) && (NEAR_ZERO((_d)[Y], SMALL_FASTF))) ? 0.0 : atan2(-((_d)[Y]), -((_d)[X])) * -RAD2DEG; \
01729         (_e) = atan2(-((_d)[Z]), sqrt((_d)[X]*(_d)[X] + (_d)[Y]*(_d)[Y])) * -RAD2DEG; \
01730 }
01731 
01732 /*** Macros suitable for declaration statement initialization. ***/
01733 
01734 /**
01735  * 3D vector macro suitable for declaration statement initialization.
01736  * this sets all vector elements to the specified value similar to
01737  * VSETALL() but as an initializer array declaration instead of as a
01738  * statement.
01739  */
01740 #define VINITALL(_v) {(_v), (_v), (_v)}
01741 
01742 /**
01743  * 2D vector macro suitable for declaration statement initialization.
01744  * this sets all vector elements to the specified value similar to
01745  * VSETALLN(hvect_t,val,2) but as an initializer array declaration
01746  * instead of as a statement.
01747  */
01748 #define V2INITALL(_v) {(_v), (_v), (_v)}
01749 
01750 /**
01751  * 4D homogeneous vector macro suitable for declaration statement
01752  * initialization.  this sets all vector elements to the specified
01753  * value similar to VSETALLN(hvect_t,val,4) but as an initializer
01754  * array declaration instead of as a statement.
01755  */
01756 #define HINITALL(_v) {(_v), (_v), (_v), (_v)}
01757 
01758 /**
01759  * 3D vector macro suitable for declaration statement initialization.
01760  * this sets all vector elements to zero similar to calling
01761  * VSETALL(0.0) but as an initializer array declaration instead of as
01762  * a statement.
01763  */
01764 #define VINIT_ZERO {0.0, 0.0, 0.0}
01765 
01766 /**
01767  * 2D vector macro suitable for declaration statement initialization.
01768  * this sets all vector elements to zero similar to calling
01769  * V2SETALL(0.0) but as an initializer array declaration instead of as
01770  * a statement.
01771  */
01772 #define V2INIT_ZERO {0.0, 0.0}
01773 
01774 /**
01775  * 4D homogenous vector macro suitable for declaration statement
01776  * initialization.  this sets all vector elements to zero similar to
01777  * calling VSETALLN(hvect_t,0.0,4) but as an initializer array
01778  * declaration instead of as a statement.
01779  */
01780 #define HINIT_ZERO {0.0, 0.0, 0.0, 0.0}
01781 
01782 /**
01783  * matrix macro suitable for declaration statement initialization.
01784  * this sets up an identity matrix similar to calling MAT_IDN but as
01785  * an initializer array declaration instead of as a statement.
01786  */
01787 #define MAT_INIT_IDN {1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0}
01788 
01789 /**
01790  * matrix macro suitable for declaration statement initialization.
01791  * this sets up a zero matrix similar to calling MAT_ZERO but as an
01792  * initializer array declaration instead of as a statement.
01793  */
01794 #define MAT_INIT_ZERO {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}
01795 
01796 __END_DECLS
01797 
01798 #endif /* __VMATH_H__ */
01799 
01800 /** @} */
01801 /*
01802  * Local Variables:
01803  * mode: C
01804  * tab-width: 8
01805  * indent-tabs-mode: t
01806  * c-file-style: "stroustrup"
01807  * End:
01808  * ex: shiftwidth=4 tabstop=8
01809  */
Generated on Tue Dec 11 13:14:27 2012 for LIBBN by  doxygen 1.6.3