Skip to content

Commit

Permalink
Adding Line3 class with its test
Browse files Browse the repository at this point in the history 
Signed-off-by: Marcos Wagner <[email protected]>
  • Loading branch information
@WagnerMarcos @francocipollone
WagnerMarcos authored and francocipollone committed Aug 18, 2021
1 parent e8bbddb commit 8456acf
Show file tree
Hide file tree
Showing 4 changed files with 342 additions and 0 deletions.
1 change: 1 addition & 0 deletions src/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,7 @@ if (PYTHONLIBS_FOUND)
Angle_TEST
GaussMarkovProcess_TEST
Line2_TEST
Line3_TEST
python_TEST
Rand_TEST
SignalStats_TEST
Expand Down
91 changes: 91 additions & 0 deletions src/Line3.i
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,91 @@
/*
* 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 line3
%{
#include <ignition/math/Line3.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Helpers.hh>
%}

%include "std_string.i"

namespace ignition
{
namespace math
{
template<typename T>
class Line3
{
public: Line3() = default;
public: Line3(const Line3<T> &_line);
public: Line3(const math::Vector3<T> &_ptA, const math::Vector3<T> &_ptB);
public: Line3(const double _x1, const double _y1,
const double _x2, const double _y2);
public: Line3(const double _x1, const double _y1,
const double _z1, const double _x2,
const double _y2, const double _z2);
public: void Set(const math::Vector3<T> &_ptA,
const math::Vector3<T> &_ptB);
public: void SetA(const math::Vector3<T> &_ptA);
public: void SetB(const math::Vector3<T> &_ptB);
public: void Set(const double _x1, const double _y1,
const double _x2, const double _y2,
const double _z = 0);
public: void Set(const double _x1, const double _y1,
const double _z1, const double _x2,
const double _y2, const double _z2);
public: math::Vector3<T> Direction() const;
public: T Length() const;
public: bool Distance(const Line3<T> &_line, Line3<T> &_result,
const double _epsilon = 1e-6) const;
public: bool Intersect(const Line3<T> &_line,
double _epsilon = 1e-6) const;
public: bool Coplanar(const Line3<T> &_line,
const double _epsilon = 1e-6) const;
public: bool Parallel(const Line3<T> &_line,
const double _epsilon = 1e-6) const;
public: bool Intersect(const Line3<T> &_line, math::Vector3<T> &_pt,
double _epsilon = 1e-6) const;
public: bool Within(const math::Vector3<T> &_pt,
double _epsilon = 1e-6) const;
public: bool operator==(const Line3<T> &_line) const;
public: bool operator!=(const Line3<T> &_line) const;
};

%extend Line3
{
ignition::math::Vector3<T> __getitem__(const unsigned int i) const
{
return (*$self)[i];
}
}

%extend Line3
{
std::string __str__() const {
std::ostringstream out;
out << *$self;
return out.str();
}
}

%template(Line3i) Line3<int>;
%template(Line3d) Line3<double>;
%template(Line3f) Line3<float>;
}
}
249 changes: 249 additions & 0 deletions src/Line3_TEST.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,249 @@
# 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 unittest
import math
from ignition.math import Line3d
from ignition.math import Vector3d


class TestLine3d(unittest.TestCase):

def test_construction(self):
lineA = Line3d(0, 0, 10, 10)
self.assertAlmostEqual(lineA[0].X(), 0.0)
self.assertAlmostEqual(lineA[0].Y(), 0.0)
self.assertAlmostEqual(lineA[0].Z(), 0.0)
self.assertAlmostEqual(lineA[1].X(), 10.0)
self.assertAlmostEqual(lineA[1].Y(), 10.0)
self.assertAlmostEqual(lineA[1].Z(), 0.0)
lineB = Line3d(Vector3d(1, 2, 3), Vector3d(4, 5, 6))
self.assertAlmostEqual(lineB[0].X(), 1.0)
self.assertAlmostEqual(lineB[0].Y(), 2.0)
self.assertAlmostEqual(lineB[0].Z(), 3.0)
self.assertAlmostEqual(lineB[1].X(), 4.0)
self.assertAlmostEqual(lineB[1].Y(), 5.0)
self.assertAlmostEqual(lineB[1].Z(), 6.0)

lineC = Line3d(0, 0, 5, 10, 10, 6)
self.assertAlmostEqual(lineC[0].X(), 0.0)
self.assertAlmostEqual(lineC[0].Y(), 0.0)
self.assertAlmostEqual(lineC[0].Z(), 5.0)
self.assertAlmostEqual(lineC[1].X(), 10.0)
self.assertAlmostEqual(lineC[1].Y(), 10.0)
self.assertAlmostEqual(lineC[1].Z(), 6.0)

self.assertAlmostEqual(lineB[2].X(), lineB[1].X())

def test_set(self):
lineA = Line3d()
lineA.Set(1, 1, 2, 2)
self.assertAlmostEqual(lineA[0].X(), 1.0)
self.assertAlmostEqual(lineA[0].Y(), 1.0)
self.assertAlmostEqual(lineA[0].Z(), 0.0)
self.assertAlmostEqual(lineA[1].X(), 2.0)
self.assertAlmostEqual(lineA[1].Y(), 2.0)
self.assertAlmostEqual(lineA[1].Z(), 0.0)

lineA.Set(10, 11, 12, 13, 14, 15)
self.assertAlmostEqual(lineA[0].X(), 10.0)
self.assertAlmostEqual(lineA[0].Y(), 11.0)
self.assertAlmostEqual(lineA[0].Z(), 12.0)
self.assertAlmostEqual(lineA[1].X(), 13.0)
self.assertAlmostEqual(lineA[1].Y(), 14.0)
self.assertAlmostEqual(lineA[1].Z(), 15.0)

lineA.SetA(Vector3d(0, -1, -2))
self.assertAlmostEqual(lineA[0].X(), 0.0)
self.assertAlmostEqual(lineA[0].Y(), -1.0)
self.assertAlmostEqual(lineA[0].Z(), -2.0)
self.assertAlmostEqual(lineA[1].X(), 13.0)
self.assertAlmostEqual(lineA[1].Y(), 14.0)
self.assertAlmostEqual(lineA[1].Z(), 15.0)

lineA.SetB(Vector3d(5, 6, 7))
self.assertAlmostEqual(lineA[0].X(), 0.0)
self.assertAlmostEqual(lineA[0].Y(), -1.0)
self.assertAlmostEqual(lineA[0].Z(), -2.0)
self.assertAlmostEqual(lineA[1].X(), 5.0)
self.assertAlmostEqual(lineA[1].Y(), 6.0)
self.assertAlmostEqual(lineA[1].Z(), 7.0)

def test_length(self):
lineA = Line3d(0, 0, 0, 10, 10, 10)
self.assertAlmostEqual(lineA.Length(), math.sqrt(300), delta=1e-10)

def test_equality(self):
lineA = Line3d(1, 1, 1, 2, 1, 2)
lineB = Line3d(1, 2, 3, 2, 2, 4)

self.assertTrue(lineA != lineB)
self.assertTrue(lineA == lineA)

lineB.Set(1, 1, 1, 2, 1.1, 2)
self.assertFalse(lineA == lineB)

lineB.Set(1, 1, 1, 2.1, 1, 2)
self.assertFalse(lineA == lineB)

lineB.Set(1, 1, 1.1, 2, 1, 2)
self.assertFalse(lineA == lineB)

lineB.Set(1.1, 1, 1, 2, 1, 2)
self.assertFalse(lineA == lineB)

def test_serialization(self):
line = Line3d(0, 1, 4, 2, 3, 7)
self.assertEqual(str(line), "0 1 4 2 3 7")

def test_copy_constructor(self):
lineA = Line3d(0, 1, 4, 2, 3, 7)
lineB = Line3d(lineA)

self.assertEqual(lineA, lineB)

def test_direction(self):
lineA = Line3d(1, 1, 1, 0, 0, 0)
lineB = Line3d(2, 2, 2, 0, 0, 0)
lineC = Line3d(0, 0, 0, 1, 1, 1)
self.assertTrue(lineA.Direction() == (lineA[1] - lineA[0]).Normalize())
self.assertTrue(lineA.Direction() == lineB.Direction())
self.assertFalse(lineA.Direction() == lineC.Direction())

lineA.Set(1, 1, 2, 1, 1, 10)
self.assertTrue(lineA.Direction() == Vector3d.UnitZ)

lineA.Set(1, 5, 1, 1, 1, 1)
self.assertTrue(lineA.Direction() == -Vector3d.UnitY)

lineA.Set(1, 1, 1, 7, 1, 1)
self.assertTrue(lineA.Direction() == Vector3d.UnitX)

def test_within(self):
line = Line3d(0, 0, 0, 1, 1, 1)
self.assertTrue(line.Within(Vector3d(0, 0, 0)))
self.assertTrue(line.Within(Vector3d(1, 1, 1)))
self.assertTrue(line.Within(Vector3d(0.5, 0.5, 0.5)))

self.assertFalse(line.Within(Vector3d(-0.5, 0.5, 0.5)))
self.assertFalse(line.Within(Vector3d(0.5, -0.5, 0.5)))
self.assertFalse(line.Within(Vector3d(0.5, 0.5, -0.5)))

def test_distance(self):
line = Line3d(0, 0, 0, 0, 1, 0)
result = Line3d()

self.assertTrue(line.Distance(Line3d(1, 0.5, 0, -1, 0.5, 0), result))
self.assertAlmostEqual(result.Length(), 0)
self.assertEqual(result, Line3d(0, 0.5, 0, 0, 0.5, 0))

self.assertTrue(line.Distance(Line3d(1, 0, 0, -1, 0, 0), result))
self.assertAlmostEqual(result.Length(), 0)
self.assertEqual(result, Line3d(0, 0, 0, 0, 0, 0))

self.assertTrue(line.Distance(Line3d(1, 1.1, 0, -1, 1.1, 0), result))
self.assertAlmostEqual(result.Length(), 0.1, delta=1e-4)
self.assertEqual(result, Line3d(0, 1, 0, 0, 1.1, 0))

self.assertTrue(line.Distance(Line3d(1, 0.5, 0.4, -1, 0.5, 0.4),
result))
self.assertAlmostEqual(result.Length(), 0.4, delta=1e-4)
self.assertEqual(result, Line3d(0, 0.5, 0, 0, 0.5, 0.4))

self.assertTrue(line.Distance(Line3d(0, 0.5, 1, 1, 0.5, 0),
result))
self.assertAlmostEqual(result.Length(), math.sin(math.pi / 4),
delta=1e-4)
self.assertEqual(result, Line3d(0, 0.5, 0, 0.5, 0.5, 0.5))

# Expect true when lines are parallel
self.assertTrue(line.Distance(Line3d(2, 0, 0, 2, 1, 0), result))
self.assertEqual(result[0], line[0])
self.assertEqual(result[1], Vector3d(2, 0, 0))

self.assertTrue(line.Distance(Line3d(2, 1, 0, 2, 0, 0), result))
self.assertEqual(result[0], line[0])
self.assertEqual(result[1], Vector3d(2, 0, 0))

self.assertTrue(line.Distance(Line3d(1, 1, 0, 1, 2, 0), result))
self.assertEqual(result[0], line[1])
self.assertEqual(result[1], Vector3d(1, 1, 0))

self.assertTrue(line.Distance(Line3d(1, 2, 0, 1, 1, 0), result))
self.assertEqual(result[0], line[1])
self.assertEqual(result[1], Vector3d(1, 1, 0))

# Expect false when the passed in line is a point
self.assertFalse(line.Distance(Line3d(2, 0, 0, 2, 0, 0), result))

# Expect false when the first line is a point.
line.Set(0, 0, 0, 0, 0, 0)
self.assertFalse(line.Distance(Line3d(2, 0, 0, 2, 1, 0), result))

def test_interesct(self):
line = Line3d(0, 0, 0, 0, 1, 0)
pt = Vector3d()

self.assertTrue(line.Intersect(Line3d(1, 0.5, 0, -1, 0.5, 0)))
self.assertTrue(line.Intersect(Line3d(1, 0.5, 0, -1, 0.5, 0), pt))
self.assertEqual(pt, Vector3d(0, 0.5, 0))

self.assertTrue(line.Intersect(Line3d(1, 0, 0, -1, 0, 0)))
self.assertTrue(line.Intersect(Line3d(1, 0, 0, -1, 0, 0), pt))
self.assertEqual(pt, Vector3d(0, 0, 0))

self.assertTrue(line.Intersect(Line3d(1, 1, 0, -1, 1, 0)))
self.assertTrue(line.Intersect(Line3d(1, 1, 0, -1, 1, 0), pt))
self.assertEqual(pt, Vector3d(0, 1, 0))

self.assertTrue(line.Intersect(Line3d(0, 0.5, -1, 0, 0.5, 1)))
self.assertTrue(line.Intersect(Line3d(0, 0.5, -1, 0, 0.5, 1), pt))
self.assertEqual(pt, Vector3d(0, 0.5, 0))

self.assertTrue(line.Intersect(Line3d(-1, 0.5, -1, 1, 0.5, 1)))
self.assertTrue(line.Intersect(Line3d(-1, 0.5, -1, 1, 0.5, 1), pt))
self.assertEqual(pt, Vector3d(0, 0.5, 0))

self.assertFalse(line.Intersect(Line3d(1, 1.1, 0, -1, 1.1, 0)))
self.assertFalse(line.Intersect(Line3d(1, -0.1, 0, -1, -0.1, 0)))

self.assertFalse(line.Intersect(Line3d(0.1, 0.1, 0, 0.6, 0.6, 0)))
self.assertFalse(line.Intersect(Line3d(-0.1, 0, 0, -0.1, 1, 0)))

self.assertTrue(line.Intersect(Line3d(0, -1, 0, 0, 0.1, 0)))
self.assertTrue(line.Intersect(Line3d(0, 1, 0, 0, 1.1, 0)))

def test_parallel(self):
line = Line3d(0, 0, 0, 0, 1, 0)
self.assertTrue(line.Parallel(Line3d(1, 0, 0, 1, 1, 0)))
self.assertTrue(line.Parallel(Line3d(1, 1, 0, 1, 0, 0)))
self.assertTrue(line.Parallel(Line3d(0, 0, 0, 0, 10, 0)))
self.assertTrue(line.Parallel(Line3d(-100, 100, 20, -100, 200, 20)))

self.assertFalse(line.Parallel(Line3d(1, 0, 0, 1, 1, 1)))
self.assertFalse(line.Parallel(Line3d(1, 0, 0, 2, 0, 0)))
self.assertFalse(line.Parallel(Line3d(1, 0, 1, 2, 0, 1)))

def test_coplanar(self):
line = Line3d(0, 0, 0, 0, 1, 0)
self.assertTrue(line.Coplanar(Line3d(1, 0, 0, 1, 1, 0)))
self.assertTrue(line.Coplanar(Line3d(0, 0, 0, 0, 10, 0)))
self.assertTrue(line.Coplanar(Line3d(-100, 100, 20, -100, 200, 20)))

self.assertFalse(line.Coplanar(Line3d(1, 0, 0, 1, 1, 1)))
self.assertFalse(line.Coplanar(Line3d(1, 0, 1, 2, 0, 0)))


if __name__ == '__main__':
unittest.main()
1 change: 1 addition & 0 deletions src/python/python.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,4 +7,5 @@
%include ../Vector3.i
%include ../Vector4.i
%include ../Line2.i
%include ../Line3.i
%include ../SignalStats.i

0 comments on commit 8456acf

Please sign in to comment.