/*
 * @(#)ThreedM.c
 *
 * Copyright 1994 - 2013  David A. Bagley, bagleyd AT verizon.net
 *
 * All rights reserved.
 *
 * Permission to use, copy, modify, and distribute this software and
 * its documentation for any purpose and without fee is hereby granted,
 * provided that the above copyright notice appear in all copies and
 * that both that copyright notice and this permission notice appear in
 * supporting documentation, and that the name of the author not be
 * used in advertising or publicity pertaining to distribution of the
 * software without specific, written prior permission.
 *
 * 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.
 */

/* Mathematics file for Threed */

#include "ThreedP.h"

static double sine[RT_ANGLE + 1];

/* Black Art of 3D Game Programming Andre LaMothe */
void
createVector3D(Point3D *init, Point3D *term, Point3D *result)
{
	/* this creates a vector from 2 points in 3D space */
	result->x = term->x - init->x;
	result->y = term->y - init->y;
	result->z = term->z - init->z;
}

double
magnitudeVector3D(Vector3D *v)
{
	return sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
}

double
dotProduct3D(Vector3D *u, Vector3D *v)
{
	return (u->x * v->x + u->y * v->y + u->z * v->z);
}

void
crossProduct3D(Vector3D *u, Vector3D *v, Vector3D *normal)
{
	/* left hand rule */
	normal->x = u->y * v->z - u->z * v->y;
	normal->y = u->z * v->x - u->x * v->z;
	normal->z = u->x * v->y - u->y * v->x;
}

#if 0
static void
addProduct3D(Vector3D *u, Vector3D *v, Vector3D *sum)
{
	sum->x = u->x + v->x;
	sum->y = u->y + v->y;
	sum->z = u->z + v->z;
}

static void
subtractProduct3D(Vector3D *u, Vector3D *v, Vector3D *difference)
{
	difference->x = u->x - v->x;
	difference->y = u->y - v->y;
	difference->z = u->z - v->z;
}
#endif

static void
identityMatrix4x4(Matrix4x4 a)
{
#ifdef SLOW
	a[0][1] = a[0][2] = a[0][3] = 0.0;
	a[1][0] = a[1][2] = a[1][3] = 0.0;
	a[2][0] = a[2][1] = a[2][3] = 0.0;
	a[3][0] = a[3][1] = a[3][2] = 0.0;
#else
	(void) memset((void *) a, 0, sizeof(double) * 16);
#endif
	/* diagonal of 1's */
	a[0][0] = a[1][1] = a[2][2] = a[3][3] = 1.0;
}

#if 0
static void
zeroMatrix4x4(Matrix4x4 a)
{
#ifdef SLOW
	a[0][0] = a[0][1] = a[0][2] = a[0][3] = 0.0;
	a[1][0] = a[1][1] = a[1][2] = a[1][3] = 0.0;
	a[2][0] = a[2][1] = a[2][2] = a[2][3] = 0.0;
	a[3][0] = a[3][1] = a[3][2] = a[3][3] = 0.0;
#else
	(void) memset((void *) a, 0, sizeof(double) * 16);
#endif
}
#endif

static void
copyMatrix4x4(Matrix4x4 source, Matrix4x4 destination)
{
	int i, j;

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++)
			destination[i][j] = source[i][j];
}

#if 0
static void
printMatrix4x4(Matrix4x4 a)
{
	int i, j;

	for (j = 0; j < 4; j++) {
		for (i = 0; i < 4; i++)
			(void) printf("%g ", a[i][j]);
		(void) printf("\n");
	}
}

void
printMatrix1x4(Matrix1x4 a)
{
	int i;

	for (i = 0; i < 4; i++)
		(void) printf("%g ", a[i]);
	(void) printf("\n");
}
#endif

