transform2D.cpp

Go to the documentation of this file.

#include <dlrNumeric/transform2D.h>

namespace dlr {

  namespace numeric {
    
    // Build a Transform2D from a homogeneous 3x3 matrix.
    Transform2D::
    Transform2D(const Array2D<double>& source)
    {
      if((source.rows() != 3) || (source.columns() != 3)) {
        std::ostringstream message;
        message << "Can't create a Transform2D from a " << source.rows()
                << " x " << source.columns() << "Array2D<double> instance.";
        DLR_THROW(ValueException, "Transform2D::Transform2D()",
                  message.str().c_str());
      }
      m_00 = source(0); m_01 = source(1); m_02 = source(2);
      m_10 = source(3); m_11 = source(4); m_12 = source(5);
      m_20 = source(6); m_21 = source(7);
      this->normalize(source(8));
    }


    // This member function returns a functor which makes it easier to
    // transform arrays of points using algorithms such as
    // std::transform().
    Transform2DFunctor
    Transform2D::
    getFunctor() const {
      return Transform2DFunctor(*this);
    }    
  
  
    // This member function returns the inverse of *this.
    Transform2D
    Transform2D::
    invert() const
    {
      // We use the cofactor method for now, since it's easier to code
      // than Gauss-Jordan elimination.  We suspect that it's less
      // efficient, however.
    
      // Notation for determinant values is detRRCC, where the
      // Rs indicate the involved rows, from top to bottom, and the Cs
      // indicate the involved columns, from left to right.

      double det0101 = m_00 * m_11 - m_01 * m_10;
      double det0102 = m_00 * m_12 - m_02 * m_10;
      double det0112 = m_01 * m_12 - m_02 * m_11;

      double det0201 = m_00 * m_21 - m_01 * m_20;
      double det0202 = m_00 - m_02 * m_20;
      double det0212 = m_01 - m_02 * m_21;

      double det1201 = m_10 * m_21 - m_11 * m_20;
      double det1202 = m_10 - m_12 * m_20;
      double det1212 = m_11 - m_12 * m_21;

      double det012012 = (
        m_00 * det1212 - m_01 * det1202 + m_02 * det1201
        - m_10 * det0212 + m_11 * det0202 - m_12 * det0201
        + m_20 * det0112 - m_21 * det0102 + det0101);
    
      // Note that in general, roundoff error will make us pass these
      // tests, even for singular matrices.
      if(det012012 == 0.0) {
        DLR_THROW(ValueException, "Transform2D::invert()",
                  "Transform is not invertible.");
      }
      if(det0101 == 0.0) {
        DLR_THROW(LogicException, "Transform2D::invert()",
                  "Illegal value for projective scale.");
      }
    
      return Transform2D(
        det1212 / det012012, -det0212 / det012012, det0112 / det012012,
        -det1202 / det012012, det0202 / det012012, -det0102 / det012012,
        det1201 / det012012, -det0201 / det012012, det0101 / det012012);
    }

  
    // Change the Transform2D value by explicitly setting element values
    // as if setting the elements of a 3x3 transformation matrix:
    //    [[a00, a01, a02],
    //     [a10, a11, a12],
    //     [a20, a21, a22]]
    void
    Transform2D::
    setTransform(double a00, double a01, double a02,
                 double a10, double a11, double a12,
                 double a20, double a21, double a22)
    {
      m_00 = a00; m_01 = a01; m_02 = a02;
      m_10 = a10; m_11 = a11; m_12 = a12;
      m_20 = a20; m_21 = a21;
      this->normalize(a22);
    }


