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

Add 'LinearAlgebra/' from commit 'b60b248f7529a56244681bce0b204443abceacb2'

Browse Source 
git-subtree-dir: LinearAlgebra
git-subtree-mainline: b87c4df0e3c2ee2aaf58bb8446d4b970d6fcb433
git-subtree-split: b60b248f7529a56244681bce0b204443abceacb2
This commit is contained in:
Pascal Serrarens 2025-03-21 09:54:30 +01:00
commit 208b9056dc
10 changed files with 748 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

170
LinearAlgebra/.gitignore vendored Normal file
View File

@ -0,0 +1,170 @@
# ---> Python
# Byte-compiled / optimized / DLL files
__pycache__/
*.py[cod]
*$py.class
# C extensions
*.so
# Distribution / packaging
.Python
build/
develop-eggs/
dist/
downloads/
eggs/
.eggs/
lib/
lib64/
parts/
sdist/
var/
wheels/
share/python-wheels/
*.egg-info/
.installed.cfg
*.egg
MANIFEST
# PyInstaller
# Usually these files are written by a python script from a template
# before PyInstaller builds the exe, so as to inject date/other infos into it.
*.manifest
*.spec
# Installer logs
pip-log.txt
pip-delete-this-directory.txt
# Unit test / coverage reports
htmlcov/
.tox/
.nox/
.coverage
.coverage.*
.cache
nosetests.xml
coverage.xml
*.cover
*.py,cover
.hypothesis/
.pytest_cache/
cover/
# Translations
*.mo
*.pot
# Django stuff:
*.log
local_settings.py
db.sqlite3
db.sqlite3-journal
# Flask stuff:
instance/
.webassets-cache
# Scrapy stuff:
.scrapy
# Sphinx documentation
docs/_build/
# PyBuilder
.pybuilder/
target/
# Jupyter Notebook
.ipynb_checkpoints
# IPython
profile_default/
ipython_config.py
# pyenv
# For a library or package, you might want to ignore these files since the code is
# intended to run in multiple environments; otherwise, check them in:
# .python-version
# pipenv
# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
# However, in case of collaboration, if having platform-specific dependencies or dependencies
# having no cross-platform support, pipenv may install dependencies that don't work, or not
# install all needed dependencies.
#Pipfile.lock
# UV
# Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control.
# This is especially recommended for binary packages to ensure reproducibility, and is more
# commonly ignored for libraries.
#uv.lock
# poetry
# Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
# This is especially recommended for binary packages to ensure reproducibility, and is more
# commonly ignored for libraries.
# https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
#poetry.lock
# pdm
# Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
#pdm.lock
# pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
# in version control.
# https://pdm.fming.dev/latest/usage/project/#working-with-version-control
.pdm.toml
.pdm-python
.pdm-build/
# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
__pypackages__/
# Celery stuff
celerybeat-schedule
celerybeat.pid
# SageMath parsed files
*.sage.py
# Environments
.env
.venv
env/
venv/
ENV/
env.bak/
venv.bak/
# Spyder project settings
.spyderproject
.spyproject
# Rope project settings
.ropeproject
# mkdocs documentation
/site
# mypy
.mypy_cache/
.dmypy.json
dmypy.json
# Pyre type checker
.pyre/
# pytype static type analyzer
.pytype/
# Cython debug symbols
cython_debug/
# PyCharm
# JetBrains specific template is maintained in a separate JetBrains.gitignore that can
# be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
# and can be added to the global gitignore or merged into this file. For a more nuclear
# option (not recommended) you can uncomment the following to ignore the entire idea folder.
#.idea/

11
LinearAlgebra/.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,11 @@
{
"python.testing.unittestArgs": [
"-v",
"-s",
"./test",
"-p",
"*_test.py"
],
"python.testing.pytestEnabled": false,
"python.testing.unittestEnabled": true
}

13
LinearAlgebra/Angle.py Normal file
View File

@ -0,0 +1,13 @@
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 Normal file
View File

@ -0,0 +1,52 @@
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)

373
LinearAlgebra/LICENSE Normal file
View File

