matvec.h

   1 /* Matrix and vector arithmetic.
   2  * By Richard W.M. Jones <rich@annexia.org>
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Library General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2 of the License, or (at your option) any later version.
   8  *
   9  * This library is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * Library General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU Library General Public
  15  * License along with this library; if not, write to the Free
  16  * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  17  *
  18  * $Id: matvec.h,v 1.3 2001/11/19 17:10:54 rich Exp $
  19  */
  20
  21 #ifndef MATVEC_H
  22 #define MATVEC_H
  23
  24 #include <pool.h>
  25 #include <math.h>
  26
  27 /* Notes:
  28  *
  29  * This library only handles 4x4 matrices and 4-vectors, for
  30  * basic 3D graphics use. A matrix is just an array float[16]
  31  * and a vector is just an array float[4]. You can either allocate
  32  * these in a pool using ``new_matrix'' and ``new_vec'', or
  33  * else you can allocate them statically.
  34  *
  35  * All matrices are stored in COLUMN-MAJOR ORDER! This is for
  36  * compatibility with OpenGL, but it is the OPPOSITE of the
  37  * normal C row-major ordering, so beware. Matrix elements in
  38  * column-major order are as follows:
  39  *
  40  *   M = | m0  m4  m8  m12 |
  41  *       | m1  m5  m9  m13 |
  42  *       | m2  m6  m10 m14 |
  43  *       | m3  m7  m11 m15 |
  44  *
  45  * Some of these functions have been inlined. I only inline
  46  * functions based on evidence from profiling the code as a
  47  * whole or where the function is so simple that the overhead
  48  * of calling it is larger than the inlined code. Excessive
  49  * inlining can itself cause performance problems, particularly
  50  * on modern processors which are very good at making jumps
  51  * and function calls.
  52  *
  53  */
  54
  55 /* Function: new_matrix - allocate a new matrix or vector
  56  * Function: new_vec
  57  *
  58  * @code{new_matrix} allocates a new 4x4 matrix of floats
  59  * in @code{pool}.
  60  *
  61  * @code{new_vec} allocates a new 4-vector of floats in
  62  * @code{pool}.
  63  *
  64  * You may use these functions to allocate matrices and
  65  * vectors dynamically, or you may allocate them statically.
  66  * The other matrix and vector functions available do not
  67  * distriguish between dynamically and statically allocated
  68  * variables.
  69  *
  70  * Note: All matrices are stored in COLUMN-MAJOR ORDER!
  71  * This is for compatibility with OpenGL, but it is the
  72  * OPPOSITE of the natural C row-major ordering, so beware.
  73  *
  74  * See also: @ref{new_identity_matrix(3)}, @ref{new_zero_vec(3)}.
  75  */
  76 #define new_matrix(pool) ((float *) pmalloc ((pool), sizeof (float) * 16))
  77 #define new_vec(pool) ((float *) pmalloc ((pool), sizeof (float) * 4))
  78
  79 /* Variable: identity_matrix - identity matrix and zero vector
  80  * Variable: zero_vec
  81  * Function: new_identity_matrix
  82  * Function: new_zero_vec
  83  * Function: make_identity_matrix
  84  * Function: make_zero_vec
  85  *
  86  * The @code{identity_matrix} variable contains a read-only
  87  * copy of the identity matrix. The @code{zero_vec} variable
  88  * contains a read-only copy of the zero vector.
  89  *
  90  * Use @code{new_identity_matrix} to allocate a new
  91  * identity matrix variable in @code{pool}. Use @code{new_zero_vec}
  92  * to similarly allocate a new zero vector.
  93  *
  94  * Use @code{make_identity_matrix} to copy the identity
  95  * matrix over an existing matrix @code{m}. Use @code{make_zero_vec}
  96  * to similarly copy the zero vector over an existing vector @code{v}.
  97  *
  98  * See also: @ref{new_matrix(3)}, @ref{new_vec(3)}.
  99  */
 100 extern float identity_matrix[16];
 101 extern float zero_vec[4];
 102 extern float *new_identity_matrix (pool);
 103 extern float *new_zero_vec (pool);
 104 #define make_identity_matrix(m) memcpy (m, identity_matrix, sizeof(float)*16);
 105 #define make_zero_vec(v) memcpy (v, zero_vec, sizeof (float) * 4);
 106
 107 extern void make_rotation_matrix (float angle,
 108                                   float x, float y, float z,
 109                                   float *m);
 110
 111 extern void make_translation_matrix (float x, float y, float z, float *m);
 112
 113 extern void make_scaling_matrix (float x, float y, float z, float *m);
 114
 115 extern void matrix_euler_to_rotation (float angle_x, float angle_y,
 116                                       float angle_z,
 117                                       float *mat);
 118 extern void matrix_rotation_to_euler (const float *mat,
 119                                       float *angle_x, float *angle_y,
 120                                       float *angle_z);
 121
 122 extern void matrix_multiply (const float *a, const float *b,
 123                              float *product);
 124
 125 extern void matrix_vec_multiply (const float *m, const float *v,
 126                                  float *result);
 127
 128 /* Function: vec_magnitude - calculate magnitude (length) of a vector
 129  * Function: vec_magnitude2d
 130  *
 131  * @code{vec_magnitude} calculates the magnitude (length) of a
 132  * 3D vector. @code{vec_magnitude2d} calculates the magnitude
 133  * of a 2D vector.
 134  *
 135  * See also: @ref{vec_normalize(3)}, @ref{vec_normalize2d(3)}.
 136  */
 137 extern float vec_magnitude (const float *v);
 138 extern float vec_magnitude2d (const float *v);
 139
 140 /* Function: vec_normalize - normalize a vector
 141  * Function: vec_normalize2d
 142  *
 143  * These two functions normalize respectively a 3D or 2D
 144  * vector @code{v}. The original vector @code{v} is not touched,
 145  * and the result is placed in vector @code{r}.
 146  *
 147  * To normalize a vector in-place (ie. modifying the original
 148  * vector), do:
 149  *
 150  * @code{vec_normalize (v, v);}
 151  *
 152  * See also: @ref{vec_magnitude(3)}, @ref{vec_magnitude2d(3)}.
 153  */
 154 extern void vec_normalize (const float *v, float *r);
 155 extern void vec_normalize2d (const float *v, float *r);
 156
 157 extern void vec_unit_normal_to_side (float *side, float *normal);
 158
 159 /* Function: vec_dot_product - calculate the dot product of two vectors
 160  *
 161  * @code{vec_dot_product} calculates the dot product of two
 162  * vectors @code{v1} and @code{v2} and returns it. The dot
 163  * product is formally defined as:
 164  *
 165  * @code{|v1| |v2| cos theta}
 166  *
 167  * where @code{theta} is the angle between the two vectors.
 168  *
 169  * One interesting consequence of this is that if both @code{v1}
 170  * and @code{v2} are already normalized, then the dot product
 171  * is 1 if the vectors are parallel and running in the same
 172  * direction, and 0 if the vectors are
 173  * perpendicular.
 174  *
 175  * See also: @ref{vec_magnitude_in_direction(3)}, @ref{vec_angle_between(3)}
 176  */
 177 extern float vec_dot_product (const float *v1, const float *v2);
 178
 179 /* Function: vec_magnitude_in_direction - calculate relative direction of two vectors
 180  *
 181  * If @code{v1} and @code{v2} are parallel and point in the
 182  * same direction, then @code{vec_magnitude_in_direction} returns +1.
 183  * If @code{v1} and @code{v2} are perpendicular, this returns
 184  * 0. If @code{v1} and @code{v2} are parallel and point in
 185  * opposite directions to each other, this returns -1.
 186  * For other vectors, this function returns the cosine of
 187  * the angle between the vectors.
 188  *
 189  * See also: @ref{vec_dot_product(3)}, @ref{vec_angle_between(3)}
 190  */
 191 extern float vec_magnitude_in_direction (const float *v1, const float *v2);
 192
 193 /* Function: vec_angle_between - calculate the angle between two vectors
 194  *
 195  * This function returns the angle between two vectors
 196  * @code{v1} and @code{v2}.
 197  *
 198  * See also: @ref{vec_dot_product(3)}, @ref{vec_magnitude_in_direction(3)}
 199  */
 200 extern float vec_angle_between (const float *v1, const float *v2);
 201
 202 extern void vec_cross_product (const float *v, const float *w, float *r);
 203
 204 extern void vec_scale (const float *a, float n, float *r);
 205 extern void vec_add (const float *a, const float *b, float *r);
 206 extern void vec_subtract (const float *a, const float *b, float *r);
 207
 208 float point_distance_to_point (const float *p1, const float *p2);
 209
 210 extern void point_midpoint (const float *p1, const float *p2, float *mp);
 211 extern void point_midpoint2d (const float *p1, const float *p2, float *mp);
 212 extern float point_distance (const float *p, const float *q);
 213
 214 /* Function: point_distance_to_plane - distance from point to plane
 215  * Function: point_is_inside_plane
 216  *
 217  * @code{point_distance_to_plane} calculates the (shortest) distance from
 218  * the point @code{point} to the plane @code{plane}. This distance is
 219  * positive if the point is "inside" the plane -- that is, if the
 220  * normal vector drawn from the plane points towards the point. It
 221  * is negative if the point is "outside" the plane. It is zero if the
 222  * point lies on the plane.
 223  *
 224  * @code{point_is_inside_plane} returns true if the point is strictly
 225  * inside the plane, and false if the point lies on the plane or is
 226  * outside. It is much faster to compute this than to use
 227  * @code{point_distance_to_plane} and take the sign of the result.
 228  *
 229  * See also: @ref{plane_coefficients(3)}, @ref{point_distance_to_face(3)}.
 230  */
 231 extern float point_distance_to_plane (const float *plane, const float *point);
 232 extern int point_is_inside_plane (const float *plane, const float *point);
 233
 234 extern void point_footprint_on_line (const float *point, const float *line_point, const float *line_vector, float *footprint);
 235
 236 /* Function: point_distance_to_line - shortest distance from a point to a line
 237  *
 238  * Given a @code{point} and a line, expressed as @code{line_point}
 239  * and parallel @code{line_vector}, compute the shortest distance
 240  * from the point to the line.
 241  *
 242  * See also: @ref{point_distance_to_plane(3)}, @ref{point_distance_to_face(3)},
 243  * @ref{point_distance_to_line_segment(3)}.
 244  */
 245 extern float point_distance_to_line (const float *point, const float *line_point, const float *line_vector);
 246
 247 /* Function: point_distance_to_line_segment - shortest distance from a point to a line segment
 248  *
 249  * Given a @code{point} and a line segment from @code{line_point0} to
 250  * @code{line_point1}, compute the shortest distance from the
 251  * point to the line segment.
 252  *
 253  * See also: @ref{point_distance_to_line(3)}.
 254  */
 255 extern float point_distance_to_line_segment (const float *point, const float *line_point0, const float *line_point1);
 256
 257 /* Function: point_lies_in_face - does a point lie on the interior of a bounded convex polygon
 258  * Function: point_face_angle_sum
 259  *
 260  * Take a bounded convex polygon (a "face") and a point. The function
 261  * @code{point_lies_in_face} returns true iff the point is both
 262  * (a) coplanar with the face, and
 263  * (b) lies inside the edges of the face.
 264  *
 265  * In order to do this, @code{point_lies_in_face} calls
 266  * @code{point_face_angle_sum} which works out the sum of
 267  * the interior angles. If conditions (a) and (b) are both
 268  * satisfied then the sum of the interior angles will be
 269  * very close to 2.PI.
 270  *
 271  * The face is expressed as a flat list of points (3-vectors).
 272  *
 273  * See also: @ref{plane_coefficients(3)}, @ref{point_distance_to_face(3)}.
 274  */
 275 extern int point_lies_in_face (const float *points, int nr_points, const float *point);
 276 extern float point_face_angle_sum (const float *points, int nr_points, const float *point);
 277
 278 /* Function: point_distance_to_face - distance from point to bounded convex polygon (face)
 279  *
 280  * Given a point and a bounded convex polygon (a "face"), the
 281  * function @code{point_distance_to_face} calculates the distance
 282  * from the point to the face. There are two importance cases to
 283  * consider here:
 284  *
 285  * (a) The point is directly above or below the face. In other words,
 286  * a line dropped from the point perpendicular to the face intersects
 287  * the face within the boundary of the polygon. In this case, the
 288  * function returns the shortest distance from the point to the
 289  * intersection (and is essentially equivalent to
 290  * @code{point_distance_to_plane}).
 291  *
 292  * (b) The point is not directly above or below the face. In this case
 293  * the function works out the distance to the nearest edge of the face.
 294  *
 295  * The face is specified as a list of points and a plane (ie.
 296  * plane coefficients). If @code{plane} is @code{NULL}, then the
 297  * function calls @ref{plane_coefficients(3)} on your behalf. If
 298  * the face is fixed, and you will call this function lots of
 299  * times, then it is a good idea to calculate the plane coefficients
 300  * once only and cache them.
 301  *
 302  * Returns: The distance of the point from the face. The distance
 303  * will be positive if the point is above the face (ie. inside
 304  * the plane: see @ref{point_distance_to_plane(3)}), or negative
 305  * otherwise.
 306  *
 307  * If @code{edge} is not @code{NULL}, then it is set to one of
 308  * the following values:
 309  *
 310  * @code{*edge == -1} if the point is directly above or below
 311  * the face, corresponding to case (a) above.
 312  *
 313  * @code{*edge == 0 .. nr_points-1} if the point is closest to
 314  * that particular edge, corresponding to case (b) above.
 315  *
 316  * See also: @ref{point_distance_to_plane(3)},
 317  * @ref{plane_coefficients(3)}, @ref{point_lies_in_face(3)},
 318  * @ref{point_distance_to_line(3)}.
 319  */
 320 extern float point_distance_to_face (const float *points, int nr_points, const float *plane, const float *point, int *edge);
 321
 322 /* Function: plane_coefficients - calculate the coefficient form for a plane
 323  *
 324  * Given three points, not colinear, which naturally form a plane, calculate
 325  * the 4-vector form for the plane coefficients. The three points
 326  * are passed as @code{p}, @code{q} and @code{r}. The coefficients
 327  * are returned in vector @code{co}.
 328  *
 329  * The four coefficients returned are respectively @code{a}, @code{b},
 330  * @code{c} and @code{d} in the standard plane equation:
 331  *
 332  * @code{a x + b y + c z + d = 0}
 333  *
 334  * (Note that many texts use @code{- d}, so be warned).
 335  *
 336  * The normal (perpendicular) vector to the plane may be derived
 337  * immediately: it is just @code{(a, b, c)}. Note that the normal
 338  * vector is not normalized!
 339  *
 340  * Planes are unbounded: they stretch off to infinity in all directions.
 341  * If what you really want are bounded convex polygons, then you
 342  * need to use a c2lib "face".
 343  */
 344 extern void plane_coefficients (const float *p, const float *q, const float *r, float *co);
 345
 346 /* Function: plane_translate_along_normal - translate a plane or face some distance in the direction of the normal
 347  * Function: face_translate_along_normal
 348  *
 349  * Given an existing @code{plane} (expressed as plane coefficients),
 350  * produce a new plane @code{new_plane} which has been translated
 351  * by @code{distance} units along the direction of the normal. The
 352  * new plane is also returned as plane coefficients.
 353  *
 354  * @code{face_translate_along_normal} is similar, except that it also
 355  * translates a list of points by the same distance.
 356  *
 357  * See also: @ref{plane_coefficients(3)}.
 358  */
 359 extern void plane_translate_along_normal (const float *plane, float distance, float *new_plane);
 360 extern void face_translate_along_normal (const float *points, int nr_points, const float *plane, float distance, float *new_points, float *new_plane);
 361
 362 extern void quaternion_normalize (const float *a, float *r);
 363 extern void quaternion_conjugate (const float *a, float *r);
 364 extern float quaternion_magnitude (const float *a);
 365 extern void quaternion_multiply (const float *a, const float *b, float *r);
 366 extern void quaternion_to_rotation_matrix (const float *q, float *mat);
 367 extern void make_quaternion_from_axis_angle (const float *axis, float angle,
 368                                              float *q);
 369
 370 /* Function: collision_moving_sphere_and_face - detect collision between a moving sphere and a fixed face
 371  *
 372  * This function detects collisions between a sphere which is moving
 373  * at constant speed along a linear path and a fixed bounded
 374  * convex polygon ("face").
 375  *
 376  * The centre of the sphere moves from point @code{p0} to point @code{p1}.
 377  * The sphere has radius @code{radius}.
 378  *
 379  * The face is described by the list of bounding points, and the plane
 380  * coefficients of the plane of the face (you may pass @code{plane}
 381  * as @code{NULL} in which case the function works out the plane
 382  * coefficients for you, although this is generally less efficient).
 383  *
 384  * Returns: If there was a collision, this function returns true and
 385  * sets the collision point in @code{collision_point}. Note that the
 386  * collision point is the position of the centre of the sphere at
 387  * the point of collision, NOT the place where the sphere touches
 388  * the face. If there was no collision, this function returns false.
 389  */
 390 extern int collision_moving_sphere_and_face (const float *p0, const float *p1, float radius, const float *points, int nr_points, const float *plane, float *collision_point);
 391
 392 #endif /* MATVEC_H */