commonMain.ru.casperix.math.quaternion.float32.QuaternionFloat.kt Maven / Gradle / Ivy

Go to download
package ru.casperix.math.quaternion.float32

import ru.casperix.misc.toPrecision
import ru.casperix.math.quad_matrix.float32.Matrix4f
import ru.casperix.math.vector.float32.Vector3f
import kotlinx.serialization.Serializable
import kotlin.math.*

/**
 * 	source: https://euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm
 * 	https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
 */
@Serializable
data class QuaternionFloat(val x: Float, val y: Float, val z: Float, val w: Float) {

	constructor(): this (0f, 0f, 0f, 1f)

	override fun toString(): String {
		return "Q(x=${x.toPrecision(2)}, y=${y.toPrecision(2)}, z=${z.toPrecision(2)}, W=${w.toPrecision(2)})"
	}

	fun magnitude(): Float {
		return sqrt(x * x + y * y + z * z + w * w)
	}

	fun length(): Float {
		return magnitude()
	}

	fun lengthSquared(): Float {
		return (x * x + y * y + z * z + w * w)
	}

	fun norm(): Float {
		return lengthSquared()
	}

	fun conjugate(): QuaternionFloat {
		return QuaternionFloat(-x, -y, -z, w)
	}

	operator fun plus(other: QuaternionFloat): QuaternionFloat {
		return QuaternionFloat(x + other.x, y + other.y, z + other.z, w + other.w)
	}

	operator fun minus(other: QuaternionFloat): QuaternionFloat {
		return QuaternionFloat(x - other.x, y - other.y, z - other.z, w - other.w)
	}

	operator fun div(value: Float): QuaternionFloat {
		return QuaternionFloat(x / value, y / value, z / value, w / value)
	}

	operator fun times(value: Float): QuaternionFloat {
		return QuaternionFloat(x * value, y * value, z * value, w * value)
	}

	operator fun times(other: QuaternionFloat): QuaternionFloat {
		val dot = (this.x * other.x + this.y * other.y + this.z * other.z)

		val crossX = this.y * other.z - this.z * other.y
		val crossY = this.z * other.x - this.x * other.z
		val crossZ = this.x * other.y - this.y * other.x

		val addX = this.w * other.x + other.w * this.x
		val addY = this.w * other.y + other.w * this.y
		val addZ = this.w * other.z + other.w * this.z

		val W = w * other.w - dot

		return QuaternionFloat(crossX + addX, crossY + addY, crossZ + addZ, W)
	}

	fun getVector3f(): Vector3f {
		return Vector3f(x, y, z)
	}

	operator fun unaryMinus(): QuaternionFloat {
		return this * (-1f)
	}

	fun toPrecision(precision: Int): String {
		return "${x.toPrecision(precision)}; ${y.toPrecision(precision)}; ${z.toPrecision(precision)}; s=${w.toPrecision(precision)}"
	}

	fun transform(value: Vector3f): Vector3f {
		return (this * value.getQuaternion() * inverse()).getVector3f()
	}

	fun toEuler(): Vector3f {

		// roll (x-axis rotation)
		val sinr_cosp = 2 * (this.w * this.x + this.y * this.z);
		val cosr_cosp = 1 - 2 * (this.x * this.x + this.y * this.y);
		val roll = atan2(sinr_cosp, cosr_cosp);

		// pitch (y-axis rotation)
		val sinp = 2 * (this.w * this.y - this.z * this.x);
		val pitch = if (abs(sinp) >= 1)
			(PI.toFloat() / 2f).withSign(sinp)
		else
			asin(sinp);

		// yaw (z-axis rotation)
		val siny_cosp = 2 * (this.w * this.z + this.x * this.y);
		val cosy_cosp = 1 - 2 * (this.y * this.y + this.z * this.z);
		val yaw = atan2(siny_cosp, cosy_cosp);

		return Vector3f(roll, pitch, yaw);
	}

	fun dot(other: QuaternionFloat): Float {
		return this.x * other.x + this.y * other.y + this.z * other.z + this.w * other.w
	}

	fun inverse(): QuaternionFloat {
		return conjugate() / norm()
	}

	fun normalize(): QuaternionFloat {
		val len = length()
		if (len == 0f) {
			return this
		}
		return this / len
	}

	fun isFinite(): Boolean {
		return x.isFinite() && y.isFinite() && z.isFinite() && w.isFinite()
	}

