opengl_widget.cpp

/** @file
    @brief Header

    @date 2015

    @author
    Russell Taylor
    <russ@reliasolve.com>
    <http://sensics.com>
*/

// Copyright 2015 Sensics, Inc.
//
// (Licensed under the Apache License, Version 2.0)

#include "opengl_widget.h"
#include "mainwindow.h"

#include <QtGui>
#include <QtOpenGL>
#include <QColor>
#include <QFileDialog>
#include <QCursor>
#include <QMainWindow>
#include <sstream>
#include <math.h>
#include <stdio.h>
#include <quat.h>

#ifndef GL_MULTISAMPLE
#define GL_MULTISAMPLE  0x809D
#endif

//----------------------------------------------------------------------
// File-scoped globals

// Minimum interval between analog and tracker trigger events.
static const double ANALOG_DELAY = 0.5;
static const double TRACKER_DELAY = 0.5;

//----------------------------------------------------------------------
// Helper functions

//----------------------------------------------------------------------
// Member functions

OpenGL_Widget::OpenGL_Widget(QWidget *parent)
    : QGLWidget(QGLFormat(QGL::SampleBuffers), parent)
{
    d_last_frame_time = 0;
    d_frame_count = 0;

    // Initial target radius is 5 pixels.
    d_r = 12;

    // Initial clear color is black;
    d_clearColor = Qt::black;

    // Start out oscillating the cursor and display
    d_oscillate = true;
    d_oscillate_freq = 2;
    d_oscillate_phase = 0;
    d_oscillate_mag = 60;

    // Call the timer that will cause OpenGL to draw
    // faster than the maximum display update rate.
    connect(&d_timer,SIGNAL(timeout()),this,SLOT(idle()));
    d_osc_time.start();
    d_timer.start();

    // Set a cursor that is symmetric around its center.
    this->setCursor(Qt::CrossCursor);

    // Initialize VRPN objects and other things set by slots.
    d_button = NULL;
    d_analog = NULL;
    d_tracker = NULL;

    // Initialize the vertex array based on the number of
    // quads;
    d_num_quads = 1;
    d_vertex_array = NULL;
    updateVertexArray();

    // Set the minimum and maximum rotations such that they
    // will be overridden by any valid readings.
    d_tracker_min_rotation = 1e10;
    d_tracker_max_rotation = -1e10;
}

OpenGL_Widget::~OpenGL_Widget()
{
    if (d_button != NULL) { delete d_button; }
    if (d_analog != NULL) { delete d_analog; }
    if (d_tracker != NULL) { delete d_tracker; }
}

void OpenGL_Widget::useVRPNButton(QString name, int which)
{
	d_button = new vrpn_Button_Remote(name.toStdString().c_str());
	d_button->register_change_handler(this, handleButton);
	d_button_to_use = which;
}

void OpenGL_Widget::useVRPNAnalog(QString name, int which, double threshold)
{
    d_analog = new vrpn_Analog_Remote(name.toStdString().c_str());
    d_analog->register_change_handler(this, handleAnalog);
    d_analog_to_use = which;
    d_analog_threshold = threshold;
    d_analog_last_trigger.tv_sec = 0;
    d_analog_last_trigger.tv_sec = 0;
}

void OpenGL_Widget::useVRPNTracker(QString name, int sensor, double transThresh, double rotThresh)
{
    d_tracker = new vrpn_Tracker_Remote(name.toStdString().c_str());
    d_tracker->register_change_handler(this, handleTracker, sensor);
    d_tracker_trans_thresh = transThresh;
    d_tracker_rot_thresh = rotThresh;
    d_tracker_rotate_axis = -1; // Disable rotation.
    d_tracker_last_trigger.tv_sec = 0;
    d_tracker_last_trigger.tv_sec = 0;
}

void OpenGL_Widget::useVRPNTrackerRotate(QString name, int sensor, int whichAxis)
{
    d_tracker = new vrpn_Tracker_Remote(name.toStdString().c_str());
    d_tracker->register_change_handler(this, handleTracker, sensor);
    d_tracker_rotate_axis = whichAxis;
}

void OpenGL_Widget::idle(void)
{
    if (d_button != NULL) { d_button->mainloop(); }
    if (d_analog != NULL) { d_analog->mainloop(); }
    if (d_tracker != NULL) { d_tracker->mainloop(); }
    updateGL();
}

void OpenGL_Widget::initializeGL()
{
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    glShadeModel(GL_SMOOTH);
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    static GLfloat lightPosition[4] = { 0.5, 5.0, 7.0, 1.0 };
    glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
    glDisable(GL_MULTISAMPLE);

    // Makes the colors for the primitives be what we want.
    glDisable(GL_BLEND);
    glDisable(GL_TEXTURE_2D);
    glDisable(GL_LIGHTING);
}

