@@ -52,35 +52,71 @@ public float Length()
5252 return new BabylonVector3 { X = a . X * b , Y = a . Y * b , Z = a . Z * b } ;
5353 }
5454
55- public BabylonQuaternion toQuaternion ( )
56- {
57- return RotationYawPitchRollToRefBabylon ( Y , X , Z ) ;
55+ public enum EulerRotationOrder
56+ {
57+ XYZ ,
58+ YZX ,
59+ ZXY ,
60+ XZY ,
61+ YXZ ,
62+ ZYX
5863 }
5964
60- /**
61- * (Copy pasted from babylon)
62- * Sets the passed quaternion "result" from the passed float Euler angles (radians) (y, x, z).
63- */
64- private BabylonQuaternion RotationYawPitchRollToRefBabylon ( float yaw , float pitch , float roll )
65+ // https://www.mathworks.com/matlabcentral/fileexchange/20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors?s_tid=mwa_osa_a
66+ // https://github.com/mrdoob/three.js/blob/09cfc67a3f52aeb4dd0009921d82396fd5dc5172/src/math/Quaternion.js#L199-L272
67+
68+ public BabylonQuaternion toQuaternion ( EulerRotationOrder rotationOrder = EulerRotationOrder . XYZ )
6569 {
66- // Produces a quaternion from Euler angles in the z-y-x orientation (Tait-Bryan angles)
67- var halfRoll = roll * 0.5 ;
68- var halfPitch = pitch * 0.5 ;
69- var halfYaw = yaw * 0.5 ;
70-
71- var sinRoll = Math . Sin ( halfRoll ) ;
72- var cosRoll = Math . Cos ( halfRoll ) ;
73- var sinPitch = Math . Sin ( halfPitch ) ;
74- var cosPitch = Math . Cos ( halfPitch ) ;
75- var sinYaw = Math . Sin ( halfYaw ) ;
76- var cosYaw = Math . Cos ( halfYaw ) ;
77-
78- var result = new BabylonQuaternion ( ) ;
79- result . X = ( float ) ( ( cosYaw * sinPitch * cosRoll ) + ( sinYaw * cosPitch * sinRoll ) ) ;
80- result . Y = ( float ) ( ( sinYaw * cosPitch * cosRoll ) - ( cosYaw * sinPitch * sinRoll ) ) ;
81- result . Z = ( float ) ( ( cosYaw * cosPitch * sinRoll ) - ( sinYaw * sinPitch * cosRoll ) ) ;
82- result . W = ( float ) ( ( cosYaw * cosPitch * cosRoll ) + ( sinYaw * sinPitch * sinRoll ) ) ;
83- return result ;
70+ BabylonQuaternion quaternion = new BabylonQuaternion ( ) ;
71+
72+ var c1 = Math . Cos ( 0.5 * this . X ) ;
73+ var c2 = Math . Cos ( 0.5 * this . Y ) ;
74+ var c3 = Math . Cos ( 0.5 * this . Z ) ;
75+
76+ var s1 = Math . Sin ( 0.5 * this . X ) ;
77+ var s2 = Math . Sin ( 0.5 * this . Y ) ;
78+ var s3 = Math . Sin ( 0.5 * this . Z ) ;
79+
80+ switch ( rotationOrder )
81+ {
82+ case EulerRotationOrder . XYZ :
83+ quaternion . X = ( float ) ( s1 * c2 * c3 + c1 * s2 * s3 ) ;
84+ quaternion . Y = ( float ) ( c1 * s2 * c3 - s1 * c2 * s3 ) ;
85+ quaternion . Z = ( float ) ( c1 * c2 * s3 + s1 * s2 * c3 ) ;
86+ quaternion . W = ( float ) ( c1 * c2 * c3 - s1 * s2 * s3 ) ;
87+ break ;
88+ case EulerRotationOrder . YZX :
89+ quaternion . X = ( float ) ( s1 * c2 * c3 + c1 * s2 * s3 ) ;
90+ quaternion . Y = ( float ) ( c1 * s2 * c3 + s1 * c2 * s3 ) ;
91+ quaternion . Z = ( float ) ( c1 * c2 * s3 - s1 * s2 * c3 ) ;
92+ quaternion . W = ( float ) ( c1 * c2 * c3 - s1 * s2 * s3 ) ;
93+ break ;
94+ case EulerRotationOrder . ZXY :
95+ quaternion . X = ( float ) ( s1 * c2 * c3 - c1 * s2 * s3 ) ;
96+ quaternion . Y = ( float ) ( c1 * s2 * c3 + s1 * c2 * s3 ) ;
97+ quaternion . Z = ( float ) ( c1 * c2 * s3 + s1 * s2 * c3 ) ;
98+ quaternion . W = ( float ) ( c1 * c2 * c3 - s1 * s2 * s3 ) ;
99+ break ;
100+ case EulerRotationOrder . XZY :
101+ quaternion . X = ( float ) ( s1 * c2 * c3 - c1 * s2 * s3 ) ;
102+ quaternion . Y = ( float ) ( c1 * s2 * c3 - s1 * c2 * s3 ) ;
103+ quaternion . Z = ( float ) ( c1 * c2 * s3 + s1 * s2 * c3 ) ;
104+ quaternion . W = ( float ) ( c1 * c2 * c3 + s1 * s2 * s3 ) ;
105+ break ;
106+ case EulerRotationOrder . YXZ :
107+ quaternion . X = ( float ) ( s1 * c2 * c3 + c1 * s2 * s3 ) ;
108+ quaternion . Y = ( float ) ( c1 * s2 * c3 - s1 * c2 * s3 ) ;
109+ quaternion . Z = ( float ) ( c1 * c2 * s3 - s1 * s2 * c3 ) ;
110+ quaternion . W = ( float ) ( c1 * c2 * c3 + s1 * s2 * s3 ) ;
111+ break ;
112+ case EulerRotationOrder . ZYX :
113+ quaternion . X = ( float ) ( s1 * c2 * c3 - c1 * s2 * s3 ) ;
114+ quaternion . Y = ( float ) ( c1 * s2 * c3 + s1 * c2 * s3 ) ;
115+ quaternion . Z = ( float ) ( c1 * c2 * s3 - s1 * s2 * c3 ) ;
116+ quaternion . W = ( float ) ( c1 * c2 * c3 + s1 * s2 * s3 ) ;
117+ break ;
118+ }
119+ return quaternion ;
84120 }
85121
86122 /**
0 commit comments