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

Replacing LinearAlgebra by repo subtree...

Browse Source
This commit is contained in:
Pascal Serrarens 2025-03-21 09:54:25 +01:00
parent 2065789576
commit b87c4df0e3
6 changed files with 0 additions and 178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
LinearAlgebra/Angle.py
View File

@ -1,13 +0,0 @@
import math
class Angle:
Rad2Deg = 360 / (math.pi * 2)
Deg2Rad = (math.pi * 2) / 360
@staticmethod
def Normalize(angle):
while angle < -180:
angle += 360
while angle >= 180:
angle -= 360
return angle

52
LinearAlgebra/Direction.py
View File

@ -1,52 +0,0 @@
import math
from LinearAlgebra.Angle import Angle
class Direction:
def __init__(self, horizontal=0, vertical=0):
self.horizontal: float = Angle.Normalize(horizontal)
self.vertical: float = Angle.Normalize(vertical)
self.Normalize()
@staticmethod
def Degrees(horizontal: float, vertical: float):
direction = Direction (horizontal, vertical)
return direction
def __add__(self, other):
return Direction(self.horizontal + other.x, self.vertical + other.y)
def __neg__(self):
return Direction(self.horizontal + 180, -self.vertical)
def __sub__(self, other):
return Direction(self.horizontal - other.x, self.vertical - other.y)
def __mul__(self, scalar):
return Direction(self.horizontal * scalar, self.vertical * scalar)
def __truediv__(self, scalar):
if scalar != 0:
return Direction(self.horizontal / scalar, self.vertical / scalar)
else:
raise ValueError("Cannot divide by zero")
def Magnitude(self):
return math.sqrt(self.horizontal**2 + self.vertical**2)
def Normalize(self):
if self.vertical > 90 or self.vertical < -90:
self.horizontal += 180
self.verical = 180 - self.verical
def Dot(self, other):
return self.horizontal * other.x + self.vertical * other.y
def __repr__(self):
return f"Direction(x={self.horizontal}, y={self.vertical})"
Direction.forward = Direction(0, 0)
Direction.backward = Direction(-180, 0)
Direction.up = Direction(0, 90)
Direction.down = Direction(0, -90)
Direction.left = Direction(-90, 0)
Direction.right = Direction(90, 0)

44
LinearAlgebra/Quaternion.py
View File

@ -1,44 +0,0 @@
import math
Deg2Rad = (math.pi * 2) / 360
class Quaternion:
def __init__(self):
self.x = 0
self.y = 0
self.z = 0
self.w = 1
@staticmethod
def Euler(x, y, z):
yaw = x * Deg2Rad
pitch = y * Deg2Rad
roll = z * Deg2Rad
roll_over_2 = roll * 0.5
sin_roll_over_2 = math.sin(roll_over_2)
cos_roll_over_2 = math.cos(roll_over_2)
pitch_over_2 = pitch * 0.5
sin_pitch_over_2 = math.sin(pitch_over_2)
cos_pitch_over_2 = math.cos(pitch_over_2)
yaw_over_2 = yaw * 0.5
sin_yaw_over_2 = math.sin(yaw_over_2)
cos_yaw_over_2 = math.cos(yaw_over_2)
result = Quaternion()
result.w = (cos_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
sin_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
result.x = (sin_yaw_over_2 * cos_pitch_over_2 * cos_roll_over_2 +
cos_yaw_over_2 * sin_pitch_over_2 * sin_roll_over_2)
result.y = (cos_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2 -
sin_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2)
result.z = (cos_yaw_over_2 * cos_pitch_over_2 * sin_roll_over_2 -
sin_yaw_over_2 * sin_pitch_over_2 * cos_roll_over_2)
return result
Quaternion.identity = Quaternion()

41
LinearAlgebra/Spherical.py
View File

@ -1,41 +0,0 @@
import math
from LinearAlgebra.Direction import Direction
class Spherical:
def __init__(self, distance, direction):
if distance < 0:
self.distance = -distance
self.direction = -direction
else:
self.distance: float = distance
self.direction: Direction = direction
# def __init__(self, distance, horizontal, vertical):
# self.distance = distance
# self.direction = Direction(horizontal, vertical)
def to_cartesian(self):
x = self.distance * math.sin(self.direction.horizontal) * math.cos(self.direction.vertical)
y = self.distance * math.sin(self.direction.horizontal) * math.sin(self.direction.vertical)
z = self.distance * math.cos(self.direction.horizontal)
return x, y, z
def from_cartesian(self, x, y, z):
self.distance = math.sqrt(x**2 + y**2 + z**2)
self.direction.horizontal = math.acos(z / self.distance)
self.direction.vertical = math.atan2(y, x)
def __add__(self, other):
x1, y1, z1 = self.to_cartesian()
x2, y2, z2 = other.to_cartesian()
return Spherical.from_cartesian(x1 + x2, y1 + y2, z1 + z2)
def __sub__(self, other):
x1, y1, z1 = self.to_cartesian()
x2, y2, z2 = other.to_cartesian()
return Spherical.from_cartesian(x1 - x2, y1 - y2, z1 - z2)
def __repr__(self):
return f"Spherical(r={self.distance}, horizontal={self.direction.horizontal}, phi={self.direction.vertical})"
Spherical.zero = Spherical(0, Direction.forward)

28
LinearAlgebra/SwingTwist.py
View File

@ -1,28 +0,0 @@
from LinearAlgebra.Direction import Direction
from LinearAlgebra.Quaternion import Quaternion
class SwingTwist:
"""A rotation using swing and twist angle components"""
def __init__(self, swing: Direction, twist: float):
if swing.vertical > 90 or swing.vertical < -90:
swing.horizontal += 180
swing.vertical = 180 - swing.vertical
twist += 180
## Swing component of the rotation
self.swing = swing
## The twist component of the rotation
self.twist = twist
@staticmethod
def Degrees(horizontal: float, vertical: float, twist: float):
direction = Direction(horizontal, vertical)
swing_twist = SwingTwist(direction, twist)
return swing_twist
def ToQuaternion(self) -> Quaternion:
"""Convert the SwingTwist rotation to a Quaternion"""
q = Quaternion.Euler(-self.swing.vertical,
self.swing.horizontal,
self.twist)
return q

0
test/BB2B_test.py → test/BB2B_dummytest.py
View File