glmath.cpp

由TIN生成DEM

/////////////////////////////////////////////////////////////////////////////
// glMath.c : global glMath function implementation file
//
// glOOP (OpenGL Object Oriented Programming library)
// Copyright (c) Craig Fahrnbach 1997, 1998
//
// OpenGL is a registered trademark of Silicon Graphics
//
//
// This program is provided for educational and personal use only and
// is provided without guarantee or warrantee expressed or implied.
//
// Commercial use is strickly prohibited without written permission
// from ImageWare Development.
//
// This program is -not- in the public domain.
//
/////////////////////////////////////////////////////////////////////////////

#include "StdAfx.h"

#include <math.h>
#include "glMath.h"


/*****************************************************************************
 *
 *  Misc. Math Routines
 *
 *****************************************************************************
 */

GLfloat Radiansf(GLfloat Angle)
{
	double r = (double)Angle*PiOver180; 
	return(GLfloat)r;
}

GLfloat Degreesf(GLfloat Angle)
{
	double d = (double)Angle*PiUnder180;
	return(GLfloat)d;
}

GLfloat Cosf(GLfloat Angle)
{
	return((GLfloat)cos(Radiansf(Angle)));
}

GLfloat Sinf(GLfloat Angle)
{
	return((GLfloat)sin(Radiansf(Angle)));
}

GLfloat Powerf(GLfloat Base, int Exponent)
{
	GLfloat BPower;
	int   t;

	if(Exponent==0)
		return(1.0f);
	else
	{
		BPower=1.0f;
		for(t=1; t<=Exponent; t++)
		{
			BPower*=Base;
		}
		return(BPower);
	}
}

GLfloat Sqrf(GLfloat x)
{
	return(x*x);	
}

int Roundf(GLfloat x)
{
	if(x<0)
		return((int)(x-0.5));
	else
		return((int)(x+0.5));
}

GLdouble Radiansd(GLdouble Angle)
{
	double r = Angle*PiOver180; 
	return r;
}

GLdouble Degreesd(GLdouble Angle)
{
	double d = Angle*PiUnder180;
	return d;
}

GLdouble Cosd(GLdouble Angle)
{
	return((GLdouble)cos(Radiansd(Angle)));
}

GLdouble Sind(GLdouble Angle)
{
	return((GLdouble)sin(Radiansd(Angle)));
}


/*****************************************************************************
 *
 *  Vector Functions:
 *
 *****************************************************************************
 */

void Vec3f(GLfloat r, GLfloat s, GLfloat t, VECTORF A)
{
	A[0]=r;
	A[1]=s;
	A[2]=t;
}

void UnVec3f(VECTORF A, GLfloat *r, GLfloat *s, GLfloat *t)
{
	*r=A[0];
	*s=A[1];
	*t=A[2];
}

void Vec4f(GLfloat r, GLfloat s, GLfloat t, GLfloat u, VECTORF A)
{
	A[0]=r;
	A[1]=s;
	A[2]=t;
	A[3]=u;
}

void UnVec4f(VECTORF A, GLfloat *r, GLfloat *s, GLfloat *t, GLfloat *u)
{
	*r=A[0];
	*s=A[1];
	*t=A[2];
	*u=A[3];
}

void VecClear3f(VECTORF A)
{
	A[X] = 0.0f;
	A[Y] = 0.0f;
	A[Z] = 0.0f;
}

void VecClear4f(VECTORF A)
{
	A[X] = 0.0f;
	A[Y] = 0.0f;
	A[Z] = 0.0f;
	A[W] = 0.0f;
}

void VecCopy3f(VECTORF A, VECTORF B)
{
	B[X]=A[X];
	B[Y]=A[Y];
	B[Z]=A[Z];
}

void VecCopy4f(VECTORF A, VECTORF B)
{
	B[X]=A[X];
	B[Y]=A[Y];
	B[Z]=A[Z];
	B[W]=A[W];
}

void VecSubf(VECTORF A, VECTORF B, VECTORF C)
{
	C[0]=A[0]-B[0];
	C[1]=A[1]-B[1];
	C[2]=A[2]-B[2];
}

