/*** $Id: math3d.c,v 1.4 2004/06/16 09:30:21 weiym Exp $**** math3d.c: the three-Dimension math routines.**** Copyright (C) 2003 Feynman Software.** ** Current maintainer: Wei Yongming.*//*** This program is free software; you can redistribute it and/or modify** it under the terms of the GNU General Public License as published by** the Free Software Foundation; either version 2 of the License, or** (at your option) any later version.**** This program is distributed in the hope that it will be useful,** but WITHOUT ANY WARRANTY; without even the implied warranty of** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the** GNU General Public License for more details.**** You should have received a copy of the GNU General Public License** along with this program; if not, write to the Free Software** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA*//*         ______   ___    ___  *        /\  _  \ /\_ \  /\_ \  *        \ \ \L\ \\//\ \ \//\ \      __     __   _ __   ___  *         \ \  __ \ \ \ \  \ \ \   /'__`\ /'_ `\/\`'__\/ __`\ *          \ \ \/\ \ \_\ \_ \_\ \_/\  __//\ \L\ \ \ \//\ \L\ \ *           \ \_\ \_\/\____\/\____\ \____\ \____ \ \_\\ \____/ *            \/_/\/_/\/____/\/____/\/____/\/___L\ \/_/ \/___/ *                                           /\____/ *                                           \_/__/ * *      Vector and matrix manipulation routines. * *      By Shawn Hargreaves. * *      See readme.txt for copyright information. */#include "common.h"#ifdef _MATH_3D#include "fixedmath.h"#ifndef M_PI   #define M_PI   3.14159265358979323846#endif#define floatcos(x)     cos((x) * M_PI / 128.0)#define floatsin(x)     sin((x) * M_PI / 128.0)#define floattan(x)     tan((x) * M_PI / 128.0)MATRIX identity_matrix = {   {      /* 3x3 identity */      { 1<<16, 0,     0     },      { 0,     1<<16, 0     },      { 0,     0,     1<<16 },   },   /* zero translation */   { 0, 0, 0 }};MATRIX_f identity_matrix_f = {   {      /* 3x3 identity */      { 1.0, 0.0, 0.0 },      { 0.0, 1.0, 0.0 },      { 0.0, 0.0, 1.0 },   },   /* zero translation */   { 0.0, 0.0, 0.0 }};/* get_translation_matrix: *  Constructs a 3d translation matrix. When applied to the vector  *  (vx, vy, vx), this will produce (vx+x, vy+y, vz+z). */void get_translation_matrix(MATRIX *m, fixed x, fixed y, fixed z){   *m = identity_matrix;   m->t[0] = x;   m->t[1] = y;   m->t[2] = z;}/* get_translation_matrix_f: *  Floating point version of get_translation_matrix(). */void get_translation_matrix_f(MATRIX_f *m, float x, float y, float z){   *m = identity_matrix_f;   m->t[0] = x;   m->t[1] = y;   m->t[2] = z;}/* get_scaling_matrix: *  Constructs a 3d scaling matrix. When applied to the vector  *  (vx, vy, vx), this will produce (vx*x, vy*y, vz*z). */void get_scaling_matrix(MATRIX *m, fixed x, fixed y, fixed z){   *m = identity_matrix;   m->v[0][0] = x;   m->v[1][1] = y;   m->v[2][2] = z;}/* get_scaling_matrix_f: *  Floating point version of get_scaling_matrix(). */void get_scaling_matrix_f(MATRIX_f *m, float x, float y, float z){   *m = identity_matrix_f;   m->v[0][0] = x;   m->v[1][1] = y;   m->v[2][2] = z;}/* get_x_rotate_matrix: *  Constructs a 3d transformation matrix, which will rotate points around  *  the x axis by the specified amount (given in the Allegro fixed point,  *  256 degrees to a circle format). */void get_x_rotate_matrix(MATRIX *m, fixed r){   fixed c = fcos(r);   fixed s = fsin(r);   *m = identity_matrix;   m->v[1][1] = c;   m->v[1][2] = -s;   m->v[2][1] = s;   m->v[2][2] = c;}/* get_x_rotate_matrix_f: *  Floating point version of get_x_rotate_matrix(). */void get_x_rotate_matrix_f(MATRIX_f *m, float r){   float c = floatcos(r);   float s = floatsin(r);   *m = identity_matrix_f;   m->v[1][1] = c;   m->v[1][2] = -s;   m->v[2][1] = s;   m->v[2][2] = c;}/* get_y_rotate_matrix: *  Constructs a 3d transformation matrix, which will rotate points around  *  the y axis by the specified amount (given in the Allegro fixed point,  *  256 degrees to a circle format). */void get_y_rotate_matrix(MATRIX *m, fixed r){   fixed c = fcos(r);   fixed s = fsin(r);   *m = identity_matrix;   m->v[0][0] = c;   m->v[0][2] = s;   m->v[2][0] = -s;   m->v[2][2] = c;}/* get_y_rotate_matrix_f: *  Floating point version of get_y_rotate_matrix(). */void get_y_rotate_matrix_f(MATRIX_f *m, float r){   float c = floatcos(r);   float s = floatsin(r);   *m = identity_matrix_f;   m->v[0][0] = c;   m->v[0][2] = s;   m->v[2][0] = -s;   m->v[2][2] = c;}/* get_z_rotate_matrix: *  Constructs a 3d transformation matrix, which will rotate points around  *  the z axis by the specified amount (given in the Allegro fixed point,  *  256 degrees to a circle format). */void get_z_rotate_matrix(MATRIX *m, fixed r){   fixed c = fcos(r);   fixed s = fsin(r);   *m = identity_matrix;   m->v[0][0] = c;   m->v[0][1] = -s;   m->v[1][0] = s;   m->v[1][1] = c;}/* get_z_rotate_matrix_f: *  Floating point version of get_z_rotate_matrix(). */void get_z_rotate_matrix_f(MATRIX_f *m, float r){   float c = floatcos(r);   float s = floatsin(r);   *m = identity_matrix_f;   m->v[0][0] = c;   m->v[0][1] = -s;   m->v[1][0] = s;   m->v[1][1] = c;}/* magical formulae for constructing rotation matrices */#define MAKE_ROTATION(x, y, z)                  \   fixed sin_x = fsin(x);                       \   fixed cos_x = fcos(x);                       \						\   fixed sin_y = fsin(y);                       \   fixed cos_y = fcos(y);                       \						\   fixed sin_z = fsin(z);                       \   fixed cos_z = fcos(z);                       \						\   fixed sinx_siny = fmul(sin_x, sin_y);        \   fixed cosx_siny = fmul(cos_x, sin_y);#define MAKE_ROTATION_f(x, y, z)                \   float sin_x = floatsin(x);                   \   float cos_x = floatcos(x);                   \						\   float sin_y = floatsin(y);                   \   float cos_y = floatcos(y);                   \						\   float sin_z = floatsin(z);                   \   float cos_z = floatcos(z);                   \						\   float sinx_siny = sin_x * sin_y;             \   float cosx_siny = cos_x * sin_y;#define R00 (fmul(cos_y, cos_z))#define R10 (fmul(sinx_siny, cos_z) - fmul(cos_x, sin_z))#define R20 (fmul(cosx_siny, cos_z) + fmul(sin_x, sin_z))#define R01 (fmul(cos_y, sin_z))#define R11 (fmul(sinx_siny, sin_z) + fmul(cos_x, cos_z))#define R21 (fmul(cosx_siny, sin_z) - fmul(sin_x, cos_z))#define R02 (-sin_y)#define R12 (fmul(sin_x, cos_y))#define R22 (fmul(cos_x, cos_y))#define R00_f (cos_y * cos_z)#define R10_f ((sinx_siny * cos_z) - (cos_x * sin_z))#define R20_f ((cosx_siny * cos_z) + (sin_x * sin_z))#define R01_f (cos_y * sin_z)#define R11_f ((sinx_siny * sin_z) + (cos_x * cos_z))#define R21_f ((cosx_siny * sin_z) - (sin_x * cos_z))#define R02_f (-sin_y)#define R12_f (sin_x * cos_y)#define R22_f (cos_x * cos_y)/* get_rotation_matrix: *  Constructs a 3d transformation matrix, which will rotate points around *  all three axis by the specified amounts (given in the Allegro fixed  *  point, 256 degrees to a circle format). */void get_rotation_matrix(MATRIX *m, fixed x, fixed y, fixed z){   MAKE_ROTATION(x, y, z);   m->v[0][0] = R00;   m->v[0][1] = R01;   m->v[0][2] = R02;   m->v[1][0] = R10;   m->v[1][1] = R11;   m->v[1][2] = R12;   m->v[2][0] = R20;   m->v[2][1] = R21;   m->v[2][2] = R22;   m->t[0] = m->t[1] = m->t[2] = 0;}/* get_rotation_matrix_f: *  Floating point version of get_rotation_matrix(). */void get_rotation_matrix_f(MATRIX_f *m, float x, float y, float z){   MAKE_ROTATION_f(x, y, z);   m->v[0][0] = R00_f;   m->v[0][1] = R01_f;   m->v[0][2] = R02_f;   m->v[1][0] = R10_f;   m->v[1][1] = R11_f;   m->v[1][2] = R12_f;   m->v[2][0] = R20_f;   m->v[2][1] = R21_f;   m->v[2][2] = R22_f;   m->t[0] = m->t[1] = m->t[2] = 0;}/* get_align_matrix: *  Aligns a matrix along an arbitrary coordinate system. */void get_align_matrix(MATRIX *m, fixed xfront, fixed yfront, fixed zfront, fixed xup, fixed yup, fixed zup){   fixed xright, yright, zright;   normalize_vector(&xfront, &yfront, &zfront);   normalize_vector(&xup, &yup, &zup);   cross_product(xfront, yfront, zfront, xup, yup, zup, &xright, &yright, &zright);   cross_product(xright, yright, zright, xfront, yfront, zfront, &xup, &yup, &zup);   m->v[0][0] = xright;    m->v[0][1] = xup;    m->v[0][2] = xfront;    m->v[1][0] = yright;   m->v[1][1] = yup;   m->v[1][2] = yfront;   m->v[2][0] = zright;   m->v[2][1] = zup;   m->v[2][2] = zfront;   m->t[0] = m->t[1] = m->t[2] = 0;}/* get_align_matrix_f: *  Floating point version of get_align_matrix(). */void get_align_matrix_f(MATRIX_f *m, float xfront, float yfront, float zfront, float xup, float yup, float zup){   float xright, yright, zright;   normalize_vector_f(&xfront, &yfront, &zfront);   normalize_vector_f(&xup, &yup, &zup);   cross_product_f(xfront, yfront, zfront, xup, yup, zup, &xright, &yright, &zright);   cross_product_f(xright, yright, zright, xfront, yfront, zfront, &xup, &yup, &zup);   m->v[0][0] = xright;    m->v[0][1] = xup;    m->v[0][2] = xfront;    m->v[1][0] = yright;   m->v[1][1] = yup;   m->v[1][2] = yfront;   m->v[2][0] = zright;   m->v[2][1] = zup;   m->v[2][2] = zfront;   m->t[0] = m->t[1] = m->t[2] = 0;}/* get_vector_rotation_matrix: *  Constructs a 3d transformation matrix, which will rotate points around *  the specified x,y,z vector by the specified angle (given in the Allegro  *  fixed point, 256 degrees to a circle format), in a clockwise direction. */void get_vector_rotation_matrix(MATRIX *m, fixed x, fixed y, fixed z, fixed a){