oxiphysics_python/world_api/
math_helpers.rs

1// Copyright 2026 COOLJAPAN OU (Team KitaSan)
2// SPDX-License-Identifier: Apache-2.0
3
4//! Vec3 / Quaternion conversion helpers for the Python binding layer.
5
6#![allow(missing_docs)]
7
8// ===========================================================================
9// Vec3 / Quaternion conversion helpers (for Python binding layer)
10// ===========================================================================
11
12/// Convert a `[f64; 3]` array to `PyVec3`.
13#[inline]
14#[allow(dead_code)]
15pub fn array_to_vec3(arr: [f64; 3]) -> crate::types::PyVec3 {
16    crate::types::PyVec3::from_array(arr)
17}
18
19/// Convert `PyVec3` to a `[f64; 3]` array.
20#[inline]
21#[allow(dead_code)]
22pub fn vec3_to_array(v: crate::types::PyVec3) -> [f64; 3] {
23    v.to_array()
24}
25
26/// Normalize a quaternion `[x, y, z, w]` and return it.
27///
28/// If the quaternion is near-zero, returns the identity `[0,0,0,1]`.
29#[inline]
30#[allow(dead_code)]
31pub fn quat_normalize(q: [f64; 4]) -> [f64; 4] {
32    let norm = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]).sqrt();
33    if norm < 1e-12 {
34        [0.0, 0.0, 0.0, 1.0]
35    } else {
36        [q[0] / norm, q[1] / norm, q[2] / norm, q[3] / norm]
37    }
38}
39
40/// Multiply two quaternions q1 * q2 (Hamilton product).
41///
42/// Both inputs are `[x, y, z, w]`.
43#[allow(dead_code)]
44pub fn quat_mul(q1: [f64; 4], q2: [f64; 4]) -> [f64; 4] {
45    let (ax, ay, az, aw) = (q1[0], q1[1], q1[2], q1[3]);
46    let (bx, by, bz, bw) = (q2[0], q2[1], q2[2], q2[3]);
47    [
48        aw * bx + ax * bw + ay * bz - az * by,
49        aw * by - ax * bz + ay * bw + az * bx,
50        aw * bz + ax * by - ay * bx + az * bw,
51        aw * bw - ax * bx - ay * by - az * bz,
52    ]
53}
54
55/// Conjugate (inverse for unit quaternion) of `[x, y, z, w]`.
56#[allow(dead_code)]
57pub fn quat_conjugate(q: [f64; 4]) -> [f64; 4] {
58    [-q[0], -q[1], -q[2], q[3]]
59}
60
61/// Rotate a vector `v` by unit quaternion `q`.
62///
63/// Uses sandwich product: v' = q * \[v,0\] * q*.
64#[allow(dead_code)]
65pub fn quat_rotate_vec(q: [f64; 4], v: [f64; 3]) -> [f64; 3] {
66    let (qx, qy, qz, qw) = (q[0], q[1], q[2], q[3]);
67    let (vx, vy, vz) = (v[0], v[1], v[2]);
68    // t = 2 * cross(q.xyz, v)
69    let tx = 2.0 * (qy * vz - qz * vy);
70    let ty = 2.0 * (qz * vx - qx * vz);
71    let tz = 2.0 * (qx * vy - qy * vx);
72    [
73        vx + qw * tx + qy * tz - qz * ty,
74        vy + qw * ty + qz * tx - qx * tz,
75        vz + qw * tz + qx * ty - qy * tx,
76    ]
77}
78
79/// Create a quaternion from an axis-angle representation.
80///
81/// `axis` need not be normalised; `angle` is in radians.
82#[allow(dead_code)]
83pub fn quat_from_axis_angle(axis: [f64; 3], angle: f64) -> [f64; 4] {
84    let len = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]).sqrt();
85    if len < 1e-12 {
86        return [0.0, 0.0, 0.0, 1.0];
87    }
88    let half = angle * 0.5;
89    let s = half.sin() / len;
90    [axis[0] * s, axis[1] * s, axis[2] * s, half.cos()]
91}
oxiphysics_python/world_api/math_helpers.rs

oxiphysics_python/world_api/
math_helpers.rs