void VecAddf(VECTORF A, VECTORF B, VECTORF C)
{
	C[0]=A[0]+B[0];
	C[1]=A[1]+B[1];
	C[2]=A[2]+B[2];
}

void VecAdd3f(VECTORF A, VECTORF B, VECTORF C, VECTORF D)
{
	D[0]=A[0]+B[0]+C[0];
	D[1]=A[1]+B[1]+C[1];
	D[2]=A[2]+B[2]+C[2];
}

GLfloat VecDiFFf(VECTORF A, VECTORF B)
{
	GLfloat fDiff;

	fDiff  = A[X]-B[X];
	fDiff += A[Y]-B[Y];
	fDiff += A[Z]-B[Z];

	return fDiff;
}

GLfloat VecDotf(VECTORF A, VECTORF B)
{
	return(A[0]*B[0] + A[1]*B[1] + A[2]*B[2]);
}

void VecCrossf(VECTORF A, VECTORF B, VECTORF C)
{
	C[0]=A[1]*B[2] - A[2]*B[1];
	C[1]=A[2]*B[0] - A[0]*B[2];
	C[2]=A[0]*B[1] - A[1]*B[0];
}

GLfloat VecLenf(VECTORF A)
{
	return(GLfloat)(sqrt(Sqrf(A[0])+Sqrf(A[1])+Sqrf(A[2])));
}

void VecNormalizef(VECTORF A)
{
	GLfloat dist,invdist;

	dist=VecLenf(A);
	if(dist==0.0)
		return;
	else {
		invdist=1.0f/dist;
		A[0]*=invdist;
		A[1]*=invdist;
		A[2]*=invdist;
	}
}

void CalNormalf(VECTORF A, VECTORF B, VECTORF C, VECTORF N)
{
	// Calculate the surface normals of a plane, given points A, B and C
	// which reside on the surface of the plane.
	/*
                     ^ Vn (Normal)
                     |
                     | 
                     |
                     | 
                 Va  *------------------* Vb  (Vu)
                    /                 /
                  /                 / 
                /                 /
              /                 /
         Vc *------------------
       (Vv)	

	*/
	VECTORF	u, v;

	VecSubf(B, A, u);
	VecSubf(C, A, v);
	VecCrossf(v, u, N);
	VecNormalizef(N);
}


/*****************************************************************************
 *
 *  Affine Matrix Transformation Routines
 *
 *****************************************************************************

  We define a 4x4 matrix array, referenced as Row,Column as:

		| 0,0  0,1  0,2  0,3 |
		|                    |	
		| 1,0  1,1  1,2  1,3 |
		|                    |	
		| 2,0  2,1  2,2  2,3 |
		|                    |	
		| 3,0  3,1  3,2  3,3 |
*/

void ZeroMatrix(Matx4x4 A)
{
	// Initialize the matrix to the following values:
	//		0.0		0.0		0.0		0.0
	//		0.0		0.0		0.0		0.0
	//		0.0		0.0		0.0		0.0
	//		0.0		0.0		0.0		0.0
	//
	int i, j;

	for (i=0; i<4; i++) {
		for (j=0; j<4; j++)
			A[i][j]=0.0;
	}
}

void Translate3D(float tx, float ty, float tz, Matx4x4 A)
{
	//	Translation matrix identified as:
	//		 ----------------
	//		| 1   0   0   Tx |
	//		| 0   1   0   Ty |
	//		| 0   0   1   Tz |
	//		| 0   0   0   1  |
	//		 ----------------
	int i;

	ZeroMatrix(A);
	for (i=0; i<4; i++)
		A[i][i]=1.0;

	A[0][3]=tx;
	A[1][3]=ty;
	A[2][3]=tz;
}

