renamed dist to distance (dist still works but now deprecated)

gtsam/geometry/Cal3DS2.cpp

@@ -83,7 +83,7 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
   const int maxIterations = 10;
   int iteration;
   for ( iteration = 0 ; iteration < maxIterations ; ++iteration ) {
-    if ( uncalibrate(pn).dist(pi) <= tol ) break;
+    if ( uncalibrate(pn).distance(pi) <= tol ) break;
     const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y ;
     const double rr = xx + yy ;
     const double g = (1+k1_*rr+k2_*rr*rr) ;

gtsam/geometry/Point3.h

@@ -153,8 +153,13 @@ namespace gtsam {
     Point3 operator / (double s) const;
 
     /** distance between two points */
-    double dist(const Point3& p2) const {
+    inline double distance(const Point3& p2) const {
-      return std::sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0) + pow(z()-p2.z(),2.0));
+      return (p2 - *this).norm();
+    }
+
+    /** @deprecated The following function has been deprecated, use distance above */
+    inline double dist(const Point3& p2) const {
+      return (p2 - *this).norm();
     }
 
     /** Distance of the point from the origin */

gtsam/geometry/tests/testPoint2.cpp

@@ -75,19 +75,28 @@ TEST( Point2, norm)
   Point2 p0(cos(5.0), sin(5.0));
   DOUBLES_EQUAL(1,p0.norm(),1e-6);
   Point2 p1(4, 5), p2(1, 1);
-  DOUBLES_EQUAL( 5,p1.dist(p2),1e-6);
+  DOUBLES_EQUAL( 5,p1.distance(p2),1e-6);
   DOUBLES_EQUAL( 5,(p2-p1).norm(),1e-6);
 }
 
 /* ************************************************************************* */
 TEST( Point2, unit)
 {
-  Point2 p0(10.0, 0.0), p1(0.0,-10.0), p2(10.0, 10.0);
+  Point2 p0(10, 0), p1(0,-10), p2(10, 10);
-  EXPECT(assert_equal(Point2(1.0, 0.0), p0.unit(), 1e-6));
+  EXPECT(assert_equal(Point2(1, 0), p0.unit(), 1e-6));
-  EXPECT(assert_equal(Point2(0.0,-1.0), p1.unit(), 1e-6));
+  EXPECT(assert_equal(Point2(0,-1), p1.unit(), 1e-6));
   EXPECT(assert_equal(Point2(sqrt(2.0)/2.0, sqrt(2.0)/2.0), p2.unit(), 1e-6));
 }
 
+/* ************************************************************************* */
+TEST( Point2, stream)
+{
+  Point2 p(1,2);
+  std::ostringstream os;
+  os << p;
+  EXPECT(os.str() == "(1, 2)");
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam_unstable/dynamics/PoseRTV.cpp

@@ -232,7 +232,7 @@ double PoseRTV::range(const PoseRTV& other,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
   if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
   if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
-  return t().dist(other.t());
+  return t().distance(other.t());
 }
 
 /* ************************************************************************* */

gtsam_unstable/geometry/SimPolygon2D.cpp

@@ -152,7 +152,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
     Pose2 xC = xB.retract(delta(3, 0, dBC));
 
     // use triangle equality to verify non-degenerate triangle
-    double dAC = xA.t().dist(xC.t());
+    double dAC = xA.t().distance(xC.t());
 
     // form a triangle and test if it meets requirements
     SimPolygon2D test_tri = SimPolygon2D::createTriangle(xA.t(), xB.t(), xC.t());
@@ -165,7 +165,7 @@ SimPolygon2D SimPolygon2D::randomTriangle(
         insideBox(side_len, test_tri.landmark(0)) &&
         insideBox(side_len, test_tri.landmark(1)) &&
         insideBox(side_len, test_tri.landmark(2)) &&
-        test_tri.landmark(1).dist(test_tri.landmark(2)) > min_side_len &&
+        test_tri.landmark(1).distance(test_tri.landmark(2)) > min_side_len &&
         !nearExisting(lms, test_tri.landmark(0), min_vertex_dist) &&
         !nearExisting(lms, test_tri.landmark(1), min_vertex_dist) &&
         !nearExisting(lms, test_tri.landmark(2), min_vertex_dist) &&
@@ -321,7 +321,7 @@ bool SimPolygon2D::insideBox(double s, const Point2& p) {
 bool SimPolygon2D::nearExisting(const std::vector<Point2>& S,
     const Point2& p, double threshold) {
   BOOST_FOREACH(const Point2& Sp, S)
-    if (Sp.dist(p) < threshold)
+    if (Sp.distance(p) < threshold)
       return true;
   return false;
 }

gtsam_unstable/geometry/SimWall2D.cpp

@@ -139,7 +139,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
     Point2 cur_intersection;
     if (wall.intersects(traj,cur_intersection)) {
       collision = true;
-      if (cur_pose.t().dist(cur_intersection) < cur_pose.t().dist(intersection)) {
+      if (cur_pose.t().distance(cur_intersection) < cur_pose.t().distance(intersection)) {
         intersection = cur_intersection;
         closest_wall = wall;
       }
@@ -155,7 +155,7 @@ std::pair<Pose2, bool> moveWithBounce(const Pose2& cur_pose, double step_size,
     norm = norm / norm.norm();
 
     // Simple check to make sure norm is on side closest robot
-    if (cur_pose.t().dist(intersection + norm) > cur_pose.t().dist(intersection - norm))
+    if (cur_pose.t().distance(intersection + norm) > cur_pose.t().distance(intersection - norm))
       norm = norm.inverse();
 
     // using the reflection

gtsam_unstable/geometry/SimWall2D.h

@@ -43,7 +43,7 @@ namespace gtsam {
     SimWall2D scale(double s) const { return SimWall2D(s*a_, s*b_); }
 
     /** geometry */
-    double length() const { return a_.dist(b_); }
+    double length() const { return a_.distance(b_); }
     Point2 midpoint() const;
 
     /**