Base classes of the FrameStore implementation.

Feel free to review the stuff and tell me what I did wrong.
Test files are next on my list.
Missing from the commit is also the actual FrameStore class, which needs
a bit more polish, before I add it.

The story so far:
- ATrans: Affine transform class
  * as the matrix is not necessarily a rotation matrix, the 
    accessor is probably named wrong
  * tries to be "compatible" with other matrix classes,
    but is probalby missing some features in that respect
- TreeNode: Generic tree implementation for n-iary trees
  * could be probably used in other places, where trees
    with varying number of childs are required, so feel free
    to take a look if you'd like to generalize it further
  * it's pretty much optimized for child->parent lookup
    the other way round is slow, due to the use of
    single-linked, un-ordered list.
- Frame: Named affine transform
  * Is probably missing some helper methods to beautify
    rpy assignements etc
  * The RAPID folks would most likely want to see a vector<string>
    extras be added
  * The typical use case would probably benefit from the ability
    to cache single-rotation-axis updates
- FrameTreeNode: Typedef and helpers for a tree of frame-transforms

I also added doxygen documenation and added the Geometry module to the
Doxyfile. Please revert, if Geometry is not intended to be documented
by default.

src/Doxyfile

@@ -82,7 +82,7 @@ WARN_LOGFILE           =
 #---------------------------------------------------------------------------
 # configuration options related to the input files
 #---------------------------------------------------------------------------
-INPUT                  = vw vw/Core vw/Image vw/Math vw/FileIO vw/Camera vw/HDR vw/Mosaic vw/Cartography vw/GPU
+INPUT                  = vw vw/Core vw/Image vw/Math vw/FileIO vw/Camera vw/HDR vw/Mosaic vw/Cartography vw/GPU vw/Geometry
 FILE_PATTERNS          = *.h
 RECURSIVE              = NO
 EXCLUDE                = 

src/vw/Geometry.h

@@ -17,5 +17,9 @@
 #include <vw/Geometry/Sphere.h>
 #include <vw/Geometry/SpatialTree.h>
 #include <vw/Geometry/PointListIO.h>
+#include <vw/Geometry/ATrans.h>
+#include <vw/Geometry/Frame.h>
+#include <vw/Geometry/TreeNode.h>
+#include <vw/Geometry/FrameTreeNode.h>
 
 #endif // __VW_GEOMETRY_H__

src/vw/Geometry/ATrans.h

