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

using isclose

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-31 12:20:52 +02:00
parent de57d5fe97
commit 8949a87956
14 changed files with 481 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
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/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/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

12
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)
@ -312,6 +319,11 @@ class Spherical(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 Normalized(self) -> float:
if self.distance == 0:

6
LinearAlgebra/SwingTwist.py
View File

@ -79,6 +79,12 @@ class SwingTwist:
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:

416
Vector.py Normal file
View File

@ -0,0 +1,416 @@
import math
from LinearAlgebra.Angle import *
epsilon = 1E-05
class Vector2:
def __init__(self, right: float = 0, up: float = 0):
"""! A new 2-dimensional vector
@param right The distance in the right direction in meters
@param up The distance in the upward direction in meters
"""
## The right axis of the vector
self.right: float = right
## The upward axis of the vector
self.up: float = up
def __eq__(self, other) -> bool:
"""! Check if this vector is equal to the given vector
@param v The vector to check against
@return true if it is identical to the given vector
@note This uses float comparison to check equality which may have strange
effects. Equality on floats should be avoided.
"""
return (
self.right == other.right and
self.up == other.up
)
def SqrMagnitude(self) -> float:
"""! The squared vector length
@return The squared vector length
@remark The squared length is computationally simpler than the real
length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
calculation of the squared root of C.
"""
return self.right ** 2 + self.up ** 2
def Magnitude(self) -> float:
"""! The vector length
@return The vector length
"""
return math.sqrt(self.SqrMagnitude())
def Normalized(self):
"""! Convert the vector to a length of 1
@return The vector normalized to a length of 1
"""
length: float = self.Magnitude();
result = Vector2.zero
if length > epsilon:
result = self / length;
return result
def __neg__(self):
"""! Negate te vector such that it points in the opposite direction
@return The negated vector
"""
return Vector2(-self.right, -self.up)
def __sub__(self, other):
"""! Subtract a vector from this vector
@param other The vector to subtract from this vector
@return The result of this subtraction
"""
return Vector2(
self.right - other.right,
self.up - other.up
)
def __add__(self, other):
"""! Add a vector to this vector
@param other The vector to add to this vector
@return The result of the addition
"""
return Vector2(
self.right + other.right,
self.up + other.up
)
def Scale(self, scaling):
"""! Scale the vector using another vector
@param scaling A vector with the scaling factors
@return The scaled vector
@remark Each component of the vector will be multiplied with the
matching component from the scaling vector.
"""
return Vector2(
self.right * scaling.right,
self.up * scaling.up
)
def __mul__(self, factor):
"""! Scale the vector uniformly up
@param factor The scaling factor
@return The scaled vector
@remark Each component of the vector will be multiplied by the same factor.
"""
return Vector2(
self.right * factor,
self.up * factor
)
def __truediv__(self, factor):
"""! Scale the vector uniformly down
@param f The scaling factor
@return The scaled vector
@remark Each component of the vector will be divided by the same factor.
"""
return Vector2(
self.right / factor,
self.up / factor
)
@staticmethod
def Distance(v1, v2) -> float:
"""! The distance between two vectors
@param v1 The first vector
@param v2 The second vector
@return The distance between the two vectors
"""
return (v1 - v2).Magnitude()
@staticmethod
def Dot(v1, v2) -> float:
"""! The dot product of two vectors
@param v1 The first vector
@param v2 The second vector
@return The dot product of the two vectors
"""
return v1.right * v2.right + v1.up * v2.up
@staticmethod
def Angle(v1, v2) -> Angle:
"""! The angle between two vectors
@param v1 The first vector
@param v2 The second vector
@return The angle between the two vectors
@remark This reterns an unsigned angle which is the shortest distance
between the two vectors. Use Vector3::SignedAngle if a signed angle is
needed.
"""
denominator: float = math.sqrt(v1.SqrMagnitude() * v2.SqrMagnitude())
if denominator < epsilon:
return Angle.zero
dot: float = Vector2.Dot(v1, v2)
fraction: float = dot / denominator
# if math.nan(fraction):
# return Angle.Degrees(fraction) # short cut to returning NaN universally
cdot: float = Float.Clamp(fraction, -1.0, 1.0)
r: float = math.acos(cdot)
return Angle.Radians(r);
@staticmethod
def SignedAngle(v1, v2) -> Angle:
"""! The signed angle between two vectors
@param v1 The starting vector
@param v2 The ending vector
@param axis The axis to rotate around
@return The signed angle between the two vectors
"""
sqr_mag_from: float = v1.SqrMagnitude()
sqr_mag_to: float = v2.SqrMagnitude()
if sqr_mag_from == 0 or sqr_mag_to == 0:
return Angle.zero
# if (!isfinite(sqrMagFrom) || !isfinite(sqrMagTo))
# return nanf("");
angle_from = math.atan2(v1.up, v1.right)
angle_to = math.atan2(v2.up, v2.right)
return Angle.Radians(-(angle_to - angle_from))
@staticmethod
def Lerp(v1, v2, f: float):
"""! Lerp (linear interpolation) between two vectors
@param v1 The starting vector
@param v2 The ending vector
@param f The interpolation distance
@return The lerped vector
@remark The factor f is unclamped. Value 0 matches the vector *v1*, Value
1 matches vector *v2*. Value -1 is vector *v1* minus the difference
between *v1* and *v2* etc.
"""
return v1 + (v2 - v1) * f
## A vector with zero for all axis
Vector2.zero = Vector2(0, 0)
## A vector with one for all axis
Vector2.one = Vector2(1, 1)
## A normalized right-oriented vector
Vector2.right = Vector2(1, 0)
## A normalized left-oriented vector
Vector2.left = Vector2(-1, 0)
## A normalized up-oriented vector
Vector2.up = Vector2(0, 1)
## A normalized down-oriented vector
Vector2.down = Vector2(0, -1)
class Vector3(Vector2):
def __init__(self, right: float = 0, up: float = 0, forward: float = 0):
"""! A new 3-dimensional vector
@param right The distance in the right direction in meters
@param up The distance in the upward direction in meters
@param forward The distance in the forward direction in meters
"""
## The right axis of the vector
self.right: float = right
## The upward axis of the vector
self.up: float = up
## The forward axis of the vector
self.forward: float = forward
def __eq__(self, other) -> bool:
"""! Check if this vector is equal to the given vector
@param v The vector to check against
@return true if it is identical to the given vector
@note This uses float comparison to check equality which may have strange
effects. Equality on floats should be avoided.
"""
return (
self.right == other.right and
self.up == other.up and
self.forward == other.forward
)
def SqrMagnitude(self) -> float:
"""! The squared vector length
@return The squared vector length
@remark The squared length is computationally simpler than the real
length. Think of Pythagoras A^2 + B^2 = C^2. This leaves out the
calculation of the squared root of C.
"""
return self.right ** 2 + self.up ** 2 + self.forward ** 2
def Normalized(self):
"""! Convert the vector to a length of 1
@return The vector normalized to a length of 1
"""
length: float = self.Magnitude();
result = Vector3()
if length > epsilon:
result = self / length;
return result
def __neg__(self):
"""! Negate te vector such that it points in the opposite direction
@return The negated vector
"""
return Vector3(-self.right, -self.up, -self.forward)
def __sub__(self, other):
"""! Subtract a vector from this vector
@param other The vector to subtract from this vector
@return The result of this subtraction
"""
return Vector3(
self.right - other.right,
self.up - other.up,
self.forward - other.forward
)
def __add__(self, other):
"""! Add a vector to this vector
@param other The vector to add to this vector
@return The result of the addition
"""
return Vector3(
self.right + other.right,
self.up + other.up,
self.forward + other.forward
)
def Scale(self, scaling):
"""! Scale the vector using another vector
@param scaling A vector with the scaling factors
@return The scaled vector
@remark Each component of the vector will be multiplied with the
matching component from the scaling vector.
"""
return Vector3(
self.right * scaling.right,
self.up * scaling.up,
self.forward * scaling.forward
)
def __mul__(self, factor):
"""! Scale the vector uniformly up
@param factor The scaling factor
@return The scaled vector
@remark Each component of the vector will be multiplied by the same factor.
"""
return Vector3(
self.right * factor,
self.up * factor,
self.forward * factor
)
def __truediv__(self, factor):
"""! Scale the vector uniformly down
@param f The scaling factor
@return The scaled vector
@remark Each component of the vector will be divided by the same factor.
"""
return Vector3(
self.right / factor,
self.up / factor,
self.forward / factor
)
@staticmethod
def Dot(v1, v2) -> float:
"""! The dot product of two vectors
@param v1 The first vector
@param v2 The second vector
@return The dot product of the two vectors
"""
return v1.right * v2.right + v1.up * v2.up + v1.forward * v2.forward
@staticmethod
def Cross(v1, v2):
"""! The cross product of two vectors
@param v1 The first vector
@param v2 The second vector
@return The cross product of the two vectors
"""
return Vector3(
v1.up * v2.forward - v1.forward * v2.up,
v1.forward * v2.right - v1.right * v2.forward,
v1.right * v2.up - v1.up * v2.right
)
def Project(self, other):
"""! Project the vector on another vector
@param other The normal vecto to project on
@return The projected vector
"""
sqrMagnitude = other.SqrMagnitude()
if sqrMagnitude < epsilon:
return Vector3.zero
else:
dot = Vector3.Dot(self, other)
return other * dot / sqrMagnitude;
def ProjectOnPlane(self, normal):
"""! Project the vector on a plane defined by a normal orthogonal to the
plane.
@param normal The normal of the plane to project on
@return Teh projected vector
"""
return self - self.Project(normal)
@staticmethod
def Angle(v1, v2) -> Angle:
"""! The angle between two vectors
@param v1 The first vector
@param v2 The second vector
@return The angle between the two vectors
@remark This reterns an unsigned angle which is the shortest distance
between the two vectors. Use Vector3::SignedAngle if a signed angle is
needed.
"""
denominator: float = math.sqrt(v1.SqrMagnitude() * v2.SqrMagnitude())
if denominator < epsilon:
return Angle.zero
dot: float = Vector3.Dot(v1, v2)
fraction: float = dot / denominator
if math.isnan(fraction):
return Angle.Degrees(fraction) # short cut to returning NaN universally
cdot: float = Float.Clamp(fraction, -1.0, 1.0)
r: float = math.acos(cdot)
return Angle.Radians(r);
@staticmethod
def SignedAngle(v1, v2, axis) -> Angle:
"""! The signed angle between two vectors
@param v1 The starting vector
@param v2 The ending vector
@param axis The axis to rotate around
@return The signed angle between the two vectors
"""
# angle in [0,180]
angle: Angle = Vector3.Angle(v1, v2)
cross: Vector3 = Vector3.Cross(v1, v2)
b: float = Vector3.Dot(axis, cross)
sign:int = 0
if b < 0:
sign = -1
elif b > 0:
sign = 1
# angle in [-179,180]
return angle * sign
## A vector with zero for all axis
Vector3.zero = Vector3(0, 0, 0)
## A vector with one for all axis
Vector3.one = Vector3(1, 1, 1)
## A normalized forward-oriented vector
Vector3.forward = Vector3(0, 0, 1)
## A normalized back-oriented vector
Vector3.back = Vector3(0, 0, -1)
## A normalized right-oriented vector
Vector3.right = Vector3(1, 0, 0)
## A normalized left-oriented vector
Vector3.left = Vector3(-1, 0, 0)
## A normalized up-oriented vector
Vector3.up = Vector3(0, 1, 0)
## A normalized down-oriented vector
Vector3.down = Vector3(0, -1, 0)

6
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
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
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
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
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
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
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
test/__init__.py Normal file
View File