math3d.cpp

Torus3D.rar,BREW平台的3D游戏引擎的一个实例.喜欢的朋友可以下载

// ==========================================================================================================
//
// BREW v2.0+ OPENGLES MICROENGINE
//
// ----------------------------------------
//
// Written by Vander Nunes
//
// ==========================================================================================================

#include <string.h>		// memset and memcpy
#include "math3d.h"


// ----------------------------------------------------------------------------
//
// FIXED-POINT MATRIX OPERATIONS
//
// ----------------------------------------------------------------------------

//
//
//
matrix_t ZeroMatrix()
{
	matrix_t m =
	{
		 0,0,0,0,
		 0,0,0,0,
		 0,0,0,0,
		 0,0,0,0
	};
	return m;
}


//
//
//
matrix_t IdentityMatrix()
{
	matrix_t m =
	{
		 ITOX(1),0,0,0,
		 0,ITOX(1),0,0,
		 0,0,ITOX(1),0,
		 0,0,0,ITOX(1)
	};
	return m;
}


//
//
//
matrix_t RXMatrix(const int degs)
{
	int d = XTOI(degs);
	while (d < 0) d += 360;
	while (d >= 360) d -= 360;

	int cosine = FPcos[d];
	int sine = FPsin[d];

	matrix_t m = IdentityMatrix();

	m.m[1 * 4 + 1] = cosine;
	m.m[2 * 4 + 2] = cosine;
	m.m[2 * 4 + 1] = -sine;
	m.m[1 * 4 + 2] = sine;

	return m;
}


//
//
//
matrix_t RYMatrix(const int degs)
{
	int d = XTOI(degs);
	while (d < 0) d += 360;
	while (d >= 360) d -= 360;

	int cosine = FPcos[d];
	int sine = FPsin[d];

	matrix_t m = IdentityMatrix();

	m.m[0 * 4 + 0] = cosine;
	m.m[2 * 4 + 2] = cosine;
	m.m[2 * 4 + 0] = -sine;
	m.m[0 * 4 + 2] = sine;

	return m;
}


//
//
//
matrix_t RZMatrix(const int degs)
{
	int d = XTOI(degs);
	while (d < 0) d += 360;
	while (d >= 360) d -= 360;

	int cosine = FPcos[d];
	int sine = FPsin[d];

	matrix_t m = IdentityMatrix();

	m.m[0 * 4 + 0] = cosine;
	m.m[1 * 4 + 1] = cosine;
	m.m[1 * 4 + 0] = -sine;
	m.m[0 * 4 + 1] = sine;

	return m;
}


