site/j2s/JU/Quat.js

   1 Clazz.declarePackage ("JU");\r
   2 Clazz.load (["JU.P4"], "JU.Quat", ["java.lang.Float", "JU.A4", "$.M3", "$.V3"], function () {\r
   3 c$ = Clazz.decorateAsClass (function () {\r
   4 this.q0 = 0;\r
   5 this.q1 = 0;\r
   6 this.q2 = 0;\r
   7 this.q3 = 0;\r
   8 this.mat = null;\r
   9 Clazz.instantialize (this, arguments);\r
  10 }, JU, "Quat");\r
  11 Clazz.makeConstructor (c$, \r
  12 function () {\r
  13 this.q0 = 1;\r
  14 });\r
  15 c$.newQ = Clazz.defineMethod (c$, "newQ", \r
  16 function (q) {\r
  17 var q1 =  new JU.Quat ();\r
  18 q1.set (q);\r
  19 return q1;\r
  20 }, "JU.Quat");\r
  21 c$.newVA = Clazz.defineMethod (c$, "newVA", \r
  22 function (v, theta) {\r
  23 var q =  new JU.Quat ();\r
  24 q.setTA (v, theta);\r
  25 return q;\r
  26 }, "JU.T3,~N");\r
  27 c$.newM = Clazz.defineMethod (c$, "newM", \r
  28 function (mat) {\r
  29 var q =  new JU.Quat ();\r
  30 q.setM (JU.M3.newM3 (mat));\r
  31 return q;\r
  32 }, "JU.M3");\r
  33 c$.newAA = Clazz.defineMethod (c$, "newAA", \r
  34 function (a) {\r
  35 var q =  new JU.Quat ();\r
  36 q.setAA (a);\r
  37 return q;\r
  38 }, "JU.A4");\r
  39 c$.newP4 = Clazz.defineMethod (c$, "newP4", \r
  40 function (pt) {\r
  41 var q =  new JU.Quat ();\r
  42 q.setP4 (pt);\r
  43 return q;\r
  44 }, "JU.P4");\r
  45 c$.new4 = Clazz.defineMethod (c$, "new4", \r
  46 function (q1, q2, q3, q0) {\r
  47 var q =  new JU.Quat ();\r
  48 if (q0 < -1) {\r
  49 q.q0 = -1;\r
  50 return q;\r
  51 }if (q0 > 1) {\r
  52 q.q0 = 1;\r
  53 return q;\r
  54 }q.q0 = q0;\r
  55 q.q1 = q1;\r
  56 q.q2 = q2;\r
  57 q.q3 = q3;\r
  58 return q;\r
  59 }, "~N,~N,~N,~N");\r
  60 Clazz.defineMethod (c$, "set", \r
  61 function (q) {\r
  62 this.q0 = q.q0;\r
  63 this.q1 = q.q1;\r
  64 this.q2 = q.q2;\r
  65 this.q3 = q.q3;\r
  66 }, "JU.Quat");\r
  67 Clazz.defineMethod (c$, "setP4", \r
  68  function (pt) {\r
  69 var factor = (pt == null ? 0 : pt.distance4 (JU.Quat.qZero));\r
  70 if (factor == 0) {\r
  71 this.q0 = 1;\r
  72 return;\r
  73 }this.q0 = pt.w / factor;\r
  74 this.q1 = pt.x / factor;\r
  75 this.q2 = pt.y / factor;\r
  76 this.q3 = pt.z / factor;\r
  77 }, "JU.P4");\r
  78 Clazz.defineMethod (c$, "setTA", \r
  79 function (pt, theta) {\r
  80 if (pt.x == 0 && pt.y == 0 && pt.z == 0) {\r
  81 this.q0 = 1;\r
  82 return;\r
  83 }var fact = (Math.sin (theta / 2 * 0.017453292519943295) / Math.sqrt (pt.x * pt.x + pt.y * pt.y + pt.z * pt.z));\r
  84 this.q0 = (Math.cos (theta / 2 * 0.017453292519943295));\r
  85 this.q1 = (pt.x * fact);\r
  86 this.q2 = (pt.y * fact);\r
  87 this.q3 = (pt.z * fact);\r
  88 }, "JU.T3,~N");\r
  89 Clazz.defineMethod (c$, "setAA", \r
  90 function (a) {\r
  91 var aa = JU.A4.newAA (a);\r
  92 if (aa.angle == 0) aa.y = 1;\r
  93 this.setM ( new JU.M3 ().setAA (aa));\r
  94 }, "JU.A4");\r
  95 Clazz.defineMethod (c$, "setM", \r
  96  function (mat) {\r
  97 this.mat = mat;\r
  98 var trace = mat.m00 + mat.m11 + mat.m22;\r
  99 var temp;\r
 100 var w;\r
 101 var x;\r
 102 var y;\r
 103 var z;\r
 104 if (trace >= 0.5) {\r
 105 w = Math.sqrt (1.0 + trace);\r
 106 x = (mat.m21 - mat.m12) / w;\r
 107 y = (mat.m02 - mat.m20) / w;\r
 108 z = (mat.m10 - mat.m01) / w;\r
 109 } else if ((temp = mat.m00 + mat.m00 - trace) >= 0.5) {\r
 110 x = Math.sqrt (1.0 + temp);\r
 111 w = (mat.m21 - mat.m12) / x;\r
 112 y = (mat.m10 + mat.m01) / x;\r
 113 z = (mat.m20 + mat.m02) / x;\r
 114 } else if ((temp = mat.m11 + mat.m11 - trace) >= 0.5 || mat.m11 > mat.m22) {\r
 115 y = Math.sqrt (1.0 + temp);\r
 116 w = (mat.m02 - mat.m20) / y;\r
 117 x = (mat.m10 + mat.m01) / y;\r
 118 z = (mat.m21 + mat.m12) / y;\r
 119 } else {\r
 120 z = Math.sqrt (1.0 + mat.m22 + mat.m22 - trace);\r
 121 w = (mat.m10 - mat.m01) / z;\r
 122 x = (mat.m20 + mat.m02) / z;\r
 123 y = (mat.m21 + mat.m12) / z;\r
 124 }this.q0 = (w * 0.5);\r
 125 this.q1 = (x * 0.5);\r
 126 this.q2 = (y * 0.5);\r
 127 this.q3 = (z * 0.5);\r
 128 }, "JU.M3");\r
 129 Clazz.defineMethod (c$, "setRef", \r
 130 function (qref) {\r
 131 if (qref == null) {\r
 132 this.mul (this.getFixFactor ());\r
 133 return;\r
 134 }if (this.dot (qref) >= 0) return;\r
 135 this.q0 *= -1;\r
 136 this.q1 *= -1;\r
 137 this.q2 *= -1;\r
 138 this.q3 *= -1;\r
 139 }, "JU.Quat");\r
 140 c$.getQuaternionFrame = Clazz.defineMethod (c$, "getQuaternionFrame", \r
 141 function (center, x, xy) {\r
 142 var vA = JU.V3.newV (x);\r
 143 var vB = JU.V3.newV (xy);\r
 144 if (center != null) {\r
 145 vA.sub (center);\r
 146 vB.sub (center);\r
 147 }return JU.Quat.getQuaternionFrameV (vA, vB, null, false);\r
 148 }, "JU.P3,JU.T3,JU.T3");\r
 149 c$.getQuaternionFrameV = Clazz.defineMethod (c$, "getQuaternionFrameV", \r
 150 function (vA, vB, vC, yBased) {\r
 151 if (vC == null) {\r
 152 vC =  new JU.V3 ();\r
 153 vC.cross (vA, vB);\r
 154 if (yBased) vA.cross (vB, vC);\r
 155 }var vBprime =  new JU.V3 ();\r
 156 vBprime.cross (vC, vA);\r
 157 vA.normalize ();\r
 158 vBprime.normalize ();\r
 159 vC.normalize ();\r
 160 var mat =  new JU.M3 ();\r
 161 mat.setColumnV (0, vA);\r
 162 mat.setColumnV (1, vBprime);\r
 163 mat.setColumnV (2, vC);\r
 164 var q = JU.Quat.newM (mat);\r
 165 return q;\r
 166 }, "JU.V3,JU.V3,JU.V3,~B");\r
 167 Clazz.defineMethod (c$, "getMatrix", \r
 168 function () {\r
 169 if (this.mat == null) this.setMatrix ();\r
 170 return this.mat;\r
 171 });\r
 172 Clazz.defineMethod (c$, "setMatrix", \r
 173  function () {\r
 174 this.mat =  new JU.M3 ();\r
 175 this.mat.m00 = this.q0 * this.q0 + this.q1 * this.q1 - this.q2 * this.q2 - this.q3 * this.q3;\r
 176 this.mat.m01 = 2 * this.q1 * this.q2 - 2 * this.q0 * this.q3;\r
 177 this.mat.m02 = 2 * this.q1 * this.q3 + 2 * this.q0 * this.q2;\r
 178 this.mat.m10 = 2 * this.q1 * this.q2 + 2 * this.q0 * this.q3;\r
 179 this.mat.m11 = this.q0 * this.q0 - this.q1 * this.q1 + this.q2 * this.q2 - this.q3 * this.q3;\r
 180 this.mat.m12 = 2 * this.q2 * this.q3 - 2 * this.q0 * this.q1;\r
 181 this.mat.m20 = 2 * this.q1 * this.q3 - 2 * this.q0 * this.q2;\r
 182 this.mat.m21 = 2 * this.q2 * this.q3 + 2 * this.q0 * this.q1;\r
 183 this.mat.m22 = this.q0 * this.q0 - this.q1 * this.q1 - this.q2 * this.q2 + this.q3 * this.q3;\r
 184 });\r
 185 Clazz.defineMethod (c$, "add", \r
 186 function (x) {\r
 187 return JU.Quat.newVA (this.getNormal (), this.getTheta () + x);\r
 188 }, "~N");\r
 189 Clazz.defineMethod (c$, "mul", \r
 190 function (x) {\r
 191 return (x == 1 ? JU.Quat.new4 (this.q1, this.q2, this.q3, this.q0) : JU.Quat.newVA (this.getNormal (), this.getTheta () * x));\r
 192 }, "~N");\r
 193 Clazz.defineMethod (c$, "mulQ", \r
 194 function (p) {\r
 195 return JU.Quat.new4 (this.q0 * p.q1 + this.q1 * p.q0 + this.q2 * p.q3 - this.q3 * p.q2, this.q0 * p.q2 + this.q2 * p.q0 + this.q3 * p.q1 - this.q1 * p.q3, this.q0 * p.q3 + this.q3 * p.q0 + this.q1 * p.q2 - this.q2 * p.q1, this.q0 * p.q0 - this.q1 * p.q1 - this.q2 * p.q2 - this.q3 * p.q3);\r
 196 }, "JU.Quat");\r
 197 Clazz.defineMethod (c$, "div", \r
 198 function (p) {\r
 199 return this.mulQ (p.inv ());\r
 200 }, "JU.Quat");\r
 201 Clazz.defineMethod (c$, "divLeft", \r
 202 function (p) {\r
 203 return this.inv ().mulQ (p);\r
 204 }, "JU.Quat");\r
 205 Clazz.defineMethod (c$, "dot", \r
 206 function (q) {\r
 207 return this.q0 * q.q0 + this.q1 * q.q1 + this.q2 * q.q2 + this.q3 * q.q3;\r
 208 }, "JU.Quat");\r
 209 Clazz.defineMethod (c$, "inv", \r
 210 function () {\r
 211 return JU.Quat.new4 (-this.q1, -this.q2, -this.q3, this.q0);\r
 212 });\r
 213 Clazz.defineMethod (c$, "negate", \r
 214 function () {\r
 215 return JU.Quat.new4 (-this.q1, -this.q2, -this.q3, -this.q0);\r
 216 });\r
 217 Clazz.defineMethod (c$, "getFixFactor", \r
 218  function () {\r
 219 return (this.q0 < 0 || this.q0 == 0 && (this.q1 < 0 || this.q1 == 0 && (this.q2 < 0 || this.q2 == 0 && this.q3 < 0)) ? -1 : 1);\r
 220 });\r
 221 Clazz.defineMethod (c$, "getVector", \r
 222 function (i) {\r
 223 return this.getVectorScaled (i, 1);\r
 224 }, "~N");\r
 225 Clazz.defineMethod (c$, "getVectorScaled", \r
 226 function (i, scale) {\r
 227 if (i == -1) {\r
 228 scale *= this.getFixFactor ();\r
 229 return JU.V3.new3 (this.q1 * scale, this.q2 * scale, this.q3 * scale);\r
 230 }if (this.mat == null) this.setMatrix ();\r
 231 var v =  new JU.V3 ();\r
 232 this.mat.getColumnV (i, v);\r
 233 if (scale != 1) v.scale (scale);\r
 234 return v;\r
 235 }, "~N,~N");\r
 236 Clazz.defineMethod (c$, "getNormal", \r
 237 function () {\r
 238 var v = JU.Quat.getRawNormal (this);\r
 239 v.scale (this.getFixFactor ());\r
 240 return v;\r
 241 });\r
 242 c$.getRawNormal = Clazz.defineMethod (c$, "getRawNormal", \r
 243  function (q) {\r
 244 var v = JU.V3.new3 (q.q1, q.q2, q.q3);\r
 245 if (v.length () == 0) return JU.V3.new3 (0, 0, 1);\r
 246 v.normalize ();\r
 247 return v;\r
 248 }, "JU.Quat");\r
 249 Clazz.defineMethod (c$, "getTheta", \r
 250 function () {\r
 251 return (Math.acos (Math.abs (this.q0)) * 2 * 180 / 3.141592653589793);\r
 252 });\r
 253 Clazz.defineMethod (c$, "getThetaRadians", \r
 254 function () {\r
 255 return (Math.acos (Math.abs (this.q0)) * 2);\r
 256 });\r
 257 Clazz.defineMethod (c$, "getNormalDirected", \r
 258 function (v0) {\r
 259 var v = this.getNormal ();\r
 260 if (v.x * v0.x + v.y * v0.y + v.z * v0.z < 0) {\r
 261 v.scale (-1);\r
 262 }return v;\r
 263 }, "JU.V3");\r
 264 Clazz.defineMethod (c$, "get3dProjection", \r
 265 function (v3d) {\r
 266 v3d.set (this.q1, this.q2, this.q3);\r
 267 return v3d;\r
 268 }, "JU.V3");\r
 269 Clazz.defineMethod (c$, "getThetaDirected", \r
 270 function (axisAngle) {\r
 271 var theta = this.getTheta ();\r
 272 var v = this.getNormal ();\r
 273 if (axisAngle.x * this.q1 + axisAngle.y * this.q2 + axisAngle.z * this.q3 < 0) {\r
 274 v.scale (-1);\r
 275 theta = -theta;\r
 276 }axisAngle.set4 (v.x, v.y, v.z, theta);\r
 277 return axisAngle;\r
 278 }, "JU.P4");\r
 279 Clazz.defineMethod (c$, "getThetaDirectedV", \r
 280 function (vector) {\r
 281 var theta = this.getTheta ();\r
 282 var v = this.getNormal ();\r
 283 if (vector.x * this.q1 + vector.y * this.q2 + vector.z * this.q3 < 0) {\r
 284 v.scale (-1);\r
 285 theta = -theta;\r
 286 }return theta;\r
 287 }, "JU.V3");\r
 288 Clazz.defineMethod (c$, "toPoint4f", \r
 289 function () {\r
 290 return JU.P4.new4 (this.q1, this.q2, this.q3, this.q0);\r
 291 });\r
 292 Clazz.defineMethod (c$, "toAxisAngle4f", \r
 293 function () {\r
 294 var theta = 2 * Math.acos (Math.abs (this.q0));\r
 295 var sinTheta2 = Math.sin (theta / 2);\r
 296 var v = this.getNormal ();\r
 297 if (sinTheta2 < 0) {\r
 298 v.scale (-1);\r
 299 theta = 3.141592653589793 - theta;\r
 300 }return JU.A4.newVA (v, theta);\r
 301 });\r
 302 Clazz.defineMethod (c$, "transform2", \r
 303 function (pt, ptNew) {\r
 304 if (this.mat == null) this.setMatrix ();\r
 305 this.mat.rotate2 (pt, ptNew);\r
 306 return ptNew;\r
 307 }, "JU.T3,JU.T3");\r
 308 Clazz.defineMethod (c$, "leftDifference", \r
 309 function (q2) {\r
 310 var q2adjusted = (this.dot (q2) < 0 ? q2.negate () : q2);\r
 311 return this.inv ().mulQ (q2adjusted);\r
 312 }, "JU.Quat");\r
 313 Clazz.defineMethod (c$, "rightDifference", \r
 314 function (q2) {\r
 315 var q2adjusted = (this.dot (q2) < 0 ? q2.negate () : q2);\r
 316 return this.mulQ (q2adjusted.inv ());\r
 317 }, "JU.Quat");\r
 318 Clazz.overrideMethod (c$, "toString", \r
 319 function () {\r
 320 return "{" + this.q1 + " " + this.q2 + " " + this.q3 + " " + this.q0 + "}";\r
 321 });\r
 322 c$.div = Clazz.defineMethod (c$, "div", \r
 323 function (data1, data2, nMax, isRelative) {\r
 324 var n;\r
 325 if (data1 == null || data2 == null || (n = Math.min (data1.length, data2.length)) == 0) return null;\r
 326 if (nMax > 0 && n > nMax) n = nMax;\r
 327 var dqs =  new Array (n);\r
 328 for (var i = 0; i < n; i++) {\r
 329 if (data1[i] == null || data2[i] == null) return null;\r
 330 dqs[i] = (isRelative ? data1[i].divLeft (data2[i]) : data1[i].div (data2[i]));\r
 331 }\r
 332 return dqs;\r
 333 }, "~A,~A,~N,~B");\r
 334 c$.sphereMean = Clazz.defineMethod (c$, "sphereMean", \r
 335 function (data, retStddev, criterion) {\r
 336 if (data == null || data.length == 0) return  new JU.Quat ();\r
 337 if (retStddev == null) retStddev =  Clazz.newFloatArray (1, 0);\r
 338 if (data.length == 1) {\r
 339 retStddev[0] = 0;\r
 340 return JU.Quat.newQ (data[0]);\r
 341 }var diff = 3.4028235E38;\r
 342 var lastStddev = 3.4028235E38;\r
 343 var qMean = JU.Quat.simpleAverage (data);\r
 344 var maxIter = 100;\r
 345 var iter = 0;\r
 346 while (diff > criterion && lastStddev != 0 && iter < maxIter) {\r
 347 qMean = JU.Quat.newMean (data, qMean);\r
 348 retStddev[0] = JU.Quat.stdDev (data, qMean);\r
 349 diff = Math.abs (retStddev[0] - lastStddev);\r
 350 lastStddev = retStddev[0];\r
 351 }\r
 352 return qMean;\r
 353 }, "~A,~A,~N");\r
 354 c$.simpleAverage = Clazz.defineMethod (c$, "simpleAverage", \r
 355  function (ndata) {\r
 356 var mean = JU.V3.new3 (0, 0, 1);\r
 357 var v = ndata[0].getNormal ();\r
 358 mean.add (v);\r
 359 for (var i = ndata.length; --i >= 0; ) mean.add (ndata[i].getNormalDirected (mean));\r
 360 \r
 361 mean.sub (v);\r
 362 mean.normalize ();\r
 363 var f = 0;\r
 364 for (var i = ndata.length; --i >= 0; ) f += Math.abs (ndata[i].get3dProjection (v).dot (mean));\r
 365 \r
 366 if (f != 0) mean.scale (f / ndata.length);\r
 367 f = Math.sqrt (1 - mean.lengthSquared ());\r
 368 if (Float.isNaN (f)) f = 0;\r
 369 return JU.Quat.newP4 (JU.P4.new4 (mean.x, mean.y, mean.z, f));\r
 370 }, "~A");\r
 371 c$.newMean = Clazz.defineMethod (c$, "newMean", \r
 372  function (data, mean) {\r
 373 var sum =  new JU.V3 ();\r
 374 var v;\r
 375 var q;\r
 376 var dq;\r
 377 for (var i = data.length; --i >= 0; ) {\r
 378 q = data[i];\r
 379 dq = q.div (mean);\r
 380 v = dq.getNormal ();\r
 381 v.scale (dq.getTheta ());\r
 382 sum.add (v);\r
 383 }\r
 384 sum.scale (1 / data.length);\r
 385 var dqMean = JU.Quat.newVA (sum, sum.length ());\r
 386 return dqMean.mulQ (mean);\r
 387 }, "~A,JU.Quat");\r
 388 c$.stdDev = Clazz.defineMethod (c$, "stdDev", \r
 389  function (data, mean) {\r
 390 var sum2 = 0;\r
 391 var n = data.length;\r
 392 for (var i = n; --i >= 0; ) {\r
 393 var theta = data[i].div (mean).getTheta ();\r
 394 sum2 += theta * theta;\r
 395 }\r
 396 return Math.sqrt (sum2 / n);\r
 397 }, "~A,JU.Quat");\r
 398 Clazz.defineMethod (c$, "getEulerZYZ", \r
 399 function () {\r
 400 var rA;\r
 401 var rB;\r
 402 var rG;\r
 403 if (this.q1 == 0 && this.q2 == 0) {\r
 404 var theta = this.getTheta ();\r
 405 return  Clazz.newFloatArray (-1, [this.q3 < 0 ? -theta : theta, 0, 0]);\r
 406 }rA = Math.atan2 (2 * (this.q2 * this.q3 + this.q0 * this.q1), 2 * (-this.q1 * this.q3 + this.q0 * this.q2));\r
 407 rB = Math.acos (this.q3 * this.q3 - this.q2 * this.q2 - this.q1 * this.q1 + this.q0 * this.q0);\r
 408 rG = Math.atan2 (2 * (this.q2 * this.q3 - this.q0 * this.q1), 2 * (this.q0 * this.q2 + this.q1 * this.q3));\r
 409 return  Clazz.newFloatArray (-1, [(rA / 0.017453292519943295), (rB / 0.017453292519943295), (rG / 0.017453292519943295)]);\r
 410 });\r
 411 Clazz.defineMethod (c$, "getEulerZXZ", \r
 412 function () {\r
 413 var rA;\r
 414 var rB;\r
 415 var rG;\r
 416 if (this.q1 == 0 && this.q2 == 0) {\r
 417 var theta = this.getTheta ();\r
 418 return  Clazz.newFloatArray (-1, [this.q3 < 0 ? -theta : theta, 0, 0]);\r
 419 }rA = Math.atan2 (2 * (this.q1 * this.q3 - this.q0 * this.q2), 2 * (this.q0 * this.q1 + this.q2 * this.q3));\r
 420 rB = Math.acos (this.q3 * this.q3 - this.q2 * this.q2 - this.q1 * this.q1 + this.q0 * this.q0);\r
 421 rG = Math.atan2 (2 * (this.q1 * this.q3 + this.q0 * this.q2), 2 * (-this.q2 * this.q3 + this.q0 * this.q1));\r
 422 return  Clazz.newFloatArray (-1, [(rA / 0.017453292519943295), (rB / 0.017453292519943295), (rG / 0.017453292519943295)]);\r
 423 });\r
 424 c$.qZero = c$.prototype.qZero =  new JU.P4 ();\r
 425 Clazz.defineStatics (c$,\r
 426 "RAD_PER_DEG", 0.017453292519943295);\r
 427 });\r