commonMain.ru.casperix.math.quaternion.float64.QuaternionDouble.kt Maven / Gradle / Ivy

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

import ru.casperix.math.quad_matrix.float64.Matrix4d
import ru.casperix.math.vector.float64.Vector3d
import ru.casperix.misc.toPrecision
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 QuaternionDouble(val x: Double, val y: Double, val z: Double, val w: Double) {

	constructor(): this (0.0, 0.0, 0.0, 1.0)

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

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

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

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

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

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

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

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

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

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

	operator fun times(other: QuaternionDouble): QuaternionDouble {
		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 QuaternionDouble(crossX + addX, crossY + addY, crossZ + addZ, W)
	}

	fun getVector3d(): Vector3d {
		return Vector3d(x, y, z)
	}

	operator fun unaryMinus(): QuaternionDouble {
		return this * (-1.0)
	}

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

	fun transform(value: Vector3d): Vector3d {
		return (this * value.getQuaternion() * inverse()).getVector3d()
	}

	fun toEuler(): Vector3d {

		// 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 / 2.0).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 Vector3d(roll, pitch, yaw);
	}

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

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

	fun normalize(): QuaternionDouble {
		val len = length()
		if (len == 0.0) {
			return this
		}
		return this / len
	}

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

	companion object {
		val IDENTITY = QuaternionDouble(0.0, 0.0, 0.0, 1.0)

		fun fromAxisAnge(axis: Vector3d, angle: Double): QuaternionDouble {
			val s = sin(angle / 2.0)
			return QuaternionDouble(
					axis.x * s,
					axis.y * s,
					axis.z * s,
					cos(angle / 2.0))
		}

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

		fun getRotation2(source: Vector3d, target: Vector3d): QuaternionDouble {
			val angle = ru.casperix.math.vector.getAngle(source, target)
			if (angle < 0.000001) return fromAxisAnge(source, 0.0)
			return fromAxisAnge(source.cross(target).normalize(), angle)
		}

		fun getRotation(v1: Vector3d, v2: Vector3d): QuaternionDouble {
			val d = v1.dot(v2)
			val axis = v1.cross(v2)
			val qw = sqrt(v1.lengthSquared() * v2.lengthSquared()) + d
			if (qw < 0.0001) { // vectors are 180 degrees apart
				return QuaternionDouble(-v1.z, v1.y, v1.x, 0.0)
			}
			return QuaternionDouble(axis.x, axis.y, axis.z, qw)
		}

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

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

			return QuaternionDouble(
					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: Vector3d, zAxis: Vector3d): QuaternionDouble {
			val yAxis = zAxis.cross(xAxis)
			val matrix = Matrix4d.createByAxis(xAxis, yAxis, zAxis)
			return fromMatrix(matrix)
		}

		fun fromYZ(yAxis: Vector3d, zAxis: Vector3d): QuaternionDouble {
			val xAxis = yAxis.cross(zAxis)
			val matrix = Matrix4d.createByAxis(xAxis, yAxis, zAxis)
			return fromMatrix(matrix)
		}

		fun fromAxis(xAxis: Vector3d, yAxis: Vector3d, zAxis: Vector3d): QuaternionDouble {
			val matrix = Matrix4d.createByAxis(xAxis, yAxis, zAxis)
			return fromMatrix(matrix)
		}

		fun fromMatrix(m: Matrix4d): QuaternionDouble {
			val q = mutableListOf(.0, .0, .0, .0)

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

			if (tr > 0.0) {
				val s = sqrt(tr + 1.0)
				val t = 0.5 / s
				return QuaternionDouble(
						(m[1, 2] - m[2, 1]) * t,
						(m[2, 0] - m[0, 2]) * t,
						(m[0, 1] - m[1, 0]) * t,
						s / 2.0)
			} 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])) + 1.0)

				q[i] = s * 0.5;

				if (s != 0.0) s = 0.5 / 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 QuaternionDouble(q[0], q[1], q[2], q[3])
			}
		}
	}
}