v01

thirdparty/Pangolin/include/pangolin/scene/axis.h

@@ -0,0 +1,117 @@
+#pragma once
+
+#include <pangolin/display/opengl_render_state.h>
+#include <pangolin/display/viewport.h>
+#include <pangolin/gl/gldraw.h>
+
+#include <pangolin/scene/renderable.h>
+#include <pangolin/scene/interactive_index.h>
+
+#ifdef HAVE_EIGEN
+#  include <Eigen/Geometry>
+#endif
+
+namespace pangolin {
+
+struct Axis : public Renderable, public Interactive
+{
+    Axis()
+        : axis_length(1.0),
+          label_x(InteractiveIndex::I().Store(this)),
+          label_y(InteractiveIndex::I().Store(this)),
+          label_z(InteractiveIndex::I().Store(this))
+    {
+    }
+
+    void Render(const RenderParams&) override {
+        glColor4f(1,0,0,1);
+        glPushName(label_x.Id());
+        glDrawLine(0,0,0, axis_length,0,0);
+        glPopName();
+
+        glColor4f(0,1,0,1);
+        glPushName(label_y.Id());
+        glDrawLine(0,0,0, 0,axis_length,0);
+        glPopName();
+
+        glColor4f(0,0,1,1);
+        glPushName(label_z.Id());
+        glDrawLine(0,0,0, 0,0,axis_length);
+        glPopName();
+    }
+
+    bool Mouse(
+        int button,
+        const GLprecision /*win*/[3], const GLprecision /*obj*/[3], const GLprecision /*normal*/[3],
+        bool /*pressed*/, int button_state, int pickId
+    ) override
+    {
+        PANGOLIN_UNUSED(button);
+        PANGOLIN_UNUSED(button_state);
+        PANGOLIN_UNUSED(pickId);
+
+#ifdef HAVE_EIGEN
+        if((button == MouseWheelUp || button == MouseWheelDown) ) {
+            float scale = (button == MouseWheelUp) ? 0.01f : -0.01f;
+            if(button_state & KeyModifierShift) scale /= 10;
+
+            Eigen::Vector3d rot = Eigen::Vector3d::Zero();
+            Eigen::Vector3d xyz = Eigen::Vector3d::Zero();
+
+
+            if(button_state & KeyModifierCtrl) {
+                // rotate
+                if(pickId == label_x.Id()) {
+                    rot << 1,0,0;
+                }else if(pickId == label_y.Id()) {
+                    rot << 0,1,0;
+                }else if(pickId == label_z.Id()) {
+                    rot << 0,0,1;
+                }else{
+                    return false;
+                }
+            }else if(button_state & KeyModifierShift){
+                // translate
+                if(pickId == label_x.Id()) {
+                    xyz << 1,0,0;
+                }else if(pickId == label_y.Id()) {
+                    xyz << 0,1,0;
+                }else if(pickId == label_z.Id()) {
+                    xyz << 0,0,1;
+                }else{
+                    return false;
+                }
+            }else{
+                return false;
+            }
+
+            // old from new
+            Eigen::Matrix<double,4,4> T_on = Eigen::Matrix<double,4,4>::Identity();
+            T_on.block<3,3>(0,0) = Eigen::AngleAxis<double>(scale,rot).toRotationMatrix();
+            T_on.block<3,1>(0,3) = scale*xyz;
+
+            // Update
+            T_pc = (ToEigen<double>(T_pc) * T_on.inverse()).eval();
+
+            return true;
+        }
+#endif // HAVE_EIGEN
+
+        return false;
+    }
+
+    virtual bool MouseMotion(
+        const GLprecision /*win*/[3], const GLprecision /*obj*/[3], const GLprecision /*normal*/[3],
+        int /*button_state*/, int /*pickId*/
+    ) override
+    {
+        return false;
+    }
+
+    float axis_length;
+    const InteractiveIndex::Token label_x;
+    const InteractiveIndex::Token label_y;
+    const InteractiveIndex::Token label_z;
+};
+
+}

thirdparty/Pangolin/include/pangolin/scene/interactive.h

