static void kalman_correct(kalman_t *kf, float pos, float speed)
{
/* update H matrix: */
kf->H.me[1][1] = 1.0f * (kf->use_speed && speed != 0.0f);
kf->z.ve[0] = pos;
kf->z.ve[1] = speed;
/* K = P * HT * inv(H * P * HT + R) */
mat_mul(&kf->T0, &kf->H, &kf->P); // T0 = H * P
mmtr_mul(&kf->T1, &kf->T0, &kf->H); // T1 = T0 * HT
mat_add(&kf->T0, &kf->T1, &kf->R); // T0 = T1 + R
mat_inv(&kf->T1, &kf->T0); // T1 = inv(T0)
mmtr_mul(&kf->T0, &kf->P, &kf->H); // T0 = P * HT
mat_mul(&kf->K, &kf->T0, &kf->T1); // K = T0 * T1
/* x = x + K * (z - H * x) */
mat_vec_mul(&kf->t0, &kf->H, &kf->x); // t0 = H * x
vec_sub(&kf->t1, &kf->z, &kf->t0); // t1 = z - t0
mat_vec_mul(&kf->t0, &kf->K, &kf->t1); // t0 = K * t1
vec_add(&kf->x, &kf->x, &kf->t0); // x = x + t0
/* P = (I - K * H) * P */
mat_mul(&kf->T0, &kf->K, &kf->H); // T0 = K * H
mat_sub(&kf->T1, &kf->I, &kf->T0); // T1 = I - T0
mat_mul(&kf->T0, &kf->T1, &kf->P); // T0 = T1 * P
mat_copy(&kf->P, &kf->T0); // P = T0
}
void AD_TaperModifier::update(float4 framepos)
{
AD_Vect3D sub_center;
AD_Matrix mrot, imrot, pret, ipret;
if (center_track!=(postrack *)NULL)
center_track->get_data(framepos, ¢er);
if (amount_track!=(rolltrack *)NULL)
amount_track->get_data(framepos, &amount);
if (curve_track!=(rolltrack *)NULL)
curve_track->get_data(framepos, &curve);
if (uplim_track!=(rolltrack *)NULL)
uplim_track->get_data(framepos, &uplim);
if (lowlim_track!=(rolltrack *)NULL)
lowlim_track->get_data(framepos, &lowlim);
// costruzione matrice di trasformazione e la sua
// inversa
sub_center.x=-center.x;
sub_center.y=-center.y;
sub_center.z=-center.z;
switch (axis)
{
case 0: // asse X
mat_setmatrix_of_eulerrotationZ(&mrot, (float)(-M_PI/2.0));
l=bbx2-bbx1;
break;
case 1: // asse Y
mat_identity(&mrot);
l=bby2-bby1;
break;
case 2: // asse Z
mat_setmatrix_of_eulerrotationX(&mrot, (float)(M_PI/2.0));
l=bbz2-bbz1;
break;
}
mat_transpose(&mrot, &imrot);
mat_setmatrix_of_pretraslation(&pret, &sub_center);
mat_setmatrix_of_pretraslation(&ipret, ¢er);
mat_mul (&mrot, &pret, &tm);
mat_mul (&ipret, &imrot, &invtm);
switch (effectaxis)
{
case 0: doX = 1; doY = 0; break;
case 1: doX = 0; doY = 1; break;
case 2: doX = 1; doY = 1; break;
}
}
void AD_TwistModifier::update(float4 framepos)
{
AD_Vect3D sub_center;
AD_Matrix mrot, imrot, pret, ipret;
if (center_track!=(postrack *)NULL)
center_track->get_data(framepos, ¢er);
if (angle_track!=(rolltrack *)NULL)
angle_track->get_data(framepos, &angle);
if (bias_track!=(rolltrack *)NULL)
bias_track->get_data(framepos, &bias);
if (uplim_track!=(rolltrack *)NULL)
uplim_track->get_data(framepos, &uplim);
if (lowlim_track!=(rolltrack *)NULL)
lowlim_track->get_data(framepos, &lowlim);
// costruzione matrice di trasformazione e la sua
// inversa
sub_center.x=-center.x;
sub_center.y=-center.y;
sub_center.z=-center.z;
switch (axis)
{
case 0: // asse X
mat_setmatrix_of_eulerrotationZ(&mrot, (float)(-M_PI/2.0));
height=bbx2-bbx1;
break;
case 1: // asse Y
mat_identity(&mrot);
height=bby2-bby1;
break;
case 2: // asse Z
mat_setmatrix_of_eulerrotationX(&mrot, (float)(M_PI/2.0));
height=bbz2-bbz1;
break;
}
mat_transpose(&mrot, &imrot);
mat_setmatrix_of_pretraslation(&pret, &sub_center);
mat_setmatrix_of_pretraslation(&ipret, ¢er);
mat_mul (&mrot, &pret, &tm);
mat_mul (&ipret, &imrot, &invtm);
if (height==0)
{
angle = 0.0f;
angleOverHeight = 0.0f;
}
else angleOverHeight = angle / height;
}
void AD_WindModifier::build_objectmatrix (float4 framepos)
{
AD_Vect3D postmp, stmp;
AD_Quaternion objrot;
AD_Matrix posttrans, scaling, maux;
accum_scale.x=accum_scale.y=accum_scale.z=1.0f;
mat_identity(¤tmatrix_rot);
// estrazione dei dati col keyframer: niente di piu' facile col c++ !!!
if (rotationtrack.numkey>0)
{
rotationtrack.get_data(framepos, &objrot);
quat_rotquat_to_matrix(&objrot, ¤tmatrix_rot);
}
mat_copy(¤tmatrix_rot, ¤tmatrix);
if (scaletrack.numkey>0)
{
scaletrack.get_data(framepos, &stmp);
mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z);
accum_scale.x*=stmp.x;
accum_scale.y*=stmp.y;
accum_scale.z*=stmp.z;
}
else mat_identity(&scaling);
if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos);
mat_setmatrix_of_pretraslation(&posttrans, ¤tpos);
mat_mul(&scaling, ¤tmatrix_rot, &maux);
mat_mul(&posttrans, &maux, ¤tmatrix);
if (father!=(AD_Object3D *)NULL)
{
mat_mulaffine(&father->currentmatrix_rot, ¤tmatrix_rot, ¤tmatrix_rot);
mat_mul(&father->currentmatrix, ¤tmatrix, ¤tmatrix);
mat_mulvect(&father->currentmatrix, ¤tpos, &postmp);
vect_copy(&postmp, ¤tpos);
accum_scale.x*=father->accum_scale.x;
accum_scale.y*=father->accum_scale.y;
accum_scale.z*=father->accum_scale.z;
}
mat_transpose(¤tmatrix_rot, &inverse_rotmatrix);
mat_get_row(¤tmatrix, 1, &forza);
vect_auto_normalize(&forza);
vect_scale(&forza, strenght*0.00016f*forceScaleFactor, &forza);
}
void CScene3D::m_RotateCamera(float4 ax, float4 ay, float4 az)
{
AD_Vect3D cameradir, xAxis, yAxis, zAxis, v;
AD_Vect3D vx, vy, vz, ptmp;
AD_Quaternion q1, q2;
AD_Matrix rot1, rot2, rot12, pretrans;
if (!p_CurrentCamera) return;
// estrazione assi locali
mat_get_row(&p_CurrentCamera->p_CurrentRotationMatrix, 0, &xAxis);
mat_get_row(&p_CurrentCamera->p_CurrentRotationMatrix, 1, &yAxis);
mat_get_row(&p_CurrentCamera->p_CurrentRotationMatrix, 2, &zAxis);
// creazioni delle matrici di rotazione
quat_set(&q1, xAxis.x, xAxis.y, xAxis.z, ax);
quat_set(&q2, yAxis.x, yAxis.y, yAxis.z, ay);
quat_quat_to_rotquat(&q1); quat_rotquat_to_matrix(&q1, &rot1);
quat_quat_to_rotquat(&q2); quat_rotquat_to_matrix(&q2, &rot2);
mat_mul(&rot1, &rot2, &rot12);
// ruoto la posizione
vect_sub(&p_CurrentCamera->p_CurrentPosition,
&p_CurrentCamera->p_CurrentTargetPosition,
&cameradir);
mat_mulvect(&rot12, &cameradir, &v);
vect_add(&v, &p_CurrentCamera->p_CurrentTargetPosition,
&p_CurrentCamera->p_CurrentPosition);
// ruoto il target
/*vect_sub(&p_CurrentCamera->p_CurrentTargetPosition,
&p_CurrentCamera->p_CurrentPosition,
&cameradir);
mat_mulvect(&rot12, &cameradir, &v);
vect_add(&v, &p_CurrentCamera->p_CurrentPosition,
&p_CurrentCamera->p_CurrentTargetPosition);*/
mat_mulvect(&rot12, &xAxis, &vx); vect_auto_normalize(&vx);
mat_mulvect(&rot12, &yAxis, &vy); vect_auto_normalize(&vy);
mat_mulvect(&rot12, &zAxis, &vz); vect_auto_normalize(&vz);
mat_insert_row(&p_CurrentCamera->p_CurrentRotationMatrix, 0, &vx);
mat_insert_row(&p_CurrentCamera->p_CurrentRotationMatrix, 1, &vy);
mat_insert_row(&p_CurrentCamera->p_CurrentRotationMatrix, 2, &vz);
vect_neg(&p_CurrentCamera->p_CurrentPosition, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
mat_mul(&p_CurrentCamera->p_CurrentRotationMatrix,
&pretrans,
&p_CurrentCamera->p_WorldMatrix);
}
void AD_PatchObject::build_objectmatrix (float4 framepos)
// costruisce la matrice di trasformazione, che servira' poi per trasformare
// i vertici dell'oggetto;
{
AD_Vect3D postmp, stmp;
AD_Quaternion objrot;
AD_Matrix posttrans, scaling, maux;
accum_scale.x=accum_scale.y=accum_scale.z=1.0f;
mat_identity(¤tmatrix_rot);
// estrazione dei dati col keyframer: niente di piu' facile col c++ !!!
if (rotationtrack.numkey>0)
{
rotationtrack.get_data(framepos, &objrot);
quat_rotquat_to_matrix(&objrot, ¤tmatrix_rot);
}
mat_copy(¤tmatrix_rot, ¤tmatrix);
if (scaletrack.numkey>0)
{
scaletrack.get_data(framepos, &stmp);
mat_setmatrix_of_scaling(&scaling, stmp.x, stmp.y, stmp.z);
accum_scale.x=accum_scale.x*stmp.x;
accum_scale.y=accum_scale.x*stmp.y;
accum_scale.z=accum_scale.x*stmp.z;
}
else mat_identity(&scaling);
if (positiontrack.numkey>0) positiontrack.get_data(framepos, ¤tpos);
mat_setmatrix_of_pretraslation(&posttrans, ¤tpos);
mat_mul(&scaling, ¤tmatrix_rot, &maux);
mat_mul(&posttrans, &maux, ¤tmatrix);
if (father!=(AD_Object3D *)NULL)
{
mat_mulaffine(&father->currentmatrix_rot, ¤tmatrix_rot, ¤tmatrix_rot);
mat_mul(&father->currentmatrix, ¤tmatrix, ¤tmatrix);
mat_mulvect(&father->currentmatrix, ¤tpos, &postmp);
vect_copy(&postmp, ¤tpos);
accum_scale.x*=father->accum_scale.x;
accum_scale.y*=father->accum_scale.y;
accum_scale.z*=father->accum_scale.z;
}
mat_transpose(¤tmatrix_rot, &inverse_rotmatrix);
}
// dcm better be 3 x 3
MATRIX EulerToDcm(MATRIX euler, double decA, MATRIX dcm)
{
MATRIX A,B;
double cPHI,sPHI,cTHE,sTHE,cPSI,sPSI;
cPHI = cos(euler[2][0]); sPHI = sin(euler[2][0]);
cTHE = cos(euler[1][0]); sTHE = sin(euler[1][0]);
cPSI = cos(euler[0][0]); sPSI = sin(euler[0][0]);
A = mat_creat(3,3,ZERO_MATRIX);
B = mat_creat(3,3,UNDEFINED);
A[0][0] = cos(decA); A[0][1] =-sin(decA);
A[1][0] = sin(decA); A[1][1] = cos(decA);
A[2][2] = 1;
B[0][0] = cTHE*cPSI; B[0][1] = sPHI*sTHE*cPSI-cPHI*sPSI; B[0][2] = cPHI*sTHE*cPSI+sPHI*sPSI;
B[1][0] = cTHE*sPSI; B[1][1] = sPHI*sTHE*sPSI+cPHI*cPSI; B[1][2] = cPHI*sTHE*sPSI-sPHI*cPSI;
B[2][0] =-sTHE; B[2][1] = sPHI*cTHE; B[2][2] = cPHI*cTHE;
mat_mul(A,B,dcm);
mat_free(A);
mat_free(B);
return(dcm);
}
void
mat_make_look_at(struct matrix *m, const struct vector *o,
const struct vector *p)
{
struct vector n, u, v, t;
struct matrix a;
vec_sub(&n, p, o);
vec_normalize(&n);
/* v' = (0, 1, 0) */
vec_set(&v, 0.f, 1.f, 0.f);
/* v = v' - (v'*n)*n */
vec_scalar_mul_copy(&t, &n, vec_dot_product(&v, &n));
vec_sub_from(&v, &t);
vec_normalize(&v);
vec_cross_product(&u, &v, &n);
vec_normalize(&u);
m->m11 = u.x; m->m12 = u.y; m->m13 = u.z; m->m14 = 0.f;
m->m21 = v.x; m->m22 = v.y; m->m23 = v.z; m->m24 = 0.f;
m->m31 = n.x; m->m32 = n.y; m->m33 = n.z; m->m34 = 0.f;
mat_make_translation(&a, -o->x, -o->y, -o->z);
mat_mul(m, &a);
}
void mat_LU_decompose(float* A, float* L, float* U, float *P, uint8_t n)
{
memset(L,0,n*n*sizeof(float));
memset(U,0,n*n*sizeof(float));
memset(P,0,n*n*sizeof(float));
mat_pivot(A,P,n);
float *APrime = mat_mul(P,A,n);
for(uint8_t i = 0; i < n; i++) {
L[i*n + i] = 1;
}
for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) {
if(j <= i) {
U[j*n + i] = APrime[j*n + i];
for(uint8_t k = 0; k < j; k++) {
U[j*n + i] -= L[j*n + k] * U[k*n + i];
}
}
if(j >= i) {
L[j*n + i] = APrime[j*n + i];
for(uint8_t k = 0; k < i; k++) {
L[j*n + i] -= L[j*n + k] * U[k*n + i];
}
L[j*n + i] /= U[i*n + i];
}
}
}
free(APrime);
}
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 CScene3D::m_TranslateCamera(float4 dx, float4 dy, float4 dz)
{
AD_Vect3D x, y, z, sum;
AD_Vect3D ptmp;
AD_Matrix pretrans;
if (!p_CurrentCamera) return;
mat_get_row(&p_CurrentCamera->p_WorldMatrix, 0, &x);
mat_get_row(&p_CurrentCamera->p_WorldMatrix, 1, &y);
mat_get_row(&p_CurrentCamera->p_WorldMatrix, 2, &z);
vect_auto_scale(&x, dx);
vect_auto_scale(&y, dy);
vect_auto_scale(&z, dz);
vect_add(&x, &y, &sum);
vect_auto_add(&sum, &z);
vect_auto_add(&p_CurrentCamera->p_CurrentPosition, &sum);
vect_auto_add(&p_CurrentCamera->p_CurrentTargetPosition, &sum);
vect_neg(&p_CurrentCamera->p_CurrentPosition, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
mat_mul(&p_CurrentCamera->p_CurrentRotationMatrix,
&pretrans,
&p_CurrentCamera->p_WorldMatrix);
}
/**
* 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;
}
void CCamera::m_BuildWorldMatrix(void)
{
float4 sinx, cosx, siny, cosy, sinz, cosz;
AD_Matrix pretrans, irot, swapXY;
AD_Vect3D ptmp;
sinx = fast_sinf(p_CurrentAngX);
cosx = fast_cosf(p_CurrentAngX);
siny = fast_sinf(p_CurrentAngY);
cosy = fast_cosf(p_CurrentAngY);
sinz = fast_sinf(p_CurrentAngZ);
cosz = fast_cosf(p_CurrentAngZ);
vect_neg(&p_CurrentPosition, &ptmp);
mat_setmatrix_of_pretraslation(&pretrans, &ptmp);
mat_identity(&p_CurrentRotationMatrix);
p_CurrentRotationMatrix.a[0][0] = sinx * siny * sinz + cosx * cosz;
p_CurrentRotationMatrix.a[0][1] = cosy * sinz;
p_CurrentRotationMatrix.a[0][2] = sinx * cosz - cosx * siny * sinz;
p_CurrentRotationMatrix.a[1][0] = sinx * siny * cosz - cosx * sinz;
p_CurrentRotationMatrix.a[1][1] = cosy * cosz;
p_CurrentRotationMatrix.a[1][2] = -cosx * siny * cosz - sinx * sinz;
p_CurrentRotationMatrix.a[2][0] = -sinx * cosy;
p_CurrentRotationMatrix.a[2][1] = siny;
p_CurrentRotationMatrix.a[2][2] = cosx * cosy;
mat_mul(&p_CurrentRotationMatrix, &pretrans, &p_ViewMatrix);
// costruzione matrice inversa
mat_transpose(&p_CurrentRotationMatrix, &irot);
mat_setmatrix_of_pretraslation(&pretrans, &p_CurrentPosition);
mat_mul(&pretrans, &irot, &p_InverseViewMatrix);
// costruzione matrice per il linking con oggetti
// 1) matrice di swap YZ
mat_identity(&swapXY);
swapXY.a[1][1]=0;
swapXY.a[2][2]=0;
swapXY.a[1][2]=1;
swapXY.a[2][1]=-1;
mat_mul(&p_InverseViewMatrix, &swapXY, &p_WorldMatrix);
//mat_mul(&swapXY, &p_InverseViewMatrix, &p_WorldMatrix);
//mat_copy(&p_InverseViewMatrix, &p_WorldMatrix);
}
// [RING] Calculate Pseudoinverse
static void calc_pinv(unsigned int Nint, complex float pinv[3][2 * Nint], const complex float A[2 * Nint][3])
{
//AH
complex float AH[3][2 * Nint];
mat_transpose(2 * Nint, 3, AH, A);
// (AH * A)^-1
complex float dot[3][3];
mat_mul(3, 2 * Nint, 3, dot, AH, A);
complex float inv[3][3];
mat_inverse(3, inv, dot);
// (AH * A)^-1 * AH
mat_mul(3, 3, 2 * Nint, pinv, inv, AH);
return;
}
/*
* matrix inverse code for any square matrix using LU decomposition
* inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix
* ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf
* @param m, input 4x4 matrix
* @param inv, Output inverted 4x4 matrix
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool mat_inverse(float *A, float *inv, uint8_t n)
{
float *L, *U, *P;
bool ret = true;
L = new float[n * n];
U = new float[n * n];
P = new float[n * n];
mat_LU_decompose(A, L, U, P, n);
float *L_inv = new float[n * n];
float *U_inv = new float[n * n];
memset(L_inv, 0, n * n * sizeof(float));
mat_forward_sub(L, L_inv, n);
memset(U_inv, 0, n * n * sizeof(float));
mat_back_sub(U, U_inv, n);
// decomposed matrices no longer required
delete[] L;
delete[] U;
float *inv_unpivoted = mat_mul(U_inv, L_inv, n);
float *inv_pivoted = mat_mul(inv_unpivoted, P, n);
//check sanity of results
for (uint8_t i = 0; i < n; i++) {
for (uint8_t j = 0; j < n; j++) {
if (!PX4_ISFINITE(inv_pivoted[i * n + j])) {
ret = false;
}
}
}
memcpy(inv, inv_pivoted, n * n * sizeof(float));
//free memory
delete[] inv_pivoted;
delete[] inv_unpivoted;
delete[] P;
delete[] U_inv;
delete[] L_inv;
return ret;
}
int
anim_play(struct AnimationInstance *anim_inst, float dt)
{
struct Animation *anim = anim_inst->anim;
int n_joints = anim->skeleton->joint_count;
// reset joint processing flags
memset(anim_inst->processed_joints, 0, sizeof(bool) * n_joints);
// compute the relative animation time in ticks, which default to 25
// frames (ticks) per second
anim_inst->time += dt;
float speed = anim->speed != 0 ? anim->speed : 25.0f;
float time_in_ticks = anim_inst->time * speed;
float local_time = fmod(time_in_ticks, anim->duration);
// lookup the key poses indices for given timestamp
size_t key0, key1;
find_poses(anim, local_time, &key0, &key1);
// compute the pose time where t = 0 matches pose 0 and t = 1 pose 1
float t0 = anim->timestamps[key0], t1 = anim->timestamps[key1];
float pose_time = (local_time - t0) / (t1 - t0);
// lookup the key poses
struct SkeletonPose *sp0 = &anim->poses[key0], *sp1 = &anim->poses[key1];
// for each joint, compute its local transformation matrix;
// the process is iterative and keeps track of which joints have already
// their transformations computed, in order to re-use them and skip
// their processing
for (int j = 0; j < n_joints; j++) {
if (!anim_inst->processed_joints[j]) {
joint_compute_pose(
anim, // animation
sp0, // pose before t
sp1, // pose after t
j, // joint index
pose_time, // exact pose time
anim_inst->joint_transforms, // output transforms array
anim_inst->processed_joints // joint processing status array
);
}
}
// compute skinning matrices for each joint
for (int j = 0; j < n_joints; j++) {
mat_mul(
&anim_inst->joint_transforms[j],
&anim->skeleton->joints[j].inv_bind_pose,
&anim_inst->skin_transforms[j]
);
}
return 1;
}
/*
* matrix inverse code for any square matrix using LU decomposition
* inv = inv(U)*inv(L)*P, where L and U are triagular matrices and P the pivot matrix
* ref: http://www.cl.cam.ac.uk/teaching/1314/NumMethods/supporting/mcmaster-kiruba-ludecomp.pdf
* @param m, input 4x4 matrix
* @param inv, Output inverted 4x4 matrix
* @param n, dimension of square matrix
* @returns false = matrix is Singular, true = matrix inversion successful
*/
bool mat_inverse(float* A, float* inv, uint8_t n)
{
float *L, *U, *P;
bool ret = true;
L = new float[n*n];
U = new float[n*n];
P = new float[n*n];
mat_LU_decompose(A,L,U,P,n);
float *L_inv = new float[n*n];
float *U_inv = new float[n*n];
memset(L_inv,0,n*n*sizeof(float));
mat_forward_sub(L,L_inv,n);
memset(U_inv,0,n*n*sizeof(float));
mat_back_sub(U,U_inv,n);
// decomposed matrices no loger required
free(L);
free(U);
float *inv_unpivoted = mat_mul(U_inv,L_inv,n);
float *inv_pivoted = mat_mul(inv_unpivoted, P, n);
//check sanity of results
for(uint8_t i = 0; i < n; i++) {
for(uint8_t j = 0; j < n; j++) {
if(isnan(inv_pivoted[i*n+j]) || isinf(inv_pivoted[i*n+j])){
ret = false;
}
}
}
memcpy(inv,inv_pivoted,n*n*sizeof(float));
//free memory
free(inv_pivoted);
free(inv_unpivoted);
free(P);
free(U_inv);
free(L_inv);
return ret;
}
int main(void)
{
Item x = 5, y = 2, x2 = 6, y2 = 3;
mat m1, m2, k1, k2;
int i, j;
printf("\nItems: ");
item_print(x);
printf(" ");
item_print(y);
printf("\n");
printf("Item Addition: ");
item_print(item_add(x, y));
printf("\n");
printf("Item Multiplication: ");
item_print(item_mul(x, y));
printf("\n");
printf("Item Substraction: ");
item_print(item_sub(x, y));
printf("\n");
printf("Item Division: ");
item_print(item_div(x, y));
printf("\n\n");
m1 = mat_create(3, 2);
m2 = mat_create(2, 3);
k1 = mat_create(2, 2);
k2 = mat_create(2, 2);
for (i = 0; i < mat_rows(m1); i++)
for (j = 0; j < mat_cols(m1); j++)
mat_add_elem(m1, i, j, x);
for (i = 0; i < mat_rows(m2); i++)
for (j = 0; j < mat_cols(m2); j++)
mat_add_elem(m2, i, j, y);
for (i = 0; i < mat_rows(k1); i++)
for (j = 0; j < mat_cols(k1); j++)
{
mat_add_elem(k1, i, j, x2);
mat_add_elem(k2, i, j, y2);
}
printf("k1 Matrix: \n");
mat_print(k1);
printf("k2 Matrix: \n");
mat_print(k2);
printf("Addition: \n");
mat_print(mat_add(k1, k2));
printf("Substraction: \n");
mat_print(mat_sub(k1, k2));
printf("m1 Matrix: \n");
mat_print(m1);
printf("m2 Matrix: \n");
mat_print(m2);
printf("Multiplication: \n");
mat_print(mat_mul(m1, m2));
return 0;
}
static int
child_aabb(struct sprite *s, struct srt *srt, struct matrix * mat, int aabb[4]) {
struct matrix temp;
struct matrix *t = mat_mul(s->t.mat, mat, &temp);
switch (s->type) {
case TYPE_PICTURE:
quad_aabb(s->s.pic, srt, t, aabb);
return 0;
case TYPE_POLYGON:
polygon_aabb(s->s.poly, srt, t, aabb);
return 0;
case TYPE_LABEL:
label_aabb(s->s.label, srt, t, aabb);
return 0;
case TYPE_ANIMATION:
break;
case TYPE_PANNEL:
panel_aabb(s->s.pannel, srt, t, aabb);
return s->data.scissor;
default:
// todo : invalid type
return 0;
}
// draw animation
struct pack_animation *ani = s->s.ani;
int frame = real_frame(s) + s->start_frame;
struct pack_frame * pf = &ani->frame[frame];
int i;
for (i=0;i<pf->n;i++) {
struct pack_part *pp = &pf->part[i];
int index = pp->component_id;
struct sprite * child = s->data.children[index];
if (child == NULL || child->visible == false) {
continue;
}
struct matrix temp2;
struct matrix *ct = mat_mul(pp->t.mat, t, &temp2);
if (child_aabb(child, srt, ct, aabb))
break;
}
return 0;
}
MatP* pivot_world_transform(const struct Pivot* pivot, Mat world_transform) {
Mat translation = {0};
mat_translate(NULL, pivot->position, translation);
Mat rotation = {0};
quat_to_mat(pivot->orientation, rotation);
mat_mul(rotation, translation, world_transform);
return world_transform;
}
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));
}
MatP* pivot_between_transform(const struct Pivot* pivot1, const struct Pivot* pivot2, Mat between_transform) {
Mat local = {0};
pivot_local_transform(pivot2, local);
Mat world = {0};
pivot_world_transform(pivot1, world);
mat_mul(world, local, between_transform);
return between_transform;
}
static void rotateAsteroid(gameComponentT* component, float dt) {
asteroidEntityDataT* asteroid = component->entity->data;
graphicsComponentDataT* gfx = getComponent(component->entity, "graphics")->data;
physicsComponentDataT* phys = getComponent(component->entity, "physics" )->data;
float o = bodyOrientation(phys->body);
//vec2 x = bodyPosition (phys->body);
mat4x4 m, r;
mat_identity (&m);
mat_rot_z (o, &r);
mat_mul (&r, &m, &m);
asteroid->angle1 += dt * asteroid->a1;
mat4x4 lol;
mat_rot(&asteroid->rot_axis_1, asteroid->angle1, &lol);
mat_mul(&lol, &m, &m);
gfx->transform = m;
}
Mat mat_exp(Mat a, unsigned int n){
static Mat ret;
int i, j;
bzero(ret.mat, sizeof(Mat));
for(i = 0; i < N; i++){
for(j = 0; j < N; j++){
ret.mat[i][j] = (i == j);
}
}
while(n){
if(n&1)
ret = mat_mul(ret, a);
a = mat_mul(a, a);
n >>= 1;
}
return ret;
}
// Main get_control function
extern void get_ss02_control(double* measurement, double* control_signal) {
y[0][0] = measurement[0]; // nzCBfwd
y[1][0] = measurement[1]; // nzCBaft
y[2][0] = measurement[2]; // nzLRWTave
// Output Calculation
u = mat_mul(M, y, u);
control_signal[0] = u[0][0]; // L1R1 symmetric [rad]
control_signal[1] = u[1][0]; // L4R4 symmetric [rad]
}
void kalman2(const float *A, const float *Q, const float *R, float *x, const float *z, float *P) {
float x_apriori[2], P_apriori[4], At[4];
float K[4];
float tmp1[4], tmp2[4];
// x_apriori = A*x
mat_mul(A, x, x_apriori, 2, 2, 1);
// P_apriori = A*P_aposteriori*At + Q
mat_mul(A, P, tmp1, 2, 2, 2);
transpose(A, At, 2, 2);
mat_mul(tmp1, At, tmp2, 2, 2, 2);
mat_add(tmp2, Q, P_apriori, 2, 2);
// K = P_apriori(P_apriori + R)^-1
mat_add(P_apriori, R, tmp1, 2, 2);
mat2_inv(tmp1, tmp2);
mat_mul(P_apriori, tmp2, K, 2, 2, 2);
// x_aposteriori = x_apriori + K(z - x_apriori)
mat_sub(z, x_apriori, tmp1, 2, 1);
mat_mul(K, tmp1, tmp2, 2, 2, 1);
mat_add(x_apriori, tmp2, x, 2, 1);
// P_aposteriori = (I - K)P_apriori
mat_sub(I, K, tmp1, 2, 2);
mat_mul(tmp1, P_apriori, P, 2, 2, 2);
}
MatP* pivot_local_transform(const struct Pivot* pivot, Mat local_transform) {
Mat rotation = {0};
quat_invert(pivot->orientation, rotation);
mat_rotate(NULL, rotation, rotation);
Mat translation = {0};
vec_invert(pivot->position, translation);
mat_translate(NULL, translation, translation);
mat_mul(translation, rotation, local_transform);
return local_transform;
}
static void kalman_predict(kalman_t *kf, float a)
{
/* x = A * x + B * u */
kf->u.ve[0] = a;
mat_vec_mul(&kf->t0, &kf->A, &kf->x); /* t0 = A * x */
mat_vec_mul(&kf->t1, &kf->B, &kf->u); /* t1 = B * u */
vec_add(&kf->x, &kf->t0, &kf->t1); /* x = t0 + t1 */
/* P = A * P * AT + Q */
mat_mul(&kf->T0, &kf->A, &kf->P); /* T0 = A * P */
mmtr_mul(&kf->T1, &kf->T0, &kf->A); /* T1 = T0 * AT */
mat_add(&kf->P, &kf->T1, &kf->Q); /* P = T1 * Q */
}
/*-----------------------------------------------------------------------------*/
static MATRIX tasReflectionToQC(tasQEPosition r, MATRIX UB){
MATRIX Q, QC;
Q = makeVector();
if(Q == NULL){
return NULL;
}
vectorSet(Q,0,r.qh);
vectorSet(Q,1,r.qk);
vectorSet(Q,2,r.ql);
QC = mat_mul(UB,Q);
killVector(Q);
return QC;
}
