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

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