@@ -0,0 +1,69 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+#include <pangolin/display/opengl_render_state.h>
+
+namespace pangolin {
+
+
+struct Interactive
+{
+    static __thread GLuint current_id;
+
+    virtual ~Interactive() {}
+
+    virtual bool Mouse(
+        int button,
+        const GLprecision win[3], const GLprecision obj[3], const GLprecision normal[3],
+        bool pressed, int button_state, int pickId
+    )  = 0;
+
+    virtual bool MouseMotion(
+        const GLprecision win[3], const GLprecision obj[3], const GLprecision normal[3],
+        int button_state, int pickId
+    ) = 0;
+};
+
+struct RenderParams
+{
+    RenderParams()
+      : render_mode(GL_RENDER)
+    {
+    }
+
+    GLint render_mode;
+};
+
+struct Manipulator : public Interactive
+{
+    virtual void Render(const RenderParams& params) = 0;
+};
+
+}

thirdparty/Pangolin/include/pangolin/scene/interactive_index.h

@@ -0,0 +1,115 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <map>
+
+#include <pangolin/scene/interactive.h>
+
+namespace pangolin {
+
+class InteractiveIndex
+{
+public:
+    class Token
+    {
+    public:
+        friend class InteractiveIndex;
+
+        Token()
+            : id(0)
+        {
+        }
+
+        Token(Token&& o)
+            : id(o.id)
+        {
+            o.id = 0;
+        }
+
+        GLint Id() const
+        {
+            return id;
+        }
+
+        ~Token()
+        {
+            if(id) {
+                InteractiveIndex::I().Unstore(*this);
+            }
+        }
+
+    private:
+        Token(GLint id)
+            : id(id)
+        {
+        }
+
+
+        GLint id;
+    };
+
+    static InteractiveIndex& I()
+    {
+        static InteractiveIndex instance;
+        return instance;
+    }
+
+    Interactive* Find(GLuint id)
+    {
+        auto kv = index.find(id);
+        if(kv != index.end()) {
+            return kv->second;
+        }
+        return nullptr;
+    }
+
+    Token Store(Interactive* r)
+    {
+        index[next_id] = r;
+        return Token(next_id++);
+    }
+
+    void Unstore(Token& t)
+    {
+        index.erase(t.id);
+        t.id = 0;
+    }
+
+private:
+    // Private constructor.
+    InteractiveIndex()
+        : next_id(1)
+    {
+    }
+
+    GLint next_id;
+    std::map<GLint, Interactive*> index;
+};
+
+}

thirdparty/Pangolin/include/pangolin/scene/renderable.h

@@ -0,0 +1,117 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use,
+ * copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following
+ * conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <memory>
+#include <map>
+#include <random>
+
+#include <pangolin/display/opengl_render_state.h>
+#include <pangolin/scene/interactive.h>
+
+namespace pangolin {
+
+class Renderable
+{
+public:
+    using guid_t = GLuint;
+
+    static guid_t UniqueGuid()
+    {
+        static std::random_device rd;
+        static std::mt19937 gen(rd());
+        return (guid_t)gen();
+    }
+
+    Renderable(const std::weak_ptr<Renderable>& parent = std::weak_ptr<Renderable>())
+        : guid(UniqueGuid()), parent(parent), T_pc(IdentityMatrix()), should_show(true)
+    {
+    }
+
+    virtual ~Renderable()
+    {
+    }
+
+    // Default implementation simply renders children.
+    virtual void Render(const RenderParams& params = RenderParams()) {
+        RenderChildren(params);
+    }
+
+    void RenderChildren(const RenderParams& params)
+    {
+        for(auto& p : children) {
+            Renderable& r = *p.second;
+            if(r.should_show) {
+                glPushMatrix();
+                r.T_pc.Multiply();
+                r.Render(params);
+                if(r.manipulator) {
+                    r.manipulator->Render(params);
+                }
+                glPopMatrix();
+            }
+        }
+    }
+
+    std::shared_ptr<Renderable> FindChild(guid_t guid)
+    {
+        auto o = children.find(guid);
+        if(o != children.end()) {
+            return o->second;
+        }
+
+        for(auto& kv : children ) {
+            std::shared_ptr<Renderable> c = kv.second->FindChild(guid);
+            if(c) return c;
+        }
+
+        return std::shared_ptr<Renderable>();
+    }
+
+    Renderable& Add(const std::shared_ptr<Renderable>& child)
+    {
+        if(child) {
+            children[child->guid] = child;
+        };
+        return *this;
+    }
+
+    // Renderable properties
+    const guid_t guid;
+    std::weak_ptr<Renderable> parent;
+    pangolin::OpenGlMatrix T_pc;
+    bool should_show;
+
+    // Children
+    std::map<guid_t, std::shared_ptr<Renderable>> children;
+
+    // Manipulator (handler, thing)
+    std::shared_ptr<Manipulator> manipulator;
+};
+
+}

