mirror of
https://gitee.com/chfenger/goNum.git
synced 2025-12-06 16:49:24 +08:00
e9ac846d52
update comments in all of files in convenient to generate package information using `godoc` command or give tips in LiteIDE editor when the cursor keeps resting on a function or method.
104 lines
2.9 KiB
Go
104 lines
2.9 KiB
Go
// VectorRotation_test
|
||
/*
|
||
------------------------------------------------------
|
||
作者 : Black Ghost
|
||
日期 : 2018-12-20
|
||
版本 : 0.0.0
|
||
------------------------------------------------------
|
||
向量在三维空间的旋转
|
||
理论：
|
||
|
||
------------------------------------------------------
|
||
输入 :
|
||
u 初始向量，3x1
|
||
angle 旋转角度，3x1，按绕x、y、z顺序，弧度
|
||
seq 旋转顺序 []int
|
||
输出 :
|
||
sol 解,向量
|
||
err 解出标志：false-未解出或达到边界；
|
||
true-全部解出
|
||
------------------------------------------------------
|
||
*/
|
||
|
||
package goNum_test
|
||
|
||
import (
|
||
"math"
|
||
"testing"
|
||
|
||
"github.com/chfenger/goNum"
|
||
)
|
||
|
||
// VectorRotation 向量在三维空间的旋转
|
||
func VectorRotation(u, angle goNum.Matrix, seq []int) (goNum.Matrix, bool) {
|
||
/*
|
||
向量在三维空间的旋转
|
||
输入 :
|
||
u 初始向量，3x1
|
||
angle 旋转角度，3x1，按绕x、y、z顺序，弧度
|
||
seq 旋转顺序 []int
|
||
输出 :
|
||
sol 解,向量
|
||
err 解出标志：false-未解出或达到边界；
|
||
true-全部解出
|
||
*/
|
||
//向量大小
|
||
if u.Rows != 3 {
|
||
panic("Error in goNum.VectorRotation: Vector length is not right")
|
||
}
|
||
if angle.Rows != 3 {
|
||
panic("Error in goNum.VectorRotation: angles number is not right")
|
||
}
|
||
if len(seq) != 3 {
|
||
panic("Error in goNum.VectorRotation: seq length is not right")
|
||
}
|
||
//判断角度大小
|
||
for i := 0; i < 3; i++ {
|
||
if angle.Data[i] > math.Pi/2.0 {
|
||
panic("Error in goNum.VectorRotation: angle value is not right")
|
||
}
|
||
}
|
||
sol := goNum.ZeroMatrix(3, 1)
|
||
var err bool = false
|
||
//matrix around x
|
||
Rx := goNum.NewMatrix(3, 3, []float64{
|
||
1.0, 0.0, 0.0,
|
||
0.0, math.Cos(angle.Data[0]), -1.0 * math.Sin(angle.Data[0]),
|
||
0.0, math.Sin(angle.Data[0]), math.Cos(angle.Data[0])})
|
||
//matrix around y
|
||
Ry := goNum.NewMatrix(3, 3, []float64{
|
||
math.Cos(angle.Data[1]), 0.0, math.Sin(angle.Data[1]),
|
||
0.0, 1.0, 0.0,
|
||
-1.0 * math.Sin(angle.Data[1]), 0.0, math.Cos(angle.Data[1])})
|
||
//matrix around z
|
||
Rz := goNum.NewMatrix(3, 3, []float64{
|
||
math.Cos(angle.Data[2]), -1.0 * math.Sin(angle.Data[2]), 0.0,
|
||
math.Sin(angle.Data[2]), math.Cos(angle.Data[2]), 0.0,
|
||
0.0, 0.0, 1.0})
|
||
W := goNum.IdentityE(3)
|
||
for i := 0; i < 3; i++ {
|
||
switch seq[i] {
|
||
case 1:
|
||
W = goNum.DotPruduct(Rx, W)
|
||
case 2:
|
||
W = goNum.DotPruduct(Ry, W)
|
||
case 3:
|
||
W = goNum.DotPruduct(Rz, W)
|
||
default:
|
||
panic("Error in goNum.VectorRotation: sequence number is not right")
|
||
}
|
||
}
|
||
sol = goNum.DotPruduct(W, u)
|
||
err = true
|
||
return sol, err
|
||
}
|
||
|
||
func BenchmarkVectorRotation(b *testing.B) {
|
||
u45 := goNum.NewMatrix(3, 1, []float64{1.0, 0.0, 0.0})
|
||
angle45 := goNum.NewMatrix(3, 1, []float64{math.Pi / 2.0, 0.0, math.Pi / 2.0})
|
||
seq45 := []int{2, 3, 1}
|
||
for i := 0; i < b.N; i++ {
|
||
goNum.VectorRotation(u45, angle45, seq45) //{0,0,1}
|
||
}
|
||
}
|