github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-06 02:01:58 +02:00
Code Issues 160 Packages Projects Releases Wiki Activity

Added package io.github.axisangles.ktmath

Browse Source
This commit is contained in:
AxisAngles
2023-01-25 14:13:22 -08:00
parent a36a6bceee
commit 5cecb3ed17
8 changed files with 1751 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

236
server/resources/ThirdPartyNotices.txt
View File

@@ -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.
---------------------------------------------------------------

102
server/src/main/java/io/github/axisangles/ktmath/EulerAngles.kt Normal file
View File

@@ -0,0 +1,102 @@
@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}
// prefer Y.toX
// but if ambiguous, use X.fromY
/*
* Euler Angles contains both the x y z angle parameters and the order of application
*/
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) {
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)
}
}
}

201
server/src/main/java/io/github/axisangles/ktmath/LICENSE-APACHE Normal file
View File

@@ -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.

21
server/src/main/java/io/github/axisangles/ktmath/LICENSE-MIT Normal file
View File

@@ -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.

364
server/src/main/java/io/github/axisangles/ktmath/Main.kt Normal file
View File

@@ -0,0 +1,364 @@
package io.github.axisangles.ktmath
import kotlin.math.*
import kotlin.system.measureTimeMillis
var randSeed = 0
fun randInt(): Int {
randSeed = (1103515245*randSeed + 12345).mod(2147483648).toInt()
return randSeed
}
fun randFloat(): Float {
return randInt().toFloat()/2147483648f
}
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())
}
fun testEulerMatrix(order: EulerOrder, M: Matrix3, exception: String) {
// We convert to euler angles and back and see if they are reasonably similar
val N = M.toEulerAngles(order).toMatrix()
if ((N - M).norm() > 1e-6) {
println("norm error: " + (N - M).norm().toString())
throw Exception(exception)
}
}
fun testEulerConversion(order: EulerOrder, exception: String) {
for (i in 1..1000) {
testEulerMatrix(order, randRotMatrix(), exception)
}
}
fun testMatrixOrthonormalize() {
for (i in 1..1000) {
val M = randMatrix()
val N = M.invTranspose().orthonormalize()
val O = M.orthonormalize()
if ((N - O).norm() > 1e-5) {
println("norm error: " + (N - O).norm().toString())
throw Exception("Matrix orthonormalization accuracy test failed")
}
}
}
fun testQuatMatrixConversion() {
for (i in 1..1000) {
val M = randRotMatrix()
val N = (randGaussian()*M.toQuaternion()).toMatrix()
if ((N - M).norm() > 1e-6) {
println("norm error: " + (N - M).norm().toString())
throw Exception("Quaternion Matrix conversion accuracy test failed")
}
}
}
fun relError(a: Matrix3, b: Matrix3): Float {
val combinedLen = sqrt((a.normSq() + b.normSq())/2f)
if (combinedLen == 0f) return 0f
return (b - a).norm()/combinedLen
}
fun relError(a: Vector3, b: Vector3): Float {
val combinedLen = sqrt((a.lenSq() + b.lenSq())/2f)
if (combinedLen == 0f) return 0f
return (b - a).len()/combinedLen
}
fun relError(a: Quaternion, b: Quaternion): Float {
val combinedLen = sqrt((a.lenSq() + b.lenSq())/2f)
if (combinedLen == 0f) return 0f
return (b - a).len()/combinedLen
}
fun checkError(eta: Float, a: Matrix3, b: Matrix3): Boolean {
return (b - a).normSq() <= eta*eta*(a.normSq() + b.normSq())
}
fun checkError(eta: Float, a: Quaternion, b: Quaternion): Boolean {
return (b - a).lenSq() <= eta*eta*(a.lenSq() + b.lenSq())
}
fun checkError(eta: Float, a: Vector3, b: Vector3): Boolean {
return (b - a).lenSq() <= eta*eta*(a.lenSq() + b.lenSq())
}
fun checkError(eta: Float, A: Quaternion): Boolean {
return A.lenSq() <= eta*eta
}
fun testQuaternionInv() {
for (i in 1..1000) {
val Q = randQuaternion()
if (relError(Q*Q.inv(), Quaternion.ONE) > 1e-6f)
throw Exception("Quaternion inv accuracy test failed")
}
}
fun testQuaternionDiv() {
for (i in 1..1000) {
val Q = randQuaternion()
if (!checkError(1e-6f, Q/Q, Quaternion.ONE))
throw Exception("Quaternion div accuracy test failed")
if (!checkError(1e-6f, 2f/Q, 2f*Q.inv()))
throw Exception("Float/Quaternion accuracy test failed")
if (!checkError(1e-6f, Q/2f, 0.5f*Q))
throw Exception("Quaternion/Float accuracy test failed")
}
}
// 19 binary digits of accuracy
fun testQuaternionPow() {
for (i in 1..1000) {
val Q = randQuaternion()
if (!checkError(2e-6f, Q.pow(-1f), Q.inv()))
throw Exception("Quaternion pow -1 accuracy test failed")
if (!checkError(2e-6f, Q.pow(0f), Quaternion.ONE))
throw Exception("Quaternion pow 0 accuracy test failed")
if (!checkError(2e-6f, Q.pow(1f), Q))
throw Exception("Quaternion pow 1 accuracy test failed")
if (!checkError(2e-6f, Q.pow(2f), Q*Q))
throw Exception("Quaternion pow 2 accuracy test failed")
}
}
fun testQuaternionSandwich() {
for (i in 1..1000) {
val Q = randQuaternion()
val v = randVector()
if (!checkError(5e-7f, Q.toMatrix()*v, Q.sandwich(v)))
throw Exception("Quaternion sandwich accuracy test failed")
}
}
// projection and alignment are expected to be less accurate in some extreme cases
// so we expect to see some cases in which half the bits are lost
fun testQuaternionProjectAlign() {
for (i in 1..1000) {
val Q = randQuaternion()
val v = randVector()
if (!checkError(1e-4f, Q.align(v, v), Q.project(v))) {
println(Q.align(v, v) - Q.project(v))
println(Q.align(v, v))
println(Q.project(v))
println(Q)
throw Exception("Quaternion project/align accuracy test failed")
}
}
}
fun testQuaternionRotationVector() {
for (i in 1..1000) {
val Q = randQuaternion().unit()
val P = Quaternion.fromRotationVector(Q.toRotationVector())
if (!checkError(5e-7f, Q, P)) {
throw Exception("Quaternion toRotationVector fromRotationVector accuracy test failed")
}
}
}
fun testQuaternionEulerAngles(order: EulerOrder, exception: String) {
for (i in 1..1000) {
val Q = randQuaternion().unit()
val P = Q.toEulerAngles(order).toQuaternion().twinNearest(Q)
if (!checkError(2e-7f, Q, P)) {
println(relError(Q, P))
throw Exception(exception)
}
}
}
fun testEulerSingularity(order: EulerOrder, M: Matrix3, exception: String) {
for (i in 1..1000) {
val R = 1e-6f*randMatrix()
val S = M + R
if (S.det() <= 0f) return
val error = (S.toEulerAnglesAssumingOrthonormal(order).toMatrix() - S).norm()
if (error > 2f*R.norm() + 1e-6f) {
throw Exception(exception)
}
}
}
fun testEulerConversions(order: EulerOrder, exception: String) {
for (i in 1..1000) {
val e = EulerAngles(order, 6.28318f*randFloat(), 6.28318f*randFloat(), 6.28318f*randFloat())
val N = e.toMatrix()
val M = e.toQuaternion().toMatrix()
if ((N - M).norm() > 1e-6) {
throw Exception(exception)
}
}
}
fun main() {
val X90 = Matrix3(
1f, 0f, 0f,
0f, 0f, -1f,
0f, 1f, 0f
)
val Y90 = Matrix3(
0f, 0f, 1f,
0f, 1f, 0f,
-1f, 0f, 0f
)
val Z90 = Matrix3(
0f, -1f, 0f,
1f, 0f, 0f,
0f, 0f, 1f
)
testMatrixOrthonormalize()
testQuatMatrixConversion()
testQuaternionEulerAngles(EulerOrder.XYZ, "Quaternion EulerAnglesXYZ accuracy test failed")
testQuaternionEulerAngles(EulerOrder.YZX, "Quaternion EulerAnglesYZX accuracy test failed")
testQuaternionEulerAngles(EulerOrder.ZXY, "Quaternion EulerAnglesZXY accuracy test failed")
testQuaternionEulerAngles(EulerOrder.ZYX, "Quaternion EulerAnglesZYX accuracy test failed")
testQuaternionEulerAngles(EulerOrder.YXZ, "Quaternion EulerAnglesYXZ accuracy test failed")
testQuaternionEulerAngles(EulerOrder.XZY, "Quaternion EulerAnglesXZY accuracy test failed")
testQuaternionInv()
testQuaternionDiv()
testQuaternionPow()
testQuaternionSandwich()
testQuaternionProjectAlign()
testQuaternionRotationVector()
println(Matrix3.IDENTITY.average(Y90))
testEulerConversions(EulerOrder.XYZ, "fromEulerAnglesXYZ Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.YZX, "fromEulerAnglesYZX Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.ZXY, "fromEulerAnglesZXY Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.ZYX, "fromEulerAnglesZYX Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.YXZ, "fromEulerAnglesYXZ Quaternion or Matrix3 accuracy test failed")
testEulerConversions(EulerOrder.XZY, "fromEulerAnglesXZY Quaternion or Matrix3 accuracy test failed")
// EULER ANGLE TESTS
testEulerConversion(EulerOrder.XYZ, "toEulerAnglesXYZ accuracy test failed")
testEulerConversion(EulerOrder.YZX, "toEulerAnglesYZX accuracy test failed")
testEulerConversion(EulerOrder.ZXY, "toEulerAnglesZXY accuracy test failed")
testEulerConversion(EulerOrder.ZYX, "toEulerAnglesZYX accuracy test failed")
testEulerConversion(EulerOrder.YXZ, "toEulerAnglesYXZ accuracy test failed")
testEulerConversion(EulerOrder.XZY, "toEulerAnglesXZY accuracy test failed")
// test robustness to noise
testEulerSingularity(EulerOrder.XYZ, Y90, "toEulerAnglesXYZ singularity accuracy test failed")
testEulerSingularity(EulerOrder.YZX, Z90, "toEulerAnglesYZX singularity accuracy test failed")
testEulerSingularity(EulerOrder.ZXY, X90, "toEulerAnglesZXY singularity accuracy test failed")
testEulerSingularity(EulerOrder.ZYX, Y90, "toEulerAnglesZYX singularity accuracy test failed")
testEulerSingularity(EulerOrder.YXZ, X90, "toEulerAnglesYXZ singularity accuracy test failed")
testEulerSingularity(EulerOrder.XZY, Z90, "toEulerAnglesXZY singularity accuracy test failed")
// speed test a linear (align) method against some standard math functions
// var x = Quaternion(1f, 2f, 3f, 4f)
//
// var dtAlignTotal: Long = 0
// var dtOrthonormalizeTotal: Long = 0
// var dtAtan2Total: Long = 0
// var dtAsinTotal: Long = 0
//
// for (i in 1..10) {
// val dtAlign = measureTimeMillis {
// for (i in 1..1_000_000) {
// val u = Vector3(1f, 0f, 0f)
// val v = Vector3(0f, 1f, 0f)
// // to make sure it is not optimized away
// x = x.align(u, v)
// // internally, x.align is:
// // val U = Quaternion(0f, u)
// // val V = Quaternion(0f, v)
// // x = (V*x/U + (V/U).len()*x)/2f
// }
// }
//
// var y = x.toMatrix()
// val dtOrthonormalize = measureTimeMillis {
// for (i in 1..1_000_000) {
// // to make sure it is not optimized away
// y = y.orthonormalize()
// // internally, x.align is:
// // val U = Quaternion(0f, u)
// // val V = Quaternion(0f, v)
// // x = (V*x/U + (V/U).len()*x)/2f
// }
// }
//
// var z = 0f;
// val dtAtan2 = measureTimeMillis {
// for (i in 1..1_000_000) {
// z+= atan2(i.toFloat(), i.toFloat()) // 45 degrees
// }
// }
//
// var w = 0f;
// val dtAsin = measureTimeMillis {
// for (i in 1..1_000_000) {
// w+= asin(i.toFloat()*0.7071f/i.toFloat()) // 45 degrees
// }
// }
//
// dtAlignTotal += dtAlign
// dtOrthonormalizeTotal += dtOrthonormalize
// dtAtan2Total += dtAtan2
// dtAsinTotal += dtAsin
// }
//
// println(x)
//
// println(dtAlignTotal) // 213
// println(dtOrthonormalizeTotal) // 244
// println(dtAtan2Total) // 610
// println(dtAsinTotal) // 3558
// var x = Quaternion(2f, 1f, 4f, 3f)
// val dtPow = measureTimeMillis {
// for (i in 1..10_000_000) {
// x = x.pow(1f)
// }
// }
//
// println(dtPow)
}

