By: Yi-Xin Liu
Re-posted from: http://www.yxliu.group/2020/04/rotation-test
Numerical experiments are carried out to demonstrate the usage as well as serving as a test for the rotation.jl
submodule. This is an appendix to the previous post.
First, we load some necessary packages
1
2
3
4
|
using Revise
using Scattering
using LinearAlgebra
using Test
|
Some Experiments on Rotation Operations
$\vu, \vv, \vw$ are the basis vectors of the UVW internal coordinate system with their components expressed in the reference XYZ coordinate system. Let $\vw$ point to the principle direction of a scatterer.
1
2
3
4
5
6
7
8
9
10
11
|
w = [1.0, 2.0, 3.0]
normalize!(w)
# pick a vector that is not parallel to w
a = [0, 1, 2]
# find u by cross product
u = cross(w, a)
normalize!(u)
# find v perpendicular to both w and u
v = cross(w, u)
normalize!(v)
w
|
1
2
3
4
|
3-element Array{Float64,1}:
0.2672612419124244
0.5345224838248488
0.8017837257372732
|
$\mP$ is the rotation matrix which rotates XYZ coordinate system (basis vectors) to UVW coordinate system: $[\vu\;\vv\;\vw] = [\ve_x\;\ve_y\;\ve_z]\mP$.
1
2
3
4
|
3×3 Array{Float64,2}:
0.408248 0.872872 0.267261
-0.816497 0.218218 0.534522
0.408248 -0.436436 0.801784
|
$\mR$ is the rotation matrix which rotates a vector in XYZ system to UVW coordinate system: $\mR = \mP^{-1} = \mP^T$ and $\mR\vu = [1\;0\;0]^T, \mR\vv = [0\;1\;0]^T, \mR\vw = [0\;0\;1]^T$
1
2
|
@test transpose(P) ≈ inv(P)
R = transpose(P)
|
1
2
3
4
|
3×3 LinearAlgebra.Transpose{Float64,Array{Float64,2}}:
0.408248 -0.816497 0.408248
0.872872 0.218218 -0.436436
0.267261 0.534522 0.801784
|
Verify that $\mR$ indeed transforms basis vectors of the XYZ coordinate system to $\vu, \vv, \vw$, whose components are expressed in the XYZ coordinate system.
1
2
3
|
@test R * u ≈ [1, 0, 0]
@test R * v ≈ [0, 1, 0]
@test R * w ≈ [0, 0, 1]
|
Compute the rotation angle:
1
2
|
θ = acos((tr(R)-1)/2)
rad2deg(θ)
|
Compute the rotation axis expressed in the XYZ system:
1
|
uu = [R[3,2]-R[2,3], R[1,3]-R[3,1], R[2,1]-R[1,2]] / (2sin(θ))
|
1
2
3
4
|
3-element Array{Float64,1}:
0.49700656431759865
0.07216735383016143
0.8647406247345901
|
Alternative way to compute the rotation axis. Note that the result may be different from the one computed above with only a sign.
1
2
3
4
|
vals, vecs = eigen(R)
idx = findfirst(x -> x ≈ one(x), vals)
uu2 = real(vecs[:, idx])
@test uu2 ≈ uu || uu2 ≈ -uu
|
Alternative way to compute the rotation angle
1
2
|
vv = [R[3,2]-R[2,3], R[1,3]-R[3,1], R[2,1]-R[1,2]]
@test asin(norm(vv)/2) ≈ θ
|
A proper rotation matrix should have $\det(\mR) = +1$
1
2
3
4
|
3×3 Array{Float64,2}:
0.408248 0.872872 0.267261
-0.816497 0.218218 0.534522
0.408248 -0.436436 0.801784
|
1
2
3
4
|
3×3 LinearAlgebra.Transpose{Float64,Array{Float64,2}}:
0.408248 0.872872 0.267261
-0.816497 0.218218 0.534522
0.408248 -0.436436 0.801784
|
A proper rotation matrix should be an orthogonal matrix
1
|
@test transpose(R) ≈ inv(R)
|
Convert current rotation matrix representation to Euler angles representation. Convention used: Z1Y2Z3
Procedure:
- Rotate about +z by eta (counter-clockwise in x-y plane),
- Rotate about the former y-axis (which is y’), counter clockwise in x’-z plane. Then,
- Rotate about the former z-axis (which is z’), counter-clockwise in x’-y’ plane.
See wiki page:
- https://en.wikipedia.org/wiki/Rotation_matrix
- https://en.wikipedia.org/wiki/Euler_angles#Intrinsic_rotations
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
α = atan(R[2,3], R[1,3])
β = acos(R[3,3])
γ = atan(R[3,2], -R[3,1])
rad2deg.([α, β, γ])
c1 = cos(α)
c2 = cos(β)
c3 = cos(γ)
s1 = sin(α)
s2 = sin(β)
s3 = sin(γ)
Ra = zero(R)
Ra[1, 1] = c1*c2*c3 - s1*s3
Ra[1, 2] = -c3*s1 - c1*c2*s3
Ra[1, 3] = c1 * s2
Ra[2, 1] = c1*s3 + c2*c3*s1
Ra[2, 2] = c1*c3 - c2*s1*s3
Ra[2, 3] = s1*s2
Ra[3, 1] = -c3*s2
Ra[3, 2] = s2*s3
Ra[3, 3] = c2
@test Ra ≈ R
|
1
2
3
4
|
3-element Array{Float64,1}:
-46.91127686463718
36.69922520048988
116.56505117707799
|
Convention: Z1X2Z3
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
α = atan(R[1,3], -R[2,3])
β = acos(R[3,3])
γ = atan(R[3,1], R[3,2])
rad2deg.([α, β, γ])
c1 = cos(α)
c2 = cos(β)
c3 = cos(γ)
s1 = sin(α)
s2 = sin(β)
s3 = sin(γ)
Rb = zero(R)
Rb[1, 1] = c1*c3 - c2*s1*s3
Rb[1, 2] = -c1*s3 - c2*c3*s1
Rb[1, 3] = s1 * s2
Rb[2, 1] = c3*s1 + c1*c2*s3
Rb[2, 2] = c1*c2*c3 - s1*s3
Rb[2, 3] = -c1*s2
Rb[3, 1] = s2*s3
Rb[3, 2] = c3*s2
Rb[3, 3] = c2
@test Rb ≈ R
|
Convention Z1Y2X3
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
α = atan(R[2,1],R[1,1])
β = asin(-R[3,1])
γ = atan(R[3,2], R[3,3])
rad2deg.([α, β, γ])
c1 = cos(α)
c2 = cos(β)
c3 = cos(γ)
s1 = sin(α)
s2 = sin(β)
s3 = sin(γ)
Z1Y2X3 = zero(R)
Z1Y2X3[1, 1] = c1*c2
Z1Y2X3[1, 2] = c1*s2*s3 - c3*s1
Z1Y2X3[1, 3] = s1*s3 + c1*c3*s2
Z1Y2X3[2, 1] = c2*s1
Z1Y2X3[2, 2] = c1*c3 + s1*s2*s3
Z1Y2X3[2, 3] = c3*s1*s2 - c1*s3
Z1Y2X3[3, 1] = -s2
Z1Y2X3[3, 2] = c2*s3
Z1Y2X3[3, 3] = c2*c3
@test Z1Y2X3 ≈ R
|
Convention: Z1Y2Z3
1
2
3
4
5
6
7
8
9
|
Rp = hcat([-1, 0, 0], [0, 0, 1], [0, 1, 0])
α = atan(Rp[2,3], Rp[1,3])
β = acos(Rp[3,3])
γ = atan(Rp[3,2], -Rp[3,1])
rad2deg.([α, β, γ])
θp = acos((tr(Rp)-1)/2)
up =[Rp[3,2]-Rp[2,3], Rp[1,3]-Rp[3,1], Rp[2,1]-Rp[1,2]]
θpp = asin(norm(up)/2)
|
1
|
rad2deg(θp), rad2deg(θpp), up
|
1
|
(180.0, 0.0, [0, 0, 0])
|
Find rotation axis by diagonalization
1
2
3
|
vals, vecs = eigen(Rp)
idx = findfirst(x -> x ≈ one(x), vals)
uu = real(vecs[:, idx])
|
1
2
3
4
|
3-element Array{Float64,1}:
0.0
0.7071067811865475
0.7071067811865477
|
Testing and Usage of Scattering/rotation.jl
Testing EulerAngle constructors
1
2
|
using StaticArrays: SVector, FieldVector
α, β, γ = π/4, π/3, π/2
|
1
|
(0.7853981633974483, 1.0471975511965976, 1.5707963267948966)
|
Constructors of EulerAngle
. Initialize by a vector
1
|
Scattering.Rotation.EulerAngle{Float64}(0.7853981633974483, 1.0471975511965976, 1.5707963267948966)
|
1
|
Scattering.Rotation.EulerAngle{Float64}(1.3, 0.0, 0.0)
|
Initialize by three angles
1
|
euler = EulerAngle(α, β, γ)
|
1
|
Scattering.Rotation.EulerAngle{Float64}(0.7853981633974483, 1.0471975511965976, 1.5707963267948966)
|
Copy constructor
1
|
Scattering.Rotation.EulerAngle{Float64}(0.7853981633974483, 1.0471975511965976, 1.5707963267948966)
|
Initialize by a static vector
1
2
|
sv = SVector(1.0, 2.0, 3.0)
EulerAngle(sv)
|
1
|
Scattering.Rotation.EulerAngle{Float64}(1.0, 2.0, 3.0)
|
Follow the Z1Y2Z3 convention
1
2
3
4
|
α = atan(v[3], u[3])
β = acos(w[3])
γ = atan(w[2], -w[1])
rad2deg.([α, β, γ])
|
1
2
3
4
|
3-element Array{Float64,1}:
-46.91127686463718
36.69922520048988
116.56505117707799
|
Testing RotationMatrix constructors
Constructor of RotationMatrix
initialized with a matrix
1
2
|
rotmat = RotationMatrix(Vector3D(u), Vector3D(v), Vector3D(w))
@test transpose(RotMatrix(hcat(u,v,w))) ≈ rotmat.R
|
Convert an EulerAngle
instance to a RotationMatrix
instance
1
2
|
rotmat_euler = RotationMatrix(EulerAngle(α, β, γ))
@test rotmat_euler.R ≈ rotmat.R
|
Convert a RotationMatrix
instance to an EulerAngle
instance
1
2
|
euler = EulerAngle(rotmat_euler)
@test [euler.α, euler.β, euler.γ] ≈ [α, β, γ]
|
Testing EulerAxisAngle Constructors
Constructor of AxisAngleRepresentation
initialized with a RotationMatrix
instance: conversion from rotation matrix representation to axis-angle representation
1
|
axisangle = EulerAxisAngle(rotmat)
|
1
|
Scattering.Rotation.EulerAxisAngle{Float64}([0.49700656431759865, 0.07216735383016143, 0.8647406247345901], 1.3550004093288814, [0.0 -0.8647406247345901 0.07216735383016143; 0.8647406247345901 0.0 -0.49700656431759865; -0.07216735383016143 0.49700656431759865 0.0])
|
Convert Euler axis-angle representation to rotation matrix representation
1
2
|
rotmat_axisangle = RotationMatrix(axisangle)
@test rotmat_axisangle.R ≈ rotmat.R atol=1e-15
|
1
2
|
axisangle2 = EulerAxisAngle(axisangle.ω, axisangle.θ)
@test axisangle2.K ≈ axisangle.K
|
1
|
Scattering.Rotation.EulerAngle{Float64}(-0.8187562376025611, 0.6405223126794245, 2.0344439357957027)
|
1
|
Scattering.Rotation.EulerAxisAngle{Float64}([0.49700656431759865, 0.07216735383016143, 0.8647406247345901], 1.3550004093288814, [0.0 -0.8647406247345901 0.07216735383016143; 0.8647406247345901 0.0 -0.49700656431759865; -0.07216735383016143 0.49700656431759865 0.0])
|
1
|
Scattering.Rotation.EulerAxisAngle{Float64}([0.4970065643175987, 0.07216735383016142, 0.86474062473459], 1.3550004093288814, [0.0 -0.86474062473459 0.07216735383016142; 0.86474062473459 0.0 -0.4970065643175987; -0.07216735383016142 0.4970065643175987 0.0])
|
1
|
RotationMatrix(euler) |> EulerAxisAngle
|
1
|
Scattering.Rotation.EulerAxisAngle{Float64}([0.4970065643175987, 0.07216735383016142, 0.86474062473459], 1.3550004093288814, [0.0 -0.86474062473459 0.07216735383016142; 0.86474062473459 0.0 -0.4970065643175987; -0.07216735383016142 0.4970065643175987 0.0])
|
1
|
Scattering.Rotation.EulerAngle{Float64}(-0.8187562376025609, 0.6405223126794247, 2.0344439357957027)
|
1
|
convert(EulerAngle, axisangle)
|
1
|
Scattering.Rotation.EulerAngle{Float64}(-0.8187562376025609, 0.6405223126794247, 2.0344439357957027)
|
1
|
convert(EulerAngle, euler)
|
1
|
Scattering.Rotation.EulerAngle{Float64}(-0.8187562376025611, 0.6405223126794245, 2.0344439357957027)
|
1
|
@test euler == axisangle
|
1
|
@test promote(euler) == rotmat
|
1
|
@test promote(axisangle) == rotmat
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0])
|
1
|
Scattering.Rotation.EulerAngle{Float64}(0.0, 0.0, 0.0)
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([1.0 -0.0 0.0; 0.0 1.0 0.0; -0.0 0.0 1.0])
|
1
|
Scattering.Rotation.EulerAxisAngle{Float64}([1.0, 0.0, 0.0], 0.0, [0.0 -0.0 0.0; 0.0 0.0 -1.0; -0.0 1.0 0.0])
|
1
|
promote(one(axisangle))
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0])
|
Testing inv function
1
|
Scattering.Rotation.RotationMatrix{Float64}([0.408248290463863 -0.816496580927726 0.408248290463863; 0.8728715609439696 0.21821789023599242 -0.4364357804719848; 0.2672612419124244 0.5345224838248488 0.8017837257372732])
|
Test inv
function for RotationMatrix
1
2
|
irot1 = inv(rot1)
@test transpose(rot1.R) ≈ irot1.R
|
1
|
rot2 = EulerAxisAngle(rot1)
|
1
|
Scattering.Rotation.EulerAxisAngle{Float64}([0.49700656431759865, 0.07216735383016143, 0.8647406247345901], 1.3550004093288814, [0.0 -0.8647406247345901 0.07216735383016143; 0.8647406247345901 0.0 -0.49700656431759865; -0.07216735383016143 0.49700656431759865 0.0])
|
Test inv
function for EulerAxisAngle
1
2
|
irot2 = inv(rot2)
@test irot2 == irot1
|
1
|
rot3 = EulerAngle(rot1)
|
1
|
Scattering.Rotation.EulerAngle{Float64}(-0.818756237602561, 0.6405223126794245, 2.0344439357957027)
|
Test inv
function for EulerAngle
1
2
|
irot3 = inv(rot3)
@test irot3 == irot1
|
Testing multiplication of two AbstractRotation instances
Test multiplication of two RotationMatrix
instances.
1
2
3
4
|
R1 = rotmat
M = RotationMatrix(Rp)
R2 = R1 * M
@test R2.R ≈ R1.R*M.R
|
Test multiplication of a EulerAxisAngle
instance and a RotationMatrix
instance.
1
2
3
4
5
|
R1 = rotmat
R1a = EulerAxisAngle(R1)
M = RotationMatrix(Rp)
R2 = R1a * M
@test R2 == R1 * M
|
Test multiplication of a RotationMatrix
instance and a EulerAxisAngle
instance.
1
2
3
4
5
|
R1 = rotmat
M = RotationMatrix(Rp)
Ma = EulerAxisAngle(M)
R2 = R1 * Ma
@test R2 == R1 * M
|
Test multiplication of a EulerAxisAngle
instance and a EulerAxisAngle
instance.
1
2
3
4
5
6
|
R1 = rotmat
R1a = EulerAxisAngle(R1)
M = RotationMatrix(Rp)
Ma = EulerAxisAngle(M)
R2 = R1a * Ma
@test R2 == R1 * M
|
Testing computing power of a AbstractRotation instance
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
|
function pow1(rot::AbstractRotation, n::Integer)
rot = promote(rot)
result = rot
for i in 1:(n-1)
result *= rot
end
result
end
function pow(rot::RotationMatrix, n::Integer)
if n == zero(n)
return one(rot)
elseif n == one(n)
return rot
else
if isodd(n)
return rot * pow(rot, n-1)
else
rothalf = pow(rot, n÷2)
return rothalf * rothalf
end
end
end
pow(rot::AbstractRotation, n::Integer) = pow(promote(rot), n)
|
1
|
pow (generic function with 2 methods)
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0])
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([0.40824829046386313 -0.8164965809277259 0.4082482904638629; 0.8728715609439694 0.21821789023599242 -0.4364357804719848; 0.26726124191242434 0.5345224838248487 0.8017837257372732])
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([-0.4369210333151354 -0.2932896043722907 0.8503418245705543; 0.43018214432212887 -0.8983623348886722 -0.08881687880007882; 0.7899641342195538 0.32699590703939707 0.5186813505672173])
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([-0.20711300761088602 0.7472703153125533 0.6314200487243415; -0.6322711178708509 -0.5947556020638506 0.4964866637886771; 0.7465503570320742 -0.29639981387703873 0.5956590591512387])
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([0.7364715816744584 0.6696830270043788 0.09557328459445946; -0.644577211376976 0.651844177986726 0.3995239494677259; 0.2052555187063243 -0.35584239624735736 0.9117271308201462])
|
1
2
3
4
|
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9} with indices SOneTo(3)×SOneTo(3):
0.910754 -0.404104 0.0850187
0.412606 0.882094 -0.227304
0.0168598 0.242097 0.970106
|
1
2
3
4
|
3×3 StaticArrays.SArray{Tuple{3,3},Float64,2,9} with indices SOneTo(3)×SOneTo(3):
-0.443094 0.414668 0.794807
-0.277188 -0.906518 0.318422
0.852546 -0.0792197 0.516614
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([0.40824829046386313 -0.8164965809277259 0.4082482904638629; 0.8728715609439694 0.21821789023599242 -0.4364357804719848; 0.26726124191242434 0.5345224838248487 0.8017837257372732])
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([-0.4369210333151354 -0.2932896043722907 0.8503418245705543; 0.43018214432212887 -0.8983623348886722 -0.08881687880007882; 0.7899641342195538 0.32699590703939707 0.5186813505672173])
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([-0.20711300761088602 0.7472703153125533 0.6314200487243415; -0.6322711178708509 -0.5947556020638506 0.4964866637886771; 0.7465503570320742 -0.29639981387703873 0.5956590591512387])
|
1
|
Scattering.Rotation.RotationMatrix{Float64}([0.7364715816744584 0.6696830270043788 0.09557328459445946; -0.644577211376976 0.651844177986726 0.3995239494677259; 0.2052555187063243 -0.35584239624735736 0.9117271308201462])
|
1
|
@test pow(euler, 100).R ≈ (euler^100).R
|
Benchmarks
1
2
3
|
using BenchmarkTools
@btime pow1($euler, 1000)
|
1
2
|
142.561 μs (3004 allocations: 172.31 KiB)
Scattering.Rotation.RotationMatrix{Float64}([-0.17617351956438138 0.7712758102440354 0.6116342988556001; -0.6592241548072412 -0.553880454852226 0.50856656934094; 0.7310173764848875 -0.31360814126062625 0.606022713280731])
|
The implementation of pow (or Scattering.^ which is the same as pow here)
is significantly faster than the naive implementation.
1
2
|
2.327 μs (49 allocations: 3.02 KiB)
Scattering.Rotation.RotationMatrix{Float64}([-0.1761735195643819 0.7712758102440348 0.6116342988555798; -0.6592241548072477 -0.5538804548522176 0.5085665693409378; 0.7310173764848691 -0.3136081412606315 0.6060227132807026])
|
Testing applying AbstractRotation instance to a vector
Test multiplication of a RotationMatrix
instance and a Vector
1
2
3
|
M = RotationMatrix(Rp)
v = [1.0, 2.0, 3.0]
@test M*v ≈ M.R * v
|
Test multiplication of a RotationMatrix
instance and a RVector
(SVector
, QVector
)
1
2
3
|
M = RotationMatrix(Rp)
r = RVector([1.0, 2.0, 3.0])
@test M*r ≈ M.R * r
|
Test multiplication of a EulerAxisAngle
instance and a Vector
1
2
3
4
|
M = RotationMatrix(Rp)
Ma = EulerAxisAngle(M)
v = [1.0, 2.0, 3.0]
@test Ma*v ≈ M.R * v
|
Test multiplication of a EulerAxisAngle
instance and a RVector
(SVector
, QVector
)
1
2
3
4
|
M = RotationMatrix(Rp)
Ma = EulerAxisAngle(M)
v = RVector([1.0, 2.0, 3.0])
@test Ma*v ≈ M.R * v
|
Test multiplication of a EulerAngle
instance and a Vector
1
2
3
4
|
M = RotationMatrix(Rp)
Ma = EulerAngle(M)
v = [1.0, 2.0, 3.0]
@test Ma*v ≈ M.R * v
|
Test multiplication of a EulerAngle
instance and a RVector
(SVector
, QVector
)
1
2
3
4
|
M = RotationMatrix(Rp)
Ma = EulerAngle(M)
v = RVector([1.0, 2.0, 3.0])
@test Ma*v ≈ M.R * v
|
Acknowledgements
This work is partially supported by the General Program of the National Natural Science Foundation of China (No. 21873021).
Usage and Testing of rotation.jl was originally published by Yi-Xin Liu at Yi-Xin Liu on April 19, 2020.
Related