Shinya Kitaoka 79e39b
#include <memory></memory>
Shinya Kitaoka 79e39b
#include <array></array>
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#include "toonz/tdistort.h"
Toshihiro Shimizu 890ddd
#include "traster.h"
Toshihiro Shimizu 890ddd
#include "trastercm.h"
Toshihiro Shimizu 890ddd
#include "tgeometry.h"
Toshihiro Shimizu 890ddd
#include "tpixelutils.h"
Toshihiro Shimizu 890ddd
#include <qlist></qlist>
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//****************************************************************************************
Toshihiro Shimizu 890ddd
//    Local namespace stuff
Toshihiro Shimizu 890ddd
//****************************************************************************************
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
namespace {
Shinya Kitaoka 120a6e
inline double dist(const TPointD &a, const TPointD &b) { return norm(b - a); }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//========================================================================================
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
namespace {
Shinya Kitaoka 120a6e
typedef struct {
Shinya Kitaoka 120a6e
  TUINT32 val;
Shinya Kitaoka 120a6e
  double tot;
Toshihiro Shimizu 890ddd
} BLOB24;
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TPixelCM32 filterPixel(const TPointD &pos, const TRasterCM32P &rasIn) {
Shinya Kitaoka 120a6e
  TPointD distance = TPointD(
Shinya Kitaoka 120a6e
      areAlmostEqual(pos.x, tfloor(pos.x), 0.001) ? 0.0
Shinya Kitaoka 120a6e
                                                  : fabs(pos.x - tfloor(pos.x)),
Shinya Kitaoka 120a6e
      areAlmostEqual(pos.y, tfloor(pos.y), 0.001) ? 0.0 : fabs(pos.y -
Shinya Kitaoka 120a6e
                                                               tfloor(pos.y)));
Shinya Kitaoka 120a6e
  TPoint nearPos(tfloor(pos.x), tfloor(pos.y));
Shinya Kitaoka 120a6e
  if (distance == TPointD(0.0, 0.0)) {
Shinya Kitaoka 120a6e
    if (nearPos.x >= 0 && nearPos.x < rasIn->getLx() && nearPos.y >= 0 &&
Shinya Kitaoka 120a6e
        nearPos.y < rasIn->getLy())
Shinya Kitaoka 120a6e
      return rasIn->pixels(nearPos.y)[nearPos.x];
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      return TPixelCM32();
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int i, j, k = 0;
Shinya Kitaoka 120a6e
  TPixelCM32 P[4];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (j = 0; j < 2; ++j)
Shinya Kitaoka 120a6e
    for (i         = 0; i < 2; ++i)
Shinya Kitaoka 120a6e
      P[i + 2 * j] = nearPos.x + i < rasIn->getLx() && nearPos.x + i >= 0 &&
Shinya Kitaoka 120a6e
                             nearPos.y + j < rasIn->getLy() &&
Shinya Kitaoka 120a6e
                             nearPos.y + j >= 0
Shinya Kitaoka 120a6e
                         ? rasIn->pixels(nearPos.y + j)[nearPos.x + i]
Shinya Kitaoka 120a6e
                         : TPixelCM32();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (P[0] == P[1] && P[1] == P[2] && P[2] == P[3]) return P[0];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double w[4], sum = 0;
Shinya Kitaoka 120a6e
  for (j = 1; j >= 0; --j)
Shinya Kitaoka 120a6e
    for (i          = 1; i >= 0; --i)
Shinya Kitaoka 120a6e
      sum += w[k++] = fabs(distance.x - i) * fabs(distance.y - j);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (i = 0; i < 4; i++) w[i] /= sum;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  TPixelCM32 outPix;
Shinya Kitaoka 120a6e
  TUINT32 tone;
Shinya Kitaoka 120a6e
  double tone_tot = 0.0;
Shinya Kitaoka 120a6e
  BLOB24 currBlob, paintBlobs[4], inkBlobs[4];
Shinya Kitaoka 120a6e
  int paintBlobsSize = 0, inkBlobsSize = 0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Doing the same thing in TRop::resample
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  for (i = 0; i < 4; ++i) {
Shinya Kitaoka 120a6e
    // Build paint blobs and sort them
Shinya Kitaoka 120a6e
    tone = P[i].getTone();
Shinya Kitaoka 120a6e
    tone_tot += tone * w[i];
Shinya Kitaoka 120a6e
    currBlob.val = P[i].getPaint();
Shinya Kitaoka 120a6e
    currBlob.tot = tone * w[i];
Shinya Kitaoka 120a6e
    for (j = 0; j < paintBlobsSize; j++)
Shinya Kitaoka 120a6e
      if (paintBlobs[j].val == currBlob.val) break;
Shinya Kitaoka 120a6e
    if (j < paintBlobsSize)
Shinya Kitaoka 120a6e
      paintBlobs[j].tot += currBlob.tot;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      paintBlobs[paintBlobsSize++] = currBlob;
Shinya Kitaoka 120a6e
    for (; j > 0 && paintBlobs[j].tot > paintBlobs[j - 1].tot; j--)
Shinya Kitaoka 120a6e
      tswap(paintBlobs[j], paintBlobs[j - 1]);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Same for ink blobs
Shinya Kitaoka 120a6e
    currBlob.val = P[i].getInk();
Shinya Kitaoka 120a6e
    currBlob.tot = (TPixelCM32::getToneMask() - tone) * w[i];
Shinya Kitaoka 120a6e
    for (j = 0; j < inkBlobsSize; j++)
Shinya Kitaoka 120a6e
      if (inkBlobs[j].val == currBlob.val) break;
Shinya Kitaoka 120a6e
    if (j < inkBlobsSize)
Shinya Kitaoka 120a6e
      inkBlobs[j].tot += currBlob.tot;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      inkBlobs[inkBlobsSize++] = currBlob;
Shinya Kitaoka 120a6e
    for (; j > 0 && inkBlobs[j].tot > inkBlobs[j - 1].tot; j--)
Shinya Kitaoka 120a6e
      tswap(inkBlobs[j], inkBlobs[j - 1]);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  tone = troundp(tone_tot);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  outPix.setPaint(paintBlobs[0].val);
Shinya Kitaoka 120a6e
  outPix.setInk(inkBlobs[0].val);
Shinya Kitaoka 120a6e
  outPix.setTone(tone);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return outPix;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void resample(const TRasterCM32P &rasIn, TRasterCM32P &rasOut,
Shinya Kitaoka 120a6e
              const TDistorter &distorter, const TPoint &p) {
Shinya Kitaoka 120a6e
  if (rasOut->getLx() < 1 || rasOut->getLy() < 1 || rasIn->getLx() < 1 ||
Shinya Kitaoka 120a6e
      rasIn->getLy() < 1)
Shinya Kitaoka 120a6e
    return;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  std::unique_ptr<tpointd[]> preImages(new TPointD[distorter.maxInvCount()]);</tpointd[]>
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int x, y;
Shinya Kitaoka 120a6e
  int i;
Shinya Kitaoka 120a6e
  for (y = 0; y < rasOut->getLy(); y++) {
Shinya Kitaoka 120a6e
    TPixelCM32 *pix    = rasOut->pixels(y);
Shinya Kitaoka 120a6e
    TPixelCM32 *endPix = pix + rasOut->getLx();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    for (x = 0; pix != endPix; pix++, x++) {
Shinya Kitaoka 120a6e
      TPixelCM32 pixDown;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      int count = distorter.invMap(convert(p) + TPointD(x + 0.5, y + 0.5),
Shinya Kitaoka 120a6e
                                   preImages.get());
Shinya Kitaoka 120a6e
      for (i = count - 1; i >= 0; --i) {
Shinya Kitaoka 120a6e
        TPixelCM32 pixUp;
Shinya Kitaoka 120a6e
        TPointD preImage(preImages[i].x - 0.5, preImages[i].y - 0.5);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        if (preImage.x > -1 && preImage.y > -1 &&
Shinya Kitaoka 120a6e
            preImage.x < rasIn->getLx() + 1 && preImage.y < rasIn->getLy() + 1)
Shinya Kitaoka 120a6e
          pixUp = filterPixel(preImage, rasIn);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        pixDown.setPaint(pixUp.getPaint() ? pixUp.getPaint()
Shinya Kitaoka 120a6e
                                          : pixDown.getPaint());
Shinya Kitaoka 120a6e
        pixDown.setInk(pixUp.getInk() ? pixUp.getInk() : pixDown.getInk());
Shinya Kitaoka 120a6e
        pixDown.setTone(pixUp == TPixelCM32() ? pixDown.getTone()
Shinya Kitaoka 120a6e
                                              : pixUp.getTone());
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
      *pix = pixDown;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <typename channel_type="" pixel,="" typename=""></typename>
Shinya Kitaoka 120a6e
PIXEL closest_pixel(const TPointD &pos, const TRasterPT<pixel> &rasIn) {</pixel>
Shinya Kitaoka 120a6e
  TPoint nearPos(tround(pos.x), tround(pos.y));
Shinya Kitaoka 120a6e
  if (!rasIn->getBounds().contains(nearPos)) return PIXEL(0, 0, 0, 0);
Shinya Kitaoka 120a6e
  return rasIn->pixels(nearPos.y)[nearPos.x];
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <typename channel_type="" t,="" typename=""></typename>
Toshihiro Shimizu 890ddd
void resampleClosestPixel(const TRasterPT<t> &rasIn, TRasterPT<t> &rasOut,</t></t>
Shinya Kitaoka 120a6e
                          const TDistorter &distorter, const TPoint &p) {
Shinya Kitaoka 120a6e
  if (rasOut->getLx() < 1 || rasOut->getLy() < 1 || rasIn->getLx() < 1 ||
Shinya Kitaoka 120a6e
      rasIn->getLy() < 1)
Shinya Kitaoka 120a6e
    return;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  std::unique_ptr<tpointd[]> preImages(new TPointD[distorter.maxInvCount()]);</tpointd[]>
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int x, y;
Shinya Kitaoka 120a6e
  int i;
Shinya Kitaoka 120a6e
  for (y = 0; y < rasOut->getLy(); y++) {
Shinya Kitaoka 120a6e
    T *pix    = rasOut->pixels(y);
Shinya Kitaoka 120a6e
    T *endPix = pix + rasOut->getLx();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    x = 0;
Shinya Kitaoka 120a6e
    for (; pix != endPix; pix++, x++) {
Shinya Kitaoka 120a6e
      T pixDown(0, 0, 0, 0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      // preImages.clear();
Shinya Kitaoka 120a6e
      int count = distorter.invMap(convert(p) + TPointD(x + 0.5, y + 0.5),
Shinya Kitaoka 120a6e
                                   preImages.get());
Shinya Kitaoka 120a6e
      for (i = count - 1; i >= 0; --i) {
Shinya Kitaoka 120a6e
        T pixUp(0, 0, 0, 0);
Shinya Kitaoka 120a6e
        TPointD preImage(preImages[i].x - 0.5, preImages[i].y - 0.5);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        if (preImage.x > -1 && preImage.y > -1 &&
Shinya Kitaoka 120a6e
            preImage.x < rasIn->getLx() + 1 && preImage.y < rasIn->getLy() + 1)
Shinya Kitaoka 120a6e
          pixUp = closest_pixel<t, channel_type="">(preImage, rasIn);</t,>
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        pixDown = pixUp;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
      *pix = pixDown;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TPixelCM32 closest_pixel(const TPointD &pos, const TRasterCM32P &rasIn) {
Shinya Kitaoka 120a6e
  TPoint nearPos(tround(pos.x), tround(pos.y));
Shinya Kitaoka 120a6e
  if (!rasIn->getBounds().contains(nearPos)) return TPixelCM32(0, 0, 255);
Shinya Kitaoka 120a6e
  return rasIn->pixels(nearPos.y)[nearPos.x];
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
void resampleClosestPixel(const TRasterCM32P &rasIn, TRasterCM32P &rasOut,
Shinya Kitaoka 120a6e
                          const TDistorter &distorter, const TPoint &p) {
Shinya Kitaoka 120a6e
  if (rasOut->getLx() < 1 || rasOut->getLy() < 1 || rasIn->getLx() < 1 ||
Shinya Kitaoka 120a6e
      rasIn->getLy() < 1)
Shinya Kitaoka 120a6e
    return;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  std::unique_ptr<tpointd[]> preImages(new TPointD[distorter.maxInvCount()]);</tpointd[]>
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int x, y;
Shinya Kitaoka 120a6e
  int i;
Shinya Kitaoka 120a6e
  for (y = 0; y < rasOut->getLy(); y++) {
Shinya Kitaoka 120a6e
    TPixelCM32 *pix    = rasOut->pixels(y);
Shinya Kitaoka 120a6e
    TPixelCM32 *endPix = pix + rasOut->getLx();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    x = 0;
Shinya Kitaoka 120a6e
    for (; pix != endPix; pix++, x++) {
Shinya Kitaoka 120a6e
      TPixelCM32 pixDown(0, 0, 255);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      int count = distorter.invMap(convert(p) + TPointD(x + 0.5, y + 0.5),
Shinya Kitaoka 120a6e
                                   preImages.get());
Shinya Kitaoka 120a6e
      for (i = count - 1; i >= 0; --i) {
Shinya Kitaoka 120a6e
        TPixelCM32 pixUp(0, 0, 255);
Shinya Kitaoka 120a6e
        TPointD preImage(preImages[i].x - 0.5, preImages[i].y - 0.5);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        if (preImage.x > -1 && preImage.y > -1 &&
Shinya Kitaoka 120a6e
            preImage.x < rasIn->getLx() + 1 && preImage.y < rasIn->getLy() + 1)
Shinya Kitaoka 120a6e
          pixUp = closest_pixel(preImage, rasIn);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        pixDown = pixUp;
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
      *pix = pixDown;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//=================================================================================
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <typename channel_type="" pixel,="" typename=""></typename>
Shinya Kitaoka 120a6e
PIXEL filterPixel(double a, double b, PIXEL *lineSrc, int lineLength,
Shinya Kitaoka 120a6e
                  int lineWrap) {
Shinya Kitaoka 120a6e
  // Retrieve the interesting pixel interval
Shinya Kitaoka 120a6e
  double x0 = std::max(a, 0.0);
Shinya Kitaoka 120a6e
  double x1 = std::min(b, (double)lineLength);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int x0Floor = tfloor(x0);
Shinya Kitaoka 120a6e
  int x0Ceil  = tceil(x0);
Shinya Kitaoka 120a6e
  int x1Floor = tfloor(x1);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (x0 >= x1) return PIXEL::Transparent;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  PIXEL *pix = lineSrc + x0Floor * lineWrap;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Build the sums
Shinya Kitaoka 120a6e
  double k, sumr = 0, sumg = 0, sumb = 0, summ = 0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Fractionary part on the beginning
Shinya Kitaoka 120a6e
  if (x0Ceil > x0) {
Shinya Kitaoka 120a6e
    k = x0Ceil - x0;
Shinya Kitaoka 120a6e
    sumr += k * pix->r;
Shinya Kitaoka 120a6e
    sumg += k * pix->g;
Shinya Kitaoka 120a6e
    sumb += k * pix->b;
Shinya Kitaoka 120a6e
    summ += k * pix->m;
Shinya Kitaoka 120a6e
    pix += lineWrap;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Intermediate pixels (ie when pixels contract)
Shinya Kitaoka 120a6e
  int x;
Shinya Kitaoka 120a6e
  for (x = x0Ceil; x < x1Floor; ++x, pix += lineWrap) {
Shinya Kitaoka 120a6e
    sumr += pix->r;
Shinya Kitaoka 120a6e
    sumg += pix->g;
Shinya Kitaoka 120a6e
    sumb += pix->b;
Shinya Kitaoka 120a6e
    summ += pix->m;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Fractionary part on the end
Shinya Kitaoka 120a6e
  if (x1 < lineLength) {
Shinya Kitaoka 120a6e
    k = x1 - x;
Shinya Kitaoka 120a6e
    sumr += k * pix->r;
Shinya Kitaoka 120a6e
    sumg += k * pix->g;
Shinya Kitaoka 120a6e
    sumb += k * pix->b;
Shinya Kitaoka 120a6e
    summ += k * pix->m;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Finally, divide per the total weight
Shinya Kitaoka 120a6e
  double length =
Shinya Kitaoka 120a6e
      b - a;  // 'transparent' pixels outside image range *are* considered
Shinya Kitaoka 120a6e
  sumr = sumr / length;
Shinya Kitaoka 120a6e
  sumg = sumg / length;
Shinya Kitaoka 120a6e
  sumb = sumb / length;
Shinya Kitaoka 120a6e
  summ = summ / length;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return PIXEL(sumr, sumg, sumb, summ);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <typename channel_type="" pixel,="" typename=""></typename>
Shinya Kitaoka 120a6e
PIXEL filterPixel(double a, double b, double c, double d,
Shinya Kitaoka 120a6e
                  const TRasterPT<pixel> &rasIn, PIXEL *temp) {</pixel>
Shinya Kitaoka 120a6e
  if (bool(a < b) == false && bool(b <= a) == false)
Shinya Kitaoka 120a6e
    return PIXEL::Transparent;  // NaNs
Shinya Kitaoka 120a6e
  else
Shinya Kitaoka 120a6e
    std::swap(a, b);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (bool(c < d) == false && bool(d <= c) == false)
Shinya Kitaoka 120a6e
    return PIXEL::Transparent;
Shinya Kitaoka 120a6e
  else
Shinya Kitaoka 120a6e
    std::swap(c, d);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Deal the magnification case, assuming that intervals have at least length
Shinya Kitaoka 120a6e
  // 1. This actually stands for:
Shinya Kitaoka 120a6e
  //  1. Their midpoint is bilinear filtered whenever their former length wass
Shinya Kitaoka 120a6e
  //  less than 1 (see fractionary
Shinya Kitaoka 120a6e
  //     parts computing above).
Shinya Kitaoka 120a6e
  //  2. This behaviour is continuous with respect to interval lengths - that
Shinya Kitaoka 120a6e
  //  is, we pass from supersampling to
Shinya Kitaoka 120a6e
  //     subsampling in a smooth manner.
Shinya Kitaoka 120a6e
  if (b - a < 1) {
Shinya Kitaoka 120a6e
    double v = 0.5 * (a + b);
Shinya Kitaoka 120a6e
    a        = v - 0.5;
Shinya Kitaoka 120a6e
    b        = v + 0.5;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (d - c < 1) {
Shinya Kitaoka 120a6e
    double v = 0.5 * (c + d);
Shinya Kitaoka 120a6e
    c        = v - 0.5;
Shinya Kitaoka 120a6e
    d        = v + 0.5;
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Now, filter each column in [a, b]
Shinya Kitaoka 120a6e
  double x0 = std::max(a, 0.0);
Shinya Kitaoka 120a6e
  double x1 = std::min(b, (double)rasIn->getLx());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (x0 >= x1) return PIXEL::Transparent;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int xEnd = tceil(x1);
Shinya Kitaoka 120a6e
  for (int x = tfloor(x0); x < xEnd; ++x)
Shinya Kitaoka 120a6e
    temp[x]  = filterPixel<pixel, channel_type="">(</pixel,>
Shinya Kitaoka 120a6e
        c, d, rasIn->pixels(0) + x, rasIn->getLy(), rasIn->getWrap());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Then, filter temp
Shinya Kitaoka 120a6e
  return filterPixel<pixel, channel_type="">(a, b, temp, rasIn->getLx(), 1);</pixel,>
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
template <typename channel_type="" t,="" typename=""></typename>
Shinya Kitaoka 120a6e
void resample(const TRasterPT<t> &rasIn, TRasterPT<t> &rasOut,</t></t>
Shinya Kitaoka 120a6e
              const TDistorter &distorter, const TPoint &p) {
Shinya Kitaoka 120a6e
  if (rasOut->getLx() < 1 || rasOut->getLy() < 1 || rasIn->getLx() < 1 ||
Shinya Kitaoka 120a6e
      rasIn->getLy() < 1)
Shinya Kitaoka 120a6e
    return;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int invsCount = distorter.maxInvCount();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Allocate buffers
Shinya Kitaoka 120a6e
  std::array<std::unique_ptr<tpointd[]>, 2> invs = {</std::unique_ptr<tpointd[]>
Shinya Kitaoka 120a6e
      std::unique_ptr<tpointd[]>(</tpointd[]>
Shinya Kitaoka 120a6e
          new TPointD[invsCount * (rasOut->getLx() + 1)]),
Shinya Kitaoka 120a6e
      std::unique_ptr<tpointd[]>(</tpointd[]>
Shinya Kitaoka 120a6e
          new TPointD[invsCount * (rasOut->getLx() + 1)]),
Shinya Kitaoka 120a6e
  };
Shinya Kitaoka 120a6e
  std::array<std::unique_ptr<int[]>, 2> counts = {</std::unique_ptr<int[]>
Shinya Kitaoka 120a6e
      std::unique_ptr<int[]>(new int[rasOut->getLx() + 1]),</int[]>
Shinya Kitaoka 120a6e
      std::unique_ptr<int[]>(new int[rasOut->getLx() + 1]),</int[]>
Shinya Kitaoka 120a6e
  };
Shinya Kitaoka 120a6e
  std::unique_ptr<t[]> temp(new T[rasIn->getLx()]);</t[]>
Shinya Kitaoka 120a6e
  TPointD shift(convert(p) + TPointD(0.5, 0.5));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // Fill in the first inverses (lower edge of output image)
Shinya Kitaoka 120a6e
  {
Shinya Kitaoka 120a6e
    TPointD *currOldInv = invs[0].get();
Shinya Kitaoka 120a6e
    int *oldCounts      = counts[0].get();
Shinya Kitaoka 120a6e
    for (int x     = 0; x <= rasOut->getLx(); currOldInv += invsCount, ++x)
Shinya Kitaoka 120a6e
      oldCounts[x] = distorter.invMap(shift + TPointD(x, 0.0), currOldInv);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // For each output row
Shinya Kitaoka 120a6e
  for (int y = 0; y < rasOut->getLy(); ++y) {
Shinya Kitaoka 120a6e
    // Alternate inverse buffers
Shinya Kitaoka 120a6e
    TPointD *oldInvs = invs[y % 2].get();
Shinya Kitaoka 120a6e
    TPointD *newInvs = invs[(y + 1) % 2].get();
Shinya Kitaoka 120a6e
    int *oldCounts   = counts[y % 2].get();
Shinya Kitaoka 120a6e
    int *newCounts   = counts[(y + 1) % 2].get();
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Build the new inverses
Shinya Kitaoka 120a6e
    {
Shinya Kitaoka 120a6e
      TPointD *currNewInv = newInvs;
Shinya Kitaoka 120a6e
      for (int x = 0; x <= rasOut->getLx(); currNewInv += invsCount, ++x)
Shinya Kitaoka 120a6e
        newCounts[x] =
Shinya Kitaoka 120a6e
            distorter.invMap(shift + TPointD(x, y + 1.0), currNewInv);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    // Filter each pixel in the row
Shinya Kitaoka 120a6e
    T *pix              = rasOut->pixels(y);
Shinya Kitaoka 120a6e
    TPointD *currOldInv = oldInvs;
Shinya Kitaoka 120a6e
    TPointD *currNewInv = newInvs;
Shinya Kitaoka 120a6e
    for (int x = 0; x < rasOut->getLx();
Shinya Kitaoka 120a6e
         currOldInv += invsCount, currNewInv += invsCount, ++x, ++pix) {
Shinya Kitaoka 120a6e
      T pixDown(0, 0, 0, 0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      int count = std::min({oldCounts[x], oldCounts[x + 1], newCounts[x]});
Shinya Kitaoka 120a6e
      for (int i = 0; i < count; ++i) {
Shinya Kitaoka 120a6e
        T pixUp(0, 0, 0, 0);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        pixUp = filterPixel<t, channel_type="">(</t,>
Shinya Kitaoka 120a6e
            currOldInv[i].x - 0.5, (currOldInv + invsCount)[i].x - 0.5,
Shinya Kitaoka 120a6e
            currOldInv[i].y - 0.5, currNewInv[i].y - 0.5, rasIn, temp.get());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
        pixDown = overPix(pixDown, pixUp);
Shinya Kitaoka 120a6e
      }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      *pix = pixDown;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
}  // namespace
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//==============================================================================================
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void distort(TRasterP &outRas, const TRasterP &inRas,
Shinya Kitaoka 120a6e
             const TDistorter &distorter, const TPoint &dstPos,
Shinya Kitaoka 120a6e
             const TRop::ResampleFilterType &filter) {
Shinya Kitaoka 120a6e
  TRaster32P inRas32      = inRas;
Shinya Kitaoka 120a6e
  TRaster64P inRas64      = inRas;
Shinya Kitaoka 120a6e
  TRasterCM32P inRasCM32  = inRas;
Shinya Kitaoka 120a6e
  TRaster32P outRas32     = outRas;
Shinya Kitaoka 120a6e
  TRaster64P outRas64     = outRas;
Shinya Kitaoka 120a6e
  TRasterCM32P outRasCM32 = outRas;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (filter == TRop::Bilinear) {
Shinya Kitaoka 120a6e
    if (inRas32)
Shinya Kitaoka 120a6e
      ::resample<tpixel32, uchar="">(inRas32, outRas32, distorter, dstPos);</tpixel32,>
Shinya Kitaoka 120a6e
    else if (inRas64)
Shinya Kitaoka 120a6e
      ::resample<tpixel64, ushort="">(inRas64, outRas64, distorter, dstPos);</tpixel64,>
Shinya Kitaoka 120a6e
    else if (inRasCM32 && outRasCM32)
Shinya Kitaoka 120a6e
      ::resample(inRasCM32, outRasCM32, distorter, dstPos);
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      assert(0);
Shinya Kitaoka 120a6e
  } else if (filter == TRop::ClosestPixel) {
Shinya Kitaoka 120a6e
    if (inRas32)
Shinya Kitaoka 120a6e
      ::resampleClosestPixel<tpixel32, uchar="">(inRas32, outRas32, distorter,</tpixel32,>
Shinya Kitaoka 120a6e
                                              dstPos);
Shinya Kitaoka 120a6e
    else if (inRas64)
Shinya Kitaoka 120a6e
      ::resampleClosestPixel<tpixel64, ushort="">(inRas64, outRas64, distorter,</tpixel64,>
Shinya Kitaoka 120a6e
                                               dstPos);
Shinya Kitaoka 120a6e
    else if (inRasCM32 && outRasCM32)
Shinya Kitaoka 120a6e
      ::resampleClosestPixel(inRasCM32, outRasCM32, distorter, dstPos);
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      assert(0);
Shinya Kitaoka 120a6e
  } else
Shinya Kitaoka 120a6e
    assert(0);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//================================================================================================
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
// BilinearDistorterBase
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
//================================================================================================
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
BilinearDistorterBase::BilinearDistorterBase(
Shinya Kitaoka 120a6e
    const TPointD &p00s, const TPointD &p10s, const TPointD &p01s,
Shinya Kitaoka 120a6e
    const TPointD &p11s, const TPointD &p00d, const TPointD &p10d,
Shinya Kitaoka 120a6e
    const TPointD &p01d, const TPointD &p11d)
Shinya Kitaoka 120a6e
    : TQuadDistorter(p00s, p10s, p01s, p11s, p00d, p10d, p01d, p11d)
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
{
Shinya Kitaoka 120a6e
  m_A = p00d;
Shinya Kitaoka 120a6e
  m_B = p10d - p00d;
Shinya Kitaoka 120a6e
  m_C = p01d - p00d;
Shinya Kitaoka 120a6e
  m_D = p11d - p01d - p10d + p00d;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  m_a  = m_D.x * m_C.y - m_C.x * m_D.y;
Shinya Kitaoka 120a6e
  m_b0 = m_B.x * m_C.y - m_C.x * m_B.y;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//--------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
inline TPointD BilinearDistorterBase::map(const TPointD &p) const {
Shinya Kitaoka 120a6e
  double t = (p.x - m_p00s.x) / (m_p10s.x - m_p00s.x);
Shinya Kitaoka 120a6e
  double s = (p.y - m_p00s.y) / (m_p01s.y - m_p00s.y);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return (1 - s) * (1 - t) * m_p00d + (1 - s) * t * m_p10d +
Shinya Kitaoka 120a6e
         s * (1 - t) * m_p01d + s * t * m_p11d;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//--------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
int BilinearDistorterBase::invMap(const TPointD &p, TPointD *results) const {
Shinya Kitaoka 120a6e
  // See the book "Digital Image Warping" by G. Wolberg
Shinya Kitaoka 120a6e
  double b = m_D.x * (m_A.y - p.y) + m_D.y * (p.x - m_A.x) + m_b0;
Shinya Kitaoka 120a6e
  double c = m_B.x * (m_A.y - p.y) + m_B.y * (p.x - m_A.x);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (fabs(m_a) > 0.001) {
Shinya Kitaoka 120a6e
    // If delta <0, the points cannot be inverse-mapped
Shinya Kitaoka 120a6e
    double delta = sq(b) - 4.0 * m_a * c;
Shinya Kitaoka 120a6e
    if (delta < 0) return 0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double sqrtDelta = sqrt(delta);
Shinya Kitaoka 120a6e
    double k         = 0.5 / m_a;
Shinya Kitaoka 120a6e
    double v1        = k * (-b + sqrtDelta);
Shinya Kitaoka 120a6e
    double v2        = k * (-b - sqrtDelta);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double u1, u2;
Shinya Kitaoka 120a6e
    double den = m_B.x + m_D.x * v1;
Shinya Kitaoka 120a6e
    if (fabs(den) > 0.01)
Shinya Kitaoka 120a6e
      u1 = (p.x - m_A.x - m_C.x * v1) / den;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      u1 = (p.y - m_A.y - m_C.y * v1) / (m_B.y + m_D.y * v1);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    den = m_B.x + m_D.x * v2;
Shinya Kitaoka 120a6e
    if (fabs(den) > 0.01)
Shinya Kitaoka 120a6e
      u2 = (p.x - m_A.x - m_C.x * v2) / den;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      u2 = (p.y - m_A.y - m_C.y * v2) / (m_B.y + m_D.y * v2);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    TPointD difP10_P00 = m_p10s - m_p00s;
Shinya Kitaoka 120a6e
    TPointD difP01_P00 = m_p01s - m_p00s;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    results[0] = m_p00s + u1 * difP10_P00 + v1 * difP01_P00;
Shinya Kitaoka 120a6e
    results[1] = m_p00s + u2 * difP10_P00 + v2 * difP01_P00;
Shinya Kitaoka 120a6e
    return 2;
Shinya Kitaoka 120a6e
  } else {
Shinya Kitaoka 120a6e
    // The equation reduces to first order.
Shinya Kitaoka 120a6e
    double v = -c / b;
Shinya Kitaoka 120a6e
    double u = (p.x - m_A.x - m_C.x * v) / (m_B.x + m_D.x * v);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    results[0] = m_p00s + u * (m_p10s - m_p00s) + v * (m_p01s - m_p00s);
Shinya Kitaoka 120a6e
    return 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//================================================================================================
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
// BilinearDistorter
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
//================================================================================================
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
BilinearDistorter::BilinearDistorter(const TPointD &p00s, const TPointD &p10s,
Shinya Kitaoka 120a6e
                                     const TPointD &p01s, const TPointD &p11s,
Shinya Kitaoka 120a6e
                                     const TPointD &p00d, const TPointD &p10d,
Shinya Kitaoka 120a6e
                                     const TPointD &p01d, const TPointD &p11d)
Shinya Kitaoka 120a6e
    : TQuadDistorter(p00s, p10s, p01s, p11s, p00d, p10d, p01d, p11d) {
Shinya Kitaoka 120a6e
  m_refToSource.m_p00 = p00s;
Shinya Kitaoka 120a6e
  m_refToSource.m_p10 = p10s;
Shinya Kitaoka 120a6e
  m_refToSource.m_p01 = p01s;
Shinya Kitaoka 120a6e
  m_refToSource.m_p11 = p11s;
Shinya Kitaoka 120a6e
  m_refToDest.m_p00   = p00d;
Shinya Kitaoka 120a6e
  m_refToDest.m_p10   = p10d;
Shinya Kitaoka 120a6e
  m_refToDest.m_p01   = p01d;
Shinya Kitaoka 120a6e
  m_refToDest.m_p11   = p11d;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  m_refToSource.m_A = p00s;
Shinya Kitaoka 120a6e
  m_refToSource.m_B = p10s - p00s;
Shinya Kitaoka 120a6e
  m_refToSource.m_C = p01s - p00s;
Shinya Kitaoka 120a6e
  m_refToSource.m_D = p11s - p01s - p10s + p00s;
Shinya Kitaoka 120a6e
  m_refToDest.m_A   = p00d;
Shinya Kitaoka 120a6e
  m_refToDest.m_B   = p10d - p00d;
Shinya Kitaoka 120a6e
  m_refToDest.m_C   = p01d - p00d;
Shinya Kitaoka 120a6e
  m_refToDest.m_D   = p11d - p01d - p10d + p00d;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  m_refToSource.m_a = m_refToSource.m_D.x * m_refToSource.m_C.y -
Shinya Kitaoka 120a6e
                      m_refToSource.m_C.x * m_refToSource.m_D.y;
Shinya Kitaoka 120a6e
  m_refToSource.m_b0 = m_refToSource.m_B.x * m_refToSource.m_C.y -
Shinya Kitaoka 120a6e
                       m_refToSource.m_C.x * m_refToSource.m_B.y;
Shinya Kitaoka 120a6e
  m_refToDest.m_a = m_refToDest.m_D.x * m_refToDest.m_C.y -
Shinya Kitaoka 120a6e
                    m_refToDest.m_C.x * m_refToDest.m_D.y;
Shinya Kitaoka 120a6e
  m_refToDest.m_b0 = m_refToDest.m_B.x * m_refToDest.m_C.y -
Shinya Kitaoka 120a6e
                     m_refToDest.m_C.x * m_refToDest.m_B.y;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
BilinearDistorter::~BilinearDistorter() {}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//--------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TPointD BilinearDistorter::map(const TPointD &p) const {
Shinya Kitaoka 120a6e
  TPointD sourceToRefPoints[2];
Shinya Kitaoka 120a6e
  int returnCount = m_refToSource.invMap(p, sourceToRefPoints);
Shinya Kitaoka 120a6e
  if (returnCount > 0) return m_refToDest.map(sourceToRefPoints[0]);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  return TConsts::napd;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//--------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
inline int BilinearDistorter::invMap(const TPointD &p, TPointD *results) const {
Shinya Kitaoka 120a6e
  int returnCount = m_refToDest.invMap(p, results);
Shinya Kitaoka 120a6e
  for (int i   = 0; i < returnCount; ++i)
Shinya Kitaoka 120a6e
    results[i] = m_refToSource.map(results[i]);
Shinya Kitaoka 120a6e
  return returnCount;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//--------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
inline TPointD BilinearDistorter::Base::map(const TPointD &p) const {
Shinya Kitaoka 120a6e
  return (1 - p.y) * (1 - p.x) * m_p00 + (1 - p.y) * p.x * m_p10 +
Shinya Kitaoka 120a6e
         p.y * (1 - p.x) * m_p01 + p.y * p.x * m_p11;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//--------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
int BilinearDistorter::Base::invMap(const TPointD &p, TPointD *results) const {
Shinya Kitaoka 120a6e
  // See the book "Digital Image Warping" by G. Wolberg
Shinya Kitaoka 120a6e
  double b = m_D.x * (m_A.y - p.y) + m_D.y * (p.x - m_A.x) + m_b0;
Shinya Kitaoka 120a6e
  double c = m_B.x * (m_A.y - p.y) + m_B.y * (p.x - m_A.x);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (fabs(m_a) > 0.001) {
Shinya Kitaoka 120a6e
    // If delta <0, the points cannot be inverse-mapped
Shinya Kitaoka 120a6e
    double delta = sq(b) - 4.0 * m_a * c;
Shinya Kitaoka 120a6e
    if (delta < 0) return 0;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double sqrtDelta = sqrt(delta);
Shinya Kitaoka 120a6e
    double k         = 0.5 / m_a;
Shinya Kitaoka 120a6e
    double v1        = k * (-b + sqrtDelta);
Shinya Kitaoka 120a6e
    double v2        = k * (-b - sqrtDelta);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    double u1, u2;
Shinya Kitaoka 120a6e
    double den = m_B.x + m_D.x * v1;
Shinya Kitaoka 120a6e
    if (fabs(den) > 0.01)
Shinya Kitaoka 120a6e
      u1 = (p.x - m_A.x - m_C.x * v1) / den;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      u1 = (p.y - m_A.y - m_C.y * v1) / (m_B.y + m_D.y * v1);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    den = m_B.x + m_D.x * v2;
Shinya Kitaoka 120a6e
    if (fabs(den) > 0.01)
Shinya Kitaoka 120a6e
      u2 = (p.x - m_A.x - m_C.x * v2) / den;
Shinya Kitaoka 120a6e
    else
Shinya Kitaoka 120a6e
      u2 = (p.y - m_A.y - m_C.y * v2) / (m_B.y + m_D.y * v2);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    results[0] = TPointD(u1, v1);
Shinya Kitaoka 120a6e
    results[1] = TPointD(u2, v2);
Shinya Kitaoka 120a6e
    return 2;
Shinya Kitaoka 120a6e
  } else {
Shinya Kitaoka 120a6e
    // The equation reduces to first order.
Shinya Kitaoka 120a6e
    double v = -c / b;
Shinya Kitaoka 120a6e
    double u = (p.x - m_A.x - m_C.x * v) / (m_B.x + m_D.x * v);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    results[0] = TPointD(u, v);
Shinya Kitaoka 120a6e
    return 1;
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//=================================================================================
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
// TPerspect
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
//=================================================================================
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
PerspectiveDistorter::TPerspect::TPerspect()
Shinya Kitaoka 120a6e
    : a11(1.0)
Shinya Kitaoka 120a6e
    , a12(0.0)
Shinya Kitaoka 120a6e
    , a13(0.0)
Shinya Kitaoka 120a6e
    , a21(0.0)
Shinya Kitaoka 120a6e
    , a22(1.0)
Shinya Kitaoka 120a6e
    , a23(0.0)
Shinya Kitaoka 120a6e
    , a31(0.0)
Shinya Kitaoka 120a6e
    , a32(0.0)
Shinya Kitaoka 120a6e
    , a33(1.0) {}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
PerspectiveDistorter::TPerspect::TPerspect(double p11, double p12, double p13,
Shinya Kitaoka 120a6e
                                           double p21, double p22, double p23,
Shinya Kitaoka 120a6e
                                           double p31, double p32, double p33)
Shinya Kitaoka 120a6e
    : a11(p11)
Shinya Kitaoka 120a6e
    , a12(p12)
Shinya Kitaoka 120a6e
    , a13(p13)
Shinya Kitaoka 120a6e
    , a21(p21)
Shinya Kitaoka 120a6e
    , a22(p22)
Shinya Kitaoka 120a6e
    , a23(p23)
Shinya Kitaoka 120a6e
    , a31(p31)
Shinya Kitaoka 120a6e
    , a32(p32)
Shinya Kitaoka 120a6e
    , a33(p33) {}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
PerspectiveDistorter::TPerspect::TPerspect(const TPerspect &p)
Shinya Kitaoka 120a6e
    : a11(p.a11)
Shinya Kitaoka 120a6e
    , a12(p.a12)
Shinya Kitaoka 120a6e
    , a13(p.a13)
Shinya Kitaoka 120a6e
    , a21(p.a21)
Shinya Kitaoka 120a6e
    , a22(p.a22)
Shinya Kitaoka 120a6e
    , a23(p.a23)
Shinya Kitaoka 120a6e
    , a31(p.a31)
Shinya Kitaoka 120a6e
    , a32(p.a32)
Shinya Kitaoka 120a6e
    , a33(p.a33) {}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::TPerspect::~TPerspect() {}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::TPerspect &PerspectiveDistorter::TPerspect::operator=(
Shinya Kitaoka 120a6e
    const PerspectiveDistorter::TPerspect &p) {
Shinya Kitaoka 120a6e
  a11 = p.a11;
Shinya Kitaoka 120a6e
  a12 = p.a12;
Shinya Kitaoka 120a6e
  a13 = p.a13;
Shinya Kitaoka 120a6e
  a21 = p.a21;
Shinya Kitaoka 120a6e
  a22 = p.a22;
Shinya Kitaoka 120a6e
  a23 = p.a23;
Shinya Kitaoka 120a6e
  a31 = p.a31;
Shinya Kitaoka 120a6e
  a32 = p.a32;
Shinya Kitaoka 120a6e
  a33 = p.a33;
Shinya Kitaoka 120a6e
  return *this;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::TPerspect PerspectiveDistorter::TPerspect::operator*(
Shinya Kitaoka 120a6e
    const PerspectiveDistorter::TPerspect &p) const {
Shinya Kitaoka 120a6e
  return PerspectiveDistorter::TPerspect(
Shinya Kitaoka 120a6e
      a11 * p.a11 + a12 * p.a21 + a13 * p.a31,
Shinya Kitaoka 120a6e
      a11 * p.a12 + a12 * p.a22 + a13 * p.a32,
Shinya Kitaoka 120a6e
      a11 * p.a13 + a12 * p.a23 + a13 * p.a33,
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      a21 * p.a11 + a22 * p.a21 + a23 * p.a31,
Shinya Kitaoka 120a6e
      a21 * p.a12 + a22 * p.a22 + a23 * p.a32,
Shinya Kitaoka 120a6e
      a21 * p.a13 + a22 * p.a23 + a23 * p.a33,
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
      a31 * p.a11 + a32 * p.a21 + a33 * p.a31,
Shinya Kitaoka 120a6e
      a31 * p.a12 + a32 * p.a22 + a33 * p.a32,
Shinya Kitaoka 120a6e
      a31 * p.a13 + a32 * p.a23 + a33 * p.a33);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::TPerspect PerspectiveDistorter::TPerspect::operator*(
Shinya Kitaoka 120a6e
    const TAffine &aff) const {
Shinya Kitaoka 120a6e
  return operator*(TPerspect(aff.a11, aff.a12, aff.a13, aff.a21, aff.a22,
Shinya Kitaoka 120a6e
                             aff.a23, 0.0, 0.0, 1.0));
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::TPerspect operator*(
Shinya Kitaoka 120a6e
    const TAffine &aff, const PerspectiveDistorter::TPerspect &p) {
Shinya Kitaoka 120a6e
  return PerspectiveDistorter::TPerspect(aff.a11, aff.a12, aff.a13, aff.a21,
Shinya Kitaoka 120a6e
                                         aff.a22, aff.a23, 0.0, 0.0, 1.0) *
Shinya Kitaoka 120a6e
         p;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::TPerspect PerspectiveDistorter::TPerspect::operator*=(
Shinya Kitaoka 120a6e
    const PerspectiveDistorter::TPerspect &p) {
Shinya Kitaoka 120a6e
  return *this * p;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::TPerspect PerspectiveDistorter::TPerspect::inv() const {
Shinya Kitaoka 120a6e
  return TPerspect(
Shinya Kitaoka 120a6e
      a22 * a33 - a23 * a32, a13 * a32 - a12 * a33, a12 * a23 - a13 * a22,
Shinya Kitaoka 120a6e
      a23 * a31 - a21 * a33, a11 * a33 - a13 * a31, a13 * a21 - a11 * a23,
Shinya Kitaoka 120a6e
      a21 * a32 - a22 * a31, a12 * a31 - a11 * a32, a11 * a22 - a12 * a21);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double PerspectiveDistorter::TPerspect::det() const {
Shinya Kitaoka 120a6e
  return a11 * a22 * a33 + a12 * a23 * a31 + a13 * a21 * a32 - a13 * a22 * a31 -
Shinya Kitaoka 120a6e
         a11 * a23 * a32 - a12 * a21 * a33;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
bool PerspectiveDistorter::TPerspect::operator==(
Shinya Kitaoka 120a6e
    const PerspectiveDistorter::TPerspect &p) const {
Shinya Kitaoka 120a6e
  return a11 == p.a11 && a12 == p.a12 && a13 == p.a13 && a21 == p.a21 &&
Shinya Kitaoka 120a6e
         a22 == p.a22 && a23 == p.a23 && a31 == p.a31 && a32 == p.a32 &&
Shinya Kitaoka 120a6e
         a33 == p.a33;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
bool PerspectiveDistorter::TPerspect::operator!=(
Shinya Kitaoka 120a6e
    const PerspectiveDistorter::TPerspect &p) const {
Shinya Kitaoka 120a6e
  return !(*this == p);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
bool PerspectiveDistorter::TPerspect::isIdentity(double err) const {
Shinya Kitaoka 120a6e
  return ((a11 - 1.0) * (a11 - 1.0) + (a22 - 1.0) * (a22 - 1.0) +
Shinya Kitaoka 120a6e
          (a33 - 1.0) * (a33 - 1.0) + a12 * a12 + a13 * a13 + a21 * a21 +
Shinya Kitaoka 120a6e
          a23 * a23 + a31 * a31 + a32 * a32) < err;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TPointD PerspectiveDistorter::TPerspect::operator*(const TPointD &p) const {
Shinya Kitaoka 120a6e
  double den = (a31 * p.x + a32 * p.y + a33);
Shinya Kitaoka 120a6e
  double x   = (a11 * p.x + a12 * p.y + a13) / den;
Shinya Kitaoka 120a6e
  double y   = (a21 * p.x + a22 * p.y + a23) / den;
Shinya Kitaoka 120a6e
  return TPointD(x, y);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
T3DPointD PerspectiveDistorter::TPerspect::operator*(const T3DPointD &p) const {
Shinya Kitaoka 120a6e
  return T3DPointD(a11 * p.x + a12 * p.y + a13 * p.z,
Shinya Kitaoka 120a6e
                   a21 * p.x + a22 * p.y + a23 * p.z,
Shinya Kitaoka 120a6e
                   a31 * p.x + a32 * p.y + a33 * p.z);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TRectD PerspectiveDistorter::TPerspect::operator*(const TRectD &rect) const {
Shinya Kitaoka 120a6e
  if (rect != TConsts::infiniteRectD) {
Shinya Kitaoka 120a6e
    TPointD p1 = *this * rect.getP00(), p2 = *this * rect.getP01(),
Shinya Kitaoka 120a6e
            p3 = *this * rect.getP10(), p4 = *this * rect.getP11();
Shinya Kitaoka 120a6e
    return TRectD(
Shinya Kitaoka 120a6e
        std::min({p1.x, p2.x, p3.x, p4.x}), std::min({p1.y, p2.y, p3.y, p4.y}),
Shinya Kitaoka 120a6e
        std::max({p1.x, p2.x, p3.x, p4.x}), std::max({p1.y, p2.y, p3.y, p4.y}));
Shinya Kitaoka 120a6e
  } else
Shinya Kitaoka 120a6e
    return TConsts::infiniteRectD;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//================================================================================================
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
// PerspectiveDistorter
Toshihiro Shimizu 890ddd
//
Toshihiro Shimizu 890ddd
//================================================================================================
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::PerspectiveDistorter(
Shinya Kitaoka 120a6e
    const TPointD &p00s, const TPointD &p10s, const TPointD &p01s,
Shinya Kitaoka 120a6e
    const TPointD &p11s, const TPointD &p00d, const TPointD &p10d,
Shinya Kitaoka 120a6e
    const TPointD &p01d, const TPointD &p11d)
Shinya Kitaoka 120a6e
    : TQuadDistorter(p00s, p10s, p01s, p11s, p00d, p10d, p01d, p11d) {
Shinya Kitaoka 120a6e
  computeMatrix();
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
PerspectiveDistorter::~PerspectiveDistorter() {}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void PerspectiveDistorter::computeMatrix() {
Shinya Kitaoka 120a6e
  // Since source and destination points are intended in hundreds of pixels in
Shinya Kitaoka 120a6e
  // their refs,
Shinya Kitaoka 120a6e
  // and inverting makes squares with respect to their elements' size, we'd
Shinya Kitaoka 120a6e
  // better put the
Shinya Kitaoka 120a6e
  // quadrilaterals in more numerically stable references before inversions.
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  double srcSize = std::max({dist(m_p00s, m_p10s), dist(m_p00s, m_p01s),
Shinya Kitaoka 120a6e
                             dist(m_p10s, m_p11s), dist(m_p01s, m_p11s)});
Shinya Kitaoka 120a6e
  double dstSize = std::max({dist(m_p00d, m_p10d), dist(m_p00d, m_p01d),
Shinya Kitaoka 120a6e
                             dist(m_p10d, m_p11d), dist(m_p01d, m_p11d)});
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  TAffine toSrcNormalizedRef(TScale(1.0 / srcSize) * TTranslation(-m_p00s));
Shinya Kitaoka 120a6e
  TAffine toSrcRef(TTranslation(m_p00s) * TScale(srcSize));
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  TAffine toDstNormalizedRef(TScale(1.0 / dstSize) * TTranslation(-m_p00d));
Shinya Kitaoka 120a6e
  TAffine toDstRef(TTranslation(m_p00d) * TScale(dstSize));
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  TPointD p00s;
Shinya Kitaoka 120a6e
  TPointD p10s(toSrcNormalizedRef * m_p10s);
Shinya Kitaoka 120a6e
  TPointD p01s(toSrcNormalizedRef * m_p01s);
Shinya Kitaoka 120a6e
  TPointD p11s(toSrcNormalizedRef * m_p11s);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  TPointD p00d;
Shinya Kitaoka 120a6e
  TPointD p10d(toDstNormalizedRef * m_p10d);
Shinya Kitaoka 120a6e
  TPointD p01d(toDstNormalizedRef * m_p01d);
Shinya Kitaoka 120a6e
  TPointD p11d(toDstNormalizedRef * m_p11d);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // compute a matrix from (0,0), (1,0), (1,1), (0,1) to m_startPoints.
Shinya Kitaoka 120a6e
  TPerspect m1 = computeSquareToMatrix(p00s, p10s, p01s, p11s);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // compute a matrix from (0,0), (1,0), (1,1), (0,1) to m_endPoints.
Shinya Kitaoka 120a6e
  TPerspect m2 = computeSquareToMatrix(p00d, p10d, p01d, p11d);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  m_matrix    = m2 * m1.inv();
Shinya Kitaoka 120a6e
  m_matrixInv = toSrcRef * m_matrix.inv() * toDstNormalizedRef;
Shinya Kitaoka 120a6e
  m_matrix    = toDstRef * m_matrix * toSrcNormalizedRef;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double PerspectiveDistorter::determinant(double a11, double a12, double a21,
Shinya Kitaoka 120a6e
                                         double a22) {
Shinya Kitaoka 120a6e
  return a11 * a22 - a12 * a21;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
PerspectiveDistorter::TPerspect PerspectiveDistorter::computeSquareToMatrix(
Shinya Kitaoka 120a6e
    const TPointD &p00, const TPointD &p10, const TPointD &p01,
Shinya Kitaoka 120a6e
    const TPointD &p11) {
Shinya Kitaoka 120a6e
  TPointD d1 = p10 - p11;
Shinya Kitaoka 120a6e
  TPointD d2 = p01 - p11;
Shinya Kitaoka 120a6e
  TPointD d3 = p00 - p10 + p11 - p01;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  TPerspect matrix;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  matrix.a31 =
Shinya Kitaoka 120a6e
      determinant(d3.x, d2.x, d3.y, d2.y) / determinant(d1.x, d2.x, d1.y, d2.y);
Shinya Kitaoka 120a6e
  matrix.a32 =
Shinya Kitaoka 120a6e
      determinant(d1.x, d3.x, d1.y, d3.y) / determinant(d1.x, d2.x, d1.y, d2.y);
Shinya Kitaoka 120a6e
  matrix.a11 = p10.x - p00.x + matrix.a31 * p10.x;
Shinya Kitaoka 120a6e
  matrix.a12 = p01.x - p00.x + matrix.a32 * p01.x;
Shinya Kitaoka 120a6e
  matrix.a13 = p00.x;
Shinya Kitaoka 120a6e
  matrix.a21 = p10.y - p00.y + matrix.a31 * p10.y;
Shinya Kitaoka 120a6e
  matrix.a22 = p01.y - p00.y + matrix.a32 * p01.y;
Shinya Kitaoka 120a6e
  matrix.a23 = p00.y;
Shinya Kitaoka 120a6e
  matrix.a33 = 1;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return matrix;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TPointD PerspectiveDistorter::map(const TPointD &p) const {
Shinya Kitaoka 120a6e
  return m_matrix * p;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
int PerspectiveDistorter::invMap(const TPointD &p, TPointD *results) const {
Shinya Kitaoka 120a6e
  results[0] = m_matrixInv * p;
Shinya Kitaoka 120a6e
  return 1;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//**********************************************************************************************
Toshihiro Shimizu 890ddd
//    Rect inversion methods
Toshihiro Shimizu 890ddd
//**********************************************************************************************
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//=============================
Toshihiro Shimizu 890ddd
//    PerspectiveDistorter
Toshihiro Shimizu 890ddd
//=============================
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// IDEA: Lines are mapped into lines through this distortion and through the
Shinya Kitaoka 120a6e
// inverse; plus,
Shinya Kitaoka 120a6e
// the distortion is 1-1. There is one major issue: across the horizon line of
Shinya Kitaoka 120a6e
// the perspective
Shinya Kitaoka 120a6e
// projection (ie the pre-image of {z=0}, which could eventually degenerate to
Shinya Kitaoka 120a6e
// the whole plane)
Shinya Kitaoka 120a6e
// the jacobian sign is inverted. Observe, further, that the mapping is
Shinya Kitaoka 120a6e
// infinitely differentiable
Shinya Kitaoka 120a6e
// apart from neighbourhoods of the horizon line.
Shinya Kitaoka 120a6e
// It can be shown that bounding estimates based on corner samples on one side
Shinya Kitaoka 120a6e
// of the horizon line
Shinya Kitaoka 120a6e
// may prove false on the other. Therefore, we shall separate such estimates in
Shinya Kitaoka 120a6e
// the two cases,
Shinya Kitaoka 120a6e
// and then sum them together.
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
//! Returns the jacobian (on source ref) associated to the pre-image of passed
Shinya Kitaoka 120a6e
//! destination point.
Shinya Kitaoka 120a6e
void PerspectiveDistorter::getJacobian(const TPointD &destPoint,
Shinya Kitaoka 120a6e
                                       TPointD &srcPoint, TPointD &xDer,
Shinya Kitaoka 120a6e
                                       TPointD &yDer) const {
Shinya Kitaoka 120a6e
  srcPoint = m_matrixInv * destPoint;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  T3DPointD im(m_matrix * T3DPointD(srcPoint.x, srcPoint.y, 1.0));
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  double imZInv = 1.0 / im.z, minusImZInvSq = -sq(imZInv);
Shinya Kitaoka 120a6e
  TPerspect projectionJac(imZInv, 0.0, minusImZInvSq * im.x, 0.0, imZInv,
Shinya Kitaoka 120a6e
                          minusImZInvSq * im.y, 0.0, 0.0, 0.0);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  TPerspect result(projectionJac * m_matrix);
Shinya Kitaoka 120a6e
  xDer.x = result.a11;
Shinya Kitaoka 120a6e
  xDer.y = result.a21;
Shinya Kitaoka 120a6e
  yDer.x = result.a12;
Shinya Kitaoka 120a6e
  yDer.y = result.a22;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
inline void updateResult(const TPointD &srcCorner, const TPointD &xDer,
Shinya Kitaoka 120a6e
                         const TPointD &yDer, int rectSideX, int rectSideY,
Shinya Kitaoka 120a6e
                         bool &hasPositiveResults, bool &hasNegativeResults,
Shinya Kitaoka 120a6e
                         TRectD &posResult, TRectD &negResult) {
Shinya Kitaoka 120a6e
  const int securityAddendum = 5;  // Adding a 5 border to the result just to be
Shinya Kitaoka 120a6e
                                   // sure about approx errors...
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int jacobianSign = tsign(cross(xDer, yDer));
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int sideDerXAgainstRectSideX = rectSideX * tsign(-xDer.y);
Shinya Kitaoka 120a6e
  int sideDerXAgainstRectSideY = rectSideY * tsign(xDer.x);
Shinya Kitaoka 120a6e
  int sideDerYAgainstRectSideX = rectSideX * tsign(-yDer.y);
Shinya Kitaoka 120a6e
  int sideDerYAgainstRectSideY = rectSideY * tsign(yDer.x);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (jacobianSign > 0) {
Shinya Kitaoka 120a6e
    hasPositiveResults = true;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (sideDerXAgainstRectSideX != -sideDerXAgainstRectSideY)
Shinya Kitaoka 120a6e
      // Rect lies on one side of the derivative line extension. Therefore, the
Shinya Kitaoka 120a6e
      // inverted rect can be updated.
Shinya Kitaoka 120a6e
      if (sideDerXAgainstRectSideX > 0 || sideDerXAgainstRectSideY > 0)
Shinya Kitaoka 120a6e
        posResult.y0 = std::min(posResult.y0, srcCorner.y - securityAddendum);
Shinya Kitaoka 120a6e
      else
Shinya Kitaoka 120a6e
        posResult.y1 = std::max(posResult.y1, srcCorner.y + securityAddendum);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (sideDerYAgainstRectSideX != -sideDerYAgainstRectSideY)
Shinya Kitaoka 120a6e
      if (sideDerYAgainstRectSideX > 0 || sideDerYAgainstRectSideY > 0)
Shinya Kitaoka 120a6e
        posResult.x1 = std::max(posResult.x1, srcCorner.x + securityAddendum);
Shinya Kitaoka 120a6e
      else
Shinya Kitaoka 120a6e
        posResult.x0 = std::min(posResult.x0, srcCorner.x - securityAddendum);
Shinya Kitaoka 120a6e
  } else if (jacobianSign < 0) {
Shinya Kitaoka 120a6e
    hasNegativeResults = true;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (sideDerXAgainstRectSideX != -sideDerXAgainstRectSideY)
Shinya Kitaoka 120a6e
      if (sideDerXAgainstRectSideX > 0 || sideDerXAgainstRectSideY > 0)
Shinya Kitaoka 120a6e
        negResult.y1 = std::max(posResult.y1, srcCorner.y + securityAddendum);
Shinya Kitaoka 120a6e
      else
Shinya Kitaoka 120a6e
        negResult.y0 = std::min(posResult.y0, srcCorner.y - securityAddendum);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    if (sideDerYAgainstRectSideX != -sideDerYAgainstRectSideY)
Shinya Kitaoka 120a6e
      if (sideDerYAgainstRectSideX > 0 || sideDerYAgainstRectSideY > 0)
Shinya Kitaoka 120a6e
        negResult.x0 = std::min(posResult.x0, srcCorner.x - securityAddendum);
Shinya Kitaoka 120a6e
      else
Shinya Kitaoka 120a6e
        negResult.x1 = std::max(posResult.x1, srcCorner.x + securityAddendum);
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TRectD PerspectiveDistorter::invMap(const TRectD &rect) const {
Shinya Kitaoka 120a6e
  // For each corner, find the jacobian. Then, decide by which side the rect's
Shinya Kitaoka 120a6e
  // pre-image lie with respect to the partial derivatives.
Shinya Kitaoka 120a6e
  // Observe that the two horizon-separated semiplanes do not compete -
Shinya Kitaoka 120a6e
  // the requirement for each is added to the result.
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  TPointD srcCorner, xDer, yDer;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double maxD = (std::numeric_limits<double>::max)();</double>
Shinya Kitaoka 120a6e
  TRectD positiveResult(maxD, maxD, -maxD, -maxD);
Shinya Kitaoka 120a6e
  TRectD negativeResult(positiveResult);
Shinya Kitaoka 120a6e
  bool hasPositiveResults = false, hasNegativeResults = false;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  getJacobian(rect.getP00(), srcCorner, xDer, yDer);
Shinya Kitaoka 120a6e
  updateResult(srcCorner, xDer, yDer, 1, 1, hasPositiveResults,
Shinya Kitaoka 120a6e
               hasNegativeResults, positiveResult, negativeResult);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  getJacobian(rect.getP10(), srcCorner, xDer, yDer);
Shinya Kitaoka 120a6e
  updateResult(srcCorner, xDer, yDer, -1, 1, hasPositiveResults,
Shinya Kitaoka 120a6e
               hasNegativeResults, positiveResult, negativeResult);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  getJacobian(rect.getP01(), srcCorner, xDer, yDer);
Shinya Kitaoka 120a6e
  updateResult(srcCorner, xDer, yDer, 1, -1, hasPositiveResults,
Shinya Kitaoka 120a6e
               hasNegativeResults, positiveResult, negativeResult);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  getJacobian(rect.getP11(), srcCorner, xDer, yDer);
Shinya Kitaoka 120a6e
  updateResult(srcCorner, xDer, yDer, -1, -1, hasPositiveResults,
Shinya Kitaoka 120a6e
               hasNegativeResults, positiveResult, negativeResult);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  // If some maxD remain, no bound on that side was found. So replace with
Shinya Kitaoka 120a6e
  // the opposite (unlimited on that side) maxD.
Shinya Kitaoka 120a6e
  if (positiveResult.x0 == maxD) positiveResult.x0  = -maxD;
Shinya Kitaoka 120a6e
  if (positiveResult.x1 == -maxD) positiveResult.x1 = maxD;
Shinya Kitaoka 120a6e
  if (positiveResult.y0 == maxD) positiveResult.y0  = -maxD;
Shinya Kitaoka 120a6e
  if (positiveResult.y1 == -maxD) positiveResult.y1 = maxD;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (negativeResult.x0 == maxD) negativeResult.x0  = -maxD;
Shinya Kitaoka 120a6e
  if (negativeResult.x1 == -maxD) negativeResult.x1 = maxD;
Shinya Kitaoka 120a6e
  if (negativeResult.y0 == maxD) negativeResult.y0  = -maxD;
Shinya Kitaoka 120a6e
  if (negativeResult.y1 == -maxD) negativeResult.y1 = maxD;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return hasPositiveResults
Shinya Kitaoka 120a6e
             ? hasNegativeResults ? positiveResult + negativeResult
Shinya Kitaoka 120a6e
                                  : positiveResult
Shinya Kitaoka 120a6e
             : hasNegativeResults ? negativeResult : TConsts::infiniteRectD;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//=================================================================================
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//=============================
Toshihiro Shimizu 890ddd
//    BilinearDistorter
Toshihiro Shimizu 890ddd
//=============================
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// IDEA: This time, lines are mapped to curves, in general. Plus, from 0 to 2
Shinya Kitaoka 120a6e
// possible inverses
Shinya Kitaoka 120a6e
// may exist for the same destination point.
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// Given the separation of the bilinear mapping in two bilinear mappings with an
Shinya Kitaoka 120a6e
// intermediate reference for the
Shinya Kitaoka 120a6e
// convex coordinates, it can be shown that a bounding estimate for the inverse
Shinya Kitaoka 120a6e
// of a rect can be found this way:
Shinya Kitaoka 120a6e
//  1. Inverse-map the corners of the rect in the convex-reference (ie find
Shinya Kitaoka 120a6e
//  their convex coordinates)
Toshihiro Shimizu 890ddd
//  2. Make their bounding box
Toshihiro Shimizu 890ddd
//  3. Map its corners to the source reference
Toshihiro Shimizu 890ddd
//  4. Return their bounding box
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// In order to show that this actually works, consider the following result:
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
// A rect maps to a convex quadrilateral through a forward bilinear mapping (ie
Shinya Kitaoka 120a6e
// passages 3->4 and 2->1).
Shinya Kitaoka 120a6e
// It is sufficient to consider that lines map to lines through one such
Shinya Kitaoka 120a6e
// mapping, and that since it is
Shinya Kitaoka 120a6e
// also everywhere differentiable, its local behaviour around corners is that of
Shinya Kitaoka 120a6e
// a linear function, through
Shinya Kitaoka 120a6e
// which convex angles are only mappable to other convex angles.
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
//---------------------------------------------------------------------------------
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
TRectD BilinearDistorter::invMap(const TRectD &rect) const {
Shinya Kitaoka 120a6e
  // Build the convex coordinates of the rect's corners.
Shinya Kitaoka 120a6e
  TPointD invs[8];
Shinya Kitaoka 120a6e
  int count[4];
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  count[0] = m_refToDest.invMap(rect.getP00(), &invs[0]);
Shinya Kitaoka 120a6e
  count[1] = m_refToDest.invMap(rect.getP10(), &invs[2]);
Shinya Kitaoka 120a6e
  count[2] = m_refToDest.invMap(rect.getP01(), &invs[4]);
Shinya Kitaoka 120a6e
  count[3] = m_refToDest.invMap(rect.getP11(), &invs[6]);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  double maxD = (std::numeric_limits<double>::max)();</double>
Shinya Kitaoka 120a6e
  TRectD bbox(maxD, maxD, -maxD, -maxD);
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  int i, j;
Shinya Kitaoka 120a6e
  for (i = 0; i < 4; ++i) {
Shinya Kitaoka 120a6e
    for (j = 0; j < count[i]; ++j) {
Shinya Kitaoka 120a6e
      TPointD &inv(invs[j + 2 * i]);
Shinya Kitaoka 120a6e
      bbox.x0 = std::min(bbox.x0, inv.x);
Shinya Kitaoka 120a6e
      bbox.y0 = std::min(bbox.y0, inv.y);
Shinya Kitaoka 120a6e
      bbox.x1 = std::max(bbox.x1, inv.x);
Shinya Kitaoka 120a6e
      bbox.y1 = std::max(bbox.y1, inv.y);
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  if (bbox.x1 <= bbox.x0 || bbox.y1 <= bbox.y0)
Shinya Kitaoka 120a6e
    return TConsts::infiniteRectD;  // Should happen only if all counts are 0
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  invs[0] = m_refToSource.map(bbox.getP00());
Shinya Kitaoka 120a6e
  invs[1] = m_refToSource.map(bbox.getP10());
Shinya Kitaoka 120a6e
  invs[2] = m_refToSource.map(bbox.getP01());
Shinya Kitaoka 120a6e
  invs[3] = m_refToSource.map(bbox.getP11());
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  bbox.x0 = std::min({invs[0].x, invs[1].x, invs[2].x, invs[3].x});
Shinya Kitaoka 120a6e
  bbox.y0 = std::min({invs[0].y, invs[1].y, invs[2].y, invs[3].y});
Shinya Kitaoka 120a6e
  bbox.x1 = std::max({invs[0].x, invs[1].x, invs[2].x, invs[3].x});
Shinya Kitaoka 120a6e
  bbox.y1 = std::max({invs[0].y, invs[1].y, invs[2].y, invs[3].y});
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  return bbox.enlarge(5);  // Enlarge a little just to be sure
Toshihiro Shimizu 890ddd
}