diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index dec0ea3d4803a7b452f5deb604643ef6ad8db06a..c2d8fcf4e1c8d2c6a28a561296e16365964b6e8a 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -361,7 +361,8 @@ HEADERS += src/MG.h \ src/ui/mavlink/QGCMAVLinkMessageSender.h \ src/ui/firmwareupdate/QGCFirmwareUpdateWidget.h \ src/ui/QGCPluginHost.h \ - src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h + src/ui/firmwareupdate/QGCPX4FirmwareUpdate.h \ + src/ui/map3D/gpl.h # Google Earth is only supported on Mac OS and Windows with Visual Studio Compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::HEADERS += src/ui/map3D/QGCGoogleEarthView.h @@ -494,7 +495,8 @@ SOURCES += src/main.cc \ src/ui/mavlink/QGCMAVLinkMessageSender.cc \ src/ui/firmwareupdate/QGCFirmwareUpdateWidget.cc \ src/ui/QGCPluginHost.cc \ - src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc + src/ui/firmwareupdate/QGCPX4FirmwareUpdate.cc \ + src/ui/map3D/gpl.cc # Enable Google Earth only on Mac OS and Windows with Visual Studio compiler macx|macx-g++|macx-g++42|win32-msvc2008|win32-msvc2010::SOURCES += src/ui/map3D/QGCGoogleEarthView.cc diff --git a/src/ui/map3D/Pixhawk3DWidget.cc b/src/ui/map3D/Pixhawk3DWidget.cc index 71ca8c12b493248936ef7eade92f1e7c9ac2f1cb..3be202147f8a35eb82df5d0d3a655d628f2f0c43 100644 --- a/src/ui/map3D/Pixhawk3DWidget.cc +++ b/src/ui/map3D/Pixhawk3DWidget.cc @@ -43,6 +43,7 @@ #include "UASManager.h" #include "QGC.h" +#include "gpl.h" #ifdef QGC_PROTOBUF_ENABLED #include @@ -55,7 +56,7 @@ Pixhawk3DWidget::Pixhawk3DWidget(QWidget* parent) , mode(DEFAULT_MODE) , selectedWpIndex(-1) , displayGrid(true) - , displayTrail(false) + , displayTrail(true) , displayImagery(true) , displayWaypoints(true) , displayRGBD2D(false) @@ -831,6 +832,20 @@ Pixhawk3DWidget::mousePressEvent(QMouseEvent* event) Q3DWidget::mousePressEvent(event); } +void +Pixhawk3DWidget::showEvent(QShowEvent* event) +{ + Q3DWidget::showEvent(event); + emit visibilityChanged(true); +} + +void +Pixhawk3DWidget::hideEvent(QHideEvent* event) +{ + Q3DWidget::hideEvent(event); + emit visibilityChanged(false); +} + void Pixhawk3DWidget::mouseMoveEvent(QMouseEvent* event) { @@ -1242,10 +1257,10 @@ Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ) } } - osg::Geometry* geometry = reinterpret_cast(trailNode->getDrawable(0)); - osg::Vec3dArray* vertices = reinterpret_cast(geometry->getVertexArray()); + osg::Geometry* geometry = trailNode->getDrawable(0)->asGeometry(); osg::DrawArrays* drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0)); - vertices->clear(); + + osg::ref_ptr vertices(new osg::Vec3Array); for (int i = 0; i < trail.size(); ++i) { vertices->push_back(osg::Vec3d(trail[i].y() - robotY, @@ -1253,6 +1268,7 @@ Pixhawk3DWidget::updateTrail(double robotX, double robotY, double robotZ) -(trail[i].z() - robotZ))); } + geometry->setVertexArray(vertices); drawArrays->setFirst(0); drawArrays->setCount(vertices->size()); geometry->dirtyBound(); @@ -1359,137 +1375,6 @@ Pixhawk3DWidget::updateTarget(double robotX, double robotY) sd->setColor(osg::Vec4f(1.0f, 0.8f, 0.0f, 1.0f)); } -float colormap_jet[128][3] = { - {0.0f,0.0f,0.53125f}, - {0.0f,0.0f,0.5625f}, - {0.0f,0.0f,0.59375f}, - {0.0f,0.0f,0.625f}, - {0.0f,0.0f,0.65625f}, - {0.0f,0.0f,0.6875f}, - {0.0f,0.0f,0.71875f}, - {0.0f,0.0f,0.75f}, - {0.0f,0.0f,0.78125f}, - {0.0f,0.0f,0.8125f}, - {0.0f,0.0f,0.84375f}, - {0.0f,0.0f,0.875f}, - {0.0f,0.0f,0.90625f}, - {0.0f,0.0f,0.9375f}, - {0.0f,0.0f,0.96875f}, - {0.0f,0.0f,1.0f}, - {0.0f,0.03125f,1.0f}, - {0.0f,0.0625f,1.0f}, - {0.0f,0.09375f,1.0f}, - {0.0f,0.125f,1.0f}, - {0.0f,0.15625f,1.0f}, - {0.0f,0.1875f,1.0f}, - {0.0f,0.21875f,1.0f}, - {0.0f,0.25f,1.0f}, - {0.0f,0.28125f,1.0f}, - {0.0f,0.3125f,1.0f}, - {0.0f,0.34375f,1.0f}, - {0.0f,0.375f,1.0f}, - {0.0f,0.40625f,1.0f}, - {0.0f,0.4375f,1.0f}, - {0.0f,0.46875f,1.0f}, - {0.0f,0.5f,1.0f}, - {0.0f,0.53125f,1.0f}, - {0.0f,0.5625f,1.0f}, - {0.0f,0.59375f,1.0f}, - {0.0f,0.625f,1.0f}, - {0.0f,0.65625f,1.0f}, - {0.0f,0.6875f,1.0f}, - {0.0f,0.71875f,1.0f}, - {0.0f,0.75f,1.0f}, - {0.0f,0.78125f,1.0f}, - {0.0f,0.8125f,1.0f}, - {0.0f,0.84375f,1.0f}, - {0.0f,0.875f,1.0f}, - {0.0f,0.90625f,1.0f}, - {0.0f,0.9375f,1.0f}, - {0.0f,0.96875f,1.0f}, - {0.0f,1.0f,1.0f}, - {0.03125f,1.0f,0.96875f}, - {0.0625f,1.0f,0.9375f}, - {0.09375f,1.0f,0.90625f}, - {0.125f,1.0f,0.875f}, - {0.15625f,1.0f,0.84375f}, - {0.1875f,1.0f,0.8125f}, - {0.21875f,1.0f,0.78125f}, - {0.25f,1.0f,0.75f}, - {0.28125f,1.0f,0.71875f}, - {0.3125f,1.0f,0.6875f}, - {0.34375f,1.0f,0.65625f}, - {0.375f,1.0f,0.625f}, - {0.40625f,1.0f,0.59375f}, - {0.4375f,1.0f,0.5625f}, - {0.46875f,1.0f,0.53125f}, - {0.5f,1.0f,0.5f}, - {0.53125f,1.0f,0.46875f}, - {0.5625f,1.0f,0.4375f}, - {0.59375f,1.0f,0.40625f}, - {0.625f,1.0f,0.375f}, - {0.65625f,1.0f,0.34375f}, - {0.6875f,1.0f,0.3125f}, - {0.71875f,1.0f,0.28125f}, - {0.75f,1.0f,0.25f}, - {0.78125f,1.0f,0.21875f}, - {0.8125f,1.0f,0.1875f}, - {0.84375f,1.0f,0.15625f}, - {0.875f,1.0f,0.125f}, - {0.90625f,1.0f,0.09375f}, - {0.9375f,1.0f,0.0625f}, - {0.96875f,1.0f,0.03125f}, - {1.0f,1.0f,0.0f}, - {1.0f,0.96875f,0.0f}, - {1.0f,0.9375f,0.0f}, - {1.0f,0.90625f,0.0f}, - {1.0f,0.875f,0.0f}, - {1.0f,0.84375f,0.0f}, - {1.0f,0.8125f,0.0f}, - {1.0f,0.78125f,0.0f}, - {1.0f,0.75f,0.0f}, - {1.0f,0.71875f,0.0f}, - {1.0f,0.6875f,0.0f}, - {1.0f,0.65625f,0.0f}, - {1.0f,0.625f,0.0f}, - {1.0f,0.59375f,0.0f}, - {1.0f,0.5625f,0.0f}, - {1.0f,0.53125f,0.0f}, - {1.0f,0.5f,0.0f}, - {1.0f,0.46875f,0.0f}, - {1.0f,0.4375f,0.0f}, - {1.0f,0.40625f,0.0f}, - {1.0f,0.375f,0.0f}, - {1.0f,0.34375f,0.0f}, - {1.0f,0.3125f,0.0f}, - {1.0f,0.28125f,0.0f}, - {1.0f,0.25f,0.0f}, - {1.0f,0.21875f,0.0f}, - {1.0f,0.1875f,0.0f}, - {1.0f,0.15625f,0.0f}, - {1.0f,0.125f,0.0f}, - {1.0f,0.09375f,0.0f}, - {1.0f,0.0625f,0.0f}, - {1.0f,0.03125f,0.0f}, - {1.0f,0.0f,0.0f}, - {0.96875f,0.0f,0.0f}, - {0.9375f,0.0f,0.0f}, - {0.90625f,0.0f,0.0f}, - {0.875f,0.0f,0.0f}, - {0.84375f,0.0f,0.0f}, - {0.8125f,0.0f,0.0f}, - {0.78125f,0.0f,0.0f}, - {0.75f,0.0f,0.0f}, - {0.71875f,0.0f,0.0f}, - {0.6875f,0.0f,0.0f}, - {0.65625f,0.0f,0.0f}, - {0.625f,0.0f,0.0f}, - {0.59375f,0.0f,0.0f}, - {0.5625f,0.0f,0.0f}, - {0.53125f,0.0f,0.0f}, - {0.5f,0.0f,0.0f} -}; - #ifdef QGC_PROTOBUF_ENABLED void Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) @@ -1517,9 +1402,11 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) int idx = fminf(depth[c], 7.0f) / 7.0f * 127.0f; idx = 127 - idx; - pixel[0] = colormap_jet[idx][2] * 255.0f; - pixel[1] = colormap_jet[idx][1] * 255.0f; - pixel[2] = colormap_jet[idx][0] * 255.0f; + float r, g, b; + qgc::colormap("jet", idx, r, g, b); + pixel[0] = r * 255.0f; + pixel[1] = g * 255.0f; + pixel[2] = b * 255.0f; } pixel += 3; @@ -1562,10 +1449,11 @@ Pixhawk3DWidget::updateRGBD(double robotX, double robotY, double robotZ) { double dist = sqrt(x * x + y * y + z * z); int colorIndex = static_cast(fmin(dist / 7.0 * 127.0, 127.0)); - (*colors)[i].set(colormap_jet[colorIndex][0], - colormap_jet[colorIndex][1], - colormap_jet[colorIndex][2], - 1.0f); + + float r, g, b; + qgc::colormap("jet", colorIndex, r, g, b); + + (*colors)[i].set(r, g, b, 1.0f); } } @@ -1592,19 +1480,67 @@ Pixhawk3DWidget::updatePath(double robotX, double robotY, double robotZ) { px::Path path = uas->getPath(); - osg::Geometry* geometry = reinterpret_cast(pathNode->getDrawable(0)); - osg::Vec3dArray* vertices = reinterpret_cast(geometry->getVertexArray()); + osg::Geometry* geometry = pathNode->getDrawable(0)->asGeometry(); osg::DrawArrays* drawArrays = reinterpret_cast(geometry->getPrimitiveSet(0)); - vertices->clear(); - for (int i = 0; i < path.waypoints_size(); ++i) + osg::Vec4Array* colorArray = reinterpret_cast(geometry->getColorArray()); + + geometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX); + osg::ref_ptr linewidth(new osg::LineWidth()); + linewidth->setWidth(2.0f); + geometry->getStateSet()->setAttributeAndModes(linewidth, osg::StateAttribute::ON); + + colorArray->clear(); + + osg::ref_ptr vertices(new osg::Vec3Array); + + // find path length + float length = 0.0f; + for (int i = 0; i < path.waypoints_size() - 1; ++i) + { + const px::Waypoint& wp0 = path.waypoints(i); + const px::Waypoint& wp1 = path.waypoints(i+1); + + length += qgc::hypot3f(wp0.x() - wp1.x(), + wp0.y() - wp1.y(), + wp0.z() - wp1.z()); + } + + // build path + if (path.waypoints_size() > 0) + { + const px::Waypoint& wp0 = path.waypoints(0); + + vertices->push_back(osg::Vec3d(wp0.y() - robotY, + wp0.x() - robotX, + -(wp0.z() - robotZ))); + + float r, g, b; + qgc::colormap("autumn", 0, r, g, b); + colorArray->push_back(osg::Vec4d(r, g, b, 1.0f)); + } + + float lengthCurrent = 0.0f; + for (int i = 0; i < path.waypoints_size() - 1; ++i) { - const px::Waypoint& wp = path.waypoints(i); + const px::Waypoint& wp0 = path.waypoints(i); + const px::Waypoint& wp1 = path.waypoints(i+1); - vertices->push_back(osg::Vec3d(wp.y() - robotY, - wp.x() - robotX, - -(wp.z() - robotZ))); + lengthCurrent += qgc::hypot3f(wp0.x() - wp1.x(), + wp0.y() - wp1.y(), + wp0.z() - wp1.z()); + + vertices->push_back(osg::Vec3d(wp1.y() - robotY, + wp1.x() - robotX, + -(wp1.z() - robotZ))); + + int colorIdx = lengthCurrent / length * 127.0f; + + float r, g, b; + qgc::colormap("autumn", colorIdx, r, g, b); + colorArray->push_back(osg::Vec4f(r, g, b, 1.0f)); } + geometry->setVertexArray(vertices); drawArrays->setFirst(0); drawArrays->setCount(vertices->size()); geometry->dirtyBound(); diff --git a/src/ui/map3D/Pixhawk3DWidget.h b/src/ui/map3D/Pixhawk3DWidget.h index cc9933d4a1a64c4ab53064c382704fa47cc95346..2023d80a8f5bc01a390bf8826ebd02cd538f29e6 100644 --- a/src/ui/map3D/Pixhawk3DWidget.h +++ b/src/ui/map3D/Pixhawk3DWidget.h @@ -87,17 +87,8 @@ protected: virtual void display(void); virtual void keyPressEvent(QKeyEvent* event); virtual void mousePressEvent(QMouseEvent* event); - void showEvent(QShowEvent* event) - { - QWidget::showEvent(event); - emit visibilityChanged(true); - } - - void hideEvent(QHideEvent* event) - { - QWidget::hideEvent(event); - emit visibilityChanged(false); - } + virtual void showEvent(QShowEvent* event); + virtual void hideEvent(QHideEvent* event); virtual void mouseMoveEvent(QMouseEvent* event); UASInterface* uas; diff --git a/src/ui/map3D/gpl.cc b/src/ui/map3D/gpl.cc new file mode 100644 index 0000000000000000000000000000000000000000..ae0240c64bac8d2743bea775d9a01caa4ee42da8 --- /dev/null +++ b/src/ui/map3D/gpl.cc @@ -0,0 +1,333 @@ +#include "gpl.h" + +namespace qgc +{ + +double hypot3(double x, double y, double z) +{ + return sqrt(square(x) + square(y) + square(z)); +} + +float hypot3f(float x, float y, float z) +{ + return sqrtf(square(x) + square(y) + square(z)); +} + +double d2r(double deg) +{ + return deg / 180.0 * M_PI; +} + +float d2r(float deg) +{ + return deg / 180.0f * M_PI; +} + +double r2d(double rad) +{ + return rad / M_PI * 180.0; +} + +float r2d(float rad) +{ + return rad / M_PI * 180.0f; +} + +float colormapAutumn[128][3] = +{ + {1.0f,0.f,0.f}, + {1.0f,0.007874f,0.f}, + {1.0f,0.015748f,0.f}, + {1.0f,0.023622f,0.f}, + {1.0f,0.031496f,0.f}, + {1.0f,0.03937f,0.f}, + {1.0f,0.047244f,0.f}, + {1.0f,0.055118f,0.f}, + {1.0f,0.062992f,0.f}, + {1.0f,0.070866f,0.f}, + {1.0f,0.07874f,0.f}, + {1.0f,0.086614f,0.f}, + {1.0f,0.094488f,0.f}, + {1.0f,0.10236f,0.f}, + {1.0f,0.11024f,0.f}, + {1.0f,0.11811f,0.f}, + {1.0f,0.12598f,0.f}, + {1.0f,0.13386f,0.f}, + {1.0f,0.14173f,0.f}, + {1.0f,0.14961f,0.f}, + {1.0f,0.15748f,0.f}, + {1.0f,0.16535f,0.f}, + {1.0f,0.17323f,0.f}, + {1.0f,0.1811f,0.f}, + {1.0f,0.18898f,0.f}, + {1.0f,0.19685f,0.f}, + {1.0f,0.20472f,0.f}, + {1.0f,0.2126f,0.f}, + {1.0f,0.22047f,0.f}, + {1.0f,0.22835f,0.f}, + {1.0f,0.23622f,0.f}, + {1.0f,0.24409f,0.f}, + {1.0f,0.25197f,0.f}, + {1.0f,0.25984f,0.f}, + {1.0f,0.26772f,0.f}, + {1.0f,0.27559f,0.f}, + {1.0f,0.28346f,0.f}, + {1.0f,0.29134f,0.f}, + {1.0f,0.29921f,0.f}, + {1.0f,0.30709f,0.f}, + {1.0f,0.31496f,0.f}, + {1.0f,0.32283f,0.f}, + {1.0f,0.33071f,0.f}, + {1.0f,0.33858f,0.f}, + {1.0f,0.34646f,0.f}, + {1.0f,0.35433f,0.f}, + {1.0f,0.3622f,0.f}, + {1.0f,0.37008f,0.f}, + {1.0f,0.37795f,0.f}, + {1.0f,0.38583f,0.f}, + {1.0f,0.3937f,0.f}, + {1.0f,0.40157f,0.f}, + {1.0f,0.40945f,0.f}, + {1.0f,0.41732f,0.f}, + {1.0f,0.4252f,0.f}, + {1.0f,0.43307f,0.f}, + {1.0f,0.44094f,0.f}, + {1.0f,0.44882f,0.f}, + {1.0f,0.45669f,0.f}, + {1.0f,0.46457f,0.f}, + {1.0f,0.47244f,0.f}, + {1.0f,0.48031f,0.f}, + {1.0f,0.48819f,0.f}, + {1.0f,0.49606f,0.f}, + {1.0f,0.50394f,0.f}, + {1.0f,0.51181f,0.f}, + {1.0f,0.51969f,0.f}, + {1.0f,0.52756f,0.f}, + {1.0f,0.53543f,0.f}, + {1.0f,0.54331f,0.f}, + {1.0f,0.55118f,0.f}, + {1.0f,0.55906f,0.f}, + {1.0f,0.56693f,0.f}, + {1.0f,0.5748f,0.f}, + {1.0f,0.58268f,0.f}, + {1.0f,0.59055f,0.f}, + {1.0f,0.59843f,0.f}, + {1.0f,0.6063f,0.f}, + {1.0f,0.61417f,0.f}, + {1.0f,0.62205f,0.f}, + {1.0f,0.62992f,0.f}, + {1.0f,0.6378f,0.f}, + {1.0f,0.64567f,0.f}, + {1.0f,0.65354f,0.f}, + {1.0f,0.66142f,0.f}, + {1.0f,0.66929f,0.f}, + {1.0f,0.67717f,0.f}, + {1.0f,0.68504f,0.f}, + {1.0f,0.69291f,0.f}, + {1.0f,0.70079f,0.f}, + {1.0f,0.70866f,0.f}, + {1.0f,0.71654f,0.f}, + {1.0f,0.72441f,0.f}, + {1.0f,0.73228f,0.f}, + {1.0f,0.74016f,0.f}, + {1.0f,0.74803f,0.f}, + {1.0f,0.75591f,0.f}, + {1.0f,0.76378f,0.f}, + {1.0f,0.77165f,0.f}, + {1.0f,0.77953f,0.f}, + {1.0f,0.7874f,0.f}, + {1.0f,0.79528f,0.f}, + {1.0f,0.80315f,0.f}, + {1.0f,0.81102f,0.f}, + {1.0f,0.8189f,0.f}, + {1.0f,0.82677f,0.f}, + {1.0f,0.83465f,0.f}, + {1.0f,0.84252f,0.f}, + {1.0f,0.85039f,0.f}, + {1.0f,0.85827f,0.f}, + {1.0f,0.86614f,0.f}, + {1.0f,0.87402f,0.f}, + {1.0f,0.88189f,0.f}, + {1.0f,0.88976f,0.f}, + {1.0f,0.89764f,0.f}, + {1.0f,0.90551f,0.f}, + {1.0f,0.91339f,0.f}, + {1.0f,0.92126f,0.f}, + {1.0f,0.92913f,0.f}, + {1.0f,0.93701f,0.f}, + {1.0f,0.94488f,0.f}, + {1.0f,0.95276f,0.f}, + {1.0f,0.96063f,0.f}, + {1.0f,0.9685f,0.f}, + {1.0f,0.97638f,0.f}, + {1.0f,0.98425f,0.f}, + {1.0f,0.99213f,0.f}, + {1.0f,1.0f,0.0f} +}; + + +float colormapJet[128][3] = +{ + {0.0f,0.0f,0.53125f}, + {0.0f,0.0f,0.5625f}, + {0.0f,0.0f,0.59375f}, + {0.0f,0.0f,0.625f}, + {0.0f,0.0f,0.65625f}, + {0.0f,0.0f,0.6875f}, + {0.0f,0.0f,0.71875f}, + {0.0f,0.0f,0.75f}, + {0.0f,0.0f,0.78125f}, + {0.0f,0.0f,0.8125f}, + {0.0f,0.0f,0.84375f}, + {0.0f,0.0f,0.875f}, + {0.0f,0.0f,0.90625f}, + {0.0f,0.0f,0.9375f}, + {0.0f,0.0f,0.96875f}, + {0.0f,0.0f,1.0f}, + {0.0f,0.03125f,1.0f}, + {0.0f,0.0625f,1.0f}, + {0.0f,0.09375f,1.0f}, + {0.0f,0.125f,1.0f}, + {0.0f,0.15625f,1.0f}, + {0.0f,0.1875f,1.0f}, + {0.0f,0.21875f,1.0f}, + {0.0f,0.25f,1.0f}, + {0.0f,0.28125f,1.0f}, + {0.0f,0.3125f,1.0f}, + {0.0f,0.34375f,1.0f}, + {0.0f,0.375f,1.0f}, + {0.0f,0.40625f,1.0f}, + {0.0f,0.4375f,1.0f}, + {0.0f,0.46875f,1.0f}, + {0.0f,0.5f,1.0f}, + {0.0f,0.53125f,1.0f}, + {0.0f,0.5625f,1.0f}, + {0.0f,0.59375f,1.0f}, + {0.0f,0.625f,1.0f}, + {0.0f,0.65625f,1.0f}, + {0.0f,0.6875f,1.0f}, + {0.0f,0.71875f,1.0f}, + {0.0f,0.75f,1.0f}, + {0.0f,0.78125f,1.0f}, + {0.0f,0.8125f,1.0f}, + {0.0f,0.84375f,1.0f}, + {0.0f,0.875f,1.0f}, + {0.0f,0.90625f,1.0f}, + {0.0f,0.9375f,1.0f}, + {0.0f,0.96875f,1.0f}, + {0.0f,1.0f,1.0f}, + {0.03125f,1.0f,0.96875f}, + {0.0625f,1.0f,0.9375f}, + {0.09375f,1.0f,0.90625f}, + {0.125f,1.0f,0.875f}, + {0.15625f,1.0f,0.84375f}, + {0.1875f,1.0f,0.8125f}, + {0.21875f,1.0f,0.78125f}, + {0.25f,1.0f,0.75f}, + {0.28125f,1.0f,0.71875f}, + {0.3125f,1.0f,0.6875f}, + {0.34375f,1.0f,0.65625f}, + {0.375f,1.0f,0.625f}, + {0.40625f,1.0f,0.59375f}, + {0.4375f,1.0f,0.5625f}, + {0.46875f,1.0f,0.53125f}, + {0.5f,1.0f,0.5f}, + {0.53125f,1.0f,0.46875f}, + {0.5625f,1.0f,0.4375f}, + {0.59375f,1.0f,0.40625f}, + {0.625f,1.0f,0.375f}, + {0.65625f,1.0f,0.34375f}, + {0.6875f,1.0f,0.3125f}, + {0.71875f,1.0f,0.28125f}, + {0.75f,1.0f,0.25f}, + {0.78125f,1.0f,0.21875f}, + {0.8125f,1.0f,0.1875f}, + {0.84375f,1.0f,0.15625f}, + {0.875f,1.0f,0.125f}, + {0.90625f,1.0f,0.09375f}, + {0.9375f,1.0f,0.0625f}, + {0.96875f,1.0f,0.03125f}, + {1.0f,1.0f,0.0f}, + {1.0f,0.96875f,0.0f}, + {1.0f,0.9375f,0.0f}, + {1.0f,0.90625f,0.0f}, + {1.0f,0.875f,0.0f}, + {1.0f,0.84375f,0.0f}, + {1.0f,0.8125f,0.0f}, + {1.0f,0.78125f,0.0f}, + {1.0f,0.75f,0.0f}, + {1.0f,0.71875f,0.0f}, + {1.0f,0.6875f,0.0f}, + {1.0f,0.65625f,0.0f}, + {1.0f,0.625f,0.0f}, + {1.0f,0.59375f,0.0f}, + {1.0f,0.5625f,0.0f}, + {1.0f,0.53125f,0.0f}, + {1.0f,0.5f,0.0f}, + {1.0f,0.46875f,0.0f}, + {1.0f,0.4375f,0.0f}, + {1.0f,0.40625f,0.0f}, + {1.0f,0.375f,0.0f}, + {1.0f,0.34375f,0.0f}, + {1.0f,0.3125f,0.0f}, + {1.0f,0.28125f,0.0f}, + {1.0f,0.25f,0.0f}, + {1.0f,0.21875f,0.0f}, + {1.0f,0.1875f,0.0f}, + {1.0f,0.15625f,0.0f}, + {1.0f,0.125f,0.0f}, + {1.0f,0.09375f,0.0f}, + {1.0f,0.0625f,0.0f}, + {1.0f,0.03125f,0.0f}, + {1.0f,0.0f,0.0f}, + {0.96875f,0.0f,0.0f}, + {0.9375f,0.0f,0.0f}, + {0.90625f,0.0f,0.0f}, + {0.875f,0.0f,0.0f}, + {0.84375f,0.0f,0.0f}, + {0.8125f,0.0f,0.0f}, + {0.78125f,0.0f,0.0f}, + {0.75f,0.0f,0.0f}, + {0.71875f,0.0f,0.0f}, + {0.6875f,0.0f,0.0f}, + {0.65625f,0.0f,0.0f}, + {0.625f,0.0f,0.0f}, + {0.59375f,0.0f,0.0f}, + {0.5625f,0.0f,0.0f}, + {0.53125f,0.0f,0.0f}, + {0.5f,0.0f,0.0f} +}; + +bool colormap(const QString& name, unsigned char idx, + float& r, float& g, float& b) +{ + if (idx > 127) + { + return false; + } + + if (name.compare("jet") == 0) + { + float* color = colormapJet[idx]; + + r = color[0]; + g = color[1]; + b = color[2]; + + return true; + } + else if (name.compare("autumn") == 0) + { + float* color = colormapAutumn[idx]; + + r = color[0]; + g = color[1]; + b = color[2]; + + return true; + } + + return false; +} + +} diff --git a/src/ui/map3D/gpl.h b/src/ui/map3D/gpl.h new file mode 100644 index 0000000000000000000000000000000000000000..d5e9ce61aa89f4bcef81e1a7bf4352b5dc079fd2 --- /dev/null +++ b/src/ui/map3D/gpl.h @@ -0,0 +1,52 @@ +#ifndef GPL_H +#define GPL_H + +#include +#include + +namespace qgc +{ + +template +const T clamp(const T& v, const T& a, const T& b) +{ + return qMin(b, qMax(a, v)); +} + +double hypot3(double x, double y, double z); +float hypot3f(float x, float y, float z); + +template +const T normalizeTheta(const T& theta) +{ + T normTheta = theta; + + while (normTheta < - M_PI) + { + normTheta += 2.0 * M_PI; + } + while (normTheta > M_PI) + { + normTheta -= 2.0 * M_PI; + } + + return normTheta; +} + +double d2r(double deg); +float d2r(float deg); +double r2d(double rad); +float r2d(float rad); + +template +const T square(const T& x) +{ + return x * x; +} + +bool colormap(const QString& name, unsigned char idx, + float& r, float& g, float& b); + +} + +#endif