示例#1
0
文件: pos.c 项目: Aerobota/PenguPilot
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
}
示例#2
0
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, &center);

   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, &center);
   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;
   }
}
示例#3
0
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, &center);

   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, &center);
   mat_mul (&mrot, &pret, &tm);
   mat_mul (&ipret, &imrot, &invtm);
   if (height==0)
   {
	 angle = 0.0f;
	 angleOverHeight = 0.0f;
   }
   else angleOverHeight = angle / height;
}
示例#4
0
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(&currentmatrix_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, &currentmatrix_rot);
   }
   mat_copy(&currentmatrix_rot, &currentmatrix);

   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, &currentpos);
   mat_setmatrix_of_pretraslation(&posttrans, &currentpos);

   mat_mul(&scaling, &currentmatrix_rot, &maux);
   mat_mul(&posttrans, &maux, &currentmatrix);
 
   if (father!=(AD_Object3D *)NULL)
   {
     mat_mulaffine(&father->currentmatrix_rot, &currentmatrix_rot, &currentmatrix_rot);
	 mat_mul(&father->currentmatrix, &currentmatrix, &currentmatrix);
     mat_mulvect(&father->currentmatrix, &currentpos, &postmp);
	 vect_copy(&postmp, &currentpos);
	 
	 accum_scale.x*=father->accum_scale.x;
	 accum_scale.y*=father->accum_scale.y;
	 accum_scale.z*=father->accum_scale.z;
   }

   mat_transpose(&currentmatrix_rot, &inverse_rotmatrix);
   mat_get_row(&currentmatrix, 1, &forza);
   vect_auto_normalize(&forza);
   vect_scale(&forza, strenght*0.00016f*forceScaleFactor, &forza);
}
示例#5
0
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);
}
示例#6
0
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(&currentmatrix_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, &currentmatrix_rot);
   }
   mat_copy(&currentmatrix_rot, &currentmatrix);

   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, &currentpos);
   mat_setmatrix_of_pretraslation(&posttrans, &currentpos);

   mat_mul(&scaling, &currentmatrix_rot, &maux);
   mat_mul(&posttrans, &maux, &currentmatrix);
 
   if (father!=(AD_Object3D *)NULL)
   {
     mat_mulaffine(&father->currentmatrix_rot, &currentmatrix_rot, &currentmatrix_rot);
	 mat_mul(&father->currentmatrix, &currentmatrix, &currentmatrix);
     mat_mulvect(&father->currentmatrix, &currentpos, &postmp);
	 vect_copy(&postmp, &currentpos);
	 
	 accum_scale.x*=father->accum_scale.x;
	 accum_scale.y*=father->accum_scale.y;
	 accum_scale.z*=father->accum_scale.z;
   }

   mat_transpose(&currentmatrix_rot, &inverse_rotmatrix);
}
示例#7
0
// 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);

}
示例#8
0
文件: matrix.c 项目: dfyockey/vulcan
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);
}
示例#9
0
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);
}
示例#10
0
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));
}
示例#11
0
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);
}
示例#12
0
/**
 * 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;
}
示例#13
0
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);
}
示例#14
0
文件: estdelay.c 项目: mrirecon/bart
// [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;
}
示例#15
0
/*
 *    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;
}
示例#16
0
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;
}
示例#17
0
/*
 *    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;
}
示例#19
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;
}
示例#20
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;
}
示例#21
0
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));
}
示例#22
0
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));
}
示例#23
0
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;
}
示例#24
0
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;
}
示例#25
0
文件: mat_mul.c 项目: 91he/Test
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;
}
示例#26
0
// 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]
}
示例#27
0
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);
}
示例#28
0
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;
}
示例#29
0
文件: pos.c 项目: Aerobota/PenguPilot
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 */
}
示例#30
0
/*-----------------------------------------------------------------------------*/
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;
}