From d0507535cdc1e33d4d85123d0f104a1ff18df685 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Fri, 2 Sep 2011 01:50:12 +0000
Subject: [PATCH] Added unit() to Rot2 as syntactic sugar

---
 gtsam/geometry/Rot2.h             | 5 +++++
 gtsam/geometry/tests/testRot2.cpp | 7 +++++++
 2 files changed, 12 insertions(+)

diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 8ec39a6ba..ff159364f 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -171,6 +171,11 @@ namespace gtsam {
     Point2 unrotate(const Point2& p, boost::optional<Matrix&> H1 =
   			boost::none, boost::optional<Matrix&> H2 = boost::none) const;
 
+    /**
+     * Creates a unit vector as a Point2
+     */
+    inline Point2 unit() const { return Point2(c_, s_); }
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;
diff --git a/gtsam/geometry/tests/testRot2.cpp b/gtsam/geometry/tests/testRot2.cpp
index 6ecccac2c..3edc53e3a 100644
--- a/gtsam/geometry/tests/testRot2.cpp
+++ b/gtsam/geometry/tests/testRot2.cpp
@@ -33,6 +33,13 @@ TEST( Rot2, constructors_and_angle)
 	CHECK(assert_equal(R,Rot2::atan2(s*5,c*5)));
 }
 
+/* ************************************************************************* */
+TEST( Rot2, unit)
+{
+	EXPECT(assert_equal(Point2(1.0, 0.0), Rot2::fromAngle(0).unit()));
+	EXPECT(assert_equal(Point2(0.0, 1.0), Rot2::fromAngle(M_PI_2).unit()));
+}
+
 /* ************************************************************************* */
 TEST( Rot2, transpose)
 {