static void
multiplyMatrix4x4_4x4(Matrix4x4 a, Matrix4x4 b, Matrix4x4 result)
{
#ifdef SLOW
	int i, j, k;
	double sum;

	for (i = 0; i < 4; i++)
		for (j = 0; j < 4; j++) {
			sum = 0.0;
			for (k = 0; k < 4; k++)
				sum += a[i][k] * b[k][j];
			result[i][j] = sum;
		}
#else
	/* sparse aware, not for general use */
	double *a00 = &(a[0][0]), *a01 = &(a[0][1]), *a02 = &(a[0][2]);
	double *a10 = &(a[1][0]), *a11 = &(a[1][1]), *a12 = &(a[1][2]);
	double *a20 = &(a[2][0]), *a21 = &(a[2][1]), *a22 = &(a[2][2]);
	double *b00 = &(b[0][0]), *b01 = &(b[0][1]), *b02 = &(b[0][2]);
	double *b10 = &(b[1][0]), *b11 = &(b[1][1]), *b12 = &(b[1][2]);
	double *b20 = &(b[2][0]), *b21 = &(b[2][1]), *b22 = &(b[2][2]);

	result[0][0] = *a00 * *b00 + *a01 * *b10 + *a02 * *b20;
	result[0][1] = *a00 * *b01 + *a01 * *b11 + *a02 * *b21;
	result[0][2] = *a00 * *b02 + *a01 * *b12 + *a02 * *b22;
	result[1][0] = *a10 * *b00 + *a11 * *b10 + *a12 * *b20;
	result[1][1] = *a10 * *b01 + *a11 * *b11 + *a12 * *b21;
	result[1][2] = *a10 * *b02 + *a11 * *b12 + *a12 * *b22;
	result[2][0] = *a20 * *b00 + *a21 * *b10 + *a22 * *b20;
	result[2][1] = *a20 * *b01 + *a21 * *b11 + *a22 * *b21;
	result[2][2] = *a20 * *b02 + *a21 * *b12 + *a22 * *b22;
	result[0][3] = result[1][3] = result[2][3] = 0.0;
	result[3][0] = result[3][1] = result[3][2] = 0.0;
	result[3][3] = 1.0;
#endif
}

#if 0
static void
multiplyMatrix1x4_4x4(Matrix1x4 a, Matrix4x4 b, Matrix1x4 result)
{
	int j, k;
	double sum;

	for (j = 0; j < 4; j++) {
		sum = 0.0;
		for (k = 0; k < 4; k++)
			sum += a[k] * b[k][j];
		result[j] = sum;
	}
}
#endif

/* I guess the average radius would be the average of the vertices and
 * the average of the midpoint of the faces (minimumObjectRadius),
 * averaged together */

double
minimumObjectRadius(Object3D * obj)
{
	double radius, minRadius, x, y, z;
	int surf, vert, vertex;

	minRadius = -1.0;
	for (surf = 0; surf < obj->numSurfaces; surf++) {
		int mapIndex = obj->surface[surf].mapIndex;

		x = y = z = 0.0;
		for (vert = 0; vert < obj->surface[surf].numVertices;
				vert++, mapIndex++) {
			vertex = obj->map[mapIndex];
			x += obj->local[vertex].x;
			y += obj->local[vertex].y;
			z += obj->local[vertex].z;
		}
		x /= vert;
		y /= vert;
		z /= vert;
		radius = sqrt(x * x + y * y + z * z);
		if (radius < minRadius || minRadius == -1.0)
			minRadius = radius;
	}
	return minRadius;
}

double
maximumObjectRadius(Object3D *obj)
{
	double radius, maxRadius, x, y, z;
	int vert;

	maxRadius = 0.0;
	for (vert = 0; vert < obj->numVertices; vert++) {
		x = obj->local[vert].x;
		y = obj->local[vert].y;
		z = obj->local[vert].z;
		radius = sqrt(x * x + y * y + z * z);
		if (radius > maxRadius)
			maxRadius = radius;
	}
	return maxRadius;
}

#if 0
static void
translateObject(Object3D *obj, Vector3D *v)
{
	obj->origin.x += v->x;
	obj->origin.y += v->y;
	obj->origin.z += v->z;
}

static void
positionObject(Object3D *obj, Point3D *p)
{
	obj->origin.x = p->x;
	obj->origin.y = p->y;
	obj->origin.z = p->z;
}

