CCDIKSolver.js 8.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394
  1. /**
  2. * @author takahiro / https://github.com/takahirox
  3. *
  4. * CCD Algorithm
  5. * https://sites.google.com/site/auraliusproject/ccd-algorithm
  6. *
  7. * mesh.geometry needs to have iks array.
  8. *
  9. * // ik parameter example
  10. * //
  11. * // target, effector, index in links are bone index in skeleton.
  12. * // the bones relation should be
  13. * // <-- parent child -->
  14. * // links[ n ], links[ n - 1 ], ..., links[ 0 ], effector
  15. * ik = {
  16. * target: 1,
  17. * effector: 2,
  18. * links: [ { index: 5, limitation: new THREE.Vector3( 1, 0, 0 ) }, { index: 4, enabled: false }, { index : 3 } ],
  19. * iteration: 10,
  20. * minAngle: 0.0,
  21. * maxAngle: 1.0,
  22. * };
  23. */
  24. THREE.CCDIKSolver = function ( mesh ) {
  25. this.mesh = mesh;
  26. this._valid();
  27. };
  28. THREE.CCDIKSolver.prototype = {
  29. constructor: THREE.CCDIKSolver,
  30. _valid: function () {
  31. var iks = this.mesh.geometry.iks;
  32. var bones = this.mesh.skeleton.bones;
  33. for ( var i = 0, il = iks.length; i < il; i ++ ) {
  34. var ik = iks[ i ];
  35. var effector = bones[ ik.effector ];
  36. var links = ik.links;
  37. var link0, link1;
  38. link0 = effector;
  39. for ( var j = 0, jl = links.length; j < jl; j ++ ) {
  40. link1 = bones[ links[ j ].index ];
  41. if ( link0.parent !== link1 ) {
  42. console.warn( 'THREE.CCDIKSolver: bone ' + link0.name + ' is not the child of bone ' + link1.name );
  43. }
  44. link0 = link1;
  45. }
  46. }
  47. },
  48. update: function () {
  49. var q = new THREE.Quaternion();
  50. var targetPos = new THREE.Vector3();
  51. var targetVec = new THREE.Vector3();
  52. var effectorPos = new THREE.Vector3();
  53. var effectorVec = new THREE.Vector3();
  54. var linkPos = new THREE.Vector3();
  55. var invLinkQ = new THREE.Quaternion();
  56. var axis = new THREE.Vector3();
  57. var bones = this.mesh.skeleton.bones;
  58. var iks = this.mesh.geometry.iks;
  59. var boneParams = this.mesh.geometry.bones;
  60. // for reference overhead reduction in loop
  61. var math = Math;
  62. this.mesh.updateMatrixWorld( true );
  63. for ( var i = 0, il = iks.length; i < il; i++ ) {
  64. var ik = iks[ i ];
  65. var effector = bones[ ik.effector ];
  66. var target = bones[ ik.target ];
  67. // don't use getWorldPosition() here for the performance
  68. // because it calls updateMatrixWorld( true ) inside.
  69. targetPos.setFromMatrixPosition( target.matrixWorld );
  70. var links = ik.links;
  71. var iteration = ik.iteration !== undefined ? ik.iteration : 1;
  72. for ( var j = 0; j < iteration; j++ ) {
  73. var rotated = false;
  74. for ( var k = 0, kl = links.length; k < kl; k++ ) {
  75. var link = bones[ links[ k ].index ];
  76. // skip this link and following links.
  77. // this skip is used for MMD performance optimization.
  78. if ( links[ k ].enabled === false ) break;
  79. var limitation = links[ k ].limitation;
  80. // don't use getWorldPosition/Quaternion() here for the performance
  81. // because they call updateMatrixWorld( true ) inside.
  82. linkPos.setFromMatrixPosition( link.matrixWorld );
  83. invLinkQ.setFromRotationMatrix( link.matrixWorld ).inverse();
  84. effectorPos.setFromMatrixPosition( effector.matrixWorld );
  85. // work in link world
  86. effectorVec.subVectors( effectorPos, linkPos );
  87. effectorVec.applyQuaternion( invLinkQ );
  88. effectorVec.normalize();
  89. targetVec.subVectors( targetPos, linkPos );
  90. targetVec.applyQuaternion( invLinkQ );
  91. targetVec.normalize();
  92. var angle = targetVec.dot( effectorVec );
  93. if ( angle > 1.0 ) {
  94. angle = 1.0;
  95. } else if ( angle < -1.0 ) {
  96. angle = -1.0;
  97. }
  98. angle = math.acos( angle );
  99. // skip if changing angle is too small to prevent vibration of bone
  100. // Refer to http://www20.atpages.jp/katwat/three.js_r58/examples/mytest37/mmd.three.js
  101. if ( angle < 1e-5 ) continue;
  102. if ( ik.minAngle !== undefined && angle < ik.minAngle ) {
  103. angle = ik.minAngle;
  104. }
  105. if ( ik.maxAngle !== undefined && angle > ik.maxAngle ) {
  106. angle = ik.maxAngle;
  107. }
  108. axis.crossVectors( effectorVec, targetVec );
  109. axis.normalize();
  110. q.setFromAxisAngle( axis, angle );
  111. link.quaternion.multiply( q );
  112. // TODO: re-consider the limitation specification
  113. if ( limitation !== undefined ) {
  114. var c = link.quaternion.w;
  115. if ( c > 1.0 ) {
  116. c = 1.0;
  117. }
  118. var c2 = math.sqrt( 1 - c * c );
  119. link.quaternion.set( limitation.x * c2,
  120. limitation.y * c2,
  121. limitation.z * c2,
  122. c );
  123. }
  124. link.updateMatrixWorld( true );
  125. rotated = true;
  126. }
  127. if ( ! rotated ) break;
  128. }
  129. }
  130. // just in case
  131. this.mesh.updateMatrixWorld( true );
  132. }
  133. };
  134. THREE.CCDIKHelper = function ( mesh ) {
  135. if ( mesh.geometry.iks === undefined || mesh.skeleton === undefined ) {
  136. throw 'THREE.CCDIKHelper requires iks in mesh.geometry and skeleton in mesh.';
  137. }
  138. THREE.Object3D.call( this );
  139. this.root = mesh;
  140. this.matrix = mesh.matrixWorld;
  141. this.matrixAutoUpdate = false;
  142. this.sphereGeometry = new THREE.SphereBufferGeometry( 0.25, 16, 8 );
  143. this.targetSphereMaterial = new THREE.MeshBasicMaterial( {
  144. color: new THREE.Color( 0xff8888 ),
  145. depthTest: false,
  146. depthWrite: false,
  147. transparent: true
  148. } );
  149. this.effectorSphereMaterial = new THREE.MeshBasicMaterial( {
  150. color: new THREE.Color( 0x88ff88 ),
  151. depthTest: false,
  152. depthWrite: false,
  153. transparent: true
  154. } );
  155. this.linkSphereMaterial = new THREE.MeshBasicMaterial( {
  156. color: new THREE.Color( 0x8888ff ),
  157. depthTest: false,
  158. depthWrite: false,
  159. transparent: true
  160. } );
  161. this.lineMaterial = new THREE.LineBasicMaterial( {
  162. color: new THREE.Color( 0xff0000 ),
  163. depthTest: false,
  164. depthWrite: false,
  165. transparent: true
  166. } );
  167. this._init();
  168. this.update();
  169. };
  170. THREE.CCDIKHelper.prototype = Object.create( THREE.Object3D.prototype );
  171. THREE.CCDIKHelper.prototype.constructor = THREE.CCDIKHelper;
  172. THREE.CCDIKHelper.prototype._init = function () {
  173. var self = this;
  174. var mesh = this.root;
  175. var iks = mesh.geometry.iks;
  176. function createLineGeometry( ik ) {
  177. var geometry = new THREE.BufferGeometry();
  178. var vertices = new Float32Array( ( 2 + ik.links.length ) * 3 );
  179. geometry.addAttribute( 'position', new THREE.BufferAttribute( vertices, 3 ) );
  180. return geometry;
  181. }
  182. function createTargetMesh() {
  183. return new THREE.Mesh( self.sphereGeometry, self.targetSphereMaterial );
  184. }
  185. function createEffectorMesh() {
  186. return new THREE.Mesh( self.sphereGeometry, self.effectorSphereMaterial );
  187. }
  188. function createLinkMesh() {
  189. return new THREE.Mesh( self.sphereGeometry, self.linkSphereMaterial );
  190. }
  191. function createLine( ik ) {
  192. return new THREE.Line( createLineGeometry( ik ), self.lineMaterial );
  193. }
  194. for ( var i = 0, il = iks.length; i < il; i ++ ) {
  195. var ik = iks[ i ];
  196. this.add( createTargetMesh() );
  197. this.add( createEffectorMesh() );
  198. for ( var j = 0, jl = ik.links.length; j < jl; j ++ ) {
  199. this.add( createLinkMesh() );
  200. }
  201. this.add( createLine( ik ) );
  202. }
  203. };
  204. THREE.CCDIKHelper.prototype.update = function () {
  205. var offset = 0;
  206. var mesh = this.root;
  207. var iks = mesh.geometry.iks;
  208. var bones = mesh.skeleton.bones;
  209. var matrixWorldInv = new THREE.Matrix4().getInverse( mesh.matrixWorld );
  210. var vector = new THREE.Vector3();
  211. function getPosition( bone ) {
  212. vector.setFromMatrixPosition( bone.matrixWorld );
  213. vector.applyMatrix4( matrixWorldInv );
  214. return vector;
  215. }
  216. function setPositionOfBoneToAttributeArray( array, index, bone ) {
  217. var v = getPosition( bone );
  218. array[ index * 3 + 0 ] = v.x;
  219. array[ index * 3 + 1 ] = v.y;
  220. array[ index * 3 + 2 ] = v.z;
  221. }
  222. for ( var i = 0, il = iks.length; i < il; i ++ ) {
  223. var ik = iks[ i ];
  224. var targetBone = bones[ ik.target ];
  225. var effectorBone = bones[ ik.effector ];
  226. var targetMesh = this.children[ offset ++ ];
  227. var effectorMesh = this.children[ offset ++ ];
  228. targetMesh.position.copy( getPosition( targetBone ) );
  229. effectorMesh.position.copy( getPosition( effectorBone ) );
  230. for ( var j = 0, jl = ik.links.length; j < jl; j ++ ) {
  231. var link = ik.links[ j ];
  232. var linkBone = bones[ link.index ];
  233. var linkMesh = this.children[ offset ++ ];
  234. linkMesh.position.copy( getPosition( linkBone ) );
  235. }
  236. var line = this.children[ offset ++ ];
  237. var array = line.geometry.attributes.position.array;
  238. setPositionOfBoneToAttributeArray( array, 0, targetBone );
  239. setPositionOfBoneToAttributeArray( array, 1, effectorBone );
  240. for ( var j = 0, jl = ik.links.length; j < jl; j ++ ) {
  241. var link = ik.links[ j ];
  242. var linkBone = bones[ link.index ];
  243. setPositionOfBoneToAttributeArray( array, j + 2, linkBone );
  244. }
  245. line.geometry.attributes.position.needsUpdate = true;
  246. }
  247. };