use std::f64::consts::PI;
const TWO_THIRDS: f64 = 2.0 / 3.0;
pub fn clarke(a: f64, b: f64, c: f64) -> [f64; 3] {
let alpha = TWO_THIRDS * (a - 0.5 * b - 0.5 * c);
let beta = (b - c) / 3.0_f64.sqrt();
let zero = (a + b + c) / 3.0;
[alpha, beta, zero]
}
pub fn clarke_inv(alpha: f64, beta: f64, zero: f64) -> [f64; 3] {
let a = alpha + zero;
let b = -0.5 * alpha + 0.5 * 3.0_f64.sqrt() * beta + zero;
let c = -0.5 * alpha - 0.5 * 3.0_f64.sqrt() * beta + zero;
[a, b, c]
}
pub fn park(alpha: f64, beta: f64, zero: f64, theta: f64) -> [f64; 3] {
let d = alpha * theta.cos() + beta * theta.sin();
let q = -alpha * theta.sin() + beta * theta.cos();
[d, q, zero]
}
pub fn park_inv(d: f64, q: f64, zero: f64, theta: f64) -> [f64; 3] {
let alpha = d * theta.cos() - q * theta.sin();
let beta = d * theta.sin() + q * theta.cos();
[alpha, beta, zero]
}
pub fn clarke_park(a: f64, b: f64, c: f64, theta: f64) -> [f64; 3] {
let [alpha, beta, zero] = clarke(a, b, c);
park(alpha, beta, zero, theta)
}
pub fn clarke_park_inv(d: f64, q: f64, zero: f64, theta: f64) -> [f64; 3] {
let [alpha, beta, zero] = park_inv(d, q, zero, theta);
clarke_inv(alpha, beta, zero)
}
pub fn anglewrap(alpha: f64, beta: f64) -> f64 {
let theta = beta.atan2(alpha);
if theta >= 0.0 {
theta
} else {
theta + 2.0 * PI
}
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum CoordinateTransform {
Clarke,
ClarkeInv,
Park,
ParkInv,
ClarkePark,
ClarkeParkInv,
}
impl CoordinateTransform {
pub fn input_count(self) -> usize {
match self {
CoordinateTransform::Clarke | CoordinateTransform::ClarkeInv => 3,
CoordinateTransform::Park
| CoordinateTransform::ParkInv
| CoordinateTransform::ClarkePark
| CoordinateTransform::ClarkeParkInv => 4,
}
}
pub fn name(self) -> &'static str {
match self {
CoordinateTransform::Clarke => "clarke",
CoordinateTransform::ClarkeInv => "clarkeinv",
CoordinateTransform::Park => "park",
CoordinateTransform::ParkInv => "parkinv",
CoordinateTransform::ClarkePark => "clarkepark",
CoordinateTransform::ClarkeParkInv => "clarkeparkinv",
}
}
pub fn output_names(self) -> [&'static str; 3] {
match self {
CoordinateTransform::Clarke => ["alpha", "beta", "zero"],
CoordinateTransform::ClarkeInv => ["a", "b", "c"],
CoordinateTransform::Park | CoordinateTransform::ClarkePark => ["d", "q", "zero"],
CoordinateTransform::ParkInv => ["alpha", "beta", "zero"],
CoordinateTransform::ClarkeParkInv => ["a", "b", "c"],
}
}
pub fn call(self, inputs: &[f64]) -> [f64; 3] {
match self {
CoordinateTransform::Clarke => clarke(inputs[0], inputs[1], inputs[2]),
CoordinateTransform::ClarkeInv => clarke_inv(inputs[0], inputs[1], inputs[2]),
CoordinateTransform::Park => park(inputs[0], inputs[1], inputs[2], inputs[3]),
CoordinateTransform::ParkInv => park_inv(inputs[0], inputs[1], inputs[2], inputs[3]),
CoordinateTransform::ClarkePark => {
clarke_park(inputs[0], inputs[1], inputs[2], inputs[3])
}
CoordinateTransform::ClarkeParkInv => {
clarke_park_inv(inputs[0], inputs[1], inputs[2], inputs[3])
}
}
}
}
#[cfg(test)]
mod tests {
use super::*;
const TOL: f64 = 1e-9;
fn assert_close3(got: [f64; 3], want: [f64; 3]) {
for i in 0..3 {
assert!(
(got[i] - want[i]).abs() < TOL,
"component {i}: got {}, want {}",
got[i],
want[i]
);
}
}
const A: f64 = 1.0;
const B: f64 = -0.5;
const C: f64 = -0.5;
#[test]
fn clarke_matches_hand_derived_balanced_snapshot() {
assert_close3(clarke(A, B, C), [1.0, 0.0, 0.0]);
}
#[test]
fn clarke_inv_is_exact_inverse_of_clarke() {
let [alpha, beta, zero] = clarke(A, B, C);
assert_close3(clarke_inv(alpha, beta, zero), [A, B, C]);
let (a, b, c) = (3.0, -1.0, 0.4);
let [alpha, beta, zero] = clarke(a, b, c);
assert_close3(clarke_inv(alpha, beta, zero), [a, b, c]);
}
#[test]
fn park_inv_is_exact_inverse_of_park() {
let (alpha, beta, zero, theta) = (0.7, -1.3, 0.2, 1.234);
let [d, q, z] = park(alpha, beta, zero, theta);
assert_close3(park_inv(d, q, z, theta), [alpha, beta, zero]);
}
#[test]
fn balanced_three_phase_at_its_own_angle_gives_constant_dq() {
let amplitude = 2.0_f64;
for &theta in &[0.0, 0.4, PI / 2.0, 2.1, 5.9] {
let a = amplitude * theta.cos();
let b = amplitude * (theta - 2.0 * PI / 3.0).cos();
let c = amplitude * (theta + 2.0 * PI / 3.0).cos();
let [d, q, zero] = clarke_park(a, b, c, theta);
assert!((d - amplitude).abs() < TOL, "theta={theta}: d={d}");
assert!(q.abs() < TOL, "theta={theta}: q={q}");
assert!(zero.abs() < TOL, "theta={theta}: zero={zero}");
}
}
#[test]
fn clarke_park_matches_clarke_then_park() {
let (a, b, c, theta) = (1.1, -0.6, -0.3, 0.77);
let [alpha, beta, zero] = clarke(a, b, c);
let composed = park(alpha, beta, zero, theta);
let fused = clarke_park(a, b, c, theta);
assert_close3(fused, composed);
}
#[test]
fn clarke_park_inv_matches_park_inv_then_clarke_inv() {
let (d, q, zero, theta) = (0.9, -0.2, 0.1, 2.3);
let [alpha, beta, z] = park_inv(d, q, zero, theta);
let composed = clarke_inv(alpha, beta, z);
let fused = clarke_park_inv(d, q, zero, theta);
assert_close3(fused, composed);
}
#[test]
fn clarke_park_inv_is_exact_inverse_of_clarke_park() {
let (a, b, c, theta) = (2.0, -1.0, -1.0, 0.5);
let [d, q, zero] = clarke_park(a, b, c, theta);
assert_close3(clarke_park_inv(d, q, zero, theta), [a, b, c]);
}
#[test]
fn anglewrap_matches_hand_derived_quadrants_and_wraps_negative_into_0_2pi() {
assert!((anglewrap(1.0, 1.0) - PI / 4.0).abs() < TOL);
assert!((anglewrap(0.0, 1.0) - PI / 2.0).abs() < TOL);
assert!((anglewrap(-1.0, -1.0) - 5.0 * PI / 4.0).abs() < TOL);
assert!((anglewrap(0.0, -1.0) - 3.0 * PI / 2.0).abs() < TOL);
}
#[test]
fn anglewrap_tracks_a_full_rotation_consistent_with_clarke() {
let amplitude = 1.0_f64;
for &theta in &[0.0, 0.1, PI / 2.0, PI, 3.0 * PI / 2.0 + 0.2, 2.0 * PI - 0.1] {
let a = amplitude * theta.cos();
let b = amplitude * (theta - 2.0 * PI / 3.0).cos();
let c = amplitude * (theta + 2.0 * PI / 3.0).cos();
let [alpha, beta, _zero] = clarke(a, b, c);
let recovered = anglewrap(alpha, beta);
assert!(
(recovered - theta).abs() < 1e-6,
"theta={theta}: recovered={recovered}"
);
}
}
#[test]
fn coordinate_transform_dispatch_matches_direct_calls() {
let (a, b, c, theta) = (1.0, -0.5, -0.5, 0.3);
assert_eq!(
CoordinateTransform::Clarke.call(&[a, b, c]),
clarke(a, b, c)
);
assert_eq!(CoordinateTransform::Clarke.input_count(), 3);
assert_eq!(
CoordinateTransform::Clarke.output_names(),
["alpha", "beta", "zero"]
);
assert_eq!(
CoordinateTransform::ClarkePark.call(&[a, b, c, theta]),
clarke_park(a, b, c, theta)
);
assert_eq!(CoordinateTransform::ClarkePark.input_count(), 4);
assert_eq!(
CoordinateTransform::ClarkePark.output_names(),
["d", "q", "zero"]
);
}
}