normtest.cpp

空战游戏flacon源码

#include <stdio.h>
#include <math.h>

#define	ANGLE_FACTOR	(180.0 / 3.141592654)

struct Plane {
	double	a, b, c, d;
};

struct Vertex {
	double	x, y, z;
};

struct Matrix {
	double M11, M12, M13;
	double M21, M22, M23;
	double M31, M32, M33;
};

Vertex VertexList[] = {
	{ -10, -10, 0 },
	{ -10, 10, 0 },
	{ 10, 10, 0 },
	{ 10, -10, 0 },
};

int PolyList [] = {
		4, 0, 1, 2, 3, 0,
		-1
};

void CalculatePlane (int *face, Plane *plane);
void CalculatePlane1 (int *face, Plane *plane);

void glGetSinCos (double *sinOut, double *cosOut, int angle);
void MultiplyMatrix (Matrix *mat, Matrix *mat1, Matrix *mat2);
void MultiplyVertex (Vertex *vtx, Vertex *vtx1, Matrix *mat);

void CreateIdentity (Matrix *mat);

void CreateXRotation (Matrix *mat, int angle);
void CreateYRotation (Matrix *mat, int angle);
void CreateZRotation (Matrix *mat, int angle);
void CreateTransformationMatrix (Matrix *mat, int yaw, int pitch, int roll);

void CreateXRotation (Matrix *mat, double sine, double cosine);
void CreateYRotation (Matrix *mat, double sine, double cosine);
void CreateZRotation (Matrix *mat, double sine, double cosine);
void CreateTransformationMatrix (Matrix *mat, Vertex *Normal, Vertex *Up);

void CreateTransformationMatrix1 (Matrix *mat, Vertex *Normal, Vertex *Up);

inline void glCrossProduct (Vertex *p, Vertex *p1, Vertex *p2)
{
	p -> x = p1 -> y * p2 -> z - p1 -> z * p2 -> y;
	p -> y = p1 -> z * p2 -> x - p1 -> x * p2 -> z;
	p -> z = p1 -> x * p2 -> y - p1 -> y * p2 -> x;
}

inline void glNormalVector (Vertex *p1, Vertex *p2, Vertex *p3, Vertex *pout)
{
	Vertex	delta1, delta2;

	delta1.x = p2 -> x - p1 -> x;
	delta1.y = p2 -> y - p1 -> y;
	delta1.z = p2 -> z - p1 -> z;
	delta2.x = p3 -> x - p2 -> x;
	delta2.y = p3 -> y - p2 -> y;
	delta2.z = p3 -> z - p2 -> z;
	glCrossProduct (pout, &delta1, &delta2);
}

inline double glDotProduct (Vertex *p1, Vertex *p2)
{
	return (p1 -> x * p2 -> x + p1 -> y * p2 -> y + p1 -> z * p2 -> z);
}

inline void glNormalizedVector (Vertex *vtx)
{
	double dist = sqrt (	vtx -> x * vtx -> x + 
							vtx -> y * vtx -> y + 
							vtx -> z * vtx -> z);
	if (dist) {
		dist = 1.0 / dist;
		vtx -> x *= dist;
		vtx -> y *= dist;
		vtx -> z *= dist;
	}
}

inline void glNormalizedVector (Vertex *vtx1, Vertex *vtx2)
{
	double dist = sqrt (	vtx2 -> x * vtx2 -> x + 
							vtx2 -> y * vtx2 -> y + 
							vtx2 -> z * vtx2 -> z);
	if (dist) dist = 1.0 / dist;
	else dist = 1.0;
	vtx1 -> x = vtx2 -> x * dist;
	vtx1 -> y = vtx2 -> y * dist;
	vtx1 -> z = vtx2 -> z * dist;
}

