path.hpp

Go to the documentation of this file.

#ifndef __PATH__
#define __PATH__

#include <tucano/camera.hpp>
#include <tucano/mesh.hpp>
#include <tucano/shapes/coordinateaxes.hpp>
#include <tucano/shapes/sphere.hpp>
#include <Eigen/Dense>
#include <cmath>

namespace Tucano
{


class Path: public Tucano::Camera {

private:

    float anim_speed = 0.025;

        float anim_time = 0.0;

        bool animating = false;

    bool loop_animation = false;

        bool draw_control_points = false;

        bool draw_quaternions = false;

        vector< Eigen::Vector4f > key_positions;

    vector< float > key_intervals;

        vector< Eigen::Vector4f > control_points_1;

        vector< Eigen::Vector4f > control_points_2;

        vector< vector <float> > arc_lengths;
    
        float path_length;

        vector< Eigen::Quaternion<float> > key_quaternions;

    vector < Eigen::Quaternion<float> > control_quaternions;

        vector< Eigen::Quaternion<float> > key_directions;

        Mesh curve;

        // Mesh containing segments from key points to control points, for visual debug
        Mesh control_segments;

        Shapes::Sphere sphere;

        Shapes::CoordinateAxes axes;

    Shader camerapath_shader;

        Shader phong_shader;
        
public:

    Path ()
    {
        initOpenGLMatrices();

        #ifdef TUCANOSHADERDIR
        initialize(stringify(TUCANOSHADERDIR));
        #endif
    }

    void reset (void)
    {
                key_positions.clear();
                key_quaternions.clear();
                key_directions.clear();
                control_points_1.clear();
                control_points_2.clear();
                arc_lengths.clear();
                path_length = 0.0;
    }

    void initOpenGLMatrices (void)
    {
        // reset all matrices
        reset();
    }

    void initialize (string shader_dir = "../effects/shaders/")
    {
        camerapath_shader.load("beziercurve", shader_dir);
                camerapath_shader.initialize();

                phong_shader.load("phongshader", shader_dir);
                phong_shader.initialize();
    }

        void fillVertexData (void)
        {
                computeInnerControlPoints(); 
        computeControlQuaternions();
//      insertPausePositions();
                computeArcLength();

                curve = Mesh();
                curve.loadVertices(key_positions);
                curve.createAttribute("in_ControlPoint1", control_points_1);
                curve.createAttribute("in_ControlPoint2", control_points_2);

                control_segments = Mesh();

                vector < Eigen::Vector4f > control_segs;
                for (unsigned int i = 0; i < key_positions.size()-1; ++i)
                {
                        control_segs.push_back(key_positions[i]);
                        control_segs.push_back(control_points_1[i]);
                        control_segs.push_back(key_positions[i+1]);
                        control_segs.push_back(control_points_2[i]);            
                }
                control_segments.loadVertices(control_segs);
                
                #ifdef TUCANODEBUG
        Misc::errorCheckFunc(__FILE__, __LINE__);
                #endif

        }

        void addKeyPosition (const Tucano::Camera& camera, float interval = 0.0)
        {
                Eigen::Vector4f center; 
                center << camera.getCenter(), 1.0;
                key_positions.push_back ( center );
        key_intervals.push_back ( interval );
                Eigen::Quaternionf quat (camera.getViewMatrix().rotation() );
                key_quaternions.push_back (quat.inverse());

        // if this is a pause pos don't compute yet, wait for next key
        if (key_positions.size() > 1 && interval == 0.0)
                        fillVertexData();

        }

        void addKeyDirection (const Eigen::Quaternion<float> q)
        {
                key_directions.push_back ( q );        
        }


