1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libs/anim/anim.c	Sat Feb 01 19:58:19 2014 +0200
     1.3 @@ -0,0 +1,532 @@
     1.4 +#include <stdlib.h>
     1.5 +#include <limits.h>
     1.6 +#include <assert.h>
     1.7 +#include "anim.h"
     1.8 +#include "dynarr.h"
     1.9 +
    1.10 +#define ROT_USE_SLERP
    1.11 +
    1.12 +static void invalidate_cache(struct anm_node *node);
    1.13 +
    1.14 +int anm_init_node(struct anm_node *node)
    1.15 +{
    1.16 +	int i, j;
    1.17 +	static const float defaults[] = {
    1.18 +		0.0f, 0.0f, 0.0f,		/* default position */
    1.19 +		0.0f, 0.0f, 0.0f, 1.0f,	/* default rotation quat */
    1.20 +		1.0f, 1.0f, 1.0f		/* default scale factor */
    1.21 +	};
    1.22 +
    1.23 +	memset(node, 0, sizeof *node);
    1.24 +
    1.25 +	/* initialize thread-local matrix cache */
    1.26 +	pthread_key_create(&node->cache_key, 0);
    1.27 +	pthread_mutex_init(&node->cache_list_lock, 0);
    1.28 +
    1.29 +	for(i=0; i<ANM_NUM_TRACKS; i++) {
    1.30 +		if(anm_init_track(node->tracks + i) == -1) {
    1.31 +			for(j=0; j<i; j++) {
    1.32 +				anm_destroy_track(node->tracks + i);
    1.33 +			}
    1.34 +		}
    1.35 +		anm_set_track_default(node->tracks + i, defaults[i]);
    1.36 +	}
    1.37 +	return 0;
    1.38 +}
    1.39 +
    1.40 +void anm_destroy_node(struct anm_node *node)
    1.41 +{
    1.42 +	int i;
    1.43 +	free(node->name);
    1.44 +
    1.45 +	for(i=0; i<ANM_NUM_TRACKS; i++) {
    1.46 +		anm_destroy_track(node->tracks + i);
    1.47 +	}
    1.48 +
    1.49 +	/* destroy thread-specific cache */
    1.50 +	pthread_key_delete(node->cache_key);
    1.51 +
    1.52 +	while(node->cache_list) {
    1.53 +		struct mat_cache *tmp = node->cache_list;
    1.54 +		node->cache_list = tmp->next;
    1.55 +		free(tmp);
    1.56 +	}
    1.57 +}
    1.58 +
    1.59 +void anm_destroy_node_tree(struct anm_node *tree)
    1.60 +{
    1.61 +	struct anm_node *c, *tmp;
    1.62 +
    1.63 +	if(!tree) return;
    1.64 +
    1.65 +	c = tree->child;
    1.66 +	while(c) {
    1.67 +		tmp = c;
    1.68 +		c = c->next;
    1.69 +
    1.70 +		anm_destroy_node_tree(tmp);
    1.71 +	}
    1.72 +	anm_destroy_node(tree);
    1.73 +}
    1.74 +
    1.75 +struct anm_node *anm_create_node(void)
    1.76 +{
    1.77 +	struct anm_node *n;
    1.78 +
    1.79 +	if((n = malloc(sizeof *n))) {
    1.80 +		if(anm_init_node(n) == -1) {
    1.81 +			free(n);
    1.82 +			return 0;
    1.83 +		}
    1.84 +	}
    1.85 +	return n;
    1.86 +}
    1.87 +
    1.88 +void anm_free_node(struct anm_node *node)
    1.89 +{
    1.90 +	anm_destroy_node(node);
    1.91 +	free(node);
    1.92 +}
    1.93 +
    1.94 +void anm_free_node_tree(struct anm_node *tree)
    1.95 +{
    1.96 +	struct anm_node *c, *tmp;
    1.97 +
    1.98 +	if(!tree) return;
    1.99 +
   1.100 +	c = tree->child;
   1.101 +	while(c) {
   1.102 +		tmp = c;
   1.103 +		c = c->next;
   1.104 +
   1.105 +		anm_free_node_tree(tmp);
   1.106 +	}
   1.107 +
   1.108 +	anm_free_node(tree);
   1.109 +}
   1.110 +
   1.111 +int anm_set_node_name(struct anm_node *node, const char *name)
   1.112 +{
   1.113 +	char *str;
   1.114 +
   1.115 +	if(!(str = malloc(strlen(name) + 1))) {
   1.116 +		return -1;
   1.117 +	}
   1.118 +	strcpy(str, name);
   1.119 +	free(node->name);
   1.120 +	node->name = str;
   1.121 +	return 0;
   1.122 +}
   1.123 +
   1.124 +const char *anm_get_node_name(struct anm_node *node)
   1.125 +{
   1.126 +	return node->name ? node->name : "";
   1.127 +}
   1.128 +
   1.129 +void anm_set_interpolator(struct anm_node *node, enum anm_interpolator in)
   1.130 +{
   1.131 +	int i;
   1.132 +
   1.133 +	for(i=0; i<ANM_NUM_TRACKS; i++) {
   1.134 +		anm_set_track_interpolator(node->tracks + i, in);
   1.135 +	}
   1.136 +	invalidate_cache(node);
   1.137 +}
   1.138 +
   1.139 +void anm_set_extrapolator(struct anm_node *node, enum anm_extrapolator ex)
   1.140 +{
   1.141 +	int i;
   1.142 +
   1.143 +	for(i=0; i<ANM_NUM_TRACKS; i++) {
   1.144 +		anm_set_track_extrapolator(node->tracks + i, ex);
   1.145 +	}
   1.146 +	invalidate_cache(node);
   1.147 +}
   1.148 +
   1.149 +void anm_link_node(struct anm_node *p, struct anm_node *c)
   1.150 +{
   1.151 +	c->next = p->child;
   1.152 +	p->child = c;
   1.153 +
   1.154 +	c->parent = p;
   1.155 +	invalidate_cache(c);
   1.156 +}
   1.157 +
   1.158 +int anm_unlink_node(struct anm_node *p, struct anm_node *c)
   1.159 +{
   1.160 +	struct anm_node *iter;
   1.161 +
   1.162 +	if(p->child == c) {
   1.163 +		p->child = c->next;
   1.164 +		c->next = 0;
   1.165 +		invalidate_cache(c);
   1.166 +		return 0;
   1.167 +	}
   1.168 +
   1.169 +	iter = p->child;
   1.170 +	while(iter->next) {
   1.171 +		if(iter->next == c) {
   1.172 +			iter->next = c->next;
   1.173 +			c->next = 0;
   1.174 +			invalidate_cache(c);
   1.175 +			return 0;
   1.176 +		}
   1.177 +	}
   1.178 +	return -1;
   1.179 +}
   1.180 +
   1.181 +void anm_clear(struct anm_node *node)
   1.182 +{
   1.183 +	int i;
   1.184 +
   1.185 +	for(i=0; i<ANM_NUM_TRACKS; i++) {
   1.186 +		anm_clear_track(&node->tracks[i]);
   1.187 +	}
   1.188 +	invalidate_cache(node);
   1.189 +}
   1.190 +
   1.191 +void anm_set_position(struct anm_node *node, vec3_t pos, anm_time_t tm)
   1.192 +{
   1.193 +	anm_set_value(node->tracks + ANM_TRACK_POS_X, tm, pos.x);
   1.194 +	anm_set_value(node->tracks + ANM_TRACK_POS_Y, tm, pos.y);
   1.195 +	anm_set_value(node->tracks + ANM_TRACK_POS_Z, tm, pos.z);
   1.196 +	invalidate_cache(node);
   1.197 +}
   1.198 +
   1.199 +vec3_t anm_get_node_position(struct anm_node *node, anm_time_t tm)
   1.200 +{
   1.201 +	vec3_t v;
   1.202 +	v.x = anm_get_value(node->tracks + ANM_TRACK_POS_X, tm);
   1.203 +	v.y = anm_get_value(node->tracks + ANM_TRACK_POS_Y, tm);
   1.204 +	v.z = anm_get_value(node->tracks + ANM_TRACK_POS_Z, tm);
   1.205 +	return v;
   1.206 +}
   1.207 +
   1.208 +void anm_set_rotation(struct anm_node *node, quat_t rot, anm_time_t tm)
   1.209 +{
   1.210 +	anm_set_value(node->tracks + ANM_TRACK_ROT_X, tm, rot.x);
   1.211 +	anm_set_value(node->tracks + ANM_TRACK_ROT_Y, tm, rot.y);
   1.212 +	anm_set_value(node->tracks + ANM_TRACK_ROT_Z, tm, rot.z);
   1.213 +	anm_set_value(node->tracks + ANM_TRACK_ROT_W, tm, rot.w);
   1.214 +	invalidate_cache(node);
   1.215 +}
   1.216 +
   1.217 +quat_t anm_get_node_rotation(struct anm_node *node, anm_time_t tm)
   1.218 +{
   1.219 +#ifndef ROT_USE_SLERP
   1.220 +	quat_t q;
   1.221 +	q.x = anm_get_value(node->tracks + ANM_TRACK_ROT_X, tm);
   1.222 +	q.y = anm_get_value(node->tracks + ANM_TRACK_ROT_Y, tm);
   1.223 +	q.z = anm_get_value(node->tracks + ANM_TRACK_ROT_Z, tm);
   1.224 +	q.w = anm_get_value(node->tracks + ANM_TRACK_ROT_W, tm);
   1.225 +	return q;
   1.226 +#else
   1.227 +	int idx0, idx1, last_idx;
   1.228 +	anm_time_t tstart, tend;
   1.229 +	float t, dt;
   1.230 +	struct anm_track *track_x, *track_y, *track_z, *track_w;
   1.231 +	quat_t q, q1, q2;
   1.232 +
   1.233 +	track_x = node->tracks + ANM_TRACK_ROT_X;
   1.234 +	track_y = node->tracks + ANM_TRACK_ROT_Y;
   1.235 +	track_z = node->tracks + ANM_TRACK_ROT_Z;
   1.236 +	track_w = node->tracks + ANM_TRACK_ROT_W;
   1.237 +
   1.238 +	if(!track_x->count) {
   1.239 +		q.x = track_x->def_val;
   1.240 +		q.y = track_y->def_val;
   1.241 +		q.z = track_z->def_val;
   1.242 +		q.w = track_w->def_val;
   1.243 +		return q;
   1.244 +	}
   1.245 +
   1.246 +	last_idx = track_x->count - 1;
   1.247 +
   1.248 +	tstart = track_x->keys[0].time;
   1.249 +	tend = track_x->keys[last_idx].time;
   1.250 +
   1.251 +	if(tstart == tend) {
   1.252 +		q.x = track_x->keys[0].val;
   1.253 +		q.y = track_y->keys[0].val;
   1.254 +		q.z = track_z->keys[0].val;
   1.255 +		q.w = track_w->keys[0].val;
   1.256 +		return q;
   1.257 +	}
   1.258 +
   1.259 +	tm = anm_remap_time(track_x, tm, tstart, tend);
   1.260 +
   1.261 +	idx0 = anm_get_key_interval(track_x, tm);
   1.262 +	assert(idx0 >= 0 && idx0 < track_x->count);
   1.263 +	idx1 = idx0 + 1;
   1.264 +
   1.265 +	if(idx0 == last_idx) {
   1.266 +		q.x = track_x->keys[idx0].val;
   1.267 +		q.y = track_y->keys[idx0].val;
   1.268 +		q.z = track_z->keys[idx0].val;
   1.269 +		q.w = track_w->keys[idx0].val;
   1.270 +		return q;
   1.271 +	}
   1.272 +
   1.273 +	dt = (float)(track_x->keys[idx1].time - track_x->keys[idx0].time);
   1.274 +	t = (float)(tm - track_x->keys[idx0].time) / dt;
   1.275 +
   1.276 +	q1.x = track_x->keys[idx0].val;
   1.277 +	q1.y = track_y->keys[idx0].val;
   1.278 +	q1.z = track_z->keys[idx0].val;
   1.279 +	q1.w = track_w->keys[idx0].val;
   1.280 +
   1.281 +	q2.x = track_x->keys[idx1].val;
   1.282 +	q2.y = track_y->keys[idx1].val;
   1.283 +	q2.z = track_z->keys[idx1].val;
   1.284 +	q2.w = track_w->keys[idx1].val;
   1.285 +
   1.286 +	/*q1 = quat_normalize(q1);
   1.287 +	q2 = quat_normalize(q2);*/
   1.288 +
   1.289 +	return quat_slerp(q1, q2, t);
   1.290 +#endif
   1.291 +}
   1.292 +
   1.293 +void anm_set_scaling(struct anm_node *node, vec3_t scl, anm_time_t tm)
   1.294 +{
   1.295 +	anm_set_value(node->tracks + ANM_TRACK_SCL_X, tm, scl.x);
   1.296 +	anm_set_value(node->tracks + ANM_TRACK_SCL_Y, tm, scl.y);
   1.297 +	anm_set_value(node->tracks + ANM_TRACK_SCL_Z, tm, scl.z);
   1.298 +	invalidate_cache(node);
   1.299 +}
   1.300 +
   1.301 +vec3_t anm_get_node_scaling(struct anm_node *node, anm_time_t tm)
   1.302 +{
   1.303 +	vec3_t v;
   1.304 +	v.x = anm_get_value(node->tracks + ANM_TRACK_SCL_X, tm);
   1.305 +	v.y = anm_get_value(node->tracks + ANM_TRACK_SCL_Y, tm);
   1.306 +	v.z = anm_get_value(node->tracks + ANM_TRACK_SCL_Z, tm);
   1.307 +	return v;
   1.308 +}
   1.309 +
   1.310 +
   1.311 +vec3_t anm_get_position(struct anm_node *node, anm_time_t tm)
   1.312 +{
   1.313 +	mat4_t xform;
   1.314 +	vec3_t pos = {0.0, 0.0, 0.0};
   1.315 +
   1.316 +	if(!node->parent) {
   1.317 +		return anm_get_node_position(node, tm);
   1.318 +	}
   1.319 +
   1.320 +	anm_get_matrix(node, xform, tm);
   1.321 +	return v3_transform(pos, xform);
   1.322 +}
   1.323 +
   1.324 +quat_t anm_get_rotation(struct anm_node *node, anm_time_t tm)
   1.325 +{
   1.326 +	quat_t rot, prot;
   1.327 +	rot = anm_get_node_rotation(node, tm);
   1.328 +
   1.329 +	if(!node->parent) {
   1.330 +		return rot;
   1.331 +	}
   1.332 +
   1.333 +	prot = anm_get_rotation(node->parent, tm);
   1.334 +	return quat_mul(prot, rot);
   1.335 +}
   1.336 +
   1.337 +vec3_t anm_get_scaling(struct anm_node *node, anm_time_t tm)
   1.338 +{
   1.339 +	vec3_t s, ps;
   1.340 +	s = anm_get_node_scaling(node, tm);
   1.341 +
   1.342 +	if(!node->parent) {
   1.343 +		return s;
   1.344 +	}
   1.345 +
   1.346 +	ps = anm_get_scaling(node->parent, tm);
   1.347 +	return v3_mul(s, ps);
   1.348 +}
   1.349 +
   1.350 +void anm_set_pivot(struct anm_node *node, vec3_t piv)
   1.351 +{
   1.352 +	node->pivot = piv;
   1.353 +}
   1.354 +
   1.355 +vec3_t anm_get_pivot(struct anm_node *node)
   1.356 +{
   1.357 +	return node->pivot;
   1.358 +}
   1.359 +
   1.360 +void anm_get_node_matrix(struct anm_node *node, mat4_t mat, anm_time_t tm)
   1.361 +{
   1.362 +	int i;
   1.363 +	mat4_t rmat;
   1.364 +	vec3_t pos, scale;
   1.365 +	quat_t rot;
   1.366 +
   1.367 +	pos = anm_get_node_position(node, tm);
   1.368 +	rot = anm_get_node_rotation(node, tm);
   1.369 +	scale = anm_get_node_scaling(node, tm);
   1.370 +
   1.371 +	m4_set_translation(mat, node->pivot.x, node->pivot.y, node->pivot.z);
   1.372 +
   1.373 +	quat_to_mat4(rmat, rot);
   1.374 +	for(i=0; i<3; i++) {
   1.375 +		mat[i][0] = rmat[i][0];
   1.376 +		mat[i][1] = rmat[i][1];
   1.377 +		mat[i][2] = rmat[i][2];
   1.378 +	}
   1.379 +	/* this loop is equivalent to: m4_mult(mat, mat, rmat); */
   1.380 +
   1.381 +	mat[0][0] *= scale.x; mat[0][1] *= scale.y; mat[0][2] *= scale.z; mat[0][3] += pos.x;
   1.382 +	mat[1][0] *= scale.x; mat[1][1] *= scale.y; mat[1][2] *= scale.z; mat[1][3] += pos.y;
   1.383 +	mat[2][0] *= scale.x; mat[2][1] *= scale.y; mat[2][2] *= scale.z; mat[2][3] += pos.z;
   1.384 +
   1.385 +	m4_translate(mat, -node->pivot.x, -node->pivot.y, -node->pivot.z);
   1.386 +
   1.387 +	/* that's basically: pivot * rotation * translation * scaling * -pivot */
   1.388 +}
   1.389 +
   1.390 +void anm_get_node_inv_matrix(struct anm_node *node, mat4_t mat, anm_time_t tm)
   1.391 +{
   1.392 +	mat4_t tmp;
   1.393 +	anm_get_node_matrix(node, tmp, tm);
   1.394 +	m4_inverse(mat, tmp);
   1.395 +}
   1.396 +
   1.397 +void anm_eval_node(struct anm_node *node, anm_time_t tm)
   1.398 +{
   1.399 +	anm_get_node_matrix(node, node->matrix, tm);
   1.400 +}
   1.401 +
   1.402 +void anm_eval(struct anm_node *node, anm_time_t tm)
   1.403 +{
   1.404 +	struct anm_node *c;
   1.405 +
   1.406 +	anm_eval_node(node, tm);
   1.407 +
   1.408 +	if(node->parent) {
   1.409 +		/* due to post-order traversal, the parent matrix is already evaluated */
   1.410 +		m4_mult(node->matrix, node->parent->matrix, node->matrix);
   1.411 +	}
   1.412 +
   1.413 +	/* recersively evaluate all children */
   1.414 +	c = node->child;
   1.415 +	while(c) {
   1.416 +		anm_eval(c, tm);
   1.417 +		c = c->next;
   1.418 +	}
   1.419 +}
   1.420 +
   1.421 +void anm_get_matrix(struct anm_node *node, mat4_t mat, anm_time_t tm)
   1.422 +{
   1.423 +	struct mat_cache *cache = pthread_getspecific(node->cache_key);
   1.424 +	if(!cache) {
   1.425 +		cache = malloc(sizeof *cache);
   1.426 +		assert(cache);
   1.427 +
   1.428 +		pthread_mutex_lock(&node->cache_list_lock);
   1.429 +		cache->next = node->cache_list;
   1.430 +		node->cache_list = cache;
   1.431 +		pthread_mutex_unlock(&node->cache_list_lock);
   1.432 +
   1.433 +		cache->time = ANM_TIME_INVAL;
   1.434 +		cache->inv_time = ANM_TIME_INVAL;
   1.435 +		pthread_setspecific(node->cache_key, cache);
   1.436 +	}
   1.437 +
   1.438 +	if(cache->time != tm) {
   1.439 +		anm_get_node_matrix(node, cache->matrix, tm);
   1.440 +
   1.441 +		if(node->parent) {
   1.442 +			mat4_t parent_mat;
   1.443 +
   1.444 +			anm_get_matrix(node->parent, parent_mat, tm);
   1.445 +			m4_mult(cache->matrix, parent_mat, cache->matrix);
   1.446 +		}
   1.447 +		cache->time = tm;
   1.448 +	}
   1.449 +	m4_copy(mat, cache->matrix);
   1.450 +}
   1.451 +
   1.452 +void anm_get_inv_matrix(struct anm_node *node, mat4_t mat, anm_time_t tm)
   1.453 +{
   1.454 +	struct mat_cache *cache = pthread_getspecific(node->cache_key);
   1.455 +	if(!cache) {
   1.456 +		cache = malloc(sizeof *cache);
   1.457 +		assert(cache);
   1.458 +
   1.459 +		pthread_mutex_lock(&node->cache_list_lock);
   1.460 +		cache->next = node->cache_list;
   1.461 +		node->cache_list = cache;
   1.462 +		pthread_mutex_unlock(&node->cache_list_lock);
   1.463 +
   1.464 +		cache->inv_time = ANM_TIME_INVAL;
   1.465 +		cache->inv_time = ANM_TIME_INVAL;
   1.466 +		pthread_setspecific(node->cache_key, cache);
   1.467 +	}
   1.468 +
   1.469 +	if(cache->inv_time != tm) {
   1.470 +		anm_get_matrix(node, mat, tm);
   1.471 +		m4_inverse(cache->inv_matrix, mat);
   1.472 +		cache->inv_time = tm;
   1.473 +	}
   1.474 +	m4_copy(mat, cache->inv_matrix);
   1.475 +}
   1.476 +
   1.477 +anm_time_t anm_get_start_time(struct anm_node *node)
   1.478 +{
   1.479 +	int i;
   1.480 +	struct anm_node *c;
   1.481 +	anm_time_t res = LONG_MAX;
   1.482 +
   1.483 +	for(i=0; i<ANM_NUM_TRACKS; i++) {
   1.484 +		if(node->tracks[i].count) {
   1.485 +			anm_time_t tm = node->tracks[i].keys[0].time;
   1.486 +			if(tm < res) {
   1.487 +				res = tm;
   1.488 +			}
   1.489 +		}
   1.490 +	}
   1.491 +
   1.492 +	c = node->child;
   1.493 +	while(c) {
   1.494 +		anm_time_t tm = anm_get_start_time(c);
   1.495 +		if(tm < res) {
   1.496 +			res = tm;
   1.497 +		}
   1.498 +		c = c->next;
   1.499 +	}
   1.500 +	return res;
   1.501 +}
   1.502 +
   1.503 +anm_time_t anm_get_end_time(struct anm_node *node)
   1.504 +{
   1.505 +	int i;
   1.506 +	struct anm_node *c;
   1.507 +	anm_time_t res = LONG_MIN;
   1.508 +
   1.509 +	for(i=0; i<ANM_NUM_TRACKS; i++) {
   1.510 +		if(node->tracks[i].count) {
   1.511 +			anm_time_t tm = node->tracks[i].keys[node->tracks[i].count - 1].time;
   1.512 +			if(tm > res) {
   1.513 +				res = tm;
   1.514 +			}
   1.515 +		}
   1.516 +	}
   1.517 +
   1.518 +	c = node->child;
   1.519 +	while(c) {
   1.520 +		anm_time_t tm = anm_get_end_time(c);
   1.521 +		if(tm > res) {
   1.522 +			res = tm;
   1.523 +		}
   1.524 +		c = c->next;
   1.525 +	}
   1.526 +	return res;
   1.527 +}
   1.528 +
   1.529 +static void invalidate_cache(struct anm_node *node)
   1.530 +{
   1.531 +	struct mat_cache *cache = pthread_getspecific(node->cache_key);
   1.532 +	if(cache) {
   1.533 +	   cache->time = cache->inv_time = ANM_TIME_INVAL;
   1.534 +	}
   1.535 +}