    // This member function sets one element from the matrix
    // representation of the coordinate transform to the specified
    // value.
    void
    Transform2D::
    setValue(size_t row, size_t column, double value)
    {
      switch(row) {
      case 0:
        switch(column) {
        case 0: m_00 = value; return; break;
        case 1: m_01 = value; return; break;
        case 2: m_02 = value; return; break;
        default: break;
        }
        break;
      case 1:
        switch(column) {
        case 0: m_10 = value; return; break;
        case 1: m_11 = value; return; break;
        case 2: m_12 = value; return; break;
        default: break;
        }
        break;
      case 2:
        switch(column) {
        case 0: m_20 = value; return; break;
        case 1: m_21 = value; return; break;
        default: break;
        }
        break;
      default:
        break;
      }
      std::ostringstream message;
      message << "Indices (" << row << ", " << column << ") are out of bounds.";
      DLR_THROW(IndexException, "Transform2D::operator()(size_t, size_t)",
                message.str().c_str());
    }

  
    // This operator returns one element from the matrix
    // representation of the coordinate transform by value.
    double
    Transform2D::
    operator()(size_t row, size_t column) const
    {
      // // Avoid ugly duplication of code using ugly const_cast.
      // return const_cast<Transform2D*>(this)->operator()(row, column);
      switch(row) {
      case 0:
        switch(column) {
        case 0: return m_00; break;
        case 1: return m_01; break;
        case 2: return m_02; break;
        default: break;
        }
        break;
      case 1:
        switch(column) {
        case 0: return m_10; break;
        case 1: return m_11; break;
        case 2: return m_12; break;
        default: break;
        }
        break;
      case 2:
        switch(column) {
        case 0: return this->value<2, 0>(); break;
        case 1: return m_21; break;
        case 2: return 1.0; break;
        default: break;
        }
        break;
      default:
        break;
      }
      std::ostringstream message;
      message << "Index (" << row << ", " << column << ") out of bounds.";
      DLR_THROW3(IndexException, "Transform2D::value()",
                 message.str().c_str());
      return 0.0; // Dummy return to keep the compiler happy.
    }
  
    // This operator takes a point and applies the coordinate
    // transform, returning the result.
    Vector2D
    Transform2D::
    operator*(const Vector2D& vector0) const
    {
      return Vector2D(
        m_00 * vector0.x() + m_01 * vector0.y() + m_02,
        m_10 * vector0.x() + m_11 * vector0.y() + m_12,
        m_20 * vector0.x() + m_21 * vector0.y() + 1.0);
    }

    // The assignment operator simply duplicates its argument.
    Transform2D&
    Transform2D::
    operator=(const Transform2D& source)
    {
      m_00 = source.m_00; m_01 = source.m_01; m_02 = source.m_02;
      m_10 = source.m_10; m_11 = source.m_11; m_12 = source.m_12;
      m_20 = source.m_20; m_21 = source.m_21;
      return *this;
    }

    void
    Transform2D::
    normalize(double scaleFactor)
    {
      if(scaleFactor == 0.0) {
        DLR_THROW3(ValueException, "Trahnsform2D::normalize(double)",
                   "Invalid normalization constant. "
                   "The bottom right element of a homogeneous transformation "
                   "cannot be equal to 0.0.");
      }
      if(scaleFactor != 1.0) {
        m_00 /= scaleFactor;
        m_01 /= scaleFactor;
        m_02 /= scaleFactor;
        m_10 /= scaleFactor;
        m_11 /= scaleFactor;
        m_12 /= scaleFactor;
        m_20 /= scaleFactor;
        m_21 /= scaleFactor;
      }
    }

