- float scale;
- scale = 3.0 / (in1->m[0][0] * in1->m[0][0]
- + in1->m[0][1] * in1->m[0][1]
- + in1->m[0][2] * in1->m[0][2]
- + in1->m[1][0] * in1->m[1][0]
- + in1->m[1][1] * in1->m[1][1]
- + in1->m[1][2] * in1->m[1][2]
- + in1->m[2][0] * in1->m[2][0]
- + in1->m[2][1] * in1->m[2][1]
- + in1->m[2][2] * in1->m[2][2]);
+ out->m[0][0] = in1->m[0][0];
+ out->m[0][1] = in1->m[1][0];
+ out->m[0][2] = in1->m[2][0];
+ out->m[0][3] = in1->m[3][0];
+ out->m[1][0] = in1->m[0][1];
+ out->m[1][1] = in1->m[1][1];
+ out->m[1][2] = in1->m[2][1];
+ out->m[1][3] = in1->m[3][1];
+ out->m[2][0] = in1->m[0][2];
+ out->m[2][1] = in1->m[1][2];
+ out->m[2][2] = in1->m[2][2];
+ out->m[2][3] = in1->m[3][2];
+ out->m[3][0] = in1->m[0][3];
+ out->m[3][1] = in1->m[1][3];
+ out->m[3][2] = in1->m[2][3];
+ out->m[3][3] = in1->m[3][3];
+}
+
+void Matrix4x4_Transpose3x3 (matrix4x4_t *out, const matrix4x4_t *in1)
+{
+ out->m[0][0] = in1->m[0][0];
+ out->m[0][1] = in1->m[1][0];
+ out->m[0][2] = in1->m[2][0];
+ out->m[1][0] = in1->m[0][1];
+ out->m[1][1] = in1->m[1][1];
+ out->m[1][2] = in1->m[2][1];
+ out->m[2][0] = in1->m[0][2];
+ out->m[2][1] = in1->m[1][2];
+ out->m[2][2] = in1->m[2][2];
+
+ out->m[0][3] = in1->m[0][3];
+ out->m[1][3] = in1->m[1][3];
+ out->m[2][3] = in1->m[2][3];
+ out->m[3][0] = in1->m[0][3];
+ out->m[3][1] = in1->m[1][3];
+ out->m[3][2] = in1->m[2][3];
+ out->m[3][3] = in1->m[3][3];
+}
+
+void Matrix4x4_Invert_Simple (matrix4x4_t *out, const matrix4x4_t *in1)
+{
+ // we only support uniform scaling, so assume the first row is enough
+ // (note the lack of sqrt here, because we're trying to undo the scaling,
+ // this means multiplying by the inverse scale twice - squaring it, which
+ // makes the sqrt a waste of time)
+#if 1
+ double scale = 1.0 / (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]);
+#else
+ double scale = 3.0 / sqrt
+ (in1->m[0][0] * in1->m[0][0] + in1->m[0][1] * in1->m[0][1] + in1->m[0][2] * in1->m[0][2]
+ + in1->m[1][0] * in1->m[1][0] + in1->m[1][1] * in1->m[1][1] + in1->m[1][2] * in1->m[1][2]
+ + in1->m[2][0] * in1->m[2][0] + in1->m[2][1] * in1->m[2][1] + in1->m[2][2] * in1->m[2][2]);
+ scale *= scale;
+#endif
+
+ // invert the rotation by transposing and multiplying by the squared
+ // recipricol of the input matrix scale as described above