コード例 #1
0
ファイル: matpca.c プロジェクト: caomw/matrixlab
MATSTACK mat_eig_sym(MATRIX symmat, MATSTACK result)
{
    int m, n;
    MATRIX im, tmp_result0 = NULL, tmp_result1 = NULL;
    INT_VECTOR indcs = NULL;
    MATSTACK tmp = NULL;
    m = MatCol(symmat);
    n = MatRow(symmat);
    if(m!=n) mat_error(MAT_SIZEMISMATCH);
    if(result==NULL)
    {
        if ((result = matstack_creat(2)) == NULL)
            return matstack_error(MATSTACK_MALLOC);
        result[0] = NULL;
        result[1] = NULL;
    }
    im = mat_creat(m, 1, UNDEFINED);
    tmp_result0 = mat_creat(m, 1, UNDEFINED);
    tmp_result1 = mat_copy(symmat, tmp_result1);
    mat_tred2(tmp_result1, tmp_result0, im);
    mat_tqli(tmp_result0, im, tmp_result1);

    tmp = mat_qsort(tmp_result0, ROWS, tmp);
    result[0] = mat_copy(tmp[0], result[0]);
    indcs = mat_2int_vec(tmp[1]);
    result[1] = mat_get_sub_matrix_from_cols(tmp_result1, indcs, result[1]);
    int_vec_free(indcs);
    mat_free(im);
    mat_free(tmp_result0);
    mat_free(tmp_result1);

    return result;
}
コード例 #2
0
ファイル: 5-7-n4_matrix.c プロジェクト: myuuuuun/classwork
// 掛け算
int mat_mul(matrix *mat1, matrix mat2, matrix mat3){
  int i, j, k;
  matrix mat2_w, mat3_w;
  
  if(check_mul(*mat1, mat2, mat3) != 0){
    printf("%s\n", "エラーが発生しました。");
    return -1;
  }

  mat_alloc(&mat2_w, mat2.row, mat2.col);
  mat_alloc(&mat3_w, mat3.row, mat3.col);
  mat_copy(&mat2_w, mat2);
  mat_copy(&mat3_w, mat3);
  
  for(i=0; i < mat1->row; i++){
    for(j=0; j < mat1->col; j++){
      mat1->element[i][j] = 0;

      for(k=0; k<mat2_w.col; k++){
        mat1->element[i][j] += mat2_w.element[i][k] * mat3_w.element[k][j];
      }
    }
  }

  mat_free(&mat2_w);
  mat_free(&mat3_w);
  
  return 0;
}
コード例 #3
0
ファイル: matrix.c プロジェクト: PAAW/mAEWing1
/*
*-----------------------------------------------------------------------------
*	funct:	mat_lsolve
*	desct:	solve linear equations
*	given:	a = square matrix A
*		b = column matrix B
*	retrn:	column matrix X (of AX = B)
*-----------------------------------------------------------------------------
*/
MATRIX mat_lsolve(MATRIX a,MATRIX b,MATRIX X)
{
	MATRIX	A, B, P;
	int n;

	n = MatCol(a);
	if ( ( A = mat_creat(n, n, UNDEFINED) ) == NULL )
		return (NULL);
	if ( ( B = mat_creat(n, 1, UNDEFINED) ) == NULL )
		return (NULL);
	mat_copy(a,A);
	mat_copy(b,B);
	if ( ( P = mat_creat(n, 1, UNDEFINED) ) == NULL )
		return (NULL);

	// if dimensions of C is wrong
	if ( MatRow(X) != n || MatCol(X) != 1 ) {
		printf("mat_lsolve error: incompatible output matrix size\n");
	    _exit(-1);
	// if dimensions of C is correct
	} else {

		mat_lu( A, P );
		mat_backsubs1( A, B, X, P, 0 );
	}
	mat_free(A);
	mat_free(B);
	mat_free(P);

	return(X);
}
コード例 #4
0
ファイル: matconcat.c プロジェクト: mohammadul/matrixlab
MATRIX mat_concat(MATRIX A, MATRIX B, int dim)
{
    int i, j, m, n, o, p;
    MATRIX result;
    if(A==NULL)
    {
        return mat_copy(B, NULL);
    }
    else
    {
        m = MatCol(A);
        n = MatRow(A);
    }
    if(B==NULL)
    {
        return mat_copy(A, NULL);
    }
    else
    {
        o = MatCol(B);
        p = MatRow(B);
    }
    if((dim==ROWS)&&((m==o) ||!((m==0)&&(o==0))))
    {
        if((result = mat_creat(n+p, m, UNDEFINED))==NULL) return NULL;
        #pragma omp parallel for private(j)
        for(i=0; i<n; ++i)
        {
            for(j=0; j<m; ++j)
            {
                result[i][j] = A[i][j];
            }
        }
        #pragma omp parallel for private(j)
        for(i=0; i<p; ++i)
        {
            for(j=0; j<m; ++j)
            {
                result[i+n][j] = B[i][j];
            }
        }
        return result;
    }
    if((dim==COLS)&&((n==p) ||!((n==0)&&(p==0))))
    {
        if((result = mat_creat(n, m+o, UNDEFINED))==NULL) return NULL;
        #pragma omp parallel for private(j)
        for(i=0; i<n; ++i)
        {
            for(j=0; j<m; ++j) result[i][j] = A[i][j];
            for(j=0; j<o; ++j) result[i][j+m] = B[i][j];
        }
        return result;
    }
    return mat_error(MAT_SIZEMISMATCH);
}
コード例 #5
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
}
コード例 #6
0
ファイル: model-iqm.c プロジェクト: mksn/objview
static void
_load_bones (struct ov_skeleton *s,
             unsigned char       *data,
             struct _iqm_header  *h)
{
    struct _iqm_joint *j = malloc(h->num_joints * sizeof(struct _iqm_joint));
    memcpy(j, data + h->ofs_joints, h->num_joints * sizeof(struct _iqm_joint));
    s->num_bones = h->num_joints;
    s->bones = malloc(h->num_joints * sizeof(struct iqm_bone));
    int i;
    for (i=0; i<h->num_joints; i++) {
        float q[16];
        char *name = (char *)(data + h->ofs_text + j[i].name);
        
        s->bones[i].parent = j[i].parent;
        s->bones[i].name = malloc(strlen(name)*sizeof(char)+1);
        strcpy(s->bones[i].name, name);
        read_pose(&s->bones[i].bind_pose, &j[i]);
        mat_from_pose(q,
                      s->bones[i].bind_pose.translate,
                      s->bones[i].bind_pose.rotate,
                      s->bones[i].bind_pose.scale);
        if (s->bones[i].parent >= 0) {
            struct ov_bone *parent = &s->bones[s->bones[i].parent];
            mat_mul44(s->bones[i].bind_matrix, parent->bind_matrix, q);
        } else {
            mat_copy(s->bones[i].bind_matrix, q);
        }
        mat_invert(s->bones[i].inv_bind_matrix, s->bones[i].bind_matrix);
    }
    free (j);
}
コード例 #7
0
int32 CGeometricObject::m_Clone(CGeometricObject *source)
{
   if (!source) return(0);

   strcpy(p_FatherName, source->p_FatherName);
   
   p_Father=source->p_Father;
   p_HasChildrens=source->p_HasChildrens;

   p_BaseMaterial=source->p_BaseMaterial;
   for (int32 i=0; i<MAX_LODS; i++)
	  p_Lods[i]=source->p_Lods[i];
   p_NumLods=source->p_NumLods;

   p_VertexBuffer=source->p_VertexBuffer;
   p_StaticVertex=source->p_StaticVertex;

   p_PosTrack=source->p_PosTrack;
   p_RotTrack=source->p_RotTrack;
   p_ScaleTrack=source->p_ScaleTrack;

   vect_copy(&source->p_Pivot, &p_Pivot);
   vect_copy(&source->p_CurrentPosition, &p_CurrentPosition);
   quat_copy(&source->p_CurrentRotationQuaternion, &p_CurrentRotationQuaternion);
   mat_copy(&source->p_CurrentRotationMatrix, &p_CurrentRotationMatrix);
   vect_copy(&source->p_CurrentScale, &p_CurrentScale);
   vect_copy(&source->p_TotalScale, &p_TotalScale);
   mat_copy(&source->p_WorldMatrix, &p_WorldMatrix);

   if (source->p_SPHEREBoundVolume)
   {
      p_SPHEREBoundVolume=new CBoundVolume;
      source->p_SPHEREBoundVolume->m_Copy(CLONE, p_SPHEREBoundVolume);
   }
   if (source->p_AABBBoundVolume)
   {
      p_AABBBoundVolume=new CBoundVolume;
      source->p_AABBBoundVolume->m_Copy(CLONE, p_AABBBoundVolume);
   }
   if (source->p_OBBBoundVolume)
   {
      p_OBBBoundVolume=new CBoundVolume;
      source->p_OBBBoundVolume->m_Copy(CLONE, p_OBBBoundVolume);
   }
   return(1);
}
コード例 #8
0
ファイル: blocked_plaq.c プロジェクト: daschaich/susy
void blocked_plaq(int Nsmear, int block) {
  register int i;
  register site *s;
  int j, stride = 1;
  double plaq = 0.0, plaqSq = 0.0, re = 0.0, reSq = 0.0, im = 0.0, imSq = 0.0;
  double sum = 0.0, norm = 1.0 / volume, tr;
  complex det = cmplx(0.0, 0.0), tc;
  matrix tmat, tmat2, tmat3;

  // Set number of links to stride, 2^block
  for (j = 0; j < block; j++)
    stride *= 2;

  // Compute the strided plaquette, exploiting a symmetry under TUP<-->XUP
  // Copy links to tempmat and tempmat2 to be shifted
  FORALLSITES(i, s) {
    mat_copy(&(s->link[TUP]), &(tempmat[i]));
    mat_copy(&(s->link[XUP]), &(tempmat2[i]));
  }
コード例 #9
0
ファイル: kalman.c プロジェクト: Colyette/P15230
static void take_inverse( m_elem **in, m_elem **out, int n )
{
#ifdef PRINT_DEBUG
  printf( "ekf: calculating inverse\n" );
#endif
  /*  Nothing fancy for now, just a Gauss-Jordan technique,
      with good pivoting (thanks to NR).     */

  gaussj( in, n, out, 0 );  /* out is SCRATCH  */
  mat_copy( in, out, n, n );
}
コード例 #10
0
ファイル: kalman.c プロジェクト: Colyette/P15230
void extended_kalman_init( m_elem **GQGt, m_elem **R, m_elem **P, m_elem *x,
              int num_state, int num_measurement )
{
#ifdef PRINT_DEBUG
  printf( "ekf: Initializing filter\n" );
#endif

  alloc_globals( num_state, num_measurement );

  sys_transfer = matrix( 1, num_state, 1, num_state );
  mea_transfer = matrix( 1, num_measurement, 1, num_state );

  /*  Init the global variables using the arguments.  */

  vec_copy( x, state_post, state_size );
  vec_copy( x, state_pre, state_size );
  mat_copy( P, cov_post, state_size, state_size );
  mat_copy( P, cov_pre, state_size, state_size );

  sys_noise_cov = GQGt;
  mea_noise_cov = R;
}
コード例 #11
0
ファイル: matrix.c プロジェクト: PAAW/mAEWing1
/*
*-----------------------------------------------------------------------------
*	funct:	mat_inv
*	desct:	find inverse of a matrix
*	given:	a = square matrix a
*	retrn:	square matrix Inverse(A)
*		NULL = fails, singular matrix, or malloc() fails
*		1 = success
*-----------------------------------------------------------------------------
*/
MATRIX mat_inv(MATRIX a, MATRIX C)
{
	MATRIX	A, B, P;
	int		i, n;

	n = MatCol(a);
	if ( (	A = mat_creat( n, n, UNDEFINED ) ) == NULL )
		return (NULL);
	mat_copy(a,A);
	if ( ( B = mat_creat( n, 1, UNDEFINED ) ) == NULL )
		return (NULL);
	if ( ( P = mat_creat( n, 1, UNDEFINED ) ) == NULL )
		return (NULL);

	// if dimensions of C is wrong
	if ( MatRow(a) != MatRow(C) || MatCol(a) != MatCol(C) ) {
		printf("mat_inv error: incompatible output matrix size\n");
		_exit(-1);
	// if dimensions of C is correct
	} else {
		/*
		*	- LU-decomposition -
		*	also check for singular matrix
		*/
		if (mat_lu(A, P) == -1) {
			mat_free(A);
			mat_free(B);
			mat_free(C);
			mat_free(P);
			printf("mat_inv error: failed to invert\n");
			return (NULL);
		}

		for (i=0; i<n; i++) {
			mat_fill(B, ZERO_MATRIX);
			B[i][0] = 1.0;
			mat_backsubs1( A, B, C, P, i );
		}
	}

	mat_free(A);
	mat_free(B);
	mat_free(P);

	if (C==NULL) {
			printf("mat_inv error: failed to invert\n");
			return(NULL);
	} else {
		return (C);
	}
}
コード例 #12
0
ファイル: matrix.c プロジェクト: djachoc/R-Package-npRmpi
/*
 *-----------------------------------------------------------------------------
 * funct:  mat_det
 * desct:  find determinant
 * given:  A = matrix
 * retrn:  the determinant of A
 * comen:
 *-----------------------------------------------------------------------------
 */
double mat_det( MATRIX a )
{
	MATRIX  A, P;
	int  j;
	int i, n;
	double  result;

	n = MatRow(a);
	A = mat_creat(MatRow(a), MatCol(a), UNDEFINED);
	A = mat_copy(a, A);
	P = mat_creat(n, 1, UNDEFINED);



/*
 * take a LUP-decomposition
 */
	i = mat_lu(A, P);
	switch (i)
	{


/*
 * case for singular matrix
 */
		case -1:
			result = 0.0;
			break;



/*
 * normal case: |A| = |L||U||P|
 * |L| = 1,
 * |U| = multiplication of the diagonal
 * |P| = +-1
 */
		default:
			result = 1.0;
			for (j=0; j<MatRow(A); j++)
			{
				result *= A[(int)P[j][0]][j];
			}
			result *= signa[i%2];
			break;
	}

	mat_free(A);
	mat_free(P);
	return (result);
}
コード例 #13
0
ファイル: WSMobj.cpp プロジェクト: ApocalypseDesign/ad_public
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);
}
コード例 #14
0
ファイル: patch.cpp プロジェクト: ApocalypseDesign/ad_public
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);
}
コード例 #15
0
ファイル: block_N_fatten.c プロジェクト: daschaich/KS_nHYP_FA
void block_N_fatten(int N) {
  register int i, dir;
  register site* s;
  int j;

  block_and_fatten();
  if (N > 1) {
    // Save unsmeared links in gauge_field_save
    for (dir = 0; dir < 4; dir++) {
      FORALLSITES(i, s)
        mat_copy(gauge_field_thin[dir] + i, gauge_field_save[dir] + i);
    }

    // block_and_fatten starts by copying s->link into gauge_field_thin
    for (j = 1; j < N; j++)
      block_and_fatten();

    // Reset gauge_field_thin to hold unsmeared links
    for (dir = 0; dir < 4; dir++) {
      FORALLSITES(i, s)
        mat_copy(gauge_field_save[dir] + i, gauge_field_thin[dir] + i);
    }
  }
}
コード例 #16
0
ファイル: nav_functions.c プロジェクト: Jmcjack/hasp
void EcefToEnu(MATRIX outputVector, MATRIX inputVector, MATRIX position)
{

  int i;
  double lat, lon;
  MATRIX C, ned, ref_position;
  MATRIX position_copy, delta_pos;

  C = mat_creat(3,3,ZERO_MATRIX);
  ref_position = mat_creat(3,1,ZERO_MATRIX);
  delta_pos = mat_creat(3,1,ZERO_MATRIX);
  position_copy = mat_creat(MatRow(position),MatCol(position),ZERO_MATRIX);
  mat_copy(position, position_copy);

  lat = position[0][0];
  lon = position[1][0];

  LatLonAltToEcef(ref_position,position_copy);

  mat_sub(inputVector,ref_position,delta_pos);

  C[0][0] = -sin(lon);
  C[0][1] = cos(lon);
  C[0][2] = 0;

  C[1][0] = -sin(lat)*cos(lon);
  C[1][1] = -sin(lat)*sin(lon);
  C[1][2] = cos(lat);

  C[2][0] = cos(lat)*cos(lon);
  C[2][1] = cos(lat)*sin(lon);
  C[2][2] = sin(lat);

  ned = mat_creat(MatRow(C),MatCol(delta_pos),ZERO_MATRIX);
  mat_mul(C,delta_pos,ned);

  for (i = 0; i < 3; i++)
    {
      outputVector[i][0] = ned[i][0];
    }

  mat_free(ned);
  mat_free(C);
  mat_free(delta_pos);
  mat_free(ref_position);
  mat_free(position_copy);

}
コード例 #17
0
void CGeometricObject::m_BuildWorldMatrix(void)
{
   AD_Vect3D postmp;
   AD_Matrix posttrans, scaling, maux;
   AD_Matrix iRot, iScale, iTrans;

   // matrice di rotazione e sua inversa
   quat_rotquat_to_matrix(&p_CurrentRotationQuaternion, &p_CurrentRotationMatrix);
   mat_copy(&p_CurrentRotationMatrix, &p_WorldMatrix);
   mat_transpose(&p_CurrentRotationMatrix, &iRot);
   
   // matrice di scaling e sua inversa
   mat_setmatrix_of_scaling(&scaling, p_CurrentScale.x, p_CurrentScale.y, p_CurrentScale.z);
   mat_setmatrix_of_scaling(&iScale, 1.0f/p_CurrentScale.x, 1.0f/p_CurrentScale.y, 1.0f/p_CurrentScale.z);

   // matrice di traslazione e sua inversa
   mat_setmatrix_of_pretraslation(&posttrans, &p_CurrentPosition);
   vect_neg(&p_CurrentPosition, &postmp);
   mat_setmatrix_of_pretraslation(&iTrans, &postmp);

   // prima pivot, poi scaling, poi rotazione, e infine traslazione
   //vect_neg(&p_Pivot, &postmp);
   //mat_setmatrix_of_pretraslation(&pivot, &postmp);
   //mat_mul(&scaling, &pivot, &maux);
   //mat_mul(&p_CurrentRotationMatrix, &maux, &maux);
   mat_mul(&p_CurrentRotationMatrix, &scaling, &maux);
   mat_mul(&posttrans, &maux, &p_WorldMatrix);

   // per la inversa: prima traslazione, rotazione, scaling e pivot
   //mat_setmatrix_of_pretraslation(&iPivot, &p_Pivot);
   mat_mul(&iRot, &iTrans, &maux);
   mat_mul(&iScale, &maux, &p_InverseWorldMatrix);
   //mat_mul(&iScale, &maux, &maux);
   //mat_mul(&iPivot, &maux, &p_InverseWorldMatrix);

   if (p_Father)
   {
     mat_mulaffine(&p_Father->p_CurrentRotationMatrix,
                   &p_CurrentRotationMatrix,
                   &p_CurrentRotationMatrix);
     mat_mul(&p_Father->p_WorldMatrix, &p_WorldMatrix, &p_WorldMatrix);
     mat_mul(&p_InverseWorldMatrix, &p_Father->p_InverseWorldMatrix, &p_InverseWorldMatrix);
     p_TotalScale.x*=p_Father->p_TotalScale.x;
     p_TotalScale.y*=p_Father->p_TotalScale.y;
     p_TotalScale.z*=p_Father->p_TotalScale.z;
   }
}
コード例 #18
0
ファイル: hvy_pot_polar.c プロジェクト: rgjha/susy
void hvy_pot_polar() {
  register int i;
  register site *s;
  int j, t_dist, x_dist, y_dist, z_dist, y_start, z_start;
  double wloop;
  matrix tmat, tmat2, *mat;
  msg_tag *mtag = NULL;

  node0_printf("hvy_pot_polar: MAX_T = %d, MAX_X = %d\n", MAX_T, MAX_X);

  FORALLSITES(i, s) {
   // Polar projection of gauge-fixed links
   // To be multiplied together after projecting
   // !!! Overwrites links
   polar(&(s->link[TUP]), &tmat, &tmat2);
   mat_copy(&tmat, &(s->link[TUP]));
  }
コード例 #19
0
ファイル: stout_step_rk.c プロジェクト: daschaich/KS_nHYP_FA
// -----------------------------------------------------------------
// Calculate U = exp(A).U
// Goes to eighth order in the exponential:
//   exp(A) * U = ( 1 + A + A^2/2 + A^3/3 ...) * U
//              = U + A*(U + (A/2)*(U + (A/3)*( ... )))
void exp_mult(int dir, double eps, anti_hermitmat *A) {
  register int i;
  register site *s;
  matrix *link, temp1, temp2, htemp;
  register Real t2, t3, t4, t5, t6, t7, t8;

  // Take divisions out of site loop (can't be done by compiler)
  t2 = eps / 2.0;
  t3 = eps / 3.0;
  t4 = eps / 4.0;
  t5 = eps / 5.0;
  t6 = eps / 6.0;
  t7 = eps / 7.0;
  t8 = eps / 8.0;

  FORALLSITES(i, s) {
    uncompress_anti_hermitian(&(A[i]), &htemp);
    link = &(s->link[dir]);

    mult_nn(&htemp, link, &temp1);
    scalar_mult_add_matrix(link, &temp1, t8, &temp2);

    mult_nn(&htemp, &temp2, &temp1);
    scalar_mult_add_matrix(link, &temp1, t7, &temp2);

    mult_nn(&htemp, &temp2, &temp1);
    scalar_mult_add_matrix(link, &temp1, t6, &temp2);

    mult_nn(&htemp, &temp2, &temp1);
    scalar_mult_add_matrix(link, &temp1, t5, &temp2);

    mult_nn(&htemp, &temp2, &temp1);
    scalar_mult_add_matrix(link, &temp1, t4, &temp2);

    mult_nn(&htemp, &temp2, &temp1);
    scalar_mult_add_matrix(link, &temp1, t3, &temp2);

    mult_nn(&htemp, &temp2, &temp1);
    scalar_mult_add_matrix(link, &temp1, t2, &temp2);

    mult_nn(&htemp, &temp2, &temp1);
    scalar_mult_add_matrix(link, &temp1, eps, &temp2);

    mat_copy(&temp2, link);    // This step updates the link U[dir]
  }
コード例 #20
0
ファイル: kalman.c プロジェクト: Colyette/P15230
void kalman_init( m_elem **GQGt, m_elem **Phi, m_elem **H, m_elem **R,
        m_elem **P, m_elem *x, int num_state, int num_measurement )
{
  alloc_globals( num_state, num_measurement );

  /*  Init the global variables using the arguments.  */

  vec_copy( x, state_post, state_size );
  mat_copy( P, cov_post, state_size, state_size );

  sys_noise_cov = GQGt;
  mea_noise_cov = R;

  sys_transfer = Phi;
  mea_transfer = H;

  /*****************  Gain Loop  *****************/

  estimate_prob( cov_post, sys_transfer, sys_noise_cov, cov_pre );
  update_prob( cov_pre, mea_noise_cov, mea_transfer, cov_post, kalman_gain );
}
コード例 #21
0
ファイル: 5-7-n4_matrix.c プロジェクト: myuuuuun/classwork
// 転置行列を返す
int mat_trans(matrix *mat1, matrix mat2){
  int i, j;
  matrix mat2_w;

  if(check_trans(*mat1, mat2) != 0){
    printf("%s\n", "エラーが発生しました。");
    return -1;
  }

  mat_alloc(&mat2_w, mat2.row, mat2.col);
  mat_copy(&mat2_w, mat2);
  
  for(i=0; i < mat1->row; i++){
    for(j=0; j < mat1->col; j++){
      mat1->element[i][j] = mat2_w.element[j][i];
    }
  }

  mat_free(&mat2_w);

  return 0;
}
コード例 #22
0
ファイル: matrix.c プロジェクト: ktakasu/mtfc1.c
//逆行列(はき出し法)a_oriの逆行列
struct matrix *mat_inv(struct matrix *a_ori){
  int i,j,k,m,n;
  double d,dd;
  struct matrix *a;
  struct matrix *a_inv;

  if(a_ori==NULL){ printf("error at mat_inv:input is NULL\n");return NULL;}
  if(a_ori->row!=a_ori->col){printf("error at mat_inv:a_ori\n");return NULL;}
  a_inv=mat_alloc(a_ori->row,a_ori->col);

  a=mat_copy(a_ori);
  for(i=0;i<a->row;i++){
    for(j=0;j<a->row;j++){
      if(i==j)*(a_inv->element+j+i*a->col)=1;
      else *(a_inv->element+j+i*a->col)=0;
    }
  }

  for(i=0;i<a->row;i++){
    d=*(a->element+i+i*a->col);
    for(j=0;j<a->row;j++){
      *(a->element+j+i*a->col)=(1/d)* *(a->element+j+i*a->col);
      *(a_inv->element+j+i*a->col)=(1/d)* *(a_inv->element+j+i*a->col);
    }
    for(k=0;k<a->row;k++){
      if(i!=k){
	dd=*(a->element+i+k*a->col);
	for(n=0;n<a->row;n++){
          *(a->element+n+k*a->col)=
	    *(a->element+n+k*a->col)- dd * *(a->element+n+i*a->col);
	  *(a_inv->element+n+k*a->col)=
	    *(a_inv->element+n+k*a->col)-dd * *(a_inv->element+n+i*a->col);
	}
      }
    }
  }
  return(a_inv);
}
コード例 #23
0
void cvx_clustering ( double ** dist_mat, int fw_max_iter, int max_iter, int D, int N, double* lambda, double ** W) {
    // parameters
    double alpha = 0.1;
    double rho = 1;
    // iterative optimization
    double error = INF;
    double ** wone = mat_init (N, N);
    double ** wtwo = mat_init (N, N);
    double ** yone = mat_init (N, N);
    double ** ytwo = mat_init (N, N);
    double ** z = mat_init (N, N);
    double ** diffzero = mat_init (N, N);
    double ** diffone = mat_init (N, N);
    double ** difftwo = mat_init (N, N);
    mat_zeros (yone, N, N);
    mat_zeros (ytwo, N, N);
    mat_zeros (z, N, N);
    mat_zeros (diffzero, N, N);
    mat_zeros (diffone, N, N);
    mat_zeros (difftwo, N, N);

    int iter = 0; // Ian: usually we count up (instead of count down)
    while ( iter < max_iter ) { // stopping criteria

#ifdef SPARSE_CLUSTERING_DUMP
        cout << "it is place 0 iteration #" << iter << ", going to get into frank_wolfe"  << endl;
#endif
        // mat_set (wone, z, N, N);
        // mat_set (wtwo, z, N, N);
        // mat_zeros (wone, N, N);
        // mat_zeros (wtwo, N, N);
        // STEP ONE: resolve w_1 and w_2
        frank_wolf (dist_mat, yone, z, wone, rho, N, fw_max_iter); // for w_1
#ifdef SPARSE_CLUSTERING_DUMP
        cout << "norm2(w_1) = " << mat_norm2 (wone, N, N) << endl;
#endif

        blockwise_closed_form (ytwo, z, wtwo, rho, lambda, N);  // for w_2
#ifdef SPARSE_CLUSTERING_DUMP
        cout << "norm2(w_2) = " << mat_norm2 (wtwo, N, N) << endl;
#endif

        // STEP TWO: update z by averaging w_1 and w_2
        mat_add (wone, wtwo, z, N, N);
        mat_dot (0.5, z, z, N, N);
#ifdef SPARSE_CLUSTERING_DUMP
        cout << "norm2(z) = " << mat_norm2 (z, N, N) << endl;
#endif

        // STEP THREE: update the y_1 and y_2 by w_1, w_2 and z

        mat_sub (wone, z, diffone, N, N);
        double trace_wone_minus_z = mat_norm2 (diffone, N, N);
        mat_dot (alpha, diffone, diffone, N, N);
        mat_add (yone, diffone, yone, N, N);

        mat_sub (wtwo, z, difftwo, N, N);
        //double trace_wtwo_minus_z = mat_norm2 (difftwo, N, N);
        mat_dot (alpha, difftwo, difftwo, N, N);
        mat_add (ytwo, difftwo, ytwo, N, N);

        // STEP FOUR: trace the objective function
        if (iter % 100 == 0) {
            error = overall_objective (dist_mat, lambda, N, z);
            // get current number of employed centroid
#ifdef NCENTROID_DUMP
            int nCentroids = get_nCentroids (z, N, N);
#endif
            cout << "[Overall] iter = " << iter
                 << ", Overall Error: " << error
#ifdef NCENTROID_DUMP
                 << ", nCentroids: " << nCentroids
#endif
                 << endl;
        }
        iter ++;
    }

    // STEP FIVE: memory recollection
    mat_free (wone, N, N);
    mat_free (wtwo, N, N);
    mat_free (yone, N, N);
    mat_free (ytwo, N, N);
    mat_free (diffone, N, N);
    mat_free (difftwo, N, N);
    // STEP SIX: put converged solution to destination W
    mat_copy (z, W, N, N);
}
コード例 #24
0
int main()
{
  int i, j;
  int w, h;
  int levels, ct_levels, wt_levels,level_init;
  double rate,rate_init;
  ivec dfb_levels;
  mat source, dest;
  contourlet_t *contourlet;
  mat wavelet;
  int length;
  unsigned char *buffer;

  //³õʼ»¯²ÎÊý
  int argc=6;
      rate_init=2;
	  level_init=5;


#define LEVELS 5
#define IMPULSE 100.

  source = mat_pgm_read("1.pgm");
  h = mat_height(source);
  w = mat_width(source);
  dest = mat_new(w, h);
  rate = rate_init * w * h;
  levels = level_init;
  ct_levels = argc - 4;              /* contourlet levels */
  wt_levels = levels - ct_levels;    /* wavelet levels */
  dfb_levels = ivec_new(ct_levels);
  for(i = 0; i < ct_levels; i++)
    dfb_levels[i] = 4+i;


  buffer = bvec_new_zeros(BUFFER_SIZE);

  contourlet = contourlet_new(ct_levels, dfb_levels);
  contourlet->wt_levels = wt_levels;

  contourlet_transform(contourlet, source);
  wavelet = it_dwt2D(contourlet->low, it_wavelet_lifting_97, wt_levels);
  contourlet->dwt = it_wavelet2D_split(wavelet, wt_levels);

  /* normalize the subbands */
  for(i = 0; i < ct_levels; i++)
    for(j = 0; j < (1 << dfb_levels[i]); j++)
      mat_mul_by(contourlet->high[i][j], norm_high[1+i][dfb_levels[i]][j]);
  mat_mul_by(contourlet->low, norm_low[ct_levels]);

  /* make flat images */
  mat_pgm_write("dwt.pgm", wavelet);
  for(i = 0; i < ct_levels; i++) {
    char filename[256];

    mat dfb_rec = mat_new((h >> i) + 1, (w >> i) + 1);
    if(dfb_levels[i])
      dfb_flatten(contourlet->high[i], dfb_rec, dfb_levels[i]);
    else
      mat_set_submatrix(dfb_rec, contourlet->high[i][0], 0, 0);
    mat_incr(dfb_rec, 128);
    sprintf(filename, "dfb%d.pgm", i);
    mat_pgm_write(filename, dfb_rec);
    mat_decr(dfb_rec, 128);
    mat_delete(dfb_rec);
  }

  /* EZBC encoding */
  length = ezbc_encode(contourlet, buffer, BUFFER_SIZE, rate);

  /* EZBC decoding */
  ezbc_decode(contourlet, buffer, BUFFER_SIZE, rate);

  mat_pgm_write("rec_low.pgm", contourlet->dwt[0]);

  /* make flat images */
  for(i = 0; i < ct_levels; i++) {
    char filename[256];

    mat dfb_rec = mat_new((h >> i) + 1, (w >> i) + 1);
    if(dfb_levels[i])
      dfb_flatten(contourlet->high[i], dfb_rec, dfb_levels[i]);
    else
      mat_set_submatrix(dfb_rec, contourlet->high[i][0], 0, 0);
    mat_incr(dfb_rec, 128);
    sprintf(filename, "rec_dfb%d.pgm", i);
    mat_pgm_write(filename, dfb_rec);
    mat_decr(dfb_rec, 128);
    mat_delete(dfb_rec);
  }

  /* normalize the subbands */
  for(i = 0; i < ct_levels; i++)
    for(j = 0; j < (1 << dfb_levels[i]); j++)
      mat_div_by(contourlet->high[i][j], norm_high[1+i][dfb_levels[i]][j]);
  mat_div_by(contourlet->low, norm_low[ct_levels]);


  //  mat_pgm_write("rec_low.pgm", contourlet->dwt[0]);

  /* TODO: fix this in libit */
  if(wt_levels)
    wavelet = it_wavelet2D_merge(contourlet->dwt, wt_levels);
  else
    mat_copy(wavelet, contourlet->dwt[0]);

  mat_pgm_write("rec_dwt.pgm", wavelet);

  contourlet->low = it_idwt2D(wavelet, it_wavelet_lifting_97, wt_levels);

  contourlet_itransform(contourlet, dest);

  contourlet_delete(contourlet);

  mat_pgm_write("rec.pgm", dest);

  printf("rate = %f PSNR = %f\n", length * 8. / (w*h), 10*log10(255*255/mat_distance_mse(source, dest, 0)));

  ivec_delete(dfb_levels);
  mat_delete(dest);
  mat_delete(source);
  bvec_delete(buffer);

  return(0);
}
コード例 #25
0
ファイル: random_gauge_trans.c プロジェクト: daschaich/susy
void random_gauge_trans(Twist_Fermion *TF) {
  int a, b, i, j, x = 1, t = 1, s = node_index(x, t);
  complex tc;
  matrix Gmat, tmat, etamat, psimat[NUMLINK], chimat;

  if (this_node != 0) {
    printf("random_gauge_trans: only implemented in serial so far\n");
    fflush(stdout);
    terminate(1);
  }
  if (nx < 4 || nt < 4) {
    printf("random_gauge_trans: doesn't deal with boundaries, ");
    printf("needs to be run on larger volume\n");
    fflush(stdout);
    terminate(1);
  }

  // Set up random gaussian matrix, then unitarize it
  clear_mat(&tmat);
  for (j = 0; j < DIMF; j++) {
#ifdef SITERAND
    tc.real = gaussian_rand_no(&(lattice[0].site_prn));
    tc.imag = gaussian_rand_no(&(lattice[0].site_prn));
#else
    tc.real = gaussian_rand_no(&(lattice[0].node_prn));
    tc.imag = gaussian_rand_no(&(lattice[0].node_prn));
#endif
    c_scalar_mult_sum_mat(&(Lambda[j]), &tc, &tmat);
  }
  polar(&tmat, &Gmat);

  // Confirm unitarity or check invariance when Gmat = I
//  mult_na(&Gmat, &Gmat, &tmat);
//  dumpmat(&tmat);
//  mat_copy(&tmat, &Gmat);

  // Left side of local eta
  clear_mat(&etamat);
  // Construct G eta = sum_j eta^j G Lambda^j
  for (j = 0; j < DIMF; j++) {
    mult_nn(&Gmat, &(Lambda[j]), &tmat);
    tc = TF[s].Fsite.c[j];
    c_scalar_mult_sum_mat(&tmat, &tc, &etamat);
  }
  // Project out eta^j = -Tr[Lambda^j G eta]
  for (j = 0; j < DIMF; j++) {
    mult_nn(&(Lambda[j]), &etamat, &tmat);
    tc = trace(&tmat);
    CNEGATE(tc, TF[s].Fsite.c[j]);
  }

  // Right side of local eta
  clear_mat(&etamat);
  // Construct eta Gdag = sum_j eta^j Lambda^j Gdag
  for (j = 0; j < DIMF; j++) {
    mult_na(&(Lambda[j]), &Gmat, &tmat);
    tc = TF[s].Fsite.c[j];
    c_scalar_mult_sum_mat(&tmat, &tc, &etamat);
  }
  // Project out eta^j = -Tr[eta Gdag Lambda^j]
  for (j = 0; j < DIMF; j++) {
    mult_nn(&etamat, &(Lambda[j]), &tmat);
    tc = trace(&tmat);
    CNEGATE(tc, TF[s].Fsite.c[j]);
  }

  // Left side of local links and psis; right side of local chis
  FORALLDIR(a) {
    mult_nn(&Gmat, &(lattice[s].link[a]), &tmat);
    mat_copy(&tmat, &(lattice[s].link[a]));

    clear_mat(&(psimat[a]));
    for (j = 0; j < DIMF; j++) {
      mult_nn(&Gmat, &(Lambda[j]), &tmat);
      tc = TF[s].Flink[a].c[j];
      c_scalar_mult_sum_mat(&tmat, &tc, &(psimat[a]));
    }
    for (j = 0; j < DIMF; j++) {
      mult_nn(&(Lambda[j]), &(psimat[a]), &tmat);
      tc = trace(&tmat);
      CNEGATE(tc, TF[s].Flink[a].c[j]);
    }

    for (b = a + 1; b < NUMLINK; b++) {
      clear_mat(&(chimat));
      for (j = 0; j < DIMF; j++) {
        mult_na(&(Lambda[j]), &Gmat, &tmat);
        tc = TF[s].Fplaq.c[j];
        c_scalar_mult_sum_mat(&tmat, &tc, &(chimat));
      }
      for (j = 0; j < DIMF; j++) {
        mult_nn(&(chimat), &(Lambda[j]), &tmat);
        tc = trace(&tmat);
        CNEGATE(tc, TF[s].Fplaq[i].c[j]);
      }
    }
  }

  // Right side of neighboring links and psis
  // TODO: Presumably we can convert this to a loop...
  s = node_index(x - 1, t);
  mult_na(&(lattice[s].link[0]), &Gmat, &tmat);
  mat_copy(&tmat, &(lattice[s].link[0]));
  clear_mat(&(psimat[0]));
  for (j = 0; j < DIMF; j++) {
    mult_na(&(Lambda[j]), &Gmat, &tmat);
    tc = TF[s].Flink[0].c[j];
    c_scalar_mult_sum_mat(&tmat, &tc, &(psimat[0]));
  }
  for (j = 0; j < DIMF; j++) {
    mult_nn(&(psimat[0]), &(Lambda[j]), &tmat);
    tc = trace(&tmat);
    CNEGATE(tc, TF[s].Flink[0].c[j]);
  }

  s = node_index(x, t - 1);
  mult_na(&(lattice[s].link[3]), &Gmat, &tmat);
  mat_copy(&tmat, &(lattice[s].link[3]));
  clear_mat(&(psimat[3]));
  for (j = 0; j < DIMF; j++) {
    mult_na(&(Lambda[j]), &Gmat, &tmat);
    tc = TF[s].Flink[3].c[j];
    c_scalar_mult_sum_mat(&tmat, &tc, &(psimat[3]));
  }
  for (j = 0; j < DIMF; j++) {
    mult_nn(&(psimat[3]), &(Lambda[j]), &tmat);
    tc = trace(&tmat);
    CNEGATE(tc, TF[s].Flink[3].c[j]);
  }

  // Left side of neighboring chi
  s = node_index(x - 1, t - 1);
  i = plaq_index[0][3];
  clear_mat(&(chimat[i]));
  for (j = 0; j < DIMF; j++) {
    mult_nn(&Gmat, &(Lambda[j]), &tmat);
    tc = TF[s].Fplaq[i].c[j];
    c_scalar_mult_sum_mat(&tmat, &tc, &(chimat[i]));
  }
  for (j = 0; j < DIMF; j++) {
    mult_nn(&(Lambda[j]), &(chimat[i]), &tmat);
    tc = trace(&tmat);
    CNEGATE(tc, TF[s].Fplaq[i].c[j]);
  }
}
コード例 #26
0
ファイル: hvy_pot.c プロジェクト: rgjha/susy
void hvy_pot(int do_det) {
  register int i;
  register site *s;
  int t_dist, x_dist;
  double wloop;
  complex tc;
  matrix tmat, tmat2;
  msg_tag *mtag = NULL;

  node0_printf("hvy_pot: MAX_T = %d, MAX_X = %d\n", MAX_T, MAX_X);

  // Use staple to hold product of t_dist links at each point
  for (t_dist = 1; t_dist <= MAX_T; t_dist++) {
    if (t_dist == 1) {
      FORALLSITES(i, s)
        mat_copy(&(s->link[TUP]), &(staple[i]));
    }
    else {
      mtag = start_gather_field(staple, sizeof(matrix),
                                goffset[TUP], EVENANDODD, gen_pt[0]);

      // Be careful about overwriting staple;
      // gen_pt may just point to it for on-node "gathers"
      wait_gather(mtag);
      FORALLSITES(i, s)
        mult_nn(&(s->link[TUP]), (matrix *)gen_pt[0][i], &(tempmat2[i]));
      cleanup_gather(mtag);
      FORALLSITES(i, s)
        mat_copy(&(tempmat2[i]), &(staple[i]));
    }

    // Copy staple to tempmat
    // Will shoft at end of loop
    FORALLSITES(i, s)
      mat_copy(&(staple[i]), &(tempmat[i]));
    for (x_dist = 0; x_dist <= MAX_X; x_dist++) {
      // Evaluate potential at this separation
      wloop = 0.0;
      FORALLSITES(i, s) {
        // Compute the actual Coulomb gauge Wilson loop product
        mult_na(&(staple[i]), &(tempmat[i]), &tmat);

        if (do_det == 1)
          det_project(&tmat, &tmat2);
        else
          mat_copy(&tmat, &tmat2);

        tc = trace(&tmat2);
        wloop += tc.real;
      }
      g_doublesum(&wloop);

      if (do_det == 1) {  // Braces fix compiler error
        node0_printf("D_LOOP   ");
      }
      else
        node0_printf("POT_LOOP ");
      node0_printf("%d %d %.6g\n", x_dist, t_dist, wloop / volume);

      // As we increment x, shift in x direction
      shiftmat(tempmat, tempmat2, goffset[XUP]);
    } // x_dist
  } // t_dist
コード例 #27
0
ファイル: patch.cpp プロジェクト: ApocalypseDesign/ad_public
void AD_PatchObject::paint(float4 pos, AD_Camera *telecamera, AD_Omnilight *omnilight)
{
  int w, ntria_generated, wl;
  float4 inv_accum_scale, invz;
  float4 envdimx, envdimy, cosalpha;
  AD_Matrix matrixrot_all, matrixtransform_all;
  AD_Matrix matrixtall_clip;
  AD_Vect3D camerapos_inobject, vtmp, light_vertex_vector;
  AD_Vertex3D *v;


  build_objectmatrix(pos);
  inv_accum_scale=1.0f/accum_scale;

  // si porta la camera nello spazio oggetto
  vect_sub(&telecamera->currentpos, &currentpos, &vtmp);
  mat_mulvectaffine(&inverse_rotmatrix, &vtmp, &camerapos_inobject);
  camerapos_inobject.x*=inv_accum_scale;
  camerapos_inobject.y*=inv_accum_scale;
  camerapos_inobject.z*=inv_accum_scale;

  // si calcolano le matrici di trasformazione totali
  mat_mulaffine(&telecamera->currentmatrix_rot, &currentmatrix_rot, &matrixrot_all);
  mat_mul(&telecamera->currentmatrix, &currentmatrix, &matrixtall_clip);

  // calcolo della matrice di trasformazione con inclusi i
  // fattori di aspect ratio; pX e pY si trovano in render.cpp
  mat_copy(&matrixtall_clip, &matrixtransform_all);
  for (w=0; w<4; w++)
  {
    matrixtransform_all.a[0][w]=matrixtransform_all.a[0][w]*telecamera->prospettivaX;
    matrixtransform_all.a[1][w]=matrixtransform_all.a[1][w]*telecamera->prospettivaY;
  }

  // si portano le luci nello spazio della telecamera
  for (w=0; w<num_omni; w++)
  {
    vect_sub(&omnilight[w].currentpos, &currentpos, &vtmp);
	mat_mulvectaffine(&inverse_rotmatrix, &vtmp, &omnilight[w].currentpos_inobject);
    omnilight[w].currentpos_inobject.x*=inv_accum_scale;
    omnilight[w].currentpos_inobject.y*=inv_accum_scale;
    omnilight[w].currentpos_inobject.z*=inv_accum_scale;
  }

  // trasformiamo tutti i vertici geometrici
  for (w=0; w<num_points; w++)
  {
	// ottengo le posizioni dei vertici dal keyframer
	if (vert_pos[w].numkey>0)
		vert_pos[w].get_data(pos, &verteces[w]);

	// li trasformo in camera space
	mat_mulvect(&matrixtransform_all, &verteces[w], &verteces_tr[w]);
  }
  

  // trasformiamo tutti i vettori
  for (w=0; w<num_vectors; w++)
  {
	// ottengo le posizioni dei vettori dal keyframer
	if (vect_pos[w].numkey>0)
		vect_pos[w].get_data(pos, &vectors[w]);

	// li trasformo in camera space
	mat_mulvect(&matrixtransform_all, &vectors[w], &vectors_tr[w]);
  }


  ntria_generated=Tassellate();
  TRIA_PIPELINE_ENVRGB
  TRIA_PIPELINE_RGB
  TRIA_PIPELINE_ENVMAP
  TRIA_PIPELINE_ELSE
}
コード例 #28
0
ファイル: phase.c プロジェクト: daschaich/susy
 FORALLSITES(i, s) {
   mat_copy((matrix *)(gen_pt[0][i]), &(src[i].Fplaq[7]));  // 2, 3
   mat_copy((matrix *)(gen_pt[1][i]), &(src[i].Fplaq[5]));  // 1, 3
   mat_copy((matrix *)(gen_pt[2][i]), &(src[i].Fplaq[4]));  // 1, 2
 }
コード例 #29
0
ファイル: matpca.c プロジェクト: caomw/matrixlab
MATSTACK mat_pca(MATRIX data, int pca_type)
{
    int i, j, k, k2, m, n;
    MATRIX evals, im;
    MATSTACK tmmps0 = NULL;
    MATRIX symmat, symmat2;
    m = MatCol(data);
    n = MatRow(data);

    switch(pca_type)
    {
    case MAT_PCA_CORRELATION:
        tmmps0 = mat_corcol(data);
        symmat = tmmps0[1];
        break;
    case MAT_PCA_COVARIANCE:
        tmmps0 = mat_covcol(data);
        symmat = tmmps0[1];
        break;
    case MAT_PCA_SUMOFSQUARES:
        symmat = mat_scpcol(data);
        break;
    default:
        tmmps0 = mat_covcol(data);
        symmat = tmmps0[1];
        break;
    }
    evals = mat_creat(m, 1, UNDEFINED);
    im = mat_creat(m, 1, UNDEFINED);
    symmat2 = mat_copy(symmat, NULL);
    mat_tred2(symmat, evals, im);
    mat_tqli(evals, im, symmat);

    for(i=0; i<n; ++i)
    {
        for(j=0; j<m; ++j)
        {
            im[j][0] = tmmps0[2][i][j];
        }
        for(k=0; k<3; ++k)
        {
            tmmps0[2][i][k] = 0.0;
            for(k2=0; k2<m; ++k2)
            {
                tmmps0[2][i][k] += im[k2][0] * symmat[k2][m-k-1];
            }
        }
    }

    for(j=0; j<m; ++j)
    {
        for(k=0; k<m; ++k)
        {
            im[k][0] = symmat2[j][k];
        }
        for(i=0; i<3; ++i)
        {
            symmat2[j][i] = 0.0;
            for (k2=0; k2<m; ++k2)
            {
                symmat2[j][i] += im[k2][0] * symmat[k2][m-i-1];
            }
            if(evals[m-i-1][0]>0.0005)
                symmat2[j][i] /= (mtype)sqrt(evals[m-i-1][0]);
            else
                symmat2[j][i] = 0.0;
        }
    }
    mat_free(evals);
    mat_free(im);
    return tmmps0;
}
コード例 #30
0
ファイル: EKF_15state_quat.c プロジェクト: PAAW/mAEWing1
// Main get_nav filter function
void get_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
	double tnow, imu_dt;
	double dq[4], quat_new[4];

	// compute time-elapsed 'dt'
	// This compute the navigation state at the DAQ's Time Stamp
	tnow = sensorData_ptr->imuData_ptr->time;
	imu_dt = tnow - tprev;
	tprev = tnow;		
	
	// ==================  Time Update  ===================
	// Temporary storage in Matrix form
	quat[0] = navData_ptr->quat[0];
	quat[1] = navData_ptr->quat[1];
	quat[2] = navData_ptr->quat[2];
	quat[3] = navData_ptr->quat[3];
	
	a_temp31[0][0] = navData_ptr->vn; a_temp31[1][0] = navData_ptr->ve;
	a_temp31[2][0] = navData_ptr->vd;
	
	b_temp31[0][0] = navData_ptr->lat; b_temp31[1][0] = navData_ptr->lon;
	b_temp31[2][0] = navData_ptr->alt;
	
	// AHRS Transformations
	C_N2B = quat2dcm(quat, C_N2B);
	C_B2N = mat_tran(C_N2B, C_B2N);
	
	// Attitude Update
	// ... Calculate Navigation Rate
	nr = navrate(a_temp31,b_temp31,nr);
	
	dq[0] = 1;
	dq[1] = 0.5*om_ib[0][0]*imu_dt;
	dq[2] = 0.5*om_ib[1][0]*imu_dt;
	dq[3] = 0.5*om_ib[2][0]*imu_dt;
	
	qmult(quat,dq,quat_new);
	
	quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	
    if(quat[0] < 0) {
        // Avoid quaternion flips sign
        quat[0] = -quat[0];
        quat[1] = -quat[1];
        quat[2] = -quat[2];
        quat[3] = -quat[3];
    }
    
	navData_ptr->quat[0] = quat[0];
	navData_ptr->quat[1] = quat[1];
	navData_ptr->quat[2] = quat[2];
	navData_ptr->quat[3] = quat[3];
	
	quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
	
	// Velocity Update
	dx = mat_mul(C_B2N,f_b,dx);
	dx = mat_add(dx,grav,dx);
	navData_ptr->vn += imu_dt*dx[0][0];
	navData_ptr->ve += imu_dt*dx[1][0];
	navData_ptr->vd += imu_dt*dx[2][0];
	
	// Position Update
	dx = llarate(a_temp31,b_temp31,dx);
	navData_ptr->lat += imu_dt*dx[0][0];
	navData_ptr->lon += imu_dt*dx[1][0];
	navData_ptr->alt += imu_dt*dx[2][0];
	
	// JACOBIAN
	F = mat_fill(F, ZERO_MATRIX);
	// ... pos2gs
	F[0][3] = 1.0; 	F[1][4] = 1.0; 	F[2][5] = 1.0;
	// ... gs2pos
	F[5][2] = -2*g/EARTH_RADIUS;
	
	// ... gs2att
	temp33 = sk(f_b,temp33);
	atemp33 = mat_mul(C_B2N,temp33,atemp33);
	
	F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
	F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
	F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];
	
	// ... gs2acc
	F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
	F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
	F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];
	
	// ... att2att
	temp33 = sk(om_ib,temp33);
	F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
	F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
	F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];
	
	// ... att2gyr
	F[6][12] = -0.5;
	F[7][13] = -0.5;
	F[8][14] = -0.5;
	
	// ... Accel Markov Bias
	F[9][9] = -1.0/TAU_A; 	F[10][10] = -1.0/TAU_A;	F[11][11] = -1.0/TAU_A;
	F[12][12] = -1.0/TAU_G; F[13][13] = -1.0/TAU_G;	F[14][14] = -1.0/TAU_G;
	
	//fprintf(stderr,"Jacobian Created\n");
	
	// State Transition Matrix: PHI = I15 + F*dt;
	temp1515 = mat_scalMul(F,imu_dt,temp1515);
	PHI = mat_add(I15,temp1515,PHI);
	
	// Process Noise
	G = mat_fill(G, ZERO_MATRIX);
	G[3][0] = -C_B2N[0][0];	G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
	G[4][0] = -C_B2N[1][0];	G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
	G[5][0] = -C_B2N[2][0];	G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];
	
	G[6][3] = -0.5;
	G[7][4] = -0.5;
	G[8][5] = -0.5;
	
	G[9][6] = 1.0; 			G[10][7] = 1.0; 		G[11][8] = 1.0;
	G[12][9] = 1.0; 		G[13][10] = 1.0; 		G[14][11] = 1.0;
	//fprintf(stderr,"Process Noise Matrix G is created\n");
	// Discrete Process Noise
	temp1512 = mat_mul(G,Rw,temp1512);
	temp1515 = mat_transmul(temp1512,G,temp1515);	// Qw = G*Rw*G'
	Qw = mat_scalMul(temp1515,imu_dt,Qw);			// Qw = dt*G*Rw*G'
	Q = mat_mul(PHI,Qw,Q);						// Q = (I+F*dt)*Qw
	
	temp1515 = mat_tran(Q,temp1515);
	Q = mat_add(Q,temp1515,Q);
	Q = mat_scalMul(Q,0.5,Q);				// Q = 0.5*(Q+Q')
	//fprintf(stderr,"Discrete Process Noise is created\n");
	
	// Covariance Time Update
	temp1515 = mat_mul(PHI,P,temp1515);
	P = mat_transmul(temp1515,PHI,P); 		// P = PHI*P*PHI'
	P = mat_add(P,Q,P);						// P = PHI*P*PHI' + Q
	temp1515 = mat_tran(P, temp1515);
	P = mat_add(P,temp1515,P);
	P = mat_scalMul(P,0.5,P);				// P = 0.5*(P+P')
	//fprintf(stderr,"Covariance Updated through Time Update\n");
	
	navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
	navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
	navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
	navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
	navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
	
	navData_ptr->err_type = TU_only;
	//fprintf(stderr,"Time Update Done\n");
	// ==================  DONE TU  ===================
	
	if(sensorData_ptr->gpsData_ptr->newData){
	
		// ==================  GPS Update  ===================
		sensorData_ptr->gpsData_ptr->newData = 0; // Reset the flag
		
		// Position, converted to NED
		a_temp31[0][0] = navData_ptr->lat;
		a_temp31[1][0] = navData_ptr->lon; a_temp31[2][0] = navData_ptr->alt;
		pos_ins_ecef = lla2ecef(a_temp31,pos_ins_ecef);
		
		a_temp31[2][0] = 0.0;
		//pos_ref = lla2ecef(a_temp31,pos_ref);
		pos_ref = mat_copy(a_temp31,pos_ref);
		pos_ins_ned = ecef2ned(pos_ins_ecef,pos_ins_ned,pos_ref);
		
		pos_gps[0][0] = sensorData_ptr->gpsData_ptr->lat*D2R;
		pos_gps[1][0] = sensorData_ptr->gpsData_ptr->lon*D2R;
		pos_gps[2][0] = sensorData_ptr->gpsData_ptr->alt;
		
		pos_gps_ecef = lla2ecef(pos_gps,pos_gps_ecef);
		
		pos_gps_ned = ecef2ned(pos_gps_ecef,pos_gps_ned,pos_ref);
		
		// Create Measurement: y
		y[0][0] = pos_gps_ned[0][0] - pos_ins_ned[0][0];
		y[1][0] = pos_gps_ned[1][0] - pos_ins_ned[1][0];
		y[2][0] = pos_gps_ned[2][0] - pos_ins_ned[2][0];
		
		y[3][0] = sensorData_ptr->gpsData_ptr->vn - navData_ptr->vn;
		y[4][0] = sensorData_ptr->gpsData_ptr->ve - navData_ptr->ve;
		y[5][0] = sensorData_ptr->gpsData_ptr->vd - navData_ptr->vd;
		
		//fprintf(stderr,"Measurement Matrix, y, created\n");
		
		// Kalman Gain
		temp615 = mat_mul(H,P,temp615);
		temp66 = mat_transmul(temp615,H,temp66);
		atemp66 = mat_add(temp66,R,atemp66);
		temp66 = mat_inv(atemp66,temp66); // temp66 = inv(H*P*H'+R)
		//fprintf(stderr,"inv(H*P*H'+R) Computed\n");
		
		temp156 = mat_transmul(P,H,temp156); // P*H'
		//fprintf(stderr,"P*H' Computed\n");
		K = mat_mul(temp156,temp66,K);	   // K = P*H'*inv(H*P*H'+R)
		//fprintf(stderr,"Kalman Gain Computed\n");
		
		// Covariance Update
		temp1515 = mat_mul(K,H,temp1515);
		ImKH = mat_sub(I15,temp1515,ImKH);	// ImKH = I - K*H
		
		temp615 = mat_transmul(R,K,temp615);
		KRKt = mat_mul(K,temp615,KRKt);		// KRKt = K*R*K'
		
		temp1515 = mat_transmul(P,ImKH,temp1515);
		P = mat_mul(ImKH,temp1515,P);		// ImKH*P*ImKH'
		temp1515 = mat_add(P,KRKt,temp1515);
		P = mat_copy(temp1515,P);			// P = ImKH*P*ImKH' + KRKt
		//fprintf(stderr,"Covariance Updated through GPS Update\n");
		
		navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
		navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
		navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
		navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
		navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
		
		// State Update
		x = mat_mul(K,y,x);
		denom = (1.0 - (ECC2 * sin(navData_ptr->lat) * sin(navData_ptr->lat)));
		denom = sqrt(denom*denom);

		Re = EARTH_RADIUS / sqrt(denom);
		Rn = EARTH_RADIUS*(1-ECC2) / denom*sqrt(denom);
		navData_ptr->alt = navData_ptr->alt - x[2][0];
		navData_ptr->lat = navData_ptr->lat + x[0][0]/(Re + navData_ptr->alt);
		navData_ptr->lon = navData_ptr->lon + x[1][0]/(Rn + navData_ptr->alt)/cos(navData_ptr->lat);
		
		navData_ptr->vn = navData_ptr->vn + x[3][0];
		navData_ptr->ve = navData_ptr->ve + x[4][0];
		navData_ptr->vd = navData_ptr->vd + x[5][0];
		
		quat[0] = navData_ptr->quat[0];
		quat[1] = navData_ptr->quat[1];
		quat[2] = navData_ptr->quat[2];
		quat[3] = navData_ptr->quat[3];
		
		// Attitude correction
		dq[0] = 1.0;
		dq[1] = x[6][0];
		dq[2] = x[7][0];
		dq[3] = x[8][0];
		
		qmult(quat,dq,quat_new);
		
		quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		
		navData_ptr->quat[0] = quat[0];
		navData_ptr->quat[1] = quat[1];
		navData_ptr->quat[2] = quat[2];
		navData_ptr->quat[3] = quat[3];
		
		quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
		
		navData_ptr->ab[0] = navData_ptr->ab[0] + x[9][0];
		navData_ptr->ab[1] = navData_ptr->ab[1] + x[10][0];
		navData_ptr->ab[2] = navData_ptr->ab[2] + x[11][0];
		
		navData_ptr->gb[0] = navData_ptr->gb[0] + x[12][0];
		navData_ptr->gb[1] = navData_ptr->gb[1] + x[13][0];
		navData_ptr->gb[2] = navData_ptr->gb[2] + x[14][0];
		
		navData_ptr->err_type = gps_aided;
		//fprintf(stderr,"Measurement Update Done\n");
	}
	
	// Remove current estimated biases from rate gyro and accels
	sensorData_ptr->imuData_ptr->p -= navData_ptr->gb[0];
	sensorData_ptr->imuData_ptr->q -= navData_ptr->gb[1];
	sensorData_ptr->imuData_ptr->r -= navData_ptr->gb[2];
	sensorData_ptr->imuData_ptr->ax -= navData_ptr->ab[0];
	sensorData_ptr->imuData_ptr->ay -= navData_ptr->ab[1];
	sensorData_ptr->imuData_ptr->az -= navData_ptr->ab[2];

	// Get the new Specific forces and Rotation Rate,
	// use in the next time update
	f_b[0][0] = sensorData_ptr->imuData_ptr->ax;
	f_b[1][0] = sensorData_ptr->imuData_ptr->ay;
	f_b[2][0] = sensorData_ptr->imuData_ptr->az;

	om_ib[0][0] = sensorData_ptr->imuData_ptr->p;
	om_ib[1][0] = sensorData_ptr->imuData_ptr->q;
	om_ib[2][0] = sensorData_ptr->imuData_ptr->r;


}