void OpenGL_Widget::paintGL()
{
    if (d_oscillate) {
        // Figure out the new location in the oscillation.  Move both
        // the cursor and the displayed box to their respective locations.
        // Call code to move the cursor here.  Put this code immediately
        // before the 3D rendering loop so there is not additional latency
        // due to intervening operations.
        double ms = d_osc_time.elapsed();
        double sec = ms / 1000;
        int x = d_width / 2 + d_oscillate_mag * cos(sec * (2* M_PI * d_oscillate_freq));
        int y = d_height / 2 + d_oscillate_mag * sin(sec * (2* M_PI * d_oscillate_freq));
        double phase_cycles = d_oscillate_phase / 360.0;
        int box_x = d_width / 2 + d_oscillate_mag *
                cos(2* M_PI * (sec * d_oscillate_freq + phase_cycles));
        int box_y = d_height / 2 + d_oscillate_mag *
                sin(2* M_PI * (sec * d_oscillate_freq + phase_cycles));
        QCursor::setPos(QWidget::mapToGlobal(QPoint(x,y)));
        d_x = box_x;
        d_y = d_height - box_y;    // Y inverted for OpenGL and mouse
    }

    qglClearColor(d_clearColor);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();
    glTranslatef(0.0, 0.0, -10.0);

    // Translate and scale so that a quad from -1 to 1 in X and Y will
    // render correctly.  This lets us re-use our vertex array and not have
    // to change it every frame.
    glTranslatef(d_x, d_y, 0);
    glScalef(d_r, d_r, 0);

    // Draw a square box at the specified coordinates.
    // Overdraw this as many times as we are asked to draw quads.
    glColor3f(1,0,0);
    glEnableClientState( GL_VERTEX_ARRAY );
    glVertexPointer( 3, GL_FLOAT, 0, d_vertex_array );
    glDrawArrays( GL_TRIANGLES, 0, d_num_quads * 6 );
    glDisableClientState( GL_VERTEX_ARRAY );

    // Draw instructions on the screen.  This will provide additional rendering load,
    // but it did not change the results in our early testing.
    glColor3f(1,1,1);
    renderText(30, 30, "OSVR 2D vs. 3D rendering latency test program version 03.00.00 (run with -fullscreen to remove borders)");
    renderText(50, 50, "Press + to increase oscillation");
    renderText(50, 70, "Press - to decrease oscillation");
    renderText(50, 90, "Until you find the slowest oscillation where the cursor and square are in phase");

    renderText(50,110, "Press o to toggle oscillation");
    renderText(50,130, "Press ESC/q to quit");
    renderText(50,150, "Left mouse button drags square when not oscillating");
    renderText(50,170, "Middle mouse button teleports mouse when not oscillating");
    renderText(50,190, "Right mouse button toggles background brightness");

    double latency = (d_oscillate_phase/360) / (d_oscillate_freq);
    double latency_ms = latency * 1000;
    std::stringstream msg;
    msg << "Latency = " << latency_ms << "ms, Phase = " << d_oscillate_phase << " degrees";
    renderText(50,220, msg.str().c_str());

    if (++d_frame_count == 30) {
        double now = d_osc_time.elapsed() / 1000.0;
        double duration = now - d_last_frame_time;
        msg.str("");
        msg << "Rendering " << d_num_quads << " quads; ";
        msg << "Frame rate = " << d_frame_count/duration << " Hz";
        d_frame_msg = msg.str();
        d_last_frame_time = now;
        d_frame_count = 0;
    }
    renderText(50,240, d_frame_msg.c_str());

}

void OpenGL_Widget::resizeGL(int width, int height)
{
    d_width = width;
    d_height = height;
    glViewport(0, 0, width, height);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    // Make the window one unit high (-0.5 to 0.5) and have an aspect ratio that matches
    // the aspect ratio of the window.  We also make the left side of the window be at
    // the origin.
    float aspect;
    if ((height <= 0) || (width < 0)) {
        aspect = 1.0;
    } else {
        aspect = static_cast<float>(width)/height;
    }
    glOrtho(0, d_width-1, 0, d_height-1, 5.0, 15.0);
    glMatrixMode(GL_MODELVIEW);
    d_x = d_width / 2;
    d_y = d_height / 2;    
}