static void
scaleObject(Object3D *obj, double scaleFactor)
{
	int plane, point;
	double scale; /* holds the square of the scaling factor, needed to
			 resize the surface normal for lighting calculations */

	for (point = 0; point < obj->numVertices; point++) {
		obj->local[point].x *= scaleFactor;
		obj->local[point].y *= scaleFactor;
		obj->local[point].z *= scaleFactor;
	}
	scale = scaleFactor * scaleFactor;
	/* now scale all pre-computed normals */
	for (plane = 0; plane < obj->numSurfaces; plane++) {
		obj->surface[plane].normalLength *= scale;
	}
	obj->radius *= scaleFactor;
}
#endif

void
createTrigTables(void)
{
	int angle;
	double sepRadians = M_PI / ST_ANGLE;

	for (angle = 0; angle <= RT_ANGLE; angle++) {
		sine[angle] = sin(sepRadians * angle);
	}
}

static double
sinInt(int angleDegrees)
{
	int angle = (NUM_DEGREES + angleDegrees) % NUM_DEGREES;

	if (angle <= RT_ANGLE)
		return sine[angle];
	else if (angle <= ST_ANGLE)
		return sine[ST_ANGLE - angle];
	else if (angle <= NUM_DEGREES - RT_ANGLE)
		return -sine[angle - ST_ANGLE];
	else
		return -sine[NUM_DEGREES - angle];
}

static double
cosInt(int angleDegrees)
{
	int angle = (NUM_DEGREES + angleDegrees) % NUM_DEGREES;

	if (angle <= RT_ANGLE)
		return sine[RT_ANGLE - angle];
	else if (angle <= ST_ANGLE)
		return -sine[angle - RT_ANGLE];
	else if (angle <= NUM_DEGREES - RT_ANGLE)
		return -sine[NUM_DEGREES - RT_ANGLE - angle];
	else
		return sine[angle - NUM_DEGREES + RT_ANGLE];
}

void
rotateObject(Object3D *obj, IntAngle3D *angle) {
	int vert;
	int product = 0; /* used to determine which matrices need
			    multiplying */

	Matrix4x4 rotateTheta, rotatePhi, rotatePsi, rotate, temp;
	Point3D tempPoint;

	if (angle->theta == 0 && angle->phi == 0 && angle->psi == 0)
		return;
	identityMatrix4x4(rotate);
	if (angle->phi != 0) {
		identityMatrix4x4(rotatePhi);
		rotatePhi[1][1] = cosInt(angle->phi);
		rotatePhi[1][2] = sinInt(angle->phi);
		rotatePhi[2][1] = -sinInt(angle->phi);
		rotatePhi[2][2] = cosInt(angle->phi);
		product |= 4;
	}
	if (angle->theta != 0) {
		identityMatrix4x4(rotateTheta);
		rotateTheta[0][0] = cosInt(angle->theta);
		rotateTheta[0][2] = -sinInt(angle->theta);
		rotateTheta[2][0] = sinInt(angle->theta);
		rotateTheta[2][2] = cosInt(angle->theta);
		product |= 2;
	}
	if (angle->psi != 0) {
		identityMatrix4x4(rotatePsi);
		rotatePsi[0][0] = cosInt(angle->psi);
		rotatePsi[0][1] = sinInt(angle->psi);
		rotatePsi[1][0] = -sinInt(angle->psi);
		rotatePsi[1][1] = cosInt(angle->psi);
		product |= 1;
	}
	/* Compute the final rotation matrix, determine the proper product
	 * of matrices.  Determine which matrices need multiplying, this is
	 * worth the time it would take to concatenate matrices together
	 * that do not have any effect. */
	/* (void) printf("product %d\n", product); */
	switch (product) {
	case 0: /* do nothing there is no rotation */
		break;
	case 1: /* final matrix = psi */
		copyMatrix4x4(rotatePsi, rotate);
		break;
	case 2: /* final matrix = theta */
		copyMatrix4x4(rotateTheta, rotate);
		break;
	case 3: /* final matrix = theta * psi */
		multiplyMatrix4x4_4x4(rotateTheta, rotatePsi, rotate);
		break;
	case 4: /* final matrix = phi */
		copyMatrix4x4(rotatePhi, rotate);
		break;
	case 5: /* final matrix = phi * psi */
		multiplyMatrix4x4_4x4(rotatePhi, rotatePsi, rotate);
		break;
	case 6: /* final matrix = phi * theta */
		multiplyMatrix4x4_4x4(rotatePhi, rotateTheta, rotate);
		break;
	case 7: /* final matrix = phi * theta * psi */
		multiplyMatrix4x4_4x4(rotatePhi, rotateTheta, temp);
		multiplyMatrix4x4_4x4(temp, rotatePsi, rotate);
		break;
	default:
		break;
	}
	for (vert = 0; vert < obj->numVertices; vert++) {
		tempPoint.x = obj->local[vert].x * rotate[0][0] +
			obj->local[vert].y * rotate[1][0] +
			obj->local[vert].z * rotate[2][0];
		tempPoint.y = obj->local[vert].x * rotate[0][1] +
			obj->local[vert].y * rotate[1][1] +
			obj->local[vert].z * rotate[2][1];
		tempPoint.z = obj->local[vert].x * rotate[0][2] +
			obj->local[vert].y * rotate[1][2] +
			obj->local[vert].z * rotate[2][2];
		obj->local[vert].x = tempPoint.x;
		obj->local[vert].y = tempPoint.y;
		obj->local[vert].z = tempPoint.z;
		/* (void) printf("%d: x %g, y %g, z %g\n",
			vert, obj->local[vert].eye.x,
			obj->local[vert].y, obj->local[vert].z); */
	}
}

