matvec.c

   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.c,v 1.5 2002/05/04 11:54:23 rich Exp $
  19  */
  20
  21 #include "config.h"
  22
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <math.h>
  26
  27 #ifdef HAVE_STRING_H
  28 #include <string.h>
  29 #endif
  30
  31 #include "matvec.h"
  32
  33 float identity_matrix[16] = {
  34   1, 0, 0, 0,
  35   0, 1, 0, 0,
  36   0, 0, 1, 0,
  37   0, 0, 0, 1
  38 };
  39
  40 float zero_vec[4] = { 0, 0, 0, 1 };
  41
  42 float *
  43 new_identity_matrix (pool pool)
  44 {
  45   float *m = new_matrix (pool);
  46   make_identity_matrix (m);
  47   return m;
  48 }
  49
  50 float *
  51 new_zero_vec (pool pool)
  52 {
  53   float *v = new_vec (pool);
  54   make_zero_vec (v);
  55   return v;
  56 }
  57
  58 /* This code is taken from Mesa 3.0. I have exchanged degrees for radians. */
  59 void
  60 make_rotation_matrix (float angle,
  61                       float x, float y, float z,
  62                       float *m)
  63 {
  64   /* This function contributed by Erich Boleyn (erich@uruk.org) */
  65   float mag, s, c;
  66   float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
  67
  68   s = sin(angle);
  69   c = cos(angle);
  70
  71   mag = sqrt ( x*x + y*y + z*z );
  72
  73   if (mag == 0.0) {
  74     /* generate an identity matrix and return */
  75     make_identity_matrix (m);
  76     return;
  77   }
  78
  79   x /= mag;
  80   y /= mag;
  81   z /= mag;
  82
  83 #define M(row,col)  m[col*4+row]
  84
  85   /*
  86    *     Arbitrary axis rotation matrix.
  87    *
  88    *  This is composed of 5 matrices, Rz, Ry, T, Ry', Rz', multiplied
  89    *  like so:  Rz * Ry * T * Ry' * Rz'.  T is the final rotation
  90    *  (which is about the X-axis), and the two composite transforms
  91    *  Ry' * Rz' and Rz * Ry are (respectively) the rotations necessary
  92    *  from the arbitrary axis to the X-axis then back.  They are
  93    *  all elementary rotations.
  94    *
  95    *  Rz' is a rotation about the Z-axis, to bring the axis vector
  96    *  into the x-z plane.  Then Ry' is applied, rotating about the
  97    *  Y-axis to bring the axis vector parallel with the X-axis.  The
  98    *  rotation about the X-axis is then performed.  Ry and Rz are
  99    *  simply the respective inverse transforms to bring the arbitrary
 100    *  axis back to it's original orientation.  The first transforms
 101    *  Rz' and Ry' are considered inverses, since the data from the
 102    *  arbitrary axis gives you info on how to get to it, not how
 103    *  to get away from it, and an inverse must be applied.
 104    *
 105    *  The basic calculation used is to recognize that the arbitrary
 106    *  axis vector (x, y, z), since it is of unit length, actually
 107    *  represents the sines and cosines of the angles to rotate the
 108    *  X-axis to the same orientation, with theta being the angle about
 109    *  Z and phi the angle about Y (in the order described above)
 110    *  as follows:
 111    *
 112    *  cos ( theta ) = x / sqrt ( 1 - z^2 )
 113    *  sin ( theta ) = y / sqrt ( 1 - z^2 )
 114    *
 115    *  cos ( phi ) = sqrt ( 1 - z^2 )
 116    *  sin ( phi ) = z
 117    *
 118    *  Note that cos ( phi ) can further be inserted to the above
 119    *  formulas:
 120    *
 121    *  cos ( theta ) = x / cos ( phi )
 122    *  sin ( theta ) = y / sin ( phi )
 123    *
 124    *  ...etc.  Because of those relations and the standard trigonometric
 125    *  relations, it is pssible to reduce the transforms down to what
 126    *  is used below.  It may be that any primary axis chosen will give the
 127    *  same results (modulo a sign convention) using thie method.
 128    *
 129    *  Particularly nice is to notice that all divisions that might
 130    *  have caused trouble when parallel to certain planes or
 131    *  axis go away with care paid to reducing the expressions.
 132    *  After checking, it does perform correctly under all cases, since
 133    *  in all the cases of division where the denominator would have
 134    *  been zero, the numerator would have been zero as well, giving
 135    *  the expected result.
 136    */
 137
 138   xx = x * x;
 139   yy = y * y;
 140   zz = z * z;
 141   xy = x * y;
 142   yz = y * z;
 143   zx = z * x;
 144   xs = x * s;
 145   ys = y * s;
 146   zs = z * s;
 147   one_c = 1.0F - c;
 148
 149   M(0,0) = (one_c * xx) + c;
 150   M(0,1) = (one_c * xy) - zs;
 151   M(0,2) = (one_c * zx) + ys;
 152   M(0,3) = 0.0F;
 153
 154   M(1,0) = (one_c * xy) + zs;
 155   M(1,1) = (one_c * yy) + c;
 156   M(1,2) = (one_c * yz) - xs;
 157   M(1,3) = 0.0F;
 158
 159   M(2,0) = (one_c * zx) - ys;
 160   M(2,1) = (one_c * yz) + xs;
 161   M(2,2) = (one_c * zz) + c;
 162   M(2,3) = 0.0F;
 163
 164   M(3,0) = 0.0F;
 165   M(3,1) = 0.0F;
 166   M(3,2) = 0.0F;
 167   M(3,3) = 1.0F;
 168
 169 #undef M
 170 }
 171
 172 void
 173 make_translation_matrix (float x, float y, float z, float *m)
 174 {
 175   make_identity_matrix (m);
 176   m[12] = x;
 177   m[13] = y;
 178   m[14] = z;
 179 }
 180
 181 void
 182 make_scaling_matrix (float x, float y, float z, float *m)
 183 {
 184   make_identity_matrix (m);
 185   m[0] = x;
 186   m[5] = y;
 187   m[10] = z;
 188 }
 189
 190 /* The next two functions are from the matrix FAQ by <hexapod@netcom.com>, see:
 191  * http://www.flipcode.com/documents/matrfaq.html
 192  */
 193
 194 /* Quickly convert 3 Euler angles to a rotation matrix. */
 195 void
 196 matrix_euler_to_rotation (float angle_x, float angle_y, float angle_z,
 197                           float *mat)
 198 {
 199   float A       = cos(angle_x);
 200   float B       = sin(angle_x);
 201   float C       = cos(angle_y);
 202   float D       = sin(angle_y);
 203   float E       = cos(angle_z);
 204   float F       = sin(angle_z);
 205
 206   float AD      =   A * D;
 207   float BD      =   B * D;
 208
 209   mat[0]  =   C * E;
 210   mat[4]  =  -C * F;
 211   mat[8]  =  -D;
 212   mat[1]  = -BD * E + A * F;
 213   mat[5]  =  BD * F + A * E;
 214   mat[9]  =  -B * C;
 215   mat[2]  =  AD * E + B * F;
 216   mat[6]  = -AD * F + B * E;
 217   mat[10] =   A * C;
 218
 219   mat[12]  =  mat[13] = mat[14] = mat[3] = mat[7] = mat[11] = 0;
 220   mat[15] =  1;
 221 }
 222
 223 static inline float
 224 clamp (float v, float low, float high)
 225 {
 226   /* This is not very efficient ... */
 227   v -= low;
 228   while (v > high - low) v -= high - low;
 229   while (v < 0) v += high - low;
 230   v += low;
 231   return v;
 232 }
 233
 234 /* Convert a rotation matrix to 3 Euler angles. */
 235 void
 236 matrix_rotation_to_euler (const float *mat,
 237                           float *angle_x, float *angle_y, float *angle_z)
 238 {
 239   float C, trx, try;
 240
 241   *angle_y = -asin( mat[8]);        /* Calculate Y-axis angle */
 242   C           =  cos( *angle_y );
 243
 244   if ( fabs( C ) > 0.005 )             /* Gimball lock? */
 245     {
 246       trx      =  mat[10] / C;           /* No, so get X-axis angle */
 247       try      = -mat[9]  / C;
 248
 249       *angle_x  = atan2( try, trx );
 250
 251       trx      =  mat[0] / C;            /* Get Z-axis angle */
 252       try      = -mat[4] / C;
 253
 254       *angle_z  = atan2( try, trx );
 255     }
 256   else                                 /* Gimball lock has occurred */
 257     {
 258       trx      = mat[5];                 /* And calculate Z-axis angle */
 259       try      = mat[1];
 260
 261       *angle_z  = atan2( try, trx );
 262     }
 263
 264   /* Clamp all angles to range */
 265   *angle_x = clamp (*angle_x, 0, 2 * M_PI);
 266   *angle_y = clamp (*angle_y, 0, 2 * M_PI);
 267   *angle_z = clamp (*angle_z, 0, 2 * M_PI);
 268 }
 269
 270 /* This code is taken from Mesa 3.0.
 271  */
 272 void
 273 matrix_multiply (const float *a, const float *b,
 274                  float *product)
 275 {
 276    /* This matmul was contributed by Thomas Malik */
 277   int i;
 278
 279 #define A(row,col)  a[(col<<2)+row]
 280 #define B(row,col)  b[(col<<2)+row]
 281 #define P(row,col)  product[(col<<2)+row]
 282
 283    /* i-te Zeile */
 284    for (i = 0; i < 4; i++) {
 285      float ai0=A(i,0),  ai1=A(i,1),  ai2=A(i,2),  ai3=A(i,3);
 286      P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
 287      P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
 288      P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
 289      P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
 290    }
 291
 292 #undef A
 293 #undef B
 294 #undef P
 295 }
 296
 297 /* Multiply matrix by vector. */
 298 void
 299 matrix_vec_multiply (const float *m, const float *v,
 300                      float *result)
 301 {
 302   result[0] = m[0]*v[0] + m[1]*v[1] + m[2]*v[2] + m[3]*v[3];
 303   result[1] = m[4]*v[0] + m[5]*v[1] + m[6]*v[2] + m[7]*v[3];
 304   result[2] = m[8]*v[0] + m[9]*v[1] + m[10]*v[2] + m[11]*v[3];
 305   result[3] = m[12]*v[0] + m[13]*v[1] + m[14]*v[2] + m[15]*v[3];
 306 }
 307
 308 /* Compute the magnitude of a vector. */
 309 float
 310 vec_magnitude (const float *v)
 311 {
 312   return sqrt (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
 313 }
 314
 315 /* Compute the magnitude of a 2D vector. */
 316 float
 317 vec_magnitude2d (const float *v)
 318 {
 319   return sqrt (v[0]*v[0] + v[1]*v[1]);
 320 }
 321
 322 /* Normalize a vector. */
 323 void
 324 vec_normalize (const float *v, float *r)
 325 {
 326   float w = vec_magnitude (v);
 327   r[0] = v[0] / w;
 328   r[1] = v[1] / w;
 329   r[2] = v[2] / w;
 330 }
 331
 332 /* Normalize a 2D vector. */
 333 void
 334 vec_normalize2d (const float *v, float *r)
 335 {
 336   float w = vec_magnitude2d (v);
 337   r[0] = v[0] / w;
 338   r[1] = v[1] / w;
 339 }
 340
 341 void
 342 vec_unit_normal_to_side (float *side, float *normal)
 343 {
 344   float n[3] = { side[0], side[1], side[2] };
 345   vec_normalize (n, normal);
 346 }
 347
 348 /* Compute the dot product of two vectors. */
 349 float
 350 vec_dot_product (const float *v1, const float *v2)
 351 {
 352   return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
 353 }
 354
 355 /* Compute the magnitude of vector v1 in direction vector v2.
 356  * If v1 == v2, this returns 1. If v1 = -v2, this returns -1.
 357  * If v1 is perpendicular to v2 this returns 0.
 358  */
 359 float
 360 vec_magnitude_in_direction (const float *v1, const float *v2)
 361 {
 362   static float v1n[3], v2n[3];
 363
 364   /* Normalize both vectors. */
 365   vec_normalize (v1, v1n);
 366   vec_normalize (v2, v2n);
 367
 368   /* Return their dot product. */
 369   return vec_dot_product (v1n, v2n);
 370 }
 371
 372 /* Compute angle between two vectors. */
 373 float
 374 vec_angle_between (const float *v1, const float *v2)
 375 {
 376   return acos (vec_magnitude_in_direction (v1, v2));
 377 }
 378
 379 /* Scale a vector. */
 380 void
 381 vec_scale (const float *a, float n, float *r)
 382 {
 383   r[0] = a[0] * n;
 384   r[1] = a[1] * n;
 385   r[2] = a[2] * n;
 386 }
 387
 388 /* Add two vectors. */
 389 void
 390 vec_add (const float *a, const float *b, float *r)
 391 {
 392   r[0] = a[0] + b[0];
 393   r[1] = a[1] + b[1];
 394   r[2] = a[2] + b[2];
 395 }
 396
 397 /* Subtract two vectors. */
 398 void
 399 vec_subtract (const float *a, const float *b, float *r)
 400 {
 401   r[0] = a[0] - b[0];
 402   r[1] = a[1] - b[1];
 403   r[2] = a[2] - b[2];
 404 }
 405
 406 /* Calculate midpoint. */
 407 void
 408 point_midpoint (const float *p1, const float *p2, float *mp)
 409 {
 410   mp[0] = (p1[0] + p2[0]) / 2;
 411   mp[1] = (p1[1] + p2[1]) / 2;
 412   mp[2] = (p1[2] + p2[2]) / 2;
 413 }
 414
 415 /* Calculate midpoint (in 2D). */
 416 void
 417 point_midpoint2d (const float *p1, const float *p2, float *mp)
 418 {
 419   mp[0] = (p1[0] + p2[0]) / 2;
 420   mp[1] = (p1[1] + p2[1]) / 2;
 421 }
 422
 423 /* Distance between two points. */
 424 float
 425 point_distance_to_point (const float *p1, const float *p2)
 426 {
 427   float v[3];
 428
 429   vec_subtract (p1, p2, v);
 430   return vec_magnitude (v);
 431 }
 432
 433 /* Cross product of vectors v and w.
 434  * The cross product is a vector:
 435  *
 436  *   v x w = |v| |w| sin t n^
 437  *
 438  * where t is the angle between a and
 439  * b, and n^ is a normal vector perpendicular
 440  * to a and b such that a,b,n^ form a
 441  * right-handed set.
 442  */
 443 void
 444 vec_cross_product (const float *v, const float *w, float *r)
 445 {
 446   r[0] = v[1]*w[2] - v[2]*w[1];
 447   r[1] = v[2]*w[0] - v[0]*w[2];
 448   r[2] = v[0]*w[1] - v[1]*w[0];
 449 }
 450
 451 /* Distance between two points. */
 452 float
 453 point_distance (const float *p, const float *q)
 454 {
 455   double x = p[0] - q[0];
 456   double y = p[1] - q[1];
 457   double z = p[2] - q[2];
 458   return sqrt (x*x + y*y + z*z);
 459 }
 460
 461 /* Distance from a point to a plane. */
 462 float
 463 point_distance_to_plane (const float *plane, const float *point)
 464 {
 465   float a = plane[0];
 466   float b = plane[1];
 467   float c = plane[2];
 468   float d = plane[3];
 469   float x = point[0];
 470   float y = point[1];
 471   float z = point[2];
 472   float t = (a*x + b*y + c*z + d) / - (a*a + b*b + c*c);
 473   float t2 = t*t;
 474   float dist = sqrt (t2*a*a + t2*b*b + t2*c*c);
 475   /* Don't lose the sign of t. */
 476   if (t < 0) return dist; else return -dist;
 477 }
 478
 479 /* Return true if point_distance_to_plane > 0. */
 480 int
 481 point_is_inside_plane (const float *plane, const float *point)
 482 {
 483   float a = plane[0];
 484   float b = plane[1];
 485   float c = plane[2];
 486   float d = plane[3];
 487   float x = point[0];
 488   float y = point[1];
 489   float z = point[2];
 490   float t = (a*x + b*y + c*z + d) / - (a*a + b*b + c*c);
 491
 492   return t < 0;
 493
 494 #if 0
 495   float t2 = t*t;
 496   float dist = sqrt (t2*a*a + t2*b*b + t2*c*c);
 497   /* Don't lose the sign of t. */
 498   if (t < 0) return dist; else return -dist;
 499 #endif
 500 }
 501
 502 /* See: http://www.greuer.de/efaq.html */
 503 void
 504 point_footprint_on_line (const float *point,
 505                          const float *line_point, const float *line_vector,
 506                          float *footprint)
 507 {
 508   float t;
 509   float s[3];
 510
 511   vec_subtract (point, line_point, s);
 512   t = vec_dot_product (s, line_vector) /
 513       vec_dot_product (line_vector, line_vector);
 514
 515   vec_scale (line_vector, t, s);
 516   vec_add (line_point, s, footprint);
 517 }
 518
 519 float
 520 point_distance_to_line (const float *point,
 521                         const float *line_point, const float *line_vector)
 522 {
 523 #if 0
 524   float footprint[3];
 525
 526   point_footprint_on_line (point, line_point, line_vector, footprint);
 527   return point_distance_to_point (point, footprint);
 528 #else
 529   float u[3], p[3], prod[3], dist;
 530
 531   /* Normalize vector u along the line. */
 532   vec_normalize (line_vector, u);
 533
 534   /* The distance is given by | (p-a) x u | where p is the
 535    * point, a is a point on the line and x is the cross product.
 536    */
 537   vec_subtract (point, line_point, p);
 538   vec_cross_product (p, u, prod);
 539   dist = vec_magnitude (prod);
 540
 541   return dist;
 542 #endif
 543 }
 544
 545 float
 546 point_distance_to_line_segment (const float *point,
 547                                 const float *line_point0,
 548                                 const float *line_point1)
 549 {
 550   float t;
 551   float s[3], v[3];
 552
 553   vec_subtract (line_point1, line_point0, v);
 554   vec_subtract (point, line_point0, s);
 555   t = vec_dot_product (s, v) /
 556       vec_dot_product (v, v);
 557
 558   if (t >= 0 && t <= 1)
 559     {
 560       float footprint[3];
 561
 562       vec_scale (v, t, s);
 563       vec_add (line_point0, s, footprint);
 564       return point_distance_to_point (point, footprint);
 565     }
 566   else if (t < 0)
 567     return point_distance_to_point (point, line_point0);
 568   /* else t > 1 */
 569   return point_distance_to_point (point, line_point1);
 570 }
 571
 572 int
 573 point_lies_in_face (const float *points, int nr_points, const float *point)
 574 {
 575   float sum = point_face_angle_sum (points, nr_points, point);
 576   return fabs (sum - 2*M_PI) < 1e-5;
 577 }
 578
 579 /* See: http://astronomy.swin.edu.au/pbourke/geometry/insidepoly/ */
 580 float
 581 point_face_angle_sum (const float *points, int nr_points, const float *point)
 582 {
 583   float sum = 0;
 584   int i, next;
 585
 586   for (i = 0, next = 1; i < nr_points; ++i, ++next)
 587     {
 588       float p1[3], p2[3], m1, m2, costheta;
 589
 590       if (next == nr_points) next = 0;
 591
 592       p1[0] = points[i*3] - point[0];
 593       p1[1] = points[i*3+1] - point[1];
 594       p1[2] = points[i*3+2] - point[2];
 595       p2[0] = points[next*3] - point[0];
 596       p2[1] = points[next*3+1] - point[1];
 597       p2[2] = points[next*3+2] - point[2];
 598
 599       m1 = vec_magnitude (p1);
 600       m2 = vec_magnitude (p2);
 601
 602       if (m1 * m2 < 1e-5)
 603         return 2 * M_PI;
 604       else
 605         costheta = (p1[0]*p2[0] + p1[1]*p2[1] + p1[2]*p2[2]) / (m1*m2);
 606
 607       sum += acos (costheta);
 608     }
 609
 610   return sum;
 611 }
 612
 613 float
 614 point_distance_to_face (const float *points, int nr_points,
 615                         const float *plane, const float *point, int *edge)
 616 {
 617   float my_plane_coeffs[4];
 618   float a, b, c, d, tq, q[3], dist;
 619   int e, i, next;
 620
 621   /* Calculate plane coefficients, if necessary. */
 622   if (plane == 0)
 623     {
 624       plane = my_plane_coeffs;
 625       plane_coefficients (&points[0], &points[3], &points[6], (float *) plane);
 626     }
 627
 628   a = plane[0];
 629   b = plane[1];
 630   c = plane[2];
 631   d = plane[3];
 632
 633   /* q is the coordinate of the point of intersection of a
 634    * normal line from the point to the plane. It may or may
 635    * not be on the bounded face (we'll work that out in the
 636    * moment). tq is the parameter of point q.
 637    */
 638   tq = - (a*point[0] + b*point[1] + c*point[2] + d) / (a*a + b*b + c*c);
 639   q[0] = point[0] + tq*a;
 640   q[1] = point[1] + tq*b;
 641   q[2] = point[2] + tq*c;
 642
 643   /* Is q on the bounded face? */
 644   if (point_lies_in_face (points, nr_points, q))
 645     {
 646       /* Compute the distance from the point to the plane. */
 647       float t2 = tq*tq;
 648
 649       dist = sqrt (t2*a*a + t2*b*b + t2*c*c);
 650
 651       if (edge) *edge = -1;
 652
 653       return tq < 0 ? dist : -dist;
 654     }
 655
 656   /* Find the closest edge. */
 657   e = -1;
 658   dist = 0;
 659
 660   for (i = 0, next = 1; i < nr_points; ++i, ++next)
 661     {
 662       float d;
 663
 664       if (next == nr_points) next = 0;
 665
 666       d = point_distance_to_line_segment (point,
 667                                           &points[i*3], &points[next*3]);
 668
 669       if (e == -1 || d < dist)
 670         {
 671           dist = d;
 672           e = i;
 673         }
 674     }
 675
 676   if (edge) *edge = e;
 677
 678   return tq < 0 ? dist : -dist;
 679 }
 680
 681 /* Compute the four coefficients of a plane which
 682  * uniquely specify that plane, given three points
 683  * (not colinear) on the plane. Most of the variables
 684  * in this function are redundant (optimized out?),
 685  * but they get it into the same form as in
 686  * Lansdown, p. 178.
 687  */
 688 void
 689 plane_coefficients (const float *p, const float *q, const float *r,
 690                     float *co)
 691 {
 692   float x2 = p[0];
 693   float y2 = p[1];
 694   float z2 = p[2];
 695   float x1 = q[0];
 696   float y1 = q[1];
 697   float z1 = q[2];
 698   float x3 = r[0];
 699   float y3 = r[1];
 700   float z3 = r[2];
 701   float xa = x1 + x2;
 702   float xb = x2 + x3;
 703   float xc = x3 + x1;
 704   float ya = y1 + y2;
 705   float yb = y2 + y3;
 706   float yc = y3 + y1;
 707   float za = z1 + z2;
 708   float zb = z2 + z3;
 709   float zc = z3 + z1;
 710
 711   co[0] = (y1-y2) * za + (y2-y3) * zb + (y3-y1) * zc;
 712   co[1] = (z1-z2) * xa + (z2-z3) * xb + (z3-z1) * xc;
 713   co[2] = (x1-x2) * ya + (x2-x3) * yb + (x3-x1) * yc;
 714   co[3] = - (co[0]*x1 + co[1]*y1 + co[2]*z1);
 715 }
 716
 717 void
 718 plane_translate_along_normal (const float *plane, float distance,
 719                               float *new_plane)
 720 {
 721   float w = vec_magnitude (plane);
 722
 723   new_plane[0] = plane[0];
 724   new_plane[1] = plane[1];
 725   new_plane[2] = plane[2];
 726   new_plane[3] = plane[3] - w * distance;
 727 }
 728
 729 void
 730 face_translate_along_normal (const float *points, int nr_points,
 731                              const float *plane, float distance,
 732                              float *new_points, float *new_plane)
 733 {
 734   float w = vec_magnitude (plane), nv[3];
 735   int i;
 736
 737   new_plane[0] = plane[0];
 738   new_plane[1] = plane[1];
 739   new_plane[2] = plane[2];
 740   new_plane[3] = plane[3] - w * distance;
 741
 742   vec_scale (plane, distance / w, nv);
 743   for (i = 0; i < nr_points; ++i)
 744     {
 745       new_points[i*3] = points[i*3] + nv[0];
 746       new_points[i*3+1] = points[i*3+1] + nv[1];
 747       new_points[i*3+2] = points[i*3+2] + nv[2];
 748     }
 749 }
 750
 751 /* All these quaternion functions are modified from the matrix FAQ again. */
 752 void
 753 quaternion_normalize (const float *a, float *r)
 754 {
 755   float w = vec_magnitude (a);
 756   r[0] = a[0] / w;
 757   r[1] = a[1] / w;
 758   r[2] = a[2] / w;
 759   r[3] = a[3] / w;
 760 }
 761
 762 void
 763 quaternion_conjugate (const float *a, float *r)
 764 {
 765   r[0] = -a[0];
 766   r[1] = -a[1];
 767   r[2] = -a[2];
 768   r[3] = a[3];
 769 }
 770
 771 float
 772 quaternion_magnitude (const float *a)
 773 {
 774   return sqrt (a[3]*a[3] + a[0]*a[0] + a[1]*a[1] + a[2]*a[2]);
 775 }
 776
 777 void
 778 quaternion_multiply (const float *a, const float *b, float *r)
 779 {
 780   float va[3], vb[3], vc[3];
 781
 782   r[3] = vec_dot_product (a, b);
 783
 784   vec_cross_product (a, b, va);
 785   vec_scale (a, b[3], vb);
 786   vec_scale (b, a[3], vc);
 787   vec_add (va, vb, va);
 788   vec_add (va, vc, r);
 789
 790   quaternion_normalize (r, r);
 791 }
 792
 793 void
 794 quaternion_to_rotation_matrix (const float *q, float *mat)
 795 {
 796   float X = q[0];
 797   float Y = q[1];
 798   float Z = q[2];
 799   float W = q[3];
 800
 801   float xx      = X * X;
 802   float xy      = X * Y;
 803   float xz      = X * Z;
 804   float xw      = X * W;
 805
 806   float yy      = Y * Y;
 807   float yz      = Y * Z;
 808   float yw      = Y * W;
 809
 810   float zz      = Z * Z;
 811   float zw      = Z * W;
 812
 813   mat[0]  = 1 - 2 * ( yy + zz );
 814   mat[4]  =     2 * ( xy - zw );
 815   mat[8]  =     2 * ( xz + yw );
 816
 817   mat[1]  =     2 * ( xy + zw );
 818   mat[5]  = 1 - 2 * ( xx + zz );
 819   mat[9]  =     2 * ( yz - xw );
 820
 821   mat[2]  =     2 * ( xz - yw );
 822   mat[6]  =     2 * ( yz + xw );
 823   mat[10] = 1 - 2 * ( xx + yy );
 824
 825   mat[3]  = mat[7] = mat[11] = mat[12] = mat[13] = mat[14] = 0;
 826   mat[15] = 1;
 827 }
 828
 829 void
 830 make_quaternion_from_axis_angle (const float *axis, float angle,
 831                                  float *q)
 832 {
 833   double sin_a = sin (angle / 2);
 834   double cos_a = cos (angle / 2);
 835
 836   q[0] = axis[0] * sin_a;
 837   q[1] = axis[1] * sin_a;
 838   q[2] = axis[2] * sin_a;
 839   q[3] = cos_a;
 840
 841   quaternion_normalize (q, q);
 842 }
 843
 844 int
 845 collision_moving_sphere_and_face (const float *p0, const float *p1,
 846                                   float radius,
 847                                   const float *points, int nr_points,
 848                                   const float *plane,
 849                                   float *collision_point)
 850 {
 851   float my_plane_coeffs[4], raised_plane[4], t, v[3], quot;
 852   float raised_points[3 * nr_points];
 853
 854   /* Get the plane coefficients. */
 855   if (plane == 0)
 856     {
 857       plane = my_plane_coeffs;
 858       plane_coefficients (&points[0], &points[3], &points[6], (float *) plane);
 859     }
 860
 861   /* Raise the plane up by the distance of one radius. Then we can
 862    * just test for the intersection of the ray and the face.This is
 863    * something of an approximation, but hopefully will give us good
 864    * results in practice. If not, then we may need to rework this
 865    * code. (XXX)
 866    */
 867   face_translate_along_normal (points, nr_points, plane, radius,
 868                                raised_points, raised_plane);
 869
 870   /* Get the intersection point of the ray and the plane, as a
 871    * parameter, t.
 872    */
 873   vec_subtract (p1, p0, v);
 874   quot = raised_plane[0]*v[0] + raised_plane[1]*v[1] + raised_plane[2]*v[2];
 875   if (fabs (quot)
 876 #if 0
 877       < 1e-5
 878 #else
 879       == 0
 880 #endif
 881       )
 882     {
 883       /* The path of the sphere is nearly parallel to the plane. Don't
 884        * count this as a collision at all.
 885        */
 886       return 0;
 887     }
 888
 889   t = - (raised_plane[0]*p0[0] + raised_plane[1]*p0[1] + raised_plane[2]*p0[2]
 890          + raised_plane[3]) / quot;
 891   if (t < 0 || t > 1)
 892     {
 893       /* There is no collision. */
 894       return 0;
 895     }
 896
 897   /* Calculate the actual point of collision. NOTE: This is the centre
 898    * of the sphere, NOT the point where the sphere and plane touch.
 899    */
 900   vec_scale (v, t, v);
 901   vec_add (p0, v, collision_point);
 902
 903   /* Is the collision point actually within the bounded convex polygon
 904    * which defines the face? If not, then no collision actually
 905    * occurred. Note that we have to translate (ie. raise) the points
 906    * list too.
 907    */
 908   return point_lies_in_face (raised_points, nr_points, collision_point);
 909 }