1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

use serde::{
	Deserialize,
	Serialize,
};

use crate::Vector3;

#[derive(Copy, Clone, Default, Debug, PartialEq, PartialOrd, Serialize, Deserialize)]
pub struct Quaternion {
	pub x: f32,
	pub y: f32,
	pub z: f32,
	pub w: f32,
}

impl Quaternion {
	pub const IDENTITY: Self = Self {
		x: 0.0,
		y: 0.0,
		z: 0.0,
		w: 1.0,
	};

	pub fn from_axis_angle(axis: impl Into<Vector3>, theta: f32) -> Self {
		let axis = axis.into();

		let theta = theta / 2.0;

		let s = theta.sin();
		let c = theta.cos();
		Self {
			x: s * axis.x,
			y: s * axis.y,
			z: s * axis.z,
			w: c,
		}
	}

	pub fn from_euler(euler: impl Into<Vector3>) -> Self {
		let euler = euler.into() * 0.5;

		let cr = euler.x.cos();
		let sr = euler.x.sin();

		let cp = euler.y.cos();
		let sp = euler.y.sin();

		let cy = euler.z.cos();
		let sy = euler.z.sin();

		Self {
			x: sr * cp * cy - cr * sp * sy,
			y: cr * sp * cy + sr * cp * sy,
			z: cr * cp * sy - sr * sp * cy,
			w: cr * cp * cy + sr * sp * sy,
		}
	}

	pub fn len_sq(self) -> f32 {
		self.x * self.x + self.y * self.y + self.z * self.z + self.w * self.w
	}

	pub fn len(self) -> f32 {
		self.len_sq().sqrt()
	}

	pub fn norm(self) -> Self {
		let len = self.len();

		// TODO: Use a threshold
		if len == 0.0 {
			Default::default()
		} else {
			let inv = 1.0 / len;
			Self {
				x: self.x * inv,
				y: self.y * inv,
				z: self.z * inv,
				w: self.w * inv,
			}
		}
	}

	pub fn inverse(self) -> Self {
		Self {
			x: -self.x,
			y: -self.y,
			z: -self.z,
			w: self.w,
		}
	}

	pub fn rotate(self, xyz: Vector3) -> Vector3 {
		let t = self.xyz().cross(xyz) * 2.0 * self.w;
		(xyz + t) + self.xyz().cross(t)
	}

	pub fn forward(self) -> Vector3 {
		self.rotate(Vector3::FORWARD)
	}

	pub fn right(self) -> Vector3 {
		self.rotate(Vector3::RIGHT)
	}

	pub fn up(self) -> Vector3 {
		self.rotate(Vector3::UP)
	}

	pub fn xyz(self) -> Vector3 {
		Vector3 {
			x: self.x,
			y: self.y,
			z: self.z,
		}
	}
}