SkinnedMesh transform issue applying transforms to bones

[Cloakd]

ive been going round in circles for the past 2 days on this so hoping someone with a stronger maths background can lend some insight into how to correctly update each bones rotation/position within a ThreeJS skeleton (im pretty sure the issue revolves around the inverse bind transform)

The issue comes when i try and apply these positions to a SkinnedMesh model, No matter what i do the transforms i apply to the model never seem to replicate the motion correctly corresponding to what is seen in motion capture & through the debug output

Codepen: https://codepen.io/scotth-tes/pen/WNZqXgK

import * as THREE from 'https://cdn.skypack.dev/three@0.136.0';
import {GLTFLoader} from 'https://cdn.skypack.dev/three@0.136.0/examples/jsm/loaders/GLTFLoader.js';
import {FBXLoader} from 'https://cdn.skypack.dev/three@0.136.0/examples/jsm/loaders/FBXLoader.js';
import { OrbitControls  } from 'https://cdn.jsdelivr.net/npm/three@0.121.1/examples/jsm/controls/OrbitControls.js'

var debugJoints = {};
var debugLines = {};
var debugMotion = null

var trackingJoint = {
	"spine_1_joint": false,
	"spine_2_joint": false,
	"spine_3_joint": false,
	"spine_4_joint": false,
	"spine_5_joint": false,
	"spine_6_joint": false,
	"neck_1_joint": true,
	"neck_2_joint": true,
	"neck_3_joint": true,
	"head_joint": true,
	"hips_joint": false,
	"root": false,

	//Arms
	"left_arm_joint": true,
	"left_forearm_joint": true,
	"left_hand_joint": true,
	"left_shoulder_1_joint": true,

	"right_arm_joint": true,
	"right_forearm_joint": true,
	"right_hand_joint": true,
	"right_shoulder_1_joint": true,

	//Legs
	"left_leg_joint": false,
	"left_foot_joint": false,
	"left_upLeg_joint": false,
	"right_leg_joint": false,
	"right_foot_joint": false,
	"right_upLeg_joint": false,
}

var motionHistory = [];

let motionCaptureModel = null;
let skeleton = null;

let bufferedPosition = {
	position: {x: 0, y: 0, z: 0},
	rotation: {x: 0, y: 0, z: 0},
	joints: []
};

let scene,renderer,camera,controls,clock

function init() {
  renderer = new THREE.WebGLRenderer({antialias: true});
  renderer.setPixelRatio(window.devicePixelRatio);
  renderer.setSize(window.innerWidth, window.innerHeight);
  document.body.appendChild(renderer.domElement);
  
  
clock = new THREE.Clock()

scene = new THREE.Scene();

const axesHelper = new THREE.AxesHelper(15);
scene.add(axesHelper);

scene.background = new THREE.Color(0xffffff);

camera = new THREE.PerspectiveCamera(70, window.innerWidth / window.innerHeight, 1, 1000);
camera.position.set(0, 90, 90);
camera.rotateX(-30 * Math.PI / 180);

controls = new OrbitControls(camera, renderer.domElement);
controls.damping = 0.2;
window.addEventListener('resize', onWindowResize);

controls.update();

const hemiLight = new THREE.HemisphereLight(0xffffff, 0xffffff, 0.4);
scene.add(hemiLight);

const directionalLight = new THREE.DirectionalLight(0xffffff, 1);
directionalLight.position.set(5, 2, 15);
directionalLight.target.position.set(0, 0, 0);
directionalLight.castShadow = true;

scene.add(directionalLight);


const planeGeometry = new THREE.PlaneGeometry(2000, 2000);
planeGeometry.rotateX(-Math.PI / 2);
const planeMaterial = new THREE.ShadowMaterial({color: 0x000000, opacity: 0.2});

const plane = new THREE.Mesh(planeGeometry, planeMaterial);
plane.name = 'baseplane';
plane.position.y = 0;
plane.receiveShadow = true;
scene.add(plane);

const helper = new THREE.GridHelper(2000, 100);
helper.position.y = 0;
helper.material.opacity = 0.25;
helper.material.transparent = true;
scene.add(helper);
  
  
  loadRequiredModels()
}


function onWindowResize() {

	camera.aspect = window.innerWidth / window.innerHeight;
	camera.updateProjectionMatrix();

	renderer.setSize(window.innerWidth, window.innerHeight);
	renderer.shadowMap.enabled = true;
}