#if 0
static Boolean
clipPoint(Point *a, Point *b, Point *minClip, Point *maxClip)
{
	Point intersection = {0, 0};
	/*typedef enum {TOP, RIGHT, BOTTOM, LEFT} Edge;*/
	const int TOP = 0;
	const int RIGHT = 1;
	const int BOTTOM = 2;
	const int LEFT = 3;
	Boolean edges[4] = {False, False, False, False};
	Boolean success = False;
	double dx, dy;

	/* compute deltas */
	dx = b->x - a->x;
	dy = b->y - a->y;
	/* compute what boundary lines need to be clipped against */
	/* vertical intersections */
	if (b->x > maxClip->x) {
		edges[RIGHT] = True;
		if (dx == 0.0) /* invalidate intersection */
			intersection.y = -1;
		else
			intersection.y = (int) (0.5 + (dy / dx) *
				(maxClip->x - a->x) + a->y);
	} else if (b->x < minClip->x) {
		edges[LEFT] = True;
		if (dx == 0.0) /* invalidate intersection */
			intersection.y = -1;
		else
			intersection.y = (int) (0.5 + (dy / dx) *
				(minClip->x - a->x) + a->y);
	}
	/* horizontal intersections */
	if (b->y > maxClip->y) {
		edges[BOTTOM] = True;
		if (dy == 0.0) /* invalidate intersection */
			intersection.x = -1;
		else
			intersection.x = (int) (0.5 + (dx / dy) *
				(maxClip->y - a->y) + a->x);
	} else if (b->y < minClip->y) {
		edges[TOP] = True;
		if (dy == 0.0) /* invalidate intersection */
			intersection.x = -1;
		else
			intersection.x = (int) (0.5 + (dx / dy) *
				(minClip->y - a->y) + a->x);
	}
	/* now we know where the line passed through */
	/* compute which edge is the proper intersection */
	if (edges[RIGHT] && (intersection.y >= minClip->y &&
			intersection.y <= maxClip->y)) {
		b->x = maxClip->x;
		b->y = intersection.y;
		success = True;
	} else if (edges[LEFT] && (intersection.y >= minClip->y &&
			intersection.y <= maxClip->y)) {
		b->x = minClip->x;
		b->y = intersection.y;
		success = True;
	}
	if (edges[BOTTOM] && (intersection.x >= minClip->x &&
			intersection.x <= maxClip->x)) {
		b->y = maxClip->y;
		b->x = intersection.x;
		success = True;
	} else if (edges[TOP] && (intersection.x >= minClip->x &&
			intersection.x <= maxClip->x)) {
		b->y = minClip->y;
		b->x = intersection.x;
		success = True;
	}
	return success;
}

