summaryrefslogtreecommitdiffstats
path: root/lib/math3d.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'lib/math3d.cpp')
-rw-r--r--lib/math3d.cpp1062
1 files changed, 1062 insertions, 0 deletions
diff --git a/lib/math3d.cpp b/lib/math3d.cpp
new file mode 100644
index 0000000..545c093
--- /dev/null
+++ b/lib/math3d.cpp
@@ -0,0 +1,1062 @@
+// Math3d.c
+// Implementation of non-inlined functions in the Math3D Library
+// Richard S. Wright Jr.
+
+#include "math3d.h"
+
+////////////////////////////////////////////////////////////
+// LoadIdentity
+// For 3x3 and 4x4 float and double matricies.
+// 3x3 float
+void m3dLoadIdentity33(M3DMatrix33f m)
+{
+ // Don't be fooled, this is still column major
+ static M3DMatrix33f identity = { 1.0f, 0.0f, 0.0f ,
+ 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f };
+
+ memcpy(m, identity, sizeof(M3DMatrix33f));
+}
+
+// 3x3 double
+void m3dLoadIdentity33(M3DMatrix33d m)
+{
+ // Don't be fooled, this is still column major
+ static M3DMatrix33d identity = { 1.0, 0.0, 0.0 ,
+ 0.0, 1.0, 0.0,
+ 0.0, 0.0, 1.0 };
+
+ memcpy(m, identity, sizeof(M3DMatrix33d));
+}
+
+// 4x4 float
+void m3dLoadIdentity44(M3DMatrix44f m)
+{
+ // Don't be fooled, this is still column major
+ static M3DMatrix44f identity = { 1.0f, 0.0f, 0.0f, 0.0f,
+ 0.0f, 1.0f, 0.0f, 0.0f,
+ 0.0f, 0.0f, 1.0f, 0.0f,
+ 0.0f, 0.0f, 0.0f, 1.0f };
+
+ memcpy(m, identity, sizeof(M3DMatrix44f));
+}
+
+// 4x4 double
+void m3dLoadIdentity44(M3DMatrix44d m)
+{
+ static M3DMatrix44d identity = { 1.0, 0.0, 0.0, 0.0,
+ 0.0, 1.0, 0.0, 0.0,
+ 0.0, 0.0, 1.0, 0.0,
+ 0.0, 0.0, 0.0, 1.0 };
+
+ memcpy(m, identity, sizeof(M3DMatrix44d));
+}
+
+////////////////////////////////////////////////////////////////////////
+// Return the square of the distance between two points
+// Should these be inlined...?
+float m3dGetDistanceSquared(const M3DVector3f u, const M3DVector3f v)
+{
+ float x = u[0] - v[0];
+ x = x*x;
+
+ float y = u[1] - v[1];
+ y = y*y;
+
+ float z = u[2] - v[2];
+ z = z*z;
+
+ return (x + y + z);
+}
+
+// Ditto above, but for doubles
+double m3dGetDistanceSquared(const M3DVector3d u, const M3DVector3d v)
+{
+ double x = u[0] - v[0];
+ x = x*x;
+
+ double y = u[1] - v[1];
+ y = y*y;
+
+ double z = u[2] - v[2];
+ z = z*z;
+
+ return (x + y + z);
+}
+
+#define A(row,col) a[(col<<2)+row]
+#define B(row,col) b[(col<<2)+row]
+#define P(row,col) product[(col<<2)+row]
+
+///////////////////////////////////////////////////////////////////////////////
+// Multiply two 4x4 matricies
+void m3dMatrixMultiply44(M3DMatrix44f product, const M3DMatrix44f a, const M3DMatrix44f b)
+{
+ for (int i = 0; i < 4; i++) {
+ float ai0=A(i,0), ai1=A(i,1), ai2=A(i,2), ai3=A(i,3);
+ P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
+ P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
+ P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
+ P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
+}
+}
+
+// Ditto above, but for doubles
+void m3dMatrixMultiply(M3DMatrix44d product, const M3DMatrix44d a, const M3DMatrix44d b)
+{
+ for (int i = 0; i < 4; i++) {
+ double ai0=A(i,0), ai1=A(i,1), ai2=A(i,2), ai3=A(i,3);
+ P(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
+ P(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
+ P(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
+ P(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
+ }
+}
+#undef A
+#undef B
+#undef P
+
+#define A33(row,col) a[(col*3)+row]
+#define B33(row,col) b[(col*3)+row]
+#define P33(row,col) product[(col*3)+row]
+
+///////////////////////////////////////////////////////////////////////////////
+// Multiply two 3x3 matricies
+void m3dMatrixMultiply33(M3DMatrix33f product, const M3DMatrix33f a, const M3DMatrix33f b)
+{
+ for (int i = 0; i < 3; i++) {
+ float ai0=A33(i,0), ai1=A33(i,1), ai2=A33(i,2);
+ P33(i,0) = ai0 * B33(0,0) + ai1 * B33(1,0) + ai2 * B33(2,0);
+ P33(i,1) = ai0 * B33(0,1) + ai1 * B33(1,1) + ai2 * B33(2,1);
+ P33(i,2) = ai0 * B33(0,2) + ai1 * B33(1,2) + ai2 * B33(2,2);
+ }
+}
+
+// Ditto above, but for doubles
+void m3dMatrixMultiply44(M3DMatrix33d product, const M3DMatrix33d a, const M3DMatrix33d b)
+{
+ for (int i = 0; i < 3; i++) {
+ double ai0=A33(i,0), ai1=A33(i,1), ai2=A33(i,2);
+ P33(i,0) = ai0 * B33(0,0) + ai1 * B33(1,0) + ai2 * B33(2,0);
+ P33(i,1) = ai0 * B33(0,1) + ai1 * B33(1,1) + ai2 * B33(2,1);
+ P33(i,2) = ai0 * B33(0,2) + ai1 * B33(1,2) + ai2 * B33(2,2);
+ }
+}
+
+#undef A33
+#undef B33
+#undef P33
+
+#define M33(row,col) m[col*3+row]
+
+///////////////////////////////////////////////////////////////////////////////
+// Creates a 3x3 rotation matrix, takes radians NOT degrees
+void m3dRotationMatrix33(M3DMatrix33f m, float angle, float x, float y, float z)
+{
+
+ float mag, s, c;
+ float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
+
+ s = float(sin(angle));
+ c = float(cos(angle));
+
+ mag = float(sqrt( x*x + y*y + z*z ));
+
+ // Identity matrix
+ if (mag == 0.0f) {
+ m3dLoadIdentity33(m);
+ return;
+}
+
+ // Rotation matrix is normalized
+ x /= mag;
+ y /= mag;
+ z /= mag;
+
+ xx = x * x;
+ yy = y * y;
+ zz = z * z;
+ xy = x * y;
+ yz = y * z;
+ zx = z * x;
+ xs = x * s;
+ ys = y * s;
+ zs = z * s;
+ one_c = 1.0f - c;
+
+ M33(0,0) = (one_c * xx) + c;
+ M33(0,1) = (one_c * xy) - zs;
+ M33(0,2) = (one_c * zx) + ys;
+
+ M33(1,0) = (one_c * xy) + zs;
+ M33(1,1) = (one_c * yy) + c;
+ M33(1,2) = (one_c * yz) - xs;
+
+ M33(2,0) = (one_c * zx) - ys;
+ M33(2,1) = (one_c * yz) + xs;
+ M33(2,2) = (one_c * zz) + c;
+}
+
+#undef M33
+
+///////////////////////////////////////////////////////////////////////////////
+// Creates a 4x4 rotation matrix, takes radians NOT degrees
+void m3dRotationMatrix44(M3DMatrix44f m, float angle, float x, float y, float z)
+{
+ float mag, s, c;
+ float xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
+
+ s = float(sin(angle));
+ c = float(cos(angle));
+
+ mag = float(sqrt( x*x + y*y + z*z ));
+
+ // Identity matrix
+ if (mag == 0.0f) {
+ m3dLoadIdentity44(m);
+ return;
+}
+
+ // Rotation matrix is normalized
+ x /= mag;
+ y /= mag;
+ z /= mag;
+
+ #define M(row,col) m[col*4+row]
+
+ xx = x * x;
+ yy = y * y;
+ zz = z * z;
+ xy = x * y;
+ yz = y * z;
+ zx = z * x;
+ xs = x * s;
+ ys = y * s;
+ zs = z * s;
+ one_c = 1.0f - c;
+
+ M(0,0) = (one_c * xx) + c;
+ M(0,1) = (one_c * xy) - zs;
+ M(0,2) = (one_c * zx) + ys;
+ M(0,3) = 0.0f;
+
+ M(1,0) = (one_c * xy) + zs;
+ M(1,1) = (one_c * yy) + c;
+ M(1,2) = (one_c * yz) - xs;
+ M(1,3) = 0.0f;
+
+ M(2,0) = (one_c * zx) - ys;
+ M(2,1) = (one_c * yz) + xs;
+ M(2,2) = (one_c * zz) + c;
+ M(2,3) = 0.0f;
+
+ M(3,0) = 0.0f;
+ M(3,1) = 0.0f;
+ M(3,2) = 0.0f;
+ M(3,3) = 1.0f;
+
+ #undef M
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Ditto above, but for doubles
+void m3dRotationMatrix33(M3DMatrix33d m, double angle, double x, double y, double z)
+{
+ double mag, s, c;
+ double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
+
+ s = sin(angle);
+ c = cos(angle);
+
+ mag = sqrt( x*x + y*y + z*z );
+
+ // Identity matrix
+ if (mag == 0.0) {
+ m3dLoadIdentity33(m);
+ return;
+}
+
+ // Rotation matrix is normalized
+ x /= mag;
+ y /= mag;
+ z /= mag;
+
+ #define M(row,col) m[col*3+row]
+
+ xx = x * x;
+ yy = y * y;
+ zz = z * z;
+ xy = x * y;
+ yz = y * z;
+ zx = z * x;
+ xs = x * s;
+ ys = y * s;
+ zs = z * s;
+ one_c = 1.0 - c;
+
+ M(0,0) = (one_c * xx) + c;
+ M(0,1) = (one_c * xy) - zs;
+ M(0,2) = (one_c * zx) + ys;
+
+ M(1,0) = (one_c * xy) + zs;
+ M(1,1) = (one_c * yy) + c;
+ M(1,2) = (one_c * yz) - xs;
+
+ M(2,0) = (one_c * zx) - ys;
+ M(2,1) = (one_c * yz) + xs;
+ M(2,2) = (one_c * zz) + c;
+
+ #undef M
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Creates a 4x4 rotation matrix, takes radians NOT degrees
+void m3dRotationMatrix44(M3DMatrix44d m, double angle, double x, double y, double z)
+{
+ double mag, s, c;
+ double xx, yy, zz, xy, yz, zx, xs, ys, zs, one_c;
+
+ s = sin(angle);
+ c = cos(angle);
+
+ mag = sqrt( x*x + y*y + z*z );
+
+ // Identity matrix
+ if (mag == 0.0) {
+ m3dLoadIdentity44(m);
+ return;
+ }
+
+ // Rotation matrix is normalized
+ x /= mag;
+ y /= mag;
+ z /= mag;
+
+ #define M(row,col) m[col*4+row]
+
+ xx = x * x;
+ yy = y * y;
+ zz = z * z;
+ xy = x * y;
+ yz = y * z;
+ zx = z * x;
+ xs = x * s;
+ ys = y * s;
+ zs = z * s;
+ one_c = 1.0f - c;
+
+ M(0,0) = (one_c * xx) + c;
+ M(0,1) = (one_c * xy) - zs;
+ M(0,2) = (one_c * zx) + ys;
+ M(0,3) = 0.0;
+
+ M(1,0) = (one_c * xy) + zs;
+ M(1,1) = (one_c * yy) + c;
+ M(1,2) = (one_c * yz) - xs;
+ M(1,3) = 0.0;
+
+ M(2,0) = (one_c * zx) - ys;
+ M(2,1) = (one_c * yz) + xs;
+ M(2,2) = (one_c * zz) + c;
+ M(2,3) = 0.0;
+
+ M(3,0) = 0.0;
+ M(3,1) = 0.0;
+ M(3,2) = 0.0;
+ M(3,3) = 1.0;
+
+ #undef M
+}
+
+// Lifted from Mesa
+/*
+ * Compute inverse of 4x4 transformation matrix.
+ * Code contributed by Jacques Leroy jle@star.be
+ * Return GL_TRUE for success, GL_FALSE for failure (singular matrix)
+ */
+bool m3dInvertMatrix44(M3DMatrix44f dst, const M3DMatrix44f src )
+{
+ #define SWAP_ROWS(a, b) { float *_tmp = a; (a)=(b); (b)=_tmp; }
+ #define MAT(m,r,c) (m)[(c)*4+(r)]
+
+ float wtmp[4][8];
+ float m0, m1, m2, m3, s;
+ float *r0, *r1, *r2, *r3;
+
+ r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
+
+ r0[0] = MAT(src,0,0), r0[1] = MAT(src,0,1),
+ r0[2] = MAT(src,0,2), r0[3] = MAT(src,0,3),
+ r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
+
+ r1[0] = MAT(src,1,0), r1[1] = MAT(src,1,1),
+ r1[2] = MAT(src,1,2), r1[3] = MAT(src,1,3),
+ r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
+
+ r2[0] = MAT(src,2,0), r2[1] = MAT(src,2,1),
+ r2[2] = MAT(src,2,2), r2[3] = MAT(src,2,3),
+ r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
+
+ r3[0] = MAT(src,3,0), r3[1] = MAT(src,3,1),
+ r3[2] = MAT(src,3,2), r3[3] = MAT(src,3,3),
+ r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
+
+ /* choose pivot - or die */
+ if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
+ if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
+ if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
+ if (0.0 == r0[0]) return false;
+
+ /* eliminate first variable */
+ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
+ s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
+ s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
+ s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
+ s = r0[4];
+ if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
+ s = r0[5];
+ if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
+ s = r0[6];
+ if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
+ s = r0[7];
+ if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+ /* choose pivot - or die */
+ if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
+ if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
+ if (0.0 == r1[1]) return false;
+
+ /* eliminate second variable */
+ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
+ r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
+ r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
+ s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
+ s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
+ s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
+ s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+ /* choose pivot - or die */
+ if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
+ if (0.0 == r2[2]) return false;
+
+ /* eliminate third variable */
+ m3 = r3[2]/r2[2];
+ r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
+ r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
+ r3[7] -= m3 * r2[7];
+
+ /* last check */
+ if (0.0 == r3[3]) return false;
+
+ s = 1.0f/r3[3]; /* now back substitute row 3 */
+ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
+
+ m2 = r2[3]; /* now back substitute row 2 */
+ s = 1.0f/r2[2];
+ r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
+ r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
+ m1 = r1[3];
+ r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
+ r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
+ m0 = r0[3];
+ r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
+ r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
+
+ m1 = r1[2]; /* now back substitute row 1 */
+ s = 1.0f/r1[1];
+ r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
+ r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
+ m0 = r0[2];
+ r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
+ r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
+
+ m0 = r0[1]; /* now back substitute row 0 */
+ s = 1.0f/r0[0];
+ r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
+ r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
+
+ MAT(dst,0,0) = r0[4]; MAT(dst,0,1) = r0[5],
+ MAT(dst,0,2) = r0[6]; MAT(dst,0,3) = r0[7],
+ MAT(dst,1,0) = r1[4]; MAT(dst,1,1) = r1[5],
+ MAT(dst,1,2) = r1[6]; MAT(dst,1,3) = r1[7],
+ MAT(dst,2,0) = r2[4]; MAT(dst,2,1) = r2[5],
+ MAT(dst,2,2) = r2[6]; MAT(dst,2,3) = r2[7],
+ MAT(dst,3,0) = r3[4]; MAT(dst,3,1) = r3[5],
+ MAT(dst,3,2) = r3[6]; MAT(dst,3,3) = r3[7];
+
+ return true;
+
+ #undef MAT
+ #undef SWAP_ROWS
+}
+
+// Ditto above, but for doubles
+bool m3dInvertMatrix44(M3DMatrix44d dst, const M3DMatrix44d src)
+{
+ #define SWAP_ROWS(a, b) { double *_tmp = a; (a)=(b); (b)=_tmp; }
+ #define MAT(m,r,c) (m)[(c)*4+(r)]
+
+ double wtmp[4][8];
+ double m0, m1, m2, m3, s;
+ double *r0, *r1, *r2, *r3;
+
+ r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
+
+ r0[0] = MAT(src,0,0), r0[1] = MAT(src,0,1),
+ r0[2] = MAT(src,0,2), r0[3] = MAT(src,0,3),
+ r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
+
+ r1[0] = MAT(src,1,0), r1[1] = MAT(src,1,1),
+ r1[2] = MAT(src,1,2), r1[3] = MAT(src,1,3),
+ r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
+
+ r2[0] = MAT(src,2,0), r2[1] = MAT(src,2,1),
+ r2[2] = MAT(src,2,2), r2[3] = MAT(src,2,3),
+ r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
+
+ r3[0] = MAT(src,3,0), r3[1] = MAT(src,3,1),
+ r3[2] = MAT(src,3,2), r3[3] = MAT(src,3,3),
+ r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
+
+ // choose pivot - or die
+ if (fabs(r3[0])>fabs(r2[0])) SWAP_ROWS(r3, r2);
+ if (fabs(r2[0])>fabs(r1[0])) SWAP_ROWS(r2, r1);
+ if (fabs(r1[0])>fabs(r0[0])) SWAP_ROWS(r1, r0);
+ if (0.0 == r0[0]) return false;
+
+ // eliminate first variable
+ m1 = r1[0]/r0[0]; m2 = r2[0]/r0[0]; m3 = r3[0]/r0[0];
+ s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
+ s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
+ s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
+ s = r0[4];
+ if (s != 0.0) { r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
+ s = r0[5];
+ if (s != 0.0) { r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
+ s = r0[6];
+ if (s != 0.0) { r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
+ s = r0[7];
+ if (s != 0.0) { r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+ // choose pivot - or die
+ if (fabs(r3[1])>fabs(r2[1])) SWAP_ROWS(r3, r2);
+ if (fabs(r2[1])>fabs(r1[1])) SWAP_ROWS(r2, r1);
+ if (0.0 == r1[1]) return false;
+
+ // eliminate second variable
+ m2 = r2[1]/r1[1]; m3 = r3[1]/r1[1];
+ r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
+ r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
+ s = r1[4]; if (0.0 != s) { r2[4] -= m2 * s; r3[4] -= m3 * s; }
+ s = r1[5]; if (0.0 != s) { r2[5] -= m2 * s; r3[5] -= m3 * s; }
+ s = r1[6]; if (0.0 != s) { r2[6] -= m2 * s; r3[6] -= m3 * s; }
+ s = r1[7]; if (0.0 != s) { r2[7] -= m2 * s; r3[7] -= m3 * s; }
+
+ // choose pivot - or die
+ if (fabs(r3[2])>fabs(r2[2])) SWAP_ROWS(r3, r2);
+ if (0.0 == r2[2]) return false;
+
+ // eliminate third variable
+ m3 = r3[2]/r2[2];
+ r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
+ r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
+ r3[7] -= m3 * r2[7];
+
+ // last check
+ if (0.0 == r3[3]) return false;
+
+ s = 1.0f/r3[3]; // now back substitute row 3
+ r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;
+
+ m2 = r2[3]; // now back substitute row 2
+ s = 1.0f/r2[2];
+ r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
+ r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
+ m1 = r1[3];
+ r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
+ r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
+ m0 = r0[3];
+ r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
+ r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
+
+ m1 = r1[2]; // now back substitute row 1
+ s = 1.0f/r1[1];
+ r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
+ r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
+ m0 = r0[2];
+ r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
+ r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
+
+ m0 = r0[1]; // now back substitute row 0
+ s = 1.0f/r0[0];
+ r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
+ r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
+
+ MAT(dst,0,0) = r0[4]; MAT(dst,0,1) = r0[5],
+ MAT(dst,0,2) = r0[6]; MAT(dst,0,3) = r0[7],
+ MAT(dst,1,0) = r1[4]; MAT(dst,1,1) = r1[5],
+ MAT(dst,1,2) = r1[6]; MAT(dst,1,3) = r1[7],
+ MAT(dst,2,0) = r2[4]; MAT(dst,2,1) = r2[5],
+ MAT(dst,2,2) = r2[6]; MAT(dst,2,3) = r2[7],
+ MAT(dst,3,0) = r3[4]; MAT(dst,3,1) = r3[5],
+ MAT(dst,3,2) = r3[6]; MAT(dst,3,3) = r3[7];
+
+ return true;
+
+ #undef MAT
+ #undef SWAP_ROWS
+
+ return true;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+// Get Window coordinates, discard Z...
+void m3dProjectXY(const M3DMatrix44f mModelView, const M3DMatrix44f mProjection, const int iViewPort[4], const M3DVector3f vPointIn, M3DVector2f vPointOut)
+{
+ M3DVector4f vBack, vForth;
+
+ memcpy(vBack, vPointIn, sizeof(float)*3);
+ vBack[3] = 1.0f;
+
+ m3dTransformVector4(vForth, vBack, mModelView);
+ m3dTransformVector4(vBack, vForth, mProjection);
+
+ if (!m3dCloseEnough(vBack[3], 0.0f, 0.000001f)) {
+ float div = 1.0f / vBack[3];
+ vBack[0] *= div;
+ vBack[1] *= div;
+ }
+
+
+ vPointOut[0] = vBack[0] * 0.5f + 0.5f;
+ vPointOut[1] = vBack[1] * 0.5f + 0.5f;
+
+ /* Map x,y to viewport */
+ vPointOut[0] = (vPointOut[0] * iViewPort[2]) + iViewPort[0];
+ vPointOut[1] = (vPointOut[1] * iViewPort[3]) + iViewPort[1];
+}
+
+///////////////////////////////////////////////////////////////////////////////////////
+// Get window coordinates, we also want Z....
+void m3dProjectXYZ(const M3DMatrix44f mModelView, const M3DMatrix44f mProjection, const int iViewPort[4], const M3DVector3f vPointIn, M3DVector3f vPointOut)
+{
+ M3DVector4f vBack, vForth;
+
+ memcpy(vBack, vPointIn, sizeof(float)*3);
+ vBack[3] = 1.0f;
+
+ m3dTransformVector4(vForth, vBack, mModelView);
+ m3dTransformVector4(vBack, vForth, mProjection);
+
+ if (!m3dCloseEnough(vBack[3], 0.0f, 0.000001f)) {
+ float div = 1.0f / vBack[3];
+ vBack[0] *= div;
+ vBack[1] *= div;
+ vBack[2] *= div;
+ }
+
+ vPointOut[0] = vBack[0] * 0.5f + 0.5f;
+ vPointOut[1] = vBack[1] * 0.5f + 0.5f;
+ vPointOut[2] = vBack[2] * 0.5f + 0.5f;
+
+ /* Map x,y to viewport */
+ vPointOut[0] = (vPointOut[0] * iViewPort[2]) + iViewPort[0];
+ vPointOut[1] = (vPointOut[1] * iViewPort[3]) + iViewPort[1];
+}
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+// Misc. Utilities
+///////////////////////////////////////////////////////////////////////////////
+// Calculates the normal of a triangle specified by the three points
+// p1, p2, and p3. Each pointer points to an array of three floats. The
+// triangle is assumed to be wound counter clockwise.
+void m3dFindNormal(M3DVector3f result, const M3DVector3f point1, const M3DVector3f point2,
+ const M3DVector3f point3)
+{
+ M3DVector3f v1,v2; // Temporary vectors
+
+ // Calculate two vectors from the three points. Assumes counter clockwise
+ // winding!
+ v1[0] = point1[0] - point2[0];
+ v1[1] = point1[1] - point2[1];
+ v1[2] = point1[2] - point2[2];
+
+ v2[0] = point2[0] - point3[0];
+ v2[1] = point2[1] - point3[1];
+ v2[2] = point2[2] - point3[2];
+
+ // Take the cross product of the two vectors to get
+ // the normal vector.
+ m3dCrossProduct(result, v1, v2);
+}
+
+// Ditto above, but for doubles
+void m3dFindNormal(M3DVector3d result, const M3DVector3d point1, const M3DVector3d point2,
+ const M3DVector3d point3)
+{
+ M3DVector3d v1,v2; // Temporary vectors
+
+ // Calculate two vectors from the three points. Assumes counter clockwise
+ // winding!
+ v1[0] = point1[0] - point2[0];
+ v1[1] = point1[1] - point2[1];
+ v1[2] = point1[2] - point2[2];
+
+ v2[0] = point2[0] - point3[0];
+ v2[1] = point2[1] - point3[1];
+ v2[2] = point2[2] - point3[2];
+
+ // Take the cross product of the two vectors to get
+ // the normal vector.
+ m3dCrossProduct(result, v1, v2);
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////
+// Calculate the plane equation of the plane that the three specified points lay in. The
+// points are given in clockwise winding order, with normal pointing out of clockwise face
+// planeEq contains the A,B,C, and D of the plane equation coefficients
+void m3dGetPlaneEquation(M3DVector4f planeEq, const M3DVector3f p1, const M3DVector3f p2, const M3DVector3f p3)
+{
+ // Get two vectors... do the cross product
+ M3DVector3f v1, v2;
+
+ // V1 = p3 - p1
+ v1[0] = p3[0] - p1[0];
+ v1[1] = p3[1] - p1[1];
+ v1[2] = p3[2] - p1[2];
+
+ // V2 = P2 - p1
+ v2[0] = p2[0] - p1[0];
+ v2[1] = p2[1] - p1[1];
+ v2[2] = p2[2] - p1[2];
+
+ // Unit normal to plane - Not sure which is the best way here
+ m3dCrossProduct(planeEq, v1, v2);
+ m3dNormalizeVector(planeEq);
+ // Back substitute to get D
+ planeEq[3] = -(planeEq[0] * p3[0] + planeEq[1] * p3[1] + planeEq[2] * p3[2]);
+}
+
+// Ditto above, but for doubles
+void m3dGetPlaneEquation(M3DVector4d planeEq, const M3DVector3d p1, const M3DVector3d p2, const M3DVector3d p3)
+{
+ // Get two vectors... do the cross product
+ M3DVector3d v1, v2;
+
+ // V1 = p3 - p1
+ v1[0] = p3[0] - p1[0];
+ v1[1] = p3[1] - p1[1];
+ v1[2] = p3[2] - p1[2];
+
+ // V2 = P2 - p1
+ v2[0] = p2[0] - p1[0];
+ v2[1] = p2[1] - p1[1];
+ v2[2] = p2[2] - p1[2];
+
+ // Unit normal to plane - Not sure which is the best way here
+ m3dCrossProduct(planeEq, v1, v2);
+ m3dNormalizeVector(planeEq);
+ // Back substitute to get D
+ planeEq[3] = -(planeEq[0] * p3[0] + planeEq[1] * p3[1] + planeEq[2] * p3[2]);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////
+// This function does a three dimensional Catmull-Rom curve interpolation. Pass four points, and a
+// floating point number between 0.0 and 1.0. The curve is interpolated between the middle two points.
+// Coded by RSW
+// http://www.mvps.org/directx/articles/catmull/
+void m3dCatmullRom3(M3DVector3f vOut, M3DVector3f vP0, M3DVector3f vP1, M3DVector3f vP2, M3DVector3f vP3, float t)
+{
+ // Unrolled loop to speed things up a little bit...
+ float t2 = t * t;
+ float t3 = t2 * t;
+
+ // X
+ vOut[0] = 0.5f * ( ( 2.0f * vP1[0]) +
+ (-vP0[0] + vP2[0]) * t +
+ (2.0f * vP0[0] - 5.0f *vP1[0] + 4.0f * vP2[0] - vP3[0]) * t2 +
+ (-vP0[0] + 3.0f*vP1[0] - 3.0f *vP2[0] + vP3[0]) * t3);
+ // Y
+ vOut[1] = 0.5f * ( ( 2.0f * vP1[1]) +
+ (-vP0[1] + vP2[1]) * t +
+ (2.0f * vP0[1] - 5.0f *vP1[1] + 4.0f * vP2[1] - vP3[1]) * t2 +
+ (-vP0[1] + 3.0f*vP1[1] - 3.0f *vP2[1] + vP3[1]) * t3);
+
+ // Z
+ vOut[2] = 0.5f * ( ( 2.0f * vP1[2]) +
+ (-vP0[2] + vP2[2]) * t +
+ (2.0f * vP0[2] - 5.0f *vP1[2] + 4.0f * vP2[2] - vP3[2]) * t2 +
+ (-vP0[2] + 3.0f*vP1[2] - 3.0f *vP2[2] + vP3[2]) * t3);
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////
+// This function does a three dimensional Catmull-Rom curve interpolation. Pass four points, and a
+// floating point number between 0.0 and 1.0. The curve is interpolated between the middle two points.
+// Coded by RSW
+// http://www.mvps.org/directx/articles/catmull/
+void m3dCatmullRom3(M3DVector3d vOut, M3DVector3d vP0, M3DVector3d vP1, M3DVector3d vP2, M3DVector3d vP3, double t)
+{
+ // Unrolled loop to speed things up a little bit...
+ double t2 = t * t;
+ double t3 = t2 * t;
+
+ // X
+ vOut[0] = 0.5 * ( ( 2.0 * vP1[0]) +
+ (-vP0[0] + vP2[0]) * t +
+ (2.0 * vP0[0] - 5.0 *vP1[0] + 4.0 * vP2[0] - vP3[0]) * t2 +
+ (-vP0[0] + 3.0*vP1[0] - 3.0 *vP2[0] + vP3[0]) * t3);
+ // Y
+ vOut[1] = 0.5 * ( ( 2.0 * vP1[1]) +
+ (-vP0[1] + vP2[1]) * t +
+ (2.0 * vP0[1] - 5.0 *vP1[1] + 4.0 * vP2[1] - vP3[1]) * t2 +
+ (-vP0[1] + 3*vP1[1] - 3.0 *vP2[1] + vP3[1]) * t3);
+
+ // Z
+ vOut[2] = 0.5 * ( ( 2.0 * vP1[2]) +
+ (-vP0[2] + vP2[2]) * t +
+ (2.0 * vP0[2] - 5.0 *vP1[2] + 4.0 * vP2[2] - vP3[2]) * t2 +
+ (-vP0[2] + 3.0*vP1[2] - 3.0 *vP2[2] + vP3[2]) * t3);
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Determine if the ray (starting at point) intersects the sphere centered at
+// sphereCenter with radius sphereRadius
+// Return value is < 0 if the ray does not intersect
+// Return value is 0.0 if ray is tangent
+// Positive value is distance to the intersection point
+// Algorithm from "3D Math Primer for Graphics and Game Development"
+double m3dRaySphereTest(const M3DVector3d point, const M3DVector3d ray, const M3DVector3d sphereCenter, double sphereRadius)
+{
+ //m3dNormalizeVector(ray); // Make sure ray is unit length
+
+ M3DVector3d rayToCenter; // Ray to center of sphere
+ rayToCenter[0] = sphereCenter[0] - point[0];
+ rayToCenter[1] = sphereCenter[1] - point[1];
+ rayToCenter[2] = sphereCenter[2] - point[2];
+
+ // Project rayToCenter on ray to test
+ double a = m3dDotProduct(rayToCenter, ray);
+
+ // Distance to center of sphere
+ double distance2 = m3dDotProduct(rayToCenter, rayToCenter); // Or length
+
+
+ double dRet = (sphereRadius * sphereRadius) - distance2 + (a*a);
+
+ if (dRet > 0.0) // Return distance to intersection
+ dRet = a - sqrt(dRet);
+
+ return dRet;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Determine if the ray (starting at point) intersects the sphere centered at
+// ditto above, but uses floating point math
+float m3dRaySphereTest(const M3DVector3f point, const M3DVector3f ray, const M3DVector3f sphereCenter, float sphereRadius)
+{
+ //m3dNormalizeVectorf(ray); // Make sure ray is unit length
+
+ M3DVector3f rayToCenter; // Ray to center of sphere
+ rayToCenter[0] = sphereCenter[0] - point[0];
+ rayToCenter[1] = sphereCenter[1] - point[1];
+ rayToCenter[2] = sphereCenter[2] - point[2];
+
+ // Project rayToCenter on ray to test
+ float a = m3dDotProduct(rayToCenter, ray);
+
+ // Distance to center of sphere
+ float distance2 = m3dDotProduct(rayToCenter, rayToCenter); // Or length
+
+ float dRet = (sphereRadius * sphereRadius) - distance2 + (a*a);
+
+ if (dRet > 0.0) // Return distance to intersection
+ dRet = a - sqrtf(dRet);
+
+ return dRet;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////
+// Calculate the tangent basis for a triangle on the surface of a model
+// This vector is needed for most normal mapping shaders
+void m3dCalculateTangentBasis(const M3DVector3f vTriangle[3], const M3DVector2f vTexCoords[3], const M3DVector3f N, M3DVector3f vTangent)
+{
+ M3DVector3f dv2v1, dv3v1;
+ float dc2c1t, dc2c1b, dc3c1t, dc3c1b;
+ float M;
+
+ m3dSubtractVectors3(dv2v1, vTriangle[1], vTriangle[0]);
+ m3dSubtractVectors3(dv3v1, vTriangle[2], vTriangle[0]);
+
+ dc2c1t = vTexCoords[1][0] - vTexCoords[0][0];
+ dc2c1b = vTexCoords[1][1] - vTexCoords[0][1];
+ dc3c1t = vTexCoords[2][0] - vTexCoords[0][0];
+ dc3c1b = vTexCoords[2][1] - vTexCoords[0][1];
+
+ M = (dc2c1t * dc3c1b) - (dc3c1t * dc2c1b);
+ M = 1.0f / M;
+
+ m3dScaleVector3(dv2v1, dc3c1b);
+ m3dScaleVector3(dv3v1, dc2c1b);
+
+ m3dSubtractVectors3(vTangent, dv2v1, dv3v1);
+ m3dScaleVector3(vTangent, M); // This potentially changes the direction of the vector
+ m3dNormalizeVector(vTangent);
+
+ M3DVector3f B;
+ m3dCrossProduct(B, N, vTangent);
+ m3dCrossProduct(vTangent, B, N);
+ m3dNormalizeVector(vTangent);
+}
+
+////////////////////////////////////////////////////////////////////////////
+// Smoothly step between 0 and 1 between edge1 and edge 2
+double m3dSmoothStep(double edge1, double edge2, double x)
+{
+ double t;
+ t = (x - edge1) / (edge2 - edge1);
+ if (t > 1.0)
+ t = 1.0;
+
+ if (t < 0.0)
+ t = 0.0f;
+
+ return t * t * ( 3.0 - 2.0 * t);
+}
+
+////////////////////////////////////////////////////////////////////////////
+// Smoothly step between 0 and 1 between edge1 and edge 2
+float m3dSmoothStep(float edge1, float edge2, float x)
+{
+ float t;
+ t = (x - edge1) / (edge2 - edge1);
+ if (t > 1.0f)
+ t = 1.0f;
+
+ if (t < 0.0)
+ t = 0.0f;
+
+ return t * t * ( 3.0f - 2.0f * t);
+}
+
+///////////////////////////////////////////////////////////////////////////
+// Creae a projection to "squish" an object into the plane.
+// Use m3dGetPlaneEquationf(planeEq, point1, point2, point3);
+// to get a plane equation.
+void m3dMakePlanarShadowMatrix(M3DMatrix44f proj, const M3DVector4f planeEq, const M3DVector3f vLightPos)
+{
+ // These just make the code below easier to read. They will be
+ // removed by the optimizer.
+ float a = planeEq[0];
+ float b = planeEq[1];
+ float c = planeEq[2];
+ float d = planeEq[3];
+
+ float dx = -vLightPos[0];
+ float dy = -vLightPos[1];
+ float dz = -vLightPos[2];
+
+ // Now build the projection matrix
+ proj[0] = b * dy + c * dz;
+ proj[1] = -a * dy;
+ proj[2] = -a * dz;
+ proj[3] = 0.0;
+
+ proj[4] = -b * dx;
+ proj[5] = a * dx + c * dz;
+ proj[6] = -b * dz;
+ proj[7] = 0.0;
+
+ proj[8] = -c * dx;
+ proj[9] = -c * dy;
+ proj[10] = a * dx + b * dy;
+ proj[11] = 0.0;
+
+ proj[12] = -d * dx;
+ proj[13] = -d * dy;
+ proj[14] = -d * dz;
+ proj[15] = a * dx + b * dy + c * dz;
+ // Shadow matrix ready
+}
+
+///////////////////////////////////////////////////////////////////////////
+// Creae a projection to "squish" an object into the plane.
+// Use m3dGetPlaneEquationd(planeEq, point1, point2, point3);
+// to get a plane equation.
+void m3dMakePlanarShadowMatrix(M3DMatrix44d proj, const M3DVector4d planeEq, const M3DVector3f vLightPos)
+{
+ // These just make the code below easier to read. They will be
+ // removed by the optimizer.
+ double a = planeEq[0];
+ double b = planeEq[1];
+ double c = planeEq[2];
+ double d = planeEq[3];
+
+ double dx = -vLightPos[0];
+ double dy = -vLightPos[1];
+ double dz = -vLightPos[2];
+
+ // Now build the projection matrix
+ proj[0] = b * dy + c * dz;
+ proj[1] = -a * dy;
+ proj[2] = -a * dz;
+ proj[3] = 0.0;
+
+ proj[4] = -b * dx;
+ proj[5] = a * dx + c * dz;
+ proj[6] = -b * dz;
+ proj[7] = 0.0;
+
+ proj[8] = -c * dx;
+ proj[9] = -c * dy;
+ proj[10] = a * dx + b * dy;
+ proj[11] = 0.0;
+
+ proj[12] = -d * dx;
+ proj[13] = -d * dy;
+ proj[14] = -d * dz;
+ proj[15] = a * dx + b * dy + c * dz;
+ // Shadow matrix ready
+}
+
+/////////////////////////////////////////////////////////////////////////////
+// I want to know the point on a ray, closest to another given point in space.
+// As a bonus, return the distance squared of the two points.
+// In: vRayOrigin is the origin of the ray.
+// In: vUnitRayDir is the unit vector of the ray
+// In: vPointInSpace is the point in space
+// Out: vPointOnRay is the poing on the ray closest to vPointInSpace
+// Return: The square of the distance to the ray
+double m3dClosestPointOnRay(M3DVector3d vPointOnRay, const M3DVector3d vRayOrigin, const M3DVector3d vUnitRayDir,
+ const M3DVector3d vPointInSpace)
+{
+ M3DVector3d v;
+ m3dSubtractVectors3(v, vPointInSpace, vRayOrigin);
+
+ double t = m3dDotProduct(vUnitRayDir, v);
+
+ // This is the point on the ray
+ vPointOnRay[0] = vRayOrigin[0] + (t * vUnitRayDir[0]);
+ vPointOnRay[1] = vRayOrigin[1] + (t * vUnitRayDir[1]);
+ vPointOnRay[2] = vRayOrigin[2] + (t * vUnitRayDir[2]);
+
+ return m3dGetDistanceSquared(vPointOnRay, vPointInSpace);
+}
+
+// Ditto above... but with floats
+float m3dClosestPointOnRay(M3DVector3f vPointOnRay, const M3DVector3f vRayOrigin, const M3DVector3f vUnitRayDir,
+ const M3DVector3f vPointInSpace)
+{
+ M3DVector3f v;
+ m3dSubtractVectors3(v, vPointInSpace, vRayOrigin);
+
+ float t = m3dDotProduct(vUnitRayDir, v);
+
+ // This is the point on the ray
+ vPointOnRay[0] = vRayOrigin[0] + (t * vUnitRayDir[0]);
+ vPointOnRay[1] = vRayOrigin[1] + (t * vUnitRayDir[1]);
+ vPointOnRay[2] = vRayOrigin[2] + (t * vUnitRayDir[2]);
+
+ return m3dGetDistanceSquared(vPointOnRay, vPointInSpace);
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2025-03-14 22:45:48 +0000