summaryrefslogtreecommitdiffstats
path: root/libdimension
diff options
context:
space:
mode:
Diffstat (limited to 'libdimension')
-rw-r--r--libdimension/geometry.c209
1 files changed, 171 insertions, 38 deletions
diff --git a/libdimension/geometry.c b/libdimension/geometry.c
index 372a94c..8625e62 100644
--- a/libdimension/geometry.c
+++ b/libdimension/geometry.c
@@ -81,38 +81,37 @@ dmnsn_rotation_matrix(dmnsn_vector theta)
double angle, s, t, x, y, z;
angle = dmnsn_vector_norm(theta);
- if (angle != 0.0) {
- axis = dmnsn_vector_normalize(theta);
-
- /* Shorthand to fit logical lines on one line */
-
- s = sin(angle);
- t = 1.0 - cos(angle);
-
- x = axis.x;
- y = axis.y;
- z = axis.z;
-
- /* Construct vectors, then a matrix, so our dmnsn_matrix_construct() call
- is reasonably small */
-
- n1 = dmnsn_vector_construct(1.0 + t*(x*x - 1.0),
- z*s + t*x*y,
- -y*s + t*x*z);
- n2 = dmnsn_vector_construct(-z*s + t*x*y,
- 1.0 + t*(y*y - 1.0),
- x*s + t*y*z);
- n3 = dmnsn_vector_construct(y*s + t*x*z,
- -x*s + t*y*z,
- 1.0 + t*(z*z - 1.0));
-
- return dmnsn_matrix_construct(n1.x, n2.x, n3.x, 0.0,
- n1.y, n2.y, n3.y, 0.0,
- n1.z, n2.z, n3.z, 0.0,
- 0.0, 0.0, 0.0, 1.0);
- } else {
+ if (angle == 0.0) {
return dmnsn_identity_matrix();
}
+ axis = dmnsn_vector_normalize(theta);
+
+ /* Shorthand to fit logical lines on one line */
+
+ s = sin(angle);
+ t = 1.0 - cos(angle);
+
+ x = axis.x;
+ y = axis.y;
+ z = axis.z;
+
+ /* Construct vectors, then a matrix, so our dmnsn_matrix_construct() call
+ is reasonably small */
+
+ n1 = dmnsn_vector_construct(1.0 + t*(x*x - 1.0),
+ z*s + t*x*y,
+ -y*s + t*x*z);
+ n2 = dmnsn_vector_construct(-z*s + t*x*y,
+ 1.0 + t*(y*y - 1.0),
+ x*s + t*y*z);
+ n3 = dmnsn_vector_construct(y*s + t*x*z,
+ -x*s + t*y*z,
+ 1.0 + t*(z*z - 1.0));
+
+ return dmnsn_matrix_construct(n1.x, n2.x, n3.x, 0.0,
+ n1.y, n2.y, n3.y, 0.0,
+ n1.z, n2.z, n3.z, 0.0,
+ 0.0, 0.0, 0.0, 1.0);
}
/* Add two vectors */
@@ -182,35 +181,167 @@ dmnsn_vector_normalize(dmnsn_vector n)
return dmnsn_vector_div(n, dmnsn_vector_norm(n));
}
-/* Matrix inversion helper function */
+/* Matrix inversion helper functions */
+
+typedef struct { double n[2][2]; } dmnsn_matrix2;
+
+static dmnsn_matrix2 dmnsn_matrix2_construct(double a1, double a2,
+ double b1, double b2);
+static dmnsn_matrix2 dmnsn_matrix2_inverse(dmnsn_matrix2 A);
+static dmnsn_matrix2 dmnsn_matrix2_negate(dmnsn_matrix2 A);
+static dmnsn_matrix2 dmnsn_matrix2_sub(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs);
+static dmnsn_matrix2 dmnsn_matrix2_mul(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs);
+
+static dmnsn_matrix dmnsn_matrix_inverse_generic(dmnsn_matrix A);
static double dmnsn_matrix_cofactor(dmnsn_matrix A,
unsigned int row, unsigned int col);
-/* Invert a matrix */
+/* Invert a matrix, by partitioning */
dmnsn_matrix
-dmnsn_matrix_inverse(dmnsn_matrix A)
+dmnsn_matrix_inverse(dmnsn_matrix A) {
+ /*
+ * Use partitioning to invert a matrix:
+ *
+ * ( P Q ) -1
+ * ( R S )
+ *
+ * = ( PP QQ )
+ * ( RR SS ),
+ *
+ * with PP = inv(P) - inv(P)*Q*RR,
+ * QQ = -inv(P)*Q*SS,
+ * RR = -SS*R*inv(P), and
+ * SS = inv(S - R*inv(P)*Q).
+ */
+
+ /* The algorithm uses 2 inversions, 6 multiplications, and 2 subtractions,
+ giving 52 multiplications, 34 additions, and 8 divisions. */
+
+ dmnsn_matrix2 P, Q, R, S, Pi, RPi, PiQ, RPiQ, PP, QQ, RR, SS;
+ double Pdet = A.n[0][0]*A.n[1][1] - A.n[0][1]*A.n[1][0];
+
+ if (Pdet == 0.0) {
+ /* If we can't invert P, try a more generic algorithm */
+ return dmnsn_matrix_inverse_generic(A);
+ }
+
+ /* Partition the matrix */
+ P = dmnsn_matrix2_construct(A.n[0][0], A.n[0][1],
+ A.n[1][0], A.n[1][1]);
+ Q = dmnsn_matrix2_construct(A.n[0][2], A.n[0][3],
+ A.n[1][2], A.n[1][3]);
+ R = dmnsn_matrix2_construct(A.n[2][0], A.n[2][1],
+ A.n[3][0], A.n[3][1]);
+ S = dmnsn_matrix2_construct(A.n[2][2], A.n[2][3],
+ A.n[3][2], A.n[3][3]);
+
+ /* Do this inversion ourselves, since we already have the determinant */
+ Pi = dmnsn_matrix2_construct( P.n[1][1]/Pdet, -P.n[0][1]/Pdet,
+ -P.n[1][0]/Pdet, P.n[0][0]/Pdet);
+
+ /* Calculate R*inv(P), inv(P)*Q, and R*inv(P)*Q */
+ RPi = dmnsn_matrix2_mul(R, Pi);
+ PiQ = dmnsn_matrix2_mul(Pi, Q);
+ RPiQ = dmnsn_matrix2_mul(R, PiQ);
+
+ SS = dmnsn_matrix2_inverse(dmnsn_matrix2_sub(S, RPiQ));
+ RR = dmnsn_matrix2_negate(dmnsn_matrix2_mul(SS, RPi));
+ QQ = dmnsn_matrix2_negate(dmnsn_matrix2_mul(PiQ, SS));
+ PP = dmnsn_matrix2_sub(Pi, dmnsn_matrix2_mul(PiQ, RR));
+
+ /* Reconstruct the matrix */
+ return dmnsn_matrix_construct(PP.n[0][0], PP.n[0][1], QQ.n[0][0], QQ.n[0][1],
+ PP.n[1][0], PP.n[1][1], QQ.n[1][0], QQ.n[1][1],
+ RR.n[0][0], RR.n[0][1], SS.n[0][0], SS.n[0][1],
+ RR.n[1][0], RR.n[1][1], SS.n[1][0], SS.n[1][1]);
+}
+
+/* For nice shorthand */
+static dmnsn_matrix2
+dmnsn_matrix2_construct(double a1, double a2, double b1, double b2)
+{
+ dmnsn_matrix2 m = { { { a1, a2 },
+ { b1, b2 } } };
+ return m;
+}
+
+/* Invert a 2x2 matrix */
+static dmnsn_matrix2
+dmnsn_matrix2_inverse(dmnsn_matrix2 A)
+{
+ /* 4 divisions, 2 multiplications, 1 addition */
+ double det = A.n[0][0]*A.n[1][1] - A.n[0][1]*A.n[1][0];
+ return dmnsn_matrix2_construct( A.n[1][1]/det, -A.n[0][1]/det,
+ -A.n[1][0]/det, A.n[0][0]/det);
+}
+
+/* Also basically a shorthand */
+static dmnsn_matrix2
+dmnsn_matrix2_negate(dmnsn_matrix2 A)
+{
+ return dmnsn_matrix2_construct(-A.n[0][0], -A.n[0][1],
+ -A.n[1][0], -A.n[1][1]);
+}
+
+/* 2x2 matrix subtraction */
+static dmnsn_matrix2
+dmnsn_matrix2_sub(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs)
+{
+ /* 4 additions */
+ return dmnsn_matrix2_construct(
+ lhs.n[0][0] - rhs.n[0][0], lhs.n[0][1] - rhs.n[0][1],
+ lhs.n[1][0] - rhs.n[1][0], lhs.n[1][1] - rhs.n[1][1]
+ );
+}
+
+/* 2x2 matrix multiplication */
+static dmnsn_matrix2
+dmnsn_matrix2_mul(dmnsn_matrix2 lhs, dmnsn_matrix2 rhs)
+{
+ /* 8 multiplications, 4 additions */
+ return dmnsn_matrix2_construct(
+ lhs.n[0][0]*rhs.n[0][0] + lhs.n[0][1]*rhs.n[1][0],
+ lhs.n[0][0]*rhs.n[0][1] + lhs.n[0][1]*rhs.n[1][1],
+ lhs.n[1][0]*rhs.n[0][0] + lhs.n[1][1]*rhs.n[1][0],
+ lhs.n[1][0]*rhs.n[0][1] + lhs.n[1][1]*rhs.n[1][1]
+ );
+}
+
+/* Invert a matrix, if partitioning failed (|P| == 0) */
+static dmnsn_matrix
+dmnsn_matrix_inverse_generic(dmnsn_matrix A)
{
- dmnsn_matrix adj;
+ /*
+ * Simply form the matrix's adjugate and divide each element by the
+ * determinant as we go. The routine itself has 4 additions and 16 divisions
+ * plus 16 cofactor calculations, giving 144 multiplications, 84 additions,
+ * and 16 divisions.
+ */
+ dmnsn_matrix inv;
double det = 0.0, C;
unsigned int i, j;
+ /* Perform a Laplace expansion along the first row to give us the adjugate's
+ first column and the determinant */
for (j = 0; j < 4; ++j) {
C = dmnsn_matrix_cofactor(A, 0, j);
det += A.n[0][j]*C;
- adj.n[j][0] = C;
+ inv.n[j][0] = C;
}
+ /* Divide the first column by the determinant */
for (j = 0; j < 4; ++j) {
- adj.n[j][0] /= det;
+ inv.n[j][0] /= det;
}
+ /* Find columns 2 through 4 */
for (i = 1; i < 4; ++i) {
for (j = 0; j < 4; ++j) {
- adj.n[j][i] = dmnsn_matrix_cofactor(A, i, j)/det;
+ inv.n[j][i] = dmnsn_matrix_cofactor(A, i, j)/det;
}
}
- return adj;
+ return inv;
}
/* Gives the cofactor at row, col; the determinant of the matrix formed from A
@@ -218,6 +349,8 @@ dmnsn_matrix_inverse(dmnsn_matrix A)
static double
dmnsn_matrix_cofactor(dmnsn_matrix A, unsigned int row, unsigned int col)
{
+ /* Return the cofactor at (row, col) of matrix A. 9 multiplications, 5
+ additions */
double n[9], C;
unsigned int i, j, k = 0;