    void render (const Tucano::Camera& camera, const Tucano::Camera& light)
    {
                glEnable(GL_DEPTH_TEST);
                if (key_positions.size() > 1)
                {

            // draw path
                camerapath_shader.bind();
        
                camerapath_shader.setUniform("viewMatrix", camera.getViewMatrix());
                camerapath_shader.setUniform("projectionMatrix", camera.getProjectionMatrix());
                camerapath_shader.setUniform("nearPlane", camera.getNearPlane());
                camerapath_shader.setUniform("farPlane", camera.getFarPlane());

                Eigen::Vector4f color (1.0, 0.0, 0.0, 1.0);
                camerapath_shader.setUniform("modelMatrix", Eigen::Affine3f::Identity());
                camerapath_shader.setUniform("in_Color", color);

                        curve.setAttributeLocation(camerapath_shader);
                        curve.bindBuffers();
            glDrawArrays(GL_LINE_STRIP, 0, key_positions.size());
                        curve.unbindBuffers();

                camerapath_shader.unbind();

                        if (draw_control_points)
                        {
                                phong_shader.bind();
                        
                                phong_shader.setUniform("viewMatrix", camera.getViewMatrix());
                                phong_shader.setUniform("projectionMatrix", camera.getProjectionMatrix());
                                phong_shader.setUniform("lightViewMatrix", light.getViewMatrix());

                                color << 1.0, 1.0, 0.0, 1.0;
                                phong_shader.setUniform("modelMatrix", Eigen::Affine3f::Identity());
                                phong_shader.setUniform("default_color", color);
                                phong_shader.setUniform("has_color", false);
                                control_segments.setAttributeLocation(phong_shader);
                                control_segments.bindBuffers();
                                glDrawArrays(GL_LINES, 0, control_points_1.size()*4);
                                control_segments.unbindBuffers();
                                phong_shader.unbind();

                                sphere.setColor( Eigen::Vector4f (0.48, 1.0, 0.16, 1.0) );
                                // render control points
                                for (unsigned int i = 0; i < control_points_1.size(); i++)
                                {
                                        sphere.resetModelMatrix();
                                        Eigen::Vector3f translation = control_points_1[i].head(3);
                                        sphere.modelMatrix()->translate( translation );
                                        sphere.modelMatrix()->scale( 0.03 );
                                        sphere.render(camera, light);
                                }

                                sphere.setColor( Eigen::Vector4f (0.48, 0.16, 1.0, 1.0) );
                                // render control points
                                for (unsigned int i = 0; i < control_points_2.size(); i++)
                                {
                                        sphere.resetModelMatrix();
                                        Eigen::Vector3f translation = control_points_2[i].head(3);
                                        sphere.modelMatrix()->translate( translation );
                                        sphere.modelMatrix()->scale( 0.03 );
                                        sphere.render(camera, light);
                                }

                        }

                }

                if (draw_quaternions)
                {
                        for (unsigned int i = 0; i < key_quaternions.size(); ++i)
                        {
                                axes.resetModelMatrix();
                                axes.modelMatrix()->rotate(key_quaternions[i]);
                                axes.modelMatrix()->scale(0.2);
                                axes.render(camera, light);     
                        }
                }

                sphere.setColor( Eigen::Vector4f (1.0, 0.48, 0.16, 1.0) );
                // render key positions
                for (unsigned int i = 0; i < key_positions.size(); i++)
                {
                        sphere.resetModelMatrix();
                        
                        Eigen::Vector3f translation = key_positions[i].head(3);
                        sphere.modelMatrix()->translate( translation );
                        sphere.modelMatrix()->scale( 0.03 );
                        sphere.render(camera, light);
                }


        glDisable(GL_DEPTH_TEST);
                
                #ifdef TUCANODEBUG
        Misc::errorCheckFunc(__FILE__, __LINE__);
                #endif
    }

        int curveSegment (float t)
        {
                if (t < 0 || t > 1.0)
                        return 0;

                return int (t * (key_positions.size()-1));
        }

        float toLocalParameter (float t)
        {
                int segment = curveSegment(t);
                float segment_length = 1.0 / (float)(key_positions.size()-1);

                return (t - segment*segment_length)/segment_length;     
        }

        Eigen::Vector4f pointOnSegment (float t, int segment)
        {
                Eigen::Vector4f pt;
                pt = pow(1-t,3)*key_positions[segment] + 3.0*pow(1-t,2)*t*control_points_1[segment] + 3.0*(1-t)*pow(t,2)*control_points_2[segment] + pow(t, 3)*key_positions[segment+1];
                return pt;      
        }