thirdparty/Pangolin/include/pangolin/scene/scenehandler.h

@@ -0,0 +1,182 @@
+#pragma once
+
+#include <pangolin/handler/handler.h>
+#include <pangolin/scene/renderable.h>
+#include <pangolin/scene/interactive_index.h>
+
+namespace pangolin {
+
+inline void gluPickMatrix(
+    GLdouble x, GLdouble y,
+    GLdouble width, GLdouble height,
+    GLint viewport[4]
+) {
+    GLfloat m[16];
+    GLfloat sx, sy;
+    GLfloat tx, ty;
+    sx = viewport[2] / (GLfloat)width;
+    sy = viewport[3] / (GLfloat)height;
+    tx = (viewport[2] + 2.0f * (viewport[0] - (GLfloat)x)) / (GLfloat)width;
+    ty = (viewport[3] + 2.0f * (viewport[1] - (GLfloat)y)) / (GLfloat)height;
+#define M(row, col) m[col*4+row]
+    M(0, 0) = sx;
+    M(0, 1) = 0.0f;
+    M(0, 2) = 0.0f;
+    M(0, 3) = tx;
+    M(1, 0) = 0.0f;
+    M(1, 1) = sy;
+    M(1, 2) = 0.0f;
+    M(1, 3) = ty;
+    M(2, 0) = 0.0f;
+    M(2, 1) = 0.0f;
+    M(2, 2) = 1.0f;
+    M(2, 3) = 0.0f;
+    M(3, 0) = 0.0f;
+    M(3, 1) = 0.0f;
+    M(3, 2) = 0.0f;
+    M(3, 3) = 1.0f;
+#undef M
+    glMultMatrixf(m);
+}
+
+
+
+struct SceneHandler : public Handler3D
+{
+    SceneHandler(
+        Renderable& scene,
+        OpenGlRenderState& cam_state
+    ) : Handler3D(cam_state), scene(scene)
+    {
+
+    }
+
+    void ProcessHitBuffer(GLint hits, GLuint* buf, std::map<GLuint, Interactive*>& hit_map )
+    {
+        GLuint* closestNames = 0;
+        GLuint closestNumNames = 0;
+        GLuint closestZ = std::numeric_limits<GLuint>::max();
+        for (int i = 0; i < hits; i++) {
+            if (buf[1] < closestZ) {
+                closestNames = buf + 3;
+                closestNumNames = buf[0];
+                closestZ = buf[1];
+            }
+            buf += buf[0] + 3;
+        }
+        for (unsigned int i = 0; i < closestNumNames; i++) {
+            const int pickId = closestNames[i];
+            hit_map[pickId] = InteractiveIndex::I().Find(pickId);
+        }
+    }
+
+    void ComputeHits(pangolin::View& view,
+                     const pangolin::OpenGlRenderState& cam_state,
+                     int x, int y, int grab_width,
+                     std::map<GLuint, Interactive*>& hit_objects )
+    {
+        // Get views viewport / modelview /projection
+        GLint viewport[4] = {view.v.l, view.v.b, view.v.w, view.v.h};
+        pangolin::OpenGlMatrix mv = cam_state.GetModelViewMatrix();
+        pangolin::OpenGlMatrix proj = cam_state.GetProjectionMatrix();
+
+        // Prepare hit buffer object
+        const unsigned int MAX_SEL_SIZE = 64;
+        GLuint vSelectBuf[MAX_SEL_SIZE];
+        glSelectBuffer(MAX_SEL_SIZE, vSelectBuf);
+
+        // Load and adjust modelview projection matrices
+        glMatrixMode(GL_PROJECTION);
+        glLoadIdentity();
+        gluPickMatrix(x, y, grab_width, grab_width, viewport);
+        proj.Multiply();
+        glMatrixMode(GL_MODELVIEW);
+        mv.Load();
+
+        // Render scenegraph in 'select' mode
+        glRenderMode(GL_SELECT);
+        glInitNames();
+        RenderParams select;
+        select.render_mode = GL_SELECT;
+        scene.Render(select);
+        glFlush();
+
+        GLint nHits = glRenderMode(GL_RENDER);
+        //    std::cout << " -- Number of Hits are: " << nHits << std::endl;
+        //    std::cout << " -- size of hitobjects: " << hit_objects.size() << std::endl;
+        if (nHits > 0) {
+            ProcessHitBuffer(nHits, vSelectBuf, hit_objects);
+        }
+    }
+
+    void Mouse(pangolin::View& view, pangolin::MouseButton button,
+               int x, int y, bool pressed, int button_state)
+    {
+        GetPosNormal(view, x, y, p, Pw, Pc, n);
+        bool handled = false;
+
+        if (pressed) {
+            m_selected_objects.clear();
+            ComputeHits(view, *cam_state, x, y, 2*hwin+1, m_selected_objects);
+        }
+
+        for (auto kv : m_selected_objects)
+        {
+            Interactive* ir = dynamic_cast<Interactive*>(kv.second);
+            handled |= ir && ir->Mouse( button, p, Pw, n, pressed, button_state, kv.first);
+        }
+
+        if (!handled) {
+            Handler3D::Mouse(view, button, x, y, pressed, button_state);
+        }
+    }
+
+    void MouseMotion(pangolin::View& view, int x, int y, int button_state)
+    {
+        GetPosNormal(view, x, y, p, Pw, Pc, n);
+        bool handled = false;
+        for (auto kv : m_selected_objects)
+        {
+            Interactive* ir = dynamic_cast<Interactive*>(kv.second);
+
+            handled |= ir && ir->MouseMotion( p, Pw, n, button_state, kv.first);
+        }
+        if (!handled) {
+            pangolin::Handler3D::MouseMotion(view, x, y, button_state);
+        }
+    }
+
+    void Special(pangolin::View& view, pangolin::InputSpecial inType,
+                 float x, float y, float p1, float p2, float p3, float p4,
+                 int button_state)
+    {
+        GetPosNormal(view, (int)x, (int)y, p, Pw, Pc, n);
+
+        bool handled = false;
+
+        if (inType == pangolin::InputSpecialScroll)
+        {
+            m_selected_objects.clear();
+            ComputeHits(view, *cam_state, (int)x, (int)y, 2*hwin+1, m_selected_objects);
+
+            const MouseButton button = p2 > 0 ? MouseWheelUp : MouseWheelDown;
+
+            for (auto kv : m_selected_objects)
+            {
+                Interactive* ir = dynamic_cast<Interactive*>(kv.second);
+                handled |= ir && ir->Mouse( button, p, Pw, n, true, button_state, kv.first);
+            }
+        }
+
+        if (!handled) {
+            pangolin::Handler3D::Special(view, inType, x, y,
+                                         p1, p2, p3, p4, button_state);
+        }
+    }
+
+    std::map<GLuint, Interactive*> m_selected_objects;
+    Renderable& scene;
+    unsigned int grab_width;
+};
+
+}

