From 1ff8ea6a19777f9f1026d21b9a3e03d94c2b47c4 Mon Sep 17 00:00:00 2001 From: Christoph Oelckers Date: Fri, 4 Oct 2019 01:41:57 +0200 Subject: [PATCH] - added matrix class for moving the GL matrix manipulation out of the engine code. --- source/CMakeLists.txt | 1 + source/build/src/common.cpp | 2 +- source/common/utility/matrix.cpp | 679 ++++++++++++ source/common/utility/matrix.h | 214 ++++ source/common/utility/vectors.h | 1677 ++++++++++++++++++++++++++++++ source/common/utility/xs_Float.h | 238 +++++ 6 files changed, 2810 insertions(+), 1 deletion(-) create mode 100644 source/common/utility/matrix.cpp create mode 100644 source/common/utility/matrix.h create mode 100644 source/common/utility/vectors.h create mode 100644 source/common/utility/xs_Float.h diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index a55c696f3..a1c3e0125 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -968,6 +968,7 @@ set (PCH_SOURCES common/utility/configfile.cpp common/utility/file_zip.cpp common/utility/resourcefile.cpp + common/utility/matrix.cpp ) if( MSVC ) diff --git a/source/build/src/common.cpp b/source/build/src/common.cpp index cbac3f54f..a9aa1d0fc 100644 --- a/source/build/src/common.cpp +++ b/source/build/src/common.cpp @@ -272,7 +272,7 @@ void COMMON_doclearbackground(int numcols, int height) } # endif - CLEARLINES2D(0, min(ydim, numrows*8+8), editorcolors[16]); + CLEARLINES2D(0, min(ydim, height), editorcolors[16]); } void COMMON_clearbackground(int numcols, int numrows) diff --git a/source/common/utility/matrix.cpp b/source/common/utility/matrix.cpp new file mode 100644 index 000000000..90b84fba2 --- /dev/null +++ b/source/common/utility/matrix.cpp @@ -0,0 +1,679 @@ +/* -------------------------------------------------- + +Lighthouse3D + +VSMatrix - Very Simple Matrix Library + +http://www.lighthouse3d.com/very-simple-libs + +This is a simplified version of VSMatrix that has been adjusted for GZDoom's needs. + +----------------------------------------------------*/ + +#include +#include +#include "matrix.h" + +#ifdef _MSC_VER +#pragma warning(disable : 4244) // truncate from double to float +#endif + +static inline FLOATTYPE +DegToRad(FLOATTYPE degrees) +{ + return (FLOATTYPE)(degrees * (pi::pif() / 180.0f)); +}; + +// sets the square matrix mat to the identity matrix, +// size refers to the number of rows (or columns) +void +VSMatrix::setIdentityMatrix( FLOATTYPE *mat, int size) { + + // fill matrix with 0s + for (int i = 0; i < size * size; ++i) + mat[i] = 0.0f; + + // fill diagonal with 1s + for (int i = 0; i < size; ++i) + mat[i + i * size] = 1.0f; +} + + + +// gl LoadIdentity implementation +void +VSMatrix::loadIdentity() +{ + // fill matrix with 0s + for (int i = 0; i < 16; ++i) + mMatrix[i] = 0.0f; + + // fill diagonal with 1s + for (int i = 0; i < 4; ++i) + mMatrix[i + i * 4] = 1.0f; +} + + +// gl MultMatrix implementation +void +VSMatrix::multMatrix(const FLOATTYPE *aMatrix) +{ + + FLOATTYPE res[16]; + + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + res[j*4 + i] = 0.0f; + for (int k = 0; k < 4; ++k) + { + res[j*4 + i] += mMatrix[k*4 + i] * aMatrix[j*4 + k]; + } + } + } + memcpy(mMatrix, res, 16 * sizeof(FLOATTYPE)); +} + +#ifdef USE_DOUBLE +// gl MultMatrix implementation +void +VSMatrix::multMatrix(const float *aMatrix) +{ + + FLOATTYPE res[16]; + + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + res[j * 4 + i] = 0.0f; + for (int k = 0; k < 4; ++k) + { + res[j*4 + i] += mMatrix[k*4 + i] * aMatrix[j*4 + k]; + } + } + } + memcpy(mMatrix, res, 16 * sizeof(FLOATTYPE)); +} +#endif + + + +// gl LoadMatrix implementation +void +VSMatrix::loadMatrix(const FLOATTYPE *aMatrix) +{ + memcpy(mMatrix, aMatrix, 16 * sizeof(FLOATTYPE)); +} + +#ifdef USE_DOUBLE +// gl LoadMatrix implementation +void +VSMatrix::loadMatrix(const float *aMatrix) +{ + for (int i = 0; i < 16; ++i) + { + mMatrix[i] = aMatrix[i]; + } +} +#endif + + +// gl Translate implementation +void +VSMatrix::translate(FLOATTYPE x, FLOATTYPE y, FLOATTYPE z) +{ + mMatrix[12] = mMatrix[0] * x + mMatrix[4] * y + mMatrix[8] * z + mMatrix[12]; + mMatrix[13] = mMatrix[1] * x + mMatrix[5] * y + mMatrix[9] * z + mMatrix[13]; + mMatrix[14] = mMatrix[2] * x + mMatrix[6] * y + mMatrix[10] * z + mMatrix[14]; +} + + +// gl Scale implementation +void +VSMatrix::scale(FLOATTYPE x, FLOATTYPE y, FLOATTYPE z) +{ + mMatrix[0] *= x; mMatrix[1] *= x; mMatrix[2] *= x; mMatrix[3] *= x; + mMatrix[4] *= y; mMatrix[5] *= y; mMatrix[6] *= y; mMatrix[7] *= y; + mMatrix[8] *= z; mMatrix[9] *= z; mMatrix[10] *= z; mMatrix[11] *= z; +} + + +// gl Rotate implementation +void +VSMatrix::rotate(FLOATTYPE angle, FLOATTYPE x, FLOATTYPE y, FLOATTYPE z) +{ + FLOATTYPE mat[16]; + FLOATTYPE v[3]; + + v[0] = x; + v[1] = y; + v[2] = z; + + FLOATTYPE radAngle = DegToRad(angle); + FLOATTYPE co = cos(radAngle); + FLOATTYPE si = sin(radAngle); + normalize(v); + FLOATTYPE x2 = v[0]*v[0]; + FLOATTYPE y2 = v[1]*v[1]; + FLOATTYPE z2 = v[2]*v[2]; + +// mat[0] = x2 + (y2 + z2) * co; + mat[0] = co + x2 * (1 - co);// + (y2 + z2) * co; + mat[4] = v[0] * v[1] * (1 - co) - v[2] * si; + mat[8] = v[0] * v[2] * (1 - co) + v[1] * si; + mat[12]= 0.0f; + + mat[1] = v[0] * v[1] * (1 - co) + v[2] * si; +// mat[5] = y2 + (x2 + z2) * co; + mat[5] = co + y2 * (1 - co); + mat[9] = v[1] * v[2] * (1 - co) - v[0] * si; + mat[13]= 0.0f; + + mat[2] = v[0] * v[2] * (1 - co) - v[1] * si; + mat[6] = v[1] * v[2] * (1 - co) + v[0] * si; +// mat[10]= z2 + (x2 + y2) * co; + mat[10]= co + z2 * (1 - co); + mat[14]= 0.0f; + + mat[3] = 0.0f; + mat[7] = 0.0f; + mat[11]= 0.0f; + mat[15]= 1.0f; + + multMatrix(mat); +} + + +// gluLookAt implementation +void +VSMatrix::lookAt(FLOATTYPE xPos, FLOATTYPE yPos, FLOATTYPE zPos, + FLOATTYPE xLook, FLOATTYPE yLook, FLOATTYPE zLook, + FLOATTYPE xUp, FLOATTYPE yUp, FLOATTYPE zUp) +{ + FLOATTYPE dir[3], right[3], up[3]; + + up[0] = xUp; up[1] = yUp; up[2] = zUp; + + dir[0] = (xLook - xPos); + dir[1] = (yLook - yPos); + dir[2] = (zLook - zPos); + normalize(dir); + + crossProduct(dir,up,right); + normalize(right); + + crossProduct(right,dir,up); + normalize(up); + + FLOATTYPE m1[16],m2[16]; + + m1[0] = right[0]; + m1[4] = right[1]; + m1[8] = right[2]; + m1[12] = 0.0f; + + m1[1] = up[0]; + m1[5] = up[1]; + m1[9] = up[2]; + m1[13] = 0.0f; + + m1[2] = -dir[0]; + m1[6] = -dir[1]; + m1[10] = -dir[2]; + m1[14] = 0.0f; + + m1[3] = 0.0f; + m1[7] = 0.0f; + m1[11] = 0.0f; + m1[15] = 1.0f; + + setIdentityMatrix(m2,4); + m2[12] = -xPos; + m2[13] = -yPos; + m2[14] = -zPos; + + multMatrix(m1); + multMatrix(m2); +} + + +// gluPerspective implementation +void +VSMatrix::perspective(FLOATTYPE fov, FLOATTYPE ratio, FLOATTYPE nearp, FLOATTYPE farp) +{ + FLOATTYPE f = 1.0f / tan (fov * (pi::pif() / 360.0f)); + + loadIdentity(); + mMatrix[0] = f / ratio; + mMatrix[1 * 4 + 1] = f; + mMatrix[2 * 4 + 2] = (farp + nearp) / (nearp - farp); + mMatrix[3 * 4 + 2] = (2.0f * farp * nearp) / (nearp - farp); + mMatrix[2 * 4 + 3] = -1.0f; + mMatrix[3 * 4 + 3] = 0.0f; +} + + +// gl Ortho implementation +void +VSMatrix::ortho(FLOATTYPE left, FLOATTYPE right, + FLOATTYPE bottom, FLOATTYPE top, + FLOATTYPE nearp, FLOATTYPE farp) +{ + loadIdentity(); + + mMatrix[0 * 4 + 0] = 2 / (right - left); + mMatrix[1 * 4 + 1] = 2 / (top - bottom); + mMatrix[2 * 4 + 2] = -2 / (farp - nearp); + mMatrix[3 * 4 + 0] = -(right + left) / (right - left); + mMatrix[3 * 4 + 1] = -(top + bottom) / (top - bottom); + mMatrix[3 * 4 + 2] = -(farp + nearp) / (farp - nearp); +} + + +// gl Frustum implementation +void +VSMatrix::frustum(FLOATTYPE left, FLOATTYPE right, + FLOATTYPE bottom, FLOATTYPE top, + FLOATTYPE nearp, FLOATTYPE farp) +{ + FLOATTYPE m[16]; + + setIdentityMatrix(m,4); + + m[0 * 4 + 0] = 2 * nearp / (right-left); + m[1 * 4 + 1] = 2 * nearp / (top - bottom); + m[2 * 4 + 0] = (right + left) / (right - left); + m[2 * 4 + 1] = (top + bottom) / (top - bottom); + m[2 * 4 + 2] = - (farp + nearp) / (farp - nearp); + m[2 * 4 + 3] = -1.0f; + m[3 * 4 + 2] = - 2 * farp * nearp / (farp-nearp); + m[3 * 4 + 3] = 0.0f; + + multMatrix(m); +} + + +/* +// returns a pointer to the requested matrix +FLOATTYPE * +VSMatrix::get(MatrixTypes aType) +{ + return mMatrix[aType]; +} +*/ + + +/* ----------------------------------------------------- + SEND MATRICES TO OPENGL +------------------------------------------------------*/ + +// ----------------------------------------------------- +// AUX functions +// ----------------------------------------------------- + + +// Compute res = M * point +void +VSMatrix::multMatrixPoint(const FLOATTYPE *point, FLOATTYPE *res) +{ + + for (int i = 0; i < 4; ++i) + { + + res[i] = 0.0f; + + for (int j = 0; j < 4; j++) { + + res[i] += point[j] * mMatrix[j*4 + i]; + } + } +} + +// res = a cross b; +void +VSMatrix::crossProduct(const FLOATTYPE *a, const FLOATTYPE *b, FLOATTYPE *res) { + + res[0] = a[1] * b[2] - b[1] * a[2]; + res[1] = a[2] * b[0] - b[2] * a[0]; + res[2] = a[0] * b[1] - b[0] * a[1]; +} + + +// returns a . b +FLOATTYPE +VSMatrix::dotProduct(const FLOATTYPE *a, const FLOATTYPE *b) { + + FLOATTYPE res = a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; + + return res; +} + + +// Normalize a vec3 +void +VSMatrix::normalize(FLOATTYPE *a) { + + FLOATTYPE mag = sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]); + + a[0] /= mag; + a[1] /= mag; + a[2] /= mag; +} + + +// res = b - a +void +VSMatrix::subtract(const FLOATTYPE *a, const FLOATTYPE *b, FLOATTYPE *res) { + + res[0] = b[0] - a[0]; + res[1] = b[1] - a[1]; + res[2] = b[2] - a[2]; +} + + +// res = a + b +void +VSMatrix::add(const FLOATTYPE *a, const FLOATTYPE *b, FLOATTYPE *res) { + + res[0] = b[0] + a[0]; + res[1] = b[1] + a[1]; + res[2] = b[2] + a[2]; +} + + +// returns |a| +FLOATTYPE +VSMatrix::length(const FLOATTYPE *a) { + + return(sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2])); + +} + + +static inline int +M3(int i, int j) +{ + return (i*3+j); +}; + + + +// computes the derived normal matrix for the view matrix +void +VSMatrix::computeNormalMatrix(const FLOATTYPE *aMatrix) +{ + + double mMat3x3[9]; + + mMat3x3[0] = aMatrix[0]; + mMat3x3[1] = aMatrix[1]; + mMat3x3[2] = aMatrix[2]; + + mMat3x3[3] = aMatrix[4]; + mMat3x3[4] = aMatrix[5]; + mMat3x3[5] = aMatrix[6]; + + mMat3x3[6] = aMatrix[8]; + mMat3x3[7] = aMatrix[9]; + mMat3x3[8] = aMatrix[10]; + + double det, invDet; + + det = mMat3x3[0] * (mMat3x3[4] * mMat3x3[8] - mMat3x3[5] * mMat3x3[7]) + + mMat3x3[1] * (mMat3x3[5] * mMat3x3[6] - mMat3x3[8] * mMat3x3[3]) + + mMat3x3[2] * (mMat3x3[3] * mMat3x3[7] - mMat3x3[4] * mMat3x3[6]); + + invDet = 1.0/det; + + mMatrix[0] = (mMat3x3[4] * mMat3x3[8] - mMat3x3[5] * mMat3x3[7]) * invDet; + mMatrix[1] = (mMat3x3[5] * mMat3x3[6] - mMat3x3[8] * mMat3x3[3]) * invDet; + mMatrix[2] = (mMat3x3[3] * mMat3x3[7] - mMat3x3[4] * mMat3x3[6]) * invDet; + mMatrix[3] = 0.0f; + mMatrix[4] = (mMat3x3[2] * mMat3x3[7] - mMat3x3[1] * mMat3x3[8]) * invDet; + mMatrix[5] = (mMat3x3[0] * mMat3x3[8] - mMat3x3[2] * mMat3x3[6]) * invDet; + mMatrix[6] = (mMat3x3[1] * mMat3x3[6] - mMat3x3[7] * mMat3x3[0]) * invDet; + mMatrix[7] = 0.0f; + mMatrix[8] = (mMat3x3[1] * mMat3x3[5] - mMat3x3[4] * mMat3x3[2]) * invDet; + mMatrix[9] = (mMat3x3[2] * mMat3x3[3] - mMat3x3[0] * mMat3x3[5]) * invDet; + mMatrix[10] =(mMat3x3[0] * mMat3x3[4] - mMat3x3[3] * mMat3x3[1]) * invDet; + mMatrix[11] = 0.0; + mMatrix[12] = 0.0; + mMatrix[13] = 0.0; + mMatrix[14] = 0.0; + mMatrix[15] = 1.0; + +} + + +// aux function resMat = resMat * aMatrix +void +VSMatrix::multMatrix(FLOATTYPE *resMat, const FLOATTYPE *aMatrix) +{ + + FLOATTYPE res[16]; + + for (int i = 0; i < 4; ++i) + { + for (int j = 0; j < 4; ++j) + { + res[j*4 + i] = 0.0f; + for (int k = 0; k < 4; ++k) + { + res[j*4 + i] += resMat[k*4 + i] * aMatrix[j*4 + k]; + } + } + } + memcpy(resMat, res, 16 * sizeof(FLOATTYPE)); +} + +static double mat3Determinant(const FLOATTYPE *mMat3x3) +{ + return mMat3x3[0] * (mMat3x3[4] * mMat3x3[8] - mMat3x3[5] * mMat3x3[7]) + + mMat3x3[1] * (mMat3x3[5] * mMat3x3[6] - mMat3x3[8] * mMat3x3[3]) + + mMat3x3[2] * (mMat3x3[3] * mMat3x3[7] - mMat3x3[4] * mMat3x3[6]); +} + +static double mat4Determinant(const FLOATTYPE *matrix) +{ + FLOATTYPE mMat3x3_a[9] = + { + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_b[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_c[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_d[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE a, b, c, d; + FLOATTYPE value; + + a = mat3Determinant(mMat3x3_a); + b = mat3Determinant(mMat3x3_b); + c = mat3Determinant(mMat3x3_c); + d = mat3Determinant(mMat3x3_d); + + value = matrix[0 * 4 + 0] * a; + value -= matrix[0 * 4 + 1] * b; + value += matrix[0 * 4 + 2] * c; + value -= matrix[0 * 4 + 3] * d; + + return value; +} + +static void mat4Adjoint(const FLOATTYPE *matrix, FLOATTYPE *result) +{ + FLOATTYPE mMat3x3_a[9] = + { + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_b[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_c[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_d[9] = + { + matrix[1 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[1 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[1 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE mMat3x3_e[9] = + { + matrix[0 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_f[9] = + { + matrix[0 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_g[9] = + { + matrix[0 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 3], matrix[2 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_h[9] = + { + matrix[0 * 4 + 0], matrix[2 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[2 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[2 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE mMat3x3_i[9] = + { + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_j[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[3 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_k[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[3 * 4 + 3] + }; + + FLOATTYPE mMat3x3_l[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[3 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[3 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[3 * 4 + 2] + }; + + FLOATTYPE mMat3x3_m[9] = + { + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[2 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[2 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[2 * 4 + 3] + }; + + FLOATTYPE mMat3x3_n[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[2 * 4 + 0], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[2 * 4 + 2], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[2 * 4 + 3] + }; + + FLOATTYPE mMat3x3_o[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[2 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[2 * 4 + 1], + matrix[0 * 4 + 3], matrix[1 * 4 + 3], matrix[2 * 4 + 3] + }; + + FLOATTYPE mMat3x3_p[9] = + { + matrix[0 * 4 + 0], matrix[1 * 4 + 0], matrix[2 * 4 + 0], + matrix[0 * 4 + 1], matrix[1 * 4 + 1], matrix[2 * 4 + 1], + matrix[0 * 4 + 2], matrix[1 * 4 + 2], matrix[2 * 4 + 2] + }; + + result[0 * 4 + 0] = mat3Determinant(mMat3x3_a); + result[1 * 4 + 0] = -mat3Determinant(mMat3x3_b); + result[2 * 4 + 0] = mat3Determinant(mMat3x3_c); + result[3 * 4 + 0] = -mat3Determinant(mMat3x3_d); + result[0 * 4 + 1] = -mat3Determinant(mMat3x3_e); + result[1 * 4 + 1] = mat3Determinant(mMat3x3_f); + result[2 * 4 + 1] = -mat3Determinant(mMat3x3_g); + result[3 * 4 + 1] = mat3Determinant(mMat3x3_h); + result[0 * 4 + 2] = mat3Determinant(mMat3x3_i); + result[1 * 4 + 2] = -mat3Determinant(mMat3x3_j); + result[2 * 4 + 2] = mat3Determinant(mMat3x3_k); + result[3 * 4 + 2] = -mat3Determinant(mMat3x3_l); + result[0 * 4 + 3] = -mat3Determinant(mMat3x3_m); + result[1 * 4 + 3] = mat3Determinant(mMat3x3_n); + result[2 * 4 + 3] = -mat3Determinant(mMat3x3_o); + result[3 * 4 + 3] = mat3Determinant(mMat3x3_p); +} + +bool VSMatrix::inverseMatrix(VSMatrix &result) +{ + // Calculate mat4 determinant + FLOATTYPE det = mat4Determinant(mMatrix); + + // Inverse unknown when determinant is close to zero + if (fabs(det) < 1e-15) + { + for (int i = 0; i < 16; i++) + result.mMatrix[i] = FLOATTYPE(0.0); + return false; + } + else + { + mat4Adjoint(mMatrix, result.mMatrix); + + FLOATTYPE invDet = FLOATTYPE(1.0) / det; + for (int i = 0; i < 16; i++) + { + result.mMatrix[i] = result.mMatrix[i] * invDet; + } + } + return true; +} diff --git a/source/common/utility/matrix.h b/source/common/utility/matrix.h new file mode 100644 index 000000000..1c46c2397 --- /dev/null +++ b/source/common/utility/matrix.h @@ -0,0 +1,214 @@ + +// Matrix class based on code from VSML: + +/** ---------------------------------------------------------- + * \class VSMathLib + * + * Lighthouse3D + * + * VSMathLib - Very Simple Matrix Library + * + * Full documentation at + * http://www.lighthouse3d.com/very-simple-libs + * + * This class aims at easing geometric transforms, camera + * placement and projection definition for programmers + * working with OpenGL core versions. + * + * + ---------------------------------------------------------------*/ +#ifndef __VSMatrix__ +#define __VSMatrix__ + +#include +#include "vectors.h" + +#ifdef USE_DOUBLE +typedef double FLOATTYPE; +#else +typedef float FLOATTYPE; +#endif + +class VSMatrix { + + public: + + VSMatrix() + { + } + + VSMatrix(int) + { + loadIdentity(); + } + + void translate(FLOATTYPE x, FLOATTYPE y, FLOATTYPE z); + void scale(FLOATTYPE x, FLOATTYPE y, FLOATTYPE z); + void rotate(FLOATTYPE angle, FLOATTYPE x, FLOATTYPE y, FLOATTYPE z); + void loadIdentity(); +#ifdef USE_DOUBLE + void multMatrix(const float *aMatrix); +#endif + void multVector(FLOATTYPE *aVector); + void multMatrix(const FLOATTYPE *aMatrix); + void multMatrix(const VSMatrix &aMatrix) + { + multMatrix(aMatrix.mMatrix); + } + void loadMatrix(const FLOATTYPE *aMatrix); +#ifdef USE_DOUBLE + void loadMatrix(const float *aMatrix); +#endif + void lookAt(FLOATTYPE xPos, FLOATTYPE yPos, FLOATTYPE zPos, FLOATTYPE xLook, FLOATTYPE yLook, FLOATTYPE zLook, FLOATTYPE xUp, FLOATTYPE yUp, FLOATTYPE zUp); + void perspective(FLOATTYPE fov, FLOATTYPE ratio, FLOATTYPE nearp, FLOATTYPE farp); + void ortho(FLOATTYPE left, FLOATTYPE right, FLOATTYPE bottom, FLOATTYPE top, FLOATTYPE nearp=-1.0f, FLOATTYPE farp=1.0f); + void frustum(FLOATTYPE left, FLOATTYPE right, FLOATTYPE bottom, FLOATTYPE top, FLOATTYPE nearp, FLOATTYPE farp); + void copy(FLOATTYPE * pDest) + { + memcpy(pDest, mMatrix, 16 * sizeof(FLOATTYPE)); + } + +#ifdef USE_DOUBLE + void copy(float * pDest) + { + for (int i = 0; i < 16; i++) + { + pDest[i] = (float)mMatrix[i]; + } + } +#endif + + const FLOATTYPE *get() const + { + return mMatrix; + } + + void multMatrixPoint(const FLOATTYPE *point, FLOATTYPE *res); + +#ifdef USE_DOUBLE + void computeNormalMatrix(const float *aMatrix); +#endif + void computeNormalMatrix(const FLOATTYPE *aMatrix); + void computeNormalMatrix(const VSMatrix &aMatrix) + { + computeNormalMatrix(aMatrix.mMatrix); + } + bool inverseMatrix(VSMatrix &result); + void transpose(); + + protected: + static void crossProduct(const FLOATTYPE *a, const FLOATTYPE *b, FLOATTYPE *res); + static FLOATTYPE dotProduct(const FLOATTYPE *a, const FLOATTYPE * b); + static void normalize(FLOATTYPE *a); + static void subtract(const FLOATTYPE *a, const FLOATTYPE *b, FLOATTYPE *res); + static void add(const FLOATTYPE *a, const FLOATTYPE *b, FLOATTYPE *res); + static FLOATTYPE length(const FLOATTYPE *a); + static void multMatrix(FLOATTYPE *resMatrix, const FLOATTYPE *aMatrix); + + static void setIdentityMatrix(FLOATTYPE *mat, int size = 4); + + /// The storage for matrices + FLOATTYPE mMatrix[16]; + +}; + + +class Matrix3x4 // used like a 4x4 matrix with the last row always being (0,0,0,1) +{ + float m[3][4]; + +public: + + void MakeIdentity() + { + memset(m, 0, sizeof(m)); + m[0][0] = m[1][1] = m[2][2] = 1.f; + } + + void Translate(float x, float y, float z) + { + m[0][3] = m[0][0]*x + m[0][1]*y + m[0][2]*z + m[0][3]; + m[1][3] = m[1][0]*x + m[1][1]*y + m[1][2]*z + m[1][3]; + m[2][3] = m[2][0]*x + m[2][1]*y + m[2][2]*z + m[2][3]; + } + + void Scale(float x, float y, float z) + { + m[0][0] *=x; + m[1][0] *=x; + m[2][0] *=x; + + m[0][1] *=y; + m[1][1] *=y; + m[2][1] *=y; + + m[0][2] *=z; + m[1][2] *=z; + m[2][2] *=z; + } + + void Rotate(float ax, float ay, float az, float angle) + { + Matrix3x4 m1; + + FVector3 axis(ax, ay, az); + axis.MakeUnit(); + double c = cos(angle * pi::pi()/180.), s = sin(angle * pi::pi()/180.), t = 1 - c; + double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z; + double tx, ty, txx, tyy, u, v; + + tx = t*axis.X; + m1.m[0][0] = float( (txx=tx*axis.X) + c ); + m1.m[0][1] = float( (u=tx*axis.Y) - sz); + m1.m[0][2] = float( (v=tx*axis.Z) + sy); + + ty = t*axis.Y; + m1.m[1][0] = float( u + sz); + m1.m[1][1] = float( (tyy=ty*axis.Y) + c ); + m1.m[1][2] = float( (u=ty*axis.Z) - sx); + + m1.m[2][0] = float( v - sy); + m1.m[2][1] = float( u + sx); + m1.m[2][2] = float( (t-txx-tyy) + c ); + + m1.m[0][3] = 0.f; + m1.m[1][3] = 0.f; + m1.m[2][3] = 0.f; + + *this = (*this) * m1; + } + + Matrix3x4 operator *(const Matrix3x4 &other) + { + Matrix3x4 result; + + result.m[0][0] = m[0][0]*other.m[0][0] + m[0][1]*other.m[1][0] + m[0][2]*other.m[2][0]; + result.m[0][1] = m[0][0]*other.m[0][1] + m[0][1]*other.m[1][1] + m[0][2]*other.m[2][1]; + result.m[0][2] = m[0][0]*other.m[0][2] + m[0][1]*other.m[1][2] + m[0][2]*other.m[2][2]; + result.m[0][3] = m[0][0]*other.m[0][3] + m[0][1]*other.m[1][3] + m[0][2]*other.m[2][3] + m[0][3]; + + result.m[1][0] = m[1][0]*other.m[0][0] + m[1][1]*other.m[1][0] + m[1][2]*other.m[2][0]; + result.m[1][1] = m[1][0]*other.m[0][1] + m[1][1]*other.m[1][1] + m[1][2]*other.m[2][1]; + result.m[1][2] = m[1][0]*other.m[0][2] + m[1][1]*other.m[1][2] + m[1][2]*other.m[2][2]; + result.m[1][3] = m[1][0]*other.m[0][3] + m[1][1]*other.m[1][3] + m[1][2]*other.m[2][3] + m[1][3]; + + result.m[2][0] = m[2][0]*other.m[0][0] + m[2][1]*other.m[1][0] + m[2][2]*other.m[2][0]; + result.m[2][1] = m[2][0]*other.m[0][1] + m[2][1]*other.m[1][1] + m[2][2]*other.m[2][1]; + result.m[2][2] = m[2][0]*other.m[0][2] + m[2][1]*other.m[1][2] + m[2][2]*other.m[2][2]; + result.m[2][3] = m[2][0]*other.m[0][3] + m[2][1]*other.m[1][3] + m[2][2]*other.m[2][3] + m[2][3]; + + return result; + } + + FVector3 operator *(const FVector3 &vec) + { + FVector3 result; + + result.X = vec.X*m[0][0] + vec.Y*m[0][1] + vec.Z*m[0][2] + m[0][3]; + result.Y = vec.X*m[1][0] + vec.Y*m[1][1] + vec.Z*m[1][2] + m[1][3]; + result.Z = vec.X*m[2][0] + vec.Y*m[2][1] + vec.Z*m[2][2] + m[2][3]; + return result; + } +}; + +#endif \ No newline at end of file diff --git a/source/common/utility/vectors.h b/source/common/utility/vectors.h new file mode 100644 index 000000000..f9f84421a --- /dev/null +++ b/source/common/utility/vectors.h @@ -0,0 +1,1677 @@ +/* +** vectors.h +** Vector math routines. +** +**--------------------------------------------------------------------------- +** Copyright 2005-2007 Randy Heit +** All rights reserved. +** +** Redistribution and use in source and binary forms, with or without +** modification, are permitted provided that the following conditions +** are met: +** +** 1. Redistributions of source code must retain the above copyright +** notice, this list of conditions and the following disclaimer. +** 2. Redistributions in binary form must reproduce the above copyright +** notice, this list of conditions and the following disclaimer in the +** documentation and/or other materials provided with the distribution. +** 3. The name of the author may not be used to endorse or promote products +** derived from this software without specific prior written permission. +** +** THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR +** IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES +** OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. +** IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, +** INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT +** NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +** THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +**--------------------------------------------------------------------------- +** +** Since C++ doesn't let me add completely new operators, the following two +** are overloaded for vectors: +** +** | dot product +** ^ cross product +*/ + +#ifndef VECTORS_H +#define VECTORS_H + +#include +#include +#include +#include "xs_Float.h" + + + +#define EQUAL_EPSILON (1/65536.) + +// make this a local inline function to avoid any dependencies on other headers and not pollute the global namespace +namespace pi +{ + inline constexpr double pi() { return 3.14159265358979323846; } + inline constexpr double pif() { return 3.14159265358979323846f; } +} + + + +template struct TVector3; +template struct TRotator; +template struct TAngle; + +template +struct TVector2 +{ + vec_t X, Y; + + TVector2() = default; + + TVector2 (vec_t a, vec_t b) + : X(a), Y(b) + { + } + + TVector2(const TVector2 &other) = default; + + TVector2 (const TVector3 &other) // Copy the X and Y from the 3D vector and discard the Z + : X(other.X), Y(other.Y) + { + } + + void Zero() + { + Y = X = 0; + } + + bool isZero() const + { + return X == 0 && Y == 0; + } + + TVector2 &operator= (const TVector2 &other) = default; + + // Access X and Y as an array + vec_t &operator[] (int index) + { + return index == 0 ? X : Y; + } + + const vec_t &operator[] (int index) const + { + return index == 0 ? X : Y; + } + + // Test for equality + bool operator== (const TVector2 &other) const + { + return X == other.X && Y == other.Y; + } + + // Test for inequality + bool operator!= (const TVector2 &other) const + { + return X != other.X || Y != other.Y; + } + + // Test for approximate equality + bool ApproximatelyEquals (const TVector2 &other) const + { + return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON; + } + + // Test for approximate inequality + bool DoesNotApproximatelyEqual (const TVector2 &other) const + { + return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON; + } + + // Unary negation + TVector2 operator- () const + { + return TVector2(-X, -Y); + } + + // Scalar addition + TVector2 &operator+= (double scalar) + { + X += scalar, Y += scalar; + return *this; + } + + friend TVector2 operator+ (const TVector2 &v, vec_t scalar) + { + return TVector2(v.X + scalar, v.Y + scalar); + } + + friend TVector2 operator+ (vec_t scalar, const TVector2 &v) + { + return TVector2(v.X + scalar, v.Y + scalar); + } + + // Scalar subtraction + TVector2 &operator-= (vec_t scalar) + { + X -= scalar, Y -= scalar; + return *this; + } + + TVector2 operator- (vec_t scalar) const + { + return TVector2(X - scalar, Y - scalar); + } + + // Scalar multiplication + TVector2 &operator*= (vec_t scalar) + { + X *= scalar, Y *= scalar; + return *this; + } + + friend TVector2 operator* (const TVector2 &v, vec_t scalar) + { + return TVector2(v.X * scalar, v.Y * scalar); + } + + friend TVector2 operator* (vec_t scalar, const TVector2 &v) + { + return TVector2(v.X * scalar, v.Y * scalar); + } + + // Scalar division + TVector2 &operator/= (vec_t scalar) + { + scalar = 1 / scalar, X *= scalar, Y *= scalar; + return *this; + } + + TVector2 operator/ (vec_t scalar) const + { + scalar = 1 / scalar; + return TVector2(X * scalar, Y * scalar); + } + + // Vector addition + TVector2 &operator+= (const TVector2 &other) + { + X += other.X, Y += other.Y; + return *this; + } + + TVector2 operator+ (const TVector2 &other) const + { + return TVector2(X + other.X, Y + other.Y); + } + + // Vector subtraction + TVector2 &operator-= (const TVector2 &other) + { + X -= other.X, Y -= other.Y; + return *this; + } + + TVector2 operator- (const TVector2 &other) const + { + return TVector2(X - other.X, Y - other.Y); + } + + // Vector length + vec_t Length() const + { + return (vec_t)sqrt (X*X + Y*Y); + } + + vec_t LengthSquared() const + { + return X*X + Y*Y; + } + + // Return a unit vector facing the same direction as this one + TVector2 Unit() const + { + vec_t len = Length(); + if (len != 0) len = 1 / len; + return *this * len; + } + + // Scales this vector into a unit vector. Returns the old length + vec_t MakeUnit() + { + vec_t len, ilen; + len = ilen = Length(); + if (ilen != 0) ilen = 1 / ilen; + *this *= ilen; + return len; + } + + // Resizes this vector to be the specified length (if it is not 0) + TVector2 &MakeResize(double len) + { + double scale = len / Length(); + X = vec_t(X * scale); + Y = vec_t(Y * scale); + return *this; + } + + + // Dot product + vec_t operator | (const TVector2 &other) const + { + return X*other.X + Y*other.Y; + } + + // Returns the angle that the ray (0,0)-(X,Y) faces + TAngle Angle() const; + + // Returns a rotated vector. angle is in degrees. + TVector2 Rotated (double angle) + { + return Rotated((TAngle)(angle)); + } + + // Returns a rotated vector. angle is in degrees. + template + TVector2 Rotated(TAngle angle) + { + double cosval = angle.Cos(); + double sinval = angle.Sin(); + return TVector2(X*cosval - Y*sinval, Y*cosval + X*sinval); + } + + // Returns a vector rotated 90 degrees clockwise. + TVector2 Rotated90CW() + { + return TVector2(Y, -X); + } + + // Returns a vector rotated 90 degrees counterclockwise. + TVector2 Rotated90CCW() + { + return TVector2(-Y, X); + } +}; + +template +struct TVector3 +{ + typedef TVector2 Vector2; + + vec_t X, Y, Z; + + TVector3() = default; + + TVector3 (vec_t a, vec_t b, vec_t c) + : X(a), Y(b), Z(c) + { + } + + TVector3(vec_t *o) + : X(o[0]), Y(o[1]), Z(o[2]) + { + } + + TVector3(const TVector3 &other) = default; + + TVector3 (const Vector2 &xy, vec_t z) + : X(xy.X), Y(xy.Y), Z(z) + { + } + + TVector3 (const TRotator &rot); + + void Zero() + { + Z = Y = X = 0; + } + + bool isZero() const + { + return X == 0 && Y == 0 && Z == 0; + } + + TVector3 &operator= (const TVector3 &other) = default; + + // Access X and Y and Z as an array + vec_t &operator[] (int index) + { + return index == 0 ? X : index == 1 ? Y : Z; + } + + const vec_t &operator[] (int index) const + { + return index == 0 ? X : index == 1 ? Y : Z; + } + + // Test for equality + bool operator== (const TVector3 &other) const + { + return X == other.X && Y == other.Y && Z == other.Z; + } + + // Test for inequality + bool operator!= (const TVector3 &other) const + { + return X != other.X || Y != other.Y || Z != other.Z; + } + + // Test for approximate equality + bool ApproximatelyEquals (const TVector3 &other) const + { + return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON && fabs(Z - other.Z) < EQUAL_EPSILON; + } + + // Test for approximate inequality + bool DoesNotApproximatelyEqual (const TVector3 &other) const + { + return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON || fabs(Z - other.Z) >= EQUAL_EPSILON; + } + + // Unary negation + TVector3 operator- () const + { + return TVector3(-X, -Y, -Z); + } + + // Scalar addition + TVector3 &operator+= (vec_t scalar) + { + X += scalar, Y += scalar, Z += scalar; + return *this; + } + + friend TVector3 operator+ (const TVector3 &v, vec_t scalar) + { + return TVector3(v.X + scalar, v.Y + scalar, v.Z + scalar); + } + + friend TVector3 operator+ (vec_t scalar, const TVector3 &v) + { + return TVector3(v.X + scalar, v.Y + scalar, v.Z + scalar); + } + + // Scalar subtraction + TVector3 &operator-= (vec_t scalar) + { + X -= scalar, Y -= scalar, Z -= scalar; + return *this; + } + + TVector3 operator- (vec_t scalar) const + { + return TVector3(X - scalar, Y - scalar, Z - scalar); + } + + // Scalar multiplication + TVector3 &operator*= (vec_t scalar) + { + X = vec_t(X *scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar); + return *this; + } + + friend TVector3 operator* (const TVector3 &v, vec_t scalar) + { + return TVector3(v.X * scalar, v.Y * scalar, v.Z * scalar); + } + + friend TVector3 operator* (vec_t scalar, const TVector3 &v) + { + return TVector3(v.X * scalar, v.Y * scalar, v.Z * scalar); + } + + // Scalar division + TVector3 &operator/= (vec_t scalar) + { + scalar = 1 / scalar, X = vec_t(X * scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar); + return *this; + } + + TVector3 operator/ (vec_t scalar) const + { + scalar = 1 / scalar; + return TVector3(X * scalar, Y * scalar, Z * scalar); + } + + // Vector addition + TVector3 &operator+= (const TVector3 &other) + { + X += other.X, Y += other.Y, Z += other.Z; + return *this; + } + + TVector3 operator+ (const TVector3 &other) const + { + return TVector3(X + other.X, Y + other.Y, Z + other.Z); + } + + // Vector subtraction + TVector3 &operator-= (const TVector3 &other) + { + X -= other.X, Y -= other.Y, Z -= other.Z; + return *this; + } + + TVector3 operator- (const TVector3 &other) const + { + return TVector3(X - other.X, Y - other.Y, Z - other.Z); + } + + // Add a 2D vector to this 3D vector, leaving Z unchanged. + TVector3 &operator+= (const Vector2 &other) + { + X += other.X, Y += other.Y; + return *this; + } + + // Subtract a 2D vector from this 3D vector, leaving Z unchanged. + TVector3 &operator-= (const Vector2 &other) + { + X -= other.X, Y -= other.Y; + return *this; + } + + // returns the XY fields as a 2D-vector. + Vector2 XY() const + { + return{ X, Y }; + } + + // Add a 3D vector and a 2D vector. + friend TVector3 operator+ (const TVector3 &v3, const Vector2 &v2) + { + return TVector3(v3.X + v2.X, v3.Y + v2.Y, v3.Z); + } + + friend TVector3 operator- (const TVector3 &v3, const Vector2 &v2) + { + return TVector3(v3.X - v2.X, v3.Y - v2.Y, v3.Z); + } + + friend Vector2 operator+ (const Vector2 &v2, const TVector3 &v3) + { + return Vector2(v2.X + v3.X, v2.Y + v3.Y); + } + + // Subtract a 3D vector and a 2D vector. + // Discards the Z component of the 3D vector and returns a 2D vector. + friend Vector2 operator- (const TVector2 &v2, const TVector3 &v3) + { + return Vector2(v2.X - v3.X, v2.Y - v3.Y); + } + + void GetRightUp(TVector3 &right, TVector3 &up) + { + TVector3 n(X, Y, Z); + TVector3 fn(fabs(n.X), fabs(n.Y), fabs(n.Z)); + int major = 0; + + if (fn[1] > fn[major]) major = 1; + if (fn[2] > fn[major]) major = 2; + + // build right vector by hand + if (fabs(fn[0] - 1.0f) < FLT_EPSILON || fabs(fn[1] - 1.0f) < FLT_EPSILON || fabs(fn[2] - 1.0f) < FLT_EPSILON) + { + if (major == 0 && n[0] > 0.f) + { + right = { 0.f, 0.f, -1.f }; + } + else if (major == 0) + { + right = { 0.f, 0.f, 1.f }; + } + else if (major == 1 || (major == 2 && n[2] > 0.f)) + { + right = { 1.f, 0.f, 0.f }; + } + // Unconditional to ease static analysis + else // major == 2 && n[2] <= 0.0f + { + right = { -1.f, 0.f, 0.f }; + } + } + else + { + static TVector3 axis[3] = + { + { 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f } + }; + + right = axis[major] ^ n; + } + + up = n ^right; + right.MakeUnit();; + up.MakeUnit(); + } + + + // Returns the angle (in radians) that the ray (0,0)-(X,Y) faces + TAngle Angle() const; + TAngle Pitch() const; + + // Vector length + double Length() const + { + return sqrt (X*X + Y*Y + Z*Z); + } + + double LengthSquared() const + { + return X*X + Y*Y + Z*Z; + } + + // Return a unit vector facing the same direction as this one + TVector3 Unit() const + { + double len = Length(); + if (len != 0) len = 1 / len; + return *this * (vec_t)len; + } + + // Scales this vector into a unit vector + void MakeUnit() + { + double len = Length(); + if (len != 0) len = 1 / len; + *this *= (vec_t)len; + } + + // Resizes this vector to be the specified length (if it is not 0) + TVector3 &MakeResize(double len) + { + double vlen = Length(); + if (vlen != 0.) + { + double scale = len / vlen; + X = vec_t(X * scale); + Y = vec_t(Y * scale); + Z = vec_t(Z * scale); + } + return *this; + } + + TVector3 Resized(double len) + { + double vlen = Length(); + if (vlen != 0.) + { + double scale = len / vlen; + return{ vec_t(X * scale), vec_t(Y * scale), vec_t(Z * scale) }; + } + else + { + return *this; + } + } + + // Dot product + vec_t operator | (const TVector3 &other) const + { + return X*other.X + Y*other.Y + Z*other.Z; + } + + // Cross product + TVector3 operator ^ (const TVector3 &other) const + { + return TVector3(Y*other.Z - Z*other.Y, + Z*other.X - X*other.Z, + X*other.Y - Y*other.X); + } + + TVector3 &operator ^= (const TVector3 &other) + { + *this = *this ^ other; + return *this; + } +}; + +template +struct TVector4 +{ + typedef TVector3 Vector3; + + vec_t X, Y, Z, W; + + TVector4() = default; + + TVector4(vec_t a, vec_t b, vec_t c, vec_t d) + : X(a), Y(b), Z(c), W(d) + { + } + + TVector4(vec_t *o) + : X(o[0]), Y(o[1]), Z(o[2]), W(o[3]) + { + } + + TVector4(const TVector4 &other) = default; + + TVector4(const Vector3 &xyz, vec_t w) + : X(xyz.X), Y(xyz.Y), Z(xyz.Z), W(w) + { + } + + void Zero() + { + Z = Y = X = W = 0; + } + + bool isZero() const + { + return X == 0 && Y == 0 && Z == 0 && W == 0; + } + + TVector4 &operator= (const TVector4 &other) = default; + + // Access X and Y and Z as an array + vec_t &operator[] (int index) + { + return (&X)[index]; + } + + const vec_t &operator[] (int index) const + { + return (&X)[index]; + } + + // Test for equality + bool operator== (const TVector4 &other) const + { + return X == other.X && Y == other.Y && Z == other.Z && W == other.W; + } + + // Test for inequality + bool operator!= (const TVector4 &other) const + { + return X != other.X || Y != other.Y || Z != other.Z || W != other.W; + } + + // Test for approximate equality + bool ApproximatelyEquals(const TVector4 &other) const + { + return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON && fabs(Z - other.Z) < EQUAL_EPSILON && fabs(W - other.W) < EQUAL_EPSILON; + } + + // Test for approximate inequality + bool DoesNotApproximatelyEqual(const TVector4 &other) const + { + return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON || fabs(Z - other.Z) >= EQUAL_EPSILON || fabs(W - other.W) >= EQUAL_EPSILON; + } + + // Unary negation + TVector4 operator- () const + { + return TVector4(-X, -Y, -Z, -W); + } + + // Scalar addition + TVector4 &operator+= (vec_t scalar) + { + X += scalar, Y += scalar, Z += scalar; W += scalar; + return *this; + } + + friend TVector4 operator+ (const TVector4 &v, vec_t scalar) + { + return TVector4(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar); + } + + friend TVector4 operator+ (vec_t scalar, const TVector4 &v) + { + return TVector4(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar); + } + + // Scalar subtraction + TVector4 &operator-= (vec_t scalar) + { + X -= scalar, Y -= scalar, Z -= scalar, W -= scalar; + return *this; + } + + TVector4 operator- (vec_t scalar) const + { + return TVector4(X - scalar, Y - scalar, Z - scalar, W - scalar); + } + + // Scalar multiplication + TVector4 &operator*= (vec_t scalar) + { + X = vec_t(X *scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar); + return *this; + } + + friend TVector4 operator* (const TVector4 &v, vec_t scalar) + { + return TVector4(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar); + } + + friend TVector4 operator* (vec_t scalar, const TVector4 &v) + { + return TVector4(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar); + } + + // Scalar division + TVector4 &operator/= (vec_t scalar) + { + scalar = 1 / scalar, X = vec_t(X * scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar); + return *this; + } + + TVector4 operator/ (vec_t scalar) const + { + scalar = 1 / scalar; + return TVector4(X * scalar, Y * scalar, Z * scalar, W * scalar); + } + + // Vector addition + TVector4 &operator+= (const TVector4 &other) + { + X += other.X, Y += other.Y, Z += other.Z, W += other.W; + return *this; + } + + TVector4 operator+ (const TVector4 &other) const + { + return TVector4(X + other.X, Y + other.Y, Z + other.Z, W + other.W); + } + + // Vector subtraction + TVector4 &operator-= (const TVector4 &other) + { + X -= other.X, Y -= other.Y, Z -= other.Z, W -= other.W; + return *this; + } + + TVector4 operator- (const TVector4 &other) const + { + return TVector4(X - other.X, Y - other.Y, Z - other.Z, W - other.W); + } + + // Add a 3D vector to this 4D vector, leaving W unchanged. + TVector4 &operator+= (const Vector3 &other) + { + X += other.X, Y += other.Y, Z += other.Z; + return *this; + } + + // Subtract a 3D vector from this 4D vector, leaving W unchanged. + TVector4 &operator-= (const Vector3 &other) + { + X -= other.X, Y -= other.Y, Z -= other.Z; + return *this; + } + + // returns the XYZ fields as a 3D-vector. + Vector3 XYZ() const + { + return{ X, Y, Z }; + } + + // Add a 4D vector and a 3D vector. + friend TVector4 operator+ (const TVector4 &v4, const Vector3 &v3) + { + return TVector4(v4.X + v3.X, v4.Y + v3.Y, v4.Z + v3.Z, v4.W); + } + + friend TVector4 operator- (const TVector4 &v4, const Vector3 &v3) + { + return TVector4(v4.X - v3.X, v4.Y - v3.Y, v4.Z - v3.Z, v4.W); + } + + friend Vector3 operator+ (const Vector3 &v3, const TVector4 &v4) + { + return Vector3(v3.X + v4.X, v3.Y + v4.Y, v3.Z + v4.Z); + } + + // Subtract a 4D vector and a 3D vector. + // Discards the W component of the 4D vector and returns a 3D vector. + friend Vector3 operator- (const TVector3 &v3, const TVector4 &v4) + { + return Vector3(v3.X - v4.X, v3.Y - v4.Y, v3.Z - v4.Z); + } + + // Vector length + double Length() const + { + return sqrt(X*X + Y*Y + Z*Z + W*W); + } + + double LengthSquared() const + { + return X*X + Y*Y + Z*Z + W*W; + } + + // Return a unit vector facing the same direction as this one + TVector4 Unit() const + { + double len = Length(); + if (len != 0) len = 1 / len; + return *this * (vec_t)len; + } + + // Scales this vector into a unit vector + void MakeUnit() + { + double len = Length(); + if (len != 0) len = 1 / len; + *this *= (vec_t)len; + } + + // Resizes this vector to be the specified length (if it is not 0) + TVector4 &MakeResize(double len) + { + double vlen = Length(); + if (vlen != 0.) + { + double scale = len / vlen; + X = vec_t(X * scale); + Y = vec_t(Y * scale); + Z = vec_t(Z * scale); + W = vec_t(W * scale); + } + return *this; + } + + TVector4 Resized(double len) + { + double vlen = Length(); + if (vlen != 0.) + { + double scale = len / vlen; + return{ vec_t(X * scale), vec_t(Y * scale), vec_t(Z * scale), vec_t(W * scale) }; + } + else + { + return *this; + } + } + + // Dot product + vec_t operator | (const TVector4 &other) const + { + return X*other.X + Y*other.Y + Z*other.Z + W*other.W; + } +}; + +template +struct TMatrix3x3 +{ + typedef TVector3 Vector3; + + vec_t Cells[3][3]; + + TMatrix3x3() = default; + TMatrix3x3(const TMatrix3x3 &other) = default; + + TMatrix3x3(const Vector3 &row1, const Vector3 &row2, const Vector3 &row3) + { + (*this)[0] = row1; + (*this)[1] = row2; + (*this)[2] = row3; + } + + // Construct a rotation matrix about an arbitrary axis. + // (The axis vector must be normalized.) + TMatrix3x3(const Vector3 &axis, double radians) + { + double c = cos(radians), s = sin(radians), t = 1 - c; +/* In comments: A more readable version of the matrix setup. +This was found in Diana Gruber's article "The Mathematics of the +3D Rotation Matrix" at and is +attributed to Graphics Gems (Glassner, Academic Press, 1990). + + Cells[0][0] = t*axis.X*axis.X + c; + Cells[0][1] = t*axis.X*axis.Y - s*axis.Z; + Cells[0][2] = t*axis.X*axis.Z + s*axis.Y; + + Cells[1][0] = t*axis.Y*axis.X + s*axis.Z; + Cells[1][1] = t*axis.Y*axis.Y + c; + Cells[1][2] = t*axis.Y*axis.Z - s*axis.X; + + Cells[2][0] = t*axis.Z*axis.X - s*axis.Y; + Cells[2][1] = t*axis.Z*axis.Y + s*axis.X; + Cells[2][2] = t*axis.Z*axis.Z + c; + +Outside comments: A faster version with only 10 (not 24) multiplies. +*/ + double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z; + double tx, ty, txx, tyy, u, v; + + tx = t*axis.X; + Cells[0][0] = vec_t( (txx=tx*axis.X) + c ); + Cells[0][1] = vec_t( (u=tx*axis.Y) - sz); + Cells[0][2] = vec_t( (v=tx*axis.Z) + sy); + + ty = t*axis.Y; + Cells[1][0] = vec_t( u + sz); + Cells[1][1] = vec_t( (tyy=ty*axis.Y) + c ); + Cells[1][2] = vec_t( (u=ty*axis.Z) - sx); + + Cells[2][0] = vec_t( v - sy); + Cells[2][1] = vec_t( u + sx); + Cells[2][2] = vec_t( (t-txx-tyy) + c ); + } + + TMatrix3x3(const Vector3 &axis, double c/*cosine*/, double s/*sine*/) + { + double t = 1 - c; + double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z; + double tx, ty, txx, tyy, u, v; + + tx = t*axis.X; + Cells[0][0] = vec_t( (txx=tx*axis.X) + c ); + Cells[0][1] = vec_t( (u=tx*axis.Y) - sz); + Cells[0][2] = vec_t( (v=tx*axis.Z) + sy); + + ty = t*axis.Y; + Cells[1][0] = vec_t( u + sz); + Cells[1][1] = vec_t( (tyy=ty*axis.Y) + c ); + Cells[1][2] = vec_t( (u=ty*axis.Z) - sx); + + Cells[2][0] = vec_t( v - sy); + Cells[2][1] = vec_t( u + sx); + Cells[2][2] = vec_t( (t-txx-tyy) + c ); + } + + TMatrix3x3(const Vector3 &axis, TAngle degrees); + + static TMatrix3x3 Rotate2D(double radians) + { + double c = cos(radians); + double s = sin(radians); + TMatrix3x3 ret; + ret.Cells[0][0] = c; ret.Cells[0][1] = -s; ret.Cells[0][2] = 0; + ret.Cells[1][0] = s; ret.Cells[1][1] = c; ret.Cells[1][2] = 0; + ret.Cells[2][0] = 0; ret.Cells[2][1] = 0; ret.Cells[2][2] = 1; + return ret; + } + + static TMatrix3x3 Scale2D(TVector2 scaleVec) + { + TMatrix3x3 ret; + ret.Cells[0][0] = scaleVec.X; ret.Cells[0][1] = 0; ret.Cells[0][2] = 0; + ret.Cells[1][0] = 0; ret.Cells[1][1] = scaleVec.Y; ret.Cells[1][2] = 0; + ret.Cells[2][0] = 0; ret.Cells[2][1] = 0; ret.Cells[2][2] = 1; + return ret; + } + + static TMatrix3x3 Translate2D(TVector2 translateVec) + { + TMatrix3x3 ret; + ret.Cells[0][0] = 1; ret.Cells[0][1] = 0; ret.Cells[0][2] = translateVec.X; + ret.Cells[1][0] = 0; ret.Cells[1][1] = 1; ret.Cells[1][2] = translateVec.Y; + ret.Cells[2][0] = 0; ret.Cells[2][1] = 0; ret.Cells[2][2] = 1; + return ret; + } + + void Zero() + { + memset (this, 0, sizeof *this); + } + + void Identity() + { + Cells[0][0] = 1; Cells[0][1] = 0; Cells[0][2] = 0; + Cells[1][0] = 0; Cells[1][1] = 1; Cells[1][2] = 0; + Cells[2][0] = 0; Cells[2][1] = 0; Cells[2][2] = 1; + } + + Vector3 &operator[] (int index) + { + return *((Vector3 *)&Cells[index]); + } + + const Vector3 &operator[] (int index) const + { + return *((Vector3 *)&Cells[index]); + } + + // Multiply a scalar + TMatrix3x3 &operator*= (double scalar) + { + (*this)[0] *= scalar; + (*this)[1] *= scalar; + (*this)[2] *= scalar; + return *this; + } + + friend TMatrix3x3 operator* (double s, const TMatrix3x3 &m) + { + return TMatrix3x3(m[0]*s, m[1]*s, m[2]*s); + } + + TMatrix3x3 operator* (double s) const + { + return TMatrix3x3((*this)[0]*s, (*this)[1]*s, (*this)[2]*s); + } + + // Divide a scalar + TMatrix3x3 &operator/= (double scalar) + { + return *this *= 1 / scalar; + } + + TMatrix3x3 operator/ (double s) const + { + return *this * (1 / s); + } + + // Add two 3x3 matrices together + TMatrix3x3 &operator+= (const TMatrix3x3 &o) + { + (*this)[0] += o[0]; + (*this)[1] += o[1]; + (*this)[2] += o[2]; + return *this; + } + + TMatrix3x3 operator+ (const TMatrix3x3 &o) const + { + return TMatrix3x3((*this)[0] + o[0], (*this)[1] + o[1], (*this)[2] + o[2]); + } + + // Subtract two 3x3 matrices + TMatrix3x3 &operator-= (const TMatrix3x3 &o) + { + (*this)[0] -= o[0]; + (*this)[1] -= o[1]; + (*this)[2] -= o[2]; + return *this; + } + + TMatrix3x3 operator- (const TMatrix3x3 &o) const + { + return TMatrix3x3((*this)[0] - o[0], (*this)[1] - o[1], (*this)[2] - o[2]); + } + + // Concatenate two 3x3 matrices + TMatrix3x3 &operator*= (const TMatrix3x3 &o) + { + return *this = *this * o; + } + + TMatrix3x3 operator* (const TMatrix3x3 &o) const + { + return TMatrix3x3( + Vector3(Cells[0][0]*o[0][0] + Cells[0][1]*o[1][0] + Cells[0][2]*o[2][0], + Cells[0][0]*o[0][1] + Cells[0][1]*o[1][1] + Cells[0][2]*o[2][1], + Cells[0][0]*o[0][2] + Cells[0][1]*o[1][2] + Cells[0][2]*o[2][2]), + Vector3(Cells[1][0]*o[0][0] + Cells[1][1]*o[1][0] + Cells[1][2]*o[2][0], + Cells[1][0]*o[0][1] + Cells[1][1]*o[1][1] + Cells[1][2]*o[2][1], + Cells[1][0]*o[0][2] + Cells[1][1]*o[1][2] + Cells[1][2]*o[2][2]), + Vector3(Cells[2][0]*o[0][0] + Cells[2][1]*o[1][0] + Cells[2][2]*o[2][0], + Cells[2][0]*o[0][1] + Cells[2][1]*o[1][1] + Cells[2][2]*o[2][1], + Cells[2][0]*o[0][2] + Cells[2][1]*o[1][2] + Cells[2][2]*o[2][2])); + } + + // Multiply a 3D vector by a rotation matrix + friend Vector3 operator* (const Vector3 &v, const TMatrix3x3 &m) + { + return Vector3(m[0] | v, m[1] | v, m[2] | v); + } + + friend Vector3 operator* (const TMatrix3x3 &m, const Vector3 &v) + { + return Vector3(m[0] | v, m[1] | v, m[2] | v); + } +}; + +#define BAM_FACTOR (90. / 0x40000000) + +template +struct TAngle +{ + vec_t Degrees; + + // This is to catch any accidental attempt to assign an angle_t to this type. Any explicit exception will require a type cast. + TAngle(int) = delete; + TAngle(unsigned int) = delete; + TAngle(long) = delete; + TAngle(unsigned long) = delete; + TAngle &operator= (int other) = delete; + TAngle &operator= (unsigned other) = delete; + TAngle &operator= (long other) = delete; + TAngle &operator= (unsigned long other) = delete; + + TAngle() = default; + + TAngle (vec_t amt) + : Degrees(amt) + { + } + + TAngle(const TAngle &other) = default; + TAngle &operator= (const TAngle &other) = default; + + TAngle &operator= (double other) + { + Degrees = (decltype(Degrees))other; + return *this; + } + + // intentionally disabled so that common math functions cannot be accidentally called with a TAngle. + //operator vec_t() const { return Degrees; } + + TAngle operator- () const + { + return TAngle(-Degrees); + } + + TAngle &operator+= (TAngle other) + { + Degrees += other.Degrees; + return *this; + } + + TAngle &operator-= (TAngle other) + { + Degrees -= other.Degrees; + return *this; + } + + TAngle &operator*= (TAngle other) + { + Degrees *= other.Degrees; + return *this; + } + + TAngle &operator/= (TAngle other) + { + Degrees /= other.Degrees; + return *this; + } + + TAngle operator+ (TAngle other) const + { + return Degrees + other.Degrees; + } + + TAngle operator- (TAngle other) const + { + return Degrees - other.Degrees; + } + + TAngle operator* (TAngle other) const + { + return Degrees * other.Degrees; + } + + TAngle operator/ (TAngle other) const + { + return Degrees / other.Degrees; + } + + TAngle &operator+= (vec_t other) + { + Degrees = Degrees + other; + return *this; + } + + TAngle &operator-= (vec_t other) + { + Degrees = Degrees - other; + return *this; + } + + TAngle &operator*= (vec_t other) + { + Degrees = Degrees * other; + return *this; + } + + TAngle &operator/= (vec_t other) + { + Degrees = Degrees / other; + return *this; + } + + TAngle operator+ (vec_t other) const + { + return Degrees + other; + } + + TAngle operator- (vec_t other) const + { + return Degrees - other; + } + + friend TAngle operator- (vec_t o1, TAngle o2) + { + return TAngle(o1 - o2.Degrees); + } + + TAngle operator* (vec_t other) const + { + return Degrees * other; + } + + TAngle operator/ (vec_t other) const + { + return Degrees / other; + } + + // Should the comparisons consider an epsilon value? + bool operator< (TAngle other) const + { + return Degrees < other.Degrees; + } + + bool operator> (TAngle other) const + { + return Degrees > other.Degrees; + } + + bool operator<= (TAngle other) const + { + return Degrees <= other.Degrees; + } + + bool operator>= (TAngle other) const + { + return Degrees >= other.Degrees; + } + + bool operator== (TAngle other) const + { + return Degrees == other.Degrees; + } + + bool operator!= (TAngle other) const + { + return Degrees != other.Degrees; + } + + bool operator< (vec_t other) const + { + return Degrees < other; + } + + bool operator> (vec_t other) const + { + return Degrees > other; + } + + bool operator<= (vec_t other) const + { + return Degrees <= other; + } + + bool operator>= (vec_t other) const + { + return Degrees >= other; + } + + bool operator== (vec_t other) const + { + return Degrees == other; + } + + bool operator!= (vec_t other) const + { + return Degrees != other; + } + + // Ensure the angle is between [0.0,360.0) degrees + TAngle Normalized360() const + { + // Normalizing the angle converts it to a BAM, which masks it, and converts it back to a float. + // Note: We MUST use xs_Float here because it is the only method that guarantees reliable wraparound. + return (vec_t)(BAM_FACTOR * BAMs()); + } + + // Ensures the angle is between (-180.0,180.0] degrees + TAngle Normalized180() const + { + return (vec_t)(BAM_FACTOR * (signed int)BAMs()); + } + + vec_t Radians() const + { + return vec_t(Degrees * (pi::pi() / 180.0)); + } + + unsigned BAMs() const + { + return xs_CRoundToInt(Degrees * (0x40000000 / 90.)); + } + + TVector2 ToVector(vec_t length = 1) const + { + return TVector2(length * Cos(), length * Sin()); + } + + vec_t Cos() const + { + return vec_t(cos(Radians())); + } + + vec_t Sin() const + { + return vec_t(sin(Radians())); + } + + double Tan() const + { + return vec_t(tan(Radians())); + } + + // This is for calculating vertical velocity. For high pitches the tangent will become too large to be useful. + double TanClamped(double max = 5.) const + { + return clamp(Tan(), -max, max); + } + + static inline TAngle ToDegrees(double rad) + { + return TAngle(double(rad * (180.0 / pi::pi()))); + } + +}; + +// Emulates the old floatbob offset table with direct calls to trig functions. +inline double BobSin(double fb) +{ + return TAngle(double(fb * (180.0 / 32))).Sin() * 8; +} + +template +inline TAngle fabs (const TAngle °) +{ + return TAngle(fabs(deg.Degrees)); +} + +template +inline TAngle deltaangle(const TAngle &a1, const TAngle &a2) +{ + return (a2 - a1).Normalized180(); +} + +template +inline TAngle deltaangle(const TAngle &a1, double a2) +{ + return (a2 - a1).Normalized180(); +} + +template +inline TAngle deltaangle(double a1, const TAngle &a2) +{ + return (a2 - a1).Normalized180(); +} + +template +inline TAngle absangle(const TAngle &a1, const TAngle &a2) +{ + return fabs((a1 - a2).Normalized180()); +} + +template +inline TAngle absangle(const TAngle &a1, double a2) +{ + return fabs((a1 - a2).Normalized180()); +} + +inline TAngle VecToAngle(double x, double y) +{ + return atan2(y, x) * (180.0 / pi::pi()); +} + +template +inline TAngle VecToAngle (const TVector2 &vec) +{ + return (T)atan2(vec.Y, vec.X) * (180.0 / pi::pi()); +} + +template +inline TAngle VecToAngle (const TVector3 &vec) +{ + return (T)atan2(vec.Y, vec.X) * (180.0 / pi::pi()); +} + +template +TAngle TVector2::Angle() const +{ + return VecToAngle(X, Y); +} + +template +TAngle TVector3::Angle() const +{ + return VecToAngle(X, Y); +} + +template +TAngle TVector3::Pitch() const +{ + return -VecToAngle(TVector2(X, Y).Length(), Z); +} + +// Much of this is copied from TVector3. Is all that functionality really appropriate? +template +struct TRotator +{ + typedef TAngle Angle; + + Angle Pitch; // up/down + Angle Yaw; // left/right + Angle Roll; // rotation about the forward axis. + Angle CamRoll; // Roll specific to actor cameras. Used by quakes. + + TRotator() = default; + + TRotator (const Angle &p, const Angle &y, const Angle &r) + : Pitch(p), Yaw(y), Roll(r) + { + } + + TRotator(const TRotator &other) = default; + TRotator &operator= (const TRotator &other) = default; + + // Access angles as an array + Angle &operator[] (int index) + { + return *(&Pitch + index); + } + + const Angle &operator[] (int index) const + { + return *(&Pitch + index); + } + + // Test for equality + bool operator== (const TRotator &other) const + { + return fabs(Pitch - other.Pitch) < Angle(EQUAL_EPSILON) && fabs(Yaw - other.Yaw) < Angle(EQUAL_EPSILON) && fabs(Roll - other.Roll) < Angle(EQUAL_EPSILON); + } + + // Test for inequality + bool operator!= (const TRotator &other) const + { + return fabs(Pitch - other.Pitch) >= Angle(EQUAL_EPSILON) && fabs(Yaw - other.Yaw) >= Angle(EQUAL_EPSILON) && fabs(Roll - other.Roll) >= Angle(EQUAL_EPSILON); + } + + // Unary negation + TRotator operator- () const + { + return TRotator(-Pitch, -Yaw, -Roll); + } + + // Scalar addition + TRotator &operator+= (const Angle &scalar) + { + Pitch += scalar, Yaw += scalar, Roll += scalar; + return *this; + } + + friend TRotator operator+ (const TRotator &v, const Angle &scalar) + { + return TRotator(v.Pitch + scalar, v.Yaw + scalar, v.Roll + scalar); + } + + friend TRotator operator+ (const Angle &scalar, const TRotator &v) + { + return TRotator(v.Pitch + scalar, v.Yaw + scalar, v.Roll + scalar); + } + + // Scalar subtraction + TRotator &operator-= (const Angle &scalar) + { + Pitch -= scalar, Yaw -= scalar, Roll -= scalar; + return *this; + } + + TRotator operator- (const Angle &scalar) const + { + return TRotator(Pitch - scalar, Yaw - scalar, Roll - scalar); + } + + // Scalar multiplication + TRotator &operator*= (const Angle &scalar) + { + Pitch *= scalar, Yaw *= scalar, Roll *= scalar; + return *this; + } + + friend TRotator operator* (const TRotator &v, const Angle &scalar) + { + return TRotator(v.Pitch * scalar, v.Yaw * scalar, v.Roll * scalar); + } + + friend TRotator operator* (const Angle &scalar, const TRotator &v) + { + return TRotator(v.Pitch * scalar, v.Yaw * scalar, v.Roll * scalar); + } + + // Scalar division + TRotator &operator/= (const Angle &scalar) + { + Angle mul(1 / scalar.Degrees); + Pitch *= scalar, Yaw *= scalar, Roll *= scalar; + return *this; + } + + TRotator operator/ (const Angle &scalar) const + { + Angle mul(1 / scalar.Degrees); + return TRotator(Pitch * mul, Yaw * mul, Roll * mul); + } + + // Vector addition + TRotator &operator+= (const TRotator &other) + { + Pitch += other.Pitch, Yaw += other.Yaw, Roll += other.Roll; + return *this; + } + + TRotator operator+ (const TRotator &other) const + { + return TRotator(Pitch + other.Pitch, Yaw + other.Yaw, Roll + other.Roll); + } + + // Vector subtraction + TRotator &operator-= (const TRotator &other) + { + Pitch -= other.Pitch, Yaw -= other.Yaw, Roll -= other.Roll; + return *this; + } + + TRotator operator- (const TRotator &other) const + { + return TRotator(Pitch - other.Pitch, Yaw - other.Yaw, Roll - other.Roll); + } +}; + +// Create a forward vector from a rotation (ignoring roll) + +template +inline TVector3::TVector3 (const TRotator &rot) +{ + double pcos = rot.Pitch.Cos(); + X = pcos * rot.Yaw.Cos(); + Y = pcos * rot.Yaw.Sin(); + Z = rot.Pitch.Sin(); +} + +template +inline TMatrix3x3::TMatrix3x3(const TVector3 &axis, TAngle degrees) +{ + double c = degrees.Cos(), s = degrees.Sin(), t = 1 - c; + double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z; + double tx, ty, txx, tyy, u, v; + + tx = t*axis.X; + Cells[0][0] = T( (txx=tx*axis.X) + c ); + Cells[0][1] = T( (u=tx*axis.Y) - sz ); + Cells[0][2] = T( (v=tx*axis.Z) + sy ); + + ty = t*axis.Y; + Cells[1][0] = T( u + sz ); + Cells[1][1] = T( (tyy=ty*axis.Y) + c ); + Cells[1][2] = T( (u=ty*axis.Z) - sx ); + + Cells[2][0] = T( v - sy ); + Cells[2][1] = T( u + sx ); + Cells[2][2] = T( (t-txx-tyy) + c ); +} + + +typedef TVector2 FVector2; +typedef TVector3 FVector3; +typedef TVector4 FVector4; +typedef TRotator FRotator; +typedef TMatrix3x3 FMatrix3x3; +typedef TAngle FAngle; + +typedef TVector2 DVector2; +typedef TVector3 DVector3; +typedef TVector4 DVector4; +typedef TRotator DRotator; +typedef TMatrix3x3 DMatrix3x3; +typedef TAngle DAngle; + + +class Plane +{ +public: + void Set(FVector3 normal, float d) + { + m_normal = normal; + m_d = d; + } + + // same for a play-vector. Note that y and z are inversed. + void Set(DVector3 normal, double d) + { + m_normal = { (float)normal.X, (float)normal.Z, (float)normal.Y }; + m_d = (float)d; + } + + float DistToPoint(float x, float y, float z) + { + FVector3 p(x, y, z); + + return (m_normal | p) + m_d; + } + + + bool PointOnSide(float x, float y, float z) + { + return DistToPoint(x, y, z) < 0.f; + } + + bool PointOnSide(FVector3 &v) { return PointOnSide(v.X, v.Y, v.Z); } + + float A() { return m_normal.X; } + float B() { return m_normal.Y; } + float C() { return m_normal.Z; } + float D() { return m_d; } + + const FVector3 &Normal() const { return m_normal; } +protected: + FVector3 m_normal; + float m_d; +}; + +#endif diff --git a/source/common/utility/xs_Float.h b/source/common/utility/xs_Float.h new file mode 100644 index 000000000..6c676ee3a --- /dev/null +++ b/source/common/utility/xs_Float.h @@ -0,0 +1,238 @@ +// ==================================================================================================================== +// ==================================================================================================================== +// xs_Float.h +// +// Source: "Know Your FPU: Fixing Floating Fast" +// http://www.stereopsis.com/sree/fpu2006.html +// +// xs_CRoundToInt: Round toward nearest, but ties round toward even (just like FISTP) +// xs_ToInt: Round toward zero, just like the C (int) cast +// xs_FloorToInt: Round down +// xs_CeilToInt: Round up +// xs_RoundToInt: Round toward nearest, but ties round up +// ==================================================================================================================== +// ==================================================================================================================== +#ifndef _xs_FLOAT_H_ +#define _xs_FLOAT_H_ + +#include + +// ==================================================================================================================== +// Defines +// ==================================================================================================================== +#ifndef _xs_DEFAULT_CONVERSION +#define _xs_DEFAULT_CONVERSION 0 +#endif //_xs_DEFAULT_CONVERSION + + +#if __BIG_ENDIAN__ + #define _xs_iexp_ 0 + #define _xs_iman_ 1 +#else + #define _xs_iexp_ 1 //intel is little endian + #define _xs_iman_ 0 +#endif //BigEndian_ + +#ifdef __GNUC__ +#define finline inline +#else +#define finline __forceinline +#endif + +typedef double real64; + + +union _xs_doubleints +{ + real64 val; + uint32_t ival[2]; +}; + +#if 0 +#define _xs_doublecopysgn(a,b) ((int32_t*)&a)[_xs_iexp_]&=~(((int32_t*)&b)[_xs_iexp_]&0x80000000) +#define _xs_doubleisnegative(a) ((((int32_t*)&a)[_xs_iexp_])|0x80000000) +#endif + +// ==================================================================================================================== +// Constants +// ==================================================================================================================== +const real64 _xs_doublemagic = real64 (6755399441055744.0); //2^52 * 1.5, uses limited precisicion to floor +const real64 _xs_doublemagicdelta = (1.5e-8); //almost .5f = .5f + 1e^(number of exp bit) +const real64 _xs_doublemagicroundeps = (.5f-_xs_doublemagicdelta); //almost .5f = .5f - 1e^(number of exp bit) + + +// ==================================================================================================================== +// Prototypes +// ==================================================================================================================== +static int32_t xs_CRoundToInt (real64 val, real64 dmr = _xs_doublemagic); +static int32_t xs_ToInt (real64 val, real64 dme = -_xs_doublemagicroundeps); +static int32_t xs_FloorToInt (real64 val, real64 dme = _xs_doublemagicroundeps); +static int32_t xs_CeilToInt (real64 val, real64 dme = _xs_doublemagicroundeps); +static int32_t xs_RoundToInt (real64 val); + +//int32_t versions +finline static int32_t xs_CRoundToInt (int32_t val) {return val;} +finline static int32_t xs_ToInt (int32_t val) {return val;} + + + +// ==================================================================================================================== +// Fix Class +// ==================================================================================================================== +template class xs_Fix +{ +public: + typedef int32_t Fix; + + // ==================================================================================================================== + // Basic Conversion from Numbers + // ==================================================================================================================== + finline static Fix ToFix (int32_t val) {return val<>N;} + + + +protected: + // ==================================================================================================================== + // Helper function - mainly to preserve _xs_DEFAULT_CONVERSION + // ==================================================================================================================== + finline static int32_t xs_ConvertToFixed (real64 val) + { + #if _xs_DEFAULT_CONVERSION==0 + return xs_CRoundToInt(val, _xs_doublemagic/(1<= 1400 + // VC++ 2005's standard cast is a little bit faster than this + // magic number code. (Which is pretty amazing!) SSE has the + // fastest C-style float->int conversion, but unfortunately, + // checking for SSE support every time you need to do a + // conversion completely negates its performance advantage. + return int32_t(val); +#else +#if _xs_DEFAULT_CONVERSION==0 + return (val<0) ? xs_CRoundToInt(val-dme) : + xs_CRoundToInt(val+dme); +#else + return int32_t(val); +#endif +#endif +} + + +// ==================================================================================================================== +finline static int32_t xs_FloorToInt(real64 val, real64 dme) +{ +#if _xs_DEFAULT_CONVERSION==0 + return xs_CRoundToInt (val - dme); +#else + return floor(val); +#endif +} + + +// ==================================================================================================================== +finline static int32_t xs_CeilToInt(real64 val, real64 dme) +{ +#if _xs_DEFAULT_CONVERSION==0 + return xs_CRoundToInt (val + dme); +#else + return ceil(val); +#endif +} + + +// ==================================================================================================================== +finline static int32_t xs_RoundToInt(real64 val) +{ +#if _xs_DEFAULT_CONVERSION==0 + // Yes, it is important that two fadds be generated, so you cannot override the dmr + // passed to xs_CRoundToInt with _xs_doublemagic + _xs_doublemagicdelta. If you do, + // you'll end up with Banker's Rounding again. + return xs_CRoundToInt (val + _xs_doublemagicdelta); +#else + return floor(val+.5); +#endif +} + + + +// ==================================================================================================================== +// ==================================================================================================================== +// Unsigned variants +// ==================================================================================================================== +// ==================================================================================================================== +finline static uint32_t xs_CRoundToUInt(real64 val) +{ + return (uint32_t)xs_CRoundToInt(val); +} + +finline static uint32_t xs_FloorToUInt(real64 val) +{ + return (uint32_t)xs_FloorToInt(val); +} + +finline static uint32_t xs_CeilToUInt(real64 val) +{ + return (uint32_t)xs_CeilToInt(val); +} + +finline static uint32_t xs_RoundToUInt(real64 val) +{ + return (uint32_t)xs_RoundToInt(val); +} + + + +// ==================================================================================================================== +// ==================================================================================================================== +#endif // _xs_FLOAT_H_ \ No newline at end of file