Skip to content

Commit

Permalink
Merge pull request #32 from chran554/master
Browse files Browse the repository at this point in the history
Add invert functionality to mat3 (3x3) matrices
  • Loading branch information
ungerik authored Mar 5, 2022
2 parents 77a75db + 1c6717b commit 75e8aac
Show file tree
Hide file tree
Showing 6 changed files with 503 additions and 20 deletions.
5 changes: 5 additions & 0 deletions float64/mat2/mat2.go
Original file line number Diff line number Diff line change
Expand Up @@ -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()[:]
}
Expand Down Expand Up @@ -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]
Expand Down
107 changes: 97 additions & 10 deletions float64/mat3/mat3.go
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
package mat3

import (
"errors"
"fmt"
"math"

Expand All @@ -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{
Expand Down Expand Up @@ -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],
}
}

Expand Down Expand Up @@ -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.
Expand All @@ -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
}
150 changes: 150 additions & 0 deletions float64/mat3/mat3_test.go
Original file line number Diff line number Diff line change
@@ -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)
}
}
10 changes: 7 additions & 3 deletions mat2/mat2.go
Original file line number Diff line number Diff line change
Expand Up @@ -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],
}
}

Expand All @@ -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]
Expand Down
Loading

0 comments on commit 75e8aac

Please sign in to comment.