368
server/src/main/java/io/github/axisangles/ktmath/Matrix3.kt Normal file
View File

@@ -0,0 +1,368 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.*
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
) {
companion object {
val ZERO = 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)
}
/**
* computes the nearest orthonormal matrix to this matrix
* @return the rotation matrix
*/
fun orthonormalize(): Matrix3 {
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 {
if (this.det() <= 0f)
throw Exception("Attempt to convert negative determinant matrix to quaternion")
return if (yy > -zz && zz > -xx && xx > -yy) {
Quaternion(1 + xx + yy + zz, yz - zy, zx - xz, xy - yx).unit()
} else if (xx > yy && xx > zz) {
Quaternion(yz - zy, 1 + xx - yy - zz, xy + yx, xz + zx).unit()
} else if (yy > zz) {
Quaternion(zx - xz, xy + yx, 1 - xx + yy - zz, yz + zy).unit()
} else {
Quaternion(xy - yx, xz + zx, yz + zy, 1 - 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
*/
// fun toEulerAnglesXYZFaulty(): EulerAngles {
// return if (abs(zx) < 0.9999999f)
// EulerAngles(EulerOrder.XYZ,
// atan2(-zy, zz),
// asin(zx.coerceIn(-1f, 1f)),
// atan2(-yx, xx))
// else
// EulerAngles(EulerOrder.XYZ,
// atan2(yz, yy),
// asin(zx.coerceIn(-1f, 1f)),
// 0f)
// }
/**
* 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 {
if (this.det() <= 0f)
throw Exception("Attempt to convert negative determinant matrix to euler angles")
val ETA = 1.57079632f
when (order) {
EulerOrder.XYZ -> {
val kc = zy*zy + zz*zz
if (kc == 0f) return EulerAngles(EulerOrder.XYZ, atan2(yz, yy), ETA.withSign(zx), 0f)
return EulerAngles(EulerOrder.XYZ,
atan2( -zy, zz),
atan2( zx, sqrt(kc)),
atan2(xy*zz - xz*zy, yy*zz - yz*zy))
}
EulerOrder.YZX -> {
val kc = xx*xx + xz*xz
if (kc == 0f) 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, sqrt(kc)))
}
EulerOrder.ZXY -> {
val kc = yy*yy + yx*yx
if (kc == 0f) return EulerAngles(EulerOrder.ZXY, ETA.withSign(yz), 0f, atan2(xy, xx))
return EulerAngles(EulerOrder.ZXY,
atan2( yz, sqrt(kc)),
atan2(yy*zx - yx*zy, yy*xx - yx*xy),
atan2( -yx, yy))
}
EulerOrder.ZYX -> {
val kc = xy*xy + xx*xx
if (kc == 0f) 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, sqrt(kc)),
atan2( xy, xx))
}
EulerOrder.YXZ -> {
val kc = zx*zx + zz*zz
if (kc == 0f) return EulerAngles(EulerOrder.YXZ, ETA.withSign(-zy), atan2(-xz, xx), 0f)
return EulerAngles(EulerOrder.YXZ,
atan2( -zy, sqrt(kc)),
atan2( zx, zz),
atan2(yz*zx - yx*zz, xx*zz - xz*zx))
}
EulerOrder.XZY -> {
val kc = yz*yz + yy*yy
if (kc == 0f) 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, sqrt(kc)))
}
else -> {
throw Exception("EulerAngles not implemented for given EulerOrder")
}
}
}
// 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

371
server/src/main/java/io/github/axisangles/ktmath/Quaternion.kt Normal file
View File

@@ -0,0 +1,371 @@
@file:Suppress("unused")
package io.github.axisangles.ktmath
import kotlin.math.*
/**
* A Quaternion class properly implementing Quaternions as a real component and 3 imaginary components
* All operations are well-defined
*/
data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) {
companion object {
val ZERO = Quaternion(0f, 0f, 0f, 0f)
val ONE = 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 axis v by rotational angle v
/**
* creates a new quaternion representing the rotation about axis v by rotational angle of v's length
* @return the new quaternion
**/
fun fromRotationVector(v: Vector3): Quaternion = Quaternion(0f, v/2f).exp()
}
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 inversed 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) ZERO 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 inversed 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()
// for a slight improvement in performance
// not fully implemented
// fun pow(t: Float): Quaternion {
// val imLen = imLen()
// val ang = atan2(imLen, w)
//
// val len = len().pow(t)
// val co = cos(t*ang)
// val si = sin(t*ang)
//
// return if (imLen == 0f) {
// Quaternion(
// len*co,
// len*t*x,
// len*t*y,
// len*t*z
// )
// } else {
// Quaternion(
// len*co,
// len*si*x/imLen,
// len*si*y/imLen,
// len*si*z/imLen
// )
// }
// }
/**
* between this and -this, picks the one nearest to that
* @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 interp(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.interp(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 lerp(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.lerp(that.twinNearest(this), t)
/**
* computes this quaternion's angle to identity in quaternion space
* @return angle
**/
fun angle(): 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 angleTo(that: Quaternion): Float = (this/that).angle()
/**
* 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 angleAbout(u: Vector3): Float {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return atan2(uDotIm, uLen*w)
}
/**
* computes the angle this quaternion rotates about the u axis in rotation space
* @param u the axis
* @return angle
**/
fun angleAboutR(u: Vector3): Float {
val uDotIm = u.dot(xyz)
val uLen = u.len()
return if (uDotIm < 0f) {
2f*atan2(-uDotIm, -uLen*w)
} else {
2f*atan2(uDotIm, uLen*w)
}
}
/**
* 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 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*log().xyz
/**
* computes the matrix representing this quaternion's rotation
* @return rotation matrix
**/
fun toMatrix(): Matrix3 {
val d = lenSq()
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
)
}
/**
* 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

90
server/src/main/java/io/github/axisangles/ktmath/Vector3.kt Normal file
View File

@@ -0,0 +1,90 @@
@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 ZERO = 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) ZERO 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