-
Notifications
You must be signed in to change notification settings - Fork 34
Expand file tree
/
Copy pathmatrix.go
More file actions
78 lines (71 loc) · 4 KB
/
Copy pathmatrix.go
File metadata and controls
78 lines (71 loc) · 4 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
package simplify
type Matrix struct {
x00, x01, x02, x03 float64
x10, x11, x12, x13 float64
x20, x21, x22, x23 float64
x30, x31, x32, x33 float64
}
func (a Matrix) QuadricError(v Vector) float64 {
return (v.X*a.x00*v.X + v.Y*a.x10*v.X + v.Z*a.x20*v.X + a.x30*v.X +
v.X*a.x01*v.Y + v.Y*a.x11*v.Y + v.Z*a.x21*v.Y + a.x31*v.Y +
v.X*a.x02*v.Z + v.Y*a.x12*v.Z + v.Z*a.x22*v.Z + a.x32*v.Z +
v.X*a.x03 + v.Y*a.x13 + v.Z*a.x23 + a.x33)
}
func (a Matrix) QuadricVector() Vector {
b := Matrix{
a.x00, a.x01, a.x02, a.x03,
a.x10, a.x11, a.x12, a.x13,
a.x20, a.x21, a.x22, a.x23,
0, 0, 0, 1,
}
return b.Inverse().MulPosition(Vector{})
}
func (a Matrix) Add(b Matrix) Matrix {
return Matrix{
a.x00 + b.x00, a.x10 + b.x10, a.x20 + b.x20, a.x30 + b.x30,
a.x01 + b.x01, a.x11 + b.x11, a.x21 + b.x21, a.x31 + b.x31,
a.x02 + b.x02, a.x12 + b.x12, a.x22 + b.x22, a.x32 + b.x32,
a.x03 + b.x03, a.x13 + b.x13, a.x23 + b.x23, a.x33 + b.x33,
}
}
func (a Matrix) MulPosition(b Vector) Vector {
x := a.x00*b.X + a.x01*b.Y + a.x02*b.Z + a.x03
y := a.x10*b.X + a.x11*b.Y + a.x12*b.Z + a.x13
z := a.x20*b.X + a.x21*b.Y + a.x22*b.Z + a.x23
return Vector{x, y, z}
}
func (a Matrix) Determinant() float64 {
return (a.x00*a.x11*a.x22*a.x33 - a.x00*a.x11*a.x23*a.x32 +
a.x00*a.x12*a.x23*a.x31 - a.x00*a.x12*a.x21*a.x33 +
a.x00*a.x13*a.x21*a.x32 - a.x00*a.x13*a.x22*a.x31 -
a.x01*a.x12*a.x23*a.x30 + a.x01*a.x12*a.x20*a.x33 -
a.x01*a.x13*a.x20*a.x32 + a.x01*a.x13*a.x22*a.x30 -
a.x01*a.x10*a.x22*a.x33 + a.x01*a.x10*a.x23*a.x32 +
a.x02*a.x13*a.x20*a.x31 - a.x02*a.x13*a.x21*a.x30 +
a.x02*a.x10*a.x21*a.x33 - a.x02*a.x10*a.x23*a.x31 +
a.x02*a.x11*a.x23*a.x30 - a.x02*a.x11*a.x20*a.x33 -
a.x03*a.x10*a.x21*a.x32 + a.x03*a.x10*a.x22*a.x31 -
a.x03*a.x11*a.x22*a.x30 + a.x03*a.x11*a.x20*a.x32 -
a.x03*a.x12*a.x20*a.x31 + a.x03*a.x12*a.x21*a.x30)
}
func (a Matrix) Inverse() Matrix {
m := Matrix{}
r := 1 / a.Determinant()
m.x00 = (a.x12*a.x23*a.x31 - a.x13*a.x22*a.x31 + a.x13*a.x21*a.x32 - a.x11*a.x23*a.x32 - a.x12*a.x21*a.x33 + a.x11*a.x22*a.x33) * r
m.x01 = (a.x03*a.x22*a.x31 - a.x02*a.x23*a.x31 - a.x03*a.x21*a.x32 + a.x01*a.x23*a.x32 + a.x02*a.x21*a.x33 - a.x01*a.x22*a.x33) * r
m.x02 = (a.x02*a.x13*a.x31 - a.x03*a.x12*a.x31 + a.x03*a.x11*a.x32 - a.x01*a.x13*a.x32 - a.x02*a.x11*a.x33 + a.x01*a.x12*a.x33) * r
m.x03 = (a.x03*a.x12*a.x21 - a.x02*a.x13*a.x21 - a.x03*a.x11*a.x22 + a.x01*a.x13*a.x22 + a.x02*a.x11*a.x23 - a.x01*a.x12*a.x23) * r
m.x10 = (a.x13*a.x22*a.x30 - a.x12*a.x23*a.x30 - a.x13*a.x20*a.x32 + a.x10*a.x23*a.x32 + a.x12*a.x20*a.x33 - a.x10*a.x22*a.x33) * r
m.x11 = (a.x02*a.x23*a.x30 - a.x03*a.x22*a.x30 + a.x03*a.x20*a.x32 - a.x00*a.x23*a.x32 - a.x02*a.x20*a.x33 + a.x00*a.x22*a.x33) * r
m.x12 = (a.x03*a.x12*a.x30 - a.x02*a.x13*a.x30 - a.x03*a.x10*a.x32 + a.x00*a.x13*a.x32 + a.x02*a.x10*a.x33 - a.x00*a.x12*a.x33) * r
m.x13 = (a.x02*a.x13*a.x20 - a.x03*a.x12*a.x20 + a.x03*a.x10*a.x22 - a.x00*a.x13*a.x22 - a.x02*a.x10*a.x23 + a.x00*a.x12*a.x23) * r
m.x20 = (a.x11*a.x23*a.x30 - a.x13*a.x21*a.x30 + a.x13*a.x20*a.x31 - a.x10*a.x23*a.x31 - a.x11*a.x20*a.x33 + a.x10*a.x21*a.x33) * r
m.x21 = (a.x03*a.x21*a.x30 - a.x01*a.x23*a.x30 - a.x03*a.x20*a.x31 + a.x00*a.x23*a.x31 + a.x01*a.x20*a.x33 - a.x00*a.x21*a.x33) * r
m.x22 = (a.x01*a.x13*a.x30 - a.x03*a.x11*a.x30 + a.x03*a.x10*a.x31 - a.x00*a.x13*a.x31 - a.x01*a.x10*a.x33 + a.x00*a.x11*a.x33) * r
m.x23 = (a.x03*a.x11*a.x20 - a.x01*a.x13*a.x20 - a.x03*a.x10*a.x21 + a.x00*a.x13*a.x21 + a.x01*a.x10*a.x23 - a.x00*a.x11*a.x23) * r
m.x30 = (a.x12*a.x21*a.x30 - a.x11*a.x22*a.x30 - a.x12*a.x20*a.x31 + a.x10*a.x22*a.x31 + a.x11*a.x20*a.x32 - a.x10*a.x21*a.x32) * r
m.x31 = (a.x01*a.x22*a.x30 - a.x02*a.x21*a.x30 + a.x02*a.x20*a.x31 - a.x00*a.x22*a.x31 - a.x01*a.x20*a.x32 + a.x00*a.x21*a.x32) * r
m.x32 = (a.x02*a.x11*a.x30 - a.x01*a.x12*a.x30 - a.x02*a.x10*a.x31 + a.x00*a.x12*a.x31 + a.x01*a.x10*a.x32 - a.x00*a.x11*a.x32) * r
m.x33 = (a.x01*a.x12*a.x20 - a.x02*a.x11*a.x20 + a.x02*a.x10*a.x21 - a.x00*a.x12*a.x21 - a.x01*a.x10*a.x22 + a.x00*a.x11*a.x22) * r
return m
}