92 lines
2.5 KiB
Go
92 lines
2.5 KiB
Go
// VectorRotation
|
||
/*
|
||
------------------------------------------------------
|
||
作者 : Black Ghost
|
||
日期 : 2018-12-20
|
||
版本 : 0.0.0
|
||
------------------------------------------------------
|
||
向量在三维空间的旋转
|
||
理论：
|
||
|
||
------------------------------------------------------
|
||
输入 :
|
||
u 初始向量，3x1
|
||
angle 旋转角度，3x1，按绕x、y、z顺序，弧度
|
||
seq 旋转顺序 []int
|
||
输出 :
|
||
sol 解,向量
|
||
err 解出标志：false-未解出或达到边界；
|
||
true-全部解出
|
||
------------------------------------------------------
|
||
*/
|
||
|
||
package goNum
|
||
|
||
import (
|
||
"math"
|
||
)
|
||
|
||
// VectorRotation 向量在三维空间的旋转
|
||
func VectorRotation(u, angle Matrix, seq []int) (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 := ZeroMatrix(3, 1)
|
||
var err bool = false
|
||
//matrix around x
|
||
Rx := 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 := 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 := 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 := IdentityE(3)
|
||
for i := 0; i < 3; i++ {
|
||
switch seq[i] {
|
||
case 1:
|
||
W = DotPruduct(Rx, W)
|
||
case 2:
|
||
W = DotPruduct(Ry, W)
|
||
case 3:
|
||
W = DotPruduct(Rz, W)
|
||
default:
|
||
panic("Error in goNum.VectorRotation: sequence number is not right")
|
||
}
|
||
}
|
||
sol = DotPruduct(W, u)
|
||
err = true
|
||
return sol, err
|
||
}
|