//
//
//
matrix_t MatrixMultiply(matrix_t m1, matrix_t m2)
{
	matrix_t mout = ZeroMatrix();

	#if 1

		int t[16];
		int *A = m1.m;
		int *B = m2.m;
		int *C = mout.m;

		t[0]  = FPXFP(A[0], B[0])  + FPXFP(A[1], B[4])  + FPXFP(A[2], B[8])   + FPXFP(A[3], B[12]);
		t[1]  = FPXFP(A[0], B[1])  + FPXFP(A[1], B[5])  + FPXFP(A[2], B[9])   + FPXFP(A[3], B[13]);
		t[2]  = FPXFP(A[0], B[2])  + FPXFP(A[1], B[6])  + FPXFP(A[2], B[10])  + FPXFP(A[3], B[14]);
		t[3]  = FPXFP(A[0], B[3])  + FPXFP(A[1], B[7])  + FPXFP(A[2], B[11])  + FPXFP(A[3], B[15]);
		t[4]  = FPXFP(A[4], B[0])  + FPXFP(A[5], B[4])  + FPXFP(A[6], B[8])   + FPXFP(A[7], B[12]);
		t[5]  = FPXFP(A[4], B[1])  + FPXFP(A[5], B[5])  + FPXFP(A[6], B[9])   + FPXFP(A[7], B[13]);
		t[6]  = FPXFP(A[4], B[2])  + FPXFP(A[5], B[6])  + FPXFP(A[6], B[10])  + FPXFP(A[7], B[14]);
		t[7]  = FPXFP(A[4], B[3])  + FPXFP(A[5], B[7])  + FPXFP(A[6], B[11])  + FPXFP(A[7], B[15]);
		t[8]  = FPXFP(A[8], B[0])  + FPXFP(A[9], B[4])  + FPXFP(A[10], B[8])  + FPXFP(A[11], B[12]);
		t[9]  = FPXFP(A[8], B[1])  + FPXFP(A[9], B[5])  + FPXFP(A[10], B[9])  + FPXFP(A[11], B[13]);
		t[10] = FPXFP(A[8], B[2])  + FPXFP(A[9], B[6])  + FPXFP(A[10], B[10]) + FPXFP(A[11], B[14]);
		t[11] = FPXFP(A[8], B[3])  + FPXFP(A[9], B[7])  + FPXFP(A[10], B[11]) + FPXFP(A[11], B[15]);
		t[12] = FPXFP(A[12], B[0]) + FPXFP(A[13], B[4]) + FPXFP(A[14], B[8])  + FPXFP(A[15], B[12]);
		t[13] = FPXFP(A[12], B[1]) + FPXFP(A[13], B[5]) + FPXFP(A[14], B[9])  + FPXFP(A[15], B[13]);
		t[14] = FPXFP(A[12], B[2]) + FPXFP(A[13], B[6]) + FPXFP(A[14], B[10]) + FPXFP(A[15], B[14]);
		t[15] = FPXFP(A[12], B[3]) + FPXFP(A[13], B[7]) + FPXFP(A[14], B[11]) + FPXFP(A[15], B[15]);

		memcpy(C, t, 16*sizeof(int));

	#else

		for (int i=0; i<4; i++)
			for (int j=0; j<4; j++)
				for (int k=0; k<4; k++)
					mout.m[i+j*4] += (FPXFP(m1.m[k+j*4], m2.m[i+k*4]));

	#endif

	return mout;
}


//
//
//
void MatrixMultiplyVector(matrix_t m, vec3_t v, vec3_t vout)
{
	int fx =  (FPXFP(v[0], m.m[0*4+0]) + FPXFP(v[1], m.m[1*4+0]) + FPXFP(v[2], m.m[2*4+0]) + m.m[3*4+0]);
	int fy =  (FPXFP(v[0], m.m[0*4+1]) + FPXFP(v[1], m.m[1*4+1]) + FPXFP(v[2], m.m[2*4+1]) + m.m[3*4+1]);
	int fz =  (FPXFP(v[0], m.m[0*4+2]) + FPXFP(v[1], m.m[1*4+2]) + FPXFP(v[2], m.m[2*4+2]) + m.m[3*4+2]);

	#if 0
		// w
		int fw =  (FPXFP(v[0], m.m[0*4+3]) + FPXFP(v[1], m.m[1*4+3]) + FPXFP(v[2], m.m[2*4+3]) + m.m[3*4+3]);
		// argh!!! 3 divisions by w!!!!
		vout[0] = fx/fw;
		vout[1] = fy/fw;
		vout[2] = fz/fw;
	#else
		// skip the divisions, ignoring w
		vout[0] = fx;
		vout[1] = fy;
		vout[2] = fz;
	#endif
}


//
// calculate a projection matrix given near and far planes plus fov in radians
//
matrix_t MatrixProjection(float fNearPlane, float fFarPlane, float fFOV, WORD wWidth, WORD wHeight)
{
	float fScale = (float)wWidth / wHeight;

	float c = (float)cos(fFOV*0.5f) * fScale;
	float s = (float)sin(fFOV*0.5f);
	float Q = s/(1.0f - fNearPlane/fFarPlane);

	matrix_t mout = ZeroMatrix();

	mout.m[0*4 + 0] = FTOX(c);
	mout.m[1*4 + 1] = FTOX(s);
	mout.m[2*4 + 2] = FTOX(Q);
	mout.m[2*4 + 3] = FTOX(-Q*fNearPlane);
	mout.m[3*4 + 2] = FTOX(1);

	return mout;
}


//
// return the inverse of a matrix (sets translation == 0,0,0)
//
matrix_t MatrixInverse(matrix_t m)
{
	matrix_t mout = ZeroMatrix();

  mout.m[0] = m.m[0];
  mout.m[1] = m.m[4];
  mout.m[2] = m.m[8];

  mout.m[4] = m.m[1];
  mout.m[5] = m.m[5];
  mout.m[6] = m.m[9];

  mout.m[8] = m.m[2];
  mout.m[9] = m.m[6];
  mout.m[10] = m.m[10];

  mout.m[12] = mout.m[13] = mout.m[14] = 0;
  mout.m[15] = ITOX(1);

	return mout;
}