void OpenGL_Widget::keyPressEvent(QKeyEvent *event)
{
    switch (event->key()) {
    case Qt::Key_Escape:
    case Qt::Key_Q:
        //d_timer->stop();
        QApplication::quit();
        break;
    case Qt::Key_Plus:
        d_oscillate_phase += 2.5;
        break;
    case Qt::Key_Minus:
        d_oscillate_phase -= 2.5;
        break;
    case Qt::Key_O:
        d_oscillate = !d_oscillate;
        break;
    case Qt::Key_Left:
        break;
    case Qt::Key_Right:
        break;
    case Qt::Key_Down:
        break;
    case Qt::Key_Up:
        break;
    case Qt::Key_R:
        break;
    }

    updateGL();
}

void OpenGL_Widget::mousePressEvent(QMouseEvent *event)
{
    if (event->button() == Qt::LeftButton) {
        // Invert the mouse y position because the coordinates are
        // upside down in OpenGL compared to the mouse.
        d_x = event->x();
        d_y = d_height - event->y();
    }
    if (event->button() == Qt::RightButton) {
        d_clearColor = Qt::white;
    }
    if (event->button() == Qt::MiddleButton) {
        QPoint newPos = event->pos();
        newPos.setX(newPos.x() - 50);
        newPos.setY(newPos.y() - 50);
        QCursor::setPos(QWidget::mapToGlobal(newPos));
    }
    updateGL();
}

void OpenGL_Widget::mouseReleaseEvent(QMouseEvent *event)
{
    if (event->button() == Qt::RightButton) {
        d_clearColor = Qt::black;
    }
    if (event->button() == Qt::MiddleButton) {
        QPoint newPos = event->pos();
        newPos.setX(newPos.x() + 50);
        newPos.setY(newPos.y() + 50);
        QCursor::setPos(QWidget::mapToGlobal(newPos));
    }
    updateGL();
}

void OpenGL_Widget::mouseMoveEvent(QMouseEvent *event)
{
    if (event->buttons() & Qt::LeftButton) {
        // Invert the mouse y position because the coordinates are
        // upside down in OpenGL compared to the mouse.
        d_x = event->x();
        d_y = d_height - event->y();
    } else if (event->buttons() & Qt::RightButton) {
    }
    updateGL();
}

void VRPN_API OpenGL_Widget::handleButton(void *userdata, vrpn_BUTTONCB info)
{
    OpenGL_Widget *me = static_cast<OpenGL_Widget*>(userdata);

    if (info.button == me->d_button_to_use) {
        if (info.state == 1) {
            // Just pressed our button, turn the background white.
            me->d_clearColor = Qt::white;
        }
        else {
            // Just released the button, turn the background black
            me->d_clearColor = Qt::black;
        }
    }
}

void VRPN_API OpenGL_Widget::handleAnalog(void *userdata, vrpn_ANALOGCB info)
{
    OpenGL_Widget *me = static_cast<OpenGL_Widget*>(userdata);

    vrpn_float64 value = info.channel[me->d_analog_to_use];
    struct timeval now;
    vrpn_gettimeofday(&now, NULL);

    // If we haven't yet had an analog value set (time = 0),
    // don't check for changes, just update the value and
    // store the time we got it.
    if (me->d_analog_last_trigger.tv_sec == 0) {
        me->d_last_analog_value = value;
        me->d_analog_last_trigger = now;
        return;
    }

    // If it hasn't been long enough since our last trigger, just
    // store the value and return.
    if (vrpn_TimevalDurationSeconds(now, me->d_analog_last_trigger) < ANALOG_DELAY) {
        me->d_last_analog_value = value;
        return;
    }

    // See if the specified analog value has changed by more than the
    // threshold amount.  If so, toggle the background color and store
    // the new value we're comparing against.  Also record the time so
    // we will wait long enough before triggering again.
    if (fabs(value - me->d_last_analog_value) > me->d_analog_threshold) {
        me->d_last_analog_value = value;
        me->d_analog_last_trigger = now;
        if (me->d_clearColor == Qt::black) {
            me->d_clearColor = Qt::white;
        }
        else {
            me->d_clearColor = Qt::black;
        }
    }
}