thirdparty/Pangolin/include/pangolin/scene/tree.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <map>
+#include <pangolin/display/opengl_render_state.h>
+
+namespace pangolin {
+
+template<typename T, typename TEdge>
+struct TreeNode
+{
+    struct Edge
+    {
+        TEdge parent_child;
+        TreeNode node;
+    };
+
+    T item;
+    std::vector<Edge> edges;
+};
+
+template<typename T, typename TEdge>
+using NodeFunction = std::function<void(TreeNode<T,TEdge>&,const TEdge&)>;
+
+//template<typename T, typename TEdge>
+//void VisitDepthFirst(TreeNode<T,TEdge>& node, const NodeFunction<T,TEdge>& func, const TEdge& T_root_node = TEdge())
+//{
+//    func(node, T_root_node);
+//    for(auto& e : node.edges) {
+//        const TEdge T_root_child = T_root_node * e.parent_child;
+//        VisitDepthFirst(e.node, func, T_root_child);
+//    }
+//}
+
+//void Eg()
+//{
+//    using RenderNode = TreeNode<std::shared_ptr<Renderable>,OpenGlMatrix>;
+
+//    RenderNode root;
+//    VisitDepthFirst<std::shared_ptr<Renderable>,OpenGlMatrix>(
+//        root, [](RenderNode& node, const OpenGlMatrix& T_root_node) {
+//        if(node.item) {
+//            node.item->DoRender();
+//        }
+//    }, IdentityMatrix());
+
+//}
+
+
+}