Quaternion.js 10.6 KB
Newer Older
M
Mr.doob 已提交
1
/**
2
 * @author mikael emtinger / http://gomo.se/
3
 * @author alteredq / http://alteredqualia.com/
W
WestLangley 已提交
4
 * @author WestLangley / http://github.com/WestLangley
5
 * @author bhouston / http://clara.io
6 7
 */

8
THREE.Quaternion = function ( x, y, z, w ) {
9

10 11 12 13
	this._x = x || 0;
	this._y = y || 0;
	this._z = z || 0;
	this._w = ( w !== undefined ) ? w : 1;
M
Mr.doob 已提交
14

15 16
};

17 18 19
THREE.Quaternion.prototype = {

	constructor: THREE.Quaternion,
20

21 22 23 24 25 26 27 28 29
	get x () {

		return this._x;

	},

	set x ( value ) {

		this._x = value;
30
		this.onChangeCallback();
31 32 33 34 35 36 37 38 39 40 41 42

	},

	get y () {

		return this._y;

	},

	set y ( value ) {

		this._y = value;
43
		this.onChangeCallback();
44 45 46 47 48 49 50 51 52 53 54 55

	},

	get z () {

		return this._z;

	},

	set z ( value ) {

		this._z = value;
56
		this.onChangeCallback();
57 58 59 60 61 62 63 64 65 66 67 68

	},

	get w () {

		return this._w;

	},

	set w ( value ) {

		this._w = value;
69
		this.onChangeCallback();
70 71 72

	},

73
	set: function ( x, y, z, w ) {
74

75 76 77 78 79
		this._x = x;
		this._y = y;
		this._z = z;
		this._w = w;

80
		this.onChangeCallback();
81

M
Mr.doob 已提交
82
		return this;
83

M
Mr.doob 已提交
84
	},
85

86 87 88 89 90 91
	clone: function () {

		return new this.constructor( this._x, this._y, this._z, this._w );

	},

92 93
	copy: function ( quaternion ) {

M
Mr.doob 已提交
94 95 96 97
		this._x = quaternion.x;
		this._y = quaternion.y;
		this._z = quaternion.z;
		this._w = quaternion.w;
98

99
		this.onChangeCallback();
100 101 102 103 104

		return this;

	},

105
	setFromEuler: function ( euler, update ) {
M
Mr.doob 已提交
106

107
		if ( euler instanceof THREE.Euler === false ) {
108

109
			throw new Error( 'THREE.Quaternion: .setFromEuler() now expects a Euler rotation rather than a Vector3 and order.' );
G
gero3 已提交
110

111
		}
W
WestLangley 已提交
112 113 114 115

		// http://www.mathworks.com/matlabcentral/fileexchange/
		// 	20696-function-to-convert-between-dcm-euler-angles-quaternions-and-euler-vectors/
		//	content/SpinCalc.m
116

117 118 119 120 121 122
		var c1 = Math.cos( euler._x / 2 );
		var c2 = Math.cos( euler._y / 2 );
		var c3 = Math.cos( euler._z / 2 );
		var s1 = Math.sin( euler._x / 2 );
		var s2 = Math.sin( euler._y / 2 );
		var s3 = Math.sin( euler._z / 2 );
123

M
Mr.doob 已提交
124 125 126
		var order = euler.order;

		if ( order === 'XYZ' ) {
127

128 129 130 131
			this._x = s1 * c2 * c3 + c1 * s2 * s3;
			this._y = c1 * s2 * c3 - s1 * c2 * s3;
			this._z = c1 * c2 * s3 + s1 * s2 * c3;
			this._w = c1 * c2 * c3 - s1 * s2 * s3;
132

M
Mr.doob 已提交
133
		} else if ( order === 'YXZ' ) {
134

135 136 137 138
			this._x = s1 * c2 * c3 + c1 * s2 * s3;
			this._y = c1 * s2 * c3 - s1 * c2 * s3;
			this._z = c1 * c2 * s3 - s1 * s2 * c3;
			this._w = c1 * c2 * c3 + s1 * s2 * s3;
139

M
Mr.doob 已提交
140
		} else if ( order === 'ZXY' ) {
141

142 143 144 145
			this._x = s1 * c2 * c3 - c1 * s2 * s3;
			this._y = c1 * s2 * c3 + s1 * c2 * s3;
			this._z = c1 * c2 * s3 + s1 * s2 * c3;
			this._w = c1 * c2 * c3 - s1 * s2 * s3;
146

M
Mr.doob 已提交
147
		} else if ( order === 'ZYX' ) {
148

149 150 151 152
			this._x = s1 * c2 * c3 - c1 * s2 * s3;
			this._y = c1 * s2 * c3 + s1 * c2 * s3;
			this._z = c1 * c2 * s3 - s1 * s2 * c3;
			this._w = c1 * c2 * c3 + s1 * s2 * s3;
153

M
Mr.doob 已提交
154
		} else if ( order === 'YZX' ) {
155

156 157 158 159
			this._x = s1 * c2 * c3 + c1 * s2 * s3;
			this._y = c1 * s2 * c3 + s1 * c2 * s3;
			this._z = c1 * c2 * s3 - s1 * s2 * c3;
			this._w = c1 * c2 * c3 - s1 * s2 * s3;
160

M
Mr.doob 已提交
161
		} else if ( order === 'XZY' ) {
162

163 164 165 166
			this._x = s1 * c2 * c3 - c1 * s2 * s3;
			this._y = c1 * s2 * c3 - s1 * c2 * s3;
			this._z = c1 * c2 * s3 + s1 * s2 * c3;
			this._w = c1 * c2 * c3 + s1 * s2 * s3;
167

W
WestLangley 已提交
168
		}
169

170
		if ( update !== false ) this.onChangeCallback();
171

M
Mr.doob 已提交
172
		return this;
173

M
Mr.doob 已提交
174
	},
175

176 177
	setFromAxisAngle: function ( axis, angle ) {

178 179 180
		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm

		// assumes axis is normalized
181

182
		var halfAngle = angle / 2, s = Math.sin( halfAngle );
183

184 185 186 187 188
		this._x = axis.x * s;
		this._y = axis.y * s;
		this._z = axis.z * s;
		this._w = Math.cos( halfAngle );

189
		this.onChangeCallback();
190 191 192 193

		return this;

	},
194

M
Mr.doob 已提交
195
	setFromRotationMatrix: function ( m ) {
196

W
WestLangley 已提交
197
		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
198

W
WestLangley 已提交
199
		// assumes the upper 3x3 of m is a pure rotation matrix (i.e, unscaled)
200

W
WestLangley 已提交
201
		var te = m.elements,
202

203 204 205
			m11 = te[ 0 ], m12 = te[ 4 ], m13 = te[ 8 ],
			m21 = te[ 1 ], m22 = te[ 5 ], m23 = te[ 9 ],
			m31 = te[ 2 ], m32 = te[ 6 ], m33 = te[ 10 ],
206

W
WestLangley 已提交
207 208
			trace = m11 + m22 + m33,
			s;
209 210 211

		if ( trace > 0 ) {

W
WestLangley 已提交
212
			s = 0.5 / Math.sqrt( trace + 1.0 );
213

214 215 216 217
			this._w = 0.25 / s;
			this._x = ( m32 - m23 ) * s;
			this._y = ( m13 - m31 ) * s;
			this._z = ( m21 - m12 ) * s;
218

W
WestLangley 已提交
219
		} else if ( m11 > m22 && m11 > m33 ) {
220

W
WestLangley 已提交
221
			s = 2.0 * Math.sqrt( 1.0 + m11 - m22 - m33 );
222

223
			this._w = ( m32 - m23 ) / s;
224
			this._x = 0.25 * s;
225 226
			this._y = ( m12 + m21 ) / s;
			this._z = ( m13 + m31 ) / s;
227 228 229

		} else if ( m22 > m33 ) {

W
WestLangley 已提交
230
			s = 2.0 * Math.sqrt( 1.0 + m22 - m11 - m33 );
231

232 233
			this._w = ( m13 - m31 ) / s;
			this._x = ( m12 + m21 ) / s;
234
			this._y = 0.25 * s;
235
			this._z = ( m23 + m32 ) / s;
236

W
WestLangley 已提交
237
		} else {
238

W
WestLangley 已提交
239
			s = 2.0 * Math.sqrt( 1.0 + m33 - m11 - m22 );
240

241 242 243 244
			this._w = ( m21 - m12 ) / s;
			this._x = ( m13 + m31 ) / s;
			this._y = ( m23 + m32 ) / s;
			this._z = 0.25 * s;
245

M
Mr.doob 已提交
246
		}
247

248
		this.onChangeCallback();
249

M
Mr.doob 已提交
250
		return this;
251

M
Mr.doob 已提交
252
	},
253

254 255
	setFromUnitVectors: function () {

256
		// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
257 258 259

		// assumes direction vectors vFrom and vTo are normalized

260 261 262
		var v1, r;

		var EPS = 0.000001;
263

264
		return function ( vFrom, vTo ) {
265 266 267

			if ( v1 === undefined ) v1 = new THREE.Vector3();

268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288
			r = vFrom.dot( vTo ) + 1;

			if ( r < EPS ) {

				r = 0;

				if ( Math.abs( vFrom.x ) > Math.abs( vFrom.z ) ) {

					v1.set( - vFrom.y, vFrom.x, 0 );

				} else {

					v1.set( 0, - vFrom.z, vFrom.y );

				}

			} else {

				v1.crossVectors( vFrom, vTo );

			}
289

290 291 292 293
			this._x = v1.x;
			this._y = v1.y;
			this._z = v1.z;
			this._w = r;
294

295
			this.normalize();
296 297 298

			return this;

M
Mr.doob 已提交
299
		};
300 301 302

	}(),

W
WestLangley 已提交
303
	inverse: function () {
304

W
WestLangley 已提交
305
		this.conjugate().normalize();
306

M
Mr.doob 已提交
307
		return this;
308

M
Mr.doob 已提交
309
	},
310

W
WestLangley 已提交
311
	conjugate: function () {
312

313 314 315
		this._x *= - 1;
		this._y *= - 1;
		this._z *= - 1;
316

317
		this.onChangeCallback();
318

M
Mr.doob 已提交
319
		return this;
A
alteredq 已提交
320

M
Mr.doob 已提交
321
	},
322

323 324 325 326 327 328
	dot: function ( v ) {

		return this._x * v._x + this._y * v._y + this._z * v._z + this._w * v._w;

	},

329 330
	lengthSq: function () {

331
		return this._x * this._x + this._y * this._y + this._z * this._z + this._w * this._w;
332 333 334

	},

335
	length: function () {
336

337
		return Math.sqrt( this._x * this._x + this._y * this._y + this._z * this._z + this._w * this._w );
338

M
Mr.doob 已提交
339
	},
340

341
	normalize: function () {
342

343
		var l = this.length();
344

345
		if ( l === 0 ) {
346

347 348 349 350
			this._x = 0;
			this._y = 0;
			this._z = 0;
			this._w = 1;
351

M
Mr.doob 已提交
352
		} else {
353

M
Mr.doob 已提交
354
			l = 1 / l;
355

356 357 358 359
			this._x = this._x * l;
			this._y = this._y * l;
			this._z = this._z * l;
			this._w = this._w * l;
360

M
Mr.doob 已提交
361
		}
362

363 364
		this.onChangeCallback();

M
Mr.doob 已提交
365
		return this;
366

M
Mr.doob 已提交
367
	},
368

369 370 371 372
	multiply: function ( q, p ) {

		if ( p !== undefined ) {

373
			console.warn( 'THREE.Quaternion: .multiply() now only accepts one argument. Use .multiplyQuaternions( a, b ) instead.' );
374 375 376 377 378 379 380 381 382
			return this.multiplyQuaternions( q, p );

		}

		return this.multiplyQuaternions( this, q );

	},

	multiplyQuaternions: function ( a, b ) {
383

384
		// from http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/code/index.htm
M
Mr.doob 已提交
385

386 387
		var qax = a._x, qay = a._y, qaz = a._z, qaw = a._w;
		var qbx = b._x, qby = b._y, qbz = b._z, qbw = b._w;
388

389 390 391 392 393
		this._x = qax * qbw + qaw * qbx + qay * qbz - qaz * qby;
		this._y = qay * qbw + qaw * qby + qaz * qbx - qax * qbz;
		this._z = qaz * qbw + qaw * qbz + qax * qby - qay * qbx;
		this._w = qaw * qbw - qax * qbx - qay * qby - qaz * qbz;

394
		this.onChangeCallback();
395

396
		return this;
397 398 399

	},

400
	slerp: function ( qb, t ) {
A
alteredq 已提交
401

H
Henri Astre 已提交
402 403
		if ( t === 0 ) return this;
		if ( t === 1 ) return this.copy( qb );
404

405
		var x = this._x, y = this._y, z = this._z, w = this._w;
A
alteredq 已提交
406 407 408

		// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/

409
		var cosHalfTheta = w * qb._w + x * qb._x + y * qb._y + z * qb._z;
A
alteredq 已提交
410 411 412

		if ( cosHalfTheta < 0 ) {

413 414 415 416
			this._w = - qb._w;
			this._x = - qb._x;
			this._y = - qb._y;
			this._z = - qb._z;
A
alteredq 已提交
417

418
			cosHalfTheta = - cosHalfTheta;
A
alteredq 已提交
419 420 421 422 423 424 425 426 427

		} else {

			this.copy( qb );

		}

		if ( cosHalfTheta >= 1.0 ) {

428 429 430 431
			this._w = w;
			this._x = x;
			this._y = y;
			this._z = z;
A
alteredq 已提交
432 433 434 435 436 437 438 439 440

			return this;

		}

		var sinHalfTheta = Math.sqrt( 1.0 - cosHalfTheta * cosHalfTheta );

		if ( Math.abs( sinHalfTheta ) < 0.001 ) {

441 442 443 444
			this._w = 0.5 * ( w + this._w );
			this._x = 0.5 * ( x + this._x );
			this._y = 0.5 * ( y + this._y );
			this._z = 0.5 * ( z + this._z );
A
alteredq 已提交
445 446 447 448 449

			return this;

		}

450
		var halfTheta = Math.atan2( sinHalfTheta, cosHalfTheta );
A
alteredq 已提交
451 452 453
		var ratioA = Math.sin( ( 1 - t ) * halfTheta ) / sinHalfTheta,
		ratioB = Math.sin( t * halfTheta ) / sinHalfTheta;

454 455 456 457 458
		this._w = ( w * ratioA + this._w * ratioB );
		this._x = ( x * ratioA + this._x * ratioB );
		this._y = ( y * ratioA + this._y * ratioB );
		this._z = ( z * ratioA + this._z * ratioB );

459
		this.onChangeCallback();
A
alteredq 已提交
460 461 462 463 464

		return this;

	},

465
	equals: function ( quaternion ) {
466

467
		return ( quaternion._x === this._x ) && ( quaternion._y === this._y ) && ( quaternion._z === this._z ) && ( quaternion._w === this._w );
468 469 470

	},

471
	fromArray: function ( array, offset ) {
472

473 474 475 476 477 478
		if ( offset === undefined ) offset = 0;

		this._x = array[ offset ];
		this._y = array[ offset + 1 ];
		this._z = array[ offset + 2 ];
		this._w = array[ offset + 3 ];
479

480
		this.onChangeCallback();
481 482 483 484 485

		return this;

	},

A
arose 已提交
486
	toArray: function ( array, offset ) {
487

A
arose 已提交
488 489
		if ( array === undefined ) array = [];
		if ( offset === undefined ) offset = 0;
490

A
arose 已提交
491 492 493 494
		array[ offset ] = this._x;
		array[ offset + 1 ] = this._y;
		array[ offset + 2 ] = this._z;
		array[ offset + 3 ] = this._w;
495

A
arose 已提交
496
		return array;
497 498 499

	},

500 501 502 503 504 505 506 507
	onChange: function ( callback ) {

		this.onChangeCallback = callback;

		return this;

	},

508
	onChangeCallback: function () {}
509

510
};
511

