Dynamic Bones: try base implementation of stiffness and slackness

h3d/anim/Skin.hx

@@ -12,6 +12,8 @@ class Joint {
 	public var follow : Joint;
 	public var subs : Array<Joint>;
 
+	public var worldPos : h3d.Vector;
+
 	public var offsets : h3d.col.Bounds;
 	public var offsetRay : Float;
 
@@ -25,8 +27,31 @@ class Joint {
 		bindIndex = -1;
 		splitIndex = -1;
 		subs = [];
+		worldPos = new h3d.Vector(0.0, 0.0, 0.0);
 	}
+}
+
+class DynamicJoint extends Joint {
+	public static var STAMP : Float = 0.0;
+	public static var SLEEP_THRESHOLD : Float = 0.0;
+
+	public var relPos : h3d.Matrix; // Initial relative pos before joint get moved by animation / dynamic system
+	public var absPos : h3d.Matrix; // Abs pos of the joint computed each frame
 
+	// Parameters
+	public var speed : h3d.Vector;
+	public var radius : Float = 0.0;
+	public var damping : Float = 0.4;
+	public var stiffness : Float = 0.05;
+	public var resistance : Float = 0.9;
+	public var slackness : Float = 0.1;
+
+	public function new() {
+		super();
+
+		speed = new h3d.Vector(0.0, 0.0, 0.0);
+		worldPos = new h3d.Vector(0.0, 0.0, 0.0);
+	}
 }
 
 private class Permut {

h3d/scene/Object.hx

@@ -771,6 +771,7 @@ class Object {
 		if(inheritCulled)
 			ctx.cullingCollider = cullingCollider;
 
+		posChanged = true;
 		var changed = posChanged;
 		// absPos up to date during sync
 		if( changed ) calcAbsPos();

h3d/scene/Skin.hx

@@ -1,5 +1,8 @@
 package h3d.scene;
 
+import haxe.Timer;
+import h3d.anim.Skin.DynamicJoint;
+
 class Joint extends Object {
 	public var skin : Skin;
 	public var index : Int;
@@ -69,6 +72,11 @@ class Skin extends MultiMaterial {
 	var skinData : h3d.anim.Skin;
 	var currentRelPose : Array<h3d.Matrix>;
 	var currentAbsPose : Array<h3d.Matrix>;
+	var initialAbsPos : Array<h3d.Matrix> = [];
+	var dirsToParent : Array<h3d.Vector> = [];
+	var tmpCurrentAbsPose : Array<h3d.Matrix>;
+	var tmp2CurrentAbsPose : Array<h3d.Matrix>;
+	var tmp3CurrentAbsPose : Array<h3d.Matrix>;
 	var currentPalette : Array<h3d.Matrix>;
 	var splitPalette : Array<Array<h3d.Matrix>>;
 	var jointsUpdated : Bool;
@@ -76,6 +84,7 @@ class Skin extends MultiMaterial {
 	var skinShader : h3d.shader.SkinBase;
 	var jointsGraphics : Graphics;
 	var additivePose : Array<h3d.Matrix>;
+	var g : Graphics;
 
 	public var showJoints : Bool;
 	public var enableRetargeting : Bool = true;
@@ -250,30 +259,153 @@ class Skin extends MultiMaterial {
 	static var TMP_MAT = new h3d.Matrix();
 
 	@:noDebug
+	var init = false;
 	function syncJoints() {
 		if( !jointsUpdated ) return;
 		var tmpMat = TMP_MAT;
+
+		// TODO
+		tmpCurrentAbsPose = [];
+		for (tmp in currentAbsPose) {
+			var tmp2 = new h3d.Matrix();
+			tmp2.load(tmp);
+			tmpCurrentAbsPose.push(tmp2);
+		}
+
 		for( j in skinData.allJoints ) {
 			if ( j.follow != null ) continue;
 			var id = j.index;
 			var m = currentAbsPose[id];
 			var r = currentRelPose[id];
 			var bid = j.bindIndex;
-			if( r == null ) r = j.defMat else if( j.retargetAnim && enableRetargeting ) { tmpMat.load(r); r = tmpMat; r._41 = j.defMat._41; r._42 = j.defMat._42; r._43 = j.defMat._43; }
+			if( r == null )
+				r = j.defMat
+			else if( j.retargetAnim && enableRetargeting ) {
+				tmpMat.load(r);
+				r = tmpMat;
+				r._41 = j.defMat._41;
+				r._42 = j.defMat._42;
+				r._43 = j.defMat._43;
+			}
+			var dyn = Std.downcast(j, DynamicJoint);
+			if (dyn != null)
+				dyn.relPos = r;
 			if( j.parent == null )
 				m.multiply3x4inline(r, absPos);
-			else
+			else {
 				m.multiply3x4inline(r, currentAbsPose[j.parent.index]);
+			}
 			if( additivePose != null ) {
 				var a = additivePose[id];
 				if( a != null ) m.multiply3x4inline(a, m);
 			}
+
 			if( bid >= 0 )
 				currentPalette[bid].multiply3x4inline(j.transPos, m);
 		}
+
 		skinShader.bonesMatrixes = currentPalette;
 		jointsUpdated = false;
 		prevEnableRetargeting = enableRetargeting;
+
+		if (!init) {
+			tmpCurrentAbsPose = [];
+			for (tmp in currentAbsPose) {
+				var tmp2 = new h3d.Matrix();
+				tmp2.load(tmp);
+				tmpCurrentAbsPose.push(tmp2);
+			}
+
+			for( j in skinData.allJoints ) {
+				initialAbsPos[j.index] = currentAbsPose[j.index].clone();
+				if (j.parent == null) continue;
+				dirsToParent[j.index] = currentAbsPose[j.parent.index].getPosition() - currentAbsPose[j.index].getPosition();
+			}
+		}
+
+		tmp2CurrentAbsPose = [];
+		for (tmp in currentAbsPose) {
+			var tmp2 = new h3d.Matrix();
+			tmp2.load(tmp);
+			tmp2CurrentAbsPose.push(tmp2);
+		}
+
+		tmp3CurrentAbsPose = [];
+		for (tmp in currentAbsPose) {
+			var tmp2 = new h3d.Matrix();
+			tmp2.load(tmp);
+			tmp3CurrentAbsPose.push(tmp2);
+		}
+
+		// !---  DYNAMIC BONES
+		var deltaTime = Timer.stamp() - DynamicJoint.STAMP;
+		DynamicJoint.STAMP = Timer.stamp();
+
+		// if (g== null) {
+		// 	g = new Graphics(this.getScene());
+		// 	g.material.mainPass.setPassName("overlay");
+		// 	g.lineStyle(1, 0xFFFFFF, 1);
+		// }
+
+		// g.clear();
+
+		function convertVec(v : h3d.Vector) {
+			return new h3d.Vector(Math.round(v.x * 100) / 100,  Math.round(v.z * 100) / 100, Math.round(-v.y * 100) / 100);
+		}
+
+		for( j in skinData.allJoints ) {
+			if ( j.follow != null ) continue;
+			if (j.index >= currentPalette.length) continue;
+
+			var dynJoint = Std.downcast(j, h3d.anim.Skin.DynamicJoint);
+			if (dynJoint == null) continue;
+			var worldPos = tmpCurrentAbsPose[j.index];
+
+			var newWorldPos = worldPos.getPosition().clone();
+			var expectedPos = worldPos.getPosition().clone();
+			var oldWorldPos = worldPos.getPosition().clone();
+
+			// // Resistance (force resistance)
+			// var globalForce = new h3d.Vector(0, 0, 0);
+			// dynJoint.speed += globalForce * (1.0 - dynJoint.resistance);
+
+			// // Damping (inertia attenuation)
+			// dynJoint.speed *= 1.0 - dynJoint.damping;
+			// if (dynJoint.speed.lengthSq() > DynamicJoint.SLEEP_THRESHOLD) {
+			// 	newWorldPos += dynJoint.speed * deltaTime;
+			// }
+
+			// Stiffness (shape keeper)
+			var parentMovement =  tmp2CurrentAbsPose[j.parent.index].getPosition() - tmp3CurrentAbsPose[j.parent.index].getPosition();
+            expectedPos = dynJoint.relPos.multiplied(tmp3CurrentAbsPose[j.parent.index]).getPosition() + parentMovement;
+            newWorldPos.lerp(newWorldPos, expectedPos, dynJoint.stiffness);
+
+			// Slackness (length keeper)
+			var dirToParent = (newWorldPos - tmp2CurrentAbsPose[j.parent.index].getPosition()).normalized();
+			if (parentMovement.length() != 0) {
+
+			}
+			trace(dynJoint.name + " " + convertVec(dirToParent));
+			// var tmp = new h3d.Matrix();
+			// tmp.multiply3x4(dynJoint.relPos, tmp3CurrentAbsPose[j.parent.index]);
+            // var lengthToParent = tmp.getPosition().length();
+
+            // expectedPos = currentAbsPose[j.parent.index].getPosition() - dirToParent * lengthToParent;
+			// // if (j.name == "B_Tail02")
+			// // 	trace(lengthToParent);
+			// newWorldPos.lerp(expectedPos, newWorldPos, dynJoint.slackness);
+
+			// Collision
+			// TODO
+
+			// dynJoint.speed = (dynJoint.speed + (newWorldPos - oldWorldPos) * (1.0 / deltaTime)) * 0.5;
+			currentAbsPose[j.index].setPosition(newWorldPos);
+			tmp2CurrentAbsPose[j.index].setPosition(newWorldPos);
+
+			if( dynJoint.bindIndex >= 0 )
+				currentPalette[dynJoint.bindIndex].multiply3x4inline(j.transPos, currentAbsPose[j.index]);
+		}
+		init = true;
 	}
 
 	override function emit( ctx : RenderContext ) {

hxd/fmt/fbx/HMDOut.hx

@@ -1042,10 +1042,24 @@ class HMDOut extends BaseLibrary {
 		}
 		indexRec(root);
 
+		function isDynamic(dynName : String, j : TmpObject) {
+			if (j.model.getName().indexOf(dynName) >= 0)
+				return true;
+
+			var parent = j.parent;
+			while(parent != null) {
+				if (parent.model.getName().indexOf(dynName) >= 0)
+					return true;
+				parent = parent.parent;
+			}
+
+			return false;
+		}
+
 		// create joints
 		for( o in joints ) {
 			if( o.isMesh ) throw "assert";
-			var j = new h3d.anim.Skin.Joint();
+			var j = isDynamic('B_Tail02', o) ? new h3d.anim.Skin.DynamicJoint() : new h3d.anim.Skin.Joint();
 			getDefaultMatrixes(o.model); // store for later usage in animation
 			j.index = o.model.getId();
 			j.name = o.model.getName();

hxd/fmt/hmd/Library.hx

@@ -352,8 +352,23 @@ class Library {
 		s.allJoints = [];
 		s.boundJoints = [];
 		s.rootJoints = [];
+
+		function isDynamic(dynName : String, j : SkinJoint) {
+			if (j.name.indexOf(dynName) >= 0)
+				return true;
+
+			var parent = j.parent >= 0 ? s.allJoints[j.parent] : null;
+			while(parent != null) {
+				if (parent.name.indexOf(dynName) >= 0)
+					return true;
+				parent = parent.parent;
+			}
+
+			return false;
+		}
+
 		for( joint in skin.joints ) {
-			var j = new h3d.anim.Skin.Joint();
+			var j = isDynamic('B_Tail02', joint) ? new h3d.anim.Skin.DynamicJoint() : new h3d.anim.Skin.Joint();
 			j.name = joint.name;
 			j.index = s.allJoints.length;
 			j.defMat = joint.position.toMatrix();