function loadRequiredModels() {
  _loadFBX("https://app.babilu.online/static/tmp/biped_robot.fbx", (fbx) => {
		console.log("Model loaded", fbx);
		motionCaptureModel = fbx;
		skeleton = motionCaptureModel.children[0].children[0].children[0].skeleton
		const container = new THREE.Object3D();
		container.add(fbx);
		let helper = new THREE.SkeletonHelper(motionCaptureModel);
		container.add(helper);
		const box = new THREE.Box3().setFromObject(container);
		const center = box.getCenter(new THREE.Vector3());
		container.position.x += (container.position.x - center.x);
		scene.add(container)
    
    
    console.log("Playing motion capture history: ", motionHistory.length)
		stepThroughHistory()
	})
}


function _loadFBX(url, cb) {
	const loader = new FBXLoader();
	loader.load(url, function (fbx) {
		cb(fbx)
	}, undefined, function (error) {
		console.error(error);
	}, { mode: 'no-cors'});
}


function animate() {
	handleMotionCapture();
	controls.update();
	renderer.render(scene, camera);
	requestAnimationFrame(animate);
}

function handleMotionCapture() {
	if (motionCaptureModel == null)
		return; //Model not yet loaded

	//Loop joints
	adjustJointPositions();
}

function adjustJointPositions() {
	if (debugMotion === null) {
		debugMotion = new THREE.Object3D();
		scene.add(debugMotion)
	}

	//Add debug spheres
	setDebugJoints()

	setSkeleton();
}

function setDebugJoints() {
  console.log("BP", bufferedPosition)
  
	for (let k in bufferedPosition.joints) {
		const joint = bufferedPosition.joints[k];

		if (!trackingJoint[joint.joint.toLowerCase()])
			continue;

		const bidx = getBoneIndex(joint.joint)
		if (bidx == null)
			continue;

		const bone = motionCaptureModel.children[0].children[0].children[0].skeleton.bones[bidx];
		const parentBone = getParentBoneFromBuffer(bone.parent.name)

		var pos = {
			x: joint.position.x * 110,
			y: joint.position.y * 110,
			z: joint.position.z * 110,
			col: joint.joint === "head_joint" ? 0xFF0000 : joint.joint.indexOf("left") === -1 ? 0x00FF00 : 0x0000FF,
		}

		var ppos = null
		if (parentBone !== null) {
			ppos = {x: parentBone.position.x * 110, y: parentBone.position.y * 110, z: parentBone.position.z * 110}
		}

		if (!debugJoints[bidx]) {
			debugJoints[bidx] = addDebugSphere(pos)
			debugMotion.add(debugJoints[bidx])
			if (parentBone !== null) {
				debugLines[bidx] = addDebugLine(ppos, pos)
				console.log("added", debugLines[bidx])
				debugMotion.add(debugLines[bidx])
			}
		} else {
			debugJoints[bidx].position.set(pos.x, pos.y, pos.z)
			if (parentBone != null)
				debugLines[bidx].geometry = getLineGeom(ppos, pos)

			debugJoints[bidx].rotation.set(joint.rotation.x, joint.rotation.y, joint.rotation.z)
		}
	}
}

function getBoneIndex(jointName) {
	for (let bk in skeleton.bones) {
		if (skeleton.bones[bk].name.toLowerCase() === jointName.toLowerCase()) {
			return bk;
		}
	}

	return null
}


function getParentPosition(bone, mappedJoints) {
	if (bone.parent == null)
		return null

	const parentJoint = mappedJoints[bone.parent.name.toLowerCase()]
	if (parentJoint == null)
		return null

	return parentJoint.position
}

function getParentBoneIndex(jointName) {
	if (jointName === "root")
		return null;

	for (let bk in skeleton.bones) {
		if (skeleton.bones[bk].name.toLowerCase() === jointName.toLowerCase()) {
			return skeleton.bones[bk].parent.name;
		}
	}

	return null
}

function getParentBoneFromBuffer(parentJointName) {
	for (let k in bufferedPosition.joints) {
		if (bufferedPosition.joints[k].joint.toLowerCase() === parentJointName.toLowerCase()) {
			return bufferedPosition.joints[k]
		}
	}
	return null
}

function getQuaternionBetweenPoints(p1, p2) {
	const quaternion = new THREE.Quaternion();

	const v1 = new THREE.Vector3(p1.x, p1.y, p1.z);
	const v2 = new THREE.Vector3(p2.x, p2.y, p2.z);

	quaternion.setFromUnitVectors(v1, v2);
	return quaternion;
}

function setSkeleton() {
	//Construct map of our joint transforms
	const mappedJoints = {}
	for (let k in bufferedPosition.joints) {
		const joint = bufferedPosition.joints[k];
		mappedJoints[joint.joint] = joint
	}

	console.log(skeleton)

	skeleton.pose()
	iterateSkeleton(motionCaptureModel.children[1].children[0], mappedJoints)
}

