OpenRaider/src/MatMath.cpp

/* -*- Mode: C++; tab-width: 3; indent-tabs-mode: t; c-basic-offset: 3 -*- */

#include <stdlib.h>
#include <math.h>

#include <MatMath.h>
#include <Vector3d.h>
#include <Matrix.h>

#define COMPUTE

void helVectorMatrixMult4dv(double v[4], matrix_t m, double result[4])
{
	/* Column major */
	result[0] = m[ 0] * v[0] + m[ 4] * v[1] + m[ 8] * v[2] + m[12] * v[3];
	result[1] = m[ 1] * v[0] + m[ 5] * v[1] + m[ 9] * v[2] + m[13] * v[3];
	result[2] = m[ 2] * v[0] + m[ 6] * v[1] + m[10] * v[2] + m[14] * v[3];
	result[3] = m[ 3] * v[0] + m[ 7] * v[1] + m[11] * v[2] + m[15] * v[3];

	/* Row major */
   //result[0] = m[ 0] * v[0] + m[ 1] * v[1] + m[ 2] * v[2] + m[ 3] * v[3];
	//result[1] = m[ 4] * v[0] + m[ 5] * v[1] + m[ 6] * v[2] + m[ 7] * v[3];
	//result[2] = m[ 8] * v[0] + m[ 9] * v[1] + m[10] * v[2] + m[11] * v[3];
	//result[3] = m[12] * v[0] + m[13] * v[1] + m[14] * v[2] + m[15] * v[3];
}


bool tmpHelSphereIntersectLine(Vector3d pos, Vector3d lastPos,
										 Vector3d center, vec_t radius)
{
	Vector3d seg, segToCenter, delta;
	vec_t s, dSquare;


	seg = pos - lastPos;
	segToCenter = center - lastPos;

	s = seg * segToCenter;

	if (s >= 1.0f || s <= 0.0f)
		return false;

	seg.normalize();
	seg = seg * s;
	seg = seg + lastPos;

	delta = seg - center;

	dSquare = delta * delta;

	if (radius >= dSquare)
		return true;
	else
		return false;
}


vec_t helIntersectionOfAbstractSpheres(vec3_t centerA, vec_t radiusA,
													vec3_t centerB, vec_t radiusB)
{
	Vector3d a = Vector3d(centerA);
	Vector3d b = Vector3d(centerB);
	Vector3d d = a - b;
	vec_t dist, minDist;

	dist = Vector3d::dot(d, d);
	minDist = radiusA + radiusB;

	return (dist <= minDist * minDist);
}


inline vec_t square(vec_t a)
{
	return a * a;
}


// Returns number of intersections and intersection position(s)
// Got algorithm from http://astronomy.swin.edu.au/~pbourke/geometry/
int helIntersectionOfAbstractSphereAndLine(vec3_t center, vec_t radius,
														 vec3_t posA, vec3_t posB,
														 vec3_t intersectionA,
														 vec3_t intersectionB)
{
	// float x , y , z;
	vec_t a, b, c, mu, i ;


	a = (square(posB[0] - posA[0]) +
		  square(posB[1] - posA[1]) +
		  square(posB[2] - posA[2]));
	b = (2 * ((posB[0] - posA[0]) * (posA[0] - center[0]) +
				 (posB[1] - posA[1]) * (posA[1] - center[1]) +
				 (posB[2] - posA[2]) * (posA[2] - center[2])));
	c = (square(center[0]) + square(center[1]) +
		  square(center[2]) + square(posA[0]) +
		  square(posA[1]) + square(posA[2]) -
		  2 * (center[0]*posA[0] + center[1]*posA[1] + center[2]*posA[2]) -
		  square(radius));

	i = b * b - 4 * a * c;


	if (i < 0.0)
	{
		// No intersection
		return 0;
	}
	else if (i == 0.0)
	{
		// One intersection
		mu = -b/(2*a) ;
		intersectionA[1] = posA[0] + mu*(posB[0]-posA[0]);
		intersectionA[2] = posA[1] + mu*(posB[1]-posA[1]);
		intersectionA[3] = posA[2] + mu*(posB[2]-posA[2]);

		return 1;
	}
	else
	{
		// Two intersections

		// First intersection
		mu = (-b + sqrt( square(b) - 4*a*c)) / (2*a);
		intersectionA[1] = posA[0] + mu*(posB[0]-posA[0]);
		intersectionA[2] = posA[1] + mu*(posB[1]-posA[1]);
		intersectionA[3] = posA[2] + mu*(posB[2]-posA[2]);

		// Second intersection
		mu = (-b - sqrt(square(b) - 4*a*c)) / (2*a);
		intersectionB[0] = posA[0] + mu*(posB[0]-posA[0]);
		intersectionB[1] = posA[1] + mu*(posB[1]-posA[1]);
		intersectionB[2] = posA[2] + mu*(posB[2]-posA[2]);

		return 2;
	}
}