512
Object.assign( THREE.Quaternion, {
513

514 515 516 517 518 519 520 521 522 523 524
	slerp: function( qa, qb, qm, t ) {

		return qm.copy( qa ).slerp( qb, t );

	},

	slerpFlat: function(
			dst, dstOffset, src0, srcOffset0, src1, srcOffset1, t ) {

		// fuzz-free, array-based Quaternion SLERP operation

M
Mr.doob 已提交
525
		var x0 = src0[ srcOffset0 + 0 ],
526 527 528 529
			y0 = src0[ srcOffset0 + 1 ],
			z0 = src0[ srcOffset0 + 2 ],
			w0 = src0[ srcOffset0 + 3 ],

M
Mr.doob 已提交
530
			x1 = src1[ srcOffset1 + 0 ],
531 532 533 534 535 536 537 538 539 540
			y1 = src1[ srcOffset1 + 1 ],
			z1 = src1[ srcOffset1 + 2 ],
			w1 = src1[ srcOffset1 + 3 ];

		if ( w0 !== w1 || x0 !== x1 || y0 !== y1 || z0 !== z1 ) {

			var s = 1 - t,

				cos = x0 * x1 + y0 * y1 + z0 * z1 + w0 * w1,

541
				dir = ( cos >= 0 ? 1 : - 1 ),
542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575
				sqrSin = 1 - cos * cos;

			// Skip the Slerp for tiny steps to avoid numeric problems:
			if ( sqrSin > Number.EPSILON ) {

				var sin = Math.sqrt( sqrSin ),
					len = Math.atan2( sin, cos * dir );

				s = Math.sin( s * len ) / sin;
				t = Math.sin( t * len ) / sin;

			}

			var tDir = t * dir;

			x0 = x0 * s + x1 * tDir;
			y0 = y0 * s + y1 * tDir;
			z0 = z0 * s + z1 * tDir;
			w0 = w0 * s + w1 * tDir;

			// Normalize in case we just did a lerp:
			if ( s === 1 - t ) {

				var f = 1 / Math.sqrt( x0 * x0 + y0 * y0 + z0 * z0 + w0 * w0 );

				x0 *= f;
				y0 *= f;
				z0 *= f;
				w0 *= f;

			}

		}

576
		dst[ dstOffset ] = x0;
577 578 579 580 581 582 583
		dst[ dstOffset + 1 ] = y0;
		dst[ dstOffset + 2 ] = z0;
		dst[ dstOffset + 3 ] = w0;

	}

} );