tcElevationOptimization::loadSnapShot: simplify reliability expression

[tecorrec.git] / geo / tcSrtmData.cpp

/***************************************************************************
 *   This file is part of Tecorrec.                                        *
 *   Copyright 2008 James Hogan <james@albanarts.com>                      *
 *                                                                         *
 *   Tecorrec is free software: you can redistribute it and/or modify      *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation, either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   Tecorrec is distributed in the hope that it will be useful,           *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with Tecorrec.  If not, write to the Free Software Foundation,  *
 *   Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.   *
 ***************************************************************************/

/**
 * @file tcSrtmData.cpp
 * @brief Block of raw SRTM elevation data.
 */

#include "tcSrtmData.h"
#include "tcElevationData.h"
#include <SplineDefinitions.h>

#include <QFile>
#include <QDataStream>

#include <cmath>

/*
 * Constructors + destructor
 */

/// Load SRTM data.
tcSrtmData::tcSrtmData(int lon, int lat, QFile& file, QObject* progressObj, const char* progressMember)
: m_lon(lon)
, m_lat(lat)
, m_lines(0)
, m_samples(0)
, m_data(0)
{
  connect(this, SIGNAL(progressing(const QString&)), progressObj, progressMember);

  qint64 elements = file.size() / sizeof(uint16_t);
  double side = sqrt(elements);
  m_lines = m_samples = (int)side;
  Q_ASSERT(m_lines*m_samples == elements);
  m_data = new uint16_t[2*m_lines*m_samples];
  Q_ASSERT(0 != m_data);

  QDataStream in(&file);
  in.setByteOrder(QDataStream::BigEndian);
  int lastPercent = -1;
  bool alreadyFilled = false;
  for (int i = 0; i < m_lines; ++i)
  {
    int percent = 20*i/(m_lines-1);
    if (percent != lastPercent)
    {
      lastPercent = percent;
      progressing(tr("Loading SRTM (%1,%2): %3%").arg(lon).arg(lat).arg(percent*5));
    }
    for (int j = 0; j < m_samples; ++j)
    {
      Q_ASSERT(!in.atEnd());
      uint16_t sample;
      in >> sample;
      if (sample & 0x8000 && sample != 0x8000)
      {
        alreadyFilled = true;
      }
      m_data[indexData(0, i, j)] = sample;
      m_data[indexData(1, i, j)] = sample;
    }
  }
  /// @todo Handle larger files!
  Q_ASSERT(in.atEnd());

  if (!alreadyFilled)
  {
    progressing(tr("Filling SRTM voids (%1,%2)").arg(lon).arg(lat));
    fillVoidsBicubic();
  }

  progressing(tr("SRTM (%1,%2): Complete").arg(lon).arg(lat));
}

/// Destructor.
tcSrtmData::~tcSrtmData()
{
  delete [] m_data;
}

/*
 * Accessors
 */

/// Get the longitude in degrees.
int tcSrtmData::lon() const
{
  return m_lon;
}

/// Get the latitude in degrees.
int tcSrtmData::lat() const
{
  return m_lat;
}

/// Get the number of scan lines.
int tcSrtmData::lines() const
{
  return m_lines;
}

/// Get the number of samples in each scan line.
int tcSrtmData::samples() const
{
  return m_samples;
}

/// Find the geographical angles between samples.
tcGeo tcSrtmData::sampleResolution() const
{
  return tcGeo(M_PI/180 / (m_samples - 1),
               M_PI/180 / (m_lines   - 1));
}

/// Get the geographical coordinate of a sample.
tcGeo tcSrtmData::sampleCoordinate(int line, int sample) const
{
  return tcGeo(M_PI/180*((double) m_lon    + (double)sample/(m_samples-1)),
               M_PI/180*((double)(m_lat+1) - (double)line  /(m_lines  -1)));
}

/// Does data exist for a sample?
bool tcSrtmData::sampleExists(int line, int sample) const
{
  Q_ASSERT(line >= 0 && line < m_lines);
  Q_ASSERT(sample >= 0 && sample < m_samples);
  return 0x0000 == (0x8000 & m_data[indexData(0, line, sample)]);
}

