Fix angle_axis_proper (...), add test.

src/spatial.jl

@@ -13,7 +13,7 @@ end
 
 function convert{T}(::Type{SMatrix{6, 6, T}}, inertia::SpatialInertia)
     J = inertia.moment
-    C = vector_to_skew_symmetric(inertia.crossPart)
+    C = hat(inertia.crossPart)
     m = inertia.mass
     [J  C; C' m * eye(SMatrix{3, 3, T})]
 end
@@ -45,31 +45,6 @@ function (+){T}(inertia1::SpatialInertia{T}, inertia2::SpatialInertia{T})
     SpatialInertia(inertia1.frame, moment, crossPart, mass)
 end
 
-function vector_to_skew_symmetric{T}(v::SVector{3, T})
-    @SMatrix [zero(T) -v[3] v[2];
-              v[3] zero(T) -v[1];
-              -v[2] v[1] zero(T)]
-end
-
-function cross(a::SVector{3}, B::AbstractMatrix)
-    vector_to_skew_symmetric(a) * B
-end
-
-@inline function vector_to_skew_symmetric_squared(a::SVector{3})
-    aSq1 = a[1] * a[1]
-    aSq2 = a[2] * a[2]
-    aSq3 = a[3] * a[3]
-    b11 = -aSq2 - aSq3
-    b12 = a[1] * a[2]
-    b13 = a[1] * a[3]
-    b22 = -aSq1 - aSq3
-    b23 = a[2] * a[3]
-    b33 = -aSq1 - aSq2
-    @SMatrix [b11 b12 b13;
-              b12 b22 b23;
-              b13 b23 b33]
-end
-
 function transform{I, T}(inertia::SpatialInertia{I}, t::Transform3D{T})::SpatialInertia{promote_type(I, T)}
     framecheck(t.from, inertia.frame)
     S = promote_type(I, T)
@@ -83,13 +58,13 @@ function transform{I, T}(inertia::SpatialInertia{I}, t::Transform3D{T})::Spatial
         m = convert(S, inertia.mass)
         c = convert(SVector{3, S}, inertia.crossPart)
 
-        R = rotationmatrix_normalized_fsa(convert(Quaternion{S}, t.rot))
+        R = rotation_matrix(convert(Quaternion{S}, t.rot))
         p = convert(SVector{3, S}, t.trans)
 
         cnew = R * c
-        Jnew = vector_to_skew_symmetric_squared(cnew)
+        Jnew = hat_squared(cnew)
         cnew += m * p
-        Jnew -= vector_to_skew_symmetric_squared(cnew)
+        Jnew -= hat_squared(cnew)
         mInv = inv(m)
         Jnew *= mInv
         Jnew += R * J * R'
@@ -257,7 +232,7 @@ end
 
 function transform(jac::GeometricJacobian, transform::Transform3D)
     framecheck(jac.frame, transform.from)
-    R = rotationmatrix_normalized_fsa(transform.rot)
+    R = rotation_matrix(transform.rot)
     angular = R * jac.angular
     linear = R * jac.linear + cross(transform.trans, angular)
     GeometricJacobian(jac.body, jac.base, transform.to, angular, linear)
@@ -396,7 +371,7 @@ end
 
 function transform(mat::MomentumMatrix, transform::Transform3D)
     framecheck(mat.frame, transform.from)
-    R = rotationmatrix_normalized_fsa(transform.rot)
+    R = rotation_matrix(transform.rot)
     linear = R * linear_part(mat)
     T = eltype(linear)
     angular = R * angular_part(mat) + cross(transform.trans, linear)

src/third_party_addendum.jl

@@ -1,23 +1,46 @@
-rotate(x::SMatrix{3}, q::Quaternion) = rotationmatrix_normalized_fsa(q) * x
+@inline function vector_to_skew_symmetric{T}(v::SVector{3, T})
+    @SMatrix [zero(T) -v[3] v[2];
+              v[3] zero(T) -v[1];
+              -v[2] v[1] zero(T)]
+end
+
+const hat = vector_to_skew_symmetric
+
+@inline function vector_to_skew_symmetric_squared(a::SVector{3})
+    aSq1 = a[1] * a[1]
+    aSq2 = a[2] * a[2]
+    aSq3 = a[3] * a[3]
+    b11 = -aSq2 - aSq3
+    b12 = a[1] * a[2]
+    b13 = a[1] * a[3]
+    b22 = -aSq1 - aSq3
+    b23 = a[2] * a[3]
+    b33 = -aSq1 - aSq2
+    @SMatrix [b11 b12 b13;
+              b12 b22 b23;
+              b13 b23 b33]
+end
+
+const hat_squared = vector_to_skew_symmetric_squared
+
+function cross(a::SVector{3}, B::AbstractMatrix)
+    hat(a) * B
+end
+
+rotate(x::SMatrix{3}, q::Quaternion) = rotation_matrix(q) * x
 
 function rotate(x::SVector{3}, q::Quaternion)
     qret = q * Quaternion(zero(eltype(x)), x[1], x[2], x[3]) * inv(q)
     SVector(qret.v1, qret.v2, qret.v3)
 end
 
-angle_axis_proper(q::Quaternion) = angle_proper(q), axis_proper(q)
-
-angle_proper(q::Quaternion) = 2 * acos(real(Quaternions.normalize(q)))
-
-function axis_proper(q::Quaternion)
-    q = Quaternions.normalize(q)
-    s = sin(angle(q) / 2)
-        abs(s) > 0 ?
-        [q.v1, q.v2, q.v3] / s :
-        [1.0, 0.0, 0.0]
+function angle_axis_proper{T}(q::Quaternion{T})
+    Θ = 2 * atan2(√(q.v1^2 + q.v2^2 + q.v3^2), q.s)
+    axis = isapprox(Θ, zero(T)) ? SVector{3, T}(1, 0, 0) : SVector{3, T}(q.v1, q.v2, q.v3) / sin(Θ / 2)
+    Θ, axis
 end
 
-function rotationmatrix_normalized_fsa{T}(q::Quaternion{T})
+function rotation_matrix{T}(q::Quaternion{T})
     sx, sy, sz = 2q.s*q.v1, 2q.s*q.v2, 2q.s*q.v3
     xx, xy, xz = 2q.v1^2, 2q.v1*q.v2, 2q.v1*q.v3
     yy, yz, zz = 2q.v2^2, 2q.v2*q.v3, 2q.v3^2

test/runtests.jl

@@ -9,6 +9,7 @@ import ForwardDiff
 # useful utility function for computing time derivatives.
 create_autodiff(x, dx) = [ForwardDiff.Dual(x[i], dx[i]) for i in 1 : length(x)]
 
+include("test_util.jl")
 include("test_tree.jl")
 include("test_frames.jl")
 include("test_spatial.jl")

test/test_spatial.jl

@@ -1,6 +1,6 @@
 function Ad(H::Transform3D)
     R = rotationmatrix(H.rot)
-    pHat = Array(RigidBodyDynamics.vector_to_skew_symmetric(H.trans))
+    pHat = Array(RigidBodyDynamics.hat(H.trans))
     return [R zeros(3, 3); pHat * R R]
 end
 

test/test_util.jl

@@ -0,0 +1,11 @@
+import RigidBodyDynamics: angle_axis_proper, hat, rotation_matrix
+
+@testset "util" begin
+    @testset "rotation" begin
+        quat = nquatrand()
+        R = rotation_matrix(quat)
+        angle, axis = angle_axis_proper(quat)
+        rotationvector = angle * axis
+        @test isapprox(R, expm(hat(rotationvector)); atol = 1e-10)
+    end
+end