        Eigen::Vector4f pointOnPath (float global_t)
        {
                float t = toLocalParameter (global_t);
                int segment = curveSegment (global_t);
                return pointOnSegment(t, segment);      
        }       
        
        Eigen::Quaternionf logQuaternion (const Eigen::Quaternionf& qi)
        {
                Eigen::Quaternionf logq, q;    
        q = qi;

        // change sign to avoid flipping behavior during interpolation
        if (q.w() < 0)
            q.coeffs() *= -1.0;

                logq.w() = 0.0;
                float angle = acos(q.w());
                float sinangle = sin(angle);
                if ( fabs(sinangle) >= 1e-03)
                        logq.vec() = angle * q.vec() / sinangle;
                else 
                        logq.vec() = q.vec();

//              logq.w() = log ( q.norm() );
//              logq.vec() = q.vec().normalized() * acos ( q.w() / q.norm());

                return logq;
        }

        Eigen::Quaternionf expQuaternion (const Eigen::Quaternionf& q)
        {
                Eigen::Quaternionf expq;

        float angle = q.norm();
                expq.w() = cos(angle);
                float sinangle = sin(angle);
                if (fabs(sinangle) >= 1e-03)
                        expq.vec() = sin(angle) * q.vec() / angle;
                else
                        expq.vec() = q.vec();

//              expq.w() = exp(q.w()) * cos( q.vec().norm() );
//              expq.vec() = exp(q.w()) * q.vec().normalized() * sin( q.vec().norm() );

                return expq;
        }

        Eigen::Quaternionf squad (int seg, float t)
        {
                Eigen::Quaternionf s0 = control_quaternions[seg];
                Eigen::Quaternionf s1 = control_quaternions[seg+1];
        Eigen::Quaternionf q0 = key_quaternions[seg];
        Eigen::Quaternionf q1 = key_quaternions[seg+1];
        Eigen::Quaternionf p0, p1, res;
        float dot = 0.0;

        // before each slerp check if we are going the right direction (shortest path)
        // otherwise multiply one of the quaternions by -1
        // Note: this might not be necessary any more, since we are checking during log method

                dot = q0.w()*q1.w()+q0.vec().dot(q1.vec());
                if (dot < 0)
                {
            q1.coeffs() *= -1.0;
                }       
                p0 = q0.slerp(t, q1); 

                dot = s0.w()*s1.w()+s0.vec().dot(s1.vec());
                if (dot < 0)
                {
            s1.coeffs() *= -1.0;
                }       
                p1 = s0.slerp(t, s1);

        p0.normalize();
        p1.normalize();

        dot = p0.w()*p1.w()+p0.vec().dot(p1.vec());
        if (dot < 0)
        {
            p1.coeffs() *= -1.0;
        }
                res = p0.slerp(2.0*t*(1.0-t), p1);
        res.normalize();

                return res;
        }

        
        Eigen::Affine3f cameraAtTime (float global_t)
        {
                Eigen::Affine3f m = Eigen::Affine3f::Identity();
                
                float t_a = arcLengthToTime(global_t);
                if (t_a == -1.0)
                        return m;

                float t = toLocalParameter (t_a);
                int segment = curveSegment (t_a);

        Eigen::Vector4f pt;
                Eigen::Quaternionf qt;
        // check if this a pause key position
        if (key_intervals[segment] > 0.0)
        {
            pt = key_positions[segment];
            qt = key_quaternions[segment];
        }
        else 
        {
                    pt = pointOnSegment(t, segment);
                    qt = squad(segment, t);

        }

                m.rotate(qt);
                m.translation() = pt.head(3);

                return m;
        }

        Eigen::Quaternion<float> directionAtTime (float global_t)
        {
                Eigen::Quaternion<float> q = Eigen::Quaternion<float>::Identity();

                float t_a = arcLengthToTime(global_t);
                if (t_a == -1.0)
                        return q;

                float t = toLocalParameter (t_a);
                int segment = curveSegment (t_a);

                q = key_directions[segment].slerp(t, key_directions[segment+1]);

                return q;
        }
        Eigen::Affine3f cameraAtCurrentTime (void)
        {
                return cameraAtTime(anim_time);
        }