int helIntersectionLineAndPolygon(vec3_t intersect,
											 vec3_t p1, vec3_t p2,
											 unsigned int vertexCount, vec3_t *ploygon)
{
	//	vec3_t normal, a, b;
	Vector3d a, b, normal, pA, pB;
	vec_t d, denominator, mu;
	double theta;


	pA = Vector3d(p1);
	pB = Vector3d(p2);

	// Find normal
	//mtkVectorSubtract(ploygon[1], ploygon[0], a);
	a = Vector3d(ploygon[1]) - Vector3d(ploygon[0]);
	//mtkVectorSubtract(ploygon[2], ploygon[0], b);
	b = Vector3d(ploygon[2]) - Vector3d(ploygon[0]);
	normal = Vector3d::cross(a, b);
	//mtkVectorCrossProduct(a, b, normal);
	normal.normalize();
	//mtkVectorNormalize(normal, normal);

	// find D
	//d = (normal[0] * ploygon[0][0] -
	//	  normal[1] * ploygon[0][1] -
	//	  normal[2] * ploygon[0][2]);
	d = (normal.mVec[0] * ploygon[0][0] -
		  normal.mVec[1] * ploygon[0][1] -
		  normal.mVec[2] * ploygon[0][2]);

	// line segment parallel to plane?
	//mtkVectorSubtract(p2, p1, a); // cache p2 - p1 => a
	a = pB - pA;

	//denominator = (normal[0] * a[0] +
	//					normal[1] * a[1] +
	//					normal[2] * a[2]);
	denominator = Vector3d::dot(normal, a);

	if (denominator > 0.0)
		return 0;

	// Line segment contains intercept point?
	//mu = - ((d + normal[0] * p1[0] + normal[1] * p1[1] + normal[2] * p1[2]) /
	//		  denominator);
	mu = -((d + Vector3d::dot(normal, pA)) / denominator);

	if (mu < 0.0 || mu > 1.0)
		return 0;

	//intersect[0] = p1[0] + mu * a[0];
	//intersect[1] = p1[1] + mu * a[1];
	//intersect[2] = p1[2] + mu * a[2];
	b = pA + (a * mu);
	intersect[0] = b.mVec[0];
	intersect[1] = b.mVec[1];
	intersect[2] = b.mVec[2];


	// See if the intercept is bound by polygon by winding number
#ifdef WINDING_NUMBERS_TRIANGLE
	mtkVectorSubtract(ploygon[0], intersect, a);
	mtkVectorNormalize(a, a);
	mtkVectorSubtract(ploygon[1], intersect, b);
	mtkVectorNormalize(b, b);
	mtkVectorSubtract(ploygon[2], intersect, c);
	mtkVectorNormalize(c, c);

	t0 = mtkVectorDotProduct(a, b);
	t1 = mtkVectorDotProduct(b, c);
	t2 = mtkVectorDotProduct(c, a);

	total = HEL_RAD_TO_DEG(acos(t0) + acos(t1) + acos(t2));

	if (total - 360 < 0.0)
		return 0;
#else // assume convex polygons here for sure
	//mtkVectorSubtract(intersect, ploygon[0], a);
	//theta = mtkVectorDotProduct(a, normal);
	theta = Vector3d::dot(b - Vector3d(ploygon[0]), normal); // b = intersect

	if (theta >= 90.0) // Yeah I know
		return 0;
#endif

	return 1;
}


vec_t helDistToSphereFromPlane3v(vec3_t center,	vec_t radius, vec4_t plane)
{
	vec_t d;


	d = (plane[0] * center[0] +
		  plane[1] * center[1] +
		  plane[2] * center[2] +
		  plane[3]);

	if (d <= -radius)
		return 0;

	return d + radius;
}


vec_t helDistToBboxFromPlane3v(vec3_t min, vec3_t max, vec4_t plane)
{
	vec3_t center;
	vec_t d, radius;


	helMidpoint3v(min, max, center);

	d = (plane[0] * center[0] +
		  plane[1] * center[1] +
		  plane[2] * center[2] +
		  plane[3]);

	radius = helDist3v(max, center);

	if (d <= -radius)
		return 0;

	return d + radius;
}


vec_t helDist3v(vec3_t a, vec3_t b)
{
	return (sqrt( ((b[0] - a[0]) * (b[0] - a[0])) +
					  ((b[1] - a[1]) * (b[1] - a[1])) +
					  ((b[2] - a[2]) * (b[2] - a[2]))));
}


void helMidpoint3v(vec3_t a, vec3_t b, vec3_t mid)
{
	mid[0] = (a[0] + b[0]) / 2;
	mid[1] = (a[1] + b[1]) / 2;
	mid[2] = (a[2] + b[2]) / 2;
}


vec_t helNorm4v(vec4_t v)
{
	return (sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]));
}


vec_t helNorm3v(vec3_t v)
{
	return (sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
}


vec_t helNorm2v(vec2_t v)
{
	return (sqrt(v[0]*v[0] + v[1]*v[1]));
}


vec_t helRandomNum(vec_t from, vec_t to)
{
	return from + (to*rand()/(RAND_MAX+1.0));
}


vec_t helDegToRad(vec_t degrees)
{
#ifdef COMPUTE
	return ((degrees / 180.0) * HEL_PI);
#else
	// degrees * (180.0 / PI);
	return (degrees * HEL_180_OVER_PI);
#endif
}


vec_t helRadToDeg(vec_t rad)
{
#ifdef COMPUTE
	return ((rad / HEL_PI) * 180.0);
#else
	// rad * (PI / 180.0);
	return (rad * HEL_PI_OVER_180);
#endif
}