@@ -0,0 +1,164 @@
+// -*- c++ -*-
+#ifndef vw_geometry_ATrans_h
+#define vw_geometry_ATrans_h
+
+#include "vw/Math/Vector.h"
+#include "vw/Math/Matrix.h"
+
+namespace vw
+{
+namespace geometry
+{
+  template<class ElemT, int SizeN>
+  class ATrans;
+
+  //! Helper function for inverting an affine transform.
+  template<class ElemT, int SizeN>
+  ATrans<ElemT, SizeN> inverse(ATrans<ElemT, SizeN> const& atrans);
+
+  //! Output stream operator for affine transformation class.
+  template<class ElemT, int SizeN>
+  std::ostream& operator<< (std::ostream& ostr, ATrans<ElemT, SizeN> const& rhs);
+
+
+  //! Class representing an affine transform.
+  /**
+   * Affine transforms of dimension N consist of a NxN matrix and an
+   * vector of size N.  An affine transform of dimension N can be
+   * expressed as a matrix of dimension (N+1)x(N+1) with the last row
+   * consisting of [0, 0, ..., 1]. While the later allows for a more
+   * compact representation, the former is computationally more
+   * efficient.
+   *
+   * This class represents affine transforms internally as matrix plus
+   * vector. The user-visible part tries hard to follow the
+   * (N+1)x(N+1) matrix notation.
+   *
+   * The primary customers of this class are the Frame &
+   * FrameStore classes.  The affine transformation in this case holds
+   * a position-vector and rotation matrix.
+   *
+   */
+  template<class ElemT = double, int SizeN = 3>
+  class ATrans
+  {
+  public:
+    //! Vector type (translation part)
+    typedef vw::math::Vector<ElemT, SizeN> VectorT;
+    //! Matrix type (rotation part)
+    typedef vw::math::Matrix<ElemT, SizeN, SizeN> MatrixT;
+
+    //! Default constructor
+    ATrans() {}
+    //! Initializing constructor
+    ATrans(VectorT const& p, MatrixT const& r) : m_p(p), m_r(r) {}
+    //! Initialize from transformation matrix
+    ATrans(vw::math::Matrix<ElemT, SizeN + 1, SizeN + 1> const& m);
+    //! Initialize from transformation matrix
+    ATrans(vw::math::Matrix<ElemT, 0, 0> const& m);
+
+
+    //! Const accessor of the translation vector.
+    VectorT const& translation() const throw() { return m_p; }
+    //! Accessor of the translation vector.
+    VectorT& translation() throw() { return m_p; }
+
+    //! Const accessor of the (rotation) matrix.
+    MatrixT const& rotation() const throw() { return m_r; }
+    //! Accessor of the (rotation) matrix.
+    MatrixT& rotation() throw() { return m_r; }
+
+    //! Matrix N+1 style multiplication
+    ATrans operator*(ATrans const& rhs) const;
+    //! In-place matrix N+1 style multiplication
+    void operator*=(ATrans const& rhs);
+    //! Position vecotr multiplication
+    VectorT operator*(VectorT const& rhs) const;
+
+    //! Conversion to matrix N+1
+    operator vw::math::Matrix<ElemT, SizeN + 1, SizeN + 1>();
+
+  protected:
+    //! Position vector of the affine transform.
+    VectorT m_p;
+    //! (Rotation) matrix of the affine transform.
+    MatrixT m_r;
+  };
+
+  template<class ElemT, int SizeN>
+  inline
+  ATrans<ElemT, SizeN> inverse(ATrans<ElemT, SizeN> const& atrans)
+  {
+    typename ATrans<ElemT, SizeN>::MatrixT mI = inverse(atrans.rotation());
+    return ATrans<ElemT, SizeN>(mI * atrans.translation(), mI);
+  }
+
+  template<class ElemT, int SizeN>
+  std::ostream& operator<< (std::ostream& ostr, ATrans<ElemT, SizeN> const& rhs)
+  {
+    ostr << "xATrans(" << rhs.translation() << "), (" << rhs.rotation() << ")";
+    return ostr;
+  }
+
+
+  template<class ElemT, int SizeN>
+  inline
+  ATrans<ElemT, SizeN>::ATrans(vw::math::Matrix<ElemT, SizeN + 1, SizeN + 1> const& m) :
+    m_p(m(0, 3), m(1,3), m(2, 3))
+  {
+    for (int i = 0; i < SizeN; ++i) {
+      for (int j = 0; j < SizeN; ++j) {
+	m_r(i, j) = m(i, j);
+      }
+    }
+  }
+
+  template<class ElemT, int SizeN>
+  inline
+  ATrans<ElemT, SizeN>::ATrans(vw::math::Matrix<ElemT, 0,0> const& m) :
+    m_p(m(0, 3), m(1,3), m(2, 3))
+  {
+    for (int i = 0; i < SizeN; ++i) {
+      for (int j = 0; j < SizeN; ++j) {
+	m_r(i, j) = m(i, j);
+      }
+    }
+  }
+
+  template<class ElemT, int SizeN>
+  inline
+  ATrans<ElemT, SizeN>
+  ATrans<ElemT, SizeN>::operator*(ATrans const& rhs) const {
+    return ATrans(m_r * rhs.m_p + m_p, m_r * rhs.m_r);
+  }
+
+  template<class ElemT, int SizeN>
+  inline
+  void
+  ATrans<ElemT, SizeN>::operator*=(ATrans const& rhs) {
+    *this = *this * rhs;
+  }
+
+  template<class ElemT, int SizeN>
+  inline
+  typename ATrans<ElemT, SizeN>::VectorT
+  ATrans<ElemT, SizeN>::operator*(VectorT const& p) const {
+    VectorT v = m_r * p;
+    return v + p;
+  }
+
+  template<class ElemT, int SizeN>
+  inline
+  ATrans<ElemT, SizeN>::operator vw::math::Matrix<ElemT, SizeN+1, SizeN+1>()
+  {
+    vw::math::Matrix<ElemT, SizeN+1, SizeN+1> m;
+    return m;
+  }
+}
+
+  typedef geometry::ATrans<double, 3> ATrans3;
+  typedef geometry::ATrans<float, 3> ATrans3f;
+}
+
+#endif // vw_geometry_ATrans_h
+

src/vw/Geometry/Frame.h