        Eigen::Quaternion<float> directionAtCurrentTime (void)
        {
                return directionAtTime(anim_time);
        }

        float arcLengthToTime (float s)
        {
                if (key_positions.size() < 2)
                        return -1.0;
        
                if (s < 0.0)
                        return 0;

                if (s > path_length)            
                        return path_length;

                float arc_length = s;

                // find in which Beziér segment we should look
                unsigned int segment = 0;
                for (; segment < key_positions.size()-1; ++segment)
                {
                        if (arc_lengths[segment+1][0] > arc_length)
                                break;
                }       

                // look inside Beziér segment in which linear approximation we should look for 
                unsigned int sub_seg = 0;
                for (; sub_seg < arc_lengths[segment].size()-1; ++sub_seg)
                {
                        if (arc_lengths[segment][sub_seg+1] > arc_length)
                                break;
                }

                // convert from arc length (s) to time parameter (t)
                // percentage inside sub segment
                float alpha = (arc_length - arc_lengths[segment][sub_seg]) / 
                                        (arc_lengths[segment][sub_seg+1] - arc_lengths[segment][sub_seg]);

                // find parameter [0,1] inside Beziér segment
                float t_ini = sub_seg / (float)(arc_lengths[segment].size()-1);
                float t_end = (sub_seg+1) / (float)(arc_lengths[segment].size()-1);
                float t_s = t_ini + alpha * (t_end - t_ini);

                // convert to global parameter of the path
                t_s = t_s / (float)(key_positions.size()-1);

                // add time to beginning of segment
                t_s += segment / (float)(key_positions.size()-1);       

                return t_s;
        }

        void toggleAnimation (void)
        {
                animating = !animating;
        }

    void setLoopAnimation (bool f)
    {
        loop_animation = f;
    }

        void toggleDrawControlPoints (void)
        {
                draw_control_points = !draw_control_points;
        }

        void toggleDrawQuaternions (void)
        {
                draw_quaternions = !draw_quaternions;
        }


        bool isAnimating (void)
        {
                return animating;
        }

        void stepForward ( void )
        {
        anim_time += anim_speed;
            // restart if necessary
                if (anim_time >= path_length)
        {
            if (loop_animation)
                    anim_time = anim_time - path_length;
            else
            {
                anim_time -= anim_speed;
                animating = false;
            }
        }
        }

        void stepBackward ( void )
        {
                anim_time -= anim_speed;
                // restart if necessary
                if (anim_time < 0.0)
        {
                        anim_time = anim_time + path_length;
        }
        }
        void startAnimation ( void )
        {
                animating = true;
        }       

        void stopAnimation ( void )
        {
                animating = false;
        }

        void resetAnimation ( void )
        {
                anim_time = 0.0;
        }

        float animTime ( void )
        {
                return anim_time;
        }

        float animSpeed ( void )
        {
                return anim_speed;
        }
        
        void setAnimSpeed (float as)
        {
                anim_speed = as;
        }

    void computeControlQuaternions ( void )
    {
        control_quaternions.clear();

                Eigen::Quaternionf s;
        
        // s for first control point is not defined, so we just set as q0
        s = key_quaternions[0];
                control_quaternions.push_back(s);

        for (unsigned int seg = 1; seg < key_quaternions.size()-1; ++seg)
        {
                        s = -0.25*(logQuaternion(key_quaternions[seg].inverse()*key_quaternions[seg-1]).coeffs() + logQuaternion(key_quaternions[seg].inverse()*key_quaternions[seg+1]).coeffs());
                        s = key_quaternions[seg]* expQuaternion(s);

            control_quaternions.push_back(s);
        }

                // control point for last quaternion not defined since we dont have qi+1
                control_quaternions.push_back(key_quaternions.back());

    }