void main ()
{
	int	*poly = PolyList;
	Plane	plane;
	Matrix	mat;
	Vertex	Normal, Up, Right;

	CalculatePlane (poly, &plane);
	printf ("Normal1 -> %f %f %f %f\n", plane.a, plane.b, plane.c, plane.d);
	CalculatePlane1 (poly, &plane);
	printf ("Normal2 -> %f %f %f %f\n", plane.a, plane.b, plane.c, plane.d);
	CreateTransformationMatrix (&mat, 2048, 0, -4096);
	MultiplyVertex (&Normal, (Vertex *) &plane, &mat);
	printf ("Normal After Transformation -> %f %f %f\n", Normal.x, Normal.y, Normal.z);
	Normal.x = -Normal.x;
	Normal.y = -Normal.y;
	Normal.z = -Normal.z;
	plane.a = VertexList[*(poly+2)].x - VertexList[*(poly+1)].x;
	plane.b = VertexList[*(poly+2)].y - VertexList[*(poly+1)].y;
	plane.c = VertexList[*(poly+2)].z - VertexList[*(poly+1)].z;
	glNormalizedVector ((Vertex *) &plane);
	printf ("Up Normal -> %f %f %f\n", plane.a, plane.b, plane.c);
	MultiplyVertex (&Up, (Vertex *) &plane, &mat);
	printf ("Up Normal After Transformation -> %f %f %f\n", Up.x, Up.y, Up.z);
	plane.a = VertexList[*(poly+3)].x - VertexList[*(poly+2)].x;
	plane.b = VertexList[*(poly+3)].y - VertexList[*(poly+2)].y;
	plane.c = VertexList[*(poly+3)].z - VertexList[*(poly+2)].z;
	glNormalizedVector ((Vertex *) &plane);
	printf ("Right Normal -> %f %f %f\n", plane.a, plane.b, plane.c);
	glCrossProduct (&Right, &Normal, &Up);
	printf ("Right Normal After Transformation -> %f %f %f\n", Right.x, Right.y, Right.z);

	double yaw = atan2 (Normal.x,Normal.z) * ANGLE_FACTOR;
	double pitch = atan2 (Normal.y, Normal.z) * ANGLE_FACTOR;
	double roll = atan2 (Up.x, Up.y) * ANGLE_FACTOR;
	printf ("Rotation %f %f %f\n", yaw, pitch, roll);

	CreateTransformationMatrix (&mat, &Normal, &Up);
	CreateTransformationMatrix1 (&mat, &Normal, &Up);

	Vertex	vtx;
	MultiplyVertex (&vtx, (Vertex *) &VertexList[*(poly+1)], &mat);
	printf ("Vertex After Transformation -> %f %f %f\n", vtx.x, vtx.y, vtx.z);
	MultiplyVertex (&vtx, (Vertex *) &VertexList[*(poly+2)], &mat);
	printf ("Vertex After Transformation -> %f %f %f\n", vtx.x, vtx.y, vtx.z);
	MultiplyVertex (&vtx, (Vertex *) &VertexList[*(poly+3)], &mat);
	printf ("Vertex After Transformation -> %f %f %f\n", vtx.x, vtx.y, vtx.z);
	MultiplyVertex (&vtx, (Vertex *) &VertexList[*(poly+4)], &mat);
	printf ("Vertex After Transformation -> %f %f %f\n", vtx.x, vtx.y, vtx.z);
}

void CalculatePlane (int *face, Plane *plane)
{
	Vertex	*vtx1, *vtx2;
	int		*vtxlist;
	double	nx, ny, nz, len, d;
	int		i;

	nx = ny = nz = 0.0;
	vtxlist = face+1;
	i = *face;
	while (i--) {
		vtx1 = &(VertexList[*vtxlist]);
		vtx2 = &(VertexList[*(vtxlist+1)]);
		vtxlist++;
		nx += ((vtx1 -> y - vtx2 -> y) * (vtx1 -> z + vtx2 -> z));
		ny += ((vtx1 -> z - vtx2 -> z) * (vtx1 -> x + vtx2 -> x));
		nz += ((vtx1 -> x - vtx2 -> x) * (vtx1 -> y + vtx2 -> y));
	}
	len = 1.0 / (sqrt(nx * nx + ny * ny + nz * nz));
	plane -> a = (nx * len);
	plane -> b = (ny * len);
	plane -> c = (nz * len);

	d = 0.0;
	vtxlist = face+1;
	i = *face;
	while (i--) {
		vtx1 = &(VertexList[*vtxlist++]);
		d -= (nx * vtx1 -> x + ny * vtx1 -> y + nz * vtx1 -> z);
	}
	len = (len * d) / *face;
	plane -> d = len;
}

