From b7c1097f5811ceb16455bd90d16bd71c8c62e25a Mon Sep 17 00:00:00 2001
From: zhaoyang <lvzhaoyang1990@gmail.com>
Date: Wed, 24 Jun 2015 16:29:38 -0400
Subject: [PATCH] feature: add inline get methods

---
 gtsam/geometry/OrientedPlane3.cpp           | 10 ----------
 gtsam/geometry/OrientedPlane3.h             | 11 ++++++++++-
 gtsam/geometry/tests/testOrientedPlane3.cpp |  8 ++++++--
 3 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/gtsam/geometry/OrientedPlane3.cpp b/gtsam/geometry/OrientedPlane3.cpp
index b6594c29c..dff4eac9e 100644
--- a/gtsam/geometry/OrientedPlane3.cpp
+++ b/gtsam/geometry/OrientedPlane3.cpp
@@ -90,14 +90,4 @@ Vector3 OrientedPlane3::localCoordinates(const OrientedPlane3& y) const {
   return e;
 }
 
-/* ************************************************************************* */
-Vector4 OrientedPlane3::planeCoefficients() const {
-  Vector unit_vec = n_.unitVector();
-  Vector4 a;
-  a << unit_vec[0], unit_vec[1], unit_vec[2], d_;
-  return a;
-}
-
-/* ************************************************************************* */
-
 }
diff --git a/gtsam/geometry/OrientedPlane3.h b/gtsam/geometry/OrientedPlane3.h
index e75e32c96..8ff29b88a 100644
--- a/gtsam/geometry/OrientedPlane3.h
+++ b/gtsam/geometry/OrientedPlane3.h
@@ -108,12 +108,21 @@ public:
   Vector3 localCoordinates(const OrientedPlane3& s) const;
 
   /// Returns the plane coefficients
-  Vector4 planeCoefficients() const;
+  inline Vector4 planeCoefficients() const {
+    Vector3 unit_vec = n_.unitVector();
+    return Vector4(unit_vec[0], unit_vec[1], unit_vec[2], d_);
+  }
 
+  /// Return the normal
   inline Unit3 normal() const {
     return n_;
   }
 
+  /// Return the perpendicular distance to the origin
+  inline double distance() const {
+    return d_;
+  }
+
   /// @}
 };
 
diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp
index 8a0c2f846..c6189b970 100644
--- a/gtsam/geometry/tests/testOrientedPlane3.cpp
+++ b/gtsam/geometry/tests/testOrientedPlane3.cpp
@@ -35,10 +35,14 @@ TEST (OrientedPlane3, get) {
   c << -1, 0, 0, 5;
   OrientedPlane3 plane1(c);
   OrientedPlane3 plane2(c[0], c[1], c[2], c[3]);
-  Vector coefficient1 = plane1.planeCoefficients();
+  Vector4 coefficient1 = plane1.planeCoefficients();
+  double distance1 = plane1.distance();
   EXPECT(assert_equal(coefficient1, c, 1e-8));
-  Vector coefficient2 = plane2.planeCoefficients();
+  EXPECT_DOUBLES_EQUAL(distance1, 5, 1e-8);
+  Vector4 coefficient2 = plane2.planeCoefficients();
+  double distance2 = plane2.distance();
   EXPECT(assert_equal(coefficient2, c, 1e-8));
+  EXPECT_DOUBLES_EQUAL(distance2, 5, 1e-8);
 }
 
 //*******************************************************************************