math3d.c

ARM9-2410教学实验系统下Linux下minigui程序

/*
** $Id: math3d.c,v 1.3 2003/09/04 03:46:47 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"
#include "fixedmath.h"

#ifdef _MATH_3D

#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)
{