@@ -97,7 +97,7 @@ public T Z
9797 /// <summary>
9898 /// Gets or sets the rotation around X axis in degrees.
9999 /// </summary>
100- public T Rx
100+ public Degree < T > Rx
101101 {
102102 readonly get => _rotation . Rx ;
103103 set => _rotation . Rx = value ;
@@ -106,7 +106,7 @@ public T Rx
106106 /// <summary>
107107 /// Gets or sets the rotation around Y axis in degrees.
108108 /// </summary>
109- public T Ry
109+ public Degree < T > Ry
110110 {
111111 readonly get => _rotation . Ry ;
112112 set => _rotation . Ry = value ;
@@ -115,7 +115,7 @@ public T Ry
115115 /// <summary>
116116 /// Gets or sets the rotation around Z axis in degrees.
117117 /// </summary>
118- public T Rz
118+ public Degree < T > Rz
119119 {
120120 readonly get => _rotation . Rz ;
121121 set => _rotation . Rz = value ;
@@ -217,12 +217,12 @@ public Pose3<U> Truncating<U>()
217217
218218 /// <summary>
219219 /// Creates a pose from three points defining a surface plane using the right-hand rule.
220- /// Z direction is determined by (b-a) × (c-a). Reversing point order reverses Z direction.
220+ /// Z direction is determined by (b-a) x (c-a). Reversing point order reverses Z direction.
221221 /// </summary>
222222 /// <param name="a">First point - becomes the origin, also defines X-axis direction with b.</param>
223223 /// <param name="b">Second point - defines X-axis direction from a.</param>
224- /// <param name="c">Third point - completes the plane; a→b→ c order determines Z via right-hand rule.</param>
225- /// <returns>A pose where origin is at a, X-axis along a→ b, Z-axis per right-hand rule.</returns>
224+ /// <param name="c">Third point - completes the plane; a-b- c order determines Z via right-hand rule.</param>
225+ /// <returns>A pose where origin is at a, X-axis along a- b, Z-axis per right-hand rule.</returns>
226226 /// <exception cref="ArgumentException">Thrown when points a, b, c are collinear.</exception>
227227 public static Pose3 < T > FromSurface ( Point3 < T > a , Point3 < T > b , Point3 < T > c )
228228 {
@@ -232,7 +232,7 @@ public static Pose3<T> FromSurface(Point3<T> a, Point3<T> b, Point3<T> c)
232232 var ab = b - a ;
233233 var ac = c - a ;
234234
235- // Plane normal via right-hand rule: Z = (b-a) × (c-a)
235+ // Plane normal via right-hand rule: Z = (b-a) x (c-a)
236236 var zAxis = Vector3 < T > . Cross ( ab , ac ) ;
237237 var normalLength = zAxis . Length ;
238238
@@ -241,10 +241,10 @@ public static Pose3<T> FromSurface(Point3<T> a, Point3<T> b, Point3<T> c)
241241
242242 zAxis = zAxis / normalLength ; // Normalize
243243
244- // X-axis along edge a→ b, normalized
244+ // X-axis along edge a- b, normalized
245245 var xAxis = ab . Normalize ( ) ;
246246
247- // Y-axis completes right-hand system: Y = Z × X
247+ // Y-axis completes right-hand system: Y = Z x X
248248 var yAxis = Vector3 < T > . Cross ( zAxis , xAxis ) ;
249249
250250 // Convert orthonormal basis to Euler angles (ZYX convention)
@@ -296,10 +296,10 @@ public static Pose3<T> FromSurface(Point3<T> a, Point3<T> b, Point3<T> c, Point3
296296 // Z-axis points toward h
297297 var zAxis = d > T . Zero ? normal : - normal ;
298298
299- // X-axis along edge a→ b, normalized
299+ // X-axis along edge a- b, normalized
300300 var xAxis = ab . Normalize ( ) ;
301301
302- // Y-axis completes right-hand system: Y = Z × X
302+ // Y-axis completes right-hand system: Y = Z x X
303303 var yAxis = Vector3 < T > . Cross ( zAxis , xAxis ) ;
304304
305305 // Convert orthonormal basis to Euler angles (ZYX convention)
@@ -314,17 +314,7 @@ public static Pose3<T> FromSurface(Point3<T> a, Point3<T> b, Point3<T> c, Point3
314314 /// </summary>
315315 private static Rotation3 < T > RotationFromAxes ( Vector3 < T > xAxis , Vector3 < T > yAxis , Vector3 < T > zAxis )
316316 {
317- // We want Rotation3 R such that R.Rotate(e_i) = axis_i
318- // This requires extracting Euler angles from the TRANSPOSE of matrix [X|Y|Z]
319- // R^T[i,j] = R[j,i], so:
320- // R^T[0,2] = xAxis.Z
321- // R^T[1,2] = yAxis.Z
322- // R^T[2,2] = zAxis.Z
323- // R^T[0,1] = xAxis.Y
324- // R^T[0,0] = xAxis.X
325-
326317 var one = T . One ;
327- var rad2Deg = T . CreateTruncating ( 180 ) / T . Pi ;
328318 var epsilon = T . CreateTruncating ( 1e-6 ) ;
329319
330320 // Clamp to [-1, 1] to avoid NaN from asin
@@ -345,12 +335,16 @@ private static Rotation3<T> RotationFromAxes(Vector3<T> xAxis, Vector3<T> yAxis,
345335 }
346336 else
347337 {
348- // Gimbal lock - pitch is ±90°
338+ // Gimbal lock - pitch is +/-90 deg
349339 rx = T . Atan2 ( - zAxis . Y , yAxis . Y ) ;
350340 rz = T . Zero ;
351341 }
352342
353- return new Rotation3 < T > ( rx * rad2Deg , ry * rad2Deg , rz * rad2Deg ) ;
343+ // rx, ry, rz are in radians — convert via Radian<T> → Degree<T>
344+ return new Rotation3 < T > (
345+ ( Degree < T > ) Radian < T > . FromRadian ( rx ) ,
346+ ( Degree < T > ) Radian < T > . FromRadian ( ry ) ,
347+ ( Degree < T > ) Radian < T > . FromRadian ( rz ) ) ;
354348 }
355349
356350 #endregion
@@ -361,7 +355,7 @@ public static implicit operator Pose3<T>((T x, T y, T z, T rx, T ry, T rz) tuple
361355 new ( tuple . x , tuple . y , tuple . z , tuple . rx , tuple . ry , tuple . rz ) ;
362356
363357 public static implicit operator ( T x , T y , T z , T rx , T ry , T rz ) ( Pose3 < T > pose ) =>
364- ( pose . X , pose . Y , pose . Z , pose . Rx , pose . Ry , pose . Rz ) ;
358+ ( pose . X , pose . Y , pose . Z , ( T ) pose . Rx , ( T ) pose . Ry , ( T ) pose . Rz ) ;
365359
366360 /// <summary>
367361 /// Deconstructs into position and rotation.
@@ -380,9 +374,9 @@ public readonly void Deconstruct(out T x, out T y, out T z, out T rx, out T ry,
380374 x = X ;
381375 y = Y ;
382376 z = Z ;
383- rx = Rx ;
384- ry = Ry ;
385- rz = Rz ;
377+ rx = ( T ) Rx ;
378+ ry = ( T ) Ry ;
379+ rz = ( T ) Rz ;
386380 }
387381
388382 #endregion
@@ -394,7 +388,7 @@ public readonly void Deconstruct(out T x, out T y, out T z, out T rx, out T ry,
394388 public override readonly int GetHashCode ( ) => HashCode . Combine ( _position , _rotation ) ;
395389
396390 public override readonly string ToString ( ) =>
397- $ "{{X={ X } , Y={ Y } , Z={ Z } , Rx={ Rx } , Ry={ Ry } , Rz={ Rz } }}";
391+ $ "{{X={ X } , Y={ Y } , Z={ Z } , Rx={ ( T ) Rx } , Ry={ ( T ) Ry } , Rz={ ( T ) Rz } }}";
398392
399393 #endregion
400394
0 commit comments