Math updates. Renamed lerp functions, added quaternion_rotation

Cleptomania · Cleptomania · commit ca2b6ee39fbd · 2024-04-21T01:05:53.000-04:00
diff --git a/python/arcade_accelerate/__init__.py b/python/arcade_accelerate/__init__.py
@@ -36,7 +36,8 @@ def patch_math(patches):
     patches["arcade.math"].rotate_point = arcade_accelerate.rotate_point
     patches["arcade.math"].clamp = arcade_accelerate.clamp
     patches["arcade.math"].lerp = arcade_accelerate.lerp
-    patches["arcade.math"].lerp_vec = arcade_accelerate.lerp_vec
+    patches["arcade.math"].lerp_2d = arcade_accelerate.lerp_2d
+    patches["arcade.math"].lerp_3d = arcade_accelerate.lerp_3d
     patches["arcade.math"].lerp_angle = arcade_accelerate.lerp_angle
     patches["arcade.math"].get_distance = arcade_accelerate.get_distance
     patches["arcade.math"].get_angle_degrees = arcade_accelerate.get_angle_degrees
@@ -53,6 +54,7 @@ def patch_math(patches):
         arcade_accelerate.rand_vec_degree_spread
     )
     patches["arcade.math"].rand_vec_magnitude = arcade_accelerate.rand_vec_magnitude
+    patches["arcade.math"].quaternion_rotation = arcade_accelerate.quaternion_rotation
 
 
 def patch_geometry(patches):
diff --git a/src/lib.rs b/src/lib.rs
@@ -20,7 +20,8 @@ fn arcade_accelerate(_py: Python, m: &PyModule) -> PyResult<()> {
     m.add_function(wrap_pyfunction!(math::rotate_point, m)?)?;
     m.add_function(wrap_pyfunction!(math::clamp, m)?)?;
     m.add_function(wrap_pyfunction!(math::lerp, m)?)?;
-    m.add_function(wrap_pyfunction!(math::lerp_vec, m)?)?;
+    m.add_function(wrap_pyfunction!(math::lerp_2d, m)?)?;
+    m.add_function(wrap_pyfunction!(math::lerp_3d, m)?)?;
     m.add_function(wrap_pyfunction!(math::lerp_angle, m)?)?;
     m.add_function(wrap_pyfunction!(math::get_distance, m)?)?;
     m.add_function(wrap_pyfunction!(math::get_angle_degrees, m)?)?;
@@ -33,6 +34,7 @@ fn arcade_accelerate(_py: Python, m: &PyModule) -> PyResult<()> {
     m.add_function(wrap_pyfunction!(math::rand_angle_spread_deg, m)?)?;
     m.add_function(wrap_pyfunction!(math::rand_vec_degree_spread, m)?)?;
     m.add_function(wrap_pyfunction!(math::rand_vec_magnitude, m)?)?;
+    m.add_function(wrap_pyfunction!(math::quaternion_rotation, m)?)?;
     m.add_function(wrap_pyfunction!(geometry::are_polygons_intersecting, m)?)?;
     m.add_function(wrap_pyfunction!(geometry::is_point_in_polygon, m)?)?;
     m.add_function(wrap_pyfunction!(geometry::is_point_in_box, m)?)?;
diff --git a/src/math.rs b/src/math.rs
@@ -41,10 +41,19 @@ pub fn lerp(v1: f32, v2: f32, u: f32) -> f32 {
 }
 
 #[pyfunction]
-pub fn lerp_vec(v1: (f32, f32), v2: (f32, f32), u: f32) -> (f32, f32) {
+pub fn lerp_2d(v1: (f32, f32), v2: (f32, f32), u: f32) -> (f32, f32) {
     (lerp(v1.0, v2.0, u), lerp(v1.1, v2.1, u))
 }
 
+#[pyfunction]
+pub fn lerp_3d(v1: (f32, f32, f32), v2: (f32, f32, f32), u: f32) -> (f32, f32, f32) {
+    (
+        lerp(v1.0, v2.0, u),
+        lerp(v1.1, v2.1, u),
+        lerp(v1.2, v2.2, u),
+    )
+}
+
 #[pyfunction]
 pub fn lerp_angle(start_angle: f32, end_angle: f32, u: f32) -> f32 {
     let temp_start = start_angle % 360.0;
@@ -130,7 +139,7 @@ pub fn rand_on_line(pos1: (f32, f32), pos2: (f32, f32)) -> (f32, f32) {
     let mut rng = thread_rng();
     let u: f32 = rng.gen_range(0.0..1.0);
 
-    lerp_vec(pos1, pos2, u)
+    lerp_2d(pos1, pos2, u)
 }
 
 #[pyfunction]
@@ -164,6 +173,29 @@ pub fn rand_vec_magnitude(angle: f32, lo_magnitude: f32, hi_magnitude: f32) -> (
     vel.as_tuple()
 }
 
+#[pyfunction]
+pub fn quaternion_rotation(
+    axis: (f32, f32, f32),
+    vector: (f32, f32, f32),
+    angle: f32,
+) -> (f32, f32, f32) {
+    let angle_rads = -angle.to_radians();
+    let (c2, s2) = (f32::cos(angle_rads / 2.0), f32::sin(angle_rads / 2.0));
+
+    let (q0, q1, q2, q3) = (c2, s2 * axis.0, s2 * axis.1, s2 * axis.2);
+    let (q0_2, q1_2, q2_2, q3_2) = (q0.powi(2), q1.powi(2), q2.powi(2), q3.powi(2));
+    let (q01, q02, q03, q12, q13, q23) = (q0 * q1, q0 * q2, q0 * q3, q1 * q2, q1 * q3, q2 * q3);
+
+    let x = vector.0 * (q0_2 + q1_2 - q2_2 - q3_2)
+        + 2.0 * (vector.1 * (q12 - q03) + vector.2 * (q02 + q13));
+    let y = vector.1 * (q0_2 - q1_2 + q2_2 - q3_2)
+        + 2.0 * (vector.0 * (q03 + q12) + vector.2 * (q23 - q01));
+    let z = vector.2 * (q0_2 - q1_2 - q2_2 + q3_2)
+        + 2.0 * (vector.0 * (q13 + q02) + vector.1 * (q01 + q23));
+
+    (x, y, z)
+}
+
 // This is only a subset of _Vec2 methods defined in arcade.math.py
 struct _Vec2 {
     x: f32,
@@ -262,14 +294,23 @@ mod tests {
     }
 
     #[test]
-    fn test_lerp_vec() {
-        let mut result = lerp_vec((0.0, 2.0), (8.0, 4.0), 0.25);
+    fn test_lerp_2d() {
+        let mut result = lerp_2d((0.0, 2.0), (8.0, 4.0), 0.25);
         assert_eq!(result, (2.0, 2.5));
 
-        result = lerp_vec((0.0, 2.0), (8.0, 4.0), -0.25);
+        result = lerp_2d((0.0, 2.0), (8.0, 4.0), -0.25);
         assert_eq!(result, (-2.0, 1.5));
     }
 
+    #[test]
+    fn test_lerp_3d() {
+        let mut result = lerp_3d((0.0, 2.0, 4.0), (8.0, 4.0, 8.0), 0.25);
+        assert_eq!(result, (2.0, 2.5, 5.0));
+
+        result = lerp_3d((0.0, 2.0, 4.0), (8.0, 4.0, 8.0), -0.25);
+        assert_eq!(result, (-2.0, 1.5, 3.0));
+    }
+
     #[test]
     fn test_lerp_angle_normal() {
         //normal