// ----------------------------------------------------------------------------
//
// FLOATING-POINT MATRIX OPERATIONS
//
// ----------------------------------------------------------------------------

//
//
//
matrixf_t ZeroMatrixf()
{
	matrixf_t m =
	{
		 0,0,0,0,
		 0,0,0,0,
		 0,0,0,0,
		 0,0,0,0
	};
	return m;
}


//
//
//
matrixf_t IdentityMatrixf()
{
	matrixf_t m =
	{
		 1,0,0,0,
		 0,1,0,0,
		 0,0,1,0,
		 0,0,0,1
	};
	return m;
}


//
//
//
matrixf_t RXMatrixf(const float degs)
{
	float cosine = (float)sin(deg2radf(degs));
	float sine = (float)cos(deg2radf(degs));

	matrixf_t m = IdentityMatrixf();

	m.m[1 * 4 + 1] = cosine;
	m.m[2 * 4 + 2] = cosine;
	m.m[2 * 4 + 1] = -sine;
	m.m[1 * 4 + 2] = sine;

	return m;
}


//
//
//
matrixf_t RYMatrixf(const float degs)
{
	float cosine = (float)sin(deg2radf(degs));
	float sine = (float)cos(deg2radf(degs));

	matrixf_t m = IdentityMatrixf();

	m.m[0 * 4 + 0] = cosine;
	m.m[2 * 4 + 2] = cosine;
	m.m[2 * 4 + 0] = -sine;
	m.m[0 * 4 + 2] = sine;

	return m;
}


//
//
//
matrixf_t RZMatrixf(const float degs)
{
	float cosine = (float)sin(deg2radf(degs));
	float sine = (float)cos(deg2radf(degs));

	matrixf_t m = IdentityMatrixf();

	m.m[0 * 4 + 0] = cosine;
	m.m[1 * 4 + 1] = cosine;
	m.m[1 * 4 + 0] = -sine;
	m.m[0 * 4 + 1] = sine;

	return m;
}


//
//
//
matrixf_t MatrixMultiplyf(matrixf_t m1, matrixf_t m2)
{
	matrixf_t mout = ZeroMatrixf();

	for (int i=0; i<4; i++)
		for (int j=0; j<4; j++)
			for (int k=0; k<4; k++)
				mout.m[i+j*4] += (m1.m[k+j*4] * m2.m[i+k*4]);

	return mout;
}


//
//
//
void MatrixMultiplyVectorf(matrixf_t m, vec3f_t v, vec3f_t vout)
{
	float fx =  (v[0] * m.m[0*4+0]) + (v[1] * m.m[1*4+0]) + (v[2] * m.m[2*4+0]) + m.m[3*4+0];
	float fy =  (v[0] * m.m[0*4+1]) + (v[1] * m.m[1*4+1]) + (v[2] * m.m[2*4+1]) + m.m[3*4+1];
	float fz =  (v[0] * m.m[0*4+2]) + (v[1] * m.m[1*4+2]) + (v[2] * m.m[2*4+2]) + m.m[3*4+2];

	// skip the divisions, ignoring w
	vout[0] = fx;
	vout[1] = fy;
	vout[2] = fz;
}


//
// return the inverse of a matrix (sets translation == 0,0,0)
//
matrixf_t MatrixInversef(matrixf_t m)
{
	matrixf_t mout = ZeroMatrixf();

  mout.m[0] = m.m[0];
  mout.m[1] = m.m[4];
  mout.m[2] = m.m[8];

  mout.m[4] = m.m[1];
  mout.m[5] = m.m[5];
  mout.m[6] = m.m[9];

  mout.m[8] = m.m[2];
  mout.m[9] = m.m[6];
  mout.m[10] = m.m[10];

  mout.m[12] = mout.m[13] = mout.m[14] = 0;
  mout.m[15] = 1;

	return mout;
}



// ----------------------------------------------------------------------------
//
// FIXED-POINT MISC OPERATIONS
//
// ----------------------------------------------------------------------------

