www.pudn.com > Ìå»æÖÆÂë.rar > vecmath.cpp, change:2001-11-25,size:6113b
/* =========================================================
* File ........... vecmath.cpp
* Purpose ........ vector mathematics
* ========================================================= */
#include <stdlib.h>
#include <math.h>
#include "vecmath.h"
matrix44 IDENTITY_MATRIX = {0x8421, 0x8421, {{1.0f,0.0f,0.0f,0.0f},{0.0f,1.0f,0.0f,0.0f},{0.0f,0.0f,1.0f,0.0f},{0.0f,0.0f,0.0f,1.0f}}};
/* ========== vector functions */
coord3 *
vec_set(coord3 *v, float x, float y, float z)
{
v->x = x;
v->y = y;
v->z = z;
return v;
}
coord3 *
vec_normalize(coord3 *w, coord3 *v)
{
float l = (float)sqrt((v->x * v->x) + (v->y * v->y) + (v->z * v->z));
if (l == 0) return w;
w->x = v->x / l;
w->y = v->y / l;
w->z = v->z / l;
return w;
}
coord3 *
vec_crossproduct(coord3 *n,coord3 *v1, coord3 *v2)
{
n->x = (v1->y * v2->z) - (v1->z * v2->y);
n->y = (v1->z * v2->x) - (v1->x * v2->z);
n->z = (v1->x * v2->y) - (v1->y * v2->x);
return n;
}
coord3 *
vec_add(coord3 *v,coord3 *v1, coord3 *v2)
{
v->x = v1->x + v2->x;
v->y = v1->y + v2->y;
v->z = v1->z + v2->z;
return v;
}
coord3 *
vec_subtract(coord3 *v,coord3 *v1, coord3 *v2)
{
v->x = v1->x - v2->x;
v->y = v1->y - v2->y;
v->z = v1->z - v2->z;
return v;
}
coord3 *
vec_scale(coord3 *w, coord3 *v, float s)
{
w->x = v->x * s;
w->y = v->y * s;
w->z = v->z * s;
return w;
}
coord3 *
vec_rotate(coord3 *w, coord3 *v, coord3 *r, float angle)
{
float si = (float)sin(angle/2);
float s = (float)cos(angle/2);
float a = r->x * si;
float b = r->y * si;
float c = r->z * si;
w->x = v->x*(1-2*(b*b+c*c)) + v->y*( 2*(a*b-s*c)) + v->z*( 2*(a*c+s*b));
w->y = v->x*( 2*(a*b+s*c)) + v->y*(1-2*(a*a+c*c)) + v->z*( 2*(b*c-s*a));
w->z = v->x*( 2*(a*c-s*b)) + v->y*( 2*(b*c+s*a)) + v->z*(1-2*(a*a+b*b));
return w;
}
coord3 *
vec_normal(coord3 *n, coord3 *p0, coord3 *p1, coord3 *p2)
{
coord3 v1, v2;
vec_subtract(&v1, p1, p0);
vec_subtract(&v2, p2, p0);
vec_crossproduct(n, &v1, &v2);
vec_normalize(n, n);
return n;
}
float vec_length(coord3 *v)
{
return (float)sqrt((v->x * v->x) + (v->y * v->y) + (v->z * v->z));
}
float vec_angle(coord3 *v1, coord3 *v2)
{
return (float)((v1->x * v2->x) + (v1->y * v2->y) + (v1->z * v2->z));
}
/* ========== vector/matrix functions */
coord3 *
vec_matrixmul(coord3 *w, matrix44 *m, coord3 *v)
{
matrix_a *ma = &(m->matrix);
float vx = v->x;
float vy = v->y;
float vz = v->z;
float hi = ((*ma)[0][3]*vx + (*ma)[1][3]*vy + (*ma)[2][3]*vz + (*ma)[3][3]);
if (hi == 1.0f)
{
w->x = ((*ma)[0][0]*vx + (*ma)[1][0]*vy + (*ma)[2][0]*vz + (*ma)[3][0]);
w->y = ((*ma)[0][1]*vx + (*ma)[1][1]*vy + (*ma)[2][1]*vz + (*ma)[3][1]);
w->z = ((*ma)[0][2]*vx + (*ma)[1][2]*vy + (*ma)[2][2]*vz + (*ma)[3][2]);
}
else
{
float h = 1.0f / hi;
w->x = ((*ma)[0][0]*vx + (*ma)[1][0]*vy + (*ma)[2][0]*vz + (*ma)[3][0]) * h;
w->y = ((*ma)[0][1]*vx + (*ma)[1][1]*vy + (*ma)[2][1]*vz + (*ma)[3][1]) * h;
w->z = ((*ma)[0][2]*vx + (*ma)[1][2]*vy + (*ma)[2][2]*vz + (*ma)[3][2]) * h;
}
return w;
}
/* ========== matrix functions */
void matrix_identity(matrix44 *m)
{
*m = IDENTITY_MATRIX;
}
void matrix_translate(matrix44 *m, float x, float y, float z)
{
*m = IDENTITY_MATRIX;
m->maskh = 0x842f;
m->maskv = 0x9531;
matrix_a *ma = &(m->matrix);
(*ma)[3][0] = x;
(*ma)[3][1] = y;
(*ma)[3][2] = z;
}
void matrix_scale(matrix44 *m, float x, float y, float z)
{
*m = IDENTITY_MATRIX;
matrix_a *ma = &(m->matrix);
(*ma)[0][0] = x;
(*ma)[1][1] = y;
(*ma)[2][2] = z;
}
void matrix_rotate(matrix44 *m, float x, float y, float z, float angle)
{
*m = IDENTITY_MATRIX;
float si = (float)sin(angle/2)/(float)sqrt(x*x + y*y + z*z);
float s = (float)cos(angle/2);
float a = x * si;
float b = y * si;
float c = z * si;
m->maskh = 0xeee1;
m->maskv = 0xeee1;
matrix_a *ma = &(m->matrix);
(*ma)[0][0] = (1-2*(b*b+c*c));
(*ma)[1][0] = ( 2*(a*b-s*c));
(*ma)[2][0] = ( 2*(a*c+s*b));
(*ma)[0][1] = ( 2*(a*b+s*c));
(*ma)[1][1] = (1-2*(a*a+c*c));
(*ma)[2][1] = ( 2*(b*c-s*a));
(*ma)[0][2] = ( 2*(a*c-s*b));
(*ma)[1][2] = ( 2*(b*c+s*a));
(*ma)[2][2] = (1-2*(a*a+b*b));
/* (*ma)[0][0] = (1-2*(b*b+c*c));
(*ma)[0][1] = ( 2*(a*b-s*c));
(*ma)[0][2] = ( 2*(a*c+s*b));
(*ma)[1][0] = ( 2*(a*b+s*c));
(*ma)[1][1] = (1-2*(a*a+c*c));
(*ma)[1][2] = ( 2*(b*c-s*a));
(*ma)[2][0] = ( 2*(a*c-s*b));
(*ma)[2][1] = ( 2*(b*c+s*a));
(*ma)[2][2] = (1-2*(a*a+b*b)); */
}
void matrix_matrixmul(matrix44 *m, matrix44 *m1, matrix44 *m2)
{
matrix_a *ma = &(m->matrix);
matrix_a *m1a = &(m1->matrix);
matrix_a *m2a = &(m2->matrix);
float sum;
unsigned short mask1 = m1->maskv;
unsigned short mask2 = m2->maskh;
unsigned short mk1;
unsigned short mk2;
unsigned short bv, bh;
register unsigned short mkh = 0;
register unsigned short mkv = 0;
register unsigned short mk;
mk2 = mask2;
bh = 0x8000;
for (int i=0; i<4; i++)
{
bv = 0x8000 >> i;
mk1 = mask1;
for (int j=0; j<4; j++)
{
sum = 0.0f;
mk = mk1 & mk2 & 0xf000;
if (mk)
{
if (mk & 0x8000) sum += (*m1a)[0][j] * (*m2a)[i][0];
if (mk & 0x4000) sum += (*m1a)[1][j] * (*m2a)[i][1];
if (mk & 0x2000) sum += (*m1a)[2][j] * (*m2a)[i][2];
if (mk & 0x1000) sum += (*m1a)[3][j] * (*m2a)[i][3];
if (sum != 0.0)
{
mkh |= bh;
mkv |= bv;
}
}
(*ma)[i][j] = sum;
mk1 <= 4;
bh >>= 1;
bv >>= 4;
}
mk2 <= 4;
}
m->maskh = mkh;
m->maskv = mkv;
}
void matrix_matrixmul_top(matrix44 *m, matrix44 *n)
{
matrix44 mm;
matrix_matrixmul(&mm, m, n);
*m = mm;
}
void matrix_matrixmul_bottom(matrix44 *m, matrix44 *n)
{
matrix44 mm;
matrix_matrixmul(&mm, n, m);
*m = mm;
}
void matrix_clean(matrix44 *m, matrix44 *n)
{
coord3 v0 = {0.0f, 0.0f, 0.0f};
coord3 w;
matrix44 t;
vec_matrixmul(&w, n, &v0);
matrix_translate(&t, -w.x, -w.y, -w.z);
matrix_matrixmul(m, &t, n);
}