diff --git a/docs/src/rotations.md b/docs/src/rotations.md index 2691455a..a7864505 100644 --- a/docs/src/rotations.md +++ b/docs/src/rotations.md @@ -54,6 +54,10 @@ E = RotXYZ(R) Evec = params(E) ``` +```@example ORI +rotation_axis(R), rotation_angle(R) # Get an axis-angle representation +``` + ## Conventions for modeling See [Orientations and directions](@ref) diff --git a/ext/Render.jl b/ext/Render.jl index 68a1a588..9d40a696 100644 --- a/ext/Render.jl +++ b/ext/Render.jl @@ -305,6 +305,42 @@ function render!(scene, ::typeof(World), sys, sol, t) true end +function render!(scene, ::typeof(Frame), sys, sol, t) + sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true + radius = sol(sol.t[1], idxs=sys.radius) |> Float32 + length = sol(sol.t[1], idxs=sys.length) |> Float32 + T = get_frame_fun(sol, sys) + + thing = @lift begin + Ti = T($t) + Rx = Ti[:, 1] + O = Point3f(Ti[1:3, 4]) # Assume world is never moving + x = O .+ Point3f(length*Rx) + Makie.GeometryBasics.Cylinder(O, x, radius) + end + mesh!(scene, thing, color=:red) + + thing = @lift begin + Ti = T($t) + Ry = Ti[:, 2] + O = Point3f(Ti[1:3, 4]) # Assume world is never moving + y = O .+ Point3f(length*Ry) + Makie.GeometryBasics.Cylinder(O, y, radius) + end + mesh!(scene, thing, color=:green) + + thing = @lift begin + Ti = T($t) + Rz = Ti[:, 3] + O = Point3f(Ti[1:3, 4]) # Assume world is never moving + z = O .+ Point3f(length*Rz) + Makie.GeometryBasics.Cylinder(O, z, radius) + end + mesh!(scene, thing, color=:blue) + + true +end + function render!(scene, T::Union{typeof(Revolute), typeof(RevolutePlanarLoopConstraint)}, sys, sol, t) r_0 = get_fun(sol, collect(sys.frame_a.r_0)) n = get_fun(sol, collect(sys.n)) diff --git a/src/frames.jl b/src/frames.jl index 3503c822..f4f0c568 100644 --- a/src/frames.jl +++ b/src/frames.jl @@ -1,4 +1,4 @@ -@connector function Frame(; name, varw = false, r_0 = zeros(3)) +@connector function Frame(; name, varw = false, r_0 = zeros(3), render=false, length=1.0, radius=0.1) @variables r_0(t)[1:3]=r_0 [ description = "Position vector directed from the origin of the world frame to the connector frame origin, resolved in world frame", ] @@ -10,12 +10,17 @@ connect = Flow, description = "Cut torque resolved in connector frame", ] + pars = @parameters begin + render = render + length = length + radius = radius + end r_0, f, tau = collect.((r_0, f, tau)) # R: Orientation object to rotate the world frame into the connector frame R = NumRotationMatrix(; name, varw, state_priority=-1) - ODESystem(Equation[], t, [r_0; f; tau], []; name, + ODESystem(Equation[], t, [r_0; f; tau], pars; name, metadata = Dict(:orientation => R, :frame => true)) end