diff --git a/.editorconfig b/.editorconfig index 5e8edbf21..3d0b69f1d 100644 --- a/.editorconfig +++ b/.editorconfig @@ -21,3 +21,4 @@ max_line_length = 88 indent_size = 4 indent_style = tab max_line_length = 88 +ij_kotlin_packages_to_use_import_on_demand = java.util.*,kotlin.math.* diff --git a/server/build.gradle.kts b/server/build.gradle.kts index e0c415ae8..552de36fc 100644 --- a/server/build.gradle.kts +++ b/server/build.gradle.kts @@ -139,7 +139,8 @@ configure { "indent_size" to 4, "indent_style" to "tab", // "max_line_length" to 88, - "ktlint_experimental" to "enabled" + "ktlint_experimental" to "enabled", + "ij_kotlin_packages_to_use_import_on_demand" to "java.util.*,kotlin.math.*" ) val ktlintVersion = "0.47.1" kotlinGradle { diff --git a/server/resources/ThirdPartyNotices.txt b/server/resources/ThirdPartyNotices.txt index 70f187fb4..ee465f5ed 100644 --- a/server/resources/ThirdPartyNotices.txt +++ b/server/resources/ThirdPartyNotices.txt @@ -1269,7 +1269,7 @@ https://github.com/melloware/jintellitype Apache License Version 2.0, January 2004 - http://www.apache.org/licenses/ + http://www.apache.org/licenses/ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION @@ -1501,7 +1501,7 @@ exhaustive, and do not form part of our licenses. such as asking that all changes be marked or described. Although not required by our licenses, you are encouraged to respect those requests where reasonable. More_considerations - for the public: + for the public: wiki.creativecommons.org/Considerations_for_licensees ======================================================================= @@ -1844,3 +1844,235 @@ licenses. Creative Commons may be contacted at creativecommons.org. --------------------------------------------------------------- + +ktmath +axisangles@gmail.com + +MIT License + +Copyright (c) 2023 Donald F Reynolds + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. + +ktmath +axisangles@gmail.com + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2023 Donald F Reynolds + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + +--------------------------------------------------------------- diff --git a/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt b/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt new file mode 100644 index 000000000..4cf58daad --- /dev/null +++ b/server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt @@ -0,0 +1,112 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlin.math.cos +import kotlin.math.sin + +enum class EulerOrder { XYZ, YZX, ZXY, ZYX, YXZ, XZY } + +data class EulerAngles(val order: EulerOrder, val x: Float, val y: Float, val z: Float) { + /** + * creates a quaternion which represents the same rotation as this eulerAngles + * @return the quaternion + */ + fun toQuaternion(): Quaternion { + val cX = cos(x / 2f) + val cY = cos(y / 2f) + val cZ = cos(z / 2f) + val sX = sin(x / 2f) + val sY = sin(y / 2f) + val sZ = sin(z / 2f) + + return when (order) { + EulerOrder.XYZ -> Quaternion( + cX * cY * cZ - sX * sY * sZ, + cY * cZ * sX + cX * sY * sZ, + cX * cZ * sY - cY * sX * sZ, + cZ * sX * sY + cX * cY * sZ + ) + EulerOrder.YZX -> Quaternion( + cX * cY * cZ - sX * sY * sZ, + cY * cZ * sX + cX * sY * sZ, + cX * cZ * sY + cY * sX * sZ, + cX * cY * sZ - cZ * sX * sY + ) + EulerOrder.ZXY -> Quaternion( + cX * cY * cZ - sX * sY * sZ, + cY * cZ * sX - cX * sY * sZ, + cX * cZ * sY + cY * sX * sZ, + cZ * sX * sY + cX * cY * sZ + ) + EulerOrder.ZYX -> Quaternion( + cX * cY * cZ + sX * sY * sZ, + cY * cZ * sX - cX * sY * sZ, + cX * cZ * sY + cY * sX * sZ, + cX * cY * sZ - cZ * sX * sY + ) + EulerOrder.YXZ -> Quaternion( + cX * cY * cZ + sX * sY * sZ, + cY * cZ * sX + cX * sY * sZ, + cX * cZ * sY - cY * sX * sZ, + cX * cY * sZ - cZ * sX * sY + ) + EulerOrder.XZY -> Quaternion( + cX * cY * cZ + sX * sY * sZ, + cY * cZ * sX - cX * sY * sZ, + cX * cZ * sY - cY * sX * sZ, + cZ * sX * sY + cX * cY * sZ + ) + } + } + + // temp, replace with direct conversion later + // fun toMatrix(): Matrix3 = this.toQuaternion().toMatrix() + /** + * creates a matrix which represents the same rotation as this eulerAngles + * @return the matrix + */ + fun toMatrix(): Matrix3 { + val cX = cos(x) + val cY = cos(y) + val cZ = cos(z) + val sX = sin(x) + val sY = sin(y) + val sZ = sin(z) + + return when (order) { + // ktlint ruining spacing + /* ktlint-disable */ + EulerOrder.XYZ -> Matrix3( + cY*cZ , -cY*sZ , sY , + cZ*sX*sY + cX*sZ , cX*cZ - sX*sY*sZ , -cY*sX , + sX*sZ - cX*cZ*sY , cZ*sX + cX*sY*sZ , cX*cY ) + + EulerOrder.YZX -> Matrix3( + cY*cZ , sX*sY - cX*cY*sZ , cX*sY + cY*sX*sZ , + sZ , cX*cZ , -cZ*sX , + -cZ*sY , cY*sX + cX*sY*sZ , cX*cY - sX*sY*sZ ) + + EulerOrder.ZXY -> Matrix3( + cY*cZ - sX*sY*sZ , -cX*sZ , cZ*sY + cY*sX*sZ , + cZ*sX*sY + cY*sZ , cX*cZ , sY*sZ - cY*cZ*sX , + -cX*sY , sX , cX*cY ) + + EulerOrder.ZYX -> Matrix3( + cY*cZ , cZ*sX*sY - cX*sZ , cX*cZ*sY + sX*sZ , + cY*sZ , cX*cZ + sX*sY*sZ , cX*sY*sZ - cZ*sX , + -sY , cY*sX , cX*cY ) + + EulerOrder.YXZ -> Matrix3( + cY*cZ + sX*sY*sZ , cZ*sX*sY - cY*sZ , cX*sY , + cX*sZ , cX*cZ , -sX , + cY*sX*sZ - cZ*sY , cY*cZ*sX + sY*sZ , cX*cY ) + + EulerOrder.XZY -> Matrix3( + cY*cZ , -sZ , cZ*sY , + sX*sY + cX*cY*sZ , cX*cZ , cX*sY*sZ - cY*sX , + cY*sX*sZ - cX*sY , cZ*sX , cX*cY + sX*sY*sZ ) + /* ktlint-enable */ + } + } +} diff --git a/server/src/main/java/io/github/axisangles/ktmath/LICENSE-APACHE b/server/src/main/java/io/github/axisangles/ktmath/LICENSE-APACHE new file mode 100644 index 000000000..b15a06491 --- /dev/null +++ b/server/src/main/java/io/github/axisangles/ktmath/LICENSE-APACHE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2023 Donald F Reynolds + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/server/src/main/java/io/github/axisangles/ktmath/LICENSE-MIT b/server/src/main/java/io/github/axisangles/ktmath/LICENSE-MIT new file mode 100644 index 000000000..39ed2df92 --- /dev/null +++ b/server/src/main/java/io/github/axisangles/ktmath/LICENSE-MIT @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2023 Donald F Reynolds + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt b/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt new file mode 100644 index 000000000..90818007c --- /dev/null +++ b/server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt @@ -0,0 +1,419 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlin.math.* + +/* ktlint-disable */ +data class Matrix3( + val xx: Float, val yx: Float, val zx: Float, + val xy: Float, val yy: Float, val zy: Float, + val xz: Float, val yz: Float, val zz: Float +) { +/* ktlint-enable */ + companion object { + val NULL = Matrix3( + 0f, 0f, 0f, + 0f, 0f, 0f, + 0f, 0f, 0f + ) + val IDENTITY = Matrix3( + 1f, 0f, 0f, + 0f, 1f, 0f, + 0f, 0f, 1f + ) + } + + /** + * creates a new matrix from x y and z column vectors + */ + constructor(x: Vector3, y: Vector3, z: Vector3) : this( + x.x, y.x, z.x, + x.y, y.y, z.y, + x.z, y.z, z.z + ) + + // column getters + val x get() = Vector3(xx, xy, xz) + val y get() = Vector3(yx, yy, yz) + val z get() = Vector3(zx, zy, zz) + + // row getters + val xRow get() = Vector3(xx, yx, zx) + val yRow get() = Vector3(xy, yy, zy) + val zRow get() = Vector3(xz, yz, zz) + + operator fun unaryMinus(): Matrix3 = Matrix3( + -xx, -yx, -zx, + -xy, -yy, -zy, + -xz, -yz, -zz + ) + + operator fun plus(that: Matrix3): Matrix3 = Matrix3( + this.xx + that.xx, this.yx + that.yx, this.zx + that.zx, + this.xy + that.xy, this.yy + that.yy, this.zy + that.zy, + this.xz + that.xz, this.yz + that.yz, this.zz + that.zz + ) + + operator fun minus(that: Matrix3): Matrix3 = Matrix3( + this.xx - that.xx, this.yx - that.yx, this.zx - that.zx, + this.xy - that.xy, this.yy - that.yy, this.zy - that.zy, + this.xz - that.xz, this.yz - that.yz, this.zz - that.zz + ) + + operator fun times(that: Float): Matrix3 = Matrix3( + this.xx * that, this.yx * that, this.zx * that, + this.xy * that, this.yy * that, this.zy * that, + this.xz * that, this.yz * that, this.zz * that + ) + + operator fun times(that: Vector3): Vector3 = Vector3( + this.xx * that.x + this.yx * that.y + this.zx * that.z, + this.xy * that.x + this.yy * that.y + this.zy * that.z, + this.xz * that.x + this.yz * that.y + this.zz * that.z + ) + + operator fun times(that: Matrix3): Matrix3 = Matrix3( + this.xx * that.xx + this.yx * that.xy + this.zx * that.xz, + this.xx * that.yx + this.yx * that.yy + this.zx * that.yz, + this.xx * that.zx + this.yx * that.zy + this.zx * that.zz, + this.xy * that.xx + this.yy * that.xy + this.zy * that.xz, + this.xy * that.yx + this.yy * that.yy + this.zy * that.yz, + this.xy * that.zx + this.yy * that.zy + this.zy * that.zz, + this.xz * that.xx + this.yz * that.xy + this.zz * that.xz, + this.xz * that.yx + this.yz * that.yy + this.zz * that.yz, + this.xz * that.zx + this.yz * that.zy + this.zz * that.zz + ) + + /** + * computes the square of the frobenius norm of this matrix + * @return the frobenius norm squared + */ + fun normSq(): Float = + xx * xx + yx * yx + zx * zx + + xy * xy + yy * yy + zy * zy + + xz * xz + yz * yz + zz * zz + + /** + * computes the frobenius norm of this matrix + * @return the frobenius norm + */ + fun norm(): Float = sqrt(normSq()) + + /** + * computes the determinant of this matrix + * @return the determinant + */ + fun det(): Float = + (xz * yx - xx * yz) * zy + + (xx * yy - xy * yx) * zz + + (xy * yz - xz * yy) * zx + + /** + * computes the trace of this matrix + * @return the trace + */ + fun trace(): Float = xx + yy + zz + + /** + * computes the transpose of this matrix + * @return the transpose matrix + */ + fun transpose(): Matrix3 = Matrix3( + xx, xy, xz, + yx, yy, yz, + zx, zy, zz + ) + + /** + * computes the inverse of this matrix + * @return the inverse matrix + */ + fun inv(): Matrix3 { + val det = det() + return Matrix3( + (yy * zz - yz * zy) / det, (yz * zx - yx * zz) / det, (yx * zy - yy * zx) / det, + (xz * zy - xy * zz) / det, (xx * zz - xz * zx) / det, (xy * zx - xx * zy) / det, + (xy * yz - xz * yy) / det, (xz * yx - xx * yz) / det, (xx * yy - xy * yx) / det + ) + } + + operator fun div(that: Float): Matrix3 = this * (1f / that) + + /** + * computes the right division, this * that^-1 + */ + operator fun div(that: Matrix3): Matrix3 = this * that.inv() + + /** + * computes the inverse transpose of this matrix + * @return the inverse transpose matrix + */ + fun invTranspose(): Matrix3 { + val det = det() + return Matrix3( + (yy * zz - yz * zy) / det, (xz * zy - xy * zz) / det, (xy * yz - xz * yy) / det, + (yz * zx - yx * zz) / det, (xx * zz - xz * zx) / det, (xz * yx - xx * yz) / det, + (yx * zy - yy * zx) / det, (xy * zx - xx * zy) / det, (xx * yy - xy * yx) / det + ) + } + + /* + The following method returns the best guess rotation matrix. + In general, a square matrix can be represented as an + orthogonal matrix * symmetric matrix. + M = O*S + A symmetric matrix's transpose is itself. + An orthogonal matrix's transpose is its inverse. + S^T = S + O^T = O^-1 + If we perform the following process, we can factor out O. + M + M^-T + = O*S + (O*S)^-T + = O*S + O^-T*S^-T + = O*S + O*S^-T + = O*(S + S^-T) + So we see if we perform M + M^-T, the rotation, O, remains unchanged. + Iterating M = (M + M^-T)/2, we converge the symmetric part to identity. + + This converges exponentially (one digit per iteration) when it is far from a + rotation matrix, and quadratically (double the digits each iteration) when it + is close to a rotation matrix. + */ + /** + * computes the nearest orthonormal matrix to this matrix + * @return the rotation matrix + */ + fun orthonormalize(): Matrix3 { + if (this.det() <= 0f) { // maybe this doesn't have to be so + throw Exception("Attempt to convert non-positive determinant matrix to rotation matrix") + } + + var curMat = this + var curDet = Float.POSITIVE_INFINITY + + for (i in 1..100) { + val newMat = (curMat + curMat.invTranspose()) / 2f + val newDet = abs(newMat.det()) + // should almost always exit immediately + if (newDet >= curDet) return curMat + if (newDet <= 1.0000001f) return newMat + curMat = newMat + curDet = newDet + } + + return curMat + } + + /** + * finds the rotation matrix closest to all given rotation matrices. + * multiply input matrices by a weight for weighted averaging. + * WARNING: NOT ANGULAR + * @param others a variable number of additional matrices to average + * @return the average rotation matrix + */ + fun average(vararg others: Matrix3): Matrix3 { + var count = 1f + var sum = this + others.forEach { + count += 1f + sum += it + } + return (sum / count).orthonormalize() + } + + /** + * linearly interpolates this matrix to that matrix by t + * @param that the matrix towards which to interpolate + * @param t the amount by which to interpolate + * @return the interpolated matrix + */ + fun lerp(that: Matrix3, t: Float): Matrix3 = (1f - t) * this + t * that + + // assumes this matrix is orthonormal and converts this to a quaternion + /** + * creates a quaternion representing the same rotation as this matrix, + * assuming the matrix is a rotation matrix + * @return the quaternion + */ + fun toQuaternionAssumingOrthonormal(): Quaternion { + return if (yy > -zz && zz > -xx && xx > -yy) { + Quaternion(1f + xx + yy + zz, yz - zy, zx - xz, xy - yx).unit() + } else if (xx > yy && xx > zz) { + Quaternion(yz - zy, 1f + xx - yy - zz, xy + yx, xz + zx).unit() + } else if (yy > zz) { + Quaternion(zx - xz, xy + yx, 1f - xx + yy - zz, yz + zy).unit() + } else { + Quaternion(xy - yx, xz + zx, yz + zy, 1f - xx - yy + zz).unit() + } + } + + // orthogonalizes the matrix then returns the quaternion + /** + * creates a quaternion representing the same rotation as this matrix + * @return the quaternion + */ + fun toQuaternion(): Quaternion = orthonormalize().toQuaternionAssumingOrthonormal() + + /* + the standard algorithm: + + yAng = asin(clamp(zx, -1, 1)) + if (abs(zx) < 0.9999999f) { + xAng = atan2(-zy, zz) + zAng = atan2(-yx, xx) + } else { + xAng = atan2(yz, yy) + zAng = 0 + } + + + + problems with the standard algorithm: + + 1) + yAng = asin(clamp(zx, -1, 1)) + + FIX: + yAng = atan2(zx, sqrt(zy*zy + zz*zz)) + + this loses many bits of accuracy when near the singularity, zx = +-1 and + can cause the algorithm to return completely inaccurate results with only + small floating point errors in the matrix. this happens because zx is + NOT sin(pitch), but rather errorTerm*sin(pitch), and small changes in zx + when zx is near +-1 make large changes in asin(zx). + + + + 2) + if (abs(zx) < 0.9999999f) { + + FIX: + if (zy*zy + zz*zz > 0f) { + + this clause, meant to reduce the inaccuracy of the code following does + not actually test for the condition that makes the following atans unstable. + that is, when (zy, zz) and (yx, xx) are near 0. + after several matrix multiplications, the error term is expected to be + larger than 0.0000001. Often times, this clause will not catch the conditions + it is trying to catch. + + + + 3) + zAng = atan2(-yx, xx) + + FIX: + zAng = atan2(xy*zz - xz*zy, yy*zz - yz*zy) + + xAng and zAng are being computed separately. In the case of near singularity + the angles of xAng and zAng are effectively added together as they represent + the same operation (a rotation about the global y-axis). When computed + separately, it is not guaranteed that the xAng + zAng add together to give + the actual final rotation about the global y-axis. + + + + 4) + after many matrix operations are performed, without orthonormalization + the matrix will contain floating point errors that will throw off the + accuracy of any euler angles algorithm. orthonormalization should be + built into the prerequisites for this function + */ + + /** + * creates an eulerAngles representing the same rotation as this matrix, + * assuming the matrix is a rotation matrix + * @return the eulerAngles + */ + fun toEulerAnglesAssumingOrthonormal(order: EulerOrder): EulerAngles { + val ETA = 1.5707964f + when (order) { + EulerOrder.XYZ -> { + val kc = sqrt(zy * zy + zz * zz) + if (kc < 1e-7f) return EulerAngles(EulerOrder.XYZ, + atan2(yz, yy), ETA.withSign(zx), 0f) + + return EulerAngles( + EulerOrder.XYZ, + atan2(-zy, zz), + atan2(zx, kc), + atan2(xy * zz - xz * zy, yy * zz - yz * zy) + ) + } + EulerOrder.YZX -> { + val kc = sqrt(xx * xx + xz * xz) + if (kc < 1e-7f) return EulerAngles(EulerOrder.YZX, + 0f, atan2(zx, zz), ETA.withSign(xy)) + + return EulerAngles( + EulerOrder.YZX, + atan2(xx * yz - xz * yx, xx * zz - xz * zx), + atan2(-xz, xx), + atan2(xy, kc) + ) + } + EulerOrder.ZXY -> { + val kc = sqrt(yy * yy + yx * yx) + if (kc < 1e-7f) return EulerAngles(EulerOrder.ZXY, + ETA.withSign(yz), 0f, atan2(xy, xx)) + + return EulerAngles( + EulerOrder.ZXY, + atan2(yz, kc), + atan2(yy * zx - yx * zy, yy * xx - yx * xy), + atan2(-yx, yy) + ) + } + EulerOrder.ZYX -> { + val kc = sqrt(xy * xy + xx * xx) + if (kc < 1e-7f) return EulerAngles(EulerOrder.ZYX, + 0f, ETA.withSign(-xz), atan2(-yx, yy)) + + return EulerAngles( + EulerOrder.ZYX, + atan2(zx * xy - zy * xx, yy * xx - yx * xy), + atan2(-xz, kc), + atan2(xy, xx) + ) + } + + EulerOrder.YXZ -> { + val kc = sqrt(zx * zx + zz * zz) + if (kc < 1e-7f) return EulerAngles(EulerOrder.YXZ, + ETA.withSign(-zy), atan2(-xz, xx), 0f) + + return EulerAngles( + EulerOrder.YXZ, + atan2(-zy, kc), + atan2(zx, zz), + atan2(yz * zx - yx * zz, xx * zz - xz * zx) + ) + } + EulerOrder.XZY -> { + val kc = sqrt(yz * yz + yy * yy) + if (kc < 1e-7f) return EulerAngles(EulerOrder.XZY, + atan2(-zy, zz), 0f, ETA.withSign(-yx)) + + return EulerAngles( + EulerOrder.XZY, + atan2(yz, yy), + atan2(xy * yz - xz * yy, zz * yy - zy * yz), + atan2(-yx, kc) + ) + } + } + } + + // orthogonalizes the matrix then returns the euler angles + /** + * creates an eulerAngles representing the same rotation as this matrix + * @return the eulerAngles + */ + fun toEulerAngles(order: EulerOrder): EulerAngles = + orthonormalize().toEulerAnglesAssumingOrthonormal(order) +} + +operator fun Float.times(that: Matrix3): Matrix3 = that * this + +operator fun Float.div(that: Matrix3): Matrix3 = that.inv() * this diff --git a/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt b/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt new file mode 100644 index 000000000..be13fc413 --- /dev/null +++ b/server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt @@ -0,0 +1,371 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlin.math.* + +data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { + companion object { + val NULL = Quaternion(0f, 0f, 0f, 0f) + val IDENTITY = Quaternion(1f, 0f, 0f, 0f) + val I = Quaternion(0f, 1f, 0f, 0f) + val J = Quaternion(0f, 0f, 1f, 0f) + val K = Quaternion(0f, 0f, 0f, 1f) + + /** + * creates a new quaternion representing the rotation about v's axis + * by an angle of v's length + * @param v the rotation vector + * @return the new quaternion + **/ + fun fromRotationVector(v: Vector3): Quaternion = Quaternion(0f, v / 2f).exp() + + /** + * creates a new quaternion representing the rotation about axis v + * by an angle of v's length + * @param vx the rotation vector's x component + * @param vy the rotation vector's y component + * @param vz the rotation vector's z component + * @return the new quaternion + **/ + fun fromRotationVector(vx: Float, vy: Float, vz: Float): Quaternion = + fromRotationVector(Vector3(vx, vy, vz)) + + /** + * finds Q, the smallest-angled quaternion whose local u direction aligns with + * the global v direction. + * @param u the local direction + * @param v the global direction + * @return Q + **/ + fun fromTo(u: Vector3, v: Vector3): Quaternion { + val U = Quaternion(0f, u) + val V = Quaternion(0f, v) + val D = V / U + + return (D + D.len()).unit() + } + } + + /** + * @return the quaternion with w real component and xyz imaginary components + */ + constructor(w: Float, xyz: Vector3) : this(w, xyz.x, xyz.y, xyz.z) + + /** + * @return the imaginary components as a vector3 + **/ + val xyz get(): Vector3 = Vector3(x, y, z) + + /** + * @return the quaternion with only the w component + **/ + val re get(): Quaternion = Quaternion(w, 0f, 0f, 0f) + + /** + * @return the quaternion with only x y z components + **/ + val im get(): Quaternion = Quaternion(0f, x, y, z) + + operator fun unaryMinus(): Quaternion = Quaternion(-w, -x, -y, -z) + + operator fun plus(that: Quaternion): Quaternion = Quaternion( + this.w + that.w, + this.x + that.x, + this.y + that.y, + this.z + that.z + ) + + operator fun plus(that: Float): Quaternion = + Quaternion(this.w + that, this.x, this.y, this.z) + + operator fun minus(that: Quaternion): Quaternion = Quaternion( + this.w - that.w, + this.x - that.x, + this.y - that.y, + this.z - that.z + ) + + operator fun minus(that: Float): Quaternion = + Quaternion(this.w - that, this.x, this.y, this.z) + + /** + * computes the dot product of this quaternion with that quaternion + * @param that the quaternion with which to be dotted + * @return the inverse quaternion + **/ + fun dot(that: Quaternion): Float = + this.w * that.w + this.x * that.x + this.y * that.y + this.z * that.z + + /** + * computes the square of the length of this quaternion + * @return the length squared + **/ + fun lenSq(): Float = w * w + x * x + y * y + z * z + + /** + * computes the length of this quaternion + * @return the length + **/ + fun len(): Float = sqrt(w * w + x * x + y * y + z * z) + + /** + * @return the normalized quaternion + **/ + fun unit(): Quaternion { + val m = len() + return if (m == 0f) NULL else (this / m) + } + + operator fun times(that: Float): Quaternion = Quaternion( + this.w * that, + this.x * that, + this.y * that, + this.z * that + ) + + operator fun times(that: Quaternion): Quaternion = Quaternion( + this.w * that.w - this.x * that.x - this.y * that.y - this.z * that.z, + this.x * that.w + this.w * that.x - this.z * that.y + this.y * that.z, + this.y * that.w + this.z * that.x + this.w * that.y - this.x * that.z, + this.z * that.w - this.y * that.x + this.x * that.y + this.w * that.z + ) + + /** + * computes the inverse of this quaternion + * @return the inverse quaternion + **/ + fun inv(): Quaternion { + val lenSq = lenSq() + return Quaternion( + w / lenSq, + -x / lenSq, + -y / lenSq, + -z / lenSq + ) + } + + operator fun div(that: Float): Quaternion = this * (1f / that) + + /** + * computes right division, this * that^-1 + **/ + operator fun div(that: Quaternion): Quaternion = this * that.inv() + + /** + * @return the conjugate of this quaternion + **/ + fun conj(): Quaternion = Quaternion(w, -x, -y, -z) + + /** + * computes the logarithm of this quaternion + * @return the log of this quaternion + **/ + fun log(): Quaternion { + val co = w + val si = xyz.len() + val len = len() + + if (si == 0f) { + return Quaternion(ln(len), xyz / w) + } + + val ang = atan2(si, co) + return Quaternion(ln(len), ang / si * xyz) + } + + /** + * raises e to the power of this quaternion + * @return the exponentiated quaternion + **/ + fun exp(): Quaternion { + val ang = xyz.len() + val len = exp(w) + + if (ang == 0f) { + return Quaternion(len, len * xyz) + } + + val co = cos(ang) + val si = sin(ang) + return Quaternion(len * co, len * si / ang * xyz) + } + + /** + * raises this quaternion to the power of t + * @param t the power by which to raise this quaternion + * @return the powered quaternion + **/ + fun pow(t: Float): Quaternion = (log() * t).exp() + + /** + * between this and -this, picks the one nearest to that quaternion + * @param that the quaternion to be nearest + * @return nearest quaternion + **/ + fun twinNearest(that: Quaternion): Quaternion = + if (this.dot(that) < 0f) -this else this + + /** + * interpolates from this quaternion to that quaternion by t in quaternion space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun interpQ(that: Quaternion, t: Float) = + if (t == 0f) { + this + } else if (t == 1f) { + that + } else if (t < 0.5f) { + (that / this).pow(t) * this + } else { + (this / that).pow(1f - t) * that + } + + /** + * interpolates from this quaternion to that quaternion by t in rotation space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun interpR(that: Quaternion, t: Float) = this.interpQ(that.twinNearest(this), t) + + /** + * linearly interpolates from this quaternion to that quaternion by t in + * quaternion space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun lerpQ(that: Quaternion, t: Float): Quaternion = (1f - t) * this + t * that + + /** + * linearly interpolates from this quaternion to that quaternion by t in + * rotation space + * @param that the quaternion to interpolate to + * @param t the amount to interpolate + * @return interpolated quaternion + **/ + fun lerpR(that: Quaternion, t: Float) = this.lerpQ(that.twinNearest(this), t) + + /** + * computes this quaternion's angle to identity in quaternion space + * @return angle + **/ + fun angleQ(): Float = atan2(xyz.len(), w) + + /** + * computes this quaternion's angle to identity in rotation space + * @return angle + **/ + fun angleR(): Float = 2f * atan2(xyz.len(), abs(w)) + + /** + * computes the angle between this quaternion and that quaternion in quaternion space + * @param that the other quaternion + * @return angle + **/ + fun angleToQ(that: Quaternion): Float = (this / that).angleQ() + + /** + * computes the angle between this quaternion and that quaternion in rotation space + * @param that the other quaternion + * @return angle + **/ + fun angleToR(that: Quaternion): Float = (this / that).angleR() + + /** + * computes the angle this quaternion rotates about the u axis in quaternion space + * @param u the axis + * @return angle + **/ + fun angleAboutQ(u: Vector3): Float { + val si = u.dot(xyz) + val co = u.len() * w + return atan2(si, co) + } + + /** + * computes the angle this quaternion rotates about the u axis in rotation space + * @param u the axis + * @return angle + **/ + fun angleAboutR(u: Vector3): Float = 2f * twinNearest(IDENTITY).angleAboutQ(u) + + /** + * finds Q, the quaternion nearest to this quaternion representing a rotation purely + * about the global u axis. Q is NOT unitized + * @param v the global axis + * @return Q + **/ + fun project(v: Vector3) = Quaternion(w, xyz.dot(v) / v.lenSq() * v) + + /** + * finds Q, the quaternion nearest to this quaternion representing a rotation NOT + * on the global u axis. Q is NOT unitized + * @param v the global axis + * @return Q + **/ + fun reject(v: Vector3) = Quaternion(w, v.cross(xyz).cross(v) / v.lenSq()) + + /** + * finds Q, the quaternion nearest to this quaternion whose local u direction aligns + * with the global v direction. Q is NOT unitized + * @param u the local direction + * @param v the global direction + * @return Q + **/ + fun align(u: Vector3, v: Vector3): Quaternion { + val U = Quaternion(0f, u) + val V = Quaternion(0f, v) + + return (V * this / U + (V / U).len() * this) / 2f + } + + /** + * applies this quaternion's rotation to that vector + * @param that the vector to be transformed + * @return that vector transformed by this quaternion + **/ + fun sandwich(that: Vector3): Vector3 = (this * Quaternion(0f, that) / this).xyz + + /** + * computes this quaternion's unit length rotation axis + * @return rotation axis + **/ + fun axis(): Vector3 = xyz.unit() + + /** + * computes the rotation vector representing this quaternion's rotation + * @return rotation vector + **/ + fun toRotationVector(): Vector3 = 2f * twinNearest(IDENTITY).log().xyz + + /** + * computes the matrix representing this quaternion's rotation + * @return rotation matrix + **/ + fun toMatrix(): Matrix3 { + val d = lenSq() + /* ktlint-disable */ + return Matrix3( + (w*w + x*x - y*y - z*z)/d , 2f*(x*y - w*z)/d , 2f*(w*y + x*z)/d , + 2f*(x*y + w*z)/d , (w*w - x*x + y*y - z*z)/d , 2f*(y*z - w*x)/d , + 2f*(x*z - w*y)/d , 2f*(w*x + y*z)/d , (w*w - x*x - y*y + z*z)/d ) + /* ktlint-enable */ + } + + /** + * computes the euler angles representing this quaternion's rotation + * @param order the order in which to decompose this quaternion into euler angles + * @return euler angles + **/ + fun toEulerAngles(order: EulerOrder): EulerAngles = + this.toMatrix().toEulerAnglesAssumingOrthonormal(order) +} + +operator fun Float.plus(that: Quaternion): Quaternion = that + this +operator fun Float.minus(that: Quaternion): Quaternion = -that + this +operator fun Float.times(that: Quaternion): Quaternion = that * this +operator fun Float.div(that: Quaternion): Quaternion = that.inv() * this diff --git a/server/src/main/java/io/github/axisangles/ktmath/Vector3.kt b/server/src/main/java/io/github/axisangles/ktmath/Vector3.kt new file mode 100644 index 000000000..6e181f826 --- /dev/null +++ b/server/src/main/java/io/github/axisangles/ktmath/Vector3.kt @@ -0,0 +1,92 @@ +@file:Suppress("unused") + +package io.github.axisangles.ktmath + +import kotlin.math.atan2 +import kotlin.math.sqrt + +data class Vector3(val x: Float, val y: Float, val z: Float) { + companion object { + val NULL = Vector3(0f, 0f, 0f) + val POS_X = Vector3(1f, 0f, 0f) + val POS_Y = Vector3(0f, 1f, 0f) + val POS_Z = Vector3(0f, 0f, 1f) + val NEG_X = Vector3(-1f, 0f, 0f) + val NEG_Y = Vector3(0f, -1f, 0f) + val NEG_Z = Vector3(0f, 0f, -1f) + } + + operator fun unaryMinus() = Vector3(-x, -y, -z) + + operator fun plus(that: Vector3) = Vector3( + this.x + that.x, + this.y + that.y, + this.z + that.z + ) + + operator fun minus(that: Vector3) = Vector3( + this.x - that.x, + this.y - that.y, + this.z - that.z + ) + + /** + * computes the dot product of this vector with that vector + * @param that the vector with which to be dotted + * @return the dot product + **/ + fun dot(that: Vector3) = this.x * that.x + this.y * that.y + this.z * that.z + + /** + * computes the cross product of this vector with that vector + * @param that the vector with which to be crossed + * @return the cross product + **/ + fun cross(that: Vector3) = Vector3( + this.y * that.z - this.z * that.y, + this.z * that.x - this.x * that.z, + this.x * that.y - this.y * that.x + ) + + /** + * computes the square of the length of this vector + * @return the length squared + **/ + fun lenSq() = x * x + y * y + z * z + + /** + * computes the length of this quaternion + * @return the length + **/ + fun len() = sqrt(x * x + y * y + z * z) + + /** + * @return the normalized vector + **/ + fun unit(): Vector3 { + val m = len() + return if (m == 0f) NULL else this / m + } + + operator fun times(that: Float) = Vector3( + this.x * that, + this.y * that, + this.z * that + ) + + // computes division of this vector3 by a float + operator fun div(that: Float) = Vector3( + this.x / that, + this.y / that, + this.z / that + ) + + /** + * computes the angle between this vector with that vector + * @param that the vector to which the angle is computed + * @return the angle + **/ + fun angleTo(that: Vector3): Float = atan2(this.cross(that).len(), this.dot(that)) +} + +operator fun Float.times(that: Vector3): Vector3 = that * this diff --git a/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt b/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt new file mode 100644 index 000000000..5b5e4d95b --- /dev/null +++ b/server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt @@ -0,0 +1,308 @@ +package io.github.axisangles.ktmath + +import kotlin.math.* +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.test.assertTrue + +class QuaternionTest { + + @Test + fun plus() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(5f, 6f, 7f, 8f) + val q3 = Quaternion(6f, 8f, 10f, 12f) + assertEquals(q3, q1 + q2) + } + + @Test + fun times() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(5f, 6f, 7f, 8f) + val q3 = Quaternion(-60f, 12f, 30f, 24f) + assertEquals(q3, q1 * q2) + } + + @Test + fun timesScalarRhs() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(2f, 4f, 6f, 8f) + assertEquals(q2, q1 * 2f) + } + + @Test + fun timesScalarLhs() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(2f, 4f, 6f, 8f) + assertEquals(q2, 2f * q1) + } + + @Test + fun inverse() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(1f / 30f, -2f / 30f, -3f / 30f, -4f / 30f) + assertEquals(q2, q1.inv()) + } + + @Test + fun rightDiv() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(5f, 6f, 7f, 8f) + val q3 = Quaternion(-60f, 12f, 30f, 24f) + assertEquals(q1, q3 / q2) + } + + @Test + fun rightDivFloatRhs() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(2f, 4f, 6f, 8f) + assertEquals(q1, q2 / 2f) + } + + @Test + fun rightDivFloatLhs() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(1f / 15f, -2f / 15f, -1f / 5f, -4f / 15f) + + assertEquals(q2, 2f / q1) + } + + @Test + fun pow() { + val q = Quaternion(1f, 2f, 3f, 4f) + assertEquals(q.pow(1f), q, 1e-5) + assertEquals(q.pow(2f), q * q, 1e-5) + assertEquals(q.pow(0f), Quaternion.IDENTITY, 1e-5) + assertEquals(q.pow(-1f), q.inv(), 1e-5) + } + + @Test + fun interpQ() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(5f, 6f, 7f, 8f) + val q3 = Quaternion(2.405691f, 3.5124686f, 4.619246f, 5.7260237f) + assertEquals(q1.interpQ(q2, 0.5f), q3, 1e-7) + } + + @Test + fun interpR() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = -Quaternion(5f, 6f, 7f, 8f) + val q3 = Quaternion(2.405691f, 3.5124686f, 4.619246f, 5.7260237f) + assertEquals(q1.interpR(q2, 0.5f), q3, 1e-7) + } + + @Test + fun lerpQ() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(5f, 6f, 7f, 8f) + val q3 = Quaternion(3f, 4f, 5f, 6f) + assertEquals(q1.lerpQ(q2, 0.5f), q3, 1e-7) + } + + @Test + fun lerpR() { + val q1 = Quaternion(1f, 2f, 3f, 4f) + val q2 = Quaternion(-5f, -6f, -7f, -8f) + val q3 = Quaternion(3f, 4f, 5f, 6f) + assertEquals(q1.lerpR(q2, 0.5f), q3, 1e-7) + } + + @Test + fun angleToQ() { + val q1 = Quaternion(1f, 0f, 0f, 0f) + val q2 = Quaternion(0f, 1f, 0f, 0f) + assertEquals(q1.angleToQ(q2), PI.toFloat() / 2f) + } + + @Test + fun angleToR() { + val q1 = Quaternion(1f, 0f, 0f, 0f) + val q2 = Quaternion(0f, 1f, 0f, 0f) + assertEquals(q1.angleToR(q2), PI.toFloat()) + } + + @Test + fun angleQ() { + val q = Quaternion(0f, 1f, 0f, 0f) + assertEquals(q.angleQ(), PI.toFloat() / 2f) + } + + @Test + fun angleR() { + val q = Quaternion(0f, 1f, 0f, 0f) + assertEquals(q.angleR(), PI.toFloat()) + } + + @Test + fun angleAboutQ() { + val q = Quaternion(1f, 1f, 1f, 0f) + assertEquals(q.angleAboutQ(Vector3.POS_Y), PI.toFloat() / 4f) + } + + @Test + fun angleAboutR() { + val q = Quaternion(1f, 1f, 1f, 0f) + assertEquals(q.angleAboutR(Vector3.POS_Y), PI.toFloat() / 2f) + } + + @Test + fun project() { + val q1 = Quaternion(1f, 1f, 1f, 0f) + val q2 = Quaternion(1f, 0f, 1f, 0f) + assertEquals(q1.project(Vector3.POS_Y), q2) + } + + @Test + fun reject() { + val q1 = Quaternion(1f, 1f, 1f, 0f) + val q2 = Quaternion(1f, 1f, 0f, 0f) + assertEquals(q1.reject(Vector3.POS_Y), q2) + } + + @Test + fun align() { + val q1 = Quaternion(0f, 1f, 0f, 0f) + val q2 = Quaternion(0f, 0.5f, 0.5f, 0f) + assertEquals(q1.align(Vector3.POS_X, Vector3.POS_Y), q2) + } + + @Test + fun fromTo() { + val q1 = Quaternion(1f, 0f, 0f, 1f).unit() + assertEquals(q1, Quaternion.fromTo(Vector3.POS_X, Vector3.POS_Y)) + } + + @Test + fun sandwich() { + val v1 = Quaternion(1f, 1f, 0f, 0f).sandwich(Vector3(1f, 1f, 0f)) + val v2 = Vector3(1f, 0f, 1f) + assertEquals(v2, v1) + } + + @Test + fun axis() { + val v1 = Quaternion(0f, Quaternion(1f, 2f, 3f, 4f).axis()) + val v2 = Quaternion(0f, Vector3(0.37139067f, 0.557086f, 0.74278134f)) + assertEquals(v2, v1, 1e-7) + } + + @Test + fun toRotationVector() { + val v1 = Quaternion(1f, 2f, 3f, 4f).toRotationVector() + val v2 = Vector3(1.0303806f, 1.5455709f, 2.0607612f) + assertEquals(v2, v1) + } + + @Test + fun fromRotationVector() { + val v1 = Quaternion.fromRotationVector(Vector3(1f, 2f, 3f)) + val v2 = Quaternion(-0.29555118f, 0.25532186f, 0.5106437f, 0.7659656f) + assertEquals(v2, v1) + } + + @Test + fun toMatrix() { + /* ktlint-disable */ + val m1 = Matrix3( + -1f, 0f, 0f, + 0f, -1f, 0f, + 0f, 0f, 1f) + /* ktlint-enable */ + val m2 = Quaternion(0f, 0f, 0f, 2f).toMatrix() + assertEquals(m1, m2) + } + + private fun testEulerAngles(order: EulerOrder) { + val inputQ = Quaternion(1f, 2f, 3f, 4f).unit() + val outputQ = inputQ.toEulerAngles(order) + .toQuaternion().twinNearest(Quaternion.IDENTITY) + assertEquals(inputQ, outputQ, 1e-7) + } + + @Test + fun eulerAnglesXYZ() { + testEulerAngles(EulerOrder.XYZ) + } + + @Test + fun eulerAnglesYZX() { + testEulerAngles(EulerOrder.YZX) + } + + @Test + fun eulerAnglesZXY() { + testEulerAngles(EulerOrder.ZXY) + } + + @Test + fun eulerAnglesZYX() { + testEulerAngles(EulerOrder.ZYX) + } + + @Test + fun eulerAnglesYXZ() { + testEulerAngles(EulerOrder.YXZ) + } + + @Test + fun eulerAnglesXZY() { + testEulerAngles(EulerOrder.XZY) + } + + companion object { + private const val RELATIVE_TOLERANCE = 0.0 + + internal fun assertEquals( + expected: Quaternion, + actual: Quaternion, + tolerance: Double = RELATIVE_TOLERANCE + ) { + val len = (actual - expected).lenSq() + val squareSum = expected.lenSq() + actual.lenSq() + assertTrue( + len <= tolerance * tolerance * squareSum, + "Expected: $expected but got: $actual" + ) + } + } +} + +var randSeed = 0 +fun randInt(): Int { + randSeed = (1103515245 * randSeed + 12345).mod(2147483648).toInt() + return randSeed +} + +fun randFloat(): Float { + return randInt().toFloat() / 2147483648 +} + +fun randGaussian(): Float { + var thing = 1f - randFloat() + while (thing == 0f) { + // no 0s allowed + thing = 1f - randFloat() + } + return sqrt(-2f * ln(thing)) * cos(PI.toFloat() * randFloat()) +} + +fun randMatrix(): Matrix3 { + return Matrix3( + randGaussian(), randGaussian(), randGaussian(), + randGaussian(), randGaussian(), randGaussian(), + randGaussian(), randGaussian(), randGaussian() + ) +} + +fun randQuaternion(): Quaternion { + return Quaternion(randGaussian(), randGaussian(), randGaussian(), randGaussian()) +} + +fun randRotMatrix(): Matrix3 { + return randQuaternion().toMatrix() +} + +fun randVector(): Vector3 { + return Vector3(randGaussian(), randGaussian(), randGaussian()) +}