function iterateSkeleton(rootBone, mappedJoints) {
	//Handle bone position
	var newPos = mappedJoints[rootBone.name]

	updateBonePosition(rootBone, newPos, mappedJoints)

	//Handle children
	for (let i = 0; i < rootBone.children.length; i++) {
		var childBone = rootBone.children[i];
		iterateSkeleton(childBone, mappedJoints) //Map the child bone
	}
	// console.log(shoulderRotationHist)
}

function updateBonePosition(bone, newPos, mappedJoints) {
	// console.log("updateBonePosition", bone.name, newPos)

	var bn = bone.name.toLowerCase();
	if (!trackingJoint[bn]) {
		// console.log("Skipped bone - not tracked: ", bn)
		// return;
	}

	const bidx = getBoneIndex(bn)
	if (bidx == null) {
		// console.log("Cant find bone index: ", bn)
		return;
	}


	//Update
	if (!newPos) {
		// console.log("Missing pos: ", bn)
		const bpc = bone.position.clone()
		const brc = bone.quaternion.clone()

		newPos = {
			position: {x: bpc.x, y: bpc.y, z: bpc.z},
			rotation: {x: brc.x, y: brc.y, z: brc.z, r: brc.w},
			transform: [],
		}
	}

	const sub = (Math.PI / 180)
	// const sub = 0

	const ppos = getParentPosition(bone, mappedJoints)
	if (ppos === null) {
    //
	} else {
		const vPos = new THREE.Vector3(newPos.position.x,newPos.position.y,newPos.position.z)
		const vQuat = new THREE.Quaternion(newPos.rotation.x,newPos.rotation.y,newPos.rotation.z, newPos.rotation.r)

		const quat = getQuaternionBetweenPoints(ppos, newPos.position)
		//const q2 = fast_simple_rotation(new THREE.Vector3(ppos.x,ppos.y,ppos.z),vPos)

		if (bn === "left_shoulder_1_joint") {
			console.log("p1, p2", ppos, newPos.position)
			// shoulderRotationHist.push(newPos.rotation)
			// console.log(`${bn}: ${newPos.rotation.x} ${newPos.rotation.y} ${newPos.rotation.z}`)
			// console.log(newRot)
			//console.log("q1, q2", quat, q2)
		}


		if (debugJoints[bidx]) {
			const mrix = new THREE.Matrix4().set(...[].concat(newPos.transform[0]))
			const lmrix = new THREE.Matrix4().multiplyMatrices(bone.matrixWorld.invert(), bone.parent.matrixWorld)

			const qmix = new THREE.Matrix4().makeRotationFromQuaternion( quat );
			lmrix.multiply(qmix)

			bone.applyMatrix4(lmrix);
			bone.updateMatrixWorld();
		}
	}
}


function loadMocapHistory() {
	fetch("https://app.babilu.online/static/tmp/testdata_model.json")
		.then(response => response.json())
		.then(data => {
			console.log("history", data);
			motionHistory = data;
		});
}

function stepThroughHistory() {
	if (motionHistory.length === 0) {
		console.log("history ended");
		return;
	}

	bufferedPosition = motionHistory.pop();
	setTimeout(stepThroughHistory, 60)
}


//Init script
init()
loadMocapHistory()
animate()


//Debug functions

function addDebugSphere(pt) {
	const sphere = buildDebugSphere(pt)
	scene.add(sphere)
	return sphere
}

function addDebugLine(p1, p2) {
	const material = new THREE.LineBasicMaterial({
		color: 0x0000ff,
	});

	const geometry = getLineGeom(p1, p2)
	return new THREE.Line(geometry, material);
}

function getLineGeom(p1, p2) {
	const points = [];
	points.push(new THREE.Vector3(p1.x, p1.y, p1.z));
	points.push(new THREE.Vector3(p2.x, p2.y, p2.z));

	return new THREE.BufferGeometry().setFromPoints(points);
}


function buildDebugSphere(pt) {
	const geometry = new THREE.SphereGeometry(5, 32, 16);
	const material = new THREE.MeshBasicMaterial({color: pt.col});
	const sphere = new THREE.Mesh(geometry, material);

	sphere.add(new THREE.AxesHelper(0.5));

	console.log("Adding sphere at: ", pt)
	sphere.position.set(pt.x, pt.y, pt.z);
	return sphere
}

I have tried both recursively iterating through the skeleton from root & setting the position & rotation on each bone, along with iterating through the bone index but to no avail, can anyone point me in the direction where im going?