/// Get the altitude at a sample (interpolating gaps).
uint16_t tcSrtmData::sampleAltitude(int line, int sample, bool corrected, bool* exists) const
{
  Q_ASSERT(line >= 0 && line < m_lines);
  Q_ASSERT(sample >= 0 && sample < m_samples);
  if (0 != exists)
  {
    *exists = (0x0000 == (0x8000 & m_data[indexData(corrected ? 1 : 0, line, sample)]));
  }
  return 0x7FFF & m_data[indexData(corrected ? 1 : 0, line, sample)];
}

/// Get the altitude at a coordinate.
float tcSrtmData::sampleAltitude(const tcGeo& coord, bool corrected, bool* exists) const
{
  double dlon = coord.lon()*180.0/M_PI - m_lon;
  double dlat = coord.lat()*180.0/M_PI - m_lat;
  Q_ASSERT(dlon >= 0.0 && dlon < 1.0);
  Q_ASSERT(dlat >= 0.0 && dlat < 1.0);
  int line = (int)floor(dlat*(m_lines-1));
  float dy = dlat*(m_lines-1) - line;
  line = m_lines - 1 - line;
  int sample = (int)floor(dlon*(m_samples-1));
  float dx = dlon*(m_samples-1) - sample;
  Q_ASSERT(line > 0 && line < m_lines);
  Q_ASSERT(sample >= 0 && sample < m_samples-1);
  Q_ASSERT(dx >= 0.0f && dx <= 1.0f);
  Q_ASSERT(dy >= 0.0f && dy <= 1.0f);
  if (0 != exists)
  {
    *exists = (0x0000 == (0x8000 & m_data[indexData(corrected ? 1 : 0, line,   sample  )]))
           && (0x0000 == (0x8000 & m_data[indexData(corrected ? 1 : 0, line,   sample+1)]))
           && (0x0000 == (0x8000 & m_data[indexData(corrected ? 1 : 0, line-1, sample  )]))
           && (0x0000 == (0x8000 & m_data[indexData(corrected ? 1 : 0, line-1, sample+1)]));
  }
  uint16_t x00 = 0x7FFF & m_data[indexData(corrected ? 1 : 0, line,   sample  )];
  uint16_t x10 = 0x7FFF & m_data[indexData(corrected ? 1 : 0, line,   sample+1)];
  uint16_t x01 = 0x7FFF & m_data[indexData(corrected ? 1 : 0, line-1, sample  )];
  uint16_t x11 = 0x7FFF & m_data[indexData(corrected ? 1 : 0, line-1, sample+1)];
  float mdx = 1.0f-dx;
  float mdy = 1.0f-dy;
  return ((float)x00*mdx*mdy + (float)x01*mdx*dy + (float)x10*dx*mdy + (float)x11*dx*dy);
}

/*
 * Mutators
 */

/// Correct the altitude at a sample.
void tcSrtmData::setSampleAltitude(int line, int sample, uint16_t altitude)
{
  Q_ASSERT(line >= 0 && line < m_lines);
  Q_ASSERT(sample >= 0 && sample < m_samples);
  m_data[indexData(1, line, sample)] = altitude;
}

/// Update any samples affected by the elevation field.
void tcSrtmData::updateFromElevationData(const tcElevationData* elevation)
{
  tcGeo swCorner = elevation->swCorner();
  tcGeo neCorner = elevation->neCorner();
  int minl = qMax(0,         (int)floor(((double)m_lat+1 - neCorner.lat()*180.0/M_PI)*(m_lines-1)));
  int maxl = qMin(m_lines,   (int)ceil (((double)m_lat+1 - swCorner.lat()*180.0/M_PI)*(m_lines-1)));
  int mins = qMax(0,         (int)floor((swCorner.lon()*180.0/M_PI - m_lon)*(m_samples-1)));
  int maxs = qMin(m_samples, (int)ceil ((neCorner.lon()*180.0/M_PI - m_lon)*(m_samples-1)));
  // Go through all elevation samples to update
  for (int line = minl; line < maxl; ++line)
  {
    for (int sample = mins; sample < maxs; ++sample)
    {
      tcGeo corner1(((double) m_lon    + (-0.5 + sample)/(m_samples-1)) * M_PI/180,
                    ((double)(m_lat+1) - (-0.5 + line  )/(m_lines  -1)) * M_PI/180);
      tcGeo corner2(((double) m_lon    + ( 0.5 + sample)/(m_samples-1)) * M_PI/180,
                    ((double)(m_lat+1) - ( 0.5 + line  )/(m_lines  -1)) * M_PI/180);
      bool inRange = false;
      float elev = elevation->meanElevation(corner1, corner2, &inRange);
      if (inRange)
      {
        m_data[indexData(1, line, sample)] = (0x8000 & m_data[indexData(1, line, sample)]) | (uint16_t)floor(elev+0.5f);
      }
    }
  }
}