	companion object {
		val IDENTITY = QuaternionFloat(0f, 0f, 0f, 1f)

		fun fromAxisAnge(axis: Vector3f, angle: Float): QuaternionFloat {
			val sin = sin(angle / 2)
			return QuaternionFloat(
					axis.x * sin,
					axis.y * sin,
					axis.z * sin,
					cos(angle / 2))
		}

		fun getAngle(q1: QuaternionFloat, q2: QuaternionFloat): Float {
			val cos = q1.dot(q2) / q1.magnitude() / q2.magnitude()
			return acos(cos)
		}

		fun getRotation2(source: Vector3f, target: Vector3f): QuaternionFloat {
			val angle = ru.casperix.math.vector.getAngle(source, target)
			if (angle < 0.00001f) return fromAxisAnge(source, 0f)
			return fromAxisAnge(source.cross(target).normalize(), angle)
		}

		fun getRotation(v1: Vector3f, v2: Vector3f): QuaternionFloat {
			val d = v1.dot(v2)
			val axis = v1.cross(v2)
			val qw = sqrt(v1.lengthSquared() * v2.lengthSquared()) + d
			if (qw < 0.001f) { // vectors are 180 degrees apart
				return QuaternionFloat(-v1.z, v1.y, v1.x, 0f)
			}
			return QuaternionFloat(axis.x, axis.y, axis.z, qw)
		}

		fun fromEulerAngles(xyz: Vector3f): QuaternionFloat /* yaw (Z), pitch (Y), roll (X)*/ {
			return fromEulerAngles(xyz.z, xyz.y, xyz.x)
		}

		fun fromEulerAngles(yaw: Float, pitch: Float, roll: Float): QuaternionFloat /* yaw (Z), pitch (Y), roll (X)*/ {
			// Abbreviations for the various angular functions
			val sinYaw = sin(yaw * 0.5f)
			val cosYaw = cos(yaw * 0.5f)
			val sinPitch = sin(pitch * 0.5f)
			val cosPitch = cos(pitch * 0.5f)
			val sinRoll = sin(roll * 0.5f)
			val cosRoll = cos(roll * 0.5f)

			return QuaternionFloat(
					sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
					cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
					cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
					cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw)
		}


		fun fromXZ(xAxis: Vector3f, zAxis: Vector3f): QuaternionFloat {
			val yAxis = zAxis.cross(xAxis)
			val matrix = Matrix4f.createByAxis(xAxis, yAxis, zAxis)
			return fromMatrix(matrix)
		}

		fun fromYZ(yAxis: Vector3f, zAxis: Vector3f): QuaternionFloat {
			val xAxis = yAxis.cross(zAxis)
			val matrix = Matrix4f.createByAxis(xAxis, yAxis, zAxis)
			return fromMatrix(matrix)
		}

		fun fromAxis(xAxis: Vector3f, yAxis: Vector3f, zAxis: Vector3f): QuaternionFloat {
			val matrix = Matrix4f.createByAxis(xAxis, yAxis, zAxis)
			return fromMatrix(matrix)
		}

		fun fromMatrix(m: Matrix4f): QuaternionFloat {
			val q = mutableListOf(.0f, .0f, .0f, .0f)

			val nxt = arrayOf(1, 2, 0)
			val tr = m[0, 0] + m[1, 1] + m[2, 2]

			if (tr > 0f) {
				val s = sqrt(tr + 1f)
				val t = 0.5f / s
				return QuaternionFloat(
						(m[1, 2] - m[2, 1]) * t,
						(m[2, 0] - m[0, 2]) * t,
						(m[0, 1] - m[1, 0]) * t,
						s / 2f)
			} else {
				var i = 0;
				if (m[1, 1] > m[0, 0]) i = 1
				if (m[2, 2] > m[i, i]) i = 2

				val j = nxt[i]
				val k = nxt[j]

				var s = sqrt((m[i, i] - (m[j, j] + m[k, k])) + 1f)

				q[i] = s * 0.5f

				if (s != 0f) s = 0.5f / s;

				q[3] = (m[j, k] - m[k, j]) * s
				q[j] = (m[i, j] + m[j, i]) * s
				q[k] = (m[i, k] + m[k, i]) * s

				return QuaternionFloat(q[0], q[1], q[2], q[3])
			}
		}
	}
}