1 files changed, 98 insertions, 31 deletions

diff --git a/pose-widget/pose-widget.cpp b/pose-widget/pose-widget.cpp
index 53bf214f..ac3aa74a 100644
--- a/pose-widget/pose-widget.cpp
+++ b/pose-widget/pose-widget.cpp
@@ -13,12 +13,42 @@
 #include <QtEvents>
 
 #include <QDebug>
-#include <QImage>
+#include <QQuaternion>
+#include <QMatrix4x4>
 
 namespace pose_widget_impl {
 
 pose_widget::pose_widget(QWidget* parent) : QWidget(parent)
 {
+    QPainter p;
+#ifdef TEST
+    //draw rectangle frame around of Octopus, only if TEST defined
+    p.begin(&front);
+    p.setPen(QPen(Qt::red, 3, Qt::SolidLine));
+    p.drawRect(0, 0, front.width()-1, front.height()-1);
+    p.end();
+
+    p.begin(&back);
+    p.setPen(QPen(Qt::darkGreen, 3, Qt::SolidLine));
+    p.drawRect(0, 0, back.width()-1, back.height()-1);
+    p.end();
+#endif
+
+    //draw Octopus shine
+    shine.fill(QColor(255,255,255));
+    p.begin(&shine);
+    p.setCompositionMode(QPainter::CompositionMode_DestinationIn);
+    p.drawImage(QPointF(0,0), front);		
+    p.end();
+
+    //draw Octopus shadow
+    shadow.fill(QColor(0,0,0));
+    p.begin(&shadow);
+    p.setCompositionMode(QPainter::CompositionMode_DestinationIn);
+    p.drawImage(QPointF(0,0), front);		
+    p.end();
+
+    mirror.setFocusPolicy(Qt::NoFocus);
 }
 
 void pose_widget::present(double yaw, double pitch, double roll, double x, double y, double z)
@@ -29,44 +59,81 @@ void pose_widget::present(double yaw, double pitch, double roll, double x, doubl
     repaint();
 }
 
-void pose_widget::paintEvent(QPaintEvent*)
+void pose_widget::resizeEvent(QResizeEvent *event)
 {
-    auto [ yaw, pitch, roll ] = R;
-    auto [ x, y, z ] = T;
-
-    const QImage& img = (std::fabs(pitch) > 90) ^ (std::fabs(yaw) > 90)
-                        ? back
-                        : front;
-
-    int w = img.width(), h = img.height();
-
-    QTransform t;
-
-    t.translate(w*.5, h*.5);
-
-    constexpr double z_scale = 1./250;
-    constexpr double xy_scale = .0075;
-    double xy = std::sqrt(w*w + h*h) * xy_scale;
+    // adapt to widget size
+    float w = event->size().width();
+    float h = event->size().height();
 
-    double s = clamp(.4 + -z * z_scale, .1, 2);
-    t.scale(s, s);
-
-    t.rotate(pitch, Qt::XAxis);
-    t.rotate(yaw, Qt::YAxis);
-    t.rotate(roll, Qt::ZAxis);
+    // move the mirror checkbox in the lower right corner of the widget
+    mirror.setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
+    mirror.move(w - mirror.width(), h - mirror.height());
+}
 
-    t.translate(x * xy / s, y * xy / s);
+void pose_widget::paintEvent(QPaintEvent*)
+{
+    // widget settings:
+    constexpr float scale  =  0.5;   // scale of Octopus height, when x = y = z = 0.0
+    constexpr float XYZmax = 50.0;   // -XYZmax < x,y,z < +XYZmax (offset the Octopus by one body)
+    constexpr float Kz     =  0.25;  // Z scale change limit (simulate camera focus length)
 
-    t.translate(w*-.5, h*-.5);
+    // get a local copy of input data
+    auto [ yaw, pitch, roll ] = R;
+    auto [ x, y, z ] = T;
 
     QPainter p(this);
+    #ifdef TEST
+    // use antialiasing for correct frame around the Octopus, only if TEST defined
+    p.setRenderHint(QPainter::Antialiasing, true);
+    #endif
+
+    {
+        p.fillRect(rect(), palette().brush(backgroundRole()));
+        // draw axes
+        p.save();
+        p.setPen(QPen(Qt::gray, 1, Qt::SolidLine));
+        int w = width(), h = height();
+        p.drawLine(w/2,   0, w/2, h);
+        p.drawLine(  0, h/2, w,   h/2);
+        p.restore();
+    }
+
+    // check mirror state
+    if   (mirror.checkState() == Qt::Checked) x = -x;
+    else { yaw = -yaw; roll = -roll; }
+    y = -y;
+
+    // rotations
+    QQuaternion q = QQuaternion::fromEulerAngles(pitch,  yaw,  roll);
+    QMatrix4x4  m = QMatrix4x4(q.toRotationMatrix());
+
+    // x and y positions
+    const float Kxy = (float)front.height() / XYZmax;
+    QVector3D v(Kxy*x, Kxy*y, 0.0);
+    v = m.transposed().map(v);
+    m.translate(v);
+
+    // perspective projection to x-y plane
+    QTransform t = m.toTransform(1024).translate(-.5 * front.width(), -.5 * front.height());
+
+    // z position by setViewport 
+    const float mz = scale * height()/front.height()/exp(1.0) * exp(1.0 - z * (Kz/XYZmax)); 
+    p.setViewport(QRect(.5 * width(), .5 * height(), width()*mz, height()*mz));
+ 
+    // define forward or backward side by cross product of mapped x and y axes
+    QPointF point0 =  t.map(QPointF(0, 0));
+    QPointF x_dir  = (t.map(QPointF(1, 0)) -= point0);
+    QPointF y_dir  = (t.map(QPointF(0, 1)) -= point0);
+    const bool  forward  =  x_dir.ry()*y_dir.rx() - x_dir.rx()*y_dir.ry() < 0 ? true : false;
+
+    // draw red or green Octopus
     p.setTransform(t);
-    p.drawImage(rect(), img);
-}
+    p.drawImage(QPointF(0,0), forward ? front : back);
 
-QSize pose_widget::sizeHint() const
-{
-    return { 1 << 16, 1 << 16 };
+    // top lighting simulation
+    const float alpha = sin(pitch * M_PI / 180.0);
+    p.setOpacity(0.333 * fabs(alpha));
+    p.drawImage(QPointF(0,0), forward == (alpha >= 0.0) ? shine : shadow);
 }
 
 } // ns pose_widget_impl