/// Fill voids using bilinear interpolation.
void tcSrtmData::fillVoidsBilinear()
{
  for (int line = 0; line < m_lines; ++line)
  {
    int lastNonVoid = -1;
    uint16_t lastNonVoidValue;
    for (int sample = 0; sample < m_samples; ++sample)
    {
      int index = indexData(0, line, sample);
      uint16_t value = m_data[index];
      bool isVoid = (0 != (value & 0x8000));
      if (!isVoid)
      {
        if (lastNonVoid != -1)
        {
          int gap = sample - lastNonVoid;
          float startAlt = lastNonVoidValue;
          float dAlt = (float)(value - lastNonVoidValue) / gap;
          // Lovely, between lastNonVoid and sample is a void
          for (int i = lastNonVoid+1; i < sample; ++i)
          {
            int iIndex = indexData(0, line, i);
            uint16_t alt = startAlt + dAlt * (i - lastNonVoid);
            alt |= 0x8000;
            m_data[iIndex] = alt;
            // Keep track of the gap
            m_data[indexData(1, line, i)] = gap;
          }
        }
        lastNonVoid = sample;
        lastNonVoidValue = value;
      }
      else
      {
        // Clear any previous data
        m_data[index] = 0x8000;
      }
    }
  }
  for (int sample = 0; sample < m_samples; ++sample)
  {
    int lastNonVoid = -1;
    uint16_t lastNonVoidValue;
    for (int line = 0; line < m_lines; ++line)
    {
      int index = indexData(0, line, sample);
      uint16_t value = m_data[index];
      bool isVoid = (0 != (value & 0x8000));
      if (!isVoid)
      {
        if (lastNonVoid != -1)
        {
          int gap = line - lastNonVoid;
          float startAlt = lastNonVoidValue;
          float dAlt = (float)(value - lastNonVoidValue) / gap;
          // Lovely, between lastNonVoid and line is a void
          for (int i = lastNonVoid+1; i < line; ++i)
          {
            // Average with previous value if non zero, otherwise overwrite
            int iIndex = indexData(0, i, sample);
            int iIndexC = indexData(1, i, sample);
            uint16_t alt = startAlt + dAlt * (i - lastNonVoid);
            uint16_t prevAlt = 0x7fff & m_data[iIndex];
            if (prevAlt != 0)
            {
              int otherGap = m_data[iIndexC];
              // Prefer the value of the smaller gap
              alt = (float)((int)alt * otherGap + (int)prevAlt * gap) / (int)(gap+otherGap);
            }
            alt |= 0x8000;
            m_data[iIndex] = alt;
            m_data[iIndexC] = alt;
          }
        }
        else
        {
          for (int i = lastNonVoid+1; i < line; ++i)
          {
            int iIndex = indexData(0, i, sample);
            int iIndexC = indexData(1, i, sample);
            // The corrected value may contain a gap, which isn't needed anymore
            m_data[iIndexC] = m_data[iIndex];
          }
        }
        lastNonVoid = line;
        lastNonVoidValue = value;
      }
    }
  }
}

