diff --git a/.gitignore b/.gitignore index 08fe51f..dc49d8c 100644 --- a/.gitignore +++ b/.gitignore @@ -3,3 +3,4 @@ DoxyGen/DoxyWarnLogfile.txt **/__pycache__/* RoboidControl.egg-info/ +build/* \ No newline at end of file diff --git a/README.md b/README.md index fb83844..f0a003e 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,14 @@ Supporting: - Windows - MacOS - Linux -- MicroPython + +# Install + +- Go to the root folder of this repository (the folder containing this `README.md` and `setup.py`) +- Execute the command. +``` +pip install . +``` # Basic components diff --git a/RoboidControl/LinearAlgebra/.gitignore b/RoboidControl/LinearAlgebra/.gitignore deleted file mode 100644 index 0dbf2f2..0000000 --- a/RoboidControl/LinearAlgebra/.gitignore +++ /dev/null @@ -1,170 +0,0 @@ -# ---> 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/ - diff --git a/RoboidControl/LinearAlgebra/.vscode/settings.json b/RoboidControl/LinearAlgebra/.vscode/settings.json deleted file mode 100644 index 5001e91..0000000 --- a/RoboidControl/LinearAlgebra/.vscode/settings.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "python.testing.unittestArgs": [ - "-v", - "-s", - "./test", - "-p", - "*_test.py" - ], - "python.testing.pytestEnabled": false, - "python.testing.unittestEnabled": true -} \ No newline at end of file diff --git a/RoboidControl/LinearAlgebra/LICENSE b/RoboidControl/LinearAlgebra/LICENSE deleted file mode 100644 index ee6256c..0000000 --- a/RoboidControl/LinearAlgebra/LICENSE +++ /dev/null @@ -1,373 +0,0 @@ -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. diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/Angle.py b/RoboidControl/LinearAlgebra/LinearAlgebra/Angle.py deleted file mode 100644 index 255f261..0000000 --- a/RoboidControl/LinearAlgebra/LinearAlgebra/Angle.py +++ /dev/null @@ -1,275 +0,0 @@ -# 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 math - -from .Float import * - -# This is in fact AngleSingle -class Angle: - # The angle is internally limited to (-180..180] degrees or (-PI...PI] - # radians. When an angle exceeds this range, it is normalized to a value - # within the range. - Rad2Deg = 360 / (math.pi * 2) - Deg2Rad = (math.pi * 2) / 360 - - def __init__(self, degrees = 0): - self.value: float = degrees - Angle.Normalize(self) - - @staticmethod - def Degrees(degrees): - angle = Angle(degrees) - return angle - - @staticmethod - def Radians(radians): - angle = Angle(radians * Angle.Rad2Deg) - return angle; - - def InDegrees(self): - return self.value; - def InRadians(self) -> float: - return self.value * Angle.Deg2Rad; - - def __eq__(self, 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 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 - @return True when this angle is greater than the given angle, False - otherwise - """ - return self.value > angle.value - def __gte__(self, angle): - """! Tests if this angle is greater than or equal to the given angle - @param angle The given angle - @return True when this angle is greater than or equal to the given angle. - False otherwise. - """ - return self.value >= angle.value - def __lt__(self, angle): - """! Tests if this angle is less than the given angle - @param angle The given angle - @return True when this angle is less than the given angle, False - otherwise - """ - return self.value < angle.value - def __lte__(self, angle): - """! Tests if this angle is less than or equal to the given angle - @param angle The given angle - @return True when this angle is less than or equal to the given angle. - False otherwise. - """ - return self.value <= angle.value - - def Sign(self): - """! Returns the sign of the angle - @param angle The angle - @return -1 when the angle is negative, 1 when it is positive and 0 - otherwise. - """ - if self.value < 0: - return -1 - if self.value > 0: - return 1 - return 0 - def Abs(self): - """! Returns the magnitude of the angle - @param angle The angle - @return The positive magitude of the angle. - Negative values are negated to get a positive result - """ - if self.value < 0: - return -self - return self - - def __neg__(self): - """! Negate the angle - @return The negated angle - """ - return Angle(-self.value) - def Inverse(self): - """! Invert the angle: rotate by 180 degrees - """ - return self + Angle.Degrees(180) - - def __sub__(self, other): - """! Substract another angle from this angle - @param angle The angle to subtract from this angle - @return The result of the subtraction - """ - return Angle(self.value - other.value) - def __add__(self, other): - """! Add another angle from this angle - @param angle The angle to add to this angle - @return The result of the addition - """ - return Angle(self.value + other.value) - def __mul__(self, factor): - """! Multiplies the angle by a factor - @param angle The angle to multiply - @param factor The factor by which the angle is multiplied - @return The multiplied angle - """ - return Angle(self.value * factor) - def __truediv__(self, factor): - """! Divides the angle by a factor - @param angle The angle to devide - @param factor The factor by which the angle is devided - @return The devided angle - """ - return Angle(self.value / factor) - - @staticmethod - def Normalize(angle): - """! Normalizes the angle to (-180..180] or (-PI..PI] - @note Should not be needed but available in case it is. - """ - while angle.value < -180: - angle.value += 360 - while angle.value >= 180: - angle.value -= 360 - return angle - - @staticmethod - def Clamp(angle, min, max): - """! Clamps the angle value between the two given angles - @param angle The angle to clamp - @param min The minimum angle - @param max The maximum angle - @return The clamped value - @remark When the min value is greater than the max value, angle is - returned unclamped. - """ - degrees = Float.Clamp(angle.InDegrees(), min.InDegrees(), max.InDegrees()); - return Angle.Degrees(degrees) - - @staticmethod - def MoveTowards(from_angle, to_angle, max_angle): - """! Rotates an angle towards another angle with a max distance - @param from_angle The angle to start from - @param to_angle The angle to rotate towards - @param max_angle The maximum angle to rotate - @return The rotated angle - """ - max_degrees = max(0, max_angle) # filter out negative distances - delta_angle = Angle.Abs(to_angle - from_angle) - delta_degrees = delta_angle.InDegrees() - delta_degrees = Float.Clamp(delta_degrees, 0, max_degrees) - if delta_degrees < 0: - delta_degrees = -delta_degrees - return from_angle + Angle.Degrees(delta_degrees) - - @staticmethod - def Cos(angle): - """! Calculates the cosine of an angle - @param angle The given angle - @return The cosine of the angle - """ - return math.cos(angle.InRadians()) - @staticmethod - def Sin(angle): - """! Calculates the sine of an angle - @param angle The given angle - @return The sine of the angle - """ - return math.sin(angle.InRadians()) - @staticmethod - def Tan(angle): - """! Calculates the tangent of an angle - @param angle The given angle - @return The tangent of the angle - """ - return math.tan(angle.InRadians()) - - @staticmethod - def Acos(f): - """! Calculates the arc cosine angle - @param f The value - @return The arc cosine for the given value - """ - return Angle.Radians(math.acos(f)) - @staticmethod - def Asin(f): - """! Calculates the arc sine angle - @param f The value - @return The arc sine for the given value - """ - return Angle.Radians(math.asin(f)) - @staticmethod - def Atan(f): - """! Calculates the arc tangent angle - @param f The value - @return The arc tangent for the given value - """ - return Angle.Radians(math.atan(f)) - def Atan2(y, x): - """! Calculates the tangent for the given values - @param y The vertical value - @param x The horizontal value - @return The tanget for the given values - Uses the y and x signs to compute the quadrant - """ - return Angle.Radians(math.atan2(y, x)) - - @staticmethod - def CosineRuleSide(a, b, gamma): - """! Computes the length of a side of a triangle using the rule of cosines - @param a The length of side A - @param b The length of side B - @param gamma The angle of the corner opposing side C - @return The length of side C - """ - a2: float = a * a - b2: float = b * b - d: float = a2 + b2 - 2 * a * b * Angle.Cos(gamma) - # Catch edge cases where float inaccuracies lead tot NaNs - if d < 0: - return 0 - - c: float = math.sqrt(d) - return c - @staticmethod - def CosineRuleAngle(a, b, c): - """! Computes the angle of a corner of a triangle using the rule of cosines - @param a The length of side A - @param b The length of side B - @param c The length of side C - @return The angle of the corner opposing side C - """ - a2: float = a * a - b2: float = b * b - c2: float = c * c - d: float = (a2 + b2 - c2) / (2 * a * b); - # Catch edge cases where float inaccuracies lead tot NaNs - if d >= 1: - return Angle() - if d <= -1: - return Angle.Degrees(180) - gamma: Angle = Angle.Acos(d) - return gamma; - - @staticmethod - def SineRuleAngle(a, beta, c): - """! Computes the angle of a triangle corner using the rule of sines - @param a The length of side A - @param beta the angle of the corner opposing side B - @param c The length of side C - @return The angle of the corner opposing side A - """ - alpha:Angle = Angle.Asin(a * Angle.Sin(beta) / c); - return alpha; - - -Angle.zero = Angle(0) -## An zero value angle diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/Direction.py b/RoboidControl/LinearAlgebra/LinearAlgebra/Direction.py deleted file mode 100644 index 961e5d5..0000000 --- a/RoboidControl/LinearAlgebra/LinearAlgebra/Direction.py +++ /dev/null @@ -1,114 +0,0 @@ -import math -from .Angle import Angle -from .Vector import Vector3 - -class Direction: - """! A direction using angles - - * The horizontal angle ranging from -180 (inclusive) to 180 (exclusive) - degrees which is a rotation in the horizontal plane - * A vertical angle ranging from -90 (inclusive) to 90 (exclusive) degrees - which is the rotation in the up/down direction applied after the horizontal - rotation has been applied. - The angles are automatically normalized to stay within the abovenmentioned - ranges. - """ - def __init__(self, horizontal=Angle(), vertical=Angle()): - """! Create a new direction - @param horizontal The horizontal angle - @param vertical The vertical angle. - """ - ## horizontal angle, range in degrees = (-180..180] - self.horizontal: Angle = horizontal - ## vertical angle, range in degrees = (-90..90] - self.vertical: Angle = vertical - - self.Normalize() - - @staticmethod - def Degrees(horizontal: float, vertical: float): - """! Create a direction using angle values in degrees - @param horizontal The horizontal angle in degrees - @param vertical The vertical angle in degrees - @return The direction - """ - direction = Direction(Angle.Degrees(horizontal), Angle.Degrees(vertical)) - return direction - @staticmethod - def Radians(horizontal: float, vertical: float): - """! Create a direction using angle values in radians - @param horizontal The horizontal angle in radians - @param vertical The vertical angle in radians - @return The direction - """ - direction = Direction(Angle.Radians(horizontal), Angle.Radians(vertical)) - return direction - - def FromVector3(v: Vector3): - d = Direction( - horizontal = Angle.Atan2(v.right, v.forward), - vertical = Angle.Degrees(-90) - Angle.Acos(v.up) - ) - return d; - def ToVector3(self) -> Vector3: - """! Convert the direction to a Vector3 coordinate - @return The vector coordinate - """ - verticalRad = (math.pi / 2) - self.vertical.InRadians() - horizontalRad = self.horizontal.InRadians() - - cosVertical = math.cos(verticalRad) - sinVertical = math.sin(verticalRad) - cosHorizontal = math.cos(horizontalRad) - sinHorizontal = math.sin(horizontalRad) - - right = sinVertical * sinHorizontal - up = cosVertical - forward = sinVertical * cosHorizontal - return Vector3(right, up, forward) - - def __eq__(self, direction): - """! Test whether this direction is equal to another direction - @param direction The direction to compare to - @return True when the direction angles are equal, false otherwise. - """ - 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 - @return The reversed direction. - """ - h: Angle = self.horizontal + Angle.Degrees(-180) - v: Angle = -self.vertical - return Direction(h, v) - def Inverse(self): - """! This is a synonym for negation - """ - return -self - - def Normalize(self): - """! Normalize this vector to the specified ranges - @note Should not be needed but available in case it is. - """ - v = self.vertical.InDegrees() - deg180 = Angle.Degrees(-180) - if v > 90 or v < -90: - self.horizontal += deg180 - self.vertical = deg180 - self.vertical - - def __repr__(self): - return f"Direction(x={self.horizontal}, y={self.vertical})" - -Direction.zero = Direction.Degrees(0, 0) -Direction.forward = Direction.Degrees(0, 0) -Direction.backward = Direction.Degrees(-180, 0) -Direction.up = Direction.Degrees(0, 90) -Direction.down = Direction.Degrees(0, -90) -Direction.left = Direction.Degrees(-90, 0) -Direction.right = Direction.Degrees(90, 0) \ No newline at end of file diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/Float.py b/RoboidControl/LinearAlgebra/LinearAlgebra/Float.py deleted file mode 100644 index 780f656..0000000 --- a/RoboidControl/LinearAlgebra/LinearAlgebra/Float.py +++ /dev/null @@ -1,13 +0,0 @@ -# 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/. - -class Float: - def Clamp(f, min, max): - if max < min: - return f - if f < min: - return min - if f > max: - return max - return f diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/Quaternion.py b/RoboidControl/LinearAlgebra/LinearAlgebra/Quaternion.py deleted file mode 100644 index 07ae8bd..0000000 --- a/RoboidControl/LinearAlgebra/LinearAlgebra/Quaternion.py +++ /dev/null @@ -1,203 +0,0 @@ -import math - -from .Vector import Vector3 -from .Angle import Angle -from .Direction import Direction - -Deg2Rad = (math.pi * 2) / 360 - -class Quaternion: - - def __init__(self, x: float = 0, y: float = 0, z: float = 0, w: float = 1): - self.x = x - self.y = y - self.z = z - self.w = w - - magnitude: float = self.Magnitude() - self.x /= magnitude - self.y /= magnitude - self.z /= magnitude - self.w /= magnitude - - @staticmethod - def FromAngles(yaw: Angle, pitch: Angle, roll: Angle): - roll_over_2 = roll.InRadians() * 0.5 - sin_roll_over_2 = math.sin(roll_over_2) - cos_roll_over_2 = math.cos(roll_over_2) - - pitch_over_2 = pitch.InRadians() * 0.5 - sin_pitch_over_2 = math.sin(pitch_over_2) - cos_pitch_over_2 = math.cos(pitch_over_2) - - yaw_over_2 = yaw.InRadians() * 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 - - def ToAngles(self): - test: float = self.x * self.y + self.z * self.w; - if test > 0.499: # singularity at north pole - return ( - Angle.zero, - Angle.Radians(2 * math.atan2(self.x, self.w)), - Angle.Degrees(90) - ) - elif test < -0.499: # singularity at south pole - return ( - Angle.zero, - Angle.Radians(-2 * math.atan2(self.x, self.w)), - Angle.Degrees(-90) - ) - else: - sqx: float = self.x * self.x - sqy: float = self.y * self.y - sqz: float = self.z * self.z - - yaw = Angle.Radians(math.atan2(2 * self.y * self.w - 2 * self.x * self.z, 1 - 2 * sqy - 2 * sqz)) - pitch = Angle.Radians(math.atan2(2 * self.x * self.w - 2 * self.y * self.z, 1 - 2 * sqx - 2 * sqz)) - roll = Angle.Radians(math.asin(2 * test)) - return (yaw, pitch, roll) - - def Degrees(yaw: float, pitch: float, roll: float): - return Quaternion.FromAngles( - Angle.Degrees(yaw), - Angle.Degrees(pitch), - Angle.Degrees(roll) - ) - - def Radians(yaw: float, pitch: float, roll: float): - return Quaternion.FromAngles( - Angle.Radians(yaw), - Angle.Radians(pitch), - Angle.Radians(roll) - ) - - def FromAngleAxis(angle: Angle, axis: Direction): - if axis.SqrMagnitude() == 0: - return Quaternion.identity - - result: Quaternion = Quaternion.identity - radians = angle.InRadians() - radians *= 0.5 - - axis2: Vector3 = axis * math.sin(radians); - q = Quaternion( - axis2.right, - axis2.up, - axis2.forward, - math.cos(radians) - ) - return q - - def ToAngleAxis(self) -> tuple[Angle, Direction]: - angle: Angle = Angle.Radians(2 * math.acos(self.w)) - den: float = math.sqrt(1 - self.w * self.w) - if den > 0.0001: - axis = Direction.FromVector3(self.Axis() / den) - else: - # This occurs when the angle is zero. - # Not a problem: just set an arbitrary normalized axis. - axis = Direction.right - return (angle, axis) - - def __eq__(self, other) -> bool: - return ( - self.x == other.x and - self.y == other.y and - 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 - - def Magnitude(self) -> float: - """! The vector length - @return The vector length - """ - return math.sqrt(self.SqrMagnitude()) - - def __mul__(self, other): - if isinstance(other, Quaternion): - return self.MultQuaternion(other) - elif isinstance(other, Vector3): - return self.MultVector(other) - - def MultQuaternion(self, q): - return Quaternion( - self.x * q.w + self.y * q.z - self.z * q.y + self.w * q.x, - -self.x * q.z + self.y * q.w + self.z * q.x + self.w * q.y, - self.x * q.y - self.y * q.x + self.z * q.w + self.w * q.z, - -self.x * q.x - self.y * q.y - self.z * q.z + self.w * q.w - ) - - def MultVector(self, v: Vector3): - num = self.x * 2 - num2 = self.y * 2 - num3 = self.z * 2 - num4 = self.x * num - num5 = self.y * num2 - num6 = self.z * num3 - num7 = self.x * num2 - num8 = self.x * num3 - num9 = self.y * num3 - num10 = self.w * num - num11 = self.w * num2 - num12 = self.w * num3 - - px = v.right - py = v.up - pz = v.forward - rx = (1 - (num5 + num6)) * px + (num7 - num12) * py + (num8 + num11) * pz - ry = (num7 + num12) * px + (1 - (num4 + num6)) * py + (num9 - num10) * pz - rz = (num8 - num11) * px + (num9 + num10) * py + (1 - (num4 + num5)) * pz - result = Vector3(rx, ry, rz) - return result - - def Dot(q1, q2) -> float: - return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w - - def Angle(q1, q2): - f: float = Quaternion.Dot(q1, q2) - angle = Angle.Radians(math.acos(min(math.fabs(f), 1)) * 2) - return angle - # return (float)acos(fmin(fabs(f), 1)) * 2 * Rad2Deg; - - def AngleAround(self, axis: Direction): - secondaryRotation: Quaternion = self.RotationAround(axis); - (rotationAngle, rotationAxis) = secondaryRotation.ToAngleAxis(); - - # Do the axis point in opposite directions? - if Vector3.Dot(axis.ToVector3(), rotationAxis.ToVector3()) < 0: - return -rotationAngle; - - return rotationAngle; - - def RotationAround(self, axis: Direction): - ra = Vector3(self.x, self.y, self.z) # rotation axis - p: Vector3 = Vector3.Project(ra, axis.ToVector3()) # return projection ra on to axis (parallel component) - twist: Quaternion = Quaternion(p.right, p.up, p.forward, self.w) - return twist; - - def Axis(self) -> Vector3: - return Vector3(self.x, self.y, self.z) - -Quaternion.identity = Quaternion() \ No newline at end of file diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/Spherical.py b/RoboidControl/LinearAlgebra/LinearAlgebra/Spherical.py deleted file mode 100644 index a4156f7..0000000 --- a/RoboidControl/LinearAlgebra/LinearAlgebra/Spherical.py +++ /dev/null @@ -1,443 +0,0 @@ -import math - -from .Direction import * -from .Vector import * - -class Polar: - """! A polar 2D vector - """ - - def __init__(self, distance: float, angle: Angle): - """! Create a new polar vector - @param distance The length of the vector - @param angle The direction of the angle - """ - self.distance: float = distance - ## The length of the vector - self.direction: Angle = angle - ## The direction of the vector - - ## Normalizing such that distance >= 0 - if self.distance < 0: - self.distance = -self.distance - self.direction = self.direction.Inverse() - elif self.distance == 0: - self.direction = Angle.zero - - def Degrees(distance: float, degrees: float): - """! Create a polar vector without using the Angle type. All given - angles are in degrees - @param distance The distance in meters - @param horizontal The angle in degrees - @return The polar vector - """ - angle: Angle = Angle.Degrees(degrees) - r: Polar = Polar(distance, angle) - return r - - def Radians(distance: float, radians: float): - """! Create polar vector without using the Angle type. All given - angles are in radians - @param distance The distance in meters - @param horizontal The horizontal angle in radians - @param vertical The vertical angle in radians - @return The polar vector - """ - angle: Angle = Angle.Radians(radians) - r: Polar = Polar(distance, angle) - return r - - @staticmethod - def FromVector2(v: Vector2): - """! Create a polar coordinate from a Vector2 coordinate - @param v The vector coordinate - @return The polar coordinate - """ - distance = v.Magnitude() - if distance == 0: - return Polar(0, Angle.zero) - - angle = Angle.Radians(math.atan2(v.right, v.up)) - return Polar(distance, angle) - - def ToVector2(self) -> Vector2: - """! Convert the polar coordinate to a Vector2 coordinate - @return The vector coordinate - """ - horizontalRad = self.direction.InRadians() - - cosHorizontal = math.cos(horizontalRad) - sinHorizontal = math.sin(horizontalRad) - - right = self.distance * sinHorizontal - up = self.distance * cosHorizontal - return Vector2(right, 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.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) - - def Normalized(self): - if self.distance == 0: - return Polar(0, self.direction) - - return Polar(1, self.direction) - - def __neg__(self): - """! Negate the vector - @return The negated vector - This will negate the direction. Distance will stay the same. - """ - return Polar(self.distance, self.direction.Inverse()) - - def __sub__(self, other): - """! Subtract a polar vector from this vector - @param other The vector to subtract - @return The result of the subtraction - """ - # v1 = self.ToVector2() - # v2 = other.ToVector2() - # r = v1 - v2 - # return Polar.FromVector2(r) - r = self + (-other) - return r - - def __add__(self, other): - """! Add a polar vector to this vector - @param other The vector to add - @return The result of the addition - """ - # v1 = self.ToVector2() - # v2 = other.ToVector2() - # r = v1 - v2 - # return Polar.FromVector2(r) - - if other.distance == 0: - return Polar(self.distance, self.direction) - if self.distance == 0: - return other - - deltaAngle: float = (other.direction - self.direction).InDegrees(); - if deltaAngle < 0: - rotation = 180 + deltaAngle - else: - rotation = 180 - deltaAngle - - if rotation == 180 and other.distance > 0: - # angle is too small, take this angle and add the distances - return Polar(self.distance + other.distance, self.direction) - - newDistance: float = Angle.CosineRuleSide(other.distance, self.distance, Angle.Degrees(rotation)) - - angle: float = Angle.CosineRuleAngle(newDistance, self.distance, other.distance).InDegrees() - if deltaAngle < 0: - new_angle: float = self.direction.InDegrees() - angle - else: - new_angle: float = self.direction.InDegrees() + angle - new_angle_a: Angle = Angle.Degrees(new_angle) - vector = Polar(newDistance, new_angle_a) - return vector - - def __mul__(self, factor): - """! Scale the vector uniformly up - @param factor The scaling factor - @return The scaled vector - @remark This operation will scale the distance of the vector. The angle - will be unaffected. - """ - return Polar( - self.distance * factor, - self.direction - ) - - def __truediv__(self, factor): - """! Scale the vector uniformly down - @param factor The scaling factor - @return The scaled vector - @remark This operation will scale the distance of the vector. The angle - will be unaffected. - """ - return Polar( - self.distance / factor, - self.direction - ) - - @staticmethod - def Distance(v1, v2) -> float: - """! Calculate the distance between two spherical coordinates - @param v1 The first coordinate - @param v2 The second coordinate - @return The distance between the coordinates in meters - """ - v1 = v1.ToVector2() - v2 = v2.ToVector2() - distance: float = Vector2.Distance(v1, v2) - return distance - - @staticmethod - def Angle(v1, v2) -> Angle: - """! Calculate the unsigned angle between two spherical vectors - @param v1 The first vector - @param v2 The second vector - @return The unsigned angle between the vectors [0..179.9999999..., -180] - @remark the strange range is caused by the 2s complement signed values - which has range [minvalue..maxvalue). This is a hardware limitation, - not something we can change. - """ - angle: Angle = Angle.Abs(v1.direction - v2.direction) - return angle - - @staticmethod - def SignedAngle(v1, v2) -> Angle: - """! Calculate the unsigned angle between two spherical vectors - @param v1 The first vector - @param v2 The second vector - @return The unsigned angle between the vectors [0..179.9999999..., -180] - @remark the strange range is caused by the 2s complement signed values - which has range [minvalue..maxvalue). This is a hardware limitation, - not something we can change. - """ - angle: Angle = v2.direction - v1.direction - return angle - - 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 - # return v1 * (1 - f) + v2 * f - -Polar.zero = Polar(0, Angle.zero) - -class Spherical(Polar): - """! A spherical 3D vector - """ - - def __init__(self, distance: float, direction: Direction): - """! Create a new spherical vector - @param distance The length of the vector - @param direction The direction of the vector - """ - self.distance: float = distance - ## The length of the vector - self.direction: Direction = direction - ## The direction of the vector - - ## Normalizing such that distance >= 0 - if self.distance < 0: - self.distance = -self.distance - self.direction = -self.direction - elif self.distance == 0: - self.direction = Direction.zero - - def Degrees(distance: float, horizontal: float, vertical: float): - """! Create sperical vector without using the Direction type. All given - angles are in degrees - @param distance The distance in meters - @param horizontal The horizontal angle in degrees - @param vertical The vertical angle in degrees - @return The spherical vector - """ - direction: Direction = Direction.Degrees(horizontal, vertical) - r: Spherical = Spherical(distance, direction) - return r - - def Radians(distance: float, horizontal: float, vertical: float): - """! Create sperical vector without using the Direction type. All given - angles are in radians - @param distance The distance in meters - @param horizontal The horizontal angle in radians - @param vertical The vertical angle in radians - @return The spherical vector - """ - direction: Direction = Direction.Radians(horizontal, vertical) - r: Spherical = Spherical(distance, direction) - return r - - @staticmethod - def FromVector3(v: Vector3): - """! Create a Spherical coordinate from a Vector3 coordinate - @param v The vector coordinate - @return The spherical coordinate - """ - distance = v.Magnitude() - if distance == 0: - return Spherical(0, Angle(), Angle()) - - verticalAngle = Angle.Radians((math.pi / 2 - math.acos(v.up / distance))) - horizontalAngle = Angle.Radians(math.atan2(v.right, v.forward)) - return Spherical(distance, Direction(horizontalAngle, verticalAngle)) - - def ToVector3(self) -> Vector3: - """! Convert the spherical coordinate to a Vector3 coordinate - @return The vector coordinate - """ - verticalRad = (math.pi / 2) - self.direction.vertical.InRadians() - horizontalRad = self.direction.horizontal.InRadians() - - cosVertical = math.cos(verticalRad) - sinVertical = math.sin(verticalRad) - cosHorizontal = math.cos(horizontalRad) - sinHorizontal = math.sin(horizontalRad) - - right = self.distance * sinVertical * sinHorizontal - up = self.distance * cosVertical - forward = self.distance * sinVertical * cosHorizontal - return Vector3(right, up, 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.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: - return Spherical(0, self.direction) - - return Spherical(1, self.direction) - - def __neg__(self): - """! Negate the vector - @return The negated vector - This will negate the direction. Distance will stay the same. - """ - return Spherical(self.distance, self.direction.Inverse()) - - def __sub__(self, other): - """! Subtract a spherical vector from this vector - @param other The vector to subtract - @return The result of the subtraction - """ - v1 = self.ToVector3() - v2 = other.ToVector3() - r = v1 - v2 - return Spherical.FromVector3(r) - - def __add__(self, other): - """! Add a spherical vector to this vector - @param other The vector to add - @return The result of the addition - """ - v1 = self.ToVector3() - v2 = other.ToVector3() - r = v1 + v2 - return Spherical.FromVector3(r) - - def __mul__(self, factor): - """! Scale the vector uniformly up - @param factor The scaling factor - @return The scaled vector - @remark This operation will scale the distance of the vector. The angle - will be unaffected. - """ - return Spherical( - self.distance * factor, - self.direction - ) - - def __truediv__(self, factor): - """! Scale the vector uniformly down - @param factor The scaling factor - @return The scaled vector - @remark This operation will scale the distance of the vector. The angle - will be unaffected. - """ - return Spherical( - self.distance / factor, - self.direction - ) - - @staticmethod - def Distance(v1, v2) -> float: - """! Calculate the distance between two spherical coordinates - @param s1 The first coordinate - @param s2 The second coordinate - @return The distance between the coordinates in meters - """ - v1: Vector3 = v1.ToVector3() - v2: Vector3 = v2.ToVector3() - distance: float = Vector3.Distance(v1, v2) - return distance - - @staticmethod - def Angle(s1, s2) -> Angle: - """! Calculate the unsigned angle between two spherical vectors - @param s1 The first vector - @param s2 The second vector - @return The unsigned angle between the vectors [0..179.9999999..., -180] - @remark the strange range is caused by the 2s complement signed values - which has range [minvalue..maxvalue). This is a hardware limitation, - not something we can change. - """ - v1: Vector3 = s1.ToVector3() - v2: Vector3 = s2.ToVector3() - angle: Angle = Vector3.Angle(v1, v2) - return angle - - def SignedAngle(s1, s2, axis) -> Angle: - """! Calculate the signed angle between two spherical vectors - @param s1 The first vector - @param s2 The second vector - @param axis The axis around which the angle is calculated - @return The signed angle between the vectors - """ - v1 = s1.ToVector3() - v2 = s2.ToVector3() - v_axis = axis.ToVector3() - angle = Vector3.SignedAngle(v1, v2, v_axis) - return angle - - @staticmethod - def Rotate(s, horizontal: Angle, vertical: Angle): - """! Rotate a spherical vector - @param s The vector to rotate - @param horizontal The horizontal rotation angle in local space - @param vertical The vertical rotation angle in local space - @return The rotated vector - """ - direction: Direction = Direction( - s.direction.horizontal + horizontal, - s.direction.vertical + vertical - ) - r = Spherical(s.distance, direction) - return r - - def __repr__(self): - return f"Spherical(r={self.distance}, horizontal={self.direction.horizontal}, phi={self.direction.vertical})" - -Spherical.zero = Spherical(0, Direction.zero) diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/SwingTwist.py b/RoboidControl/LinearAlgebra/LinearAlgebra/SwingTwist.py deleted file mode 100644 index 5b1a126..0000000 --- a/RoboidControl/LinearAlgebra/LinearAlgebra/SwingTwist.py +++ /dev/null @@ -1,95 +0,0 @@ -from .Direction import * -from .Quaternion import * - -class SwingTwist: - """A rotation using swing and twist angle components""" - def __init__(self, swing: Direction, twist: Angle): - ## 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): - horizontal_angle = Angle.Degrees(horizontal) - vertical_angle = Angle.Degrees(vertical) - twist_angle = Angle.Degrees(twist) - - deg90 = Angle.Degrees(90) - deg180 = Angle.Degrees(180) - if vertical_angle > deg90 or vertical_angle < -deg90: - horizontal_angle += deg180 - vertical_angle = deg180 - vertical_angle - twist_angle += deg180 - - direction = Direction(horizontal_angle, vertical_angle) - swing_twist = SwingTwist(direction, twist_angle) - return swing_twist - @staticmethod - def Radians(horizontal: float, vertical: float, twist: float): - horizontal_angle = Angle.Radians(horizontal) - vertical_angle = Angle.Radians(vertical) - twist_angle = Angle.Radians(twist) - - deg90 = Angle.Radians(math.pi / 2) - deg180 = Angle.Radians(math.pi) - if vertical_angle > deg90 or vertical_angle < -deg90: - horizontal_angle += deg180 - vertical_angle = deg180 - vertical_angle - twist_angle += deg180 - - direction = Direction(horizontal_angle, vertical_angle) - swing_twist = SwingTwist(direction, twist_angle) - return swing_twist - - def ToQuaternion(self) -> Quaternion: - """Convert the SwingTwist rotation to a Quaternion""" - q = Quaternion.FromAngles( - -self.swing.vertical, - self.swing.horizontal, - self.twist - ) - return q - @staticmethod - def FromQuaternion(q: Quaternion): - angles = Quaternion.ToAngles(q) - # direction = Direction(angles[0], angles[1]) - # r: SwingTwist = SwingTwist(direction, angles[2]) - r = SwingTwist.Degrees( - angles[0].InDegrees(), - angles[1].InDegrees(), - angles[2].InDegrees() - ) - return r - - def FromAngleAxis(angle: Angle, axis: Direction): - vectorAxis: Vector3 = axis.ToVector3(); - q: Quaternion = Quaternion.FromAngleAxis(angle, vectorAxis); - return SwingTwist.FromQuaternion(q) - - - def __eq__(self, other) -> bool: - """! Check if this orientation is equal to the given orientation - @param other The orientation to check against - @return true if it is identical to the given orientation - @note This uses float comparison to check equality which may have strange - effects. Equality on floats should be avoided. - """ - 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: - q1: Quaternion = r1.ToQuaternion() - q2: Quaternion = r2.ToQuaternion() - angle: float = Quaternion.Angle(q1, q2) - return angle - \ No newline at end of file diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/Vector.py b/RoboidControl/LinearAlgebra/LinearAlgebra/Vector.py deleted file mode 100644 index 22f95ba..0000000 --- a/RoboidControl/LinearAlgebra/LinearAlgebra/Vector.py +++ /dev/null @@ -1,416 +0,0 @@ -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) diff --git a/RoboidControl/LinearAlgebra/LinearAlgebra/__init__.py b/RoboidControl/LinearAlgebra/LinearAlgebra/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/RoboidControl/LinearAlgebra/README.md b/RoboidControl/LinearAlgebra/README.md deleted file mode 100644 index c2ae159..0000000 --- a/RoboidControl/LinearAlgebra/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# LinearAlgebra-python - diff --git a/RoboidControl/LinearAlgebra/setup.py b/RoboidControl/LinearAlgebra/setup.py deleted file mode 100644 index e13bcb3..0000000 --- a/RoboidControl/LinearAlgebra/setup.py +++ /dev/null @@ -1,22 +0,0 @@ -from setuptools import setup, find_packages - -setup( - name='LinearAlgebra', - version='0.3.0', - packages=find_packages(), - install_requires=[ - # List your library dependencies here - ], - author='Your Name', - author_email='your.email@example.com', - description='A brief description of your library', - long_description=open('README.md').read(), - long_description_content_type='text/markdown', - url='https://github.com/yourusername/your_library', - classifiers=[ - 'Programming Language :: Python :: 3', - 'License :: OSI Approved :: MPL2.0 License', - 'Operating System :: OS Independent', - ], - python_requires='>=3.6', -) diff --git a/RoboidControl/LinearAlgebra/test/Angle_test.py b/RoboidControl/LinearAlgebra/test/Angle_test.py deleted file mode 100644 index 82b92e8..0000000 --- a/RoboidControl/LinearAlgebra/test/Angle_test.py +++ /dev/null @@ -1,140 +0,0 @@ -import unittest - -from LinearAlgebra.Angle import * - -class AngleTest(unittest.TestCase): - def test_Construct(self): - degrees: float = 0 - a: Angle = Angle.Degrees(degrees) - assert(a.InDegrees() == degrees) - - degrees = -180 - a = Angle.Degrees(degrees) - assert(a.InDegrees() == degrees) - - degrees = 270 - a = Angle.Degrees(degrees) - assert(a.InDegrees() == -90.0) - - def test_Negate(self): - angle = 0 - a:Angle = Angle.Degrees(angle) - a = -a - assert(a.InDegrees() == angle) - - angle = 90 - a = Angle.Degrees(angle) - a = -a - assert(a.InDegrees() == -angle) - - def test_Add(self): - a: Angle = Angle.Degrees(-45) - b: Angle = Angle.Degrees(45) - r: Angle = a + b - assert(r.InDegrees() == 0) - - def test_Subtract(self): - a: Angle = Angle.Degrees(0) - b: Angle = Angle.Degrees(45) - r: Angle = a - b - assert(r.InDegrees() == -45) - - def test_Compare(self): - a: Angle = Angle.Degrees(45) - r: bool = False - - r = a > Angle.Degrees(0) - assert(r == True) - - r = a > Angle.Degrees(90) - assert(r == False) - - r = a > Angle.Degrees(-90) - assert(r == True) - - def test_Normalize(self): - r = Angle() - - r = Angle.Normalize(Angle.Degrees(90)) - assert(r.InDegrees() == 90) - - r = Angle.Normalize(Angle.Degrees(-90)) - assert(r.InDegrees() == -90) - - r = Angle.Normalize(Angle.Degrees(270)) - assert(r.InDegrees() == -90) - - r = Angle.Normalize(Angle.Degrees(270 + 360)) - assert(r.InDegrees() == -90) - - r = Angle.Normalize(Angle.Degrees(-270)); - assert(r.InDegrees() == 90) - - r = Angle.Normalize(Angle.Degrees(-270 - 360)); - assert(r.InDegrees() == 90) - - r = Angle.Normalize(Angle.Degrees(0)); - assert(r.InDegrees() == 0) - - def test_Clamp(self): - r = Angle() - - r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(2)) - assert(r.InDegrees() == 1) - - r = Angle.Clamp(Angle.Degrees(-1), Angle.Degrees(0), Angle.Degrees(2)) - assert(r.InDegrees() == 0) - - r = Angle.Clamp(Angle.Degrees(3), Angle.Degrees(0), Angle.Degrees(2)) - assert(r.InDegrees() == 2) - - r = Angle.Clamp(Angle.Degrees(1), Angle.Degrees(0), Angle.Degrees(0)) - assert(r.InDegrees() == 0) - - r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(0), Angle.Degrees(0)) - assert(r.InDegrees() == 0) - - r = Angle.Clamp(Angle.Degrees(0), Angle.Degrees(1), Angle.Degrees(-1)) - assert(r.InDegrees() == 0) - - def test_MoveTowards(self): - r = Angle(); - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 30) - assert(r.InDegrees() == 30) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 90) - assert(r.InDegrees() == 90) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 180) - assert(r.InDegrees() == 90) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 270) - assert(r.InDegrees() == 90) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), -30) - assert(r.InDegrees() == 0) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -30) - assert(r.InDegrees() == 0) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -90) - assert(r.InDegrees() == 0) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -180) - assert(r.InDegrees() == 0) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(-90), -270) - assert(r.InDegrees() == 0) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(90), 0) - assert(r.InDegrees() == 0) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 0) - assert(r.InDegrees() == 0) - - r = Angle.MoveTowards(Angle.Degrees(0), Angle.Degrees(0), 30) - assert(r.InDegrees() == 0) - -if __name__ == '__main__': - unittest.main() \ No newline at end of file diff --git a/RoboidControl/LinearAlgebra/test/Direction_test.py b/RoboidControl/LinearAlgebra/test/Direction_test.py deleted file mode 100644 index 9fd3a8d..0000000 --- a/RoboidControl/LinearAlgebra/test/Direction_test.py +++ /dev/null @@ -1,42 +0,0 @@ -import unittest - -from LinearAlgebra.Direction import * - -class DirectionTest(unittest.TestCase): - def test_Compare(self): - d = Direction.Degrees(45, 135) - r = Direction(Angle.Degrees(45), Angle.Degrees(135)) - assert(d == r) - - r = Direction(Angle.Degrees(45 + 360), Angle.Degrees(135 - 360)) - assert (d == r) - - def test_Inverse(self): - d = Direction.Degrees(45, 135) - r = Direction.Degrees(-135, -135) - assert(-d == r) - - d = Direction.Degrees(-45, -135) - r = Direction.Degrees(135, 135) - assert(-d == r) - - d = Direction.Degrees(0, 0) - r = Direction.Degrees(180, 0) - assert(-d == r) - - d = Direction.Degrees(0, 45) - r = Direction.Degrees(180, -45) - assert(-d == r) - - def test_Equality(self): - d = Direction.Degrees(135, 45) - r = Direction.Degrees(135, 45) - assert(d == r) - r = Direction.Degrees(135 + 360, 45) - assert(d == r) - r = Direction.Degrees(135 - 360, 45) - assert(d == r) - - d = Direction.Degrees(0, 45 + 180); - r = Direction.Degrees(180, -45) - assert(d == r) diff --git a/RoboidControl/LinearAlgebra/test/Float_test.py b/RoboidControl/LinearAlgebra/test/Float_test.py deleted file mode 100644 index b243a1f..0000000 --- a/RoboidControl/LinearAlgebra/test/Float_test.py +++ /dev/null @@ -1,23 +0,0 @@ -import unittest - -from LinearAlgebra.Float import * - -class FloatTest(unittest.TestCase): - def test_Clamp(self): - r = Float.Clamp(1, 0, 2) - assert(r == 1) - - r = Float.Clamp(-1, 0, 2) - assert(r == 0) - - r = Float.Clamp(3, 0, 2) - assert(r == 2) - - r = Float.Clamp(1, 0, 0) - assert(r == 0) - - r = Float.Clamp(0, 0, 0) - assert(r == 0) - - r = Float.Clamp(0, 1, -1) - assert(r == 0) diff --git a/RoboidControl/LinearAlgebra/test/Quaternion_test.py b/RoboidControl/LinearAlgebra/test/Quaternion_test.py deleted file mode 100644 index 2a98067..0000000 --- a/RoboidControl/LinearAlgebra/test/Quaternion_test.py +++ /dev/null @@ -1,104 +0,0 @@ -import unittest - -from LinearAlgebra.Quaternion import * - -class QuaternionTest(unittest.TestCase): - def test_Equality(self): - q1 = Quaternion.identity - q2 = Quaternion(1, 0, 0, 0) - - assert(q1 != q2) - - q2 = Quaternion(0, 0, 0, 1) - assert(q1 == q2) - - def test_FromAngles(self): - q = Quaternion.FromAngles(Angle.zero, Angle.zero, Angle.zero) - assert(q == Quaternion.identity) - - q = Quaternion.FromAngles(Angle.Degrees(90), Angle.Degrees(90), Angle.Degrees(-90)) - sqrt2_2 = math.sqrt(2) / 2 - assert(Quaternion.isclose(q, Quaternion(0, sqrt2_2, -sqrt2_2, 0))) - - def test_ToAngles(self): - q1 = Quaternion.identity - angles = Quaternion.ToAngles(q1) - assert(angles == (Angle.zero, Angle.zero, Angle.zero)) - - q1 = Quaternion(1, 0, 0, 0) - angles = Quaternion.ToAngles(q1) - assert(angles == (Angle.zero, Angle.Degrees(180), Angle.zero)) - - def test_Degrees(self): - q = Quaternion.Degrees(0, 0, 0) - assert(q == Quaternion.identity) - - q = Quaternion.Degrees(90, 90, -90) - sqrt2_2 = math.sqrt(2) / 2 - 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) - assert(q == Quaternion.identity) - - q = Quaternion.Radians(math.pi / 2, math.pi / 2, -math.pi / 2) - sqrt2_2 = math.sqrt(2) / 2 - 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 - q2 = Quaternion(1, 0, 0, 0) - - r = q1 * q2; - assert(r == Quaternion(1, 0, 0, 0)) - - def test_MultiplyVector(self): - v1 = Vector3.up - - q1 = Quaternion.identity - v = q1 * v1 - assert(v == Vector3(0, 1, 0)) - - q1 = Quaternion(1, 0, 0, 0) - v = q1 * v1 - assert(v == Vector3(0, -1, 0)) - - def test_Angle(self): - q1 = Quaternion.identity - q2 = Quaternion.identity - r = Quaternion.Angle(q1, q2) - assert(r == Angle.zero) - - def test_AngleAround(self): - axis = Direction.up - q1 = Quaternion.identity - - angle = q1.AngleAround(axis) - assert(angle == Angle.Degrees(0)) - - sqrt2_2 = math.sqrt(2) / 2 - q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0) - angle = q1.AngleAround(axis) - assert(angle == Angle.Degrees(180)) - - axis = Direction.zero - angle = q1.AngleAround(axis) - assert(math.isnan(angle.InDegrees())) - - def test_RotationAround(self): - axis = Direction.up - q1 = Quaternion.identity - - q = q1.RotationAround(axis) - assert(q == Quaternion.identity) - - sqrt2_2 = math.sqrt(2) / 2 - q1 = Quaternion(0, sqrt2_2, -sqrt2_2, 0) - q = q1.RotationAround(axis) - assert(q == Quaternion(0, 1, 0, 0)) - - axis = Direction.zero - q = q1.RotationAround(axis); - assert(math.isnan(q.x) and math.isnan(q.y) and math.isnan(q.z) and math.isnan(q.w)) diff --git a/RoboidControl/LinearAlgebra/test/Spherical_test.py b/RoboidControl/LinearAlgebra/test/Spherical_test.py deleted file mode 100644 index 762e981..0000000 --- a/RoboidControl/LinearAlgebra/test/Spherical_test.py +++ /dev/null @@ -1,438 +0,0 @@ -import unittest - -from LinearAlgebra.Spherical import * - -class PolarTest(unittest.TestCase): - def test_FromVector2(self): - v: Vector2 = Vector2(0, 1) - p: Polar = Polar.FromVector2(v) - - assert(p.distance == 1) - assert(p.direction.InDegrees() == 0) - - v = Vector2(1, 0) - p = Polar.FromVector2(v) - - assert(p.distance, 1) - assert(p.direction.InDegrees(), 90) - - v = Vector2(-1, 1) - p = Polar.FromVector2(v) - - assert(p.distance == math.sqrt(2)) - assert(p.direction.InDegrees() == -45) - - def test_Equality(self): - v1: Polar = Polar.Degrees(4, 5) - - v2: Polar = Polar.Degrees(1, 2) - assert(v1 != v2) - - v2 = Polar.Degrees(4, 5) - assert(v1 == v2) - - def test_Magnitude(self): - v: Polar = Polar.Degrees(2, 30) - r: float = 0 - - r = v.Magnitude() - assert(r == 2) - - v = Polar.Degrees(-2, -30) - r = v.Magnitude() - assert(r == 2) - - v = Polar.Degrees(0, 0) - r = v.Magnitude() - assert(r == 0) - - def test_Normalize(self): - v1: Polar = Polar.Degrees(2, 90) - r: Polar = Polar.zero - - r = v1.Normalized() - assert(r == Polar.Degrees(1, 90)) - - v1 = Polar.Degrees(2, -90) - r = v1.Normalized() - assert(r == Polar.Degrees(1, -90)) - - v1 = Polar.Degrees(0, 0) - r = v1.Normalized() - assert(r == Polar.Degrees(0, 0)) - - def test_Negate(self): - v1: Polar = Polar.Degrees(2, 45) - r: Polar = Polar.zero - - r = -v1 - assert(r == Polar.Degrees(2, -135)) - - v1 = Polar.Degrees(2, -45) - r = -v1 - assert(r == Polar.Degrees(2, 135)) - - v1 = Polar.Degrees(2, 0) - r = -v1 - assert(r == Polar.Degrees(2, 180)) - - v1 = Polar.Degrees(0, 0) - r = -v1 - assert(r == Polar.Degrees(0, 0)) - - def test_Subtract(self): - r: Polar = Polar.zero - v1: Polar = Polar.Degrees(4, 45) - - v2: Polar = Polar.zero - r = v1 - v2 - assert(r == v1) - - r = v1 - r -= v2 - assert(r == v1) - - v2: Polar = Polar.Degrees(1, 45) - r = v1 - v2 - assert(r == Polar.Degrees(3, 45)) - - v2: Polar = Polar.Degrees(1, -135) - r = v1 - v2 - assert(r == Polar.Degrees(5, 45)) - - def test_Addition(self): - r = Polar.zero - - v1 = Polar.Degrees(4, 45) - v2 = Polar.zero - r = v1 + v2 - assert(r == v1) - - r = v1 - r += v2 - assert(r == v1) - - v2 = Polar.Degrees(1, 45) - r = v1 + v2 - assert(r == Polar.Degrees(5, 45)) - - v2 = Polar.Degrees(1, -135) - r = v1 + v2 - assert(r == Polar.Degrees(3, 45)) - - def test_Multiply(self): - r: Polar = Polar.zero - - v1: Polar = Polar.Degrees(4, 45) - r = v1 * 2 - assert(r == Polar.Degrees(8, 45)) - - r = v1 * -2 - assert(r == Polar.Degrees(8, -135)) - - r = v1 * 0 - assert(r == Polar.Degrees(0, 0)) - - def test_Divide(self): - r: Polar.zero - - v1 = Polar.Degrees(4, 45) - r = v1 / 2 - assert(r == Polar.Degrees(2, 45)) - - r = v1 / -2 - assert(r == Polar.Degrees(2, -135)) - - def test_Distance(self): - r: float = 0 - - v1 = Polar.Degrees(4, 45) - v2 = Polar.Degrees(1, -135) - r = Polar.Distance(v1, v2) - assert(r == 5) - - v2 = Polar.Degrees(-1, -135) - r = Polar.Distance(v1, v2) - assert(math.isclose(r, 3)) - - v2 = Polar.Degrees(0, 0) - r = Polar.Distance(v1, v2) - assert(r == 4) - - def test_Angle(self): - r = Angle.zero - - v1 = Polar.Degrees(4, 45) - v2 = Polar.Degrees(1, -45) - - r = Polar.Angle(v1, v2) - assert(r.InDegrees() == 90) - - v2 = Polar.Degrees(1, 135) - r = Polar.Angle(v1, v2) - assert(r.InDegrees() == 90) - - v2 = Polar.Degrees(1, 45) - r = Polar.Angle(v1, v2) - assert(r.InDegrees() == 0) - - def test_SignedAngle(self): - r = Angle.zero - - v1 = Polar.Degrees(4, 45) - v2 = Polar.Degrees(1, -45) - - r = Polar.SignedAngle(v1, v2) - assert(r.InDegrees() == -90) - - v2 = Polar.Degrees(1, 135) - r = Polar.SignedAngle(v1, v2) - assert(r.InDegrees() == 90) - - v2 = Polar.Degrees(1, 45) - r = Polar.SignedAngle(v1, v2) - assert(r.InDegrees() == 0) - - def test_Lerp(self): - r = Polar.zero - - v1 = Polar.Degrees(5, 45) - v2 = Polar.Degrees(1, -45) - - r = Polar.Lerp(v1, v2, 0) - assert(r == v1) - - r = Polar.Lerp(v1, v2, 1) - assert(Polar.isclose(r, v2)) - - r = Polar.Lerp(v1, v2, 0.5) - assert(Polar.isclose(r, Polar.Degrees(3, 0))) - - r = Polar.Lerp(v1, v2, -1) - assert(r == Polar.Degrees(9, 135)) - - r = Polar.Lerp(v1, v2, 2) - assert(r == Polar.Degrees(-3, -135)) - -class SphericalTest(unittest.TestCase): - def test_FromVector3(self): - v: Vector3 = Vector3(0, 0, 1) - p: Spherical = Spherical.FromVector3(v) - - assert(p.distance == 1) - assert(p.direction.horizontal.InDegrees() == 0) - assert(p.direction.vertical.InDegrees() == 0) - - v = Vector3(1, 0, 0) - p = Spherical.FromVector3(v) - - assert(p.distance, 1) - assert(p.direction.horizontal.InDegrees(), 90) - assert(p.direction.vertical.InDegrees(), 0) - - v = Vector3(0, 1, 0) - p = Spherical.FromVector3(v) - - assert(p.distance, 1) - assert(p.direction.horizontal.InDegrees(), 0) - assert(p.direction.vertical.InDegrees(), 90) - - v = Vector3(-1, 0, 1) - p = Spherical.FromVector3(v) - - assert(p.distance == math.sqrt(2)) - assert(p.direction.horizontal.InDegrees() == -45) - assert(p.direction.vertical.InDegrees() == 0) - - def test_Equality(self): - v1: Spherical = Spherical.Degrees(4, 5, 6) - - v2: Spherical = Spherical.Degrees(1, 2, 3) - assert(v1 != v2) - - v2 = Spherical.Degrees(4, 5, 6) - assert(v1 == v2) - - def test_Magnitude(self): - v: Spherical = Spherical.Degrees(2, 30, 0) - r: float = 0 - - r = v.Magnitude() - assert(r == 2) - - v = Spherical.Degrees(-2, -30, 0) - r = v.Magnitude() - assert(r == 2) - - v = Spherical.Degrees(0, 0, 0) - r = v.Magnitude() - assert(r == 0) - - def test_Normalize(self): - v1: Spherical = Spherical.Degrees(2, 90, 0) - r: Spherical = Spherical.zero - - r = v1.Normalized() - assert(r == Spherical.Degrees(1, 90, 0)) - - v1 = Spherical.Degrees(2, -90, 0) - r = v1.Normalized() - assert(r == Spherical.Degrees(1, -90, 0)) - - v1 = Spherical.Degrees(0, 0, 0) - r = v1.Normalized() - assert(r == Spherical.Degrees(0, 0, 0)) - - def test_Negate(self): - v1: Spherical = Spherical.Degrees(2, 45, 0) - r: Spherical = Spherical.zero - - r = -v1 - assert(r == Spherical.Degrees(2, -135, 0)) - - v1 = Spherical.Degrees(2, -45, 0) - r = -v1 - assert(r == Spherical.Degrees(2, 135, 0)) - - v1 = Spherical.Degrees(0, 0, 0) - r = -v1 - assert(r == Spherical.Degrees(0, 180, 0)) - - def test_Subtract(self): - r: Spherical = Spherical.zero - - v1: Spherical = Spherical.Degrees(4, 45, 0) - v2: Spherical = Spherical.zero - r = v1 - v2 - assert(r == v1) - - r = v1 - r -= v2 - assert(r == v1) - - v2 = Spherical.Degrees(1, 45, 0) - r = v1 - v2 - assert(Spherical.isclose(r, Spherical.Degrees(3, 45, 0))) - - v2 = Spherical.Degrees(1, -135, 0) - r = v1 - v2 - assert(r == Spherical.Degrees(5, 45, 0)) - - def test_Addition(self): - v1 = Spherical(1, Direction.Degrees(45, 0)) - v2 = Spherical.zero - r = Spherical.zero - - r = v1 + v2 - assert(r.distance == v1.distance) - - r = v1 - r += v2 - assert(r.distance == v1.distance) - - v2 = Spherical(1, Direction.Degrees(-45, 0)) - r = v1 + v2 - 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(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 - - v1 = Spherical.Degrees(4, 45, 0) - r = v1 * 3 - assert(r == Spherical.Degrees(12, 45, 0)) - - r = v1 * -3 - assert(r == Spherical.Degrees(12, -135, 0)) - - r = v1 * 0 - assert(r == Spherical.Degrees(0, 0, 0)) - - def test_Divide(self): - r: Spherical.zero - - v1 = Spherical.Degrees(4, 45, 0) - r = v1 / 2 - assert(r == Spherical.Degrees(2, 45, 0)) - - r = v1 / -2 - assert(r == Spherical.Degrees(2, -135, 0)) - - def test_Distance(self): - r: float = 0 - - v1 = Spherical.Degrees(4, 45, 0) - v2 = Spherical.Degrees(1, -135, 0) - r = Spherical.Distance(v1, v2) - assert(r == 5) - - v2 = Spherical.Degrees(-1, -135, 0) - r = Spherical.Distance(v1, v2) - assert(math.isclose(r, 3)) - - v2 = Spherical.Degrees(0, 0, 0) - r = Spherical.Distance(v1, v2) - assert(r == 4) - - def test_Angle(self): - r = Angle.zero - - v1 = Spherical.Degrees(4, 45, 0) - v2 = Spherical.Degrees(1, -45, 0) - - r = Spherical.Angle(v1, v2) - assert(r.InDegrees() == 90) - - v2 = Spherical.Degrees(1, 135, 0) - r = Spherical.Angle(v1, v2) - assert(r.InDegrees() == 90) - - v2 = Spherical.Degrees(1, 45, 0) - r = Spherical.Angle(v1, v2) - assert(r.InDegrees() == 0) - - def test_SignedAngle(self): - r = Angle.zero - - v1 = Spherical.Degrees(4, 45, 0) - v2 = Spherical.Degrees(1, -45, 0) - - r = Spherical.SignedAngle(v1, v2, Direction.up) - assert(r.InDegrees() == -90) - - v2 = Spherical.Degrees(1, 135, 0) - r = Spherical.SignedAngle(v1, v2, Direction.up) - assert(r.InDegrees() == 90) - - v2 = Spherical.Degrees(1, 45, 0) - r = Spherical.SignedAngle(v1, v2, Direction.up) - assert(r.InDegrees() == 0) - - def test_Lerp(self): - r = Spherical.zero - - v1 = Spherical.Degrees(5, 45, 0) - v2 = Spherical.Degrees(1, -45, 0) - - r = Spherical.Lerp(v1, v2, 0) - assert(r == v1) - - r = Spherical.Lerp(v1, v2, 1) - assert(Spherical.isclose(r, v2)) - - r = Spherical.Lerp(v1, v2, 0.5) - assert(Spherical.isclose(r, Spherical.Degrees(3, 0, 0))) - - r = Spherical.Lerp(v1, v2, -1) - assert(r == Spherical.Degrees(9, 135, 0)) - - r = Spherical.Lerp(v1, v2, 2) - assert(r == Spherical.Degrees(-3, -135, 0)) - diff --git a/RoboidControl/LinearAlgebra/test/SwingTwist_test.py b/RoboidControl/LinearAlgebra/test/SwingTwist_test.py deleted file mode 100644 index ee6c05a..0000000 --- a/RoboidControl/LinearAlgebra/test/SwingTwist_test.py +++ /dev/null @@ -1,95 +0,0 @@ -import unittest - -from LinearAlgebra.SwingTwist import * - -class SwingTwistTest(unittest.TestCase): - def test_Constructor(self): - s = SwingTwist.Degrees(0, 0, 0) - assert(s == SwingTwist.Degrees(0, 0, 0)) - - s = SwingTwist.Degrees(0, 180, 0) - assert(s == SwingTwist.Degrees(180, 0, 180)) - - s = SwingTwist.Degrees(0, 180, 180) - assert(s == SwingTwist.Degrees(180, 0, 0)) - - s = SwingTwist.Degrees(270, 90, 0) - assert(s == SwingTwist.Degrees(-90, 90, 0)) - - s = SwingTwist.Degrees(270, 270, 0) - assert(s == SwingTwist.Degrees(-90, -90, 0)) - - s = SwingTwist.Degrees(270, 225, 0) - assert(s == SwingTwist.Degrees(90, -45, -180)) - - s = SwingTwist.Degrees(270, 0, 225) - assert(s == SwingTwist.Degrees(-90, 0, -135)) - - def test_FromQuaternion(self): - q = Quaternion.identity - r = SwingTwist.FromQuaternion(q) - assert(r == SwingTwist.Degrees(0, 0, 0)) - - q = Quaternion.Degrees(90, 0, 0) - r = SwingTwist.FromQuaternion(q) - assert(SwingTwist.isclose(r, SwingTwist.Degrees(90, 0, 0))) - - q = Quaternion.Degrees(0, 90, 0) - r = SwingTwist.FromQuaternion(q) - assert(r == SwingTwist.Degrees(0, 90, 0)) - - q = Quaternion.Degrees(0, 0, 90) - r = SwingTwist.FromQuaternion(q) - assert(r == SwingTwist.Degrees(0, 0, 90)) - - q = Quaternion.Degrees(0, 180, 0) - r = SwingTwist.FromQuaternion(q) - assert(r == SwingTwist.Degrees(0, 180, 0)) - - q = Quaternion.Degrees(0, 135, 0) - r = SwingTwist.FromQuaternion(q) - assert(r == SwingTwist.Degrees(0, 135, 0)) - - def test_FromAngleAxis(self): - r = SwingTwist.FromAngleAxis(Angle.zero, Direction.up) - assert(r == SwingTwist.Degrees(0, 0, 0)) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.up) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(90, 0, 0)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.up) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(180, 0, 0)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.up); - angle = SwingTwist.Angle(r, SwingTwist.Degrees(-90, 0, 0)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.right) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 90, 0)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.right) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 180, 0)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.right) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, -90, 0)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(90), Direction.forward) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 90)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(180), Direction.forward) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, 180)) - assert(angle.InDegrees() == 0) - - r = SwingTwist.FromAngleAxis(Angle.Degrees(270), Direction.forward) - angle = SwingTwist.Angle(r, SwingTwist.Degrees(0, 0, -90)) - assert(angle.InDegrees() == 0) - - # auto r16 = SwingTwist16::AngleAxis(13, Direction16::down); - # auto s16 = SwingTwist16::Degrees(-13, 0, 0); - # assert(SwingTwist16::Angle(r16, s16), Angle16::Degrees(0)) diff --git a/RoboidControl/LinearAlgebra/test/Vector_test.py b/RoboidControl/LinearAlgebra/test/Vector_test.py deleted file mode 100644 index 91b359e..0000000 --- a/RoboidControl/LinearAlgebra/test/Vector_test.py +++ /dev/null @@ -1,547 +0,0 @@ -import unittest - -from LinearAlgebra.Vector import * - -class Vector2Test(unittest.TestCase): - def test_Equality(self): - v1: Vector2 = Vector2(4, 5) - - v2: Vector2 = Vector2(1, 2) - assert(v1 != v2) - - v2 = Vector2(4, 5) - assert(v1 == v2) - - def test_SqrMagnitude(self): - v: Vector2 = Vector2(1, 2) - m: float = 0; - - m = v.SqrMagnitude() - assert(m == 5) - - v = Vector2(-1, -2) - m = v.SqrMagnitude() - assert(m == 5) - - v = Vector2(0, 0) - m = v.SqrMagnitude() - assert(m == 0) - - def test_Magnitude(self): - v: Vector2 = Vector2(1, 2) - m: float = 0; - - m = v.Magnitude() - assert(m == 2.23606797749979) - - v = Vector2(-1, -2) - m = v.Magnitude() - assert(m == 2.23606797749979) - - v = Vector2(0, 0) - m = v.Magnitude() - assert(m == 0) - - def test_Normalize(self): - v1: Vector2 = Vector2(0, 2) - v: Vector2 = Vector2.zero - - v = v1.Normalized() - assert(v == Vector2(0, 1)) - - v1 = Vector2(0, -2) - v = v1.Normalized() - assert(v == Vector2(0, -1)) - - v1 = Vector2(0, 0) - v = v1.Normalized() - assert(v == Vector2(0, 0)) - - def test_Negate(self): - v1: Vector2 = Vector2(4, 5) - r: Vector2 = Vector2.zero - - r = -v1 - assert(r == Vector2(-4, -5)) - - v1 = Vector2(-4, -5) - r = -v1 - assert(r == Vector2(4, 5)) - - v1 = Vector2(0, 0) - r = -v1 - assert(r == Vector2(0, 0)) - - def test_Subtract(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: Vector2 = Vector2.zero - - - r = v1 - v2 - assert(v1 - v2 == Vector2(3, 3)) - - v2 = Vector2(-1, -2) - r = v1 - v2 - assert(r == Vector2(5, 7)) - - v2 = Vector2(4, 5) - r = v1 - v2 - assert(r == Vector2(0, 0)) - - v2 = Vector2(0, 0) - r = v1 - v2 - assert(r == Vector2(4, 5)) - - def test_Addition(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: Vector2 = Vector2.zero - - r = v1 + v2 - assert(r == Vector2(5, 7)) - - v2 = Vector2(-1, -2) - r = v1 + v2 - assert(r == Vector2(3, 3)) - - v2 = Vector2(0, 0) - r = v1 + v2 - assert(r == Vector2(4, 5)) - - def test_Scale(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: Vector2 = Vector2.zero - - r = v1.Scale(v2) - assert(r == Vector2(4, 10)) - - v2 = Vector2(-1, -2) - r = v1.Scale(v2) - assert(r == Vector2(-4, -10)) - - v2 = Vector2(0, 0) - r = v1.Scale(v2) - assert(r == Vector2(0, 0)) - - def test_Multiply(self): - v1: Vector2 = Vector2(4, 5) - f: float = 3 - r: Vector2 = Vector2.zero - - r = v1 * f - assert(r == Vector2(12, 15)) - - f = -3 - r = v1 * f - assert(r == Vector2(-12, -15)) - - f = 0 - r = v1 * f - assert(r == Vector2(0, 0)) - - def test_Divide(self): - v1: Vector2 = Vector2(4, 5) - f: float = 2 - v: Vector2 = Vector2.zero - - v = v1 / f - assert(v == Vector2(2, 2.5)) - - f = -2 - v = v1 / f - assert(v == Vector2(-2, -2.5)) - - def test_Distance(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: float = 0 - - r = Vector2.Distance(v1, v2) - assert(r == 4.242640687119285) - - v2 = Vector2(-1, -2) - r = Vector2.Distance(v1, v2) - assert(r == 8.602325267042627) - - v2 = Vector2(0, 0) - r = Vector2.Distance(v1, v2) - assert(r == 6.4031242374328485) - - def test_Dot(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: float = 0 - - r = Vector2.Dot(v1, v2) - assert(r == 14) - - v2 = Vector2(-1, -2) - r = Vector2.Dot(v1, v2) - assert(r, -14) - - v2 = Vector2(0, 0) - r = Vector2.Dot(v1, v2) - assert(r, 0) - - def test_Angle(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: Angle = Angle.Degrees(0) - - r = Vector2.Angle(v1, v2) - assert(r.InDegrees() == 12.094757077012119) - - v2 = Vector2(-1, -2) - r = Vector2.Angle(v1, v2) - assert(r.InDegrees() == 167.9052429229879) - - v2 = Vector2(0, 0) - r = Vector2.Angle(v1, v2) - assert(r.InDegrees() == 0) - - def test_SignedAngle(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: float = 0 - - r = Vector2.SignedAngle(v1, v2) - assert(r.InDegrees() == -12.094757077012098) - - v2 = Vector2(-1, -2) - r = Vector2.SignedAngle(v1, v2) - assert(r.InDegrees() == 167.90524292298792) - - v2 = Vector2(0, 0); - r = Vector2.SignedAngle(v1, v2) - assert(r.InDegrees() == 0) - - v1 = Vector2(0, 1) - v2 = Vector2(1, 0) - r = Vector2.SignedAngle(v1, v2) - assert(r.InDegrees(), 90) - - v1 = Vector2(0, 1) - v2 = Vector2(0, -1) - r = Vector2.SignedAngle(v1, v2) - assert(r.InDegrees(), 180) - - def test_Lerp(self): - v1: Vector2 = Vector2(4, 5) - v2: Vector2 = Vector2(1, 2) - r: Vector2 = Vector2.zero - - r = Vector2.Lerp(v1, v2, 0) - assert(Vector2.Distance(r, v1), 0) - - r = Vector2.Lerp(v1, v2, 1) - assert(Vector2.Distance(r, v2), 0) - - r = Vector2.Lerp(v1, v2, 0.5) - assert(Vector2.Distance(r, Vector2(2.5, 3.5)), 0) - - r = Vector2.Lerp(v1, v2, -1) - assert(Vector2.Distance(r, Vector2(7.0, 8.0)), 0) - - r = Vector2.Lerp(v1, v2, 2) - assert(Vector2.Distance(r, Vector2(-2.0, -1.0)), 0) - -class Vector3Test(unittest.TestCase): - def test_Equality(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - r: bool = False - - r = v1 == v2 - assert(r == False) - - v2 = Vector3(4, 5, 6); - r = v1 == v2; - assert(r == True) - - def test_SqrMagnitude(self): - v: Vector3 = Vector3(1, 2, 3) - m: float = 0; - - m = v.SqrMagnitude() - assert(m == 14) - - v = Vector3(-1, -2, -3) - m = v.SqrMagnitude() - assert(m == 14) - - v = Vector3(0, 0, 0) - m = v.SqrMagnitude() - assert(m == 0) - - def test_Magnitude(self): - v: Vector3 = Vector3(1, 2, 3) - m: float = 0; - - m = v.Magnitude() - assert(m == 3.7416573867739413) - - v = Vector3(-1, -2, -3) - m = v.Magnitude() - assert(m == 3.7416573867739413) - - v = Vector3(0, 0, 0) - m = v.Magnitude() - assert(m == 0) - - def test_Normalize(self): - r: bool = False - - v1: Vector3 = Vector3(0, 2, 0) - v: Vector3 = Vector3.zero - - v = v1.Normalized() - assert(v == Vector3(0, 1, 0)) - - v1 = Vector3(0, -2, 0) - v = v1.Normalized() - assert(v == Vector3(0, -1, 0)) - - v1 = Vector3(0, 0, 0) - v = v1.Normalized() - assert(v == Vector3(0, 0, 0)) - - def test_Negate(self): - v1: Vector3 = Vector3(4, 5, 6) - v: Vector3 = Vector3.zero - - v = -v1 - assert(v == Vector3(-4, -5, -6)) - - v1 = Vector3(-4, -5, -6) - v = -v1 - assert(v == Vector3(4, 5, 6)) - - v1 = Vector3(0, 0, 0) - v = -v1 - assert(v == Vector3(0, 0, 0)) - - def test_Subtract(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - v3: Vector3 = Vector3.zero - - v = v1 - v2 - assert(v == Vector3(3, 3, 3)) - - v2 = Vector3(-1, -2, -3) - v = v1 - v2 - assert(v == Vector3(5, 7, 9)) - - v2 = Vector3(4, 5, 6) - v = v1 - v2 - assert(v == Vector3(0, 0, 0)) - - v2 = Vector3(0, 0, 0) - v = v1 - v2 - assert(v == Vector3(4, 5, 6)) - - def test_Addition(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - v: Vector3 = Vector3.zero - - v = v1 + v2 - assert(v == Vector3(5, 7, 9)) - - v2 = Vector3(-1, -2, -3) - v = v1 + v2 - assert(v == Vector3(3, 3, 3)) - - v2 = Vector3(0, 0, 0) - v = v1 + v2 - assert(v == Vector3(4, 5, 6)) - - def test_Scale(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - v: Vector3 = Vector3.zero - - v = v1.Scale(v2) - assert(v == Vector3(4, 10, 18)) - - v2 = Vector3(-1, -2, -3) - v = v1.Scale(v2) - assert(v == Vector3(-4, -10, -18)) - - v2 = Vector3(0, 0, 0) - v = v1.Scale(v2) - assert(v == Vector3(0, 0, 0)) - - def test_Multiply(self): - v1: Vector3 = Vector3(4, 5, 6) - f: float = 3 - v: Vector3 = Vector3.zero - - v = v1 * f - assert(v == Vector3(12, 15, 18)) - - f = -3 - v = v1 * f - assert(v == Vector3(-12, -15, -18)) - - f = 0 - v = v1 * f - assert(v == Vector3(0, 0, 0)) - - def test_Divide(self): - v1: Vector3 = Vector3(4, 5, 6) - f: float = 2 - v: Vector3 = Vector3.zero - - v = v1 / f - assert(v == Vector3(2, 2.5, 3)) - - f = -2 - v = v1 / f - assert(v == Vector3(-2, -2.5, -3)) - - def test_Distance(self): - v1: Vector3 = Vector3(4, 5, 6); - v2: Vector3 = Vector3(1, 2, 3); - f: float = 0 - - f = Vector3.Distance(v1, v2); - assert(f == 5.196152422706632) - - v2 = Vector3(-1, -2, -3); - f = Vector3.Distance(v1, v2); - assert(f == 12.449899597988733) - - v2 = Vector3(0, 0, 0); - f = Vector3.Distance(v1, v2); - assert(f == 8.774964387392123) - - def test_Dot(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - f: float = 0 - - f = Vector3.Dot(v1, v2) - assert(f == 32) - - v2 = Vector3(-1, -2, -3) - f = Vector3.Dot(v1, v2) - assert(f, -32) - - v2 = Vector3(0, 0, 0) - f = Vector3.Dot(v1, v2) - assert(f, 0) - - def test_Cross(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - v: Vector3 = Vector3.zero - - v = Vector3.Cross(v1, v2) - assert(v == Vector3(3, -6, 3)) - - v2 = Vector3(-1, -2, -3) - v = Vector3.Cross(v1, v2) - assert(v == Vector3(-3, 6, -3)) - - v2 = Vector3(0, 0, 0) - v = Vector3.Cross(v1, v2) - assert(v == Vector3(0, 0, 0)) - - def test_Project(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - v: Vector3 = Vector3.zero - - v = v1.Project(v2) - assert(v == Vector3(2.2857142857142856, 4.571428571428571, 6.857142857142857)) - - v2 = Vector3(-1, -2, -3) - v = v1.Project(v2) - assert(v == Vector3(2.2857142857142856, 4.571428571428571, 6.857142857142857)) - - v2 = Vector3(0, 0, 0) - v = v1.Project(v2) - assert(v == Vector3(0, 0, 0)) - - def test_ProjectOnPlane(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - v: Vector3 = Vector3.zero - - v = v1.ProjectOnPlane(v2) - assert(v == Vector3(1.7142857142857144, 0.4285714285714288, -0.8571428571428568)) - - v2 = Vector3(-1, -2, -3) - v = v1.ProjectOnPlane(v2) - assert(v == Vector3(1.7142857142857144, 0.4285714285714288, -0.8571428571428568)) - - v2 = Vector3(0, 0, 0) - v = v1.ProjectOnPlane(v2) - assert(v == Vector3(4, 5, 6)) - - def test_Angle(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - f: Angle = Angle.Degrees(0) - - f = Vector3.Angle(v1, v2) - assert(f.InDegrees() == 12.933154491899135) - - v2 = Vector3(-1, -2, -3) - f = Vector3.Angle(v1, v2) - assert(f.InDegrees() == 167.06684550810087) - - v2 = Vector3(0, 0, 0) - f = Vector3.Angle(v1, v2) - assert(f.InDegrees() == 0) - - def test_SignedAngle(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - v3: Vector3 = Vector3(7, 8, -9) - f: Angle = Angle.Degrees(0); - r: bool = False - - f = Vector3.SignedAngle(v1, v2, v3) - assert(f.InDegrees() == -12.933154491899135) - - v2 = Vector3(-1, -2, -3) - f = Vector3.SignedAngle(v1, v2, v3) - assert(f.InDegrees(), 167.06684550810087) - - v2 = Vector3(0, 0, 0) - f = Vector3.SignedAngle(v1, v2, v3) - assert(f.InDegrees(), 0) - - v2 = Vector3(1, 2, 3) - - v3 = Vector3(-7, -8, 9) - f = Vector3.SignedAngle(v1, v2, v3) - assert(f.InDegrees(), 12.933154491899135) - - v3 = Vector3(0, 0, 0) - f = Vector3.SignedAngle(v1, v2, v3) - assert(f.InDegrees(), 0) - - def test_Lerp(self): - v1: Vector3 = Vector3(4, 5, 6) - v2: Vector3 = Vector3(1, 2, 3) - r: Vector3 = Vector3(0, 0, 0) - - r = Vector3.Lerp(v1, v2, 0) - assert(Vector3.Distance(r, v1), 0) - - r = Vector3.Lerp(v1, v2, 1) - assert(Vector3.Distance(r, v2), 0) - - r = Vector3.Lerp(v1, v2, 0.5) - assert(Vector3.Distance(r, Vector3(2.5, 3.5, 4.5)), 0) - - r = Vector3.Lerp(v1, v2, -1) - assert(Vector3.Distance(r, Vector3(7.0, 8.0, 9.0)), 0) - - r = Vector3.Lerp(v1, v2, 2) - assert(Vector3.Distance(r, Vector3(-2.0, -1.0, 0.0)), 0) diff --git a/RoboidControl/LinearAlgebra/test/__init__.py b/RoboidControl/LinearAlgebra/test/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/setup.py b/setup.py index 4c9265e..35062dd 100644 --- a/setup.py +++ b/setup.py @@ -4,8 +4,7 @@ setup( name='RoboidControl', version='0.3.0', packages=find_packages(), - install_requires=[ - ], + install_requires=['LinearAlgebra @ git+https://git.passer.life/LinearAlgebra/LinearAlgebra-python.git@main#egg=LinearAlgebra'], author='Passer Life', author_email='support@passer.life', description='Control lightweight networked robots',