1 files changed, 103 insertions, 28 deletions

diff --git a/pose-widget/pose-widget.cpp b/pose-widget/pose-widget.cpp
index 3ad475cf..c5a6b78e 100644
--- a/pose-widget/pose-widget.cpp
+++ b/pose-widget/pose-widget.cpp
@@ -13,12 +13,40 @@
 #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();
 }
 
 void pose_widget::present(double yaw, double pitch, double roll, double x, double y, double z)
@@ -29,40 +57,87 @@ void pose_widget::present(double yaw, double pitch, double roll, double x, doubl
     repaint();
 }
 
+void pose_widget::resizeEvent(QResizeEvent *event)
+{
+    // adapt to widget size
+    float w = event->size().width();
+    float h = event->size().height();
+
+    // fill background by color
+    constexpr int clr = 220;
+    QImage background(QImage(w, h, QImage::Format_ARGB32));
+    background.fill(QColor(clr,clr,clr));
+
+    // draw axes
+    QPainter p(&background);
+    p.setPen(QPen(Qt::gray, 1, Qt::SolidLine));
+    p.drawLine(0.5*w,   0  , 0.5*w,   h  );
+    p.drawLine(  0  , 0.5*h,   w  , 0.5*h);
+
+    // set AutoFillBackground
+    QPalette palette;
+    palette.setBrush(this->backgroundRole(), QBrush(background));
+    setPalette(palette);
+    setAutoFillBackground(true);
+
+    // 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());
+}
+
 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)
+
+    // get a local copy of input data
     auto [ yaw, pitch, roll ] = R;
     auto [ x, y, z ] = T;
 
-    const QImage& img = (std::fabs(pitch) > 90) ^ (std::fabs(yaw) > 90)
-                        ? back
-                        : front;
-
-    constexpr double scale = .75;
-    int w = img.width(), h = iround(img.height()*scale);
-
-    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;
-    double sx = clamp(.4 + -z * z_scale, .1, 2), sy = sx * 1/scale;
-
-    t.scale(sx, sy);
-
-    t.rotate(pitch, Qt::XAxis);
-    t.rotate(yaw, Qt::YAxis);
-    t.rotate(roll, Qt::ZAxis);
-
-    t.translate(x * xy / sx, y * xy / sy);
-
-    t.translate(w*-.5, h*-.5);
-
     QPainter p(this);
+    #ifdef TEST
+    // use antialiasing for correct frame around the Octopus, only if TEST defined
+    p.setRenderHint(QPainter::Antialiasing, true);
+    #endif
+
+    // 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);
+
+    // 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);
 }
 
 QSize pose_widget::sizeHint() const