/// Fill voids using bicubic interpolation.
void tcSrtmData::fillVoidsBicubic()
{
  for (int line = 0; line < m_lines; ++line)
  {
    int lastNonVoid = -1;
    uint16_t lastNonVoidValue;
    for (int sample = 0; sample < m_samples; ++sample)
    {
      int index = indexData(0, line, sample);
      uint16_t value = m_data[index];
      bool isVoid = (0 != (value & 0x8000));
      if (!isVoid)
      {
        if (lastNonVoid != -1)
        {
          int gap = sample - lastNonVoid;
          // Obtain some control points for the splines
          // use the two samples either side of the gap as control points 1 and 2
          // use the ones before and after these samples, extended to the size of
          // the gap as control points 0 and 3
          float startAlt = lastNonVoidValue;
          float beforeStartAlt = startAlt;
          if (lastNonVoid > 0)
          {
            uint16_t beforeStart = m_data[indexData(0, line, lastNonVoid-1)];
            if (0 == (beforeStart & 0x8000))
            {
              beforeStartAlt = startAlt + ((float)beforeStart - startAlt) * gap;
            }
          }
          float endAlt = value;
          float afterEndAlt = endAlt;
          if (sample < m_samples-1)
          {
            uint16_t afterEnd = m_data[indexData(0, line, sample+1)];
            if (0 == (afterEnd & 0x8000))
            {
              afterEndAlt = endAlt + ((float)afterEnd - endAlt) * gap;
            }
          }
          // Lovely, between lastNonVoid and sample is a void
          for (int i = lastNonVoid+1; i < sample; ++i)
          {
            int iIndex = indexData(0, line, i);
            float u = (float)(i-lastNonVoid) / gap;
            float u2 = u*u;
            float u3 = u2*u;
            uint16_t alt = maths::catmullRomSpline.Spline(u, u2, u3, beforeStartAlt, startAlt, endAlt, afterEndAlt);
            alt |= 0x8000;
            m_data[iIndex] = alt;
            // Keep track of the gap
            m_data[indexData(1, line, i)] = gap;
          }
        }
        lastNonVoid = sample;
        lastNonVoidValue = value;
      }
      else
      {
        // Clear any previous data
        m_data[index] = 0x8000;
      }
    }
  }
  for (int sample = 0; sample < m_samples; ++sample)
  {
    int lastNonVoid = -1;
    uint16_t lastNonVoidValue;
    for (int line = 0; line < m_lines; ++line)
    {
      int index = indexData(0, line, sample);
      uint16_t value = m_data[index];
      bool isVoid = (0 != (value & 0x8000));
      if (!isVoid)
      {
        if (lastNonVoid != -1)
        {
          int gap = line - lastNonVoid;
          // Obtain some control points for the splines
          // use the two samples either side of the gap as control points 1 and 2
          // use the ones before and after these samples, extended to the size of
          // the gap as control points 0 and 3
          float startAlt = lastNonVoidValue;
          float beforeStartAlt = startAlt;
          if (lastNonVoid > 0)
          {
            uint16_t beforeStart = m_data[indexData(0, lastNonVoid-1, sample)];
            if (0 == (beforeStart & 0x8000))
            {
              beforeStartAlt = startAlt + ((float)beforeStart - startAlt) * gap;
            }
          }
          float endAlt = value;
          float afterEndAlt = endAlt;
          if (line < m_lines-1)
          {
            uint16_t afterEnd = m_data[indexData(0, line+1, sample)];
            if (0 == (afterEnd & 0x8000))
            {
              afterEndAlt = endAlt + ((float)afterEnd - endAlt) * gap;
            }
          }
          // Lovely, between lastNonVoid and line is a void
          for (int i = lastNonVoid+1; i < line; ++i)
          {
            // Average with previous value if non zero, otherwise overwrite
            int iIndex = indexData(0, i, sample);
            int iIndexC = indexData(1, i, sample);
            float u = (float)(i-lastNonVoid) / gap;
            float u2 = u*u;
            float u3 = u2*u;
            uint16_t alt = maths::catmullRomSpline.Spline(u, u2, u3, beforeStartAlt, startAlt, endAlt, afterEndAlt);
            uint16_t prevAlt = 0x7fff & m_data[iIndex];
            if (prevAlt != 0)
            {
              int otherGap = m_data[iIndexC];
              // Prefer the value of the smaller gap
              alt = (float)((int)alt * otherGap + (int)prevAlt * gap) / (int)(gap+otherGap);
            }
            alt |= 0x8000;
            m_data[iIndex] = alt;
            m_data[iIndexC] = alt;
          }
        }
        else
        {
          for (int i = lastNonVoid+1; i < line; ++i)
          {
            int iIndex = indexData(0, i, sample);
            int iIndexC = indexData(1, i, sample);
            // The corrected value may contain a gap, which isn't needed anymore
            m_data[iIndexC] = m_data[iIndex];
          }
        }
        lastNonVoid = line;
        lastNonVoidValue = value;
      }
    }
  }
}

/*
 * Private functions
 */

/// Index into the data array.
unsigned int tcSrtmData::indexData(int corrected, int line, int sample) const
{
  return (corrected*m_lines + line)*m_samples + sample;
}