void kalman_init(kalman_t *kf, tsfloat_t *q, tsfloat_t *r, float pos, float speed, bool use_speed)
{
kf->use_speed = use_speed;
kf->q = q;
kf->r = r;
/* set up temporary vectors and matrices: */
vec2_init(&kf->t0);
vec2_init(&kf->t1);
mat2x2_init(&kf->T0);
mat2x2_init(&kf->T1);
mat2x2_init(&kf->I);
mat_ident(&kf->I);
/* set initial state: */
vec2_init(&kf->x);
kf->x.ve[0] = pos;
kf->x.ve[1] = speed;
/* no measurement or control yet: */
vec2_init(&kf->z);
vec1_init(&kf->u);
mat2x2_init(&kf->P);
mat_ident(&kf->P);
/* set up noise: */
mat2x2_init(&kf->Q);
mat2x2_init(&kf->R);
/* initialize kalman gain: */
mat2x2_init(&kf->K);
/* H = | 1.0 0.0 |
| 0.0 use_speed | */
mat2x2_init(&kf->H);
kf->H.me[0][0] = 1.0f;
kf->H.me[1][1] = 1.0f * use_speed;
/* A = | 1.0 dt |
| 0.0 1.0 |
note: dt value is set in kalman_run */
mat2x2_init(&kf->A);
kf->A.me[0][0] = 1.0f;
kf->A.me[1][1] = 1.0f;
/* B = | 0.5 * dt ^ 2 |
| dt |
values are set in in kalman_run */
mat2x1_init(&kf->B);
}
void
mat_rotate(Mat *m, const Vec *v, float angle)
{
Mat rm, tmp;
mat_ident(&rm);
const float x = v->data[0];
const float y = v->data[1];
const float z = v->data[2];
const float sin_a = sin(angle);
const float cos_a = cos(angle);
const float k = 1 - cos(angle);
rm.data[0] = cos_a + k * x * x;
rm.data[1] = k * x * y - z * sin_a;
rm.data[2] = k * x * z + y * sin_a;
rm.data[4] = k * x * y + z * sin_a;
rm.data[5] = cos_a + k * y * y;
rm.data[6] = k * y * z - x * sin_a;
rm.data[8] = k * x * z - y * sin_a;
rm.data[9] = k * y * z + x * sin_a;
rm.data[10] = cos_a + k * z * z;
rm.data[15] = 1.0f;
mat_mul(m, &rm, &tmp);
memcpy(m, &tmp, sizeof(Mat));
}
void
mat_translate(Mat *m, float tx, float ty, float tz)
{
Mat tm, tmp;
mat_ident(&tm);
tm.data[3] = tx;
tm.data[7] = ty;
tm.data[11] = tz;
mat_mul(m, &tm, &tmp);
memcpy(m, &tmp, sizeof(Mat));
}
void
mat_scale(Mat *m, float sx, float sy, float sz)
{
Mat sm, tmp;
mat_ident(&sm);
sm.data[0] = sx;
sm.data[5] = sy;
sm.data[10] = sz;
mat_mul(m, &sm, &tmp);
memcpy(m, &tmp, sizeof(Mat));
}
static void
joint_compute_translation(struct JointPose *p0, struct JointPose *p1, float time, Mat *r_tm)
{
/* FIXME: uncomment after fixing rotation
Vec trans;
vec_lerp(&p0->trans, &p1->trans, time, &trans);
mat_ident(r_tm);
mat_translatev(r_tm, &trans);
*/
mat_ident(r_tm);
mat_translatev(r_tm, &p0->trans);
}
static void
joint_compute_scale(struct JointPose *p0, struct JointPose *p1, float time, Mat *r_sm)
{
/* FIXME: uncomment after fixing rotation
Vec scale;
vec_lerp(&p0->scale, &p1->scale, time, &scale);
mat_ident(r_sm);
mat_scalev(r_sm, &scale);
*/
mat_ident(r_sm);
mat_scalev(r_sm, &p0->scale);
}
/**
* Compute joint pose transformation.
*
* This function computes the joint pose transformation for given timestamp by
* interpolating between the provided key poses.
* In order to compute the transformation, the function computes the entire
* parent chain of transformations up to the root node. The pose transformation
* for each traversed node will be stored in the provided array and the process
* keeps track of already computed chains and re-uses them.
*/
static const Mat*
joint_compute_pose(
struct Animation *anim,
struct SkeletonPose *sp0,
struct SkeletonPose *sp1,
uint8_t joint_id,
float time,
Mat *transforms,
bool *computed
) {
Mat *t = &transforms[joint_id];
if (!computed[joint_id]) {
struct Joint *joint = &anim->skeleton->joints[joint_id];
// lookup the previous and current joint poses
struct JointPose *p0 = &sp0->joint_poses[joint_id];
struct JointPose *p1 = &sp1->joint_poses[joint_id];
// compute interpolated local joint transform
Mat tm, rm, sm, tmp;
mat_ident(t);
joint_compute_translation(p0, p1, time, &tm);
joint_compute_rotation(p0, p1, time, &rm);
joint_compute_scale(p0, p1, time, &sm);
mat_mul(&tm, &rm, &tmp);
mat_mul(&tmp, &sm, t);
// if the joint is not the root, pre-multiply the full parent
// transformation chain
if (joint->parent != ROOT_NODE_ID) {
const Mat *parent_t = joint_compute_pose(
anim,
sp0,
sp1,
joint->parent, time, transforms,
computed
);
mat_mul(parent_t, t, &tmp);
*t = tmp;
}
computed[joint_id] = true;
}
return t;
}
