Merge 2899b91 into bf47a45

Project.toml

@@ -12,10 +12,10 @@ Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 Records = "5984c134-fa48-5ed5-a57f-fc2f6936871f"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
-Vec = "44eeaf0b-fee4-471f-9310-ed6585cb3142"
 
 [extras]
+ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test"]
+test = ["ForwardDiff", "Test"]

docs/make.jl

@@ -22,6 +22,9 @@ makedocs(
 				"simulation.md",
 				"collision_checkers.md",
 				"feature_extraction.md"
+		 ],
+		 "Internals" =>[
+			 "Vec.md"
 		 ]
 	 ]
 	 )

src/AutomotiveDrivingModels.jl

@@ -7,9 +7,12 @@ using StaticArrays
 using Distributions
 using Reexport
 using Random
-@reexport using Vec 
+
 @reexport using Records
 
+include("vec/Vec.jl")
+@reexport using .Vec
+
 # Roadways
 
 export StraightRoadway,

src/vec/Vec.jl

@@ -0,0 +1,84 @@
+__precompile__()
+
+module Vec
+
+using StaticArrays
+using LinearAlgebra
+using Printf
+import LinearAlgebra: ⋅, ×
+
+export
+    AbstractVec,
+    VecE,  # an abstract euclidean-group vector
+    VecSE, # an abstract special euclidean-group element
+
+    VecE2, # two-element Float64 vector, {x, y}
+    VecE3, # three-element Float64 vector, {x, y, z}
+    VecSE2, # point in special euclidean group of order 2, {x, y, θ}
+
+    polar, # construct a polar vector with (r,θ)
+
+    proj,  # vector projection
+           # proj(a::vec, b::vec, ::Type{Float64}) will do scalar projection of a onto b
+           # proj(a::vec, b::vec, ::Type{Vec}) will do vector projection of a onto b
+
+    lerp,  # linear interpolation between two vec's
+    invlerp,
+    lerp_angle,
+
+    normsquared,         # see docstrings below
+    scale_euclidean,
+    clamp_euclidean,
+    normalize_euclidian,
+
+    dist,  # scalar distance between two vec's
+    dist2, # squared scalar distance between two vec's
+
+    rot,   # rotate the vector, always using the Right Hand Rule
+    rot_normalized, # like rot, but assumes axis is normalized
+
+    deltaangle, # signed delta angle
+    angledist,  # distance between two angles
+
+    inertial2body,
+    body2inertial,
+
+    orientation,
+    are_collinear,
+    get_intersection,
+
+    Circ,
+    AABB,
+    OBB
+
+abstract type AbstractVec{N, R} <: FieldVector{N, R} end
+abstract type VecE{N, R} <: AbstractVec{N, R} end
+abstract type VecSE{N, R} <: AbstractVec{N, R} end
+
+lerp(a::Real, b::Real, t::Real) = a + (b-a)*t
+invlerp(a::Real, b::Real, c::Real) = (c - a)/(b-a)
+
+"The L2 norm squared."
+normsquared(a::AbstractVec) = sum(x^2 for x in a)
+
+"Scale the euclidean part of the vector by a factor b while leaving the orientation part unchanged."
+scale_euclidean(a::VecE, b::Real) = b.*a
+
+"Clamp each element of the euclidean part of the vector while leaving the orientation part unchanged."
+clamp_euclidean(a::VecE, lo::Real, hi::Real) = clamp.(a, lo, hi)
+
+"Normalize the euclidean part of the vector while leaving the orientation part unchanged."
+normalize_euclidian(a::VecE, p::Real=2) = normalize(a, p)
+
+include("common.jl")
+include("vecE2.jl")
+include("vecE3.jl")
+include("vecSE2.jl")
+
+include("geom/geom.jl")
+include("coordinate_transforms.jl")
+include("quat.jl")
+
+Base.vec(v::Union{AbstractVec, Quat, RPY}) = convert(Vector{Float64}, v)
+
+end # module

src/vec/common.jl

@@ -0,0 +1,15 @@
+const DUMMY_PRECISION = 1e-12
+
+Base.isfinite(a::AbstractVec) = all(isfinite, a)
+Base.isinf(a::AbstractVec) = any(isinf, a)
+Base.isnan(a::AbstractVec) = any(isnan, a)
+
+function StaticArrays.similar_type(::Type{V}, ::Type{F}, size::Size{N}) where {V<:AbstractVec, F<:AbstractFloat, N}
+    if size == Size(V) && eltype(V) == F
+        return V
+    else # convert back to SArray
+        return SArray{Tuple{N...},F,length(size),prod(size)}
+    end
+end
+
+Base.abs(a::AbstractVec) = error("abs(v::AbstractVec) has been removed. Use norm(v) to get the norm; abs.(v) to get the element-wise absolute value.")