@@ -96,10 +96,26 @@ struct Rotation {
9696 rot.quat [2 ] = q[2 ] / norm;
9797 rot.quat [3 ] = q[3 ] / norm;
9898
99- // Store original matrix to avoid quaternion roundtrip ULP-error
100- // in as_euler() and as_matrix().
99+ // Store quaternion-derived matrix to exactly match scipy's pipeline.
100+ // scipy: from_matrix → quat → normalize → quat_to_matrix → euler
101+ // Quat normalization can change matrix values by 1 ULP; scipy uses
102+ // the quaternion-derived matrix for as_euler(), not the raw input.
103+ // We MUST store the same quaternion-derived matrix for 0-bit match.
101104 rot.has_matrix = true ;
102- for (int i = 0 ; i < 9 ; ++i) rot.matrix [i] = matrix[i];
105+ {
106+ T x = rot.quat [0 ], y = rot.quat [1 ], z = rot.quat [2 ], w = rot.quat [3 ];
107+ T x2 = x*x, y2 = y*y, z2 = z*z, w2 = w*w;
108+ T xy = x*y, zw = z*w, xz = x*z, yw = y*w, yz = y*z, xw = x*w;
109+ rot.matrix [0 ] = x2 - y2 - z2 + w2;
110+ rot.matrix [1 ] = T (2 ) * (xy - zw);
111+ rot.matrix [2 ] = T (2 ) * (xz + yw);
112+ rot.matrix [3 ] = T (2 ) * (xy + zw);
113+ rot.matrix [4 ] = -x2 + y2 - z2 + w2;
114+ rot.matrix [5 ] = T (2 ) * (yz - xw);
115+ rot.matrix [6 ] = T (2 ) * (xz - yw);
116+ rot.matrix [7 ] = T (2 ) * (yz + xw);
117+ rot.matrix [8 ] = -x2 - y2 + z2 + w2;
118+ }
103119
104120 return rot;
105121 }
@@ -339,6 +355,28 @@ struct Rotation {
339355 throw std::invalid_argument (
340356 " Rotation::from_euler: unsupported single-axis seq '" + s + " '" );
341357 }
358+
359+ // Compute and store the quaternion-derived matrix so as_euler()
360+ // and as_matrix() use the same values as scipy's quat→matrix path.
361+ // Single-axis quaternion: no composition, no normalization needed
362+ // (norm = sh²+ch² = sin²(θ/2)+cos²(θ/2) = 1 exactly in ℝ,
363+ // but floating-point rounding may give norm ≈ 1 ± eps).
364+ // Using quat→matrix formulas matches scipy's exact path.
365+ rot.has_matrix = true ;
366+ {
367+ T x = rot.quat [0 ], y = rot.quat [1 ], z = rot.quat [2 ], w = rot.quat [3 ];
368+ T x2 = x*x, y2 = y*y, z2 = z*z, w2 = w*w;
369+ T xy = x*y, zw = z*w, xz = x*z, yw = y*w, yz = y*z, xw = x*w;
370+ rot.matrix [0 ] = x2 - y2 - z2 + w2;
371+ rot.matrix [1 ] = T (2 ) * (xy - zw);
372+ rot.matrix [2 ] = T (2 ) * (xz + yw);
373+ rot.matrix [3 ] = T (2 ) * (xy + zw);
374+ rot.matrix [4 ] = -x2 + y2 - z2 + w2;
375+ rot.matrix [5 ] = T (2 ) * (yz - xw);
376+ rot.matrix [6 ] = T (2 ) * (xz - yw);
377+ rot.matrix [7 ] = T (2 ) * (yz + xw);
378+ rot.matrix [8 ] = -x2 - y2 + z2 + w2;
379+ }
342380 return rot;
343381 }
344382
@@ -390,6 +428,26 @@ struct Rotation {
390428 result.quat [2 ] = rw*qi[2 ] + rz*qi[3 ] + rx*qi[1 ] - ry*qi[0 ];
391429 result.quat [3 ] = rw*qi[3 ] - rx*qi[0 ] - ry*qi[1 ] - rz*qi[2 ];
392430 }
431+
432+ // Compute and store the quaternion-derived matrix so as_euler()
433+ // and as_matrix() use the same values as scipy's quat→matrix path.
434+ // This ensures 0-ULP match: scipy always reads the matrix from quaternion
435+ // after composing, never stores a raw matrix.
436+ result.has_matrix = true ;
437+ {
438+ T x = result.quat [0 ], y = result.quat [1 ], z = result.quat [2 ], w = result.quat [3 ];
439+ T x2 = x*x, y2 = y*y, z2 = z*z, w2 = w*w;
440+ T xy = x*y, zw = z*w, xz = x*z, yw = y*w, yz = y*z, xw = x*w;
441+ result.matrix [0 ] = x2 - y2 - z2 + w2;
442+ result.matrix [1 ] = T (2 ) * (xy - zw);
443+ result.matrix [2 ] = T (2 ) * (xz + yw);
444+ result.matrix [3 ] = T (2 ) * (xy + zw);
445+ result.matrix [4 ] = -x2 + y2 - z2 + w2;
446+ result.matrix [5 ] = T (2 ) * (yz - xw);
447+ result.matrix [6 ] = T (2 ) * (xz - yw);
448+ result.matrix [7 ] = T (2 ) * (yz + xw);
449+ result.matrix [8 ] = -x2 - y2 + z2 + w2;
450+ }
393451 return result;
394452 }
395453};
0 commit comments