mirror of
https://github.com/gonum/gonum.git
synced 2025-10-02 05:52:17 +08:00
58 lines
1.7 KiB
Go
58 lines
1.7 KiB
Go
// Copyright ©2022 The Gonum Authors. All rights reserved.
|
||
// Use of this source code is governed by a BSD-style
|
||
// license that can be found in the LICENSE file.
|
||
|
||
package r3_test
|
||
|
||
import (
|
||
"fmt"
|
||
"math"
|
||
|
||
"gonum.org/v1/gonum/num/quat"
|
||
"gonum.org/v1/gonum/spatial/r3"
|
||
)
|
||
|
||
// slerp returns the spherical interpolation between q0 and q1
|
||
// for t in [0,1]; 0 corresponds to q0 and 1 corresponds to q1.
|
||
func slerp(r0, r1 r3.Rotation, t float64) r3.Rotation {
|
||
q0 := quat.Number(r0)
|
||
q1 := quat.Number(r1)
|
||
// Based on Simo Särkkä "Notes on Quaternions" Eq. 35
|
||
// p(t) = (q1 ∗ q0^−1) ^ t ∗ q0
|
||
// https://users.aalto.fi/~ssarkka/pub/quat.pdf
|
||
q1 = quat.Mul(q1, quat.Inv(q0))
|
||
q1 = quat.PowReal(q1, t)
|
||
return r3.Rotation(quat.Mul(q1, q0))
|
||
}
|
||
|
||
// Spherically interpolate between two quaternions to obtain a rotation.
|
||
func Example_slerp() {
|
||
const steps = 10
|
||
// An initial rotation of pi/4 around the x-axis (45 degrees).
|
||
initialRot := r3.NewRotation(math.Pi/4, r3.Vec{X: 1})
|
||
// Final rotation is pi around the x-axis (180 degrees).
|
||
finalRot := r3.NewRotation(math.Pi, r3.Vec{X: 1})
|
||
// The vector we are rotating is (1, 1, 1).
|
||
// The result should then be (1, -1, -1) when t=1 (finalRot) since we invert the y and z axes.
|
||
v := r3.Vec{X: 1, Y: 1, Z: 1}
|
||
for i := 0.0; i <= steps; i++ {
|
||
t := i / steps
|
||
rotated := slerp(initialRot, finalRot, t).Rotate(v)
|
||
fmt.Printf("%.2f %+.2f\n", t, rotated)
|
||
}
|
||
|
||
// Output:
|
||
//
|
||
// 0.00 {+1.00 -0.00 +1.41}
|
||
// 0.10 {+1.00 -0.33 +1.38}
|
||
// 0.20 {+1.00 -0.64 +1.26}
|
||
// 0.30 {+1.00 -0.92 +1.08}
|
||
// 0.40 {+1.00 -1.14 +0.83}
|
||
// 0.50 {+1.00 -1.31 +0.54}
|
||
// 0.60 {+1.00 -1.40 +0.22}
|
||
// 0.70 {+1.00 -1.41 -0.11}
|
||
// 0.80 {+1.00 -1.34 -0.44}
|
||
// 0.90 {+1.00 -1.21 -0.74}
|
||
// 1.00 {+1.00 -1.00 -1.00}
|
||
}
|