void Scale3D(float sx, float sy, float sz, Matx4x4 A)
{
	// Scaling matrix identified as:
	//		 ----------------
	//		| Sx  0   0   0 |
	//		| 0   Sy  0   0 |
	//		| 0   0   Sz  0 |
	//		| 0   0   0   1 |
	//		 ----------------

	ZeroMatrix(A);
	A[0][0]=sx;
	A[1][1]=sy;
	A[2][2]=sz;
	A[3][3]=1.0;
}

void Rotate3D(int m, float Theta, Matx4x4 A)
{
	float c, s;

	ZeroMatrix(A);
	c=Cosf(Theta);
	s=Sinf(Theta);

	// Compensate for rounding errors
	if(fabs(c)<SMALL_NUMBER)
		c=0.0f;
	if(fabs(s)<SMALL_NUMBER)
		s=0.0f;

	switch(m)
	{
		case X:
			//	Rotation about the X-Axis matrix identified as:
			//		 -----------------------
			//		| 1     0      0      0 |
			//		| 0     cosX   -sinX  0 |
			//		| 0     sinX   cosX   0 |
			//		| 0     0      0      1 |
			//		 -----------------------

			A[0][0]= 1.0;
			A[1][1]= c;
			A[1][2]=-s;
			A[2][1]= s;
			A[2][2]= c;
			A[3][3]= 1.0;
			break;

		case Y:
			//	Rotation about the Y-Axis matrix identified as:
			//		 -----------------------
			//		| cosY  0      sinY   0 |
			//		| 0     1      0      0 |
			//		| -sinY 0      cosY   0 |
			//		| 0     0      0      1 |
			//		 -----------------------

			A[0][0]= c;
			A[0][2]= s;
			A[1][1]= 1.0;
			A[2][0]=-s;
			A[2][2]= c;
			A[3][3]= 1.0;
			break;

		case Z:
			//	Rotation about the Z-Axis matrix identified as:
			//		 -----------------------
			//		| cosZ  -sinZ  0      0 |
			//		| sinZ  cosZ   0      0 |
			//		| 0     0      0      0 |
			//		| 0     0      0      1 |
			//		 -----------------------

			A[0][0]= c;
			A[0][1]=-s;
			A[1][0]= s;
			A[1][1]= c;
			A[2][2]= 1.0;
			A[3][3]= 1.0;
			break;
	}
}
	
void MultiplyMatricies(Matx4x4 A, Matx4x4 B, Matx4x4 C)
{
	int   i, j, k;

	for(i=0; i<4; i++) {
		for(j=0; j<4; j++) {
			for(k=0, C[i][j]=0; k<4; k++)
				C[i][j] += A[i][k] * B[k][j];
		}
	}
}

void MatrixCopy(Matx4x4 A, Matx4x4 B)
{
	int  i, j;

	for(i=0; i<4; i++)
	{
		for(j=0; j<4; j++)
			B[i][j]=A[i][j];
	}
}

void TransposeMatrix(Matx4x4 A)
{
	Matx4x4 M;
	int  i, j;

	for(i=0; i<4; i++) {
		for(j=0; j<4; j++)
			M[j][i]=A[i][j];
	}
	MatrixCopy(M, A);
}

void PrepareMatrix(float Ox, float Oy, float Oz,
				   float Sx, float Sy, float Sz,
				   float Rx, float Ry, float Rz,
				   float Tx, float Ty, float Tz,
				   Matx4x4 XForm)
{
	Matx4x4 M1, M2, M3, M4, M5, M6, M7, M8, M9, M10, M11;

	Translate3D(Tx, Ty, Tz, M1);
	Scale3D(Sx, Sy, Sz, M2);
	Rotate3D(Z, Rz, M3);
	Rotate3D(Y, Ry, M4);
	Rotate3D(X, Rx, M5);
	Translate3D(Ox, Oy, Oz, M6);

	MultiplyMatricies(M2, M1, M7);
	MultiplyMatricies(M3, M7, M8);
	MultiplyMatricies(M4, M8, M9);
	MultiplyMatricies(M5, M9, M10);
	MultiplyMatricies(M6, M10, M11);
	MatrixCopy(M11, XForm);
}