        void computeInnerControlPoints ( void )
        {

                control_points_1.clear();
                control_points_2.clear();
        
                // if there are only two keypoints it is a linear segment
                // just distribute control points equally along segment
                if (key_positions.size() == 2)
                {
                        Eigen::Vector4f pt = 0.75*key_positions[0] + 0.25*key_positions[1];     
                        control_points_1.push_back(pt);
                        control_points_1.push_back(pt);
                        pt << 0.25*key_positions[0] + 0.75*key_positions[1];
                        control_points_2.push_back(pt);
                        control_points_2.push_back(pt);
                        return;
                }

                const int n = key_positions.size()-1;

                control_points_1.resize(n+1);
                control_points_2.resize(n+1);

                // in theory this should be a nxn matrix, but since it is a tridiagonal matrix
                // we are just storing the non-zero elements as a, b, and c
                vector <float> a_orig;
                vector <float> b_orig;
                vector <float> c_orig;
                vector <float> a;
                vector <float> b;
                vector <float> c;
                vector <float> d;
                a_orig.resize(n);
                b_orig.resize(n);
                c_orig.resize(n);
                a.resize(n);
                b.resize(n);
                c.resize(n);    
                d.resize(n);

                // build the weight matrix, it is the same for all lines except first and last
                a_orig[0] = 0.0;
                b_orig[0] = 2.0;
                c_orig[0] = 1.0;
                
                for (int i = 1; i < n-1; i++)
                {
                        a_orig[i] = 1.0;
                        b_orig[i] = 4.0;
                        c_orig[i] = 1.0;
                }

                a_orig[n-1] = 2.0;
                b_orig[n-1] = 7.0;
                c_orig[n-1] = 0.0;

                // solve using Thomas Algorithm for tridiagonal matrices
                // solve the system three times, one for each coordinate (x, y, and z)
                // we copy the coefficients each time to avoid recomputing them
                for (int coord = 0; coord < 3; ++coord)
                {
                        // build solution vector of linear system
                        d[0] = key_positions[0][coord] + 2.0 * key_positions[1][coord]; 
                        for (int i = 1; i < n-1; i++)
                                d[i] = 4.0*key_positions[i][coord] + 2.0*key_positions[i+1][coord];
                        d[n-1] = 8.0*key_positions[n-1][coord] + key_positions[n][coord];
        
                        // copy coefficients, we are going to reuse them for every coordinate
                        a = a_orig;
                        b = b_orig;
                        c = c_orig;

                        // forward sweep
                        c[0] = c[0]/b[0];
                        d[0] = d[0]/b[0];

                        for (int i = 1; i < n; ++i)
                        {
                                float m = b[i] - a[i]*c[i-1];
                                c[i] = c[i] / m;
                                d[i] = (d[i] - a[i]*d[i-1]) / m;
                        }

                        // back substitution
                        control_points_1[n-1][coord] = d[n-1];
                        for (int i = n-2; i >= 0; --i)
                        {
                                control_points_1[i][coord] = d[i] - c[i]*control_points_1[i+1][coord];
                        }
                        // add one last control point to facilitate future operations (same size as key vector)
                        control_points_1[n][coord] = control_points_1[n-1][coord];
                }

                // set w = 1 for all control points
                for (int i = 0; i <= n; ++i)
                {
                        control_points_1[i][3] = 1.0;
                }

                // control point 2 can be determined directly now
                Eigen::Vector4f pt;
                for (int i = 0; i < n-1; ++i)
                {
                        pt = 2.0* key_positions[i+1] - control_points_1[i+1];
                        control_points_2[i] = pt;
                }
                pt = (key_positions[n] + control_points_1[n-1])*0.5;
                control_points_2[n-1] = pt;
                control_points_2[n] = pt;
        }