void CalculatePlane1 (int *face, Plane *plane)
{
	Vertex	*vtx1, *vtx2, *vtx3;

	vtx1 = &(VertexList[*(face+1)]);
	vtx2 = &(VertexList[*(face+2)]);
	vtx3 = &(VertexList[*(face+3)]);
	glNormalVector (vtx1, vtx2, vtx3, (Vertex *) plane);
	glNormalizedVector ((Vertex *) plane);
	plane -> d = -glDotProduct (vtx1, (Vertex *) plane);
}

#include "..\..\3dLib\costable.h"

void glGetSinCos (double *sinOut, double *cosOut, int angle)
{
	angle &= 0x3fff;
	if (angle & 0x1000) {
// angle between 270-360
		if (angle & 0x2000) {	
			*sinOut = -CosineTable[angle-0x3000];
			*cosOut = CosineTable[0x4000-angle];
		}
// angle between 90-180
		else {
			*sinOut = CosineTable[angle-0x1000];
			*cosOut = -CosineTable[0x2000-angle];
		}
	}
	else {
		if (angle & 0x2000) {	
// angle between 180-270
			*sinOut = -CosineTable[0x3000-angle];
			*cosOut = -CosineTable[angle-0x2000];
		}
		else {
// angle between 0-90
			*sinOut = CosineTable[0x1000-angle];
			*cosOut = CosineTable[angle];
		}
	}
}

void CreateIdentity (Matrix *mat)
{
	mat -> M11 = mat -> M22 = mat -> M33 = 1.0;
	mat -> M12 = mat -> M13 = mat -> M21 = mat -> M23 = 
	mat -> M31 = mat -> M32 = 0.0;
}

void CreateXRotation (Matrix *mat, int angle)
{
	double sine, cosine;

	glGetSinCos (&sine, &cosine, angle);
	mat -> M11 = 1.0;
	mat -> M12 = mat -> M13 = mat -> M21 = mat -> M31 = 0.0;
	mat -> M22 = mat -> M33 = cosine;
	mat -> M23 = sine;
	mat -> M32 = -sine;
}

void CreateXRotation (Matrix *mat, double sine, double cosine)
{
	mat -> M11 = 1.0;
	mat -> M12 = mat -> M13 = mat -> M21 = mat -> M31 = 0.0;
	mat -> M22 = mat -> M33 = cosine;
	mat -> M23 = sine;
	mat -> M32 = -sine;
}

void CreateYRotation (Matrix *mat, int angle)
{
	double sine, cosine;

	glGetSinCos (&sine, &cosine, angle);
	mat -> M22 = 1.0;
	mat -> M12 = mat -> M21 = mat -> M23 = mat -> M32 = 0.0;
	mat -> M11 = mat -> M33 = cosine;
	mat -> M13 = -sine;
	mat -> M31 = sine;
}

void CreateYRotation (Matrix *mat, double sine, double cosine)
{
	mat -> M22 = 1.0;
	mat -> M12 = mat -> M21 = mat -> M23 = mat -> M32 = 0.0;
	mat -> M11 = mat -> M33 = cosine;
	mat -> M13 = -sine;
	mat -> M31 = sine;
}

void CreateZRotation (Matrix *mat, int angle)
{
	double sine, cosine;

	glGetSinCos (&sine, &cosine, angle);
	mat -> M33 = 1.0;
	mat -> M13 = mat -> M23 = mat -> M31 = mat -> M32 = 0.0;
	mat -> M11 = mat -> M22 = cosine;
	mat -> M12 = sine;
	mat -> M21 = -sine;
}

void CreateZRotation (Matrix *mat, double sine, double cosine)
{
	mat -> M33 = 1.0;
	mat -> M13 = mat -> M23 = mat -> M31 = mat -> M32 = 0.0;
	mat -> M11 = mat -> M22 = cosine;
	mat -> M12 = sine;
	mat -> M21 = -sine;
}