@ -0,0 +1,373 @@
Mozilla Public License Version 2.0
==================================
1. Definitions
--------------
1.1. "Contributor"
means each individual or legal entity that creates, contributes to
the creation of, or owns Covered Software.
1.2. "Contributor Version"
means the combination of the Contributions of others (if any) used
by a Contributor and that particular Contributor's Contribution.
1.3. "Contribution"
means Covered Software of a particular Contributor.
1.4. "Covered Software"
means Source Code Form to which the initial Contributor has attached
the notice in Exhibit A, the Executable Form of such Source Code
Form, and Modifications of such Source Code Form, in each case
including portions thereof.
1.5. "Incompatible With Secondary Licenses"
means
(a) that the initial Contributor has attached the notice described
in Exhibit B to the Covered Software; or
(b) that the Covered Software was made available under the terms of
version 1.1 or earlier of the License, but not also under the
terms of a Secondary License.
1.6. "Executable Form"
means any form of the work other than Source Code Form.
1.7. "Larger Work"
means a work that combines Covered Software with other material, in
a separate file or files, that is not Covered Software.
1.8. "License"
means this document.
1.9. "Licensable"
means having the right to grant, to the maximum extent possible,
whether at the time of the initial grant or subsequently, any and
all of the rights conveyed by this License.
1.10. "Modifications"
means any of the following:
(a) any file in Source Code Form that results from an addition to,
deletion from, or modification of the contents of Covered
Software; or
(b) any new file in Source Code Form that contains any Covered
Software.
1.11. "Patent Claims" of a Contributor
means any patent claim(s), including without limitation, method,
process, and apparatus claims, in any patent Licensable by such
Contributor that would be infringed, but for the grant of the
License, by the making, using, selling, offering for sale, having
made, import, or transfer of either its Contributions or its
Contributor Version.
1.12. "Secondary License"
means either the GNU General Public License, Version 2.0, the GNU
Lesser General Public License, Version 2.1, the GNU Affero General
Public License, Version 3.0, or any later versions of those
licenses.
1.13. "Source Code Form"
means the form of the work preferred for making modifications.
1.14. "You" (or "Your")
means an individual or a legal entity exercising rights under this
License. For legal entities, "You" includes any entity that
controls, is controlled by, or is under common control with You. For
purposes of this definition, "control" means (a) the power, direct
or indirect, to cause the direction or management of such entity,
whether by contract or otherwise, or (b) ownership of more than
fifty percent (50%) of the outstanding shares or beneficial
ownership of such entity.
2. License Grants and Conditions
--------------------------------
2.1. Grants
Each Contributor hereby grants You a world-wide, royalty-free,
non-exclusive license:
(a) under intellectual property rights (other than patent or trademark)
Licensable by such Contributor to use, reproduce, make available,
modify, display, perform, distribute, and otherwise exploit its
Contributions, either on an unmodified basis, with Modifications, or
as part of a Larger Work; and
(b) under Patent Claims of such Contributor to make, use, sell, offer
for sale, have made, import, and otherwise transfer either its
Contributions or its Contributor Version.
2.2. Effective Date
The licenses granted in Section 2.1 with respect to any Contribution
become effective for each Contribution on the date the Contributor first
distributes such Contribution.
2.3. Limitations on Grant Scope
The licenses granted in this Section 2 are the only rights granted under
this License. No additional rights or licenses will be implied from the
distribution or licensing of Covered Software under this License.
Notwithstanding Section 2.1(b) above, no patent license is granted by a
Contributor:
(a) for any code that a Contributor has removed from Covered Software;
or
(b) for infringements caused by: (i) Your and any other third party's
modifications of Covered Software, or (ii) the combination of its
Contributions with other software (except as part of its Contributor
Version); or
(c) under Patent Claims infringed by Covered Software in the absence of
its Contributions.
This License does not grant any rights in the trademarks, service marks,
or logos of any Contributor (except as may be necessary to comply with
the notice requirements in Section 3.4).
2.4. Subsequent Licenses
No Contributor makes additional grants as a result of Your choice to
distribute the Covered Software under a subsequent version of this
License (see Section 10.2) or under the terms of a Secondary License (if
permitted under the terms of Section 3.3).
2.5. Representation
Each Contributor represents that the Contributor believes its
Contributions are its original creation(s) or it has sufficient rights
to grant the rights to its Contributions conveyed by this License.
2.6. Fair Use
This License is not intended to limit any rights You have under
applicable copyright doctrines of fair use, fair dealing, or other
equivalents.
2.7. Conditions
Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
in Section 2.1.
3. Responsibilities
-------------------
3.1. Distribution of Source Form
All distribution of Covered Software in Source Code Form, including any
Modifications that You create or to which You contribute, must be under
the terms of this License. You must inform recipients that the Source
Code Form of the Covered Software is governed by the terms of this
License, and how they can obtain a copy of this License. You may not
attempt to alter or restrict the recipients' rights in the Source Code
Form.
3.2. Distribution of Executable Form
If You distribute Covered Software in Executable Form then:
(a) such Covered Software must also be made available in Source Code
Form, as described in Section 3.1, and You must inform recipients of
the Executable Form how they can obtain a copy of such Source Code
Form by reasonable means in a timely manner, at a charge no more
than the cost of distribution to the recipient; and
(b) You may distribute such Executable Form under the terms of this
License, or sublicense it under different terms, provided that the
license for the Executable Form does not attempt to limit or alter
the recipients' rights in the Source Code Form under this License.
3.3. Distribution of a Larger Work
You may create and distribute a Larger Work under terms of Your choice,
provided that You also comply with the requirements of this License for
the Covered Software. If the Larger Work is a combination of Covered
Software with a work governed by one or more Secondary Licenses, and the
Covered Software is not Incompatible With Secondary Licenses, this
License permits You to additionally distribute such Covered Software
under the terms of such Secondary License(s), so that the recipient of
the Larger Work may, at their option, further distribute the Covered
Software under the terms of either this License or such Secondary
License(s).
3.4. Notices
You may not remove or alter the substance of any license notices
(including copyright notices, patent notices, disclaimers of warranty,
or limitations of liability) contained within the Source Code Form of
the Covered Software, except that You may alter any license notices to
the extent required to remedy known factual inaccuracies.
3.5. Application of Additional Terms
You may choose to offer, and to charge a fee for, warranty, support,
indemnity or liability obligations to one or more recipients of Covered
Software. However, You may do so only on Your own behalf, and not on
behalf of any Contributor. You must make it absolutely clear that any
such warranty, support, indemnity, or liability obligation is offered by
You alone, and You hereby agree to indemnify every Contributor for any
liability incurred by such Contributor as a result of warranty, support,
indemnity or liability terms You offer. You may include additional
disclaimers of warranty and limitations of liability specific to any
jurisdiction.
4. Inability to Comply Due to Statute or Regulation
---------------------------------------------------
If it is impossible for You to comply with any of the terms of this
License with respect to some or all of the Covered Software due to
statute, judicial order, or regulation then You must: (a) comply with
the terms of this License to the maximum extent possible; and (b)
describe the limitations and the code they affect. Such description must
be placed in a text file included with all distributions of the Covered
Software under this License. Except to the extent prohibited by statute
or regulation, such description must be sufficiently detailed for a
recipient of ordinary skill to be able to understand it.
5. Termination
--------------
5.1. The rights granted under this License will terminate automatically
if You fail to comply with any of its terms. However, if You become
compliant, then the rights granted under this License from a particular
Contributor are reinstated (a) provisionally, unless and until such
Contributor explicitly and finally terminates Your grants, and (b) on an
ongoing basis, if such Contributor fails to notify You of the
non-compliance by some reasonable means prior to 60 days after You have
come back into compliance. Moreover, Your grants from a particular
Contributor are reinstated on an ongoing basis if such Contributor
notifies You of the non-compliance by some reasonable means, this is the
first time You have received notice of non-compliance with this License
from such Contributor, and You become compliant prior to 30 days after
Your receipt of the notice.
5.2. If You initiate litigation against any entity by asserting a patent
infringement claim (excluding declaratory judgment actions,
counter-claims, and cross-claims) alleging that a Contributor Version
directly or indirectly infringes any patent, then the rights granted to
You by any and all Contributors for the Covered Software under Section
2.1 of this License shall terminate.
5.3. In the event of termination under Sections 5.1 or 5.2 above, all
end user license agreements (excluding distributors and resellers) which
have been validly granted by You or Your distributors under this License
prior to termination shall survive termination.
************************************************************************
* *
* 6. Disclaimer of Warranty *
* ------------------------- *
* *
* Covered Software is provided under this License on an "as is" *
* basis, without warranty of any kind, either expressed, implied, or *
* statutory, including, without limitation, warranties that the *
* Covered Software is free of defects, merchantable, fit for a *
* particular purpose or non-infringing. The entire risk as to the *
* quality and performance of the Covered Software is with You. *
* Should any Covered Software prove defective in any respect, You *
* (not any Contributor) assume the cost of any necessary servicing, *
* repair, or correction. This disclaimer of warranty constitutes an *
* essential part of this License. No use of any Covered Software is *
* authorized under this License except under this disclaimer. *
* *
************************************************************************
************************************************************************
* *
* 7. Limitation of Liability *
* -------------------------- *
* *
* Under no circumstances and under no legal theory, whether tort *
* (including negligence), contract, or otherwise, shall any *
* Contributor, or anyone who distributes Covered Software as *
* permitted above, be liable to You for any direct, indirect, *
* special, incidental, or consequential damages of any character *
* including, without limitation, damages for lost profits, loss of *
* goodwill, work stoppage, computer failure or malfunction, or any *
* and all other commercial damages or losses, even if such party *
* shall have been informed of the possibility of such damages. This *
* limitation of liability shall not apply to liability for death or *
* personal injury resulting from such party's negligence to the *
* extent applicable law prohibits such limitation. Some *
* jurisdictions do not allow the exclusion or limitation of *
* incidental or consequential damages, so this exclusion and *
* limitation may not apply to You. *
* *
************************************************************************
8. Litigation
-------------
Any litigation relating to this License may be brought only in the
courts of a jurisdiction where the defendant maintains its principal
place of business and such litigation shall be governed by laws of that
jurisdiction, without reference to its conflict-of-law provisions.
Nothing in this Section shall prevent a party's ability to bring
cross-claims or counter-claims.
9. Miscellaneous
----------------
This License represents the complete agreement concerning the subject
matter hereof. If any provision of this License is held to be
unenforceable, such provision shall be reformed only to the extent
necessary to make it enforceable. Any law or regulation which provides
that the language of a contract shall be construed against the drafter
shall not be used to construe this License against a Contributor.
10. Versions of the License
---------------------------
10.1. New Versions
Mozilla Foundation is the license steward. Except as provided in Section
10.3, no one other than the license steward has the right to modify or
publish new versions of this License. Each version will be given a
distinguishing version number.
10.2. Effect of New Versions
You may distribute the Covered Software under the terms of the version
of the License under which You originally received the Covered Software,
or under the terms of any subsequent version published by the license
steward.
10.3. Modified Versions
If you create software not governed by this License, and you want to
create a new license for such software, you may create and use a
modified version of this License if you rename the license and remove
any references to the name of the license steward (except to note that
such modified license differs from this License).
10.4. Distributing Source Code Form that is Incompatible With Secondary
Licenses
If You choose to distribute Source Code Form that is Incompatible With
Secondary Licenses under the terms of this version of the License, the
notice described in Exhibit B of this License must be attached.
Exhibit A - Source Code Form License Notice
-------------------------------------------
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/.
If it is not possible or desirable to put the notice in a particular
file, then You may include the notice in a location (such as a LICENSE
file in a relevant directory) where a recipient would be likely to look
for such a notice.
You may add additional accurate notices of copyright ownership.
Exhibit B - "Incompatible With Secondary Licenses" Notice
---------------------------------------------------------
This Source Code Form is "Incompatible With Secondary Licenses", as
defined by the Mozilla Public License, v. 2.0.

44
LinearAlgebra/Quaternion.py Normal file
View File

@ -0,0 +1,44 @@
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()

2
LinearAlgebra/README.md Normal file
View File

@ -0,0 +1,2 @@
# LinearAlgebra-python

41
LinearAlgebra/Spherical.py Normal file
View File

@ -0,0 +1,41 @@
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 Normal file
View File

@ -0,0 +1,28 @@
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

14
LinearAlgebra/test/Angle_test.py Normal file
View File

@ -0,0 +1,14 @@
import sys
from pathlib import Path
# Add the project root to sys.path
sys.path.append(str(Path(__file__).resolve().parent.parent))
import unittest
class AngleTest(unittest.TestCase):
def test_one(self):
pass
if __name__ == '__main__':
unittest.main()