diff --git a/spatial/r3/mat.go b/spatial/r3/mat.go index 930ce42e..a701b41e 100644 --- a/spatial/r3/mat.go +++ b/spatial/r3/mat.go @@ -267,3 +267,28 @@ func (m *Mat) Hessian(p, step Vec, field func(Vec) float64) { m.Set(2, 1, fyz) m.Set(2, 2, fzz) } + +// Jacobian sets the receiver to the Jacobian matrix of the vector field at the point p, +// approximated using finite differences with the given step sizes. +// Jacobian expects the field's first order partial +// derivatives are all continuous for correct results. +func (m *Mat) Jacobian(p, step Vec, field func(Vec) Vec) { + dx := Vec{X: step.X} + dy := Vec{Y: step.Y} + dz := Vec{Z: step.Z} + + dfdx := Scale(0.5/step.X, Sub(field(Add(p, dx)), field(Sub(p, dx)))) + dfdy := Scale(0.5/step.Y, Sub(field(Add(p, dy)), field(Sub(p, dy)))) + dfdz := Scale(0.5/step.Z, Sub(field(Add(p, dz)), field(Sub(p, dz)))) + m.Set(0, 0, dfdx.X) + m.Set(0, 1, dfdy.X) + m.Set(0, 2, dfdz.X) + + m.Set(1, 0, dfdx.Y) + m.Set(1, 1, dfdy.Y) + m.Set(1, 2, dfdz.Y) + + m.Set(2, 0, dfdx.Z) + m.Set(2, 1, dfdy.Z) + m.Set(2, 2, dfdz.Z) +} diff --git a/spatial/r3/mat_test.go b/spatial/r3/mat_test.go index 22a1a5ee..5e18d9ad 100644 --- a/spatial/r3/mat_test.go +++ b/spatial/r3/mat_test.go @@ -302,3 +302,24 @@ func TestMatHessian(t *testing.T) { } } } + +func TestMatJacobian(t *testing.T) { + const ( + tol = 1e-5 + h = 8e-4 + ) + step := Vec{X: h, Y: h, Z: h} + rnd := rand.New(rand.NewSource(1)) + for _, test := range vectorFields { + for i := 0; i < 1; i++ { + p := randomVec(rnd) + got := NewMat(nil) + got.Jacobian(p, step, test.field) + want := test.jacobian(p) + if !mat.EqualApprox(got, want, tol) { + t.Errorf("matrices not equal within tol\ngot: %v\nwant: %v", + mat.Formatted(got), mat.Formatted(want)) + } + } + } +} diff --git a/spatial/r3/vector_test.go b/spatial/r3/vector_test.go index 938ee9e1..72023e2b 100644 --- a/spatial/r3/vector_test.go +++ b/spatial/r3/vector_test.go @@ -259,28 +259,47 @@ func TestRotate(t *testing.T) { var vectorFields = []struct { field func(Vec) Vec divergence func(Vec) float64 + jacobian func(Vec) *Mat }{ { field: func(v Vec) Vec { + // (x*y*z, y*z, z*x) return Vec{X: v.X * v.Y * v.Z, Y: v.Y * v.Z, Z: v.Z * v.X} }, divergence: func(v Vec) float64 { return v.X + v.Y*v.Z + v.Z }, + jacobian: func(v Vec) *Mat { + return NewMat([]float64{ + v.Y * v.Z, v.X * v.Z, v.X * v.Y, + 0, v.Z, v.Y, + v.Z, 0, v.X, + }) + }, }, { field: func(v Vec) Vec { + // (x*y*z*cos(y), y*z+sin(x), z*x*sin(y)) sx := math.Sin(v.X) sy, cy := math.Sincos(v.Y) return Vec{ X: v.X * v.Y * v.Z * cy, Y: v.Y*v.Z + sx, - Z: v.Z * v.X / sy, + Z: v.Z * v.X * sy, } }, divergence: func(v Vec) float64 { sy, cy := math.Sincos(v.Y) - return v.X/sy + v.Y*v.Z*cy + v.Z + return v.X*sy + v.Y*v.Z*cy + v.Z + }, + jacobian: func(v Vec) *Mat { + cx := math.Cos(v.X) + sy, cy := math.Sincos(v.Y) + return NewMat([]float64{ + v.Y * v.Z * cy, v.X*v.Z*cy - v.X*v.Y*v.Z*sy, v.X * v.Y * cy, + cx, v.Z, v.Y, + v.Z * sy, v.X * v.Z * cy, v.X * sy, + }) }, }, }