void MultiplyMatrix (Matrix *mat, Matrix *mat1, Matrix *mat2)
{
	mat->M11 = mat1->M11*mat2->M11 + mat1->M12*mat2->M21 + mat1->M13*mat2->M31;
	mat->M12 = mat1->M11*mat2->M12 + mat1->M12*mat2->M22 + mat1->M13*mat2->M32;
	mat->M13 = mat1->M11*mat2->M13 + mat1->M12*mat2->M23 + mat1->M13*mat2->M33;
	mat->M21 = mat1->M21*mat2->M11 + mat1->M22*mat2->M21 + mat1->M23*mat2->M31;
	mat->M22 = mat1->M21*mat2->M12 + mat1->M22*mat2->M22 + mat1->M23*mat2->M32;
	mat->M23 = mat1->M21*mat2->M13 + mat1->M22*mat2->M23 + mat1->M23*mat2->M33;
	mat->M31 = mat1->M31*mat2->M11 + mat1->M32*mat2->M21 + mat1->M33*mat2->M31;
	mat->M32 = mat1->M31*mat2->M12 + mat1->M32*mat2->M22 + mat1->M33*mat2->M32;
	mat->M33 = mat1->M31*mat2->M13 + mat1->M32*mat2->M23 + mat1->M33*mat2->M33;
}

void MultiplyVertex (Vertex *vtx, Vertex *vtx1, Matrix *mat)
{
	vtx->x = vtx1->x*mat->M11 + vtx1->y*mat->M21 + vtx1->z*mat->M31;
	vtx->y = vtx1->x*mat->M12 + vtx1->y*mat->M22 + vtx1->z*mat->M32;
	vtx->z = vtx1->x*mat->M13 + vtx1->y*mat->M23 + vtx1->z*mat->M33;
}

void CreateTransformationMatrix (Matrix *mat, int yaw, int pitch, int roll)
{
	Matrix matX, matY, matZ, temp;

	CreateXRotation (&matX, pitch);
	CreateYRotation (&matY, yaw);
	CreateZRotation (&matZ, roll);
	MultiplyMatrix (&temp, &matZ, &matX);
	MultiplyMatrix (mat, &temp, &matY);
}

void CreateTransformationMatrix (Matrix *mat, Vertex *Normal, Vertex *Up)
{
	Matrix	matX, matY, matZ, temp;
	Vertex	nvtx, uvtx, vtx;
	double	sine, cosine, d;

	d = sqrt (Normal -> x * Normal -> x + Normal -> z * Normal -> z);
	if (d == 0.0) {
		CreateIdentity (&matY);
		nvtx = *Normal;
		uvtx = *Up;
	}
	else {
		d = 1.0 / d;
		sine = -Normal -> x * d;
		cosine = Normal -> z * d;
		CreateYRotation (&matY, sine, cosine);
		MultiplyVertex (&nvtx, Normal, &matY);
		MultiplyVertex (&uvtx, Up, &matY);
	}

	d = sqrt (nvtx.y * nvtx.y + nvtx.z * nvtx.z);
	if (d == 0.0) {
		CreateIdentity (&matX);
		vtx = uvtx;
	}
	else {
		d = 1.0 / d;
		sine = nvtx.y * d;
		cosine = nvtx.z * d;
		CreateXRotation (&matX, sine, cosine);
		MultiplyVertex (&vtx, &uvtx, &matX);
	}

	d = sqrt (vtx.x * vtx.x + vtx.y * vtx.y);
	if (d == 0.0) {
		CreateIdentity (&matZ);
	}
	else {
		d = 1.0 / d;
		sine = vtx.x * d;
		cosine = vtx.y * d;
		CreateZRotation (&matZ, sine, cosine);
	}

	MultiplyMatrix (&temp, &matY, &matX);
	MultiplyMatrix (mat, &temp, &matZ);
}

void CreateTransformationMatrix1 (Matrix *mat, Vertex *Normal, Vertex *Up)
{
	Vertex	vtx, vtx1, vtx2;
	glCrossProduct (&vtx, Up, Normal);
	glNormalizedVector (&vtx);
	glNormalizedVector (&vtx2, Normal);
	glCrossProduct (&vtx1, &vtx2, &vtx);
	mat -> M11 = vtx.x;
	mat -> M12 = vtx.y;
	mat -> M13 = vtx.z;
	mat -> M21 = vtx1.x;
	mat -> M22 = vtx1.y;
	mat -> M23 = vtx1.z;
	mat -> M31 = vtx2.x;
	mat -> M32 = vtx2.y;
	mat -> M33 = vtx2.z;
}