Skip to content

Commit 75c98e3

Browse files
hyrodiumhyrodium
authored
Add more elementary functions (#266)
* rename logexp to elementary_functions * add sqrt method * fix sqrt method * add cbrt method * add power methods * add tests for sqrt * fix typos * comment out some `cbrt` methods that requires `cbrt(::Quaternion)` * add tests for cbrt * add tests for power(`^`) * add tests for general dimensions * fix methods for general dimensions
1 parent 4240829 commit 75c98e3

File tree

8 files changed

+272
-94
lines changed

8 files changed

+272
-94
lines changed

‎src/Rotations.jl

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ include("rodrigues_params.jl")
2323
include("error_maps.jl")
2424
include("rotation_error.jl")
2525
include("rotation_generator.jl")
26-
include("logexp.jl")
26+
include("elementary_functions.jl")
2727
include("eigen.jl")
2828
include("rand.jl")
2929
include("rotation_between.jl")

‎src/angleaxis_types.jl

Lines changed: 0 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -125,9 +125,6 @@ end
125125
@inline Base.:*(aa1::AngleAxis, aa2::AngleAxis) = QuatRotation(aa1) * QuatRotation(aa2)
126126

127127
@inline Base.inv(aa::AngleAxis) = AngleAxis(-aa.theta, aa.axis_x, aa.axis_y, aa.axis_z)
128-
@inline Base.:^(aa::AngleAxis, t::Real) = AngleAxis(aa.theta*t, aa.axis_x, aa.axis_y, aa.axis_z)
129-
@inline Base.:^(aa::AngleAxis, t::Integer) = AngleAxis(aa.theta*t, aa.axis_x, aa.axis_y, aa.axis_z) # to avoid ambiguity
130-
131128

132129
# define identity rotations for convenience
133130
@inline Base.one(::Type{AngleAxis}) = AngleAxis(0.0, 1.0, 0.0, 0.0)
@@ -204,8 +201,6 @@ end
204201
@inline Base.:*(rv1::RotationVec, rv2::RotationVec) = QuatRotation(rv1) * QuatRotation(rv2)
205202

206203
@inline Base.inv(rv::RotationVec) = RotationVec(-rv.sx, -rv.sy, -rv.sz)
207-
@inline Base.:^(rv::RotationVec, t::Real) = RotationVec(rv.sx*t, rv.sy*t, rv.sz*t)
208-
@inline Base.:^(rv::RotationVec, t::Integer) = RotationVec(rv.sx*t, rv.sy*t, rv.sz*t) # to avoid ambiguity
209204

210205
# rotation properties
211206
@inline rotation_angle(rv::RotationVec) = sqrt(rv.sx * rv.sx + rv.sy * rv.sy + rv.sz * rv.sz)

‎src/core_types.jl

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -175,8 +175,6 @@ end
175175
end
176176

177177
Base.:*(r1::Angle2d, r2::Angle2d) = Angle2d(r1.theta + r2.theta)
178-
Base.:^(r::Angle2d, t::Real) = Angle2d(r.theta*t)
179-
Base.:^(r::Angle2d, t::Integer) = Angle2d(r.theta*t)
180178
Base.inv(r::Angle2d) = Angle2d(-r.theta)
181179

182180
@inline function Base.getindex(r::Angle2d, i::Int)

‎src/elementary_functions.jl

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
## log
2+
# 2d
3+
Base.log(R::Angle2d) = Angle2dGenerator(R.theta)
4+
Base.log(R::RotMatrix{2}) = RotMatrixGenerator(log(Angle2d(R)))
5+
#= We can define log for Rotation{2} like this,
6+
but the subtypes of Rotation{2} are only Angle2d and RotMatrix{2},
7+
so we don't need this defnition. =#
8+
# Base.log(R::Rotation{2}) = log(Angle2d(R))
9+
10+
# 3d
11+
Base.log(R::RotationVec) = RotationVecGenerator(R.sx,R.sy,R.sz)
12+
Base.log(R::RotMatrix{3}) = RotMatrixGenerator(log(RotationVec(R)))
13+
Base.log(R::Rotation{3}) = log(RotationVec(R))
14+
15+
# General dimensions
16+
function Base.log(R::RotMatrix{N}) where N
17+
# This will be faster when log(::SMatrix) is implemented in StaticArrays.jl
18+
@static if VERSION < v"1.7"
19+
# This if block is related to this PR.
20+
# https://github.com/JuliaLang/julia/pull/40573
21+
S = SMatrix{N,N}(real(log(Matrix(R))))
22+
else
23+
S = SMatrix{N,N}(log(Matrix(R)))
24+
end
25+
RotMatrixGenerator((S-S')/2)
26+
end
27+
28+
## exp
29+
# 2d
30+
Base.exp(R::Angle2dGenerator) = Angle2d(R.v)
31+
Base.exp(R::RotMatrixGenerator{2}) = RotMatrix(exp(Angle2dGenerator(R)))
32+
# Same as log(R::Rotation{2}), this definition is not necessary until someone add a new subtype of RotationGenerator{2}.
33+
# Base.exp(R::RotationGenerator{2}) = exp(Angle2dGenerator(R))
34+
35+
# 3d
36+
Base.exp(R::RotationVecGenerator) = RotationVec(R.x,R.y,R.z)
37+
Base.exp(R::RotMatrixGenerator{3}) = RotMatrix(exp(RotationVecGenerator(R)))
38+
# Same as log(R::Rotation{2}), this definition is not necessary until someone add a new subtype of RotationGenerator{3}.
39+
# Base.exp(R::RotationGenerator{3}) = exp(RotationVecGenerator(R))
40+
41+
# General dimensions
42+
Base.exp(R::RotMatrixGenerator{N}) where N = RotMatrix(exp(SMatrix(R)))
43+
44+
## sqrt
45+
# 2d
46+
Base.sqrt(r::Angle2d) = Angle2d(r.theta/2)
47+
Base.sqrt(r::RotMatrix{2}) = RotMatrix(sqrt(Angle2d(r)))
48+
49+
# 3d
50+
Base.sqrt(r::RotX) = RotX(r.theta/2)
51+
Base.sqrt(r::RotY) = RotY(r.theta/2)
52+
Base.sqrt(r::RotZ) = RotZ(r.theta/2)
53+
Base.sqrt(r::AngleAxis) = AngleAxis(r.theta/2, r.axis_x, r.axis_y, r.axis_z)
54+
Base.sqrt(r::RotationVec) = RotationVec(r.sx/2, r.sy/2, r.sz/2)
55+
Base.sqrt(r::QuatRotation) = QuatRotation(sqrt(r.q))
56+
Base.sqrt(r::RotMatrix{3}) = RotMatrix{3}(sqrt(QuatRotation(r)))
57+
Base.sqrt(r::RodriguesParam) = RodriguesParam(sqrt(QuatRotation(r)))
58+
Base.sqrt(r::MRP) = MRP(sqrt(QuatRotation(r)))
59+
Base.sqrt(r::Rotation{3}) = sqrt(QuatRotation(r))
60+
61+
# General dimensions
62+
Base.sqrt(r::Rotation{N}) where N = RotMatrix(sqrt(SMatrix(r)))
63+
64+
## cbrt
65+
# 2d
66+
Base.cbrt(r::Angle2d) = Angle2d(r.theta/3)
67+
Base.cbrt(r::RotMatrix{2}) = RotMatrix(cbrt(Angle2d(r)))
68+
69+
# 3d
70+
Base.cbrt(r::RotX) = RotX(r.theta/3)
71+
Base.cbrt(r::RotY) = RotY(r.theta/3)
72+
Base.cbrt(r::RotZ) = RotZ(r.theta/3)
73+
Base.cbrt(r::AngleAxis) = AngleAxis(r.theta/3, r.axis_x, r.axis_y, r.axis_z)
74+
Base.cbrt(r::RotationVec) = RotationVec(r.sx/3, r.sy/3, r.sz/3)
75+
# We can implement these `cbrt` methods when https://github.com/JuliaLang/julia/issues/36534 is resolved.
76+
# Base.cbrt(r::QuatRotation) = QuatRotation(cbrt(r.q))
77+
# Base.cbrt(r::RotMatrix{3}) = RotMatrix{3}(cbrt(QuatRotation(r)))
78+
# Base.cbrt(r::RodriguesParam) = RodriguesParam(cbrt(QuatRotation(r)))
79+
# Base.cbrt(r::MRP) = MRP(cbrt(QuatRotation(r)))
80+
# Base.cbrt(r::Rotation{3}) = cbrt(QuatRotation(r))
81+
82+
# General dimensions
83+
Base.cbrt(r::Rotation{N}) where N = exp(log(r)/3)
84+
85+
## power
86+
# 2d
87+
Base.:^(r::Angle2d, p::Real) = Angle2d(r.theta*p)
88+
Base.:^(r::Angle2d, p::Integer) = Angle2d(r.theta*p)
89+
Base.:^(r::RotMatrix{2}, p::Real) = RotMatrix(Angle2d(r)^p)
90+
Base.:^(r::RotMatrix{2}, p::Integer) = RotMatrix(Angle2d(r)^p)
91+
92+
# 3d
93+
Base.:^(r::RotX, p::Real) = RotX(r.theta*p)
94+
Base.:^(r::RotX, p::Integer) = RotX(r.theta*p)
95+
Base.:^(r::RotY, p::Real) = RotY(r.theta*p)
96+
Base.:^(r::RotY, p::Integer) = RotY(r.theta*p)
97+
Base.:^(r::RotZ, p::Real) = RotZ(r.theta*p)
98+
Base.:^(r::RotZ, p::Integer) = RotZ(r.theta*p)
99+
Base.:^(r::AngleAxis, p::Real) = AngleAxis(r.theta*p, r.axis_x, r.axis_y, r.axis_z)
100+
Base.:^(r::AngleAxis, p::Integer) = AngleAxis(r.theta*p, r.axis_x, r.axis_y, r.axis_z)
101+
Base.:^(r::RotationVec, p::Real) = RotationVec(r.sx*p, r.sy*p, r.sz*p)
102+
Base.:^(r::RotationVec, p::Integer) = RotationVec(r.sx*p, r.sy*p, r.sz*p)
103+
Base.:^(r::QuatRotation, p::Real) = QuatRotation((r.q)^p)
104+
Base.:^(r::QuatRotation, p::Integer) = QuatRotation((r.q)^p)
105+
Base.:^(r::RotMatrix{3}, p::Real) = RotMatrix{3}(QuatRotation(r)^p)
106+
Base.:^(r::RotMatrix{3}, p::Integer) = RotMatrix{3}(QuatRotation(r)^p)
107+
Base.:^(r::RodriguesParam, p::Real) = RodriguesParam(QuatRotation(r)^p)
108+
Base.:^(r::RodriguesParam, p::Integer) = RodriguesParam(QuatRotation(r)^p)
109+
Base.:^(r::MRP, p::Real) = MRP(QuatRotation(r)^p)
110+
Base.:^(r::MRP, p::Integer) = MRP(QuatRotation(r)^p)
111+
Base.:^(r::Rotation{3}, p::Real) = QuatRotation(r)^p
112+
Base.:^(r::Rotation{3}, p::Integer) = QuatRotation(r)^p
113+
114+
# General dimensions
115+
Base.:^(r::Rotation{N}, p::Real) where N = exp(log(r)*p)
116+
# There is the same implementation of ^(r::Rotation{N}, p::Integer) as in Base
117+
Base.:^(A::Rotation{N}, p::Integer) where N = p < 0 ? Base.power_by_squaring(inv(A), -p) : Base.power_by_squaring(A, p)

‎src/logexp.jl

Lines changed: 0 additions & 42 deletions
This file was deleted.

‎test/elementary_functions.jl

Lines changed: 153 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,153 @@
1+
@testset "log" begin
2+
all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec,
3+
QuatRotation, RodriguesParam, MRP,
4+
RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX,
5+
RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ,
6+
RotX, RotY, RotZ,
7+
RotXY, RotYZ, RotZX, RotXZ, RotYX, RotZY,
8+
RotMatrix2, RotMatrix{2}, Angle2d)
9+
10+
@testset "$(T)" for T in all_types, F in (one, rand)
11+
R = F(T)
12+
@test R ≈ exp(log(R))
13+
@test log(R) isa RotationGenerator
14+
@test exp(log(R)) isa Rotation
15+
end
16+
17+
@testset "$(N)-dim" for N in 1:5
18+
M = @SMatrix rand(N,N)
19+
R = nearest_rotation(M)
20+
@test isrotationgenerator(log(R))
21+
@test log(R) isa RotMatrixGenerator
22+
@test exp(log(R)) isa RotMatrix
23+
end
24+
end
25+
26+
@testset "exp(zero)" begin
27+
all_types = (RotMatrixGenerator{3}, RotationVecGenerator,
28+
RotMatrixGenerator{2}, Angle2dGenerator)
29+
30+
@testset "$(T)" for T in all_types
31+
r = zero(T)
32+
@test one(exp(r)) ≈ exp(r)
33+
@test exp(r) isa Rotation
34+
end
35+
end
36+
37+
@testset "exp(::RotMatrixGenerator)" begin
38+
for N in 2:3
39+
r = zero(RotMatrixGenerator{N})
40+
@test r isa RotMatrixGenerator{N}
41+
@test exp(r) isa RotMatrix{N}
42+
end
43+
end
44+
45+
@testset "sqrt" begin
46+
all_types = (
47+
RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec,
48+
QuatRotation, RodriguesParam, MRP,
49+
RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX,
50+
RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ,
51+
RotX, RotY, RotZ,
52+
RotXY, RotYZ, RotZX, RotXZ, RotYX, RotZY,
53+
RotMatrix2, RotMatrix{2}, Angle2d
54+
)
55+
56+
compat_types = (
57+
RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec,
58+
QuatRotation, RodriguesParam, MRP,
59+
RotX, RotY, RotZ,
60+
RotMatrix2, RotMatrix{2}, Angle2d
61+
)
62+
63+
@testset "$(T)" for T in all_types, F in (one, rand)
64+
R = F(T)
65+
@test R ≈ sqrt(R) * sqrt(R)
66+
@test sqrt(R) isa Rotation
67+
end
68+
69+
@testset "$(T)-compat" for T in compat_types
70+
R = one(T)
71+
@test sqrt(R) isa T
72+
end
73+
74+
@testset "$(T)-noncompat3d" for T in setdiff(all_types, compat_types)
75+
R = one(T)
76+
@test sqrt(R) isa QuatRotation
77+
end
78+
79+
@testset "$(N)-dim" for N in 1:5
80+
M = @SMatrix rand(N,N)
81+
R = nearest_rotation(M)
82+
@test R ≈ sqrt(R) * sqrt(R)
83+
@test sqrt(R) isa RotMatrix{N}
84+
end
85+
end
86+
87+
@testset "cbrt" begin
88+
supported_types = (
89+
AngleAxis, RotationVec,
90+
RotX, RotY, RotZ,
91+
RotMatrix2, RotMatrix{2}, Angle2d
92+
)
93+
94+
@testset "$(T)" for T in supported_types, F in (one, rand)
95+
R = F(T)
96+
@test R ≈ cbrt(R) * cbrt(R) * cbrt(R)
97+
@test cbrt(R) isa Rotation
98+
end
99+
100+
@testset "$(N)-dim" for N in 1:5
101+
M = @SMatrix rand(N,N)
102+
R = nearest_rotation(M)
103+
@test R ≈ cbrt(R) * cbrt(R) * cbrt(R)
104+
@test cbrt(R) isa RotMatrix{N}
105+
end
106+
end
107+
108+
@testset "power" begin
109+
all_types = (
110+
RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec,
111+
QuatRotation, RodriguesParam, MRP,
112+
RotXYZ, RotYZX, RotZXY, RotXZY, RotYXZ, RotZYX,
113+
RotXYX, RotYZY, RotZXZ, RotXZX, RotYXY, RotZYZ,
114+
RotX, RotY, RotZ,
115+
RotXY, RotYZ, RotZX, RotXZ, RotYX, RotZY,
116+
RotMatrix2, RotMatrix{2}, Angle2d
117+
)
118+
119+
compat_types = (
120+
RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec,
121+
QuatRotation, RodriguesParam, MRP,
122+
RotX, RotY, RotZ,
123+
RotMatrix2, RotMatrix{2}, Angle2d
124+
)
125+
126+
@testset "$(T)" for T in all_types, F in (one, rand)
127+
R = F(T)
128+
@test R^2 ≈ R * R
129+
@test R^1.5 ≈ sqrt(R) * sqrt(R) * sqrt(R)
130+
@test R isa Rotation
131+
end
132+
133+
@testset "$(T)-compat" for T in compat_types
134+
R = one(T)
135+
@test R^2 isa T
136+
@test R^1.5 isa T
137+
end
138+
139+
@testset "$(T)-noncompat3d" for T in setdiff(all_types, compat_types)
140+
R = one(T)
141+
@test R^2 isa QuatRotation
142+
@test R^1.5 isa QuatRotation
143+
end
144+
145+
@testset "$(N)-dim" for N in 1:5
146+
M = @SMatrix rand(N,N)
147+
R = nearest_rotation(M)
148+
@test R^2 ≈ R * R
149+
@test R^1.5 ≈ sqrt(R) * sqrt(R) * sqrt(R)
150+
@test R^2 isa RotMatrix{N}
151+
@test R^1.5 isa RotMatrix{N}
152+
end
153+
end

‎test/logexp.jl

Lines changed: 0 additions & 43 deletions
This file was deleted.

0 commit comments

Comments
 (0)