    /* ============== Non-member functions which should ============== */
    /* ============== probably live in a different file ============== */
  
  
    // This operator composes two Transform2D instances.  The resulting
    // transform satisfies the equation:
    //   (transform0 * transform1) * v0 = transform0 * (transform1 * v0),
    // where v0 is a Vector2D instance.
    Transform2D
    operator*(const Transform2D& transform0, const Transform2D& transform1)
    {
      double a00 = (transform0.value<0, 0>() * transform1.value<0, 0>()
                    + transform0.value<0, 1>() * transform1.value<1, 0>()
                    + transform0.value<0, 2>() * transform1.value<2, 0>());
      double a01 = (transform0.value<0, 0>() * transform1.value<0, 1>()
                    + transform0.value<0, 1>() * transform1.value<1, 1>()
                    + transform0.value<0, 2>() * transform1.value<2, 1>());
      double a02 = (transform0.value<0, 0>() * transform1.value<0, 2>()
                    + transform0.value<0, 1>() * transform1.value<1, 2>()
                    + transform0.value<0, 2>() * transform1.value<2, 2>());
      double a10 = (transform0.value<1, 0>() * transform1.value<0, 0>()
                    + transform0.value<1, 1>() * transform1.value<1, 0>()
                    + transform0.value<1, 2>() * transform1.value<2, 0>());
      double a11 = (transform0.value<1, 0>() * transform1.value<0, 1>()
                    + transform0.value<1, 1>() * transform1.value<1, 1>()
                    + transform0.value<1, 2>() * transform1.value<2, 1>());
      double a12 = (transform0.value<1, 0>() * transform1.value<0, 2>()
                    + transform0.value<1, 1>() * transform1.value<1, 2>()
                    + transform0.value<1, 2>() * transform1.value<2, 2>());
      double a20 = (transform0.value<2, 0>() * transform1.value<0, 0>()
                    + transform0.value<2, 1>() * transform1.value<1, 0>()
                    + transform0.value<2, 2>() * transform1.value<2, 0>());
      double a21 = (transform0.value<2, 0>() * transform1.value<0, 1>()
                    + transform0.value<2, 1>() * transform1.value<1, 1>()
                    + transform0.value<2, 2>() * transform1.value<2, 1>());
      double a22 = (transform0.value<2, 0>() * transform1.value<0, 2>()
                    + transform0.value<2, 1>() * transform1.value<1, 2>()
                    + transform0.value<2, 2>() * transform1.value<2, 2>());
      return Transform2D(a00, a01, a02,
                         a10, a11, a12,
                         a20, a21, a22);
    }


    std::ostream&
    operator<<(std::ostream& stream, const Transform2D& transform0)
    {
      stream << "Transform2D("
             << transform0.value<0, 0>() << ", "
             << transform0.value<0, 1>() << ", "
             << transform0.value<0, 2>() << ", "
             << transform0.value<1, 0>() << ", "
             << transform0.value<1, 1>() << ", "
             << transform0.value<1, 2>() << ", "
             << transform0.value<2, 0>() << ", "
             << transform0.value<2, 1>() << ", "
             << transform0.value<2, 2>() << ")";
      return stream;
    }

  
    std::istream&
    operator>>(std::istream& stream, Transform2D& transform0)
    {
      // If stream is in a bad state, we can't read from it.
      if (!stream){
        return stream;
      }
    
      // It's a lot easier to use a try block than to be constantly
      // testing whether the IO has succeeded, so we tell stream to
      // complain if anything goes wrong.
      std::ios_base::iostate oldExceptionState = stream.exceptions();
      stream.exceptions(
        std::ios_base::badbit | std::ios_base::failbit | std::ios_base::eofbit);

      // Now on with the show.
      try{
        // Construct an InputStream instance so we can use our
        // convenience functions.
        InputStream inputStream(stream);

        // Advance to the next relevant character.
        inputStream.skipWhiteSpace();
      
        // Read the "Transform2D(" part.
        inputStream.expect("Transform2D(");

        // Read all the data except the last element.
        std::vector<double> inputValues(9);
        for(size_t index = 0; index < (inputValues.size() - 1); ++index) {
          // Read the value.
          inputStream >> inputValues[index];

          // Read punctuation before the next value.
          inputStream.expect(",");
        }

        // Read the final value.
        inputStream >> inputValues[inputValues.size() - 1];

        // Read the closing parenthesis.
        inputStream.expect(")");

        // And update the transform.
        transform0.setTransform(
          inputValues[0], inputValues[1], inputValues[2],
          inputValues[3], inputValues[4], inputValues[5],
          inputValues[6], inputValues[7], inputValues[8]);

      } catch(std::ios_base::failure) {
        // Empty
      }
      stream.exceptions(oldExceptionState);
      return stream;
    }
  
  } // namespace numeric

} // namespace dlr

---