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 161 Packages Projects Releases Wiki Activity

Merge pull request #517 from AxisAngles/ktmath

Browse Source 
Added package io.github.axisangles.ktmath
This commit is contained in:
Eiren Rain
2023-02-09 17:49:38 +02:00
committed by GitHub
parent 0a90f33a09 bc69532d9a
commit 1948b7136f
10 changed files with 1761 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.editorconfig
View File

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

3
server/build.gradle.kts
View File

@@ -139,7 +139,8 @@ configure<com.diffplug.gradle.spotless.SpotlessExtension> {
"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 {

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

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

@@ -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 */
}
}
}

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.

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

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

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

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

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

308
server/src/test/java/io/github/axisangles/ktmath/QuaternionTest.kt Normal file
View File

@@ -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())
}