-
Notifications
You must be signed in to change notification settings - Fork 62
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Louise Poubel <louise@openrobotics.org>
- Loading branch information
Showing
13 changed files
with
1,736 additions
and
5 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,83 @@ | ||
| /* | ||
| * Copyright (C) 2021 Open Source Robotics Foundation | ||
| * | ||
| * Licensed under the Apache License, Version 2.0 (the "License"); | ||
| * you may not use this file except in compliance with the License. | ||
| * You may obtain a copy of the License at | ||
| * | ||
| * http://www.apache.org/licenses/LICENSE-2.0 | ||
| * | ||
| * Unless required by applicable law or agreed to in writing, software | ||
| * distributed under the License is distributed on an "AS IS" BASIS, | ||
| * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| * See the License for the specific language governing permissions and | ||
| * limitations under the License. | ||
| * | ||
| */ | ||
|
|
||
| %module line2 | ||
| %{ | ||
| #include <ignition/math/Line2.hh> | ||
| #include <ignition/math/Helpers.hh> | ||
| #include <ignition/math/Vector2.hh> | ||
| %} | ||
|
|
||
| %include "std_string.i" | ||
|
|
||
| namespace ignition | ||
| { | ||
| namespace math | ||
| { | ||
| template<typename T> | ||
| class Line2 | ||
| { | ||
| %rename("%(undercase)s", %$isfunction, %$ismember, %$not %$isconstructor) ""; | ||
| public: Line2(const math::Vector2<T> &_ptA, const math::Vector2<T> &_ptB); | ||
| public: Line2(double _x1, double _y1, double _x2, double _y2); | ||
| public: void Set(const math::Vector2<T> &_ptA, | ||
| const math::Vector2<T> &_ptB); | ||
| public: void Set(double _x1, double _y1, double _x2, double _y2); | ||
| public: double CrossProduct(const Line2<T> &_line) const; | ||
| public: double CrossProduct(const Vector2<T> &_pt) const; | ||
| public: bool Collinear(const math::Vector2<T> &_pt, | ||
| double _epsilon = 1e-6) const; | ||
| public: bool Parallel(const math::Line2<T> &_line, | ||
| double _epsilon = 1e-6) const; | ||
| public: bool Collinear(const math::Line2<T> &_line, | ||
| double _epsilon = 1e-6) const; | ||
| public: bool OnSegment(const math::Vector2<T> &_pt, | ||
| double _epsilon = 1e-6) const; | ||
| public: bool Within(const math::Vector2<T> &_pt, | ||
| double _epsilon = 1e-6) const; | ||
| public: bool Intersect(const Line2<T> &_line, | ||
| double _epsilon = 1e-6) const; | ||
| public: bool Intersect(const Line2<T> &_line, math::Vector2<T> &_pt, | ||
| double _epsilon = 1e-6) const; | ||
| public: T Length() const; | ||
| public: double Slope() const; | ||
| public: bool operator==(const Line2<T> &_line) const; | ||
| public: bool operator!=(const Line2<T> &_line) const; | ||
| }; | ||
|
|
||
| %extend Line2 | ||
| { | ||
| ignition::math::Vector2<T> __getitem__(unsigned int i) const | ||
| { | ||
| return (*$self)[i]; | ||
| } | ||
| } | ||
|
|
||
| %extend Line2 | ||
| { | ||
| std::string __str__() const { | ||
| std::ostringstream out; | ||
| out << *$self; | ||
| return out.str(); | ||
| } | ||
| } | ||
|
|
||
| %template(Line2i) Line2<int>; | ||
| %template(Line2d) Line2<double>; | ||
| %template(Line2f) Line2<float>; | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,215 @@ | ||
| # Copyright (C) 2021 Open Source Robotics Foundation | ||
| # | ||
| # Licensed under the Apache License, Version 2.0 (the "License") | ||
| # you may not use this file except in compliance with the License. | ||
| # You may obtain a copy of the License at | ||
| # | ||
| # http://www.apache.org/licenses/LICENSE-2.0 | ||
| # | ||
| # Unless required by applicable law or agreed to in writing, software | ||
| # distributed under the License is distributed on an "AS IS" BASIS, | ||
| # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| # See the License for the specific language governing permissions and | ||
| # limitations under the License. | ||
|
|
||
| import math | ||
| import unittest | ||
| from ignition.math import Line2d | ||
| from ignition.math import Vector2d | ||
|
|
||
|
|
||
| class TestLine2d(unittest.TestCase): | ||
|
|
||
| def test_construction(self): | ||
| line_a = Line2d(0, 0, 10, 10) | ||
| self.assertAlmostEqual(line_a[0].x(), 0.0) | ||
| self.assertAlmostEqual(line_a[0].y(), 0.0) | ||
| self.assertAlmostEqual(line_a[1].x(), 10.0) | ||
| self.assertAlmostEqual(line_a[1].y(), 10.0) | ||
|
|
||
| line_b = Line2d(Vector2d(1, 2), Vector2d(3, 4)) | ||
| self.assertAlmostEqual(line_b[0].x(), 1.0) | ||
| self.assertAlmostEqual(line_b[0].y(), 2.0) | ||
| self.assertAlmostEqual(line_b[1].x(), 3.0) | ||
| self.assertAlmostEqual(line_b[1].y(), 4.0) | ||
|
|
||
| self.assertAlmostEqual(line_b[2].x(), line_b[1].x()) | ||
|
|
||
| def test_length(self): | ||
| line_a = Line2d(0, 0, 10, 10) | ||
| self.assertAlmostEqual(line_a.length(), math.sqrt(200), delta=1e-10) | ||
|
|
||
| def test_slope(self): | ||
| line = Line2d(0, 0, 10, 10) | ||
| self.assertAlmostEqual(line.slope(), 1.0, delta=1e-10) | ||
|
|
||
| line = Line2d(0, 0, 0, 10) | ||
| self.assertTrue(math.isnan(line.slope())) | ||
|
|
||
| line = Line2d(-10, 0, 100, 0) | ||
| self.assertAlmostEqual(line.slope(), 0.0) | ||
|
|
||
| def test_parallel_line(self): | ||
| # Line is always parallel with itself | ||
| line = Line2d(0, 0, 10, 0) | ||
| self.assertTrue(line.parallel(line, 1e-10)) | ||
|
|
||
| # Degenerate line segment | ||
| # Still expect Line is parallel with itself | ||
| line = Line2d(0, 0, 0, 0) | ||
| self.assertTrue(line.parallel(line, 1e-10)) | ||
|
|
||
| line_a = Line2d(0, 0, 10, 0) | ||
| line_b = Line2d(0, 0, 10, 0) | ||
| self.assertTrue(line_a.parallel(line_b, 1e-10)) | ||
|
|
||
| line_b.set(0, 0, 0, 10) | ||
| self.assertFalse(line_a.parallel(line_b)) | ||
|
|
||
| line_b.set(0, 10, 10, 10) | ||
| self.assertTrue(line_a.parallel(line_b)) | ||
|
|
||
| line_b.set(0, 10, 10, 10.00001) | ||
| self.assertFalse(line_a.parallel(line_b, 1e-10)) | ||
| self.assertFalse(line_a.parallel(line_b)) | ||
| self.assertTrue(line_a.parallel(line_b, 1e-3)) | ||
|
|
||
| def test_collinear_line(self): | ||
| # Line is always collinear with itself | ||
| line = Line2d(0, 0, 10, 0) | ||
| self.assertTrue(line.collinear(line, 1e-10)) | ||
|
|
||
| line_a = Line2d(0, 0, 10, 0) | ||
| line_b = Line2d(0, 0, 10, 0) | ||
| self.assertTrue(line_a.collinear(line_b, 1e-10)) | ||
|
|
||
| line_b.set(0, 10, 10, 10) | ||
| self.assertFalse(line_a.collinear(line_b)) | ||
|
|
||
| line_b.set(9, 0, 10, 0.00001) | ||
| self.assertFalse(line_a.collinear(line_b, 1e-10)) | ||
| self.assertFalse(line_a.collinear(line_b)) | ||
| self.assertTrue(line_a.collinear(line_b, 1e-3)) | ||
|
|
||
| def test_collinear_point(self): | ||
| line_a = Line2d(0, 0, 10, 0) | ||
| pt = Vector2d(0, 0) | ||
| self.assertTrue(line_a.collinear(pt)) | ||
|
|
||
| pt_line = Line2d(pt, pt) | ||
| self.assertTrue(line_a.collinear(pt_line)) | ||
|
|
||
| pt.set(1000, 0) | ||
| self.assertTrue(line_a.collinear(pt, 1e-10)) | ||
|
|
||
| pt_line = Line2d(pt, pt) | ||
| self.assertTrue(line_a.parallel(pt_line)) | ||
| self.assertFalse(line_a.intersect(pt_line)) | ||
| self.assertFalse(line_a.collinear(pt_line, 1e-10)) | ||
|
|
||
| pt.set(10, 0) | ||
| pt_line.set(pt, pt) | ||
| self.assertTrue(line_a.collinear(pt_line, 1e-10)) | ||
|
|
||
| pt.set(0, 0.00001) | ||
| self.assertFalse(line_a.collinear(pt)) | ||
| self.assertTrue(line_a.collinear(pt, 1e-3)) | ||
|
|
||
| pt_line = Line2d(pt, pt) | ||
| self.assertFalse(line_a.collinear(pt_line)) | ||
| self.assertTrue(line_a.parallel(pt_line)) | ||
| self.assertFalse(line_a.intersect(pt_line)) | ||
| self.assertTrue(line_a.intersect(pt_line, 1e-2)) | ||
| self.assertTrue(line_a.collinear(pt_line, 1e-3)) | ||
|
|
||
| pt.set(0, -0.00001) | ||
| self.assertFalse(line_a.collinear(pt)) | ||
| self.assertTrue(line_a.collinear(pt, 1e-3)) | ||
|
|
||
| pt_line = Line2d(pt, pt) | ||
| self.assertFalse(line_a.collinear(pt_line)) | ||
| self.assertTrue(line_a.collinear(pt_line, 1e-4)) | ||
|
|
||
| def test_intersect(self): | ||
| pt = Vector2d() | ||
|
|
||
| # parallel horizontal lines | ||
| line_a = Line2d(1, 1, 2, 1) | ||
| line_b = Line2d(1, 2, 2, 2) | ||
| self.assertFalse(line_a.intersect(line_b, pt)) | ||
|
|
||
| # parallel vertical lines | ||
| line_a.set(1, 1, 1, 10) | ||
| line_b.set(2, 1, 2, 10) | ||
| self.assertFalse(line_a.intersect(line_b, pt)) | ||
|
|
||
| # Two lines that form an inverted T with a gap | ||
| line_a.set(1, 1, 1, 10) | ||
| line_b.set(0, 0, 2, 0) | ||
| self.assertFalse(line_a.intersect(line_b, pt)) | ||
|
|
||
| # Two lines that form a T with a gap | ||
| line_a.set(1, 1, 1, 10) | ||
| line_b.set(0, 10.1, 2, 10.1) | ||
| self.assertFalse(line_a.intersect(line_b, pt)) | ||
|
|
||
| # Two lines that form an inverted T with a gap | ||
| line_a.set(0, -10, 0, 10) | ||
| line_b.set(1, 0, 10, 0) | ||
| self.assertFalse(line_a.intersect(line_b, pt)) | ||
|
|
||
| # Two lines that form a T with a gap | ||
| line_a.set(0, -10, 0, 10) | ||
| line_b.set(-1, 0, -10, 0) | ||
| self.assertFalse(line_a.intersect(line_b, pt)) | ||
|
|
||
| # Two collinear lines, one starts where the other stopped | ||
| line_a.set(1, 1, 1, 10) | ||
| line_b.set(1, 10, 1, 11) | ||
| self.assertTrue(line_a.intersect(line_b, pt)) | ||
| self.assertEqual(pt, Vector2d(1, 10)) | ||
|
|
||
| # Two collinear lines, one overlaps the other | ||
| line_a.set(0, 0, 0, 10) | ||
| line_b.set(0, 9, 0, 11) | ||
| self.assertTrue(line_a.intersect(line_b, pt)) | ||
| self.assertEqual(pt, Vector2d(0, 9)) | ||
|
|
||
| # Two collinear lines, one overlaps the other | ||
| line_a.set(0, 0, 0, 10) | ||
| line_b.set(0, -10, 0, 1) | ||
| self.assertTrue(line_a.intersect(line_b, pt)) | ||
| self.assertEqual(pt, Vector2d(0, 1)) | ||
|
|
||
| # Two intersecting lines | ||
| line_a.set(0, 0, 10, 10) | ||
| line_b.set(0, 10, 10, 0) | ||
| self.assertTrue(line_a.intersect(line_b, pt)) | ||
| self.assertEqual(pt, Vector2d(5, 5)) | ||
|
|
||
| def test_equality(self): | ||
| line_a = Line2d(1, 1, 2, 1) | ||
| line_b = Line2d(1, 2, 2, 2) | ||
|
|
||
| self.assertTrue(line_a != line_b) | ||
| self.assertTrue(line_a == line_a) | ||
|
|
||
| line_b.set(1, 1, 2, 1.1) | ||
| self.assertFalse(line_a == line_b) | ||
|
|
||
| line_b.set(1, 1, 2.1, 1) | ||
| self.assertFalse(line_a == line_b) | ||
|
|
||
| line_b.set(1, 1.1, 2, 1) | ||
| self.assertFalse(line_a == line_b) | ||
|
|
||
| line_b.set(1.1, 1, 2, 1) | ||
| self.assertFalse(line_a == line_b) | ||
|
|
||
| def test_serialization(self): | ||
| line = Line2d(0, 1, 2, 3) | ||
| self.assertEqual(str(line), "0 1 2 3") | ||
|
|
||
|
|
||
| if __name__ == '__main__': | ||
| unittest.main() |
Oops, something went wrong.