void VRPN_API OpenGL_Widget::handleTracker(void *userdata, vrpn_TRACKERCB info)
{
    OpenGL_Widget *me = static_cast<OpenGL_Widget*>(userdata);

    struct timeval now;
    vrpn_gettimeofday(&now, NULL);

    //=======================================================================
    // If we're doing a rotation based on an axis, keep a running average of the
    // mean and standard deviation of recent measurements and adjust the brightness
    // to track within it.
    if ((me->d_tracker_rotate_axis >= 0) && (me->d_tracker_rotate_axis <= 2)) {

        // Convert the tracker's quaternion into Euler angles.  Then pull out
        // the appropraite Euler component (0, 1, or 2) so we can base our
        // calculations in it.
        q_vec_type yawPitchRoll;
        q_to_euler(yawPitchRoll, info.quat);
        double value = yawPitchRoll[me->d_tracker_rotate_axis];

        // Find the minimum and maximum rotation we've encountered.
        if (value < me->d_tracker_min_rotation) { me->d_tracker_min_rotation = value; }
        if (value > me->d_tracker_max_rotation) { me->d_tracker_max_rotation = value; }

        // Adjust the background brightness based on where we are with
        // respect to the minimum and maximum rotations
        double range = me->d_tracker_max_rotation - me->d_tracker_min_rotation;
        if (range == 0) { return; }
        double scaled = 255 * (value - me->d_tracker_min_rotation) / range;
        me->d_clearColor = QColor(scaled, scaled, scaled);

        return;
    }

    //=======================================================================
    // Not doing rotation, but rather bright/dark background based on a
    // motion threshold in translation or rotation.

    // If we haven't yet had a value set (time = 0),
    // don't check for changes, just update the location and
    // store the time we got it.
    if (me->d_tracker_last_trigger.tv_sec == 0) {
        memcpy(me->d_tracker_last_pos, info.pos, sizeof(me->d_tracker_last_pos));
        memcpy(me->d_tracker_last_quat, info.quat, sizeof(me->d_tracker_last_quat));
        me->d_tracker_last_trigger = now;
        return;
    }

    // If it hasn't been long enough since our last trigger, just
    // store the value and return.
    if (vrpn_TimevalDurationSeconds(now, me->d_tracker_last_trigger) < TRACKER_DELAY) {
        memcpy(me->d_tracker_last_pos, info.pos, sizeof(me->d_tracker_last_pos));
        memcpy(me->d_tracker_last_quat, info.quat, sizeof(me->d_tracker_last_quat));
        return;
    }

    // See if the position or rotation has changed by more than the
    // threshold amount.  If so, toggle the background color and store
    // the new value we're comparing against.  Also record the time so
    // we will wait long enough before triggering again.
    double deltaPos = q_vec_distance(info.pos, me->d_tracker_last_pos);
    double x, y, z, angle;
    q_type invert_old;
    q_invert(invert_old, me->d_tracker_last_quat);
    q_type composed;
    q_mult(composed, info.quat, invert_old);
    q_to_axis_angle(&x, &y, &z, &angle, composed);
    double deltaRot = fabs(angle);
    if ((deltaPos > me->d_tracker_trans_thresh) ||
        (deltaRot > me->d_tracker_rot_thresh)) {
        memcpy(me->d_tracker_last_pos, info.pos, sizeof(me->d_tracker_last_pos));
        memcpy(me->d_tracker_last_quat, info.quat, sizeof(me->d_tracker_last_quat));
        me->d_tracker_last_trigger = now;
        if (me->d_clearColor == Qt::black) {
            me->d_clearColor = Qt::white;
        }
        else {
            me->d_clearColor = Qt::black;
        }
    }
}

void OpenGL_Widget::setNumQuads(int num)
{
    d_num_quads = num;
    updateVertexArray();
}

// Delete any old array and make a new one based on the size.
// Fill it with two triangles per quad that together fill the
// region -1 to 1 in X and Y.  Make as many quads as we've been
// asked to.

void OpenGL_Widget::updateVertexArray(void)
{
    // It is safe to call delete[] on a NULL pointer.
    // Resize our buffer to suit.
    delete [] d_vertex_array;
    d_vertex_array = new GLfloat[d_num_quads * 3 * 6];

    // Fill in the buffer with the appropriate coordinates.
    for (int i = 0; i < d_num_quads; i++) {
        GLfloat *base = &d_vertex_array[i*3*6];

        // Make sure we get the order right, so that we get
        // front-facing triangles.

        // First coordinate, first triangle (X, Y, Z)
        *(base++) = -1; (*base++) = -1; *(base++) = 0;
        // Second coordinate, first triangle
        *(base++) =  1; (*base++) =  1; *(base++) = 0;
        // Third coordinate, first triangle
        *(base++) = -1; (*base++) =  1; *(base++) = 0;

        // First coordinate, second triangle (X, Y, Z)
        *(base++) =  1; (*base++) =  1; *(base++) = 0;
        // Second coordinate, second triangle
        *(base++) = -1; (*base++) = -1; *(base++) = 0;
        // Third coordinate, second triangle
        *(base++) =  1; (*base++) = -1; *(base++) = 0;
    }
}