static Boolean
clipLine(Point *a, Point *b, Point *minClip, Point *maxClip)
{
	Boolean aSuccess = False, bSuccess = False;
	Boolean aVisible = False, bVisible = False;

	/* test if line completely visible */
	if ((a->x >= minClip->x && a->x <= maxClip->x) &&
			(a->y >= minClip->y && a->y <= maxClip->y))
		aVisible = True;
	if ((b->x >= minClip->x && b->x <= maxClip->x) &&
			(b->y >= minClip->y && b->y <= maxClip->y))
		bVisible = True;
	if (aVisible && bVisible)
		return True;

	if (!aVisible && !bVisible &&
			(((a->x < minClip->x && b->x < minClip->x) ||
			(a->x >= minClip->x && b->x > maxClip->x)) ||
			((a->y >= minClip->y && b->y < minClip->y) ||
			(a->y >= minClip->y && b->y > maxClip->y))))
		/* test if line completely invisible */
		return False;
	/* take care of case where either endpoint is in clipping region */
	if (!bVisible)
		bSuccess = clipPoint(a, b, minClip, maxClip);
	/* test second endpoint */
	if (!aVisible)
		aSuccess = clipPoint(b, a, minClip, maxClip);
	return (aSuccess || bSuccess);
}
#endif

/* Arash Partow <arashp@hotmail.com>
 * Homepage: http://www.partow.net
 * Translated to C from Pacal
 */
static int
orientation(Point *a, Point *b, Point *p)
{
	int orient;

	 /* Linear determinant of the 3 points */
	orient = (b->x - a->x) * (p->y - a->y) - (p->x - a->x) * (b->y - a->y);
	/* 1 right-hand side, -1 for left-hand side */
	return (orient == 0) ? 0 : ((orient > 0) ? 1 : -1);
}

static Boolean
pointInTriangle(Point *pt, Point *a, Point *b, Point *c)
{
	int or0 = orientation(a, b, pt);
	int or1 = orientation(b, c, pt);
	int or2 = orientation(c, a, pt);

	return (((or0 == or1) && (or1 == or2)) ||
		((or0 == 0) && (or1 == or2)) ||
		((or1 == 0) && (or0 == or2)) ||
		((or2 == 0) && (or0 == or1)));
}

Boolean
pointInPolygon(Point *pt, Point *poly, int n)
{
	int tri;
	Point *a, *b, *c;

	a = poly;
	b = a;
	b++;
	c = b;
	c++;
	for (tri = 0; tri < n - 2; tri++) {
		if (pointInTriangle(pt, a, b, c))
			return True;
		b = c;
		c++;
	}
	return False;
}

#if 0
static Boolean
bresenhamLine(int startX, int startY, int endX, int endY,
		int *pixX, int *pixY, int *pixNo)
{
	int pix = 0;
	int ex = endX - startX;
	int ey = endY - startY;
	int dx, dy, error;

	(*pixNo)++;
	dx = (ex == 0) ? 0 : ((ex > 0) ? 1 : -1);
	if (ex < 0)
		ex = -ex;
	dy = (ey == 0) ? 0 : ((ey > 0) ? 1 : -1);
	if (ey < 0)
		ey = -ey;
	*pixX = startX;
	*pixY = startY;
	if (ex > ey) {
		error = 2 * ey - ex;
		while (pix != *pixNo) {
			if (error >= 0) {
				error -= 2 * ex;
				*pixY += dy;
			}
			error += 2 * ey;
			*pixX += dx;
			pix++;
			if (*pixX == endX)
				return True;
		}
		return False;
	} else {
		error = 2 * ex - ey;
		while (pix != *pixNo) {
			if (error >= 0) {
				error -= 2 * ey;
				*pixX += dx;
			}
			error += 2 * ex;
			*pixY += dy;
			pix++;
			if (*pixY == endY)
				return True;
		}
		return False;
	}
}
#endif
