1 files changed, 87 insertions, 36 deletions

diff --git a/kon/konmodel.c b/kon/konmodel.c
index 389d16c..a92fb89 100644
--- a/kon/konmodel.c
+++ b/kon/konmodel.c
@@ -416,59 +416,110 @@ static void kon_findKeys_(const kon_modelChannel_t *ch, float time, int *k0, int
 	*t = (time - ch->times[lo]) / (ch->times[hi] - ch->times[lo]);
 }
 
-void kon_modelSample(const kon_model_t *model, int clip, float time, int loop, kon_mat4_t *skin) {
-	kon_vec3_t position[KON_MODEL_MAX_BONES], scale[KON_MODEL_MAX_BONES];
-	kon_vec4_t rotation[KON_MODEL_MAX_BONES];
-	kon_mat4_t global[KON_MODEL_MAX_BONES];
+void kon_poseRest(const kon_model_t *model, kon_pose_t *pose) {
 	int i;
 
-	if (!model || !skin) return;
+	if (!model || !pose) return;
 	for (i = 0; i < model->boneCount; i++) {
-		position[i] = model->bones[i].position;
-		rotation[i] = model->bones[i].rotation;
-		scale[i] = model->bones[i].scale;
+		pose->position[i] = model->bones[i].position;
+		pose->rotation[i] = model->bones[i].rotation;
+		pose->scale[i] = model->bones[i].scale;
 	}
+}
 
-	if (clip >= 0 && clip < model->clipCount) {
-		const kon_modelClip_t *c = &model->clips[clip];
+/* the bone in model that has the same name as bone number from of source. the same number is tried first, it is nearly always right */
+static int kon_matchBone_(const kon_model_t *model, const kon_model_t *source, int from) {
+	if (source == model) return from;
+	if (from < model->boneCount && strcmp(model->bones[from].name, source->bones[from].name) == 0) return from;
+	return kon_modelFindBone(model, source->bones[from].name);
+}
 
-		if (!(time == time)) time = 0.0f;
-		if (c->duration > 0.0f) {
-			if (loop) {
-				time = fmodf(time, c->duration);
-				if (time < 0.0f) time += c->duration;
-			} else if (time > c->duration) {
-				time = c->duration;
-			}
+void kon_poseClip(const kon_model_t *model, const kon_model_t *clipModel, int clip, float time, int loop, kon_pose_t *pose) {
+	const kon_modelClip_t *c;
+	int i;
+
+	if (!model || !pose) return;
+	if (!clipModel) clipModel = model;
+	if (clip < 0 || clip >= clipModel->clipCount) return;
+	c = &clipModel->clips[clip];
+
+	if (!(time == time)) time = 0.0f;
+	if (c->duration > 0.0f) {
+		if (loop) {
+			time = fmodf(time, c->duration);
+			if (time < 0.0f) time += c->duration;
+		} else if (time > c->duration) {
+			time = c->duration;
 		}
-		if (time < 0.0f) time = 0.0f;
-
-		for (i = 0; i < c->channelCount; i++) {
-			const kon_modelChannel_t *ch = &c->channels[i];
-			int k0, k1;
-			float t;
-
-			kon_findKeys_(ch, time, &k0, &k1, &t);
-			if (ch->step) t = 0.0f;
-			if (ch->path == KON_MODEL_ROTATION) {
-				const float *a = &ch->values[k0 * 4], *b = &ch->values[k1 * 4];
-				rotation[ch->bone] = kon_quatSlerp_((kon_vec4_t){a[0], a[1], a[2], a[3]}, (kon_vec4_t){b[0], b[1], b[2], b[3]}, t);
-			} else {
-				const float *a = &ch->values[k0 * 3], *b = &ch->values[k1 * 3];
-				kon_vec3_t v = kon_vec3Lerp(KON_VEC3(a[0], a[1], a[2]), KON_VEC3(b[0], b[1], b[2]), t);
-				if (ch->path == KON_MODEL_POSITION) position[ch->bone] = v; else scale[ch->bone] = v;
-			}
+	}
+	if (time < 0.0f) time = 0.0f;
+
+	for (i = 0; i < c->channelCount; i++) {
+		const kon_modelChannel_t *ch = &c->channels[i];
+		int bone = kon_matchBone_(model, clipModel, ch->bone);
+		int k0, k1;
+		float t;
+
+		if (bone < 0) continue;
+		kon_findKeys_(ch, time, &k0, &k1, &t);
+		if (ch->step) t = 0.0f;
+		if (ch->path == KON_MODEL_ROTATION) {
+			const float *a = &ch->values[k0 * 4], *b = &ch->values[k1 * 4];
+			pose->rotation[bone] = kon_quatSlerp_((kon_vec4_t){a[0], a[1], a[2], a[3]}, (kon_vec4_t){b[0], b[1], b[2], b[3]}, t);
+		} else {
+			const float *a = &ch->values[k0 * 3], *b = &ch->values[k1 * 3];
+			kon_vec3_t v = kon_vec3Lerp(KON_VEC3(a[0], a[1], a[2]), KON_VEC3(b[0], b[1], b[2]), t);
+			if (ch->path == KON_MODEL_POSITION) pose->position[bone] = v; else pose->scale[bone] = v;
 		}
 	}
+}
+
+void kon_poseBlend(const kon_model_t *model, const kon_pose_t *a, const kon_pose_t *b, float t, const float *mask, kon_pose_t *out) {
+	int i;
 
+	if (!model || !a || !b || !out) return;
 	for (i = 0; i < model->boneCount; i++) {
-		kon_mat4_t local = kon_composeTRS_(position[i], rotation[i], scale[i]);
+		float w = mask ? t * mask[i] : t;
+		out->position[i] = kon_vec3Lerp(a->position[i], b->position[i], w);
+		out->scale[i] = kon_vec3Lerp(a->scale[i], b->scale[i], w);
+		out->rotation[i] = kon_quatSlerp_(a->rotation[i], b->rotation[i], w);
+	}
+}
+
+int kon_modelMaskBranch(const kon_model_t *model, const char *bone, float *mask) {
+	int root = kon_modelFindBone(model, bone), i;
+
+	if (root < 0 || !mask) return 0;
+	/* parents always come first, so one pass is enough */
+	for (i = 0; i < model->boneCount; i++) {
+		int parent = model->bones[i].parent;
+		mask[i] = (i == root || (parent >= 0 && mask[parent] > 0.0f)) ? 1.0f : 0.0f;
+	}
+	return 1;
+}
+
+void kon_poseSkin(const kon_model_t *model, const kon_pose_t *pose, kon_mat4_t *skin) {
+	kon_mat4_t global[KON_MODEL_MAX_BONES];
+	int i;
+
+	if (!model || !pose || !skin) return;
+	for (i = 0; i < model->boneCount; i++) {
+		kon_mat4_t local = kon_composeTRS_(pose->position[i], pose->rotation[i], pose->scale[i]);
 		int parent = model->bones[i].parent;
 		global[i] = parent >= 0 ? kon_mat4Mul(global[parent], local) : local;
 		skin[i] = kon_mat4Mul(global[i], model->bones[i].inverseBind);
 	}
 }
 
+void kon_modelSample(const kon_model_t *model, int clip, float time, int loop, kon_mat4_t *skin) {
+	kon_pose_t pose;
+
+	if (!model || !skin) return;
+	kon_poseRest(model, &pose);
+	kon_poseClip(model, NULL, clip, time, loop, &pose);
+	kon_poseSkin(model, &pose, skin);
+}
+
 /*** drawing ***/
 
 static kon_vec3_t kon_mat4MulDirection_(const kon_mat4_t *m, kon_vec3_t v) {