apply clippy float optimization recommendations

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));

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))),
         )
     }
 }

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.

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