From 5ca7ab9053c9533de32d6acc884d4114b1809064 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 7 Sep 2009 03:35:32 +0000 Subject: [PATCH] equals --- cpp/Point2.cpp | 13 ++++++++++++- cpp/Point2.h | 9 +++++---- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/cpp/Point2.cpp b/cpp/Point2.cpp index fe6b29692..2734cdfbb 100644 --- a/cpp/Point2.cpp +++ b/cpp/Point2.cpp @@ -15,9 +15,20 @@ namespace gtsam { cout << s << "(" << x_ << ", " << y_ << ")" << endl; } + /* ************************************************************************* */ + double Point2::dist(const Point2& p2) const { + return sqrt(pow(x() - p2.x(), 2.0) + pow(y() - p2.y(), 2.0)); + } + + /* ************************************************************************* */ + bool Point2::equals(const Point2& q, double tol) const { + return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); + } + /* ************************************************************************* */ bool assert_equal(const Point2& p, const Point2& q, double tol) { - if (fabs(p.x() - q.x()) < tol && fabs(p.y() - q.y()) < tol) return true; + if (p.equals(q, tol)) + return true; printf("not equal:\n"); p.print("p = "); q.print("q = "); diff --git a/cpp/Point2.h b/cpp/Point2.h index e29538c7f..b0ba92bd1 100644 --- a/cpp/Point2.h +++ b/cpp/Point2.h @@ -6,7 +6,7 @@ #pragma once -#include "Matrix.h" +#include "Vector.h" namespace gtsam { @@ -53,9 +53,10 @@ namespace gtsam { void print(const std::string& s = "") const; /** distance between two points */ - double dist(const Point2& p2) const { - return sqrt(pow(x()-p2.x(),2.0) + pow(y()-p2.y(),2.0)); - } + double dist(const Point2& p2) const; + + /** equals with an tolerance, prints out message if unequal*/ + bool equals(const Point2& q, double tol = 1e-9) const; }; /** equals with an tolerance, prints out message if unequal*/