add plotPoints for SO3

JuliaRobotics · Aug 8, 2024 · 9177c8a · 9177c8a
1 parent ab114a0
commit 9177c8a
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 0 deletions.
diff --git a/src/Arena.jl b/src/Arena.jl
@@ -24,9 +24,11 @@ using DocStringExtensions
 
 # NOTE a lot of legacy code has been moved to the attic
 
+export plotPoints
 export plotGraph3d
 
 # include("Exports.jl")
+include("services/PlotManifolds.jl")
 # include("services/PlotBoundingBox.jl")
 include("services/PlotFeatureTracks.jl")
 include("services/PlotHistogramGrid.jl")

diff --git a/src/services/PlotManifolds.jl b/src/services/PlotManifolds.jl
@@ -0,0 +1,22 @@
+
+
+
+function plotPoints(
+  ::typeof(SpecialOrthogonal(3)),
+  ps::AbstractVector{<:AbstractMatrix{<:Real}};
+  color = :red
+)
+  scene = Scene()
+  cam3d!(scene)
+  wireframe!(scene, Makie.Sphere( Point3f(0), 1.0), color=:gray)
+  Makie.scale!(scene, 1.0, 1.0, 1.0)
+  Makie.rotate!(scene, Vec3f(1, 0, 0), 0.5) # 0.5 rad around the y axis
+
+
+  for R in ps
+    vX = R*SA[1;0;0.0]
+    scatter!( scene, Point3f(vX...); color )
+  end
+
+  scene
+end