DynamicBones: make dynamic bones framerate independant

h3d/anim/BlendSpace2D.hx

@@ -104,7 +104,7 @@ class BlendSpace2D extends h3d.anim.Animation {
 			// copy modified matrices references
 			@:privateAccess
 			for (object in point.objects) {
-				object.matrices[ptIndex] = (if( object.targetSkin != null ) object.targetSkin.jointsData[object.targetJoint].currentRelPose else object.targetObject?.defaultTransform) ?? object.matrices[ptIndex];
+				object.matrices[ptIndex] = (if( object.targetSkin != null ) object.targetSkin.jointsData[object.targetJoint].currentRelPos else object.targetObject?.defaultTransform) ?? object.matrices[ptIndex];
 			}
 		}
 
@@ -164,7 +164,7 @@ class BlendSpace2D extends h3d.anim.Animation {
 			}
 
 			@:privateAccess if( object.targetSkin != null )
-				object.targetSkin.jointsData[object.targetJoint].currentRelPose = outMatrix;
+				object.targetSkin.jointsData[object.targetJoint].currentRelPos = outMatrix;
 			else if( object.targetObject != null )
 				object.targetObject.defaultTransform = outMatrix;
 		}

h3d/anim/BufferAnimation.hx

@@ -270,7 +270,7 @@ class BufferAnimation extends Animation {
 				}
 
 				if( o.targetSkin != null ) {
-					o.targetSkin.jointsData[o.targetJoint].currentRelPose = m;
+					o.targetSkin.jointsData[o.targetJoint].currentRelPos = m;
 					o.targetSkin.jointsUpdated = true;
 				} else
 					o.targetObject.defaultTransform = m;

h3d/anim/LinearAnimation.hx

@@ -279,7 +279,7 @@ class LinearAnimation extends Animation {
 			}
 
 			if( o.targetSkin != null ) {
-				o.targetSkin.jointsData[o.targetJoint].currentRelPose = o.matrix;
+				o.targetSkin.jointsData[o.targetJoint].currentRelPos = o.matrix;
 				o.targetSkin.jointsUpdated = true;
 			} else
 				o.targetObject.defaultTransform = o.matrix;

h3d/anim/SmoothTarget.hx

