RoboidControl/RoboidControl-python
RoboidControl/RoboidControl-python
1
0
Fork 0
Code Issues Pull Requests Actions Packages Releases Activity

Merge commit '8949a87956a0227ad709cb4b7c6d153642b337b8'

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-31 12:22:40 +02:00
commit a56533ffbc
14 changed files with 66 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
LinearAlgebra/LinearAlgebra/Angle.py
View File

@ -1,17 +1,9 @@
# This Source Code Form is subject to the terms of the Mozilla Public
# License, v. 2.0.If a copy of the MPL was not distributed with this
# file, You can obtain one at https ://mozilla.org/MPL/2.0/.
import sys
import os
# Make the parent directory (root of the package) discoverable
package_directory = os.path.dirname(os.path.abspath(__file__))
sys.path.insert(0, package_directory)
import math
import importlib
#from Float import *
importlib.import_module("Float")
from .Float import *
# This is in fact AngleSingle
class Angle:
@ -44,10 +36,13 @@ class Angle:
"""! Tests whether this angle is equal to the given angle
@param angle The angle to compare to
@return True when the angles are equal, False otherwise
@note The equality is determine within the limits of precision of the raw
type T
@note This uses float comparison to check equality which may have strange
effects. Equality on floats should be avoided, use isclose instead
"""
return self.value == angle.value
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-9):
return math.isclose(self.value, other.value, rel_tol=rel_tol, abs_tol=abs_tol)
def __gt__(self, angle):
"""! Tests if this angle is greater than the given angle
@param angle The given angle

5
LinearAlgebra/LinearAlgebra/Direction.py
View File

@ -74,6 +74,11 @@ class Direction:
"""
return (self.horizontal == direction.horizontal and
self.vertical == direction.vertical)
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
return (
Angle.isclose(self.horizontal, other.horizontal, rel_tol=rel_tol, abs_tol=abs_tol) and
Angle.isclose(self.vertical, other.vertical, rel_tol=rel_tol, abs_tol=abs_tol)
)
def __neg__(self):
"""! Negate/reverse the direction

7
LinearAlgebra/LinearAlgebra/Quaternion.py
View File

@ -118,6 +118,13 @@ class Quaternion:
self.z == other.z and
self.w == other.w
)
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
return (
math.isclose(self.x, other.x, rel_tol=rel_tol, abs_tol=abs_tol) and
math.isclose(self.y, other.y, rel_tol=rel_tol, abs_tol=abs_tol) and
math.isclose(self.z, other.z, rel_tol=rel_tol, abs_tol=abs_tol) and
math.isclose(self.w, other.w, rel_tol=rel_tol, abs_tol=abs_tol)
)
def SqrMagnitude(self) -> float:
return self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w

14
LinearAlgebra/LinearAlgebra/Spherical.py
View File

@ -1,4 +1,5 @@
import math
from .Direction import *
from .Vector import *
@ -83,6 +84,12 @@ class Polar:
self.distance == other.distance and
self.direction == other.direction
)
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
return (
math.isclose(self.distance, other.distance, rel_tol=rel_tol, abs_tol=abs_tol) and
self.direction.isclose(other.direction, rel_tol, abs_tol)
)
def Magnitude(self) -> float:
return math.fabs(self.distance)
@ -311,7 +318,12 @@ class Spherical(Polar):
return (
self.distance == other.distance and
self.direction == other.direction
)
)
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
return (
math.isclose(self.distance, other.distance, rel_tol=rel_tol, abs_tol=abs_tol) and
self.direction.isclose(other.direction, rel_tol, abs_tol)
)
def Normalized(self) -> float:
if self.distance == 0:

8
LinearAlgebra/LinearAlgebra/SwingTwist.py
View File

@ -78,7 +78,13 @@ class SwingTwist:
return (
self.swing == other.swing and
self.twist == other.twist
)
)
def isclose(self, other, rel_tol=1e-9, abs_tol=1e-8):
return (
self.swing.isclose(other.swing, rel_tol, abs_tol) and
Angle.isclose(self.twist, other.twist, rel_tol=rel_tol, abs_tol=abs_tol)
)
@staticmethod
def Angle(r1, r2) -> Angle:

2
LinearAlgebra/LinearAlgebra/Vector.py
View File

@ -1,6 +1,6 @@
import math
from .Angle import *
from LinearAlgebra.Angle import *
epsilon = 1E-05

6
LinearAlgebra/test/Angle_test.py
View File

@ -1,10 +1,6 @@
import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
from Angle import *
from LinearAlgebra.Angle import *
class AngleTest(unittest.TestCase):
def test_Construct(self):

6
LinearAlgebra/test/Direction_test.py
View File

@ -1,10 +1,6 @@
import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
from Direction import *
from LinearAlgebra.Direction import *
class DirectionTest(unittest.TestCase):
def test_Compare(self):

6
LinearAlgebra/test/Float_test.py
View File

@ -1,10 +1,6 @@
import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
from Float import *
from LinearAlgebra.Float import *
class FloatTest(unittest.TestCase):
def test_Clamp(self):

14
LinearAlgebra/test/Quaternion_test.py
View File

@ -1,10 +1,6 @@
import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
from Quaternion import *
from LinearAlgebra.Quaternion import *
class QuaternionTest(unittest.TestCase):
def test_Equality(self):
@ -22,7 +18,7 @@ class QuaternionTest(unittest.TestCase):
q = Quaternion.FromAngles(Angle.Degrees(90), Angle.Degrees(90), Angle.Degrees(-90))
sqrt2_2 = math.sqrt(2) / 2
assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
assert(Quaternion.isclose(q, Quaternion(0, sqrt2_2, -sqrt2_2, 0)))
def test_ToAngles(self):
q1 = Quaternion.identity
@ -39,7 +35,8 @@ class QuaternionTest(unittest.TestCase):
q = Quaternion.Degrees(90, 90, -90)
sqrt2_2 = math.sqrt(2) / 2
assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
assert(Quaternion.isclose(q, Quaternion(0, sqrt2_2, -sqrt2_2, 0)))
# assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
def test_Radians(self):
q = Quaternion.Radians(0, 0, 0)
@ -47,7 +44,8 @@ class QuaternionTest(unittest.TestCase):
q = Quaternion.Radians(math.pi / 2, math.pi / 2, -math.pi / 2)
sqrt2_2 = math.sqrt(2) / 2
assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
assert(Quaternion.isclose(q, Quaternion(0, sqrt2_2, -sqrt2_2, 0)))
# assert(q == Quaternion(0, sqrt2_2, -sqrt2_2, 0))
def test_Multiply(self):
q1 = Quaternion.identity

32
LinearAlgebra/test/Spherical_test.py
View File

@ -1,10 +1,6 @@
import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
from Spherical import *
from LinearAlgebra.Spherical import *
class PolarTest(unittest.TestCase):
def test_FromVector2(self):
@ -157,7 +153,7 @@ class PolarTest(unittest.TestCase):
v2 = Polar.Degrees(-1, -135)
r = Polar.Distance(v1, v2)
assert(r == 3)
assert(math.isclose(r, 3))
v2 = Polar.Degrees(0, 0)
r = Polar.Distance(v1, v2)
@ -207,10 +203,10 @@ class PolarTest(unittest.TestCase):
assert(r == v1)
r = Polar.Lerp(v1, v2, 1)
assert(r == v2)
assert(Polar.isclose(r, v2))
r = Polar.Lerp(v1, v2, 0.5)
assert(r == Polar.Degrees(3, 0))
assert(Polar.isclose(r, Polar.Degrees(3, 0)))
r = Polar.Lerp(v1, v2, -1)
assert(r == Polar.Degrees(9, 135))
@ -316,7 +312,7 @@ class SphericalTest(unittest.TestCase):
v2 = Spherical.Degrees(1, 45, 0)
r = v1 - v2
assert(r == Spherical.Degrees(3, 45, 0))
assert(Spherical.isclose(r, Spherical.Degrees(3, 45, 0)))
v2 = Spherical.Degrees(1, -135, 0)
r = v1 - v2
@ -336,15 +332,15 @@ class SphericalTest(unittest.TestCase):
v2 = Spherical(1, Direction.Degrees(-45, 0))
r = v1 + v2
assert(r.distance == math.sqrt(2))
assert(r.direction.horizontal.InDegrees() == 0)
assert(r.direction.vertical.InDegrees() == 0)
assert(math.isclose(r.distance, math.sqrt(2)))
assert(Angle.isclose(r.direction.horizontal, Angle.Degrees(0)))
assert(Angle.isclose(r.direction.vertical, Angle.Degrees(0)))
v2 = Spherical(1, Direction.Degrees(0, 90))
r = v1 + v2
assert(r.distance == math.sqrt(2))
assert(r.direction.horizontal.InDegrees() == 45)
assert(r.direction.vertical.InDegrees() == 45)
assert(math.isclose(r.distance, math.sqrt(2)))
assert(Angle.isclose(r.direction.horizontal, Angle.Degrees(45)))
assert(Angle.isclose(r.direction.vertical, Angle.Degrees(45)))
def test_Multiply(self):
r = Spherical.zero
@ -379,7 +375,7 @@ class SphericalTest(unittest.TestCase):
v2 = Spherical.Degrees(-1, -135, 0)
r = Spherical.Distance(v1, v2)
assert(r == 3)
assert(math.isclose(r, 3))
v2 = Spherical.Degrees(0, 0, 0)
r = Spherical.Distance(v1, v2)
@ -429,10 +425,10 @@ class SphericalTest(unittest.TestCase):
assert(r == v1)
r = Spherical.Lerp(v1, v2, 1)
assert(r == v2)
assert(Spherical.isclose(r, v2))
r = Spherical.Lerp(v1, v2, 0.5)
assert(r == Spherical.Degrees(3, 0, 0))
assert(Spherical.isclose(r, Spherical.Degrees(3, 0, 0)))
r = Spherical.Lerp(v1, v2, -1)
assert(r == Spherical.Degrees(9, 135, 0))

8
LinearAlgebra/test/SwingTwist_test.py
View File

@ -1,10 +1,6 @@
import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
from SwingTwist import *
from LinearAlgebra.SwingTwist import *
class SwingTwistTest(unittest.TestCase):
def test_Constructor(self):
@ -36,7 +32,7 @@ class SwingTwistTest(unittest.TestCase):
q = Quaternion.Degrees(90, 0, 0)
r = SwingTwist.FromQuaternion(q)
assert(r == SwingTwist.Degrees(90, 0, 0))
assert(SwingTwist.isclose(r, SwingTwist.Degrees(90, 0, 0)))
q = Quaternion.Degrees(0, 90, 0)
r = SwingTwist.FromQuaternion(q)

6
LinearAlgebra/test/Vector_test.py
View File

@ -1,10 +1,6 @@
import unittest
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
from Vector import *
from LinearAlgebra.Vector import *
class Vector2Test(unittest.TestCase):
def test_Equality(self):

0
LinearAlgebra/test/__init__.py Normal file
View File