From e4256189ed2597a5e92de7953e8dbb38d245dbec Mon Sep 17 00:00:00 2001 From: Pedro Sousa Date: Fri, 31 Dec 2021 12:14:52 -0500 Subject: [PATCH] v0.0.2 - IK Rigs & Animations --- README.md | 32 +- examples/threejs/005_ikrig.html | 84 +++++ examples/threejs/006_ik_retarget.html | 371 ++++++++++++++++++++++ examples/threejs/_lib/DynLineMesh.js | 180 +++++++++++ examples/threejs/_lib/ShapePointsMesh.js | 278 ++++++++++++++++ examples/threejs/_lib/Util.js | 4 +- examples/threejs/_lib/UtilArm.js | 56 ++++ package.json | 2 +- src/armature/Pose.ts | 72 ++++- src/ikrig/IKData.ts | 27 ++ src/ikrig/animation/BipedIKPose.ts | 66 ++++ src/ikrig/index.ts | 25 ++ src/ikrig/rigs/BipedRig.ts | 175 ++++++++++ src/ikrig/rigs/IKChain.ts | 227 +++++++++++++ src/ikrig/rigs/IKRig.ts | 44 +++ src/ikrig/solvers/HipSolver.ts | 111 +++++++ src/ikrig/solvers/ISolver.ts | 6 + src/ikrig/solvers/LimbSolver.ts | 101 ++++++ src/ikrig/solvers/SwingTwistEndsSolver.ts | 175 ++++++++++ src/ikrig/solvers/SwingTwistSolver.ts | 160 ++++++++++ src/ikrig/solvers/index.ts | 15 + src/maths/QuatUtil.ts | 23 +- src/maths/Vec3Util.ts | 5 +- src/ossos.ts | 10 +- 24 files changed, 2221 insertions(+), 28 deletions(-) create mode 100644 examples/threejs/005_ikrig.html create mode 100644 examples/threejs/006_ik_retarget.html create mode 100644 examples/threejs/_lib/DynLineMesh.js create mode 100644 examples/threejs/_lib/ShapePointsMesh.js create mode 100644 examples/threejs/_lib/UtilArm.js create mode 100644 src/ikrig/IKData.ts create mode 100644 src/ikrig/animation/BipedIKPose.ts create mode 100644 src/ikrig/index.ts create mode 100644 src/ikrig/rigs/BipedRig.ts create mode 100644 src/ikrig/rigs/IKChain.ts create mode 100644 src/ikrig/rigs/IKRig.ts create mode 100644 src/ikrig/solvers/HipSolver.ts create mode 100644 src/ikrig/solvers/ISolver.ts create mode 100644 src/ikrig/solvers/LimbSolver.ts create mode 100644 src/ikrig/solvers/SwingTwistEndsSolver.ts create mode 100644 src/ikrig/solvers/SwingTwistSolver.ts create mode 100644 src/ikrig/solvers/index.ts diff --git a/README.md b/README.md index 91bcfc4..be54e60 100644 --- a/README.md +++ b/README.md @@ -8,9 +8,7 @@ ### Character Animation Library ### -This project focus is to allow animating 3D character in web application, games, metaverses, etc that are rendered in WebGL / WebGPU. Some of the big goals is to create something that is independent from any rendering engine while trying to be more collaborative in its design the betterment of the web. The biggest inspiration for me and one of the main goals is to achieve something similar to Ubisoft's GDC Talk & Demo of their IK Rigs animation system. - -I like to look at this as being the IMGUI of Character Animations :) +This project is working toward a complete character skinning & animation library for the web. First most, this library is focused on being independent from any rendering engine with examples of how to use it in webgl based engines like threejs. The one big focus is recreating the IK Rig & IK Animations type system that was demoed several years ago from Ubisoft's GDC talk on IK Rigs. With many game engines like Unity and Unreal developing their own IK Rig like systems, this project helps fill the void for web based engines like threejs, babylon, etc. Hopefully with enough help we can create something just as good as the big boys, maybe even better since its free & open source. ### Setup ### @@ -53,6 +51,9 @@ App.add( mesh ); * Bone Springs ( Rotation & Translation ) * Basic Animator based on Tracks * Basic Animation Retargeting for similar skeletal meshes +* IK Rigs - Basic Biped +* IK Animation Retargeting using IK Rigs +* IK Solvers - Aim/SwingTwist, SwingTwistEnds, Limb * GLTF2 Asset Parsing for cherry picking what you need to load. * Several examples using ThreeJS for rendering * Some extra fun examples like converting animations to Data Textures @@ -61,13 +62,16 @@ App.add( mesh ); --- ## Future Plans ## -- [ ] Rebuilding IK Rigs as a new version for this project -- [ ] Port over my Single Pass IK Solvers ( Aim, Limb, Z, Piston, Arc, ArcSin, Trapezoid, Spring ) -- [ ] Rebuild IK Animation Retargeting for this project -- [ ] Complete FullBody IK Prototype +- [x] Rewrite IK Rigs +- [x] Port over starting IK Solvers ( Aim / SwingTwist, Limb, SwingTwistEnds ) +- [x] Rewrite IK Animation Retargeting +- [ ] Port over extra single pass IK Solvers ( Z, Piston, Arc, ArcSin, Trapezoid, Spring ) +- [ ] Create an implementation of FABIK +- [ ] Create solver based on Catenary Curve - [ ] Port over Trianglution Solver ( Alternative to CCD ) - [ ] Port over Natural CCD ( Extended version of CCD ) -- [ ] Create an implementation of FABIK +- [x] Complete FullBody IK Prototype +- [ ] Revisit FullBody IK, Make it mores stable & user friendly - [ ] Figure out how to implement VRIK - [ ] Bone Slots / Attachments - [ ] Actions or State Machine based Animator @@ -94,15 +98,3 @@ There are some things I've been wanting for my prototyping for awhile. Here's a * Something that looks nice & blends well together, doesn't look choppy * Walk, Run, Idle, Crawl, Jump. Maybe Flying & Swimming * Prefer not to be baked - -* `IK Bot` - * **Purpose** : Something to use for procedural generation & animation prototyping. Having the arm/leg made as pieces can allow me to create chains of various sizes procedurally to play with various IK solvers. - * **Inspiration** : https://twitter.com/WokkieG/status/1429130029422743561?s=20 - * **Thoughs** : - * Round Robot so its easy to just place limbs anywhere and really play with things - * Instead of arms, create two types of "Bones" that can be connected in chains. One with a ball joint, the other with a hinge joint. This can help with demoing future constraints prototypes - * Ball joint base for connecting chain to the round body - * Some sort of Hand or Feet part to attach at the end of the chain - * Does not need to be skinned or textured - * Some hard edges in the design would be cool - diff --git a/examples/threejs/005_ikrig.html b/examples/threejs/005_ikrig.html new file mode 100644 index 0000000..b0269fb --- /dev/null +++ b/examples/threejs/005_ikrig.html @@ -0,0 +1,84 @@ + + + + + \ No newline at end of file diff --git a/examples/threejs/006_ik_retarget.html b/examples/threejs/006_ik_retarget.html new file mode 100644 index 0000000..c06ba83 --- /dev/null +++ b/examples/threejs/006_ik_retarget.html @@ -0,0 +1,371 @@ + + + + + \ No newline at end of file diff --git a/examples/threejs/_lib/DynLineMesh.js b/examples/threejs/_lib/DynLineMesh.js new file mode 100644 index 0000000..235931a --- /dev/null +++ b/examples/threejs/_lib/DynLineMesh.js @@ -0,0 +1,180 @@ +import * as THREE from 'three'; + +class DynLineMesh extends THREE.LineSegments{ + _defaultColor = 0x00ff00; + _cnt = 0; + _verts = []; + _color = []; + _config = []; + _dirty = false; + + constructor( initSize = 20 ){ + super( + _newDynLineMeshGeometry( + new Float32Array( initSize * 2 * 3 ), // Two Points for Each Line + new Float32Array( initSize * 2 * 3 ), + new Float32Array( initSize * 2 * 1 ), + false + ), + newDynLineMeshMaterial() //new THREE.PointsMaterial( { color: 0xffffff, size:8, sizeAttenuation:false } ) + ); + + this.geometry.setDrawRange( 0, 0 ); + this.onBeforeRender = ()=>{ if( this._dirty ) this._updateGeometry(); } + } + + reset(){ + this._cnt = 0; + this._verts.length = 0; + this._color.length = 0; + this._config.length = 0; + this.geometry.setDrawRange( 0, 0 ); + return this; + } + + add( p0, p1, color0=this._defaultColor, color1=null, isDash=false ){ + this._verts.push( p0[0], p0[1], p0[2], p1[0], p1[1], p1[2] ); + this._color.push( ...glColor( color0 ), ...glColor( (color1 != null) ? color1:color0 ) ); + + if( isDash ){ + const len = Math.sqrt( + (p1[0] - p0[0]) ** 2 + + (p1[1] - p0[1]) ** 2 + + (p1[2] - p0[2]) ** 2 + ); + this._config.push( 0, len ); + }else{ + this._config.push( 0, 0 ); + } + + this._cnt++; + this._dirty = true; + return this; + } + + _updateGeometry(){ + const geo = this.geometry; + const bVerts = geo.attributes.position; + const bColor = geo.attributes.color; //this.geometry.index; + const bConfig = geo.attributes.config; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + if( this._verts.length > bVerts.array.length || + this._color.length > bColor.array.length || + this._config.length > bConfig.array.length + ){ + if( this.geometry ) this.geometry.dispose(); + this.geometry = _newDynLineMeshGeometry( this._verts, this._color, this._config ); + this._dirty = false; + return; + } + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + bVerts.array.set( this._verts ); + bVerts.count = this._verts.length / 3; + bVerts.needsUpdate = true; + + bColor.array.set( this._color ); + bColor.count = this._color.length / 3; + bColor.needsUpdate = true; + + bConfig.array.set( this._config ); + bConfig.count = this._config.length / 1; + bConfig.needsUpdate = true; + + geo.setDrawRange( 0, bVerts.count ); + geo.computeBoundingBox(); + geo.computeBoundingSphere(); + + this._dirty = false; + } +} + +//#region SUPPORT +function _newDynLineMeshGeometry( aVerts, aColor, aConfig, doCompute=true ){ + //if( !( aVerts instanceof Float32Array) ) aVerts = new Float32Array( aVerts ); + //if( !( aColor instanceof Float32Array) ) aColor = new Float32Array( aColor ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const bVerts = new THREE.Float32BufferAttribute( aVerts, 3 ); + const bColor = new THREE.Float32BufferAttribute( aColor, 3 ); + const bConfig = new THREE.Float32BufferAttribute( aConfig, 1 ); + bVerts.setUsage( THREE.DynamicDrawUsage ); + bColor.setUsage( THREE.DynamicDrawUsage ); + bConfig.setUsage( THREE.DynamicDrawUsage ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const geo = new THREE.BufferGeometry(); + geo.setAttribute( 'position', bVerts ); + geo.setAttribute( 'color', bColor ); + geo.setAttribute( 'config', bConfig ); + + if( doCompute ){ + geo.computeBoundingSphere(); + geo.computeBoundingBox(); + } + return geo; +} + +function glColor( hex, out = null ){ + const NORMALIZE_RGB = 1 / 255; + out = out || [0,0,0]; + + out[0] = ( hex >> 16 & 255 ) * NORMALIZE_RGB; + out[1] = ( hex >> 8 & 255 ) * NORMALIZE_RGB; + out[2] = ( hex & 255 ) * NORMALIZE_RGB; + + return out; +} +//#endregion + +//#region SHADER + +function newDynLineMeshMaterial(){ + return new THREE.RawShaderMaterial({ + depthTest : false, + transparent : true, + uniforms : { + dashSeg : { value : 1 / 0.07 }, + dashDiv : { value : 0.4 }, + }, + vertexShader : `#version 300 es + in vec3 position; + in vec3 color; + in float config; + + uniform mat4 modelViewMatrix; + uniform mat4 projectionMatrix; + uniform float u_scale; + + out vec3 fragColor; + out float fragLen; + + void main(){ + vec4 wPos = modelViewMatrix * vec4( position, 1.0 ); + + fragColor = color; + fragLen = config; + + gl_Position = projectionMatrix * wPos; + }`, + fragmentShader : `#version 300 es + precision mediump float; + + uniform float dashSeg; + uniform float dashDiv; + + in vec3 fragColor; + in float fragLen; + out vec4 outColor; + + void main(){ + float alpha = 1.0; + if( fragLen > 0.0 ) alpha = step( dashDiv, fract( fragLen * dashSeg ) ); + outColor = vec4( fragColor, alpha ); + }`}); +} + +//#endregion + +export default DynLineMesh; \ No newline at end of file diff --git a/examples/threejs/_lib/ShapePointsMesh.js b/examples/threejs/_lib/ShapePointsMesh.js new file mode 100644 index 0000000..8842a53 --- /dev/null +++ b/examples/threejs/_lib/ShapePointsMesh.js @@ -0,0 +1,278 @@ +import * as THREE from 'three'; + +class ShapePointsMesh extends THREE.Points{ + _defaultShape = 1; + _defaultSize = 6; + _defaultColor = 0x00ff00; + _cnt = 0; + _verts = []; + _color = []; + _config = []; + _dirty = false; + + constructor( initSize = 20 ){ + super( + _newShapePointsMeshGeometry( + new Float32Array( initSize * 3 ), + new Float32Array( initSize * 3 ), + new Float32Array( initSize * 2 ), + false + ), + newShapePointsMeshMaterial() //new THREE.PointsMaterial( { color: 0xffffff, size:8, sizeAttenuation:false } ) + ); + + this.geometry.setDrawRange( 0, 0 ); + this.onBeforeRender = ()=>{ if( this._dirty ) this._updateGeometry(); } + } + + reset(){ + this._cnt = 0; + this._verts.length = 0; + this._color.length = 0; + this._config.length = 0; + this.geometry.setDrawRange( 0, 0 ); + return this; + } + + add( pos, color = this._defaultColor, size = this._defaultSize, shape = this._defaultShape ){ + this._verts.push( pos[0], pos[1], pos[2] ); + this._color.push( ...glColor( color ) ); + this._config.push( size, shape ); + this._cnt++; + this._dirty = true; + return this; + } + + setColorAt( idx, color ){ + const c = glColor( color ); + idx *= 3; + + this._color[ idx ] = c[ 0 ]; + this._color[ idx + 1 ] = c[ 1 ]; + this._color[ idx + 2 ] = c[ 2 ]; + this._dirty = true; + return this; + } + + setPosAt( idx, pos ){ + idx *= 3; + this._verts[ idx ] = pos[ 0 ]; + this._verts[ idx + 1 ] = pos[ 1 ]; + this._verts[ idx + 2 ] = pos[ 2 ]; + this._dirty = true; + return this; + } + + getPosAt( idx ){ + idx *= 3; + return [ + this._verts[ idx + 0 ], + this._verts[ idx + 1 ], + this._verts[ idx + 2 ], + ]; + } + + _updateGeometry(){ + const geo = this.geometry; + const bVerts = geo.attributes.position; + const bColor = geo.attributes.color; //this.geometry.index; + const bConfig = geo.attributes.config; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + if( this._verts.length > bVerts.array.length || + this._color.length > bColor.array.length || + this._config.length > bConfig.array.length + ){ + if( this.geometry ) this.geometry.dispose(); + this.geometry = _newShapePointsMeshGeometry( this._verts, this._color, this._config ); + this._dirty = false; + return; + } + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + bVerts.array.set( this._verts ); + bVerts.count = this._verts.length / 3; + bVerts.needsUpdate = true; + + bColor.array.set( this._color ); + bColor.count = this._color.length / 3; + bColor.needsUpdate = true; + + bConfig.array.set( this._config ); + bConfig.count = this._config.length / 2; + bConfig.needsUpdate = true; + + geo.setDrawRange( 0, bVerts.count ); + geo.computeBoundingBox(); + geo.computeBoundingSphere(); + + this._dirty = false; + } +} + +//#region SUPPORT +function _newShapePointsMeshGeometry( aVerts, aColor, aConfig, doCompute=true ){ + //if( !( aVerts instanceof Float32Array) ) aVerts = new Float32Array( aVerts ); + //if( !( aColor instanceof Float32Array) ) aColor = new Float32Array( aColor ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const bVerts = new THREE.Float32BufferAttribute( aVerts, 3 ); + const bColor = new THREE.Float32BufferAttribute( aColor, 3 ); + const bConfig = new THREE.Float32BufferAttribute( aConfig, 2 ); + bVerts.setUsage( THREE.DynamicDrawUsage ); + bColor.setUsage( THREE.DynamicDrawUsage ); + bConfig.setUsage( THREE.DynamicDrawUsage ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const geo = new THREE.BufferGeometry(); + geo.setAttribute( 'position', bVerts ); + geo.setAttribute( 'color', bColor ); + geo.setAttribute( 'config', bConfig ); + + if( doCompute ){ + geo.computeBoundingSphere(); + geo.computeBoundingBox(); + } + return geo; +} + +function glColor( hex, out = null ){ + const NORMALIZE_RGB = 1 / 255; + out = out || [0,0,0]; + + out[0] = ( hex >> 16 & 255 ) * NORMALIZE_RGB; + out[1] = ( hex >> 8 & 255 ) * NORMALIZE_RGB; + out[2] = ( hex & 255 ) * NORMALIZE_RGB; + + return out; +} +//#endregion + +//#region SHADER + +function newShapePointsMeshMaterial(){ + + return new THREE.RawShaderMaterial({ + depthTest : false, + transparent : true, + uniforms : { u_scale:{ value : 20.0 } }, + vertexShader : `#version 300 es + in vec3 position; + in vec3 color; + in vec2 config; + + uniform mat4 modelViewMatrix; + uniform mat4 projectionMatrix; + uniform float u_scale; + + out vec3 fragColor; + flat out int fragShape; + + void main(){ + vec4 wPos = modelViewMatrix * vec4( position.xyz, 1.0 ); + + fragColor = color; + fragShape = int( config.y ); + + gl_Position = projectionMatrix * wPos; + gl_PointSize = config.x * ( u_scale / -wPos.z ); + + // Get pnt to be World Space Size + //gl_PointSize = view_port_size.y * projectionMatrix[1][5] * 1.0 / gl_Position.w; + //gl_PointSize = view_port_size.y * projectionMatrix[1][1] * 1.0 / gl_Position.w; + }`, + fragmentShader : `#version 300 es + precision mediump float; + + #define PI 3.14159265359 + #define PI2 6.28318530718 + + in vec3 fragColor; + flat in int fragShape; + out vec4 outColor; + + float circle(){ + vec2 coord = gl_PointCoord * 2.0 - 1.0; // v_uv * 2.0 - 1.0; + float radius = dot( coord, coord ); + float dxdy = fwidth( radius ); + return smoothstep( 0.90 + dxdy, 0.90 - dxdy, radius ); + } + + float ring( float inner ){ + vec2 coord = gl_PointCoord * 2.0 - 1.0; + float radius = dot( coord, coord ); + float dxdy = fwidth( radius ); + return smoothstep( inner - dxdy, inner + dxdy, radius ) - + smoothstep( 1.0 - dxdy, 1.0 + dxdy, radius ); + } + + float diamond(){ + // http://www.numb3r23.net/2015/08/17/using-fwidth-for-distance-based-anti-aliasing/ + const float radius = 0.5; + + float dst = dot( abs(gl_PointCoord-vec2(0.5)), vec2(1.0) ); + float aaf = fwidth( dst ); + return 1.0 - smoothstep( radius - aaf, radius, dst ); + } + + float poly( int sides, float offset, float scale ){ + // https://thebookofshaders.com/07/ + vec2 coord = gl_PointCoord * 2.0 - 1.0; + + coord.y += offset; + coord *= scale; + + float a = atan( coord.x, coord.y ) + PI; // Angle of Pixel + float r = PI2 / float( sides ); // Radius of Pixel + float d = cos( floor( 0.5 + a / r ) * r-a ) * length( coord ); + float f = fwidth( d ); + return smoothstep( 0.5, 0.5 - f, d ); + } + + // signed distance to a n-star polygon with external angle en + float sdStar( float r, int n, float m ){ // m=[2,n] + vec2 p = vec2( gl_PointCoord.x, 1.0 - gl_PointCoord.y ) * 2.0 - 1.0; + + // these 4 lines can be precomputed for a given shape + float an = 3.141593/float(n); + float en = 3.141593/m; + vec2 acs = vec2(cos(an),sin(an)); + vec2 ecs = vec2(cos(en),sin(en)); // ecs=vec2(0,1) and simplify, for regular polygon, + + // reduce to first sector + float bn = mod(atan(p.x,p.y),2.0*an) - an; + p = length(p)*vec2(cos(bn),abs(sin(bn))); + + // line sdf + p -= r*acs; + p += ecs*clamp( -dot(p,ecs), 0.0, r*acs.y/ecs.y); + + float dist = length(p)*sign(p.x); + float f = fwidth( dist ); + + return smoothstep( 0.0, 0.0 - f, dist ); + } + + + void main(){ + float alpha = 1.0; + + if( fragShape == 1 ) alpha = circle(); + if( fragShape == 2 ) alpha = diamond(); + if( fragShape == 3 ) alpha = poly( 3, 0.2, 1.0 ); // Triangle + if( fragShape == 4 ) alpha = poly( 5, 0.0, 0.65 ); // Pentagram + if( fragShape == 5 ) alpha = poly( 6, 0.0, 0.65 ); // Hexagon + if( fragShape == 6 ) alpha = ring( 0.2 ); + if( fragShape == 7 ) alpha = ring( 0.7 ); + if( fragShape == 8 ) alpha = sdStar( 1.0, 3, 2.3 ); + if( fragShape == 9 ) alpha = sdStar( 1.0, 6, 2.5 ); + if( fragShape == 10 ) alpha = sdStar( 1.0, 4, 2.4 ); + if( fragShape == 11 ) alpha = sdStar( 1.0, 5, 2.8 ); + + outColor = vec4( fragColor, alpha ); + }`}); +} + +//#endregion + +export default ShapePointsMesh; \ No newline at end of file diff --git a/examples/threejs/_lib/Util.js b/examples/threejs/_lib/Util.js index 73d6902..7087a98 100644 --- a/examples/threejs/_lib/Util.js +++ b/examples/threejs/_lib/Util.js @@ -1,12 +1,12 @@ import * as THREE from 'three'; class Util{ - static loadTexture( url ){ + static loadTexture( url, flipY=false ){ return new Promise( (resolve, reject) => { const loader = new THREE.TextureLoader() .load( url, - tex => resolve( tex ), + tex =>{ tex.flipY = flipY; resolve( tex ); }, undefined, err => reject( err ) ); diff --git a/examples/threejs/_lib/UtilArm.js b/examples/threejs/_lib/UtilArm.js new file mode 100644 index 0000000..2d4e4df --- /dev/null +++ b/examples/threejs/_lib/UtilArm.js @@ -0,0 +1,56 @@ +import { Armature, SkinMTX } from '../../../src/armature/index'; +import Clip from '../../../src/animation/Clip'; + +import BoneViewMesh from './BoneViewMesh.js'; +import SkinMTXMaterial from './SkinMTXMaterial.js'; +import { UtilGltf2 } from './UtilGltf2.js'; + + + +class UtilArm{ + + static newBoneView( arm, pose=null, meshScl, dirScl ){ + const boneView = new BoneViewMesh( arm ); + + // Because of the transform on the Armature itself, need to scale up the bones + // to offset the massive scale down of the model + if( meshScl ) boneView.material.uniforms.meshScl.value = meshScl; + if( dirScl ) boneView.material.uniforms.dirScl.value = dirScl; + + // Set Initial Data So it Renders + boneView.updateFromPose( pose || arm ); // arm.newPose().updateWorld( true ) + + return boneView; + } + + static skinMtxMesh( gltf, arm, base='cyan' ){ + const mat = SkinMTXMaterial( base, arm.getSkinOffsets()[0] ); // 3JS Example of Matrix Skinning GLSL Code + return UtilGltf2.loadMesh( gltf, null, mat ); // Pull Skinned Mesh from GLTF + } + + static clipFromGltf( gltf ){ return Clip.fromGLTF2( gltf.getAnimation() ); } + + static armFromGltf( gltf, defaultBoneLen = 0.07 ){ + const skin = gltf.getSkin(); + const arm = new Armature(); + + // Create Armature + for( let j of skin.joints ){ + arm.addBone( j.name, j.parentIndex, j.rotation, j.position, j.scale ); + } + + // Bind + arm.bind( SkinMTX, 0.07 ); + + // Save Offsets if available + arm.offset.set( skin.rotation, skin.position, skin.scale ); + //if( skin.rotation ) arm.offset.rot.copy( skin.rotation ); + //if( skin.position ) arm.offset.pos.copy( skin.position ); + //if( skin.scale ) arm.offset.scl.copy( skin.scale ); + + return arm; + } + +} + +export default UtilArm; \ No newline at end of file diff --git a/package.json b/package.json index 44bfe74..0e233a3 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,6 @@ { "name" : "ossos", - "version" : "0.0.1", + "version" : "0.0.2", "author" : "Pedro Sousa ( Vor @ SketchPunk Labs )", "description" : "Character Animation System", "keywords" : [ "animation", "skeleton", "inverse kinematrics", "armature", "ikrig" ], diff --git a/src/armature/Pose.ts b/src/armature/Pose.ts index 8c2162e..dbfd603 100644 --- a/src/armature/Pose.ts +++ b/src/armature/Pose.ts @@ -4,12 +4,13 @@ import type Armature from './Armature.js'; import type Bone from './Bone.js'; import { vec3, quat } from 'gl-matrix'; import Transform from '../maths/Transform'; +import Vec3Util from '../maths/Vec3Util'; //#endregion class Pose{ //#region MAIN arm !: Armature; - bones !: Bone[]; // Clone of Armature Bones + bones !: Bone[]; // Clone of Armature Bones offset = new Transform(); // Pose Transform Offset, useful to apply parent mesh transform constructor( arm ?: Armature ){ @@ -145,7 +146,7 @@ class Pose{ //#region COMPUTE - updateWorld( useOffset=false ): this{ + updateWorld( useOffset=true ): this{ let i, b; for( i=0; i < this.bones.length; i++ ){ b = this.bones[ i ]; @@ -157,6 +158,73 @@ class Pose{ return this; } + + getWorldTransform( bIdx: number, out ?: Transform ): Transform{ + out ??= new Transform(); + + let bone = this.bones[ bIdx ]; // get Initial Bone + out.copy( bone.local ); // Starting Transform + + // Loop up the heirarchy till we hit the root bone + while( bone.pidx != null ){ + bone = this.bones[ bone.pidx ]; + out.pmul( bone.local ); + } + + // Add offset at the end + out.pmul( this.offset ); + return out; + } + + getWorldRotation( bIdx: number, out ?: quat ): quat{ + out ??= quat.create(); + + let bone = this.bones[ bIdx ]; // get Initial Bone + //out.copy( bone.local.rot ); // Starting Rotation + quat.copy( out, bone.local.rot ); // Starting Rotation + + // Loop up the heirarchy till we hit the root bone + while( bone.pidx != null ){ + bone = this.bones[ bone.pidx ]; + //out.pmul( bone.local.rot ); + quat.mul( out, bone.local.rot, out ); + } + + // Add offset at the end + //out.pmul( this.offset.rot ); + quat.mul( out, this.offset.rot, out ); + return out; + } + + updateBoneLengths( defaultBoneLen=0 ): this{ + const bCnt = this.bones.length; + let b: Bone, p: Bone; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Compute Bone Length from Children to Parent Bones + // Leaf bones don't have children, so no way to determine this length + for( let i=bCnt-1; i >= 0; i-- ){ + //------------------------------- + b = this.bones[ i ]; + if( b.pidx == null ) continue; // No Parent to compute its length. + + //------------------------------- + // Parent Bone, Compute its length based on its position and the current bone. + p = this.bones[ b.pidx ]; + p.len = Vec3Util.len( p.world.pos, b.world.pos ); + } + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Set a default size for Leaf bones + if( defaultBoneLen != 0 ){ + for( let i=0; i < bCnt; i++ ){ + b = this.bones[ i ]; + if( b.len == 0 ) b.len = defaultBoneLen; + } + } + + return this; + } //#endregion } diff --git a/src/ikrig/IKData.ts b/src/ikrig/IKData.ts new file mode 100644 index 0000000..ecc911c --- /dev/null +++ b/src/ikrig/IKData.ts @@ -0,0 +1,27 @@ +import type { vec3 } from "gl-matrix"; + +export class DirScale{ + lenScale : number = 1; + effectorDir : vec3 = [0,0,0]; + poleDir : vec3 = [0,0,0]; +} + +export class Dir{ + effectorDir : vec3 = [0,0,0]; + poleDir : vec3 = [0,0,0]; +} + +export class DirEnds{ + startEffectorDir : vec3 = [0,0,0]; + startPoleDir : vec3 = [0,0,0]; + endEffectorDir : vec3 = [0,0,0]; + endPoleDir : vec3 = [0,0,0]; +} + +export class Hip{ + effectorDir : vec3 = [0,0,0]; + poleDir : vec3 = [0,0,0]; + pos : vec3 = [0,0,0]; + bindHeight : number = 1; + isAbsolute : boolean = false; +} diff --git a/src/ikrig/animation/BipedIKPose.ts b/src/ikrig/animation/BipedIKPose.ts new file mode 100644 index 0000000..443a9cc --- /dev/null +++ b/src/ikrig/animation/BipedIKPose.ts @@ -0,0 +1,66 @@ +//#region IMPORT +import type BipedRig from '../rigs/BipedRig'; +import type { Pose } from '../../armature/index' + +import * as IKData from '../IKData'; +//#endregion + +class BipedIKPose{ + //#region MAIN + hip = new IKData.Hip(); + spine = new IKData.DirEnds(); + head = new IKData.Dir(); + + armL = new IKData.DirScale(); + armR = new IKData.DirScale(); + legL = new IKData.DirScale(); + legR = new IKData.DirScale(); + + handL = new IKData.Dir(); + handR = new IKData.Dir(); + footL = new IKData.Dir(); + footR = new IKData.Dir(); + + constructor(){} + //#endregion + + /** Compute the IK Data from a Rig and Pose */ + computeFromRigPose( r: BipedRig, pose: Pose ): void{ + r.legL?.solver.ikDataFromPose( r.legL, pose, this.legL ); + r.legR?.solver.ikDataFromPose( r.legR, pose, this.legR ); + r.armR?.solver.ikDataFromPose( r.armR, pose, this.armR ); + r.armL?.solver.ikDataFromPose( r.armL, pose, this.armL ); + + r.footL?.solver.ikDataFromPose( r.footL, pose, this.footL ); + r.footR?.solver.ikDataFromPose( r.footR, pose, this.footR ); + r.handR?.solver.ikDataFromPose( r.handR, pose, this.handR ); + r.handR?.solver.ikDataFromPose( r.handL, pose, this.handL ); + + r.head?.solver.ikDataFromPose( r.head, pose, this.head ); + r.spine?.solver.ikDataFromPose( r.spine, pose, this.spine ); + r.hip?.solver.ikDataFromPose( r.hip, pose, this.hip ); + } + + applyToRig( r: BipedRig ): void{ + r.legL?.solver.setTargetDir( this.legL.effectorDir, this.legL.poleDir, this.legL.lenScale ); + r.legR?.solver.setTargetDir( this.legR.effectorDir, this.legR.poleDir, this.legR.lenScale ); + r.armL?.solver.setTargetDir( this.armL.effectorDir, this.armL.poleDir, this.armL.lenScale ); + r.armR?.solver.setTargetDir( this.armR.effectorDir, this.armR.poleDir, this.armR.lenScale ); + + r.footL?.solver.setTargetDir( this.footL.effectorDir, this.footL.poleDir ); + r.footR?.solver.setTargetDir( this.footR.effectorDir, this.footR.poleDir ); + r.handL?.solver.setTargetDir( this.handL.effectorDir, this.handL.poleDir ); + r.handR?.solver.setTargetDir( this.handR.effectorDir, this.handR.poleDir ); + r.head?.solver.setTargetDir( this.head.effectorDir, this.head.poleDir ); + + r.hip?.solver + .setTargetDir( this.hip.effectorDir, this.hip.poleDir ) + .setMovePos( this.hip.pos, this.hip.isAbsolute, this.hip.bindHeight ); + + r.spine?.solver + .setStartDir( this.spine.startEffectorDir, this.spine.startPoleDir ) + .setEndDir( this.spine.endEffectorDir, this.spine.endPoleDir ); + } +} + +export default BipedIKPose; \ No newline at end of file diff --git a/src/ikrig/index.ts b/src/ikrig/index.ts new file mode 100644 index 0000000..dffc086 --- /dev/null +++ b/src/ikrig/index.ts @@ -0,0 +1,25 @@ + +import IKRig from './rigs/IKRig'; +import BipedRig from './rigs/BipedRig'; +import { IKChain, IKLink } from './rigs/IKChain'; + +import BipedIKPose from './animation/BipedIKPose'; + +import * as IKData from './IKData'; + +import { + SwingTwistEndsSolver, + SwingTwistSolver, + LimbSolver, + HipSolver, +} from './solvers/index' + +export { + IKData, BipedIKPose, + IKRig, BipedRig, IKChain, IKLink, + + SwingTwistEndsSolver, + SwingTwistSolver, + LimbSolver, + HipSolver, +}; \ No newline at end of file diff --git a/src/ikrig/rigs/BipedRig.ts b/src/ikrig/rigs/BipedRig.ts new file mode 100644 index 0000000..265581a --- /dev/null +++ b/src/ikrig/rigs/BipedRig.ts @@ -0,0 +1,175 @@ +//#region IMPORTS +import type Armature from '../../armature/Armature'; +import BoneMap, { BoneChain, BoneInfo } from '../../armature/BoneMap'; +import Pose from '../../armature/Pose'; + +import HipSolver from '../solvers/HipSolver'; +import LimbSolver from '../solvers/LimbSolver'; +import SwingTwistSolver from '../solvers/SwingTwistSolver'; +//import SwingTwistChainSolver from '../solvers/SwingTwistChainSolver'; +import SwingTwistEndsSolver from '../solvers/SwingTwistEndsSolver'; + +import { IKChain } from './IKChain'; +import IKRig from './IKRig'; + +//#endregion + +class BipedRig extends IKRig{ + //#region MAIN + hip ?: IKChain = undefined; + spine ?: IKChain = undefined; + neck ?: IKChain = undefined; + head ?: IKChain = undefined; + armL ?: IKChain = undefined; + armR ?: IKChain = undefined; + legL ?: IKChain = undefined; + legR ?: IKChain = undefined; + handL ?: IKChain = undefined; + handR ?: IKChain = undefined; + footL ?: IKChain = undefined; + footR ?: IKChain = undefined; + + constructor(){ + super(); + } + //#endregion + + /** Try to find all the bones for each particular chains */ + autoRig( arm: Armature ): Boolean{ + const map = new BoneMap( arm ); // Standard Bone Map, Easier to find bones using common names. + let isComplete = true; // Are All the Parts of the AutoRigging found? + let b : BoneInfo | BoneChain | undefined; + let bi : BoneInfo; + let n : string; + let names : string[] = []; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // VERY IMPORTANT : The order of the chains should not change in this + // structure, it will determine the order in which the solvers will be + // called. Certain chains should be called before others, like Hip Before Legs or Arms + const chains = [ + { n:'hip', ch:[ 'hip' ] }, + { n:'spine', ch:[ 'spine' ] }, + { n:'legL', ch:[ 'thigh_l', 'shin_l' ] }, + { n:'legR', ch:[ 'thigh_r', 'shin_r' ] }, + { n:'armL', ch:[ 'upperarm_l', 'forearm_l' ] }, + { n:'armR', ch:[ 'upperarm_r', 'forearm_r' ] }, + { n:'neck', ch:[ 'neck' ] }, + { n:'head', ch:[ 'head' ] }, + { n:'handL', ch:[ 'hand_l' ] }, + { n:'handR', ch:[ 'hand_r' ] }, + { n:'footL', ch:[ 'foot_l' ] }, + { n:'footR', ch:[ 'foot_r' ] }, + ]; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const self : { [k:string] : any } = this; // TypeScript doesn't like "this[ n ] = this.add", using this type lets me get away with it. + + for( let itm of chains ){ + n = itm.n; // Name of Chain + names.length = 0; // Reset Bone Name Array + + //============================================= + // Find all bone names assigned to this chain. + for( let i=0; i < itm.ch.length; i++ ){ + b = map.bones.get( itm.ch[ i ] ); // Get Find Bone Reference + + //------------------------------- + // Not Found, Exit loop to work on next chain. + if( !b ){ + console.log( 'AutoRig - Missing ', itm.ch[ i ] ); + isComplete = false; + break; + } + + //------------------------------- + if( b instanceof BoneInfo ) names.push( b.name ); + else if( b instanceof BoneChain ) for( bi of b.items ) names.push( bi.name ); + } + + //============================================= + self[ n ] = this.add( arm, n, names ); // Add Chain to Rig & assign chain to Rig's property of the same name. + } + + this._setAltDirection( arm ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + + return isComplete; + } + + /** Use Solver Configuration for Retargeting Animation */ + useSolversForRetarget( pose ?: Pose ): this{ + this.hip?.setSolver( new HipSolver().initData( pose, this.hip ) ); + this.head?.setSolver( new SwingTwistSolver().initData( pose, this.head ) ); + this.armL?.setSolver( new LimbSolver().initData( pose, this.armL ) ); + this.armR?.setSolver( new LimbSolver().initData( pose, this.armR ) ); + this.legL?.setSolver( new LimbSolver().initData( pose, this.legL ) ); + this.legR?.setSolver( new LimbSolver().initData( pose, this.legR ) ); + this.footL?.setSolver( new SwingTwistSolver().initData( pose, this.footL ) ); + this.footR?.setSolver( new SwingTwistSolver().initData( pose, this.footR ) ); + this.handL?.setSolver( new SwingTwistSolver().initData( pose, this.handL ) ); + this.handR?.setSolver( new SwingTwistSolver().initData( pose, this.handR ) ); + this.spine?.setSolver( new SwingTwistEndsSolver().initData( pose, this.spine ) ); + + return this; + } + + /** Use Solver Configuration for Fullbody IK */ + useSolversForFBIK( pose ?: Pose ): this{ + // this.hip?.setSolver( new HipSolver().initData( pose, this.hip ) ); + // this.head?.setSolver( new SwingTwistSolver().initData( pose, this.head ) ); + // this.armL?.setSolver( new LimbSolver().initData( pose, this.armL ) ); + // this.armR?.setSolver( new LimbSolver().initData( pose, this.armR ) ); + // this.legL?.setSolver( new LimbSolver().initData( pose, this.legL ) ); + // this.legR?.setSolver( new LimbSolver().initData( pose, this.legR ) ); + // this.footL?.setSolver( new SwingTwistSolver().initData( pose, this.footL ) ); + // this.footR?.setSolver( new SwingTwistSolver().initData( pose, this.footR ) ); + // this.handL?.setSolver( new SwingTwistSolver().initData( pose, this.handL ) ); + // this.handR?.setSolver( new SwingTwistSolver().initData( pose, this.handR ) ); + // this.spine?.setSolver( new SwingTwistChainSolver().initData( pose, this.spine ) ); + return this; + } + + /** Setup Chain Data & Sets Alt Directions */ + bindPose( pose: Pose ): this{ + super.bindPose( pose ); + this._setAltDirection( pose ); + return this; + } + + _setAltDirection( pose: any ): void{ + const FWD = [0,0,1]; + const UP = [0,1,0]; + const DN = [0,-1,0]; + const R = [-1,0,0]; + const L = [1,0,0]; + const BAK = [0,0,-1]; + + if( this.hip ) this.hip.bindAltDirections( pose, FWD, UP ); + if( this.spine ) this.spine.bindAltDirections( pose, UP, FWD ); + if( this.neck ) this.neck.bindAltDirections( pose, FWD, UP ); + if( this.head ) this.head.bindAltDirections( pose, FWD, UP ); + + if( this.legL ) this.legL.bindAltDirections( pose, DN, FWD ); + if( this.legR ) this.legR.bindAltDirections( pose, DN, FWD ); + if( this.footL ) this.footL.bindAltDirections( pose, FWD, UP ); + if( this.footR ) this.footR.bindAltDirections( pose, FWD, UP ); + + if( this.armL ) this.armL.bindAltDirections( pose, L, BAK ); + if( this.armR ) this.armR.bindAltDirections( pose, R, BAK ); + if( this.handL ) this.handL.bindAltDirections( pose, L, BAK ); + if( this.handR ) this.handR.bindAltDirections( pose, R, BAK ); + } + + resolveToPose( pose: any, debug ?: any ){ + let ch: IKChain; + //console.time( 'resolveToPose' ); + for( ch of this.items.values() ){ + if( ch.solver ) ch.resolveToPose( pose, debug ); + } + //console.timeEnd( 'resolveToPose' ); + } +} + +export default BipedRig; \ No newline at end of file diff --git a/src/ikrig/rigs/IKChain.ts b/src/ikrig/rigs/IKChain.ts new file mode 100644 index 0000000..0c42635 --- /dev/null +++ b/src/ikrig/rigs/IKChain.ts @@ -0,0 +1,227 @@ +//#region IMPORTS +import type { Armature, Bone, Pose } from '../../armature/index'; +import { Transform } from '../../maths'; +import { vec3, quat } from 'gl-matrix'; +import Vec3Util from '../../maths/Vec3Util'; +//#endregion + +class IKLink{ + //#region MAIN + idx : number; // Bone Index + pidx : number; // Bone Parent Index + len : number; // Bone Length + bind : Transform = new Transform(); // LocalSpace BindPose ( TPose ) Transform + + effectorDir : vec3 = [0,1,0]; // WorldSpace Target Alt Direction ( May be created from Inverted Worldspace Rotation of bone ) + poleDir : vec3 = [0,0,1]; // WorldSpace Bend Alt Direction ... + + constructor( idx: number, len: number ){ + this.idx = idx; + this.pidx = -1; + this.len = len; + } + //#endregion + + //#region STATICS + static fromBone( b: Bone ): IKLink{ + const l = new IKLink( b.idx, b.len ); + l.bind.copy( b.local ); + l.pidx = ( b.pidx != null )? b.pidx : -1; + return l; + } + //#endregion +} + +class IKChain{ + //#region MAIN + links : IKLink[] = []; + solver : any = null; + count : number = 0; + length : number = 0; + + constructor( bName?: string[], arm ?:Armature ){ + if( bName && arm ) this.setBones( bName, arm ); + } + //#endregion + + //#region SETTERS + setBones( bNames: string[], arm: Armature ): this{ + let b: Bone | null; + let n: string; + + this.length = 0; // Reset Chain Length + + for( n of bNames ){ + b = arm.getBone( n ); + if( b ){ + this.length += b.len; + this.links.push( IKLink.fromBone( b ) ); + }else console.log( 'Chain.setBones - Bone Not Found:', n ); + } + + this.count = this.links.length; + return this; + } + + setSolver( s: any ): this{ this.solver = s; return this; } + + // Change the Bind Transform + // Mostly used for late binding a TPose when armature isn't naturally in a TPose + bindToPose( pose: Pose ): this{ + let lnk : IKLink; + for( lnk of this.links ){ + lnk.bind.copy( pose.bones[ lnk.idx ].local ); + } + return this; + } + + //#region METHIDS + + /** For usecase when bone lengths have been recomputed for a pose which differs from the initial armature */ + resetLengths( pose: Pose ): void{ + let lnk: IKLink; + let len: number; + + this.length = 0; + for( lnk of this.links ){ + len = pose.bones[ lnk.idx ].len; // Get Current Length in Pose + lnk.len = len; // Save it to Link + this.length += len; // Accumulate the total chain length + } + } + //#endregion + + //#endregion + + //#region GETTERS + first() : IKLink{ return this.links[ 0 ]; } + last() : IKLink{ return this.links[ this.count-1 ]; } + //#endregion + + //#region GET POSITIONS + getEndPositions( pose: Pose ): Array< vec3 >{ + let rtn: Array< vec3 > = []; + + if( this.count != 0 ) rtn.push( Vec3Util.toArray( pose.bones[ this.links[ 0 ].idx ].world.pos ) as vec3 ); + + if( this.count > 1 ){ + const lnk = this.last(); + const v = vec3.fromValues( 0, lnk.len, 0 ); + pose.bones[ lnk.idx ].world.transformVec3( v ); + + rtn.push( Vec3Util.toArray( v ) as vec3 ); + } + + return rtn; + } + + getPositionAt( pose: Pose, idx: number ): vec3{ + const b = pose.bones[ this.links[ idx ].idx ]; + return Vec3Util.toArray( b.world.pos ) as vec3; + } + + getAllPositions( pose: Pose ): Array< vec3 >{ + const rtn : Array< vec3 > = []; + let lnk : IKLink; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Get head position of every bone + for( lnk of this.links ){ + rtn.push( Vec3Util.toArray( pose.bones[ lnk.idx ].world.pos ) as vec3 ); + } + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Get tail position of the last bone + lnk = this.links[ this.count-1 ]; + const v = vec3.fromValues( 0, lnk.len, 0 ); + pose.bones[ lnk.idx ].world.transformVec3( v ); + + rtn.push( Vec3Util.toArray( v ) as vec3 ); + + return rtn; + } + + getStartPosition( pose: Pose ): vec3{ + const b = pose.bones[ this.links[ 0 ].idx ]; + return Vec3Util.toArray( b.world.pos ) as vec3; + } + + getMiddlePosition( pose: Pose ): vec3{ + if( this.count == 2 ){ + const b = pose.bones[ this.links[ 1 ].idx ]; + return Vec3Util.toArray( b.world.pos ) as vec3; + } + console.warn( 'TODO: Implemenet IKChain.getMiddlePosition' ); + return [0,0,0]; + } + + getLastPosition( pose: Pose ): vec3{ + const b = pose.bones[ this.links[ this.count-1 ].idx ]; + return Vec3Util.toArray( b.world.pos ) as vec3; + } + + getTailPosition( pose: Pose, ignoreScale=false ): vec3{ + const b = pose.bones[ this.links[ this.count - 1 ].idx ]; + const v = vec3.fromValues( 0, b.len, 0 ); + + if( !ignoreScale ) return Vec3Util.toArray( b.world.transformVec3( v ) ) as vec3; + + vec3.transformQuat( v, v, b.world.rot ); + vec3.add( v, v, b.world.pos ); + return Vec3Util.toArray( v ) as vec3; + + // return v + // .transformQuat( b.world.rot ) + // .add( b.world.pos ) + // .toArray(); + } + //#endregion + + //#region DIRECTION + getAltDirections( pose: Pose, idx = 0 ): Array< vec3 >{ + const lnk = this.links[ idx ]; // Get Link & Bone + const b = pose.bones[ lnk.idx ]; + const eff: vec3 = lnk.effectorDir.slice( 0 ) as vec3; // Clone the Directions + const pol: vec3 = lnk.poleDir.slice( 0 ) as vec3; + + //b.world.rot.transformVec3( eff ); + //b.world.rot.transformVec3( pol ); + + // Transform Directions + vec3.transformQuat( eff as vec3, eff as vec3, b.world.rot ); + vec3.transformQuat( pol as vec3, pol as vec3, b.world.rot ); + + return [ eff, pol ]; + } + + bindAltDirections( pose: Pose, effectorDir: number[], poleDir: number[] ): this{ + let l: IKLink; + let v = vec3.create(); //new Vec3(); + let inv = quat.create(); //new Quat(); + + for( l of this.links ){ + quat.invert( inv, pose.bones[ l.idx ].world.rot ); + + vec3.transformQuat( v, effectorDir as vec3, inv ); + vec3.copy( l.effectorDir as vec3, v ); + + vec3.transformQuat( v, poleDir as vec3, inv ); + vec3.copy( l.poleDir as vec3, v ); + + //inv.fromInvert( pose.bones[ l.idx ].world.rot ); + //v.fromQuat( inv, effectorDir ).copyTo( l.effectorDir ); + //v.fromQuat( inv, poleDir ).copyTo( l.poleDir ); + } + + return this; + } + //#endregion + + resolveToPose( pose: Pose, debug ?: any ): this{ + if( !this.solver ){ console.warn( 'Chain.resolveToPose - Missing Solver' ); return this; } + this.solver.resolve( this, pose, debug ); + return this; + } +} + +export { IKChain, IKLink }; \ No newline at end of file diff --git a/src/ikrig/rigs/IKRig.ts b/src/ikrig/rigs/IKRig.ts new file mode 100644 index 0000000..2c23759 --- /dev/null +++ b/src/ikrig/rigs/IKRig.ts @@ -0,0 +1,44 @@ +//#region IMPORTS +import type { Armature, Pose } from '../../armature/index' +import { IKChain } from './IKChain'; +//#endregion + +class IKRig{ + //#region MAIN + items: Map< string, IKChain > = new Map(); + constructor(){} + //#endregion + + //#region METHODS + + // Change the Bind Transform for all the chains + // Mostly used for late binding a TPose when armature isn't naturally in a TPose + bindPose( pose: Pose ): this{ + let ch: IKChain; + for( ch of this.items.values() ) ch.bindToPose( pose ); + return this; + } + + updateBoneLengths( pose: Pose ): this{ + let ch: IKChain; + + for( ch of this.items.values() ){ + ch.resetLengths( pose ); + } + + return this; + } + + get( name: string ): IKChain | undefined{ + return this.items.get( name ); + } + + add( arm: Armature, name:string, bNames: string[] ): IKChain{ + const chain = new IKChain( bNames, arm ); + this.items.set( name, chain ); + return chain; + } + //#endregion +} + +export default IKRig; \ No newline at end of file diff --git a/src/ikrig/solvers/HipSolver.ts b/src/ikrig/solvers/HipSolver.ts new file mode 100644 index 0000000..9edd2b8 --- /dev/null +++ b/src/ikrig/solvers/HipSolver.ts @@ -0,0 +1,111 @@ +//#region IMPORTS +import type Pose from '../../armature/Pose'; +import type { IKChain } from "../rigs/IKChain"; +import type { IKData } from '..'; + +import { Transform } from '../../maths'; +import { vec3 } from 'gl-matrix'; +import SwingTwistSolver from "./SwingTwistSolver"; +//#endregion + +class HipSolver{ + //#region MAIN + isAbs : boolean = true; + position : vec3 = [0,0,0]; + bindHeight : number = 0; + _swingTwist = new SwingTwistSolver(); + + initData( pose ?: Pose, chain ?: IKChain ): this{ + if( pose && chain ){ + const b = pose.bones[ chain.links[ 0 ].idx ]; + this.setMovePos( b.world.pos, true ); + + this._swingTwist.initData( pose, chain ); + } + return this; + } + //#endregion + + //#region SETTING TARGET DATA + setTargetDir( e: vec3, pole ?: vec3 ): this{ this._swingTwist.setTargetDir( e, pole ); return this; } + setTargetPos( v: vec3, pole ?: vec3 ): this{ this._swingTwist.setTargetPos( v, pole ); return this; } + setTargetPole( v: vec3 ): this{ this._swingTwist.setTargetPole( v ); return this; } + + setMovePos( pos: vec3, isAbs=true, bindHeight=0 ): this{ + this.position[ 0 ] = pos[ 0 ]; + this.position[ 1 ] = pos[ 1 ]; + this.position[ 2 ] = pos[ 2 ]; + this.isAbs = isAbs; + this.bindHeight = bindHeight; + return this; + } + //#endregion + + resolve( chain: IKChain, pose: Pose, debug?:any ): void{ + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const hipPos : vec3 = [0,0,0]; + const pt = new Transform(); + const ptInv = new Transform(); + const lnk = chain.first(); + + // Get the Starting Transform + if( lnk.pidx == -1 ) pt.copy( pose.offset ); + else pose.getWorldTransform( lnk.pidx, pt ); + + ptInv.fromInvert( pt ); // Invert Transform to Translate Position to Local Space + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Which Position Type Are we handling? + + if( this.isAbs ){ + vec3.copy( hipPos, this.position ); // Set Absolute Position of where the hip must be + }else{ + const ct = new Transform(); + ct.fromMul( pt, lnk.bind ); // Get Bone's BindPose position in relation to this pose + + if( this.bindHeight == 0 ){ + vec3.add( hipPos, ct.pos, this.position ); // Add Offset Position + }else{ + // Need to scale offset position in relation to the Hip Height of the Source + vec3.scaleAndAdd( hipPos, ct.pos, this.position, Math.abs( ct.pos[ 1 ] / this.bindHeight ) ); + } + } + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + ptInv.transformVec3( hipPos ); // To Local Space + pose.setLocalPos( lnk.idx, hipPos ); + + this._swingTwist.resolve( chain, pose, debug ); // Apply SwingTwist Rotation + } + + ikDataFromPose( chain: IKChain, pose: Pose, out: IKData.Hip ): void{ + const v : vec3 = [0,0,0]; // = new Vec3(); + const lnk = chain.first(); + const b = pose.bones[ lnk.idx ]; + const tran = new Transform(); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Figure out the Delta Change of the Hip Position from its Bind Pose to its Animated Pose + + if( b.pidx == null ) tran.fromMul( pose.offset, lnk.bind ); // Use Offset if there is no parent + else pose.getWorldTransform( lnk.pidx, tran ).mul( lnk.bind ); // Compute Parent's WorldSpace transform, then add local bind pose to it. + + vec3.sub( v, b.world.pos, tran.pos ); // Position Change from Bind Pose + + out.isAbsolute = false; // This isn't an absolute Position, its a delta change + out.bindHeight = tran.pos[ 1 ]; // Use the bind's World Space Y value as its bind height + + vec3.copy( out.pos, v ); // Save Delta Change + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Alt Effector + vec3.transformQuat( v, lnk.effectorDir, b.world.rot ); + vec3.normalize( out.effectorDir, v ); + + // Alt Pole + vec3.transformQuat( v, lnk.poleDir, b.world.rot ); + vec3.normalize( out.poleDir, v ); + } +} + +export default HipSolver; \ No newline at end of file diff --git a/src/ikrig/solvers/ISolver.ts b/src/ikrig/solvers/ISolver.ts new file mode 100644 index 0000000..ed5f935 --- /dev/null +++ b/src/ikrig/solvers/ISolver.ts @@ -0,0 +1,6 @@ +import type Pose from '../../armature/Pose'; +import type { IKChain } from '..'; + +export interface ISolver{ + resolve( chain: IKChain, pose: Pose, debug?:any ): void; +} \ No newline at end of file diff --git a/src/ikrig/solvers/LimbSolver.ts b/src/ikrig/solvers/LimbSolver.ts new file mode 100644 index 0000000..de5f458 --- /dev/null +++ b/src/ikrig/solvers/LimbSolver.ts @@ -0,0 +1,101 @@ +//#region IMPORTS +import type Pose from '../../armature/Pose'; +import type { IKChain } from '../rigs/IKChain'; +import type { IKData } from '..'; +import type { ISolver } from './ISolver'; + +import { QuatUtil } from '../../maths'; +import { vec3, quat } from 'gl-matrix'; +import Vec3Util from '../../maths/Vec3Util'; + +import SwingTwistSolver from './SwingTwistSolver'; +//#endregion + +function lawcos_sss( aLen: number, bLen: number, cLen: number ): number{ + // Law of Cosines - SSS : cos(C) = (a^2 + b^2 - c^2) / 2ab + // The Angle between A and B with C being the opposite length of the angle. + let v = ( aLen*aLen + bLen*bLen - cLen*cLen ) / ( 2 * aLen * bLen ); + if( v < -1 ) v = -1; // Clamp to prevent NaN Errors + else if( v > 1 ) v = 1; + return Math.acos( v ); +} + +class LimbSolver implements ISolver{ + //#region MAIN + _swingTwist = new SwingTwistSolver(); + + initData( pose?: Pose, chain?: IKChain ): this{ + if( pose && chain ){ + this._swingTwist.initData( pose, chain ); + } + return this; + } + //#endregion + + //#region SETTING TARGET DATA + setTargetDir( e: vec3, pole ?: vec3, effectorScale ?: number ): this{ this._swingTwist.setTargetDir( e, pole, effectorScale ); return this; } + setTargetPos( v: vec3, pole ?: vec3 ): this{ this._swingTwist.setTargetPos( v, pole ); return this; } + setTargetPole( v: vec3 ): this{ this._swingTwist.setTargetPole( v ); return this; } + //#endregion + + resolve( chain: IKChain, pose: Pose, debug?:any ): void{ + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Start by Using SwingTwist to target the bone toward the EndEffector + const ST = this._swingTwist + const [ rot, pt ] = ST.getWorldRot( chain, pose, debug ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + let b0 = chain.links[ 0 ], + b1 = chain.links[ 1 ], + alen = b0.len, + blen = b1.len, + clen = Vec3Util.len( ST.effectorPos, ST.originPos ), + prot : quat = [0,0,0,0], + rad : number; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // FIRST BONE + rad = lawcos_sss( alen, clen, blen ); // Get the Angle between First Bone and Target. + + QuatUtil.pmulAxisAngle( rot, ST.orthoDir, -rad, rot ); // Use the Axis X to rotate by Radian Angle + quat.copy( prot, rot ); // Save For Next Bone as Starting Point. + QuatUtil.pmulInvert( rot, rot, pt.rot ); // To Local + + pose.setLocalRot( b0.idx, rot ); // Save to Pose + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // SECOND BONE + // Need to rotate from Right to Left, So take the angle and subtract it from 180 to rotate from + // the other direction. Ex. L->R 70 degrees == R->L 110 degrees + rad = Math.PI - lawcos_sss( alen, blen, clen ); + + quat.mul( rot, prot, b1.bind.rot ); // Get the Bind WS Rotation for this bone + QuatUtil.pmulAxisAngle( rot, ST.orthoDir, rad, rot ); // Rotation that needs to be applied to bone. + QuatUtil.pmulInvert( rot, rot, prot ); // To Local Space + + pose.setLocalRot( b1.idx, rot ); // Save to Pose + } + + ikDataFromPose( chain: IKChain, pose: Pose, out: IKData.DirScale ): void{ + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Length Scaled & Effector Direction + const p0 : vec3 = chain.getStartPosition( pose ); + const p1 : vec3 = chain.getTailPosition( pose, true ); + const dir : vec3 = vec3.sub( [0,0,0], p1, p0 ); + + out.lenScale = vec3.len( dir ) / chain.length; + vec3.normalize( out.effectorDir, dir ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Pole Direction + const lnk = chain.first(); // Chain Link : Pole is based on the first Bone's Rotation + const bp = pose.bones[ lnk.idx ]; // Bone ref from Pose + + vec3.transformQuat( dir, lnk.poleDir, bp.world.rot ); // Get Alt Pole Direction from Pose + vec3.cross( dir, dir, out.effectorDir ); // Get orthogonal Direction... + vec3.cross( dir, out.effectorDir, dir ); // to Align Pole to Effector + vec3.normalize( out.poleDir, dir ); + } +} + +export default LimbSolver; \ No newline at end of file diff --git a/src/ikrig/solvers/SwingTwistEndsSolver.ts b/src/ikrig/solvers/SwingTwistEndsSolver.ts new file mode 100644 index 0000000..04b059b --- /dev/null +++ b/src/ikrig/solvers/SwingTwistEndsSolver.ts @@ -0,0 +1,175 @@ +//#region IMPORTS +import type Bone from '../../armature/Bone'; +import type Pose from '../../armature/Pose'; +import type { IKChain, IKLink } from '../rigs/IKChain'; +import type { ISolver } from './ISolver'; +import type { IKData } from '..'; + +import { vec3, quat } from 'gl-matrix'; +import QuatUtil from '../../maths/QuatUtil'; +//#endregion + +class SwingTwistEndsSolver implements ISolver{ + //#region TARGETTING DATA + startEffectorDir : vec3 = [ 0, 0, 0 ]; + startPoleDir : vec3 = [ 0, 0, 0 ]; + endEffectorDir : vec3 = [ 0, 0, 0 ]; + endPoleDir : vec3 = [ 0, 0, 0 ]; + //#endregion + + initData( pose?: Pose, chain?: IKChain ): this{ + if( pose && chain ){ + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const pole : vec3 = [0,0,0]; // = new Vec3(); + const eff : vec3 = [0,0,0]; // = new Vec3(); + let rot : quat; + let lnk : IKLink; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // First Direction + lnk = chain.first(); + rot = pose.bones[ lnk.idx ].world.rot; + + vec3.transformQuat( eff, lnk.effectorDir, rot ); + vec3.transformQuat( pole, lnk.poleDir, rot ); + + this.setStartDir( eff, pole ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Second Direction + lnk = chain.last(); + rot = pose.bones[ lnk.idx ].world.rot; + + vec3.transformQuat( eff, lnk.effectorDir, rot ); + vec3.transformQuat( pole, lnk.poleDir, rot ); + + this.setEndDir( eff, pole ); + } + return this; + } + + //#region SETTING TARGET DATA + setStartDir( eff: vec3, pole: vec3 ): this{ + this.startEffectorDir[ 0 ] = eff[ 0 ]; + this.startEffectorDir[ 1 ] = eff[ 1 ]; + this.startEffectorDir[ 2 ] = eff[ 2 ]; + this.startPoleDir[ 0 ] = pole[ 0 ]; + this.startPoleDir[ 1 ] = pole[ 1 ]; + this.startPoleDir[ 2 ] = pole[ 2 ]; + return this; + } + + setEndDir( eff: vec3, pole: vec3 ): this{ + this.endEffectorDir[ 0 ] = eff[ 0 ]; + this.endEffectorDir[ 1 ] = eff[ 1 ]; + this.endEffectorDir[ 2 ] = eff[ 2 ]; + this.endPoleDir[ 0 ] = pole[ 0 ]; + this.endPoleDir[ 1 ] = pole[ 1 ]; + this.endPoleDir[ 2 ] = pole[ 2 ]; + return this; + } + //#endregion + + resolve( chain: IKChain, pose: Pose, debug?:any ): void{ + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const iEnd = chain.count - 1; + const pRot : quat = [ 0, 0, 0, 1 ]; + const cRot : quat = [ 0, 0, 0, 1 ]; + const ikEffe : vec3 = [ 0, 0, 0 ]; + const ikPole : vec3 = [ 0, 0, 0 ]; + const dir : vec3 = [ 0, 0, 0 ]; + const rot : quat = [ 0, 0, 0, 1 ]; + const tmp : quat = [ 0, 0, 0, 1 ]; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + let lnk : IKLink = chain.first(); + let t : number; + + // Get Starting Parent WS Rotation + if( lnk.pidx != -1 ) pose.getWorldRotation( lnk.pidx, pRot ); + else quat.copy( pRot, pose.offset.rot ); //pRot.copy( pose.offset.rot ); + + /* DEBUG + const v = new Vec3(); + const pTran = new Transform(); + const cTran = new Transform(); + pose.getWorldTransform( lnk.pidx, pTran ); + */ + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + for( let i=0; i <= iEnd; i++ ){ + //----------------------- + // PREPARE + t = i / iEnd; // Lerp Value + lnk = chain.links[ i ]; // Which Bone to act on + + vec3.lerp( ikEffe, this.startEffectorDir, this.endEffectorDir, t ); // Get Current Effector Direction + vec3.lerp( ikPole, this.startPoleDir, this.endPoleDir, t ); // Get Current Pole Direction + + //----------------------- + // SWING + quat.mul( cRot, pRot, lnk.bind.rot ); // Get bone in WS that has yet to have any rotation applied + vec3.transformQuat( dir, lnk.effectorDir, cRot ); // What is the WS Effector Direction + quat.rotationTo( rot, dir, ikEffe ); // Create our Swing Rotation + quat.mul( cRot, rot, cRot ); // Then Apply to our Bone, so its now swong to match the ik effector dir + + /* DEBUG + cTran.fromMul( pTran, lnk.bind ); + debug.pnt.add( cTran.pos, 0x00ff00, 1 ); + debug.ln.add( cTran.pos, v.fromScale( dir, 0.1 ).add( cTran.pos ), 0x00ff00 ); + */ + + //----------------------- + // TWIST + vec3.transformQuat( dir, lnk.poleDir, cRot ); // Get our Current Pole Direction from Our Effector Rotation + quat.rotationTo( rot, dir, ikPole ); // Create our twist rotation + quat.mul( cRot, rot, cRot ); // Apply Twist so now it matches our IK Pole direction + quat.copy( tmp, cRot ); // Save as the next Parent Rotation + + /* DEBUG + debug.ln.add( cTran.pos, v.fromScale( dir, 0.2 ).add( cTran.pos ), 0x00ff00 ); + debug.ln.add( cTran.pos, v.fromScale( ikPole, 0.2 ).add( cTran.pos ), 0xff0000 ); + */ + + //----------------------- + QuatUtil.pmulInvert( cRot, cRot, pRot ); // To Local Space + pose.setLocalRot( lnk.idx, cRot ); // Save back to pose + if( i != iEnd ) quat.copy( pRot, tmp ); // Set WS Rotation for Next Bone. + + /* DEBUG + pTran.mul( cRot, lnk.bind.pos, lnk.bind.scl ); + */ + } + } + + ikDataFromPose( chain: IKChain, pose: Pose, out: IKData.DirEnds ): void{ + const dir : vec3 = [0,0,0]; + let lnk : IKLink; + let b : Bone; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // First Bone + lnk = chain.first(); + b = pose.bones[ lnk.idx ]; + + vec3.transformQuat( dir, lnk.effectorDir, b.world.rot ); + vec3.normalize( out.startEffectorDir, dir ); + + vec3.transformQuat( dir, lnk.poleDir, b.world.rot ); + vec3.normalize( out.startPoleDir, dir ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Last Bone + lnk = chain.last(); + b = pose.bones[ lnk.idx ]; + + vec3.transformQuat( dir, lnk.effectorDir, b.world.rot ); + vec3.normalize( out.endEffectorDir, dir ); + + vec3.transformQuat( dir, lnk.poleDir, b.world.rot ); + vec3.normalize( out.endPoleDir, dir ); + } + +} + +export default SwingTwistEndsSolver; \ No newline at end of file diff --git a/src/ikrig/solvers/SwingTwistSolver.ts b/src/ikrig/solvers/SwingTwistSolver.ts new file mode 100644 index 0000000..bb8b844 --- /dev/null +++ b/src/ikrig/solvers/SwingTwistSolver.ts @@ -0,0 +1,160 @@ +//#region IMPORTS +import type Pose from '../../armature/Pose'; +import type { IKChain, IKLink } from '../rigs/IKChain'; +import type { ISolver } from './ISolver'; +import type { IKData } from '..'; + +import { Transform } from '../../maths'; +import { vec3, quat } from 'gl-matrix'; +import QuatUtil from '../../maths/QuatUtil'; +//#endregion + +class SwingTwistSolver implements ISolver{ + //#region TARGETTING DATA + _isTarPosition : boolean = false; // Is the Target a Position or a Direction? + _originPoleDir : vec3 = [ 0, 0, 0 ]; // Pole gets updated based on effector direction, so keep originally set dir to compute the orthogonal poleDir + effectorScale : number = 1; + effectorPos : vec3 = [ 0, 0, 0 ]; // IK Target can be a Position or... + effectorDir : vec3 = [ 0, 0, 1 ]; // Direction. BUT if its position, need to compute dir from chain origin position. + poleDir : vec3 = [ 0, 1, 0 ]; // Direction that handles the twisitng rotation + orthoDir : vec3 = [ 1, 0, 0 ]; // Direction that handles the bending direction, like elbow/knees. + originPos : vec3 = [ 0, 0, 0 ]; // Starting World Position of the Chain + //#endregion + + initData( pose?: Pose, chain?: IKChain ): this{ + if( pose && chain ){ + // If init pose is the same used for binding, this should recreate the WORLD SPACE Pole Direction just fine + const lnk: IKLink = chain.links[ 0 ]; + const rot: quat = pose.bones[ lnk.idx ].world.rot; + + const eff : vec3 = vec3.transformQuat( [0,0,0], lnk.effectorDir, rot ); + const pole : vec3 = vec3.transformQuat( [0,0,0], lnk.poleDir, rot ); + + this.setTargetDir( eff, pole ); + //this.setTargetPos( chain.getTailPosition( pose ), pole ); + } + return this; + } + + //#region SETTING TARGET DATA + setTargetDir( e: vec3, pole ?: vec3, effectorScale ?: number ): this{ + this._isTarPosition = false; + this.effectorDir[ 0 ] = e[ 0 ]; + this.effectorDir[ 1 ] = e[ 1 ]; + this.effectorDir[ 2 ] = e[ 2 ]; + if( pole ) this.setTargetPole( pole ); + + if( effectorScale ) this.effectorScale = effectorScale; + return this; + } + + setTargetPos( v: vec3, pole ?: vec3 ): this{ + this._isTarPosition = true; + this.effectorPos[ 0 ] = v[ 0 ]; + this.effectorPos[ 1 ] = v[ 1 ]; + this.effectorPos[ 2 ] = v[ 2 ]; + if( pole ) this.setTargetPole( pole ); + return this; + } + + setTargetPole( v: vec3 ): this{ + this._originPoleDir[ 0 ] = v[ 0 ]; + this._originPoleDir[ 1 ] = v[ 1 ]; + this._originPoleDir[ 2 ] = v[ 2 ]; + return this; + } + //#endregion + + resolve( chain: IKChain, pose: Pose, debug?:any ): void{ + const [ rot, pt ] = this.getWorldRot( chain, pose, debug ); + + QuatUtil.pmulInvert( rot, rot, pt.rot ); // To Local Space + pose.setLocalRot( chain.links[ 0 ].idx, rot ); // Save to Pose + } + + ikDataFromPose( chain: IKChain, pose: Pose, out: IKData.Dir ): void{ + const dir: vec3 = [0,0,0]; //new Vec3(); + const lnk = chain.first(); + const b = pose.bones[ lnk.idx ]; + + // Alt Effector + vec3.transformQuat( dir, lnk.effectorDir, b.world.rot ); + vec3.normalize( out.effectorDir, dir ); + + // Alt Pole + vec3.transformQuat( dir, lnk.poleDir, b.world.rot ); + vec3.normalize( out.poleDir, dir ); + } + + + /** Update Target Data */ + _update( origin: vec3 ): void{ + const v: vec3 = [0,0,0]; + const o: vec3 = [0,0,0]; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Compute the Effector Direction if only given effector position + if( this._isTarPosition ){ + vec3.sub( v, this.effectorPos, origin ); // Forward Axis Z + vec3.normalize( this.effectorDir, v ); + } + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Left axis X - Only needed to make pole orthogonal to effector + vec3.cross( v, this._originPoleDir, this.effectorDir ); + vec3.normalize( this.orthoDir, v ); + + // Up Axis Y + vec3.cross( v, this.effectorDir, this.orthoDir ); + vec3.normalize( this.poleDir, v ); + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + vec3.copy( this.originPos, origin ); + } + + getWorldRot( chain: IKChain, pose: Pose, debug?:any ) : [ quat, Transform ]{ + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const pt = new Transform(); + const ct = new Transform(); + let lnk = chain.first(); + + // Get the Starting Transform + if( lnk.pidx == -1 ) pt.copy( pose.offset ); + else pose.getWorldTransform( lnk.pidx, pt ); + + ct.fromMul( pt, lnk.bind ); // Get Bone's BindPose position in relation to this pose + this._update( ct.pos ); // Update Data to use new Origin. + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + const rot : quat = quat.copy( [0,0,0,1], ct.rot ); + const dir : vec3 = [0,0,0]; + const q : quat = [0,0,0,1]; + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Swing + vec3.transformQuat( dir, lnk.effectorDir, ct.rot ); // Get WS Binding Effector Direction of the Bone + quat.rotationTo( q, dir, this.effectorDir ); // Rotation TO IK Effector Direction + quat.mul( rot, q, rot ); // Apply to Bone WS Rot + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Twist + vec3.transformQuat( dir, lnk.poleDir, rot ); // Get WS Binding Pole Direction of the Bone + quat.rotationTo( q, dir, this.poleDir ); // Rotation to IK Pole Direction + quat.mul( rot, q, rot ); // Apply to Bone WS Rot + Swing + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + // Kinda Hacky putting this here, but its the only time where there is access to chain's length for all extending solvers. + // So if not using a TargetPosition, means we're using Direction then we have to compute the effectorPos. + if( !this._isTarPosition ){ + this.effectorPos[ 0 ] = this.originPos[ 0 ] + this.effectorDir[ 0 ] * chain.length * this.effectorScale; + this.effectorPos[ 1 ] = this.originPos[ 1 ] + this.effectorDir[ 1 ] * chain.length * this.effectorScale; + this.effectorPos[ 2 ] = this.originPos[ 2 ] + this.effectorDir[ 2 ] * chain.length * this.effectorScale; + } + + //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + return [ rot, pt ]; + } + +} + +export default SwingTwistSolver; \ No newline at end of file diff --git a/src/ikrig/solvers/index.ts b/src/ikrig/solvers/index.ts new file mode 100644 index 0000000..9815ba6 --- /dev/null +++ b/src/ikrig/solvers/index.ts @@ -0,0 +1,15 @@ +//import SwingTwistChainSolver from './SwingTwistChainSolver'; +import SwingTwistEndsSolver from './SwingTwistEndsSolver'; +import SwingTwistSolver from './SwingTwistSolver'; +import LimbSolver from './LimbSolver'; +import HipSolver from './HipSolver'; +//import NaturalCCDSolver from './NaturalCCDSolver'; + +export { + //SwingTwistChainSolver, + SwingTwistEndsSolver, + SwingTwistSolver, + LimbSolver, + HipSolver, + //NaturalCCDSolver, +}; \ No newline at end of file diff --git a/src/maths/QuatUtil.ts b/src/maths/QuatUtil.ts index 1e8c205..831c599 100644 --- a/src/maths/QuatUtil.ts +++ b/src/maths/QuatUtil.ts @@ -1,4 +1,4 @@ -import { quat } from 'gl-matrix' +import { quat, vec3 } from 'gl-matrix' class QuatUtil{ @@ -45,6 +45,27 @@ class QuatUtil{ return out; } + /** PreMultiple an Axis Angle to this quaternions */ + static pmulAxisAngle( out:quat, axis: vec3, angle: number, q:quat ) : quat{ + const half = angle * .5, + s = Math.sin( half ), + ax = axis[0] * s, // A Quat based on Axis Angle + ay = axis[1] * s, + az = axis[2] * s, + aw = Math.cos( half ), + + bx = q[0], // B of mul + by = q[1], + bz = q[2], + bw = q[3]; + // Quat.mul( a, b ); + out[ 0 ] = ax * bw + aw * bx + ay * bz - az * by; + out[ 1 ] = ay * bw + aw * by + az * bx - ax * bz; + out[ 2 ] = az * bw + aw * bz + ax * by - ay * bx; + out[ 3 ] = aw * bw - ax * bx - ay * by - az * bz; + return out; + } + /** Inverts the quaternion passed in, then pre multiplies to this quaternion. */ static pmulInvert( out: quat, q: quat, qinv: quat ): quat{ //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/src/maths/Vec3Util.ts b/src/maths/Vec3Util.ts index ebd730a..65b5de5 100644 --- a/src/maths/Vec3Util.ts +++ b/src/maths/Vec3Util.ts @@ -28,7 +28,7 @@ class Vec3Util{ return out; } - + //#region LOADING / CONVERSION /** Used to get data from a flat buffer */ static fromBuf( out: vec3, ary : Array | Float32Array, idx: number ) : vec3 { out[ 0 ] = ary[ idx ]; @@ -60,6 +60,9 @@ class Vec3Util{ return v; } + static toArray( v: vec3 ): number[]{ return [ v[0], v[1], v[2] ]; } + //#endregion + } export default Vec3Util; \ No newline at end of file diff --git a/src/ossos.ts b/src/ossos.ts index ec0adcb..717f54a 100644 --- a/src/ossos.ts +++ b/src/ossos.ts @@ -13,6 +13,12 @@ import { } from './maths/index'; import Gltf2, { Accessor } from './parsers/gltf2/index'; + +import { + IKData, BipedIKPose, IKRig, BipedRig, IKChain, IKLink, + SwingTwistEndsSolver, SwingTwistSolver, LimbSolver, HipSolver, +} from './ikrig/index'; + //#endregion //#region EXPORTS @@ -21,7 +27,9 @@ export { Armature, Bone, Pose, SkinMTX, SkinDQ, SkinDQT, BoneSpring, Maths, Transform, DualQuatUtil, Mat4Util, QuatUtil, Vec3Util, Vec4Util, - Gltf2, Accessor + Gltf2, Accessor, + IKData, BipedIKPose, IKRig, BipedRig, IKChain, IKLink, + SwingTwistEndsSolver, SwingTwistSolver, LimbSolver, HipSolver, }; export type{