diff --git a/src/angleaxis_types.jl b/src/angleaxis_types.jl index 3c45605..70f34cf 100644 --- a/src/angleaxis_types.jl +++ b/src/angleaxis_types.jl @@ -172,24 +172,24 @@ function (::Type{RV})(aa::AngleAxis) where RV <: RotationVec return RV(aa.theta * aa.axis_x, aa.theta * aa.axis_y, aa.theta * aa.axis_z) end -function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation - return QuatRotation(AngleAxis(rv)) -end - -(::Type{RV})(q::QuatRotation) where {RV <: RotationVec} = RV(AngleAxis(q)) - -function Base.:*(rv::RotationVec{T1}, v::StaticVector{3, T2}) where {T1,T2} +@inline function (::Type{Q})(rv::RotationVec) where Q <: QuatRotation theta = rotation_angle(rv) - if (theta > eps(T1)) # use eps here because we have the 1st order series expansion defined - return AngleAxis(rv) * v + if theta < sqrt(eps(typeof(theta))) + ϕ = theta / π / 2 + sc = sinc(ϕ) / 2 # this form gracefully handles theta = 0 + qx, qy, qz = sc * rv.sx, sc * rv.sy, sc * rv.sz + return Q(cos(theta / 2), qx, qy, qz, false) else - return similar_type(typeof(v), promote_type(T1,T2))( - v[1] + rv.sy * v[3] - rv.sz * v[2], - v[2] + rv.sz * v[1] - rv.sx * v[3], - v[3] + rv.sx * v[2] - rv.sy * v[1]) + s, c = sincos(theta / 2) + sθ = s / theta + return Q(c, sθ * rv.sx, sθ * rv.sy, sθ * rv.sz, false) end end +(::Type{RV})(q::QuatRotation) where {RV <: RotationVec} = RV(AngleAxis(q)) + +Base.:*(rv::RotationVec, v::StaticVector{3}) = QuatRotation(rv) * v + @inline Base.:*(rv::RotationVec, r::Rotation) = QuatRotation(rv) * r @inline Base.:*(rv::RotationVec, r::RotMatrix) = QuatRotation(rv) * r @inline Base.:*(rv::RotationVec, r::MRP) = QuatRotation(rv) * r @@ -203,7 +203,7 @@ end @inline Base.inv(rv::RotationVec) = RotationVec(-rv.sx, -rv.sy, -rv.sz) # rotation properties -@inline rotation_angle(rv::RotationVec) = sqrt(rv.sx * rv.sx + rv.sy * rv.sy + rv.sz * rv.sz) +@inline rotation_angle(rv::RotationVec) = hypot(rv.sx, rv.sy, rv.sz) function rotation_axis(rv::RotationVec) # what should this return for theta = 0? theta = rotation_angle(rv) return (theta > 0 ? SVector(rv.sx / theta, rv.sy / theta, rv.sz / theta) : SVector(one(theta), zero(theta), zero(theta))) diff --git a/test/derivative_tests.jl b/test/derivative_tests.jl index 48e4138..084e8e4 100644 --- a/test/derivative_tests.jl +++ b/test/derivative_tests.jl @@ -180,6 +180,20 @@ using ForwardDiff @test FD_jac ≈ R_jac end end + + @testset "RotationVec near zero" begin + rot(x) = RotationVec(x, 0, 0) + for i = 1:10 + v = randn(SVector{3,Float64}) + rotv(x) = rot(x)*v + drotv(x) = ForwardDiff.derivative(rotv, x) + # The following broken tests will be fixed by https://github.com/JuliaDiff/ForwardDiff.jl/pull/669 + @test_broken drotv(0.0) ≈ [0, -v[3], v[2]] + @test drotv(1e-20) ≈ [0, -v[3], v[2]] + @test_broken ForwardDiff.derivative(drotv, 0.0) ≈ [0, -v[2], -v[3]] + @test ForwardDiff.derivative(drotv, 1e-20) ≈ [0, -v[2], -v[3]] + end + end #= # rotate a point by an MRP @testset "Jacobian (MRP rotation)" begin diff --git a/test/rotation_tests.jl b/test/rotation_tests.jl index c020844..16a3f8e 100644 --- a/test/rotation_tests.jl +++ b/test/rotation_tests.jl @@ -154,8 +154,7 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec, r1 = rand(R) r2 = rand(R) v = @SVector rand(3) - @test inv(r1) == adjoint(r1) - @test inv(r1) == transpose(r1) + @test inv(r1) === adjoint(r1) === transpose(r1) @test inv(r1)*r1 ≈ I @test r1*inv(r1) ≈ I @test r1/r1 ≈ I @@ -334,9 +333,25 @@ all_types = (RotMatrix3, RotMatrix{3}, AngleAxis, RotationVec, end @test norm(rotation_axis(QuatRotation(1.0, 0.0, 0.0, 0.0))) ≈ 1.0 + # TODO RotX, RotXY? end + @testset "RotationVec->QuatRotation, especially near zero" begin + for i = 1:10 + p = rand(RotationVec) + all(iszero, Rotations.params(p)) && continue + @test QuatRotation(p) ≈ QuatRotation(AngleAxis(p)) + end + + @test rotation_angle(RotationVec(0.0, 0.0, 0.0)) ≈ 0.0 + @test rotation_axis(RotationVec(0.0, 0.0, 0.0)) ≈ [1.0, 0.0, 0.0] + @test QuatRotation(RotationVec(0.0, 0.0, 0.0)) == QuatRotation(1.0, 0.0, 0.0, 0.0) + qr = QuatRotation(RotationVec(2e-30, 0.0, 0.0)) + @test qr.w ≈ 1 + @test qr.x ≈ 1e-30 + end + ######################################################################### # Check construction of QuatRotation given two vectors #########################################################################