        void computeArcLength (void)
        {
                float divs = 100.0;
                Eigen::Vector4f p0;
                Eigen::Vector4f p1;
                float dist = 0.0;
                path_length = 0.0;

                arc_lengths.clear();

                p0 = key_positions[0];
                for (unsigned int seg = 0; seg < key_positions.size()-1; ++seg)
                {
                        vector <float> seg_lengths;

            // this is a pause position, just fill with constant length
            if (key_intervals[seg] > 0.0)
            {
                float l = key_intervals[seg] / divs;
                // already include extra segment since they are all the same
                        for (int i = 0; i < divs+1; ++i)
                {
                    dist += l;
                    seg_lengths.push_back(dist);
                }
                        p1 = key_positions[seg+1];
            }
            // otherwise divide the curve segment into divs linear segments
            else
            {
                        for (int i = 0; i < divs; ++i)
                        {
                                p1 = pointOnSegment( (float)i/divs, seg);
                                dist += (p1-p0).norm();

                                    seg_lengths.push_back(dist);
                                p0 = p1;
                        }
                            // to make life easier when converting from t to s,
                        // we also repeat the last sub segment length at the end
                        // even though it will be covered by the first sub segment
                        // of the next Beziér segment
                            // note that we do not increment dist, since it will be done
                        // in the next iteration
                        p1 = key_positions[seg+1];
                        seg_lengths.push_back( dist + (p1-p0).norm() );
            }
                        arc_lengths.push_back (seg_lengths);
                }       
                // the last segment connecting to the final key position
                // there is no segment starting at this key position, so it
                // must be treated apart
                p1 = key_positions[key_positions.size()-1];
                dist += (p1-p0).norm();
                        
                path_length = dist;

                // insert a last vector with only the total distance
                // makes life easier when converting from arc length to time parameter
                vector <float> last_seg;
                last_seg.push_back (dist);
                arc_lengths.push_back(last_seg);

        }

        void printDebugInfo (void)
        {
                float global_t = anim_time;
                float t = toLocalParameter (global_t);
                int segment = curveSegment (global_t);

                Eigen::Vector4f pt = pointOnSegment(t, segment);

                Eigen::Quaternionf qt;
                if (segment >= 0 && key_positions.size() - segment > 2)
                        qt = squad(segment, t);
                else            
                        qt = key_quaternions[segment].slerp(t, key_quaternions[segment+1]);     

                Eigen::Affine3f m = Eigen::Affine3f::Identity();
                m.rotate(qt);
                m.translation() = pt.head(3);

                cout << "quat : (" << qt.w() << " , " << qt.vec().transpose() << " ) " << endl;
                cout << "t : " << pt.head(3).transpose() << endl;
                cout << "m : " << endl << m.matrix() << endl << endl;
        }


    void writeToFile (const string filename)
    {
        ofstream file;
        file.open (filename.c_str());
        
        file << key_positions.size() << endl;
        for (unsigned int i = 0; i < key_positions.size(); ++i)
        {
            file << key_positions[i].transpose() << endl; 
            file << key_quaternions[i].w() << " " << key_quaternions[i].vec().transpose() << endl;
            file << key_directions[i].w() << " " << key_directions[i].vec().transpose() << endl;
            file << key_intervals[i] << endl;

        }
        file.close();
    }

    void loadFromFile (const string filename)
    {
        ifstream file (filename.c_str());

        if (file.is_open())
        {        
            string line;
            float elem;
            int num;

            Eigen::Quaternion<float> q;
            Eigen::Vector4f v;

            getline(file, line);
            istringstream iss(line);
            iss >> num;

            for (int i = 0; i < num; ++i)
            {
                // read key position
                getline(file, line);
                iss.clear();
                iss.str(line);
                iss >> v[0] >> v[1] >> v[2] >> v[3];
                key_positions.push_back(v);

                // read key quaternion
                getline(file, line);
                iss.clear();
                iss.str(line);
                iss >> q.w() >> q.vec()[0] >> q.vec()[1] >> q.vec()[2];
                key_quaternions.push_back(q);

                // read key direction
                getline(file, line);
                iss.clear();
                iss.str(line);
                iss >> q.w() >> q.vec()[0] >> q.vec()[1] >> q.vec()[2];
                key_directions.push_back(q);

                // read key interval
                getline(file, line);
                iss.clear();
                iss.str(line);
                iss >> elem;
                key_intervals.push_back(elem);
            }
        }
        if (key_positions.size() > 1)
        {
            fillVertexData();
        }
    }

};

}
#endif