org.simantics.g3d/src/org/simantics/g3d/math/EulerTools.java

   1 package org.simantics.g3d.math;\r
   2 \r
   3 import javax.vecmath.AxisAngle4d;\r
   4 import javax.vecmath.Quat4d;\r
   5 import javax.vecmath.Vector3d;\r
   6 \r
   7 public class EulerTools {\r
   8 \r
   9         public enum Order {\r
  10                 XYX, XYZ, XZX, XZY, YXY, YXZ, YZX, YZY, ZXY, ZXZ, ZYX, ZYZ\r
  11         };\r
  12 \r
  13         public static Quat4d getQuatFromEuler(Order order, Vector3d a) {\r
  14                 return getQuatFromEuler(order, a.x, a.y, a.z);\r
  15         }\r
  16         \r
  17         public static Quat4d getQuatFromEuler(Order order, double a1, double a2, double a3) {\r
  18                 Quat4d q1 = new Quat4d();\r
  19                 Quat4d q2 = new Quat4d();\r
  20                 Quat4d q3 = new Quat4d();\r
  21                 switch (order) {\r
  22                 case XYX:\r
  23                         q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
  24                         q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
  25                         q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
  26                         break;\r
  27                 case XYZ:\r
  28                         q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
  29                         q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
  30                         q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
  31                         break;\r
  32                 case XZX:\r
  33                         q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
  34                         q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
  35                         q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
  36                         break;\r
  37                 case XZY:\r
  38                         q1.set(new AxisAngle4d(1.0, 0.0, 0.0, a1));\r
  39                         q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
  40                         q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
  41                         break;\r
  42                 case YXY:\r
  43                         q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
  44                         q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
  45                         q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
  46                         break;\r
  47                 case YXZ:\r
  48                         q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
  49                         q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
  50                         q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
  51                         break;\r
  52                 case YZX:\r
  53                         q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
  54                         q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
  55                         q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
  56                         break;\r
  57                 case YZY:\r
  58                         q1.set(new AxisAngle4d(0.0, 1.0, 0.0, a1));\r
  59                         q2.set(new AxisAngle4d(0.0, 0.0, 1.0, a2));\r
  60                         q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
  61                         break;\r
  62                 case ZXY:\r
  63                         q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
  64                         q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
  65                         q3.set(new AxisAngle4d(0.0, 1.0, 0.0, a3));\r
  66                         break;\r
  67                 case ZXZ:\r
  68                         q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
  69                         q2.set(new AxisAngle4d(1.0, 0.0, 0.0, a2));\r
  70                         q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
  71                         break;\r
  72                 case ZYX:\r
  73                         q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
  74                         q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
  75                         q3.set(new AxisAngle4d(1.0, 0.0, 0.0, a3));\r
  76                         break;\r
  77                 case ZYZ:\r
  78                         q1.set(new AxisAngle4d(0.0, 0.0, 1.0, a1));\r
  79                         q2.set(new AxisAngle4d(0.0, 1.0, 0.0, a2));\r
  80                         q3.set(new AxisAngle4d(0.0, 0.0, 1.0, a3));\r
  81                         break;\r
  82                 }\r
  83                 q1.mul(q2);\r
  84                 q1.mul(q3);\r
  85                 return q1;\r
  86         }\r
  87         \r
  88         /**\r
  89          * See http://noelhughes.net/uploads/quat_2_euler_paper_ver3.pdf\r
  90          * @param order\r
  91          * @param q\r
  92          * @return\r
  93          */\r
  94         public static Vector3d getEulerFromQuat(Order order, Quat4d q) {\r
  95                 Vector3d euler = new Vector3d();\r
  96                 \r
  97 //              Vector3d v1 = new Vector3d();\r
  98 //              Vector3d v2 = new Vector3d();\r
  99                 Vector3d v3 = new Vector3d();\r
 100                 Vector3d v3n = new Vector3d();\r
 101                 \r
 102                 switch (order) {\r
 103                 case XYX:\r
 104 //                      v1.x = 1.0;\r
 105 //                      v2.y = 1.0;\r
 106                         v3.x = 1.0;\r
 107                         v3n.y = 1.0;\r
 108                         break;\r
 109                 case XYZ:\r
 110 //                      v1.x = 1.0;\r
 111 //                      v2.y = 1.0;\r
 112                         v3.z = 1.0;\r
 113                         v3n.x = 1.0;\r
 114                         break;\r
 115                 case XZX:\r
 116 //                      v1.x = 1.0;\r
 117 //                      v2.z = 1.0;\r
 118                         v3.x = 1.0;\r
 119                         v3n.y = 1.0;\r
 120                         break;\r
 121                 case XZY:\r
 122 //                      v1.x = 1.0;\r
 123 //                      v2.z = 1.0;\r
 124                         v3.y = 1.0;\r
 125                         v3n.z = 1.0;\r
 126                         break;\r
 127                 case YXY:\r
 128 //                      v1.y = 1.0;\r
 129 //                      v2.x = 1.0;\r
 130                         v3.y = 1.0;\r
 131                         v3n.z = 1.0;\r
 132                         break;\r
 133                 case YXZ:\r
 134 //                      v1.y = 1.0;\r
 135 //                      v2.x = 1.0;\r
 136                         v3.z = 1.0;\r
 137                         v3n.x = 1.0;\r
 138                         break;\r
 139                 case YZX:\r
 140 //                      v1.y = 1.0;\r
 141 //                      v2.z = 1.0;\r
 142                         v3.x = 1.0;\r
 143                         v3n.y = 1.0;\r
 144                         break;\r
 145                 case YZY:\r
 146 //                      v1.y = 1.0;\r
 147 //                      v2.z = 1.0;\r
 148                         v3.y = 1.0;\r
 149                         v3n.z = 1.0;\r
 150                         break;\r
 151                 case ZXY:\r
 152 //                      v1.z = 1.0;\r
 153 //                      v2.x = 1.0;\r
 154                         v3.y = 1.0;\r
 155                         v3n.z = 1.0;\r
 156                         break;\r
 157                 case ZXZ:\r
 158 //                      v1.z = 1.0;\r
 159 //                      v2.x = 1.0;\r
 160                         v3.z = 1.0;\r
 161                         v3n.x = 1.0;\r
 162                         break;\r
 163                 case ZYX:\r
 164 //                      v1.z = 1.0;\r
 165 //                      v2.y = 1.0;\r
 166                         v3.x = 1.0;\r
 167                         v3n.y = 1.0;\r
 168                         break;\r
 169                 case ZYZ:\r
 170 //                      v1.z = 1.0;\r
 171 //                      v2.y = 1.0;\r
 172                         v3.z = 1.0;\r
 173                         v3n.x = 1.0;\r
 174                         break;\r
 175                 }\r
 176                 Vector3d v3r = new Vector3d();\r
 177                 MathTools.rotate(q, v3, v3r);\r
 178                 v3r.normalize();\r
 179                 \r
 180                 switch (order) {\r
 181 \r
 182                 case XZX:\r
 183                         euler.x = Math.atan2(v3r.z, v3r.y);\r
 184                         euler.y = Math.acos(v3r.x);\r
 185                         break;\r
 186                 case YXY:\r
 187                         euler.x = Math.atan2(v3r.x, v3r.z);\r
 188                         euler.y = Math.acos(v3r.y);\r
 189                         break;\r
 190                 case ZYZ:\r
 191                         euler.x = Math.atan2(v3r.y, v3r.x);\r
 192                         euler.y = Math.acos(v3r.z);\r
 193                         break;\r
 194                         \r
 195                 case XZY:\r
 196                         euler.x = Math.atan2(v3r.z, v3r.y);\r
 197                         euler.y = -Math.asin(v3r.x);\r
 198                         break;\r
 199                 case YXZ:\r
 200                         euler.x = Math.atan2(v3r.x, v3r.z);\r
 201                         euler.y = -Math.asin(v3r.y);\r
 202                         break;\r
 203                 case ZYX:\r
 204                         euler.x = Math.atan2(v3r.y, v3r.x);\r
 205                         euler.y = -Math.asin(v3r.z);\r
 206                         break;\r
 207                         \r
 208                 case XYX:\r
 209                         euler.x = Math.atan2(v3r.y, -v3r.z);\r
 210                         //euler.x = Math.atan2(v3r.y, -v3r.x);\r
 211                         euler.y = Math.acos(v3r.x);\r
 212                         break;\r
 213                 case YZY:\r
 214                         euler.x = Math.atan2(v3r.z, -v3r.x);\r
 215                         //euler.x = Math.atan2(v3r.z, -v3r.y);\r
 216                         euler.y = Math.acos(v3r.y);\r
 217                         break;\r
 218                 case ZXZ:\r
 219                         euler.x = Math.atan2(v3r.x, -v3r.y);\r
 220                         //euler.x = Math.atan2(v3r.x, -v3r.z);\r
 221                         euler.y = Math.acos(v3r.z);\r
 222                         break;\r
 223                         \r
 224                 case XYZ:\r
 225                         euler.x = Math.atan2(-v3r.y, v3r.z);\r
 226                         euler.y = Math.asin(v3r.x);\r
 227                         break;\r
 228                 case YZX:\r
 229                         euler.x = Math.atan2(-v3r.z, v3r.x);\r
 230                         euler.y = Math.asin(v3r.y);\r
 231                         break;\r
 232                 case ZXY:\r
 233                         euler.x = Math.atan2(-v3r.x, v3r.y);\r
 234                         euler.y = Math.asin(v3r.z);\r
 235                         break;\r
 236                 }\r
 237                 \r
 238                 Quat4d q1 = new Quat4d();\r
 239                 q1.w = Math.cos(euler.x*0.5);\r
 240                 Quat4d q2 = new Quat4d();\r
 241                 q2.w = Math.cos(euler.y*0.5);\r
 242                 \r
 243                 switch (order) {\r
 244                 case XYX:\r
 245                 case XYZ:\r
 246                 case XZX:\r
 247                 case XZY:\r
 248                         q1.x = Math.sin(euler.x*0.5);\r
 249                         break;\r
 250                 case YXY:\r
 251                 case YXZ:\r
 252                 case YZX:\r
 253                 case YZY:\r
 254                         q1.y = Math.sin(euler.x*0.5);\r
 255                         break;\r
 256                 case ZXY:\r
 257                 case ZXZ:\r
 258                 case ZYX:\r
 259                 case ZYZ:\r
 260                         q1.z = Math.sin(euler.x*0.5);\r
 261                         break;\r
 262                 }\r
 263                 \r
 264                 switch (order) {\r
 265                 case YXY:\r
 266                 case YXZ:\r
 267                 case ZXY:\r
 268                 case ZXZ:\r
 269                         q2.x = Math.sin(euler.y*0.5);\r
 270                         break;\r
 271                 case XYX:\r
 272                 case XYZ:\r
 273                 case ZYX:\r
 274                 case ZYZ:\r
 275                         q2.y = Math.sin(euler.y*0.5);\r
 276                         break;\r
 277                 case XZX:\r
 278                 case XZY:\r
 279                 case YZX:\r
 280                 case YZY:\r
 281                         q2.z = Math.sin(euler.y*0.5);\r
 282                         break;\r
 283                 }\r
 284                 \r
 285                 Quat4d q12 = new Quat4d();\r
 286                 q12.mul(q1, q2);\r
 287                 \r
 288                 Vector3d v3n12 = new Vector3d();\r
 289                 Vector3d v3ng = new Vector3d();\r
 290                 MathTools.rotate(q12, v3n, v3n12);\r
 291                 MathTools.rotate(q, v3n, v3ng);\r
 292                 \r
 293                 double dot = v3n12.dot(v3ng);\r
 294                 dot = MathTools.clamp(-1.0, 1.0, dot);\r
 295                 euler.z = Math.abs(Math.acos(dot));\r
 296                 Vector3d vc = new Vector3d();\r
 297                 vc.cross(v3n12, v3ng);\r
 298                 euler.z *= Math.signum(vc.dot(v3r));\r
 299                 \r
 300                 return euler;\r
 301         }\r
 302 \r
 303         \r
 304         public static void main(String args[]) {\r
 305                 \r
 306                 boolean all = false;\r
 307                 boolean allOrder = false;\r
 308                 if (all) {\r
 309                         testAll();\r
 310                 } else if (allOrder) {\r
 311                         test(Order.YXZ);\r
 312                 } else {\r
 313                         //test(Order.ZXY,30,60,45);\r
 314                         //test(Order.YXZ,30,0,0);\r
 315                         //test(Order.YXZ,30,90,60);\r
 316                         test(Order.YXZ,300,240,360);\r
 317                 }\r
 318         }\r
 319         \r
 320         private static void testAll() {\r
 321                 double start = 0.0;\r
 322                 double end = 90.0;\r
 323                 double step = 30.0;\r
 324                 for (double a1 = start;  a1 <= end; a1+= step) {\r
 325                         double r1 = MathTools.degToRad(a1);\r
 326                         for (double a2 = start;  a2 <= end; a2+= step) {\r
 327                                 double r2 = MathTools.degToRad(a2);\r
 328                                 for (double a3 = start;  a3 <= end; a3+= step) {\r
 329                                         double r3 = MathTools.degToRad(a3);\r
 330                                         for (Order order : Order.values()) {\r
 331                                                 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);\r
 332                                                 Vector3d a = EulerTools.getEulerFromQuat(order, q);\r
 333                                                 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);\r
 334                                                 a.x = MathTools.radToDeg(a.x);\r
 335                                                 a.y = MathTools.radToDeg(a.y);\r
 336                                                 a.z = MathTools.radToDeg(a.z);\r
 337                                                 System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));\r
 338                                         }\r
 339                                 }       \r
 340                         }       \r
 341                 }\r
 342         }\r
 343         \r
 344         private static void test(Order order) {\r
 345                 double start = 0.0;\r
 346                 double end = 360.0;\r
 347                 double step = 30.0;\r
 348                 for (double a1 = start;  a1 <= end; a1+= step) {\r
 349                         double r1 = MathTools.degToRad(a1);\r
 350                         for (double a2 = start;  a2 <= end; a2+= step) {\r
 351                                 double r2 = MathTools.degToRad(a2);\r
 352                                 for (double a3 = start;  a3 <= end; a3+= step) {\r
 353                                         double r3 = MathTools.degToRad(a3);\r
 354                                         \r
 355                                         Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);\r
 356                                         Vector3d a = EulerTools.getEulerFromQuat(order, q);\r
 357                                         Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);\r
 358                                         a.x = MathTools.radToDeg(a.x);\r
 359                                         a.y = MathTools.radToDeg(a.y);\r
 360                                         a.z = MathTools.radToDeg(a.z);\r
 361                                         \r
 362                                         System.out.println(toString(a1) +" " + toString(a2) + " " + toString(a3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));\r
 363                                 }       \r
 364                         }       \r
 365                 }\r
 366         }\r
 367         \r
 368         private static String toString(double d) {\r
 369                 return String.format("%1$6.2f", d);\r
 370         }\r
 371         \r
 372         private static String toString(Vector3d v) {\r
 373                 return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) +")";\r
 374         }\r
 375         \r
 376         private static String toString(Quat4d v) {\r
 377                 return "("+toString(v.x) +", "+ toString(v.y) + ", " + toString(v.z) + ", " + toString(v.w) +")";\r
 378         }\r
 379         \r
 380         private static void test(Order order, double deg1, double deg2, double deg3) {\r
 381                 double r1 = MathTools.degToRad(deg1);\r
 382                 double r2 = MathTools.degToRad(deg2);\r
 383                 double r3 = MathTools.degToRad(deg3);\r
 384                 \r
 385                 Quat4d q = EulerTools.getQuatFromEuler(order, r1, r2, r3);\r
 386                 Vector3d a = EulerTools.getEulerFromQuat(order, q);\r
 387                 Quat4d q2 = EulerTools.getQuatFromEuler(order, a.x,a.y,a.z);\r
 388                 a.x = MathTools.radToDeg(a.x);\r
 389                 a.y = MathTools.radToDeg(a.y);\r
 390                 a.z = MathTools.radToDeg(a.z);\r
 391                 System.out.println(toString(deg1) +" " + toString(deg2) + " " + toString(deg3) + " " + order + "\t" + toString(a) + "\t" + toString(q) + "\t" + toString(q2));\r
 392 \r
 393         }\r
 394 }\r