diff --git a/kinematics/src/head.rs b/kinematics/src/head.rs index 95e1ec5e..a5a0c7b7 100644 --- a/kinematics/src/head.rs +++ b/kinematics/src/head.rs @@ -141,7 +141,7 @@ impl HeadFk { ]; // cv2 camera axes: +x right, +y down, +z forward. let v = cam.quat.conjugate().rotate(v); - let flat = (v[0] * v[0] + v[2] * v[2]).sqrt(); + let flat = v[0].hypot(v[2]); [v[0].atan2(v[2]), v[1].atan2(flat)] }; @@ -166,19 +166,22 @@ impl HeadFk { // Solve (JᵀJ + λI) Δ = -Jᵀe, the 2×2 case written out. let (a, b) = ( - j[0][0] * j[0][0] + j[1][0] * j[1][0] + LAMBDA, - j[0][0] * j[0][1] + j[1][0] * j[1][1], + j[1][0].mul_add(j[1][0], j[0][0] * j[0][0]) + LAMBDA, + j[1][0].mul_add(j[1][1], j[0][0] * j[0][1]), ); - let d = j[0][1] * j[0][1] + j[1][1] * j[1][1] + LAMBDA; + let d = j[1][1].mul_add(j[1][1], j[0][1] * j[0][1]) + LAMBDA; let g = [ - j[0][0] * e[0] + j[1][0] * e[1], - j[0][1] * e[0] + j[1][1] * e[1], + j[1][0].mul_add(e[1], j[0][0] * e[0]), + j[1][1].mul_add(e[1], j[0][1] * e[0]), ]; - let det = a * d - b * b; + let det = b.mul_add(-b, a * d); if det.abs() < 1e-12 { break; // fully singular and damped-out: nothing left to gain } - let step = [(-d * g[0] + b * g[1]) / det, (b * g[0] - a * g[1]) / det]; + let step = [ + b.mul_add(g[1], -d * g[0]) / det, + a.mul_add(-g[1], b * g[0]) / det, + ]; let scale = (MAX_STEP / step[0].hypot(step[1])).min(1.0); joints[1] = clamp(joints[1] + scale * step[0], range(1)); joints[2] = clamp(joints[2] + scale * step[1], range(2)); diff --git a/kinematics/src/math.rs b/kinematics/src/math.rs index 65172acd..298ce818 100644 --- a/kinematics/src/math.rs +++ b/kinematics/src/math.rs @@ -40,7 +40,14 @@ impl Quat { /// attribute, and a model that ignores a broken quat is diagnosable where a /// model full of NaN is not. pub fn normalized(self) -> Self { - let n = (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z).sqrt(); + let n = self + .z + .mul_add( + self.z, + self.y + .mul_add(self.y, self.x.mul_add(self.x, self.w * self.w)), + ) + .sqrt(); if n < 1e-12 { return Self::IDENTITY; } @@ -57,13 +64,16 @@ impl Quat { /// full quaternion sandwich. pub fn rotate(self, v: [f64; 3]) -> [f64; 3] { let [vx, vy, vz] = v; - let tx = 2.0 * (self.y * vz - self.z * vy); - let ty = 2.0 * (self.z * vx - self.x * vz); - let tz = 2.0 * (self.x * vy - self.y * vx); + let tx = 2.0 * self.z.mul_add(-vy, self.y * vz); + let ty = 2.0 * self.x.mul_add(-vz, self.z * vx); + let tz = 2.0 * self.y.mul_add(-vx, self.x * vy); [ - vx + self.w * tx + self.y * tz - self.z * ty, - vy + self.w * ty + self.z * tx - self.x * tz, - vz + self.w * tz + self.x * ty - self.y * tx, + self.z + .mul_add(-ty, self.y.mul_add(tz, self.w.mul_add(tx, vx))), + self.x + .mul_add(-tz, self.z.mul_add(tx, self.w.mul_add(ty, vy))), + self.y + .mul_add(-tx, self.x.mul_add(ty, self.w.mul_add(tz, vz))), ] } @@ -80,8 +90,8 @@ impl Quat { /// Yaw about world +z, for an estimator that reports heading as one angle. pub fn yaw(self) -> f64 { // atan2 of the rotation matrix's (1,0) over (0,0) elements, expanded. - let siny = 2.0 * (self.w * self.z + self.x * self.y); - let cosy = 1.0 - 2.0 * (self.y * self.y + self.z * self.z); + let siny = 2.0 * self.x.mul_add(self.y, self.w * self.z); + let cosy = 2.0f64.mul_add(-self.z.mul_add(self.z, self.y * self.y), 1.0); siny.atan2(cosy) } } @@ -92,10 +102,10 @@ impl Mul for Quat { fn mul(self, b: Quat) -> Quat { let a = self; Quat::new( - a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z, - a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, - a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, - a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, + a.z.mul_add(-b.z, a.y.mul_add(-b.y, a.x.mul_add(-b.x, a.w * b.w))), + a.z.mul_add(-b.y, a.y.mul_add(b.z, a.x.mul_add(b.w, a.w * b.x))), + a.z.mul_add(b.x, a.y.mul_add(b.w, a.x.mul_add(-b.z, a.w * b.y))), + a.z.mul_add(b.w, a.y.mul_add(-b.x, a.x.mul_add(b.y, a.w * b.z))), ) } } diff --git a/kinematics/src/mjcf.rs b/kinematics/src/mjcf.rs index 374b538d..5bdbf7b7 100644 --- a/kinematics/src/mjcf.rs +++ b/kinematics/src/mjcf.rs @@ -217,7 +217,7 @@ fn parse_floats_attr( } fn normalize(v: [f64; 3]) -> [f64; 3] { - let n = (v[0] * v[0] + v[1] * v[1] + v[2] * v[2]).sqrt(); + let n = v[2].mul_add(v[2], v[1].mul_add(v[1], v[0] * v[0])).sqrt(); if n < 1e-12 { // A zero axis is a broken model; +z (MJCF's own default) keeps the // parse usable and the error visible in FK rather than as NaN. diff --git a/kinematics/src/tof.rs b/kinematics/src/tof.rs index 35cc94e4..ac4199ed 100644 --- a/kinematics/src/tof.rs +++ b/kinematics/src/tof.rs @@ -109,8 +109,8 @@ impl Reprojector { let step = 2.0 * half / (COLS as f64 - 1.0); let mut beams = [[0.0; 3]; N_ZONES]; for (i, beam) in beams.iter_mut().enumerate() { - let elevation = half - (i / COLS) as f64 * step; - let azimuth = half - (i % COLS) as f64 * step; + let elevation = ((i / COLS) as f64).mul_add(-step, half); + let azimuth = ((i % COLS) as f64).mul_add(-step, half); *beam = [ elevation.cos() * azimuth.cos(), elevation.cos() * azimuth.sin(), @@ -175,15 +175,15 @@ impl Reprojector { // Positive when the beam looks below the *world* horizon. let downward = -dir_level[2]; let point = [ - sensor.pos[0] + r * dir[0], - sensor.pos[1] + r * dir[1], - sensor.pos[2] + r * dir[2], + r.mul_add(dir[0], sensor.pos[0]), + r.mul_add(dir[1], sensor.pos[1]), + r.mul_add(dir[2], sensor.pos[2]), ]; if above_floor > 0.0 && downward > 0.0 && r * downward >= floor_threshold { zones[i] = Zone::Floor { point }; continue; } - let horizontal = r * (dir_level[0] * dir_level[0] + dir_level[1] * dir_level[1]).sqrt(); + let horizontal = r * dir_level[0].hypot(dir_level[1]); if horizontal < Self::MIN_RANGE_M { zones[i] = Zone::TooClose; continue; @@ -201,19 +201,26 @@ impl Reprojector { /// a gravity too small to trust — an IMU that has not converged should level /// nothing rather than something random. fn level_from_gravity(gravity: [f64; 3]) -> Quat { - let n = (gravity[0] * gravity[0] + gravity[1] * gravity[1] + gravity[2] * gravity[2]).sqrt(); + let n = gravity[2] + .mul_add( + gravity[2], + gravity[1].mul_add(gravity[1], gravity[0] * gravity[0]), + ) + .sqrt(); if n < 0.5 { return Quat::IDENTITY; } let g = [gravity[0] / n, gravity[1] / n, gravity[2] / n]; let down = [0.0, 0.0, -1.0f64]; let axis = [ - g[1] * down[2] - g[2] * down[1], - g[2] * down[0] - g[0] * down[2], - g[0] * down[1] - g[1] * down[0], + g[2].mul_add(-down[1], g[1] * down[2]), + g[0].mul_add(-down[2], g[2] * down[0]), + g[1].mul_add(-down[0], g[0] * down[1]), ]; - let s = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]).sqrt(); - let c = g[0] * down[0] + g[1] * down[1] + g[2] * down[2]; + let s = axis[2] + .mul_add(axis[2], axis[1].mul_add(axis[1], axis[0] * axis[0])) + .sqrt(); + let c = g[2].mul_add(down[2], g[1].mul_add(down[1], g[0] * down[0])); if s < 1e-9 { return if c > 0.0 { Quat::IDENTITY