Update MMDPhysics

examples/js/animation/MMDPhysics.js

@@ -4,1220 +4,1396 @@
  * Dependencies
  *  - Ammo.js https://github.com/kripken/ammo.js
  *
- * MMD specific Physics class.
- *
- * See THREE.MMDLoader for the passed parameter list of RigidBody/Constraint.
- *
- * Requirement:
- *  - don't change object's scale from (1,1,1) after setting physics to object
+ * MMDPhysics calculates physics with Ammo(Bullet based JavaScript Physics engine)
+ * for MMD model loaded by THREE.MMDLoader.
  *
  * TODO
- *  - optimize for the performance
- *  - use Physijs http://chandlerprall.github.io/Physijs/
- *    and improve the performance by making use of Web worker.
- *  - if possible, make this class being non-MMD specific.
- *  - object scale change support
+ *  - Physics in Worker
  */
 
-THREE.MMDPhysics = function ( mesh, params ) {
+THREE.MMDPhysics = ( function () {
+
+	/**
+	 * @param {THREE.SkinnedMesh} mesh
+	 * @param {Array<Object>} rigidBodyParams
+	 * @param {Array<Object>} (optional) constraintParams
+	 * @param {Object} params - (optional)
+	 * @param {Number} params.unitStep - Default is 1 / 65.
+	 * @param {Integer} params.maxStepNum - Default is 3.
+	 * @param {THREE.Vector3} params.gravity - Default is ( 0, - 9.8 * 10, 0 )
+	 */
+	function MMDPhysics( mesh, rigidBodyParams, constraintParams, params ) {
+
+		if ( typeof Ammo === 'undefined' ) {
+
+			throw new Error( 'THREE.MMDPhysics: Import ammo.js https://github.com/kripken/ammo.js' );
+
+		}
+
+		constraintParams = constraintParams || [];
+		params = params || {};
 
-	if ( typeof Ammo === 'undefined' ) {
+		this.manager = new ResourceManager();
 
-		throw new Error( 'THREE.MMDPhysics: Import ammo.js https://github.com/kripken/ammo.js' );
+		this.mesh = mesh;
+
+		/*
+		 * I don't know why but 1/60 unitStep easily breaks models
+		 * so I set it 1/65 so far.
+		 * Don't set too small unitStep because
+		 * the smaller unitStep can make the performance worse.
+		 */
+		this.unitStep = ( params.unitStep !== undefined ) ? params.unitStep : 1 / 65;
+		this.maxStepNum = ( params.maxStepNum !== undefined ) ? params.maxStepNum : 3;
+		this.gravity = new THREE.Vector3( 0, - 9.8 * 10, 0 );
+
+		if ( params.gravity !== undefined ) this.gravity.copy( gravity );
+
+		this.world = params.world !== undefined ? params.world : null; // experimental
+
+		this.bodies = [];
+		this.constraints = [];
+
+		this._init( mesh, rigidBodyParams, constraintParams );
 
 	}
 
-	if ( params === undefined ) params = {};
+	MMDPhysics.prototype = {
 
-	this.mesh = mesh;
-	this.helper = new THREE.MMDPhysics.ResourceHelper();
+		constructor: MMDPhysics,
 
-	/*
-	 * I don't know why but 1/60 unitStep easily breaks models
-	 * so I set it 1/65 so far.
-	 * Don't set too small unitStep because
-	 * the smaller unitStep can make the performance worse.
-	 */
-	this.unitStep = ( params.unitStep !== undefined ) ? params.unitStep : 1 / 65;
-	this.maxStepNum = ( params.maxStepNum !== undefined ) ? params.maxStepNum : 3;
+		/**
+		 * Advances Physics calculation and updates bones.
+		 *
+		 * @param {Number} delta - time in second
+		 * @return {THREE.MMDPhysics}
+		 */
+		update: function ( delta ) {
 
-	this.world = params.world !== undefined ? params.world : null;
-	this.bodies = [];
-	this.constraints = [];
+			var manager = this.manager;
+			var mesh = this.mesh;
 
-	this.init( mesh );
+			// rigid bodies and constrains are for
+			// mesh's world scale (1, 1, 1).
+			// Convert to (1, 1, 1) if it isn't.
 
-};
+			var isNonDefaultScale = false;
 
-THREE.MMDPhysics.prototype = {
+			var position = manager.allocThreeVector3();
+			var quaternion = manager.allocThreeQuaternion();
+			var scale = manager.allocThreeVector3();
 
-	constructor: THREE.MMDPhysics,
+			mesh.matrixWorld.decompose( position, quaternion, scale );
 
-	init: function ( mesh ) {
+			if ( scale.x !== 1 || scale.y !== 1 || scale.z !== 1 ) {
 
-		var parent = mesh.parent;
+				isNonDefaultScale = true;
 
-		if ( parent !== null ) {
+			}
 
-			parent.remove( mesh );
+			var parent;
 
-		}
+			if ( isNonDefaultScale ) {
 
-		var helper = this.helper;
-		var currentPosition = helper.allocThreeVector3();
-		var currentRotation = helper.allocThreeEuler();
-		var currentScale = helper.allocThreeVector3();
+				parent = mesh.parent;
 
-		currentPosition.copy( mesh.position );
-		currentRotation.copy( mesh.rotation );
-		currentScale.copy( mesh.scale );
+				if ( parent !== null ) parent.remove( mesh );
 
-		mesh.position.set( 0, 0, 0 );
-		mesh.rotation.set( 0, 0, 0 );
-		mesh.scale.set( 1, 1, 1 );
+				scale.copy( this.mesh.scale );
 
-		mesh.updateMatrixWorld( true );
+				mesh.scale.set( 1, 1, 1 );
+				mesh.updateMatrixWorld( true );
 
-		if ( this.world === null ) this.initWorld();
-		this.initRigidBodies();
-		this.initConstraints();
+			}
 
-		if ( parent !== null ) {
+			// calculate physics and update bones
 
-			parent.add( mesh );
+			this._updateRigidBodies();
+			this._stepSimulation( delta );
+			this._updateBones();
 
-		}
+			// restore mesh if converted above
 
-		mesh.position.copy( currentPosition );
-		mesh.rotation.copy( currentRotation );
-		mesh.scale.copy( currentScale );
+			if ( isNonDefaultScale ) {
 
-		mesh.updateMatrixWorld( true );
+				if ( parent !== null ) parent.add( mesh );
 
-		this.reset();
+				mesh.scale.copy( scale );
 
-		helper.freeThreeVector3( currentPosition );
-		helper.freeThreeEuler( currentRotation );
-		helper.freeThreeVector3( currentScale );
+			}
 
-	},
+			manager.freeThreeVector3( scale );
+			manager.freeThreeQuaternion( quaternion );
+			manager.freeThreeVector3( position );
 
-	initWorld: function () {
+			return this;
 
-		var config = new Ammo.btDefaultCollisionConfiguration();
-		var dispatcher = new Ammo.btCollisionDispatcher( config );
-		var cache = new Ammo.btDbvtBroadphase();
-		var solver = new Ammo.btSequentialImpulseConstraintSolver();
-		var world = new Ammo.btDiscreteDynamicsWorld( dispatcher, cache, solver, config );
-		world.setGravity( new Ammo.btVector3( 0, -9.8 * 10, 0 ) );
-		this.world = world;
+		},
 
-	},
+		/**
+		 * Resets rigid bodies transorm to current bone's.
+		 *
+		 * @return {THREE.MMDPhysics}
+		 */
+		reset: function () {
 
-	initRigidBodies: function () {
+			for ( var i = 0, il = this.bodies.length; i < il; i++ ) {
 
-		var bodies = this.mesh.geometry.rigidBodies;
+				this.bodies[ i ].reset();
 
-		for ( var i = 0; i < bodies.length; i++ ) {
+			}
 
-			var b = new THREE.MMDPhysics.RigidBody( this.mesh, this.world, bodies[ i ], this.helper );
-			this.bodies.push( b );
+			return this;
 
-		}
+		},
 
-	},
+		/**
+		 * Warm ups Physics. Calculates 1/60s cycles steps.
+		 *
+		 * @param {Integer} cycles
+		 * @return {THREE.MMDPhysics}
+		 */
+		warmup: function ( cycles ) {
 
-	initConstraints: function () {
+			for ( var i = 0; i < cycles; i++ ) {
 
-		var constraints = this.mesh.geometry.constraints;
+				this.update( 1 / 60 );
 
-		for ( var i = 0; i < constraints.length; i++ ) {
+			}
 
-			var params = constraints[ i ];
-			var bodyA = this.bodies[ params.rigidBodyIndex1 ];
-			var bodyB = this.bodies[ params.rigidBodyIndex2 ];
-			var c = new THREE.MMDPhysics.Constraint( this.mesh, this.world, bodyA, bodyB, params, this.helper );
-			this.constraints.push( c );
+			return this;
 
-		}
+		},
 
+		/**
+		 * Sets gravity.
+		 *
+		 * @param {THREE.Vector3} gravity
+		 */
+		setGravity: function ( gravity ) {
 
-	},
+			this.world.setGravity( new Ammo.btVector3( gravity.x, gravity.y, gravity.z ) );
+			this.gravity.copy( gravity );
 
-	update: function ( delta ) {
+		},
 
-		this.updateRigidBodies();
-		this.stepSimulation( delta );
-		this.updateBones();
+		// private methods
 
-	},
+		_init: function ( mesh, rigidBodyParams, constraintParams ) {
 
-	stepSimulation: function ( delta ) {
+			var manager = this.manager;
 
-		var unitStep = this.unitStep;
-		var stepTime = delta;
-		var maxStepNum = ( ( delta / unitStep ) | 0 ) + 1;
+			// rigid body/constraint parameters are for
+			// mesh's default world transform as position(0, 0, 0),
+			// quaternion(0, 0, 0, 1) and scale(0, 0, 0)
 
-		if ( stepTime < unitStep ) {
+			var parent = mesh.parent;
 
-			stepTime = unitStep;
-			maxStepNum = 1;
+			if ( parent !== null ) parent.remove( mesh );
 
-		}
+			var currentPosition = manager.allocThreeVector3();
+			var currentQuaternion = manager.allocThreeQuaternion();
+			var currentScale = manager.allocThreeVector3();
 
-		if ( maxStepNum > this.maxStepNum ) {
+			currentPosition.copy( mesh.position );
+			currentQuaternion.copy( mesh.quaternion );
+			currentScale.copy( mesh.scale );
 
-			maxStepNum = this.maxStepNum;
+			mesh.position.set( 0, 0, 0 );
+			mesh.quaternion.set( 0, 0, 0, 1 );
+			mesh.scale.set( 1, 1, 1 );
 
-		}
+			mesh.updateMatrixWorld( true );
 
-		this.world.stepSimulation( stepTime, maxStepNum, unitStep );
+			if ( this.world === null ) {
 
-	},
+				this.world = this._createWorld();
+				this.setGravity( this.gravity );
 
-	updateRigidBodies: function () {
+			}
 
-		for ( var i = 0; i < this.bodies.length; i++ ) {
+			this._initRigidBodies( rigidBodyParams );
+			this._initConstraints( constraintParams );
 
-			this.bodies[ i ].updateFromBone();
+			if ( parent !== null ) parent.add( mesh );
 
-		}
+			mesh.position.copy( currentPosition );
+			mesh.quaternion.copy( currentQuaternion );
+			mesh.scale.copy( currentScale );
 
-	},
+			mesh.updateMatrixWorld( true );
 
-	updateBones: function () {
+			this.reset();
 
-		for ( var i = 0; i < this.bodies.length; i++ ) {
+			manager.freeThreeVector3( currentPosition );
+			manager.freeThreeQuaternion( currentQuaternion );
+			manager.freeThreeVector3( currentScale );
 
-			this.bodies[ i ].updateBone();
+		},
 
-		}
+		_createWorld: function () {
 
-	},
+			var config = new Ammo.btDefaultCollisionConfiguration();
+			var dispatcher = new Ammo.btCollisionDispatcher( config );
+			var cache = new Ammo.btDbvtBroadphase();
+			var solver = new Ammo.btSequentialImpulseConstraintSolver();
+			var world = new Ammo.btDiscreteDynamicsWorld( dispatcher, cache, solver, config );
+			return world;
 
-	reset: function () {
+		},
 
-		for ( var i = 0; i < this.bodies.length; i++ ) {
+		_initRigidBodies: function ( rigidBodies ) {
 
-			this.bodies[ i ].reset();
+			for ( var i = 0, il = rigidBodies.length; i < il; i++ ) {
 
-		}
+				this.bodies.push( new RigidBody(
+					this.mesh, this.world, rigidBodies[ i ], this.manager ) );
 
-	},
+			}
 
-	warmup: function ( cycles ) {
+		},
 
-		for ( var i = 0; i < cycles; i++ ) {
+		_initConstraints: function ( constraints ) {
 
-			this.update( 1 / 60 );
+			for ( var i = 0, il = constraints.length; i < il; i++ ) {
 
-		}
+				var params = constraints[ i ];
+				var bodyA = this.bodies[ params.rigidBodyIndex1 ];
+				var bodyB = this.bodies[ params.rigidBodyIndex2 ];
+				this.constraints.push( new Constraint(
+					this.mesh, this.world, bodyA, bodyB, params, this.manager ) );
 
-	}
+			}
 
-};
 
-/**
- * This helper class responsibilies are
- *
- * 1. manage Ammo.js and Three.js object resources and
- *    improve the performance and the memory consumption by
- *    reusing objects.
- *
- * 2. provide simple Ammo object operations.
- */
-THREE.MMDPhysics.ResourceHelper = function () {
+		},
 
-	// for Three.js
-	this.threeVector3s = [];
-	this.threeMatrix4s = [];
-	this.threeQuaternions = [];
-	this.threeEulers = [];
+		_stepSimulation: function ( delta ) {
 
-	// for Ammo.js
-	this.transforms = [];
-	this.quaternions = [];
-	this.vector3s = [];
+			var unitStep = this.unitStep;
+			var stepTime = delta;
+			var maxStepNum = ( ( delta / unitStep ) | 0 ) + 1;
 
-};
+			if ( stepTime < unitStep ) {
 
-THREE.MMDPhysics.ResourceHelper.prototype = {
+				stepTime = unitStep;
+				maxStepNum = 1;
 
-	allocThreeVector3: function () {
+			}
 
-		return ( this.threeVector3s.length > 0 ) ? this.threeVector3s.pop() : new THREE.Vector3();
+			if ( maxStepNum > this.maxStepNum ) {
 
-	},
+				maxStepNum = this.maxStepNum;
 
-	freeThreeVector3: function ( v ) {
+			}
 
-		this.threeVector3s.push( v );
+			this.world.stepSimulation( stepTime, maxStepNum, unitStep );
 
-	},
+		},
 
-	allocThreeMatrix4: function () {
+		_updateRigidBodies: function () {
 
-		return ( this.threeMatrix4s.length > 0 ) ? this.threeMatrix4s.pop() : new THREE.Matrix4();
+			for ( var i = 0, il = this.bodies.length; i < il; i++ ) {
 
-	},
+				this.bodies[ i ].updateFromBone();
 
-	freeThreeMatrix4: function ( m ) {
+			}
 
-		this.threeMatrix4s.push( m );
+		},
 
-	},
+		_updateBones: function () {
 
-	allocThreeQuaternion: function () {
+			for ( var i = 0, il = this.bodies.length; i < il; i++ ) {
 
-		return ( this.threeQuaternions.length > 0 ) ? this.threeQuaternions.pop() : new THREE.Quaternion();
+				this.bodies[ i ].updateBone();
 
-	},
+			}
 
-	freeThreeQuaternion: function ( q ) {
+		}
 
-		this.threeQuaternions.push( q );
+	};
 
-	},
+	/**
+	 * This manager's responsibilies are
+	 *
+	 * 1. manage Ammo.js and Three.js object resources and
+	 *    improve the performance and the memory consumption by
+	 *    reusing objects.
+	 *
+	 * 2. provide simple Ammo object operations.
+	 */
+	function ResourceManager() {
 
-	allocThreeEuler: function () {
+		// for Three.js
+		this.threeVector3s = [];
+		this.threeMatrix4s = [];
+		this.threeQuaternions = [];
+		this.threeEulers = [];
 
-		return ( this.threeEulers.length > 0 ) ? this.threeEulers.pop() : new THREE.Euler();
+		// for Ammo.js
+		this.transforms = [];
+		this.quaternions = [];
+		this.vector3s = [];
 
-	},
+	}
 
-	freeThreeEuler: function ( e ) {
+	ResourceManager.prototype = {
 
-		this.threeEulers.push( e );
+		constructor: ResourceManager,
 
-	},
+		allocThreeVector3: function () {
 
-	allocTransform: function () {
+			return ( this.threeVector3s.length > 0 )
+				? this.threeVector3s.pop()
+				: new THREE.Vector3();
 
-		return ( this.transforms.length > 0 ) ? this.transforms.pop() : new Ammo.btTransform();
+		},
 
-	},
+		freeThreeVector3: function ( v ) {
 
-	freeTransform: function ( t ) {
+			this.threeVector3s.push( v );
 
-		this.transforms.push( t );
+		},
 
-	},
+		allocThreeMatrix4: function () {
 
-	allocQuaternion: function () {
+			return ( this.threeMatrix4s.length > 0 )
+				? this.threeMatrix4s.pop()
+				: new THREE.Matrix4();
 
-		return ( this.quaternions.length > 0 ) ? this.quaternions.pop() : new Ammo.btQuaternion();
+		},
 
-	},
+		freeThreeMatrix4: function ( m ) {
 
-	freeQuaternion: function ( q ) {
+			this.threeMatrix4s.push( m );
 
-		this.quaternions.push( q );
+		},
 
-	},
+		allocThreeQuaternion: function () {
 
-	allocVector3: function () {
+			return ( this.threeQuaternions.length > 0 )
+				? this.threeQuaternions.pop()
+				: new THREE.Quaternion();
 
-		return ( this.vector3s.length > 0 ) ? this.vector3s.pop() : new Ammo.btVector3();
+		},
 
-	},
+		freeThreeQuaternion: function ( q ) {
 
-	freeVector3: function ( v ) {
+			this.threeQuaternions.push( q );
 
-		this.vector3s.push( v );
+		},
 
-	},
+		allocThreeEuler: function () {
 
-	setIdentity: function ( t ) {
+			return ( this.threeEulers.length > 0 )
+				? this.threeEulers.pop()
+				: new THREE.Euler();
 
-		t.setIdentity();
+		},
 
-	},
+		freeThreeEuler: function ( e ) {
 
-	getBasis: function ( t ) {
+			this.threeEulers.push( e );
 
-		var q = this.allocQuaternion();
-		t.getBasis().getRotation( q );
-		return q;
+		},
 
-	},
+		allocTransform: function () {
 
-	getBasisAsMatrix3: function ( t ) {
+			return ( this.transforms.length > 0 )
+				? this.transforms.pop()
+				: new Ammo.btTransform();
 
-		var q = this.getBasis( t );
-		var m = this.quaternionToMatrix3( q );
-		this.freeQuaternion( q );
-		return m;
+		},
 
-	},
+		freeTransform: function ( t ) {
 
-	getOrigin: function( t ) {
+			this.transforms.push( t );
 
-		return t.getOrigin();
+		},
 
-	},
+		allocQuaternion: function () {
 
-	setOrigin: function( t, v ) {
+			return ( this.quaternions.length > 0 )
+				? this.quaternions.pop()
+				: new Ammo.btQuaternion();
 
-		t.getOrigin().setValue( v.x(), v.y(), v.z() );
+		},
 
-	},
+		freeQuaternion: function ( q ) {
 
-	copyOrigin: function( t1, t2 ) {
+			this.quaternions.push( q );
 
-		var o = t2.getOrigin();
-		this.setOrigin( t1, o );
+		},
 
-	},
+		allocVector3: function () {
 
-	setBasis: function( t, q ) {
+			return ( this.vector3s.length > 0 )
+				? this.vector3s.pop()
+				: new Ammo.btVector3();
 
-		t.setRotation( q );
+		},
 
-	},
+		freeVector3: function ( v ) {
 
-	setBasisFromMatrix3: function( t, m ) {
+			this.vector3s.push( v );
 
-		var q = this.matrix3ToQuaternion( m );
-		this.setBasis( t, q );
-		this.freeQuaternion( q );
+		},
 
-	},
+		setIdentity: function ( t ) {
 
-	setOriginFromArray3: function ( t, a ) {
+			t.setIdentity();
 
-		t.getOrigin().setValue( a[ 0 ], a[ 1 ], a[ 2 ] );
+		},
 
-	},
+		getBasis: function ( t ) {
 
-	setOriginFromThreeVector3: function ( t, v ) {
+			var q = this.allocQuaternion();
+			t.getBasis().getRotation( q );
+			return q;
 
-		t.getOrigin().setValue( v.x, v.y, v.z );
+		},
 
-	},
+		getBasisAsMatrix3: function ( t ) {
 
-	setBasisFromArray3: function ( t, a ) {
+			var q = this.getBasis( t );
+			var m = this.quaternionToMatrix3( q );
+			this.freeQuaternion( q );
+			return m;
 
-		var thQ = this.allocThreeQuaternion();
-		var thE = this.allocThreeEuler();
-		thE.set( a[ 0 ], a[ 1 ], a[ 2 ] );
-		this.setBasisFromThreeQuaternion( t, thQ.setFromEuler( thE ) );
+		},
 
-		this.freeThreeEuler( thE );
-		this.freeThreeQuaternion( thQ );
+		getOrigin: function( t ) {
 
-	},
+			return t.getOrigin();
 
-	setBasisFromThreeQuaternion: function ( t, a ) {
+		},
 
-		var q = this.allocQuaternion();
+		setOrigin: function( t, v ) {
 
-		q.setX( a.x );
-		q.setY( a.y );
-		q.setZ( a.z );
-		q.setW( a.w );
-		this.setBasis( t, q );
+			t.getOrigin().setValue( v.x(), v.y(), v.z() );
 
-		this.freeQuaternion( q );
+		},
 
-	},
+		copyOrigin: function( t1, t2 ) {
 
-	multiplyTransforms: function ( t1, t2 ) {
+			var o = t2.getOrigin();
+			this.setOrigin( t1, o );
 
-		var t = this.allocTransform();
-		this.setIdentity( t );
+		},
 
-		var m1 = this.getBasisAsMatrix3( t1 );
-		var m2 = this.getBasisAsMatrix3( t2 );
+		setBasis: function( t, q ) {
 
-		var o1 = this.getOrigin( t1 );
-		var o2 = this.getOrigin( t2 );
+			t.setRotation( q );
 
-		var v1 = this.multiplyMatrix3ByVector3( m1, o2 );
-		var v2 = this.addVector3( v1, o1 );
-		this.setOrigin( t, v2 );
+		},
 
-		var m3 = this.multiplyMatrices3( m1, m2 );
-		this.setBasisFromMatrix3( t, m3 );
+		setBasisFromMatrix3: function( t, m ) {
 
-		this.freeVector3( v1 );
-		this.freeVector3( v2 );
+			var q = this.matrix3ToQuaternion( m );
+			this.setBasis( t, q );
+			this.freeQuaternion( q );
 
-		return t;
+		},
 
-	},
+		setOriginFromArray3: function ( t, a ) {
 
-	inverseTransform: function ( t ) {
+			t.getOrigin().setValue( a[ 0 ], a[ 1 ], a[ 2 ] );
 
-		var t2 = this.allocTransform();
+		},
 
-		var m1 = this.getBasisAsMatrix3( t );
-		var o = this.getOrigin( t );
+		setOriginFromThreeVector3: function ( t, v ) {
 
-		var m2 = this.transposeMatrix3( m1 );
-		var v1 = this.negativeVector3( o );
-		var v2 = this.multiplyMatrix3ByVector3( m2, v1 );
+			t.getOrigin().setValue( v.x, v.y, v.z );
 
-		this.setOrigin( t2, v2 );
-		this.setBasisFromMatrix3( t2, m2 );
+		},
 
-		this.freeVector3( v1 );
-		this.freeVector3( v2 );
+		setBasisFromArray3: function ( t, a ) {
 
-		return t2;
+			var thQ = this.allocThreeQuaternion();
+			var thE = this.allocThreeEuler();
+			thE.set( a[ 0 ], a[ 1 ], a[ 2 ] );
+			this.setBasisFromThreeQuaternion( t, thQ.setFromEuler( thE ) );
 
-	},
+			this.freeThreeEuler( thE );
+			this.freeThreeQuaternion( thQ );
 
-	multiplyMatrices3: function ( m1, m2 ) {
+		},
 
-		var m3 = [];
+		setBasisFromThreeQuaternion: function ( t, a ) {
 
-		var v10 = this.rowOfMatrix3( m1, 0 );
-		var v11 = this.rowOfMatrix3( m1, 1 );
-		var v12 = this.rowOfMatrix3( m1, 2 );
+			var q = this.allocQuaternion();
 
-		var v20 = this.columnOfMatrix3( m2, 0 );
-		var v21 = this.columnOfMatrix3( m2, 1 );
-		var v22 = this.columnOfMatrix3( m2, 2 );
+			q.setX( a.x );
+			q.setY( a.y );
+			q.setZ( a.z );
+			q.setW( a.w );
+			this.setBasis( t, q );
 
-		m3[ 0 ] = this.dotVectors3( v10, v20 );
-		m3[ 1 ] = this.dotVectors3( v10, v21 );
-		m3[ 2 ] = this.dotVectors3( v10, v22 );
-		m3[ 3 ] = this.dotVectors3( v11, v20 );
-		m3[ 4 ] = this.dotVectors3( v11, v21 );
-		m3[ 5 ] = this.dotVectors3( v11, v22 );
-		m3[ 6 ] = this.dotVectors3( v12, v20 );
-		m3[ 7 ] = this.dotVectors3( v12, v21 );
-		m3[ 8 ] = this.dotVectors3( v12, v22 );
+			this.freeQuaternion( q );
 
-		this.freeVector3( v10 );
-		this.freeVector3( v11 );
-		this.freeVector3( v12 );
-		this.freeVector3( v20 );
-		this.freeVector3( v21 );
-		this.freeVector3( v22 );
+		},
 
-		return m3;
+		multiplyTransforms: function ( t1, t2 ) {
 
-	},
+			var t = this.allocTransform();
+			this.setIdentity( t );
 
-	addVector3: function( v1, v2 ) {
+			var m1 = this.getBasisAsMatrix3( t1 );
+			var m2 = this.getBasisAsMatrix3( t2 );
 
-		var v = this.allocVector3();
-		v.setValue( v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z() );
-		return v;
+			var o1 = this.getOrigin( t1 );
+			var o2 = this.getOrigin( t2 );
 
-	},
+			var v1 = this.multiplyMatrix3ByVector3( m1, o2 );
+			var v2 = this.addVector3( v1, o1 );
+			this.setOrigin( t, v2 );
 
-	dotVectors3: function( v1, v2 ) {
+			var m3 = this.multiplyMatrices3( m1, m2 );
+			this.setBasisFromMatrix3( t, m3 );
 
-		return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z();
+			this.freeVector3( v1 );
+			this.freeVector3( v2 );
 
-	},
+			return t;
 
-	rowOfMatrix3: function( m, i ) {
+		},
 
-		var v = this.allocVector3();
-		v.setValue( m[ i * 3 + 0 ], m[ i * 3 + 1 ], m[ i * 3 + 2 ] );
-		return v;
+		inverseTransform: function ( t ) {
 
-	},
+			var t2 = this.allocTransform();
 
-	columnOfMatrix3: function( m, i ) {
+			var m1 = this.getBasisAsMatrix3( t );
+			var o = this.getOrigin( t );
 
-		var v = this.allocVector3();
-		v.setValue( m[ i + 0 ], m[ i + 3 ], m[ i + 6 ] );
-		return v;
+			var m2 = this.transposeMatrix3( m1 );
+			var v1 = this.negativeVector3( o );
+			var v2 = this.multiplyMatrix3ByVector3( m2, v1 );
 
-	},
+			this.setOrigin( t2, v2 );
+			this.setBasisFromMatrix3( t2, m2 );
 
-	negativeVector3: function( v ) {
+			this.freeVector3( v1 );
+			this.freeVector3( v2 );
 
-		var v2 = this.allocVector3();
-		v2.setValue( -v.x(), -v.y(), -v.z() );
-		return v2;
+			return t2;
 
-	},
+		},
 
-	multiplyMatrix3ByVector3: function ( m, v ) {
+		multiplyMatrices3: function ( m1, m2 ) {
 
-		var v4 = this.allocVector3();
+			var m3 = [];
 
-		var v0 = this.rowOfMatrix3( m, 0 );
-		var v1 = this.rowOfMatrix3( m, 1 );
-		var v2 = this.rowOfMatrix3( m, 2 );
-		var x = this.dotVectors3( v0, v );
-		var y = this.dotVectors3( v1, v );
-		var z = this.dotVectors3( v2, v );
+			var v10 = this.rowOfMatrix3( m1, 0 );
+			var v11 = this.rowOfMatrix3( m1, 1 );
+			var v12 = this.rowOfMatrix3( m1, 2 );
 
-		v4.setValue( x, y, z );
+			var v20 = this.columnOfMatrix3( m2, 0 );
+			var v21 = this.columnOfMatrix3( m2, 1 );
+			var v22 = this.columnOfMatrix3( m2, 2 );
 
-		this.freeVector3( v0 );
-		this.freeVector3( v1 );
-		this.freeVector3( v2 );
+			m3[ 0 ] = this.dotVectors3( v10, v20 );
+			m3[ 1 ] = this.dotVectors3( v10, v21 );
+			m3[ 2 ] = this.dotVectors3( v10, v22 );
+			m3[ 3 ] = this.dotVectors3( v11, v20 );
+			m3[ 4 ] = this.dotVectors3( v11, v21 );
+			m3[ 5 ] = this.dotVectors3( v11, v22 );
+			m3[ 6 ] = this.dotVectors3( v12, v20 );
+			m3[ 7 ] = this.dotVectors3( v12, v21 );
+			m3[ 8 ] = this.dotVectors3( v12, v22 );
 
-		return v4;
+			this.freeVector3( v10 );
+			this.freeVector3( v11 );
+			this.freeVector3( v12 );
+			this.freeVector3( v20 );
+			this.freeVector3( v21 );
+			this.freeVector3( v22 );
 
-	},
+			return m3;
 
-	transposeMatrix3: function( m ) {
+		},
 
-		var m2 = [];
-		m2[ 0 ] = m[ 0 ];
-		m2[ 1 ] = m[ 3 ];
-		m2[ 2 ] = m[ 6 ];
-		m2[ 3 ] = m[ 1 ];
-		m2[ 4 ] = m[ 4 ];
-		m2[ 5 ] = m[ 7 ];
-		m2[ 6 ] = m[ 2 ];
-		m2[ 7 ] = m[ 5 ];
-		m2[ 8 ] = m[ 8 ];
-		return m2;
+		addVector3: function( v1, v2 ) {
 
-	},
+			var v = this.allocVector3();
+			v.setValue( v1.x() + v2.x(), v1.y() + v2.y(), v1.z() + v2.z() );
+			return v;
 
-	quaternionToMatrix3: function ( q ) {
+		},
 
-		var m = [];
+		dotVectors3: function( v1, v2 ) {
 
-		var x = q.x();
-		var y = q.y();
-		var z = q.z();
-		var w = q.w();
+			return v1.x() * v2.x() + v1.y() * v2.y() + v1.z() * v2.z();
 
-		var xx = x * x;
-		var yy = y * y;
-		var zz = z * z;
+		},
 
-		var xy = x * y;
-		var yz = y * z;
-		var zx = z * x;
+		rowOfMatrix3: function( m, i ) {
 
-		var xw = x * w;
-		var yw = y * w;
-		var zw = z * w;
+			var v = this.allocVector3();
+			v.setValue( m[ i * 3 + 0 ], m[ i * 3 + 1 ], m[ i * 3 + 2 ] );
+			return v;
 
-		m[ 0 ] = 1 - 2 * ( yy + zz );
-		m[ 1 ] = 2 * ( xy - zw );
-		m[ 2 ] = 2 * ( zx + yw );
-		m[ 3 ] = 2 * ( xy + zw );
-		m[ 4 ] = 1 - 2 * ( zz + xx );
-		m[ 5 ] = 2 * ( yz - xw );
-		m[ 6 ] = 2 * ( zx - yw );
-		m[ 7 ] = 2 * ( yz + xw );
-		m[ 8 ] = 1 - 2 * ( xx + yy );
+		},
 
-		return m;
+		columnOfMatrix3: function( m, i ) {
 
-	},
+			var v = this.allocVector3();
+			v.setValue( m[ i + 0 ], m[ i + 3 ], m[ i + 6 ] );
+			return v;
 
-	matrix3ToQuaternion: function( m ) {
+		},
 
-		var t = m[ 0 ] + m[ 4 ] + m[ 8 ];
-		var s, x, y, z, w;
+		negativeVector3: function( v ) {
 
-		if( t > 0 ) {
+			var v2 = this.allocVector3();
+			v2.setValue( -v.x(), -v.y(), -v.z() );
+			return v2;
 
-			s = Math.sqrt( t + 1.0 ) * 2;
-			w = 0.25 * s;
-			x = ( m[ 7 ] - m[ 5 ] ) / s;
-			y = ( m[ 2 ] - m[ 6 ] ) / s; 
-			z = ( m[ 3 ] - m[ 1 ] ) / s; 
+		},
 
-		} else if( ( m[ 0 ] > m[ 4 ] ) && ( m[ 0 ] > m[ 8 ] ) ) {
+		multiplyMatrix3ByVector3: function ( m, v ) {
 
-			s = Math.sqrt( 1.0 + m[ 0 ] - m[ 4 ] - m[ 8 ] ) * 2;
-			w = ( m[ 7 ] - m[ 5 ] ) / s;
-			x = 0.25 * s;
-			y = ( m[ 1 ] + m[ 3 ] ) / s;
-			z = ( m[ 2 ] + m[ 6 ] ) / s;
+			var v4 = this.allocVector3();
 
-		} else if( m[ 4 ] > m[ 8 ] ) {
+			var v0 = this.rowOfMatrix3( m, 0 );
+			var v1 = this.rowOfMatrix3( m, 1 );
+			var v2 = this.rowOfMatrix3( m, 2 );
+			var x = this.dotVectors3( v0, v );
+			var y = this.dotVectors3( v1, v );
+			var z = this.dotVectors3( v2, v );
 
-			s = Math.sqrt( 1.0 + m[ 4 ] - m[ 0 ] - m[ 8 ] ) * 2;
-			w = ( m[ 2 ] - m[ 6 ] ) / s;
-			x = ( m[ 1 ] + m[ 3 ] ) / s;
-			y = 0.25 * s;
-			z = ( m[ 5 ] + m[ 7 ] ) / s;
+			v4.setValue( x, y, z );
 
-		} else {
+			this.freeVector3( v0 );
+			this.freeVector3( v1 );
+			this.freeVector3( v2 );
 
-			s = Math.sqrt( 1.0 + m[ 8 ] - m[ 0 ] - m[ 4 ] ) * 2;
-			w = ( m[ 3 ] - m[ 1 ] ) / s;
-			x = ( m[ 2 ] + m[ 6 ] ) / s;
-			y = ( m[ 5 ] + m[ 7 ] ) / s;
-			z = 0.25 * s;
+			return v4;
 
-		}
+		},
 
-		var q = this.allocQuaternion();
-		q.setX( x );
-		q.setY( y );
-		q.setZ( z );
-		q.setW( w );
-		return q;
+		transposeMatrix3: function( m ) {
 
-	}
+			var m2 = [];
+			m2[ 0 ] = m[ 0 ];
+			m2[ 1 ] = m[ 3 ];
+			m2[ 2 ] = m[ 6 ];
+			m2[ 3 ] = m[ 1 ];
+			m2[ 4 ] = m[ 4 ];
+			m2[ 5 ] = m[ 7 ];
+			m2[ 6 ] = m[ 2 ];
+			m2[ 7 ] = m[ 5 ];
+			m2[ 8 ] = m[ 8 ];
+			return m2;
+
+		},
+
+		quaternionToMatrix3: function ( q ) {
+
+			var m = [];
+
+			var x = q.x();
+			var y = q.y();
+			var z = q.z();
+			var w = q.w();
+
+			var xx = x * x;
+			var yy = y * y;
+			var zz = z * z;
 
-};
+			var xy = x * y;
+			var yz = y * z;
+			var zx = z * x;
 
-THREE.MMDPhysics.RigidBody = function ( mesh, world, params, helper ) {
+			var xw = x * w;
+			var yw = y * w;
+			var zw = z * w;
 
-	this.mesh  = mesh;
-	this.world = world;
-	this.params = params;
-	this.helper = helper;
+			m[ 0 ] = 1 - 2 * ( yy + zz );
+			m[ 1 ] = 2 * ( xy - zw );
+			m[ 2 ] = 2 * ( zx + yw );
+			m[ 3 ] = 2 * ( xy + zw );
+			m[ 4 ] = 1 - 2 * ( zz + xx );
+			m[ 5 ] = 2 * ( yz - xw );
+			m[ 6 ] = 2 * ( zx - yw );
+			m[ 7 ] = 2 * ( yz + xw );
+			m[ 8 ] = 1 - 2 * ( xx + yy );
 
-	this.body = null;
-	this.bone = null;
-	this.boneOffsetForm = null;
-	this.boneOffsetFormInverse = null;
+			return m;
 
-	this.init();
+		},
 
-};
+		matrix3ToQuaternion: function( m ) {
 
-THREE.MMDPhysics.RigidBody.prototype = {
+			var t = m[ 0 ] + m[ 4 ] + m[ 8 ];
+			var s, x, y, z, w;
 
-	constructor: THREE.MMDPhysics.RigidBody,
+			if( t > 0 ) {
 
-	init: function () {
+				s = Math.sqrt( t + 1.0 ) * 2;
+				w = 0.25 * s;
+				x = ( m[ 7 ] - m[ 5 ] ) / s;
+				y = ( m[ 2 ] - m[ 6 ] ) / s; 
+				z = ( m[ 3 ] - m[ 1 ] ) / s; 
 
-		function generateShape( p ) {
+			} else if( ( m[ 0 ] > m[ 4 ] ) && ( m[ 0 ] > m[ 8 ] ) ) {
 
-			switch( p.shapeType ) {
+				s = Math.sqrt( 1.0 + m[ 0 ] - m[ 4 ] - m[ 8 ] ) * 2;
+				w = ( m[ 7 ] - m[ 5 ] ) / s;
+				x = 0.25 * s;
+				y = ( m[ 1 ] + m[ 3 ] ) / s;
+				z = ( m[ 2 ] + m[ 6 ] ) / s;
 
-				case 0:
-					return new Ammo.btSphereShape( p.width );
+			} else if( m[ 4 ] > m[ 8 ] ) {
 
-				case 1:
-					return new Ammo.btBoxShape( new Ammo.btVector3( p.width, p.height, p.depth ) );
+				s = Math.sqrt( 1.0 + m[ 4 ] - m[ 0 ] - m[ 8 ] ) * 2;
+				w = ( m[ 2 ] - m[ 6 ] ) / s;
+				x = ( m[ 1 ] + m[ 3 ] ) / s;
+				y = 0.25 * s;
+				z = ( m[ 5 ] + m[ 7 ] ) / s;
 
-				case 2:
-					return new Ammo.btCapsuleShape( p.width, p.height );
+			} else {
 
-				default:
-					throw 'unknown shape type ' + p.shapeType;
+				s = Math.sqrt( 1.0 + m[ 8 ] - m[ 0 ] - m[ 4 ] ) * 2;
+				w = ( m[ 3 ] - m[ 1 ] ) / s;
+				x = ( m[ 2 ] + m[ 6 ] ) / s;
+				y = ( m[ 5 ] + m[ 7 ] ) / s;
+				z = 0.25 * s;
 
 			}
 
+			var q = this.allocQuaternion();
+			q.setX( x );
+			q.setY( y );
+			q.setZ( z );
+			q.setW( w );
+			return q;
+
 		}
 
-		var helper = this.helper;
-		var params = this.params;
-		var bones = this.mesh.skeleton.bones;
-		var bone = ( params.boneIndex === -1 ) ? new THREE.Bone() : bones[ params.boneIndex ];
+	};
 
-		var shape = generateShape( params );
-		var weight = ( params.type === 0 ) ? 0 : params.weight;
-		var localInertia = helper.allocVector3();
-		localInertia.setValue( 0, 0, 0 );
+	/**
+	 * @param {THREE.SkinnedMesh} mesh
+	 * @param {Ammo.btDiscreteDynamicsWorld} world
+	 * @param {Object} params
+	 * @param {ResourceManager} manager
+	 */
+	function RigidBody( mesh, world, params, manager ) {
 
-		if( weight !== 0 ) {
+		this.mesh  = mesh;
+		this.world = world;
+		this.params = params;
+		this.manager = manager;
 
-			shape.calculateLocalInertia( weight, localInertia );
+		this.body = null;
+		this.bone = null;
+		this.boneOffsetForm = null;
+		this.boneOffsetFormInverse = null;
 
-		}
+		this._init();
 
-		var boneOffsetForm = helper.allocTransform();
-		helper.setIdentity( boneOffsetForm );
-		helper.setOriginFromArray3( boneOffsetForm, params.position );
-		helper.setBasisFromArray3( boneOffsetForm, params.rotation );
+	}
 
-		var vector = helper.allocThreeVector3();
-		var boneForm = helper.allocTransform();
-		helper.setIdentity( boneForm );
-		helper.setOriginFromThreeVector3( boneForm, bone.getWorldPosition( vector ) );
+	RigidBody.prototype = {
 
-		var form = helper.multiplyTransforms( boneForm, boneOffsetForm );
-		var state = new Ammo.btDefaultMotionState( form );
+		constructor: MMDPhysics.RigidBody,
 
-		var info = new Ammo.btRigidBodyConstructionInfo( weight, state, shape, localInertia );
-		info.set_m_friction( params.friction );
-		info.set_m_restitution( params.restitution );
+		/**
+		 * Resets rigid body transform to the current bone's.
+		 *
+		 * @return {RigidBody}
+		 */
+		reset: function () {
 
-		var body = new Ammo.btRigidBody( info );
+			this._setTransformFromBone();
+			return this;
 
-		if ( params.type === 0 ) {
+		},
 
-			body.setCollisionFlags( body.getCollisionFlags() | 2 );
+		/**
+		 * Updates rigid body's transform from the current bone.
+		 *
+		 * @return {RidigBody}
+		 */
+		updateFromBone: function () {
 
-			/*
-			 * It'd be better to comment out this line though in general I should call this method
-			 * because I'm not sure why but physics will be more like MMD's
-			 * if I comment out.
-			 */
-			body.setActivationState( 4 );
+			if ( this.params.boneIndex !== - 1 &&
+				this.params.type === 0 ) {
 
-		}
+				this._setTransformFromBone();
 
-		body.setDamping( params.positionDamping, params.rotationDamping );
-		body.setSleepingThresholds( 0, 0 );
+			}
 
-		this.world.addRigidBody( body, 1 << params.groupIndex, params.groupTarget );
+			return this;
 
-		this.body = body;
-		this.bone = bone;
-		this.boneOffsetForm = boneOffsetForm;
-		this.boneOffsetFormInverse = helper.inverseTransform( boneOffsetForm );
+		},
 
-		helper.freeVector3( localInertia );
-		helper.freeTransform( form );
-		helper.freeTransform( boneForm );
-		helper.freeThreeVector3( vector );
+		/**
+		 * Updates bone from the current ridid body's transform.
+		 *
+		 * @return {RidigBody}
+		 */
+		updateBone: function () {
 
-	},
+			if ( this.params.type === 0 ||
+				this.params.boneIndex === - 1 ) {
 
-	reset: function () {
+				return this;
 
-		this.setTransformFromBone();
+			}
 
-	},
+			this._updateBoneRotation();
 
-	updateFromBone: function () {
+			if ( this.params.type === 1 ) {
 
-		if ( this.params.boneIndex === -1 ) {
+				this._updateBonePosition();
 
-			return;
+			}
 
-		}
+			this.bone.updateMatrixWorld( true );
 
-		if ( this.params.type === 0 ) {
+			if ( this.params.type === 2 ) {
 
-			this.setTransformFromBone();
+				this._setPositionFromBone();
 
-		}
+			}
 
-	},
+			return this;
 
-	updateBone: function () {
+		},
 
-		if ( this.params.type === 0 || this.params.boneIndex === -1 ) {
+		// private methods
 
-			return;
+		_init: function () {
 
-		}
+			function generateShape( p ) {
 
-		this.updateBoneRotation();
+				switch( p.shapeType ) {
 
-		if ( this.params.type === 1 ) {
+					case 0:
+						return new Ammo.btSphereShape( p.width );
 
-			this.updateBonePosition();
+					case 1:
+						return new Ammo.btBoxShape( new Ammo.btVector3( p.width, p.height, p.depth ) );
 
-		}
+					case 2:
+						return new Ammo.btCapsuleShape( p.width, p.height );
 
-		this.bone.updateMatrixWorld( true );
+					default:
+						throw 'unknown shape type ' + p.shapeType;
 
-		if ( this.params.type === 2 ) {
+				}
 
-			this.setPositionFromBone();
+			}
 
-		}
+			var manager = this.manager;
+			var params = this.params;
+			var bones = this.mesh.skeleton.bones;
+			var bone = ( params.boneIndex === - 1 )
+				? new THREE.Bone()
+				: bones[ params.boneIndex ];
 
-	},
+			var shape = generateShape( params );
+			var weight = ( params.type === 0 ) ? 0 : params.weight;
+			var localInertia = manager.allocVector3();
+			localInertia.setValue( 0, 0, 0 );
 
-	getBoneTransform: function () {
+			if( weight !== 0 ) {
 
-		var helper = this.helper;
-		var p = helper.allocThreeVector3();
-		var q = helper.allocThreeQuaternion();
-		var s = helper.allocThreeVector3();
+				shape.calculateLocalInertia( weight, localInertia );
 
-		this.bone.matrixWorld.decompose( p, q, s );
+			}
+
+			var boneOffsetForm = manager.allocTransform();
+			manager.setIdentity( boneOffsetForm );
+			manager.setOriginFromArray3( boneOffsetForm, params.position );
+			manager.setBasisFromArray3( boneOffsetForm, params.rotation );
 
-		var tr = helper.allocTransform();
-		helper.setOriginFromThreeVector3( tr, p );
-		helper.setBasisFromThreeQuaternion( tr, q );
+			var vector = manager.allocThreeVector3();
+			var boneForm = manager.allocTransform();
+			manager.setIdentity( boneForm );
+			manager.setOriginFromThreeVector3( boneForm, bone.getWorldPosition( vector ) );
 
-		var form = helper.multiplyTransforms( tr, this.boneOffsetForm );
+			var form = manager.multiplyTransforms( boneForm, boneOffsetForm );
+			var state = new Ammo.btDefaultMotionState( form );
 
-		helper.freeTransform( tr );
-		helper.freeThreeVector3( s );
-		helper.freeThreeQuaternion( q );
-		helper.freeThreeVector3( p );
+			var info = new Ammo.btRigidBodyConstructionInfo( weight, state, shape, localInertia );
+			info.set_m_friction( params.friction );
+			info.set_m_restitution( params.restitution );
 
-		return form;
+			var body = new Ammo.btRigidBody( info );
 
-	},
+			if ( params.type === 0 ) {
 
-	getWorldTransformForBone: function () {
+				body.setCollisionFlags( body.getCollisionFlags() | 2 );
 
-		var helper = this.helper;
+				/*
+				 * It'd be better to comment out this line though in general I should call this method
+				 * because I'm not sure why but physics will be more like MMD's
+				 * if I comment out.
+				 */
+				body.setActivationState( 4 );
 
-		var tr = helper.allocTransform();
-		this.body.getMotionState().getWorldTransform( tr );
-		var tr2 = helper.multiplyTransforms( tr, this.boneOffsetFormInverse );
+			}
 
-		helper.freeTransform( tr );
+			body.setDamping( params.positionDamping, params.rotationDamping );
+			body.setSleepingThresholds( 0, 0 );
 
-		return tr2;
+			this.world.addRigidBody( body, 1 << params.groupIndex, params.groupTarget );
 
-	},
+			this.body = body;
+			this.bone = bone;
+			this.boneOffsetForm = boneOffsetForm;
+			this.boneOffsetFormInverse = manager.inverseTransform( boneOffsetForm );
 
-	setTransformFromBone: function () {
+			manager.freeVector3( localInertia );
+			manager.freeTransform( form );
+			manager.freeTransform( boneForm );
+			manager.freeThreeVector3( vector );
 
-		var helper = this.helper;
-		var form = this.getBoneTransform();
+		},
 
-		// TODO: check the most appropriate way to set
-		//this.body.setWorldTransform( form );
-		this.body.setCenterOfMassTransform( form );
-		this.body.getMotionState().setWorldTransform( form );
+		_getBoneTransform: function () {
 
-		helper.freeTransform( form );
+			var manager = this.manager;
+			var p = manager.allocThreeVector3();
+			var q = manager.allocThreeQuaternion();
+			var s = manager.allocThreeVector3();
 
-	},
+			this.bone.matrixWorld.decompose( p, q, s );
 
-	setPositionFromBone: function () {
+			var tr = manager.allocTransform();
+			manager.setOriginFromThreeVector3( tr, p );
+			manager.setBasisFromThreeQuaternion( tr, q );
 
-		var helper = this.helper;
-		var form = this.getBoneTransform();
+			var form = manager.multiplyTransforms( tr, this.boneOffsetForm );
 
-		var tr = helper.allocTransform();
-		this.body.getMotionState().getWorldTransform( tr );
-		helper.copyOrigin( tr, form );
+			manager.freeTransform( tr );
+			manager.freeThreeVector3( s );
+			manager.freeThreeQuaternion( q );
+			manager.freeThreeVector3( p );
 
-		// TODO: check the most appropriate way to set
-		//this.body.setWorldTransform( tr );
-		this.body.setCenterOfMassTransform( tr );
-		this.body.getMotionState().setWorldTransform( tr );
+			return form;
 
-		helper.freeTransform( tr );
-		helper.freeTransform( form );
+		},
 
-	},
+		_getWorldTransformForBone: function () {
 
-	updateBoneRotation: function () {
+			var manager = this.manager;
 
-		var helper = this.helper;
+			var tr = manager.allocTransform();
+			this.body.getMotionState().getWorldTransform( tr );
+			var tr2 = manager.multiplyTransforms( tr, this.boneOffsetFormInverse );
 
-		var tr = this.getWorldTransformForBone();
-		var q = helper.getBasis( tr );
+			manager.freeTransform( tr );
 
-		var thQ = helper.allocThreeQuaternion();
-		var thQ2 = helper.allocThreeQuaternion();
-		var thQ3 = helper.allocThreeQuaternion();
+			return tr2;
 
-		thQ.set( q.x(), q.y(), q.z(), q.w() );
-		thQ2.setFromRotationMatrix( this.bone.matrixWorld );
-		thQ2.conjugate();
-		thQ2.multiply( thQ );
+		},
 
-		//this.bone.quaternion.multiply( thQ2 );
+		_setTransformFromBone: function () {
 
-		thQ3.setFromRotationMatrix( this.bone.matrix );
-		this.bone.quaternion.copy( thQ2.multiply( thQ3 ) );
+			var manager = this.manager;
+			var form = this._getBoneTransform();
 
-		helper.freeThreeQuaternion( thQ );
-		helper.freeThreeQuaternion( thQ2 );
-		helper.freeThreeQuaternion( thQ3 );
+			// TODO: check the most appropriate way to set
+			//this.body.setWorldTransform( form );
+			this.body.setCenterOfMassTransform( form );
+			this.body.getMotionState().setWorldTransform( form );
 
-		helper.freeQuaternion( q );
-		helper.freeTransform( tr );
+			manager.freeTransform( form );
 
-	},
+		},
 
-	updateBonePosition: function () {
+		_setPositionFromBone: function () {
 
-		var helper = this.helper;
+			var manager = this.manager;
+			var form = this._getBoneTransform();
 
-		var tr = this.getWorldTransformForBone();
+			var tr = manager.allocTransform();
+			this.body.getMotionState().getWorldTransform( tr );
+			manager.copyOrigin( tr, form );
 
-		var thV = helper.allocThreeVector3();
+			// TODO: check the most appropriate way to set
+			//this.body.setWorldTransform( tr );
+			this.body.setCenterOfMassTransform( tr );
+			this.body.getMotionState().setWorldTransform( tr );
 
-		var o = helper.getOrigin( tr );
-		thV.set( o.x(), o.y(), o.z() );
+			manager.freeTransform( tr );
+			manager.freeTransform( form );
 
-		var v = this.bone.worldToLocal( thV );
-		this.bone.position.add( v );
+		},
 
-		helper.freeThreeVector3( thV );
+		_updateBoneRotation: function () {
 
-		helper.freeTransform( tr );
+			var manager = this.manager;
 
-	}
+			var tr = this._getWorldTransformForBone();
+			var q = manager.getBasis( tr );
+
+			var thQ = manager.allocThreeQuaternion();
+			var thQ2 = manager.allocThreeQuaternion();
+			var thQ3 = manager.allocThreeQuaternion();
+
+			thQ.set( q.x(), q.y(), q.z(), q.w() );
+			thQ2.setFromRotationMatrix( this.bone.matrixWorld );
+			thQ2.conjugate();
+			thQ2.multiply( thQ );
+
+			//this.bone.quaternion.multiply( thQ2 );
+
+			thQ3.setFromRotationMatrix( this.bone.matrix );
+			this.bone.quaternion.copy( thQ2.multiply( thQ3 ) );
+
+			manager.freeThreeQuaternion( thQ );
+			manager.freeThreeQuaternion( thQ2 );
+			manager.freeThreeQuaternion( thQ3 );
+
+			manager.freeQuaternion( q );
+			manager.freeTransform( tr );
+
+		},
+
+		_updateBonePosition: function () {
+
+			var manager = this.manager;
+
+			var tr = this._getWorldTransformForBone();
+
+			var thV = manager.allocThreeVector3();
+
+			var o = manager.getOrigin( tr );
+			thV.set( o.x(), o.y(), o.z() );
+
+			var v = this.bone.worldToLocal( thV );
+			this.bone.position.add( v );
+
+			manager.freeThreeVector3( thV );
+
+			manager.freeTransform( tr );
+
+		}
 
-};
+	};
 
-THREE.MMDPhysics.Constraint = function ( mesh, world, bodyA, bodyB, params, helper ) {
+	/**
+	 * @param {THREE.SkinnedMesh} mesh
+	 * @param {Ammo.btDiscreteDynamicsWorld} world
+	 * @param {RigidBody} bodyA
+	 * @param {RigidBody} bodyB
+	 * @param {Object} params
+	 * @param {ResourceManager} manager
+	 */
+	function Constraint( mesh, world, bodyA, bodyB, params, manager ) {
 
-	this.mesh  = mesh;
-	this.world = world;
-	this.bodyA = bodyA;
-	this.bodyB = bodyB;
-	this.params = params;
-	this.helper = helper;
+		this.mesh  = mesh;
+		this.world = world;
+		this.bodyA = bodyA;
+		this.bodyB = bodyB;
+		this.params = params;
+		this.manager = manager;
 
-	this.constraint = null;
+		this.constraint = null;
 
-	this.init();
+		this._init();
+
+	}
 
-};
+	Constraint.prototype = {
 
-THREE.MMDPhysics.Constraint.prototype = {
+		constructor: Constraint,
 
-	constructor: THREE.MMDPhysics.Constraint,
+		// private method
 
-	init: function () {
+		_init: function () {
 
-		var helper = this.helper;
-		var params = this.params;
-		var bodyA = this.bodyA;
-		var bodyB = this.bodyB;
+			var manager = this.manager;
+			var params = this.params;
+			var bodyA = this.bodyA;
+			var bodyB = this.bodyB;
 
-		var form = helper.allocTransform();
-		helper.setIdentity( form );
-		helper.setOriginFromArray3( form, params.position );
-		helper.setBasisFromArray3( form, params.rotation );
+			var form = manager.allocTransform();
+			manager.setIdentity( form );
+			manager.setOriginFromArray3( form, params.position );
+			manager.setBasisFromArray3( form, params.rotation );
 
-		var formA = helper.allocTransform();
-		var formB = helper.allocTransform();
+			var formA = manager.allocTransform();
+			var formB = manager.allocTransform();
 
-		bodyA.body.getMotionState().getWorldTransform( formA );
-		bodyB.body.getMotionState().getWorldTransform( formB );
+			bodyA.body.getMotionState().getWorldTransform( formA );
+			bodyB.body.getMotionState().getWorldTransform( formB );
 
-		var formInverseA = helper.inverseTransform( formA );
-		var formInverseB = helper.inverseTransform( formB );
+			var formInverseA = manager.inverseTransform( formA );
+			var formInverseB = manager.inverseTransform( formB );
 
-		var formA2 = helper.multiplyTransforms( formInverseA, form );
-		var formB2 = helper.multiplyTransforms( formInverseB, form );
+			var formA2 = manager.multiplyTransforms( formInverseA, form );
+			var formB2 = manager.multiplyTransforms( formInverseB, form );
 
-		var constraint = new Ammo.btGeneric6DofSpringConstraint( bodyA.body, bodyB.body, formA2, formB2, true );
+			var constraint = new Ammo.btGeneric6DofSpringConstraint( bodyA.body, bodyB.body, formA2, formB2, true );
 
-		var lll = helper.allocVector3();
-		var lul = helper.allocVector3();
-		var all = helper.allocVector3();
-		var aul = helper.allocVector3();
+			var lll = manager.allocVector3();
+			var lul = manager.allocVector3();
+			var all = manager.allocVector3();
+			var aul = manager.allocVector3();
 
-		lll.setValue( params.translationLimitation1[ 0 ],
-		              params.translationLimitation1[ 1 ],
-		              params.translationLimitation1[ 2 ] );
-		lul.setValue( params.translationLimitation2[ 0 ],
-		              params.translationLimitation2[ 1 ],
-		              params.translationLimitation2[ 2 ] );
-		all.setValue( params.rotationLimitation1[ 0 ],
-		              params.rotationLimitation1[ 1 ],
-		              params.rotationLimitation1[ 2 ] );
-		aul.setValue( params.rotationLimitation2[ 0 ],
-		              params.rotationLimitation2[ 1 ],
-		              params.rotationLimitation2[ 2 ] );
+			lll.setValue( params.translationLimitation1[ 0 ],
+			              params.translationLimitation1[ 1 ],
+			              params.translationLimitation1[ 2 ] );
+			lul.setValue( params.translationLimitation2[ 0 ],
+			              params.translationLimitation2[ 1 ],
+			              params.translationLimitation2[ 2 ] );
+			all.setValue( params.rotationLimitation1[ 0 ],
+			              params.rotationLimitation1[ 1 ],
+			              params.rotationLimitation1[ 2 ] );
+			aul.setValue( params.rotationLimitation2[ 0 ],
+			              params.rotationLimitation2[ 1 ],
+			              params.rotationLimitation2[ 2 ] );
 
-		constraint.setLinearLowerLimit( lll );
-		constraint.setLinearUpperLimit( lul );
-		constraint.setAngularLowerLimit( all );
-		constraint.setAngularUpperLimit( aul );
+			constraint.setLinearLowerLimit( lll );
+			constraint.setLinearUpperLimit( lul );
+			constraint.setAngularLowerLimit( all );
+			constraint.setAngularUpperLimit( aul );
 
-		for ( var i = 0; i < 3; i++ ) {
+			for ( var i = 0; i < 3; i++ ) {
 
-			if( params.springPosition[ i ] !== 0 ) {
+				if( params.springPosition[ i ] !== 0 ) {
 
-				constraint.enableSpring( i, true );
-				constraint.setStiffness( i, params.springPosition[ i ] );
+					constraint.enableSpring( i, true );
+					constraint.setStiffness( i, params.springPosition[ i ] );
+
+				}
 
 			}
 
-		}
+			for ( var i = 0; i < 3; i++ ) {
 
-		for ( var i = 0; i < 3; i++ ) {
+				if( params.springRotation[ i ] !== 0 ) {
 
-			if( params.springRotation[ i ] !== 0 ) {
+					constraint.enableSpring( i + 3, true );
+					constraint.setStiffness( i + 3, params.springRotation[ i ] );
 
-				constraint.enableSpring( i + 3, true );
-				constraint.setStiffness( i + 3, params.springRotation[ i ] );
+				}
 
 			}
 
-		}
+			/*
+			 * Currently(10/31/2016) official ammo.js doesn't support
+			 * btGeneric6DofSpringConstraint.setParam method.
+			 * You need custom ammo.js (add the method into idl) if you wanna use.
+			 * By setting this parameter, physics will be more like MMD's
+			 */
+			if ( constraint.setParam !== undefined ) {
 
-		/*
-		 * Currently(10/31/2016) official ammo.js doesn't support
-		 * btGeneric6DofSpringConstraint.setParam method.
-		 * You need custom ammo.js (add the method into idl) if you wanna use.
-		 * By setting this parameter, physics will be more like MMD's
-		 */
-		if ( constraint.setParam !== undefined ) {
+				for ( var i = 0; i < 6; i ++ ) {
 
-			for ( var i = 0; i < 6; i ++ ) {
+					// this parameter is from http://www20.atpages.jp/katwat/three.js_r58/examples/mytest37/mmd.three.js
+					constraint.setParam( 2, 0.475, i );
 
-				// this parameter is from http://www20.atpages.jp/katwat/three.js_r58/examples/mytest37/mmd.three.js
-				constraint.setParam( 2, 0.475, i );
+				}
 
 			}
 
-		}
+			this.world.addConstraint( constraint, true );
+			this.constraint = constraint;
+
+			manager.freeTransform( form );
+			manager.freeTransform( formA );
+			manager.freeTransform( formB );
+			manager.freeTransform( formInverseA );
+			manager.freeTransform( formInverseB );
+			manager.freeTransform( formA2 );
+			manager.freeTransform( formB2 );
+			manager.freeVector3( lll );
+			manager.freeVector3( lul );
+			manager.freeVector3( all );
+			manager.freeVector3( aul );
 
-		this.world.addConstraint( constraint, true );
-		this.constraint = constraint;
-
-		helper.freeTransform( form );
-		helper.freeTransform( formA );
-		helper.freeTransform( formB );
-		helper.freeTransform( formInverseA );
-		helper.freeTransform( formInverseB );
-		helper.freeTransform( formA2 );
-		helper.freeTransform( formB2 );
-		helper.freeVector3( lll );
-		helper.freeVector3( lul );
-		helper.freeVector3( all );
-		helper.freeVector3( aul );
+		}
 
-	}
+	};
 
-};
+	return MMDPhysics;
 
+} )();
 
-THREE.MMDPhysicsHelper = function ( mesh, physics ) {
 
-	if ( mesh.geometry.rigidBodies === undefined ) {
+/**
+ * Visualize Rigid bodies of MMDPhysics
+ */
+THREE.MMDPhysicsHelper = ( function () {
 
-		throw new Error( 'THREE.MMDPhysicsHelper requires rigidBodies in mesh.geometry.' );
+	/**
+	 * @param {THREE.SkinnedMesh} mesh
+	 * @param {THREE.Physics} physics
+	 */
+	function MMDPhysicsHelper( mesh, physics ) {
+
+		THREE.Object3D.call( this );
+
+		this.root = mesh;
+		this.physics = physics;
+
+		this.matrix = mesh.matrixWorld;
+		this.matrixAutoUpdate = false;
+
+		this.materials = [];
+
+		this.materials.push(
+			new THREE.MeshBasicMaterial( {
+				color: new THREE.Color( 0xff8888 ),
+				wireframe: true,
+				depthTest: false,
+				depthWrite: false,
+				opacity: 0.25,
+				transparent: true
+			} )
+		);
+
+		this.materials.push(
+			new THREE.MeshBasicMaterial( {
+				color: new THREE.Color( 0x88ff88 ),
+				wireframe: true,
+				depthTest: false,
+				depthWrite: false,
+				opacity: 0.25,
+				transparent: true
+			} )
+		);
+
+		this.materials.push(
+			new THREE.MeshBasicMaterial( {
+				color: new THREE.Color( 0x8888ff ),
+				wireframe: true,
+				depthTest: false,
+				depthWrite: false,
+				opacity: 0.25,
+				transparent: true
+			} )
+		);
+
+		this._init();
+		this.update();
 
 	}
 
-	THREE.Object3D.call( this );
+	MMDPhysicsHelper.prototype = Object.assign( Object.create( THREE.Object3D.prototype ), {
 
-	this.root = mesh;
-	this.physics = physics;
+		constructor: MMDPhysicsHelper,
 
-	this.matrix = mesh.matrixWorld;
-	this.matrixAutoUpdate = false;
+		/**
+		 * Updates Rigid Bodies visualization.
+		 *
+		 * @return {THREE.MMDPhysicsHelper}
+		 */
+		update: function () {
 
-	this.materials = [];
+			var vector = new THREE.Vector3();
+			var quaternion = new THREE.Quaternion();
+			var quaternion2 = new THREE.Quaternion();
+			var matrix = new THREE.Matrix4();
 
-	this.materials.push(
-		new THREE.MeshBasicMaterial( {
-			color: new THREE.Color( 0xff8888 ),
-			wireframe: true,
-			depthTest: false,
-			depthWrite: false,
-			opacity: 0.25,
-			transparent: true
-		} )
-	);
+			function getPosition( origin, matrixWorldInv ) {
 
-	this.materials.push(
-		new THREE.MeshBasicMaterial( {
-			color: new THREE.Color( 0x88ff88 ),
-			wireframe: true,
-			depthTest: false,
-			depthWrite: false,
-			opacity: 0.25,
-			transparent: true
-		} )
-	);
+				vector.set( origin.x(), origin.y(), origin.z() );
+				vector.applyMatrix4( matrixWorldInv );
 
-	this.materials.push(
-		new THREE.MeshBasicMaterial( {
-			color: new THREE.Color( 0x8888ff ),
-			wireframe: true,
-			depthTest: false,
-			depthWrite: false,
-			opacity: 0.25,
-			transparent: true
-		} )
-	);
+				return vector;
 
-	this._init();
-	this.update();
+			}
 
-};
+			function getQuaternion( rotation, matrixWorldInv ) {
 
-THREE.MMDPhysicsHelper.prototype = Object.create( THREE.Object3D.prototype );
-THREE.MMDPhysicsHelper.prototype.constructor = THREE.MMDPhysicsHelper;
+				quaternion.set( rotation.x(), rotation.y(), rotation.z(), rotation.w() );
+				quaternion2.setFromRotationMatrix( matrixWorldInv );
+				quaternion2.multiply( quaternion );
 
-THREE.MMDPhysicsHelper.prototype._init = function () {
+				return quaternion2;
 
-	var mesh = this.root;
-	var rigidBodies = mesh.geometry.rigidBodies;
+			}
 
-	function createGeometry( param ) {
+			return function update() {
 
-		switch ( param.shapeType ) {
+				var mesh = this.root;
+				var bodies = this.physics.bodies;
 
-			case 0:
-				return new THREE.SphereBufferGeometry( param.width, 16, 8 );
+				matrix.getInverse( mesh.matrixWorld );
 
-			case 1:
-				return new THREE.BoxBufferGeometry( param.width * 2, param.height * 2, param.depth * 2, 8, 8, 8 );
+				for ( var i = 0, il = bodies.length; i < il; i ++ ) {
 
-			case 2:
-				return new createCapsuleGeometry( param.width, param.height, 16, 8 );
+					var body = bodies[ i ].body;
+					var mesh = this.children[ i ];
 
-			default:
-				return null;
+					var tr = body.getCenterOfMassTransform();
 
-		}
+					mesh.position.copy( getPosition( tr.getOrigin(), matrix ) );
+					mesh.quaternion.copy( getQuaternion( tr.getRotation(), matrix ) );
 
-	}
+				}
 
-	// copy from http://www20.atpages.jp/katwat/three.js_r58/examples/mytest37/mytest37.js?ver=20160815
-	function createCapsuleGeometry( radius, cylinderHeight, segmentsRadius, segmentsHeight ) {
+				return this;
 
-		var geometry = new THREE.CylinderBufferGeometry( radius, radius, cylinderHeight, segmentsRadius, segmentsHeight, true );
-		var upperSphere = new THREE.Mesh( new THREE.SphereBufferGeometry( radius, segmentsRadius, segmentsHeight, 0, Math.PI * 2, 0, Math.PI / 2 ) );
-		var lowerSphere = new THREE.Mesh( new THREE.SphereBufferGeometry( radius, segmentsRadius, segmentsHeight, 0, Math.PI * 2, Math.PI / 2, Math.PI / 2 ) );
+			}
 
-		upperSphere.position.set( 0, cylinderHeight / 2, 0 );
-		lowerSphere.position.set( 0, -cylinderHeight / 2, 0 );
+		}(),
 
-		upperSphere.updateMatrix();
-		lowerSphere.updateMatrix();
+		// private method
 
-		geometry.merge( upperSphere.geometry, upperSphere.matrix );
-		geometry.merge( lowerSphere.geometry, lowerSphere.matrix );
+		_init: function () {
 
-		return geometry;
+			var mesh = this.root;
+			var bodies = this.physics.bodies;
 
-	}
+			function createGeometry( param ) {
 
-	for ( var i = 0, il = rigidBodies.length; i < il; i ++ ) {
+				switch ( param.shapeType ) {
 
-		var param = rigidBodies[ i ];
-		this.add( new THREE.Mesh( createGeometry( param ), this.materials[ param.type ] ) );
+					case 0:
+						return new THREE.SphereBufferGeometry( param.width, 16, 8 );
 
-	}
+					case 1:
+						return new THREE.BoxBufferGeometry( param.width * 2, param.height * 2, param.depth * 2, 8, 8, 8 );
 
-};
+					case 2:
+						return new createCapsuleGeometry( param.width, param.height, 16, 8 );
 
-THREE.MMDPhysicsHelper.prototype.update = function () {
+					default:
+						return null;
 
-	var mesh = this.root;
-	var rigidBodies = mesh.geometry.rigidBodies;
-	var bodies = this.physics.bodies;
+				}
 
-	var matrixWorldInv = new THREE.Matrix4().getInverse( mesh.matrixWorld );
-	var vector = new THREE.Vector3();
-	var quaternion = new THREE.Quaternion();
-	var quaternion2 = new THREE.Quaternion();
+			}
 
-	function getPosition( origin ) {
+			// copy from http://www20.atpages.jp/katwat/three.js_r58/examples/mytest37/mytest37.js?ver=20160815
+			function createCapsuleGeometry( radius, cylinderHeight, segmentsRadius, segmentsHeight ) {
 
-		vector.set( origin.x(), origin.y(), origin.z() );
-		vector.applyMatrix4( matrixWorldInv );
+				var geometry = new THREE.CylinderBufferGeometry( radius, radius, cylinderHeight, segmentsRadius, segmentsHeight, true );
+				var upperSphere = new THREE.Mesh( new THREE.SphereBufferGeometry( radius, segmentsRadius, segmentsHeight, 0, Math.PI * 2, 0, Math.PI / 2 ) );
+				var lowerSphere = new THREE.Mesh( new THREE.SphereBufferGeometry( radius, segmentsRadius, segmentsHeight, 0, Math.PI * 2, Math.PI / 2, Math.PI / 2 ) );
 
-		return vector;
+				upperSphere.position.set( 0, cylinderHeight / 2, 0 );
+				lowerSphere.position.set( 0, - cylinderHeight / 2, 0 );
 
-	}
+				upperSphere.updateMatrix();
+				lowerSphere.updateMatrix();
 
-	function getQuaternion( rotation ) {
+				geometry.merge( upperSphere.geometry, upperSphere.matrix );
+				geometry.merge( lowerSphere.geometry, lowerSphere.matrix );
 
-		quaternion.set( rotation.x(), rotation.y(), rotation.z(), rotation.w() );
-		quaternion2.setFromRotationMatrix( matrixWorldInv );
-		quaternion2.multiply( quaternion );
+				return geometry;
 
-		return quaternion2;
+			}
 
-	}
+			for ( var i = 0, il = bodies.length; i < il; i ++ ) {
 
-	for ( var i = 0, il = rigidBodies.length; i < il; i ++ ) {
+				var param = bodies[ i ].params;
+				this.add( new THREE.Mesh( createGeometry( param ), this.materials[ param.type ] ) );
 
-		var body = bodies[ i ].body;
-		var mesh = this.children[ i ];
+			}
 
-		var tr = body.getCenterOfMassTransform();
+		}
 
-		mesh.position.copy( getPosition( tr.getOrigin() ) );
-		mesh.quaternion.copy( getQuaternion( tr.getRotation() ) );
+	} );
 
-	}
+	return MMDPhysicsHelper;
 
-};
+} )();