hat instead of vector_to_skew_symmetric

JuliaRobotics · Nov 6, 2016 · 730d90e · 730d90e
1 parent cc3fdfc
commit 730d90e
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 21 deletions.
diff --git a/src/spatial.jl b/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,21 +45,6 @@ function (+){T}(inertia1::SpatialInertia{T}, inertia2::SpatialInertia{T})
     SpatialInertia(inertia1.frame, moment, crossPart, mass)
 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)
@@ -77,9 +62,9 @@ function transform{I, T}(inertia::SpatialInertia{I}, t::Transform3D{T})::Spatial
         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'

diff --git a/src/third_party_addendum.jl b/src/third_party_addendum.jl
@@ -1,13 +1,30 @@
-function vector_to_skew_symmetric{T}(v::SVector{3, T})
+@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)
-    vector_to_skew_symmetric(a) * B
+    hat(a) * B
 end
 
 rotate(x::SMatrix{3}, q::Quaternion) = rotation_matrix(q) * x

diff --git a/test/test_spatial.jl b/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