@@ -0,0 +1,50 @@
+// -*- C++ -*-
+#ifndef vw_Math_Frame_h
+#define vw_Math_Frame_h
+
+#include "ATrans.h"
+
+#include <string>
+
+namespace vw
+{
+namespace geometry
+{
+
+  /** Class representing a named coordinate transform.
+   *
+   * This class is mostly used in combination with the TreeNode 
+   * class to create a frame tree.
+   */
+  class Frame 
+  {
+  public:
+    typedef vw::ATrans3 Location;
+
+    /// Default constructor.
+    Frame() {}
+    /// Initializing constructor.
+    Frame(std::string const& name, Location const& loc = identity_matrix<4>()) :
+      m_name(name),
+      m_loc(loc)
+    {}
+
+    /// @{ Accessor methods
+
+    /// Access name field.
+    std::string const& name() const throw() { return m_name; }
+    /// Set name field.
+    void set_name(std::string const& name) { m_name = name; }
+    /// Access location field.
+    Location const& location() const throw() { return m_loc; }
+    /// Set location field.
+    void set_location(Location const& loc) { m_loc = loc; }
+    /// @}
+
+  protected:
+    std::string m_name;
+    Location m_loc;
+  };
+}
+}
+#endif // vw_Math_Frame_h

src/vw/Geometry/FrameTreeNode.cpp

@@ -0,0 +1,109 @@
+#include "FrameTreeNode.h"
+
+#include <algorithm>
+
+namespace vw
+{
+namespace geometry
+{
+  using namespace std;
+
+  Frame::Location
+  get_location(FrameTreeNode const * wrtFrame, FrameTreeNode const * source)
+  {
+    Frame::Location loc = vw::identity_matrix(4);
+
+    if (source != NULL) {
+
+      if (wrtFrame == NULL)
+	return source->data().location();
+
+      if (wrtFrame != source) {
+
+	FrameTreeNode const * ancestor = wrtFrame->last_common_ancestor(source);
+
+	if (ancestor != NULL) {
+	  // from the origin frame to the ancestor
+	  {
+	    FrameTreeNode::NodeVector const& nodes = wrtFrame->ancestry(true);
+	    FrameTreeNode::NodeVector::const_iterator iter =
+	      std::find(nodes.begin(), nodes.end(), ancestor);
+
+	    assert (iter != nodes.end());
+
+	    ++iter;
+	    if (iter != nodes.end()) {
+	      for (; iter != nodes.end(); ++iter) {
+		loc *= (*iter)->data().location();
+	      }
+	      loc = inverse(loc);
+	    }
+	  }
+
+
+	  // from the last common ancestor to the wrt frame
+	  {	
+	    FrameTreeNode::NodeVector const& nodes = source->ancestry(true);
+	    FrameTreeNode::NodeVector::const_iterator iter =
+	      std::find(nodes.begin(), nodes.end(), ancestor);
+
+	    assert (iter != nodes.end());
+
+	    for (; iter != nodes.end(); ++iter) {
+	      loc *= (*iter)->data().location();
+	    }
+	  }
+	}
+      }
+   }
+
+   return loc;
+  }
+
+  namespace 
+  {
+    struct FtnLess
+    {
+      bool operator () (FrameTreeNode const * lhs, FrameTreeNode const * rhs)
+      {
+	return lhs->data().name() < rhs->data().name();
+      }
+    };
+  }
+
+  void 
+  merge_frame_trees(FrameTreeNode * target_tree, FrameTreeNode * source_tree)
+  {
+    if (target_tree->data().name() != source_tree->data().name())
+      return;
+
+    FrameTreeNode::NodeVector src_children = target_tree->children();
+
+    if (src_children.size() > 0) {
+      FrameTreeNode::NodeVector tgt_children = source_tree->children();
+
+      FtnLess less;
+      sort(src_children.begin(), src_children.end(), less);
+      sort(tgt_children.begin(), tgt_children.end(), less);
+
+      FrameTreeNode::NodeVector::const_iterator first, last = src_children.end();
+      FrameTreeNode::NodeVector::const_iterator tgt_iter = tgt_children.end();
+      for (first = src_children.begin(); first != last; ++first) {
+	while (tgt_iter != tgt_children.end() &&
+	       (*first)->data().name() > (*tgt_iter)->data().name()) {
+	  ++tgt_iter;
+	}
+
+	if (tgt_iter == tgt_children.end() ||
+	    (*first)->data().name() < (*tgt_iter)->data().name()) {
+	  (*first)->set_parent(target_tree);
+	}
+	else if ((*first)->data().name() == (*tgt_iter)->data().name()) {
+	  merge_frame_trees(*tgt_iter, *first);
+	}
+      }
+
+    }
+  }
+}
+}