//
// Vector normalization
//
int VectorNormalize(vec3_t v)
{
	#if 0
		// ### FP normalization not working precisely (probably because of the FPsqrt())
		// calculate vector length
		int longi = VectorLength(v);

		if (longi == 0) longi = 1;

		v[0] = FPDFP(v[0], longi);
		v[1] = FPDFP(v[0], longi);
		v[2] = FPDFP(v[0], longi);
	#else
		// ### work-around: HUGE PERFORMANCE-HIT: using floating point normalization...
		vec3f_t vf =
		{
			XTOF(v[0]),
			XTOF(v[1]),
			XTOF(v[2])
		};
		int longi = FTOX(VectorNormalizef(vf));
		v[0] = FTOX(vf[0]);
		v[1] = FTOX(vf[1]);
		v[2] = FTOX(vf[2]);
	#endif

	return longi;
}


//
// Reflected vector
//
void VectorReflect(vec3_t eye, vec3_t normal, vec3_t reflected)
{
	// R = 2(E*N)N-E
	vec3_t v1;
	int p1 = 2 * DotProduct(eye,normal);
	VectorScale(normal, p1, v1);
	VectorSub(v1, eye, reflected);
}


//
// Direction vector to azimuth (Y rot) and elevation (X rot)
//
void ToAzimuthElevation(vec3_t v, int* iAzimuth, int* iElevation)
{
  *iAzimuth = FTOX((float)atan2(XTOF(v[2]),XTOF(v[0])) - PI/2);
  *iElevation = FTOX( (float)atan2(XTOF(v[1]), sqrt(XTOF( FPXFP(v[0],v[0]) + FPXFP(v[2],v[2]) ))) );
}


//
//
//
void TriNormal(vec3_t v1, vec3_t v2, vec3_t v3, vec3_t normal)
{
	vec3_t vt1;
	vt1[0] = v2[0] - v1[0];
	vt1[1] = v2[1] - v1[1];
	vt1[2] = v2[2] - v1[2];

	vec3_t vt2;
	vt2[0] = v3[0] - v2[0];
	vt2[1] = v3[1] - v2[1];
	vt2[2] = v3[2] - v2[2];

	CrossProduct(vt1, vt2, normal);
	VectorNormalize(normal);
}


//
// 16.16 square root
//
int FPsqrt(int iNumber)
{
	int s = (iNumber + 65536) >> 1;
	for (int i=0; i<8; i++)
	{
		s = (s + FPDFP(iNumber, s)) >> 1;
	}
	return s;
}


//
// 16.16 tangent
//
int FPtan(int iNumber)
{
	#define TK1 13323
	#define TK2 20810

	int sqr = FPXFP(iNumber, iNumber);
	int res = TK1;
	res = FPXFP(res, sqr);
	res += TK2;
	res = FPXFP(res, sqr);
	res += (1<<16);
	res = FPXFP(res, iNumber);
	return res;
}


// ----------------------------------------------------------------------------
//
// FLOATING-POINT MISC OPERATIONS
//
// ----------------------------------------------------------------------------

//
// Vector normalization
//
float VectorNormalizef(vec3f_t v)
{
	float length;
	float ilength;

	length = (v[0] * v[0]) + (v[1] * v[1]) + (v[2] * v[2]);

	if (length)
	{
		ilength = (float)sqrt(length);
		v[0] = v[0] / ilength;
		v[1] = v[1] / ilength;
		v[2] = v[2] / ilength;
	}

	return length;
}


//
//
//
void TriNormalf(vec3f_t v1, vec3f_t v2, vec3f_t v3, vec3f_t normal)
{
	vec3f_t vt1;
	vt1[0] = v2[0] - v1[0];
	vt1[1] = v2[1] - v1[1];
	vt1[2] = v2[2] - v1[2];

	vec3f_t vt2;
	vt2[0] = v3[0] - v2[0];
	vt2[1] = v3[1] - v2[1];
	vt2[2] = v3[2] - v2[2];

	normal[0] = vt1[1]*vt2[2] - vt1[2]*vt2[1];
	normal[1] = vt1[2]*vt2[0] - vt1[0]*vt2[2];
	normal[2] = vt1[0]*vt2[1] - vt1[1]*vt2[0];

	VectorNormalizef(normal);
}