Add invert functionality to mat3 (3x3) matrices (both float64 and flo…

…at32 version). Most functionality copied from/based on mat4 library. Fix bug in float64 mat3 MulVec3. Add tests for mat3.
ungerik · ungerik · Mar 5, 2022 · Mar 5, 2022 · Mar 5, 2022 · 1c6717b97e701597ecce1dfd5e426ba685b98e3a
commit 1c6717b97e701597ecce1dfd5e426ba685b98e3a
diff --git a/float64/mat2/mat2.go b/float64/mat2/mat2.go
@@ -71,6 +71,7 @@ func (mat *T) Size() int {
 }
 
 // Slice returns the elements of the matrix as slice.
+// The data may be a copy depending on the platform implementation.
 func (mat *T) Slice() []float64 {
 	return mat.Array()[:]
 }
@@ -138,6 +139,10 @@ func (mat *T) TransformVec2(v *vec2.T) {
 	v[0] = x
 }
 
+func (mat *T) Determinant() float64 {
+	return mat[0][0]*mat[1][1] - mat[1][0]*mat[0][1]
+}
+
 // Transpose transposes the matrix.
 func (mat *T) Transpose() *T {
 	temp := mat[0][1]

diff --git a/float64/mat3/mat3.go b/float64/mat3/mat3.go
@@ -2,6 +2,7 @@
 package mat3
 
 import (
+	"errors"
 	"fmt"
 	"math"
 
@@ -14,7 +15,11 @@ import (
 
 var (
 	// Zero holds a zero matrix.
-	Zero = T{}
+	Zero = T{
+		vec3.T{0, 0, 0},
+		vec3.T{0, 0, 0},
+		vec3.T{0, 0, 0},
+	}
 
 	// Ident holds an ident matrix.
 	Ident = T{
@@ -174,12 +179,29 @@ func (mat *T) AssignMat2x2(m *mat2.T) *T {
 	return mat
 }
 
-// MulVec3 multiplies v with T.
+// Mul multiplies every element by f and returns mat.
+func (mat *T) Mul(f float64) *T {
+	for i, col := range mat {
+		for j := range col {
+			mat[i][j] *= f
+		}
+	}
+	return mat
+}
+
+// Muled returns a copy of the matrix with every element multiplied by f.
+func (mat *T) Muled(f float64) T {
+	result := *mat
+	result.Mul(f)
+	return result
+}
+
+// MulVec3 multiplies v with mat and returns a new vector v' = M * v.
 func (mat *T) MulVec3(v *vec3.T) vec3.T {
 	return vec3.T{
 		mat[0][0]*v[0] + mat[1][0]*v[1] + mat[2][0]*v[2],
-		mat[0][1]*v[1] + mat[1][1]*v[1] + mat[2][1]*v[2],
-		mat[0][2]*v[2] + mat[1][2]*v[1] + mat[2][2]*v[2],
+		mat[0][1]*v[0] + mat[1][1]*v[1] + mat[2][1]*v[2],
+		mat[0][2]*v[0] + mat[1][2]*v[1] + mat[2][2]*v[2],
 	}
 }
 
@@ -354,12 +376,15 @@ func (mat *T) ExtractEulerAngles() (yHead, xPitch, zRoll float64) {
 
 // Determinant returns the determinant of the matrix.
 func (mat *T) Determinant() float64 {
-	return mat[0][0]*mat[1][1]*mat[2][2] +
-		mat[1][0]*mat[2][1]*mat[0][2] +
-		mat[2][0]*mat[0][1]*mat[1][2] -
-		mat[2][0]*mat[1][1]*mat[0][2] -
-		mat[1][0]*mat[0][1]*mat[2][2] -
-		mat[0][0]*mat[2][1]*mat[1][2]
+	// | a b c |
+	// | d e f | = det A
+	// | g h i |
+	return mat[0][0]*mat[1][1]*mat[2][2] + // aei
+		mat[1][0]*mat[2][1]*mat[0][2] + // dhc
+		mat[2][0]*mat[0][1]*mat[1][2] - // gbf
+		mat[2][0]*mat[1][1]*mat[0][2] - // gec
+		mat[1][0]*mat[0][1]*mat[2][2] - // dbi
+		mat[0][0]*mat[2][1]*mat[1][2] // ahf
 }
 
 // IsReflective returns true if the matrix can be reflected by a plane.
@@ -380,3 +405,65 @@ func (mat *T) Transpose() *T {
 	swap(&mat[2][1], &mat[1][2])
 	return mat
 }
+
+// Adjugate computes the adjugate of this matrix and returns mat
+func (mat *T) Adjugate() *T {
+	matOrig := *mat
+	for i := 0; i < 3; i++ {
+		for j := 0; j < 3; j++ {
+			// -1 for odd i+j, 1 for even i+j
+			sign := float64(((i+j)%2)*-2 + 1)
+			mat[i][j] = matOrig.maskedBlock(i, j).Determinant() * sign
+		}
+	}
+	return mat.Transpose()
+}
+
+// Adjugated returns an adjugated copy of the matrix.
+func (mat *T) Adjugated() T {
+	result := *mat
+	result.Adjugate()
+	return result
+}
+
+// returns a 3x3 matrix without the i-th column and j-th row
+func (mat *T) maskedBlock(blockI, blockJ int) *mat2.T {
+	var m mat2.T
+	m_i := 0
+	for i := 0; i < 3; i++ {
+		if i == blockI {
+			continue
+		}
+		m_j := 0
+		for j := 0; j < 3; j++ {
+			if j == blockJ {
+				continue
+			}
+			m[m_i][m_j] = mat[i][j]
+			m_j++
+		}
+		m_i++
+	}
+	return &m
+}
+
+// Invert inverts the given matrix. Destructive operation.
+// Does not check if matrix is singular and may lead to strange results!
+func (mat *T) Invert() (*T, error) {
+	initialDet := mat.Determinant()
+	if initialDet == 0 {
+		return &Zero, errors.New("can not create inverted matrix as determinant is 0")
+	}
+
+	mat.Adjugate()
+	mat.Scale(1 / initialDet)
+	return mat, nil
+}
+
+// Inverted inverts a copy of the given matrix.
+// Does not check if matrix is singular and may lead to strange results!
+func (mat *T) Inverted() (T, error) {
+	result := *mat
+	_, err := result.Invert()
+	return result, err
+}
diff --git a/float64/mat3/mat3_test.go b/float64/mat3/mat3_test.go
@@ -0,0 +1,150 @@
+package mat3
+
+import (
+	"math"
+	"testing"
+
+	"github.com/ungerik/go3d/float64/mat2"
+	"github.com/ungerik/go3d/float64/vec2"
+	"github.com/ungerik/go3d/float64/vec3"
+)
+
+const EPSILON = 0.0001
+
+// Some matrices used in multiple tests.
+var (
+	invertableMatrix1    = T{vec3.T{4, -2, 3}, vec3.T{8, -3, 5}, vec3.T{7, -2, 4}}
+	invertedMatrix1      = T{vec3.T{-2, 2, -1}, vec3.T{3, -5, 4}, vec3.T{5, -6, 4}}
+	nonInvertableMatrix1 = T{vec3.T{1, 1, 1}, vec3.T{1, 1, 1}, vec3.T{1, 1, 1}}
+	nonInvertableMatrix2 = T{vec3.T{1, 1, 1}, vec3.T{1, 0, 1}, vec3.T{1, 1, 1}}
+
+	testMatrix1 = T{
+		vec3.T{0.38016528, -0.0661157, -0.008264462},
+		vec3.T{-0.19834709, 0.33884296, -0.08264463},
+		vec3.T{0.11570247, -0.28099173, 0.21487603},
+	}
+
+	testMatrix2 = T{
+		vec3.T{23, -4, -0.5},
+		vec3.T{-12, 20.5, -5},
+		vec3.T{7, -17, 13},
+	}
+
+	row123Changed, _ = Parse("3 1 0.5   2 5 2   1 6 7")
+)
+
+func TestDeterminant_2(t *testing.T) {
+	detTwo := Ident
+	detTwo[0][0] = 2
+	if det := detTwo.Determinant(); det != 2 {
+		t.Errorf("Wrong determinant: %f", det)
+	}
+}
+
+func TestDeterminant_3(t *testing.T) {
+	scale2 := Ident.Scaled(2)
+	if det := scale2.Determinant(); det != 2*2*2*1 {
+		t.Errorf("Wrong determinant: %f", det)
+	}
+}
+
+func TestDeterminant_4(t *testing.T) {
+	row1changed, _ := Parse("3 0 0   2 2 0   1 0 2")
+	if det := row1changed.Determinant(); det != 12 {
+		t.Errorf("Wrong determinant: %f", det)
+	}
+}
+
+func TestDeterminant_5(t *testing.T) {
+	row12changed, _ := Parse("3 1 0   2 5 0   1 6 2")
+	if det := row12changed.Determinant(); det != 26 {
+		t.Errorf("Wrong determinant: %f", det)
+	}
+}
+
+func TestDeterminant_7(t *testing.T) {
+	randomMatrix, err := Parse("0.43685 0.81673 0.63721   0.16600 0.40608 0.53479   0.37328 0.36436 0.56356")
+	randomMatrix.Transpose()
+	if err != nil {
+		t.Errorf("Could not parse random matrix: %v", err)
+	}
+	if det := randomMatrix.Determinant(); practicallyEqual(det, 0.043437) {
+		t.Errorf("Wrong determinant for random sub 3x3 matrix: %f", det)
+	}
+}
+
+func practicallyEqual(value1 float64, value2 float64) bool {
+	return math.Abs(value1-value2) > EPSILON
+}
+
+func TestDeterminant_6(t *testing.T) {
+	row123changed := row123Changed
+	if det := row123changed.Determinant(); det != 60.500 {
+		t.Errorf("Wrong determinant for 3x3 matrix: %f", det)
+	}
+}
+
+func TestDeterminant_1(t *testing.T) {
+	detId := Ident.Determinant()
+	if detId != 1 {
+		t.Errorf("Wrong determinant for identity matrix: %f", detId)
+	}
+}
+
+func TestMaskedBlock(t *testing.T) {
+	m := row123Changed
+	blockedExpected := mat2.T{vec2.T{5, 2}, vec2.T{6, 7}}
+	if blocked := m.maskedBlock(0, 0); *blocked != blockedExpected {
+		t.Errorf("Did not block 0,0 correctly: %#v", blocked)
+	}
+}
+
+func TestAdjugate(t *testing.T) {
+	adj := row123Changed
+	adj.Adjugate()
+	// Computed in octave:
+	adjExpected := T{vec3.T{23, -4, -0.5}, vec3.T{-12, 20.5, -5}, vec3.T{7, -17, 13}}
+	if adj != adjExpected {
+		t.Errorf("Adjugate not computed correctly: %#v", adj)
+	}
+}
+
+func TestInvert_ok(t *testing.T) {
+	inv := invertableMatrix1
+	_, err := inv.Invert()
+
+	if err != nil {
+		t.Error("Inverse not computed correctly", err)
+	}
+
+	invExpected := invertedMatrix1
+	if inv != invExpected {
+		t.Errorf("Inverse not computed correctly: %#v", inv)
+	}
+}
+
+func TestInvert_nok_1(t *testing.T) {
+	inv := nonInvertableMatrix1
+	_, err := inv.Inverted()
+	if err == nil {
+		t.Error("Inverse should not be possible", err)
+	}
+}
+
+func TestInvert_nok_2(t *testing.T) {
+	inv := nonInvertableMatrix2
+	_, err := inv.Inverted()
+	if err == nil {
+		t.Error("Inverse should not be possible", err)
+	}
+}
+
+func BenchmarkAssignMul(b *testing.B) {
+	m1 := testMatrix1
+	m2 := testMatrix2
+	var mMult T
+	b.ResetTimer()
+	for i := 0; i < b.N; i++ {
+		mMult.AssignMul(&m1, &m2)
+	}
+}
diff --git a/mat2/mat2.go b/mat2/mat2.go
@@ -124,10 +124,10 @@ func (mat *T) AssignMul(a, b *T) *T {
 }
 
 // MulVec2 multiplies vec with mat.
-func (mat *T) MulVec2(v *vec2.T) vec2.T {
+func (mat *T) MulVec2(vec *vec2.T) vec2.T {
 	return vec2.T{
-		mat[0][0]*v[0] + mat[1][0]*v[1],
-		mat[0][1]*v[1] + mat[1][1]*v[1],
+		mat[0][0]*vec[0] + mat[1][0]*vec[1],
+		mat[0][1]*vec[1] + mat[1][1]*vec[1],
 	}
 }
 
@@ -139,6 +139,10 @@ func (mat *T) TransformVec2(v *vec2.T) {
 	v[0] = x
 }
 
+func (mat *T) Determinant() float32 {
+	return mat[0][0]*mat[1][1] - mat[1][0]*mat[0][1]
+}
+
 // Transpose transposes the matrix.
 func (mat *T) Transpose() *T {
 	temp := mat[0][1]