@@ -47,7 +47,7 @@ class SmoothTarget extends Animation {
 			s.targetJoint = o.targetJoint;
 			objects.push(s);
 			if( o.targetSkin != null ) {
-				mat = @:privateAccess o.targetSkin.jointsData[o.targetJoint].currentRelPose;
+				mat = @:privateAccess o.targetSkin.jointsData[o.targetJoint].currentRelPos;
 				if ( mat != null && o.targetSkin.prevEnableRetargeting ) {
 					var j = @:privateAccess o.targetSkin.skinData.allJoints[o.targetJoint];
 					if ( j != null && j.retargetAnim ) {
@@ -115,7 +115,7 @@ class SmoothTarget extends Animation {
 		var blend = hxd.Math.easeFactor(blend, easing);
 
 		for( o in objects ) {
-			var m = @:privateAccess if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPose else if( o.targetObject != null ) o.targetObject.defaultTransform else null;
+			var m = @:privateAccess if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPos else if( o.targetObject != null ) o.targetObject.defaultTransform else null;
 
 			if( m == null ) continue;
 
@@ -176,7 +176,7 @@ class SmoothTarget extends Animation {
 					mout.tz = lerp(o.tz, m.tz, blend);
 				}
 
-				@:privateAccess if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPose = mout else o.targetObject.defaultTransform = mout;
+				@:privateAccess if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPos = mout else o.targetObject.defaultTransform = mout;
 			}
 		}
 	}

h3d/anim/SmoothTransition.hx

@@ -97,14 +97,14 @@ class SmoothTransition extends Transition {
 		for( o in objects ) {
 			if( !o.isAnim1 )
 				continue;
-			o.tmpMatrix = if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPose else o.targetObject.defaultTransform;
+			o.tmpMatrix = if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPos else o.targetObject.defaultTransform;
 		}
 		anim2.sync(true);
 		var a = 1 - blendFactor, b = blendFactor;
 		var q1 = new h3d.Quat(), q2 = new h3d.Quat(), qout = new h3d.Quat();
 		for( o in objects ) {
 			var m1 = o.tmpMatrix;
-			var m2 = if( !o.isAnim2 ) o.def else if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPose else o.targetObject.defaultTransform;
+			var m2 = if( !o.isAnim2 ) o.def else if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPos else o.targetObject.defaultTransform;
 			var m = o.outMatrix;
 			// interpolate rotation
 			q1.set(m1._12, m1._13, m1._21, m1._23);
@@ -131,7 +131,7 @@ class SmoothTransition extends Transition {
 			m._42 = m1._42 * a + m2._42 * b;
 			m._43 = m1._43 * a + m2._43 * b;
 			// save matrix
-			if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPose = m else o.targetObject.defaultTransform = m;
+			if( o.targetSkin != null ) o.targetSkin.jointsData[o.targetJoint].currentRelPos = m else o.targetObject.defaultTransform = m;
 		}
 	}
 

h3d/col/SkinCollider.hx

@@ -154,7 +154,7 @@ class SkinColliderDebugObj extends h3d.scene.Object {
 				var m = skin.currentPalette[j.bindIndex];
 				b.setTransform(m);
 			} else
-				b.setTransform(skin.jointsData[j.index].currentAbsPose);
+				b.setTransform(skin.jointsData[j.index].currentRelPos);
 		}
 	}
 

h3d/scene/Skin.hx

@@ -52,7 +52,7 @@ class Joint extends Object {
 
 		if( lastFrame != skin.lastFrame) {
 			lastFrame = skin.lastFrame;
-			absPos.load(skin.jointsData[index].currentAbsPose);
+			absPos.load(skin.jointsData[index].currentAbsPos);
 		}
 	}
 
@@ -69,18 +69,21 @@ class Joint extends Object {
 
 @:access(h3d.scene.Skin)
 class JointData {
-	public var currentRelPose : h3d.Matrix;
-	public var currentAbsPose : h3d.Matrix;
+	public var currentRelPos : h3d.Matrix;
+	public var currentAbsPos : h3d.Matrix;
 	public var additivePose : h3d.Matrix;
 
+	var targetMat : h3d.Matrix = new h3d.Matrix();
+	var originMat : h3d.Matrix = new h3d.Matrix();
+
 	public function new() {
-		this.currentAbsPose = h3d.Matrix.I();
+		this.currentAbsPos = h3d.Matrix.I();
 	}
 
-	public function sync(skin: h3d.scene.Skin, j: h3d.anim.Skin.Joint) {
+	public function sync(skin: h3d.scene.Skin, j: h3d.anim.Skin.Joint, syncDyn : Bool) {
 		if ( j.follow != null ) return;
-		var m = currentAbsPose;
-		var r = currentRelPose;
+		var m = currentAbsPos;
+		var r = currentRelPos;
 		var bid = j.bindIndex;
 		if( r == null )
 			r = j.defMat
@@ -94,7 +97,7 @@ class JointData {
 		if( j.parent == null )
 			m.multiply3x4inline(r, skin.absPos);
 		else
-			m.multiply3x4inline(r, skin.jointsData[j.parent.index].currentAbsPose);
+			m.multiply3x4inline(r, skin.jointsData[j.parent.index].currentAbsPos);
 		if( additivePose != null )
 			m.multiply3x4inline(additivePose, m);
 		if( bid >= 0 )
@@ -113,6 +116,7 @@ class DynamicJointData extends JointData {
 	static var tmpVec = new Vector(0, 0, 0);
 	static var tmpVec2 = new Vector(0, 0, 0);
 	static var tmpQ = new Quat();
+	static var tmpQ2 = new Quat();
 
 	var f = -1;
 	var initialState : DynamicJointData;
@@ -123,15 +127,15 @@ class DynamicJointData extends JointData {
 	}
 
 	public function load(data : DynamicJointData) {
-		if (data.currentRelPose != null) {
-			if (currentRelPose == null)
-				currentRelPose = new Matrix();
-			currentRelPose.load(data.currentRelPose);
+		if (data.currentRelPos != null) {
+			if (currentRelPos == null)
+				currentRelPos = new Matrix();
+			currentRelPos.load(data.currentRelPos);
 		}
-		if (data.currentAbsPose != null) {
-			if (currentAbsPose == null)
-				currentAbsPose = new Matrix();
-			currentAbsPose.load(data.currentAbsPose);
+		if (data.currentAbsPos != null) {
+			if (currentAbsPos == null)
+				currentAbsPos = new Matrix();
+			currentAbsPos.load(data.currentAbsPos);
 		}
 		if (data.additivePose != null) {
 			if (additivePose == null)
@@ -146,8 +150,8 @@ class DynamicJointData extends JointData {
 		speed.load(data.speed);
 	}
 
-	public override function sync(skin: h3d.scene.Skin, j: h3d.anim.Skin.Joint) {
-		super.sync(skin, j);
+	public override function sync(skin: h3d.scene.Skin, j: h3d.anim.Skin.Joint, syncDyn : Bool) {
+		super.sync(skin, j, syncDyn);
 
 		// Ensure we compute dynamic joints data once per frame
 		if (f != hxd.Timer.frameCount) {
@@ -160,17 +164,38 @@ class DynamicJointData extends JointData {
 			this.load(initialState);
 		}
 
-		// Compute position of the current joint
-		computeDyn(skin, j);
+		var jData : DynamicJointData = Std.downcast(skin.jointsData[j.index], DynamicJointData);
+		var jParentData : DynamicJointData = Std.downcast(skin.jointsData[j.parent.index], DynamicJointData);
+		if (syncDyn) {
+			jData.originMat.load(jData.targetMat);
+
+			// Compute position of the current joint
+			computeDyn(skin, j);
+
+			// Orient parent to make him lookat his children
+			computeRotationDyn(skin, j.parent);
+		}
+
+		if (jData.originMat == null || jData.targetMat == null)
+			return;
+
+		var alpha = hxd.Math.clamp(skin.accumulator / Skin.FIXED_DT);
+
+		if( j.bindIndex >= 0 ) {
+			lerpMatrix(jData.originMat, jData.targetMat, alpha, Skin.TMP_MAT);
+			skin.currentPalette[j.bindIndex].multiply3x4inline(j.transPos, Skin.TMP_MAT);
+		}
 
-		// Orient parent to make him lookat his children
-		computeRotationDyn(skin, j.parent);
+		if( j.parent.bindIndex >= 0 && jParentData != null) {
+			lerpMatrix(jParentData.originMat, jParentData.targetMat, alpha, Skin.TMP_MAT);
+			skin.currentPalette[j.parent.bindIndex].multiply3x4inline(j.parent.transPos, Skin.TMP_MAT);
+		}
 	}
 
 	function computeDyn(skin: h3d.scene.Skin, j: h3d.anim.Skin.Joint) {
 		var j : DynamicJoint = cast j;
 		var jData : DynamicJointData = Std.downcast(skin.jointsData[j.index], DynamicJointData);
-		var absPos = jData.absPos == null ?  jData.currentAbsPose : jData.absPos;
+		var absPos = jData.absPos == null ?  jData.currentAbsPos : jData.absPos;
 		var relPos = j.defMat;
 		newWorldPos.load(absPos.getPosition());
 		expectedPos.load(absPos.getPosition());
@@ -186,28 +211,26 @@ class DynamicJointData extends JointData {
 			newWorldPos.load(newWorldPos + jData.speed * hxd.Math.clamp(hxd.Timer.dt, 0, hxd.Timer.maxDeltaTime));
 
 		if (jData.speed.lengthSq() > DynamicJoint.MAX_THRESHOLD) {
-			newWorldPos.load(jData.currentAbsPose.getPosition());
-			absPos.load(jData.currentAbsPose);
 			jData.speed.set(0, 0, 0);
 		}
 
 		// Stiffness (shape keeper)
-		Skin.TMP_MAT.multiply(relPos, skin.jointsData[j.parent.index].currentAbsPose);
-        expectedPos.load(Skin.TMP_MAT.getPosition());
-        newWorldPos.lerp(newWorldPos, expectedPos, j.stiffness);
+		Skin.TMP_MAT.multiply(relPos, skin.jointsData[j.parent.index].currentAbsPos);
+		expectedPos.load(Skin.TMP_MAT.getPosition());
+		newWorldPos.lerp(newWorldPos, expectedPos, j.stiffness);
 
 		// Slackness (length keeper)
-		var dirToParent = (newWorldPos - skin.jointsData[j.parent.index].currentAbsPose.getPosition()).normalized();
+		var dirToParent = (newWorldPos - skin.jointsData[j.parent.index].currentAbsPos.getPosition()).normalized();
 		var lengthToParent = relPos.getPosition().length();
-		var scale = skin.jointsData[j.parent.index].currentAbsPose.getScale(); //! Non uniform scale won't work
-		expectedPos.load(skin.jointsData[j.parent.index].currentAbsPose.getPosition() + (dirToParent * lengthToParent * scale.x));
+		var scale = skin.jointsData[j.parent.index].currentAbsPos.getScale(); //! Non uniform scale won't work
+		expectedPos.load(skin.jointsData[j.parent.index].currentAbsPos.getPosition() + (dirToParent * lengthToParent * scale.x));
 		newWorldPos.lerp(expectedPos, newWorldPos, j.slackness);
 
 		// Apply lock axis
-		skin.jointsData[j.parent.index].currentAbsPose.getInverse(Skin.TMP_MAT);
+		skin.jointsData[j.parent.index].currentAbsPos.getInverse(Skin.TMP_MAT);
 		tmpVec.load(newWorldPos);
 		tmpVec.transform(Skin.TMP_MAT);
-		tmpVec2.load(jData.currentAbsPose.getPosition());
+		tmpVec2.load(jData.currentAbsPos.getPosition());
 		tmpVec2.transform(Skin.TMP_MAT);
 		if (j.lockAxis.x > 0.0)
 			tmpVec.x = tmpVec2.x;
@@ -215,23 +238,22 @@ class DynamicJointData extends JointData {
 			tmpVec.y = tmpVec2.y;
 		if (j.lockAxis.z > 0.0)
 			tmpVec.z = tmpVec2.z;
-		tmpVec.transform(skin.jointsData[j.parent.index].currentAbsPose);
+		tmpVec.transform(skin.jointsData[j.parent.index].currentAbsPos);
 		newWorldPos.load(tmpVec);
 
 		// Apply computed position to joint
-		jData.speed.load((jData.speed + (newWorldPos - absPos.getPosition()) * (1.0 / hxd.Timer.dt)) * 0.5);
-		jData.currentAbsPose.setPosition(newWorldPos);
+		jData.speed.load((jData.speed + (newWorldPos - absPos.getPosition()) * (1.0 / Skin.FIXED_DT)) * 0.5);
+		jData.currentAbsPos.setPosition(newWorldPos);
 		if (jData.absPos == null)
 			jData.absPos = new h3d.Matrix();
-		jData.absPos.load(jData.currentAbsPose);
+		jData.absPos.load(jData.currentAbsPos);
 		if (jData.relPos == null)
 			jData.relPos = new Matrix();
 
-		skin.jointsData[j.parent.index].currentAbsPose.getInverse(Skin.TMP_MAT);
+		skin.jointsData[j.parent.index].currentAbsPos.getInverse(Skin.TMP_MAT);
 		jData.relPos.multiply(jData.absPos, Skin.TMP_MAT);
 
-		if( j.bindIndex >= 0 )
-			skin.currentPalette[j.bindIndex].multiply3x4inline(j.transPos, jData.currentAbsPose);
+		jData.targetMat.load(jData.currentAbsPos);
 
 		if (jData.speed.length() != 0.)
 			skin.forceJointsUpdateOnFrame = hxd.Timer.frameCount + 1;
@@ -253,18 +275,37 @@ class DynamicJointData extends JointData {
 			tmpQ.initMoveTo(tmpVec, tmpVec2);
 			tmpQ.toMatrix(Skin.TMP_MAT);
 
-			jData.currentAbsPose.multiply(Skin.TMP_MAT, jData.currentAbsPose);
+			jData.currentAbsPos.multiply(Skin.TMP_MAT, jData.currentAbsPos);
 		}
 
-		if( j.bindIndex >= 0 )
-			skin.currentPalette[j.bindIndex].multiply3x4inline(j.transPos, jData.currentAbsPose);
+		if (j.bindIndex >= 0)
+			jData.targetMat.load(jData.currentAbsPos);
+	}
+
+	function lerpMatrix(a: h3d.Matrix, b: h3d.Matrix, t: Float, out: h3d.Matrix) : h3d.Matrix {
+		var posA = a.getPosition();
+		var posB = b.getPosition();
+		var scaleA = a.getScale();
+		var scaleB = b.getScale();
+
+		tmpQ.initRotateMatrix(a);
+		tmpQ2.initRotateMatrix(b);
+
+		tmpVec.lerp(posA, posB, t);
+		tmpVec2.lerp(scaleA, scaleB, t);
 
-		if (jDynData != null)
-			jDynData.absPos.load(jDynData.currentAbsPose);
+		tmpQ.lerp(tmpQ, tmpQ2, t, true);
+
+		tmpQ.toMatrix(out);
+		out.scale(tmpVec2.x, tmpVec2.y, tmpVec2.z);
+		out.setPosition(tmpVec);
+		return out;
 	}
 }
 
 class Skin extends MultiMaterial {
+	public static var FIXED_DT = 1. / 60.;
+	public var accumulator = 0.;
 
 	var skinData : h3d.anim.Skin;
 	var jointsData : Array<JointData>; // Runtime data
@@ -336,7 +377,7 @@ class Skin extends MultiMaterial {
 		var b = new h3d.col.Bounds();
 		for( j in skinData.allJoints ) {
 			if( j.bindIndex < 0 ) continue;
-			var r = jointsData[j.index].currentAbsPose;
+			var r = jointsData[j.index].currentAbsPos;
 			b.addSpherePos(r.tx, r.ty, r.tz, 0);
 		}
 		return b;
@@ -383,7 +424,7 @@ class Skin extends MultiMaterial {
 		if( j == null ) return null;
 		if( additive )
 			return jointsData[j.index].additivePose;
-		return jointsData[j.index].currentRelPose ?? j.defMat;
+		return jointsData[j.index].currentRelPos ?? j.defMat;
 	}
 
 	public function setJointRelPosition( name : String, pos : h3d.Matrix, additive = false ) {
@@ -392,7 +433,7 @@ class Skin extends MultiMaterial {
 		if( additive ) {
 			jointsData[j.index].additivePose = pos;
 		} else
-			jointsData[j.index].currentRelPose = pos;
+			jointsData[j.index].currentRelPos = pos;
 		jointsUpdated = true;
 	}
 
@@ -477,8 +518,15 @@ class Skin extends MultiMaterial {
 
 		skinShader.calcPrevPos = computeVelocity();
 
-		for( j in skinData.allJoints )
-			jointsData[j.index].sync(this, j);
+		var syncDyn = false;
+		accumulator += hxd.Timer.dt;
+		if (accumulator >= Skin.FIXED_DT) {
+			syncDyn = true;
+			accumulator -= Skin.FIXED_DT;
+		}
+
+		for (j in skinData.allJoints)
+			jointsData[j.index].sync(this, j, syncDyn);
 
 		skinShader.bonesMatrixes = currentPalette;
 		jointsUpdated = false;
@@ -531,8 +579,8 @@ class Skin extends MultiMaterial {
 			var g = jointsGraphics;
 			g.clear();
 			for( j in skinData.allJoints ) {
-				var m = jointsData[j.index].currentAbsPose;
-				var mp = j.parent == null ? absPos : jointsData[j.parent.index].currentAbsPose;
+				var m = jointsData[j.index].currentAbsPos;
+				var mp = j.parent == null ? absPos : jointsData[j.parent.index].currentAbsPos;
 				g.lineStyle(1, j.parent == null ? 0xFF0000FF : 0xFFFFFF00);
 				g.moveTo(mp._41, mp._42, mp._43);
 				g.lineTo(m._41, m._42, m._43);