void PrepareInvMatrix(float Ox, float Oy, float Oz,
					  float Sx, float Sy, float Sz,
					  float Rx, float Ry, float Rz,
					  float Tx, float Ty, float Tz,
					  Matx4x4 XForm)
{
	Matx4x4 M1, M2, M3, M4, M5, M6, M7, M8, M9, M10, M11;

	Translate3D(-Ox, -Oy, -Oz, M1);
	Rotate3D(X, -Rx, M2);
	Rotate3D(Y, -Ry, M3);
	Rotate3D(Z, -Rz, M4);
	Scale3D(1/Sx, 1/Sy, 1/Sz, M5);
	Translate3D(-Tx, -Ty, -Tz, M6);

	MultiplyMatricies(M2, M1, M7);
	MultiplyMatricies(M3, M7, M8);
	MultiplyMatricies(M4, M8, M9);
	MultiplyMatricies(M5, M9, M10);
	MultiplyMatricies(M6, M10, M11);
	MatrixCopy(M11, XForm);
}

void VecTransformf(VECTORF S, VECTORF D, Matx4x4 M)
{
	GLdouble x, y, z;

	// Transform the Source vector 'S' by the matrix 'M'
	x = M[0][0]*S[0] + M[0][1]*S[1] + M[0][2]*S[2] + M[0][3];
	y = M[1][0]*S[0] + M[1][1]*S[1] + M[1][2]*S[2] + M[1][3];
	z = M[2][0]*S[0] + M[2][1]*S[1] + M[2][2]*S[2] + M[2][3];

	// Compensate for rounding errors
	if(fabs(x) < SMALL_NUMBER)
		x = 0.0f;
	if(fabs(y) < SMALL_NUMBER)
		y = 0.0f;
	if(fabs(z) < SMALL_NUMBER)
		z = 0.0f;

	// Store the transformed values in the Destination
	// vector 'D'
	D[0] = (GLfloat)x;
	D[1] = (GLfloat)y;
	D[2] = (GLfloat)z;
}

/*****************************************************************************
 *
 *  Misc OpenGL Related Functions:
 *
 *****************************************************************************
  We define a 4x4 matrix array,			OpenGL linear matrix format:
  referenced as Row,Column as:

	| 0,0  0,1  0,2  0,3 |					|a0  a4  a8   a12|
	|                    |					|                |
	| 1,0  1,1  1,2  1,3 |					|a1  a5  a9   a13|
	|                    |					|                |
	| 2,0  2,1  2,2  2,3 |					|a2  a6  a10  a14|
	|                    |					|                |
	| 3,0  3,1  3,2  3,3 |					|a3  a7  a11  a15|

 */

void glMatrixTo4x4(GLdouble M[16], Matx4x4 A)
{
	int i, j;

	for(i=0; i<4; i++) {
		for(j=0; j<4; j++)
			A[i][j]=(float)M[(i*4)+j];
	}
}

void Matx4x4ToglMatrix(Matx4x4 A, GLdouble M[16])
{
	int i, j;

	for(i=0; i<4; i++) {
		for(j=0; j<4; j++)
			M[(i*4)+j]=(GLdouble)A[i][j];
	}
}

void Transformf(VECTORF S, VECTORF D, GLdouble M[16])
{
	GLdouble x, y, z;

	// Transform the Source vector 'S' by the matrix 'M'
	x = M[0]*S[0] + M[1]*S[1] + M[2] *S[2]; // + M[3];
	y = M[4]*S[0] + M[5]*S[1] + M[6] *S[2]; // + M[7];
	z = M[8]*S[0] + M[9]*S[1] + M[10]*S[2]; // + M[11];

	// Compensate for rounding errors
	if(fabs(x) < SMALL_NUMBER)
		x = 0.0f;
	if(fabs(y) < SMALL_NUMBER)
		y = 0.0f;
	if(fabs(z) < SMALL_NUMBER)
		z = 0.0f;

	// Store the transformed values in the Destination
	// vector 'D'
	D[0] = (GLfloat)x;
	D[1] = (GLfloat)y;
	D[2] = (GLfloat)z;
}