Ejemplo n.º 1
0
void view_turn(view *V, const double *w)
{
#if 0
    double M[16], T[16], q[4], a[3], b[3], c[3], t[3];

    assert(V);

    /* Transform the mark and drag vectors from eye space into the world     */
    /* space current at the time of the mark.                                */

    state_matrix(M, &V->mark);

    mtranspose(T, M);
    vnormalize(t, V->a);
    vtransform(a, T, t);
    vnormalize(t,    w);
    vtransform(b, T, t);

    /* These determine the axis and angle of rotation.  Find the quaternion. */

    vcrs(t, a, b);
    vnormalize(c, t);
    qrotate(q, c, acos(vdot(a, b)));

    /* Accumulate this quaternion with the view state, rotating the view.    */

    qmultiply(view_curr(V)->q, V->mark.q, q);
#endif
}
Ejemplo n.º 2
0
static void state_inverse(double *I, const state *s)
{
    double T[16], A[16], R[16];

    mtranslate(T, s->p);
    meuler    (A, s->e);
    mtranspose(R, A);
    mmultiply (I, T, R);
}
Ejemplo n.º 3
0
/*

  READ GEO

*/
void get_geometry(TE*root)
{
	printf("_\t_\tGEOMETRIES\t_\t_\t_\t_\n");

	TE*geo = root -> FCE( "library_geometries" )->FCE("geometry");
	while ( geo )
	{
		get_object( geo );
		geo = geo -> NSE ( "geometry" );
	}


	//get transforms, tranform vertices
	TE*node = root -> FCE( "library_visual_scenes" )->
		FCE("visual_scene") -> FCE("node");;
	
	size_t i=0;
	while ( node && i < dae_geo_objects.size() )
	{
	
		dae_geo_t &d = dae_geo_objects[i];
		std::string name = node->Attribute( "name" );
		
		if( name == d.name ){
			
			std::vector< float >mat=str_fv(node->FCE("matrix")->GetText(),16);
			
			//transpose 
			float M[16];
			mtranspose( M, &mat[0] );
			
			//multiply each vertex
			for( size_t k = 0; k < d.pos.size(); k+=3 ){
				vec3 vp={d.pos[k],d.pos[k+1],d.pos[k+2]};
				vp = mmulv( M, vp, 1.f );
				d.pos[k] = vp.x;
				d.pos[k+1] = vp.y;
				d.pos[k+2] = vp.z;
			}
			for( size_t k = 0; k < d.normal.size(); k+=3 ){
				vec3 vp={d.normal[k],d.normal[k+1],d.normal[k+2]};
				vp = vnormal(mmulv( M, vp, 0.f));
				d.normal[k] = vp.x;
				d.normal[k+1] = vp.y;
				d.normal[k+2] = vp.z;
			}
			++i;
		}

		node = node -> NSE( "node" );
	}

	printf("count = %d\n", dae_geo_objects.size());
}
Ejemplo n.º 4
0
void linsolve_lower(double *L,int N,double *b,double *x) {
	int i,j,c1,l;
	double *y,*A;
	double sum,temp;
	
	y = (double*) malloc(sizeof(double) *N);
	A = (double*) malloc(sizeof(double) *N *N);
	
	for(i = 0; i < N;++i) {
		y[i] = 0.;
		x[i] = 0.;
		if ( L[i*N + i] == 0.) {
			printf("The Matrix system does not have a unique solution");
			exit(1);
		}
		//printf("\n B %d",ipiv[i]);
	}
	
	// Forward Substitution
	
	y[0] = b[0]/L[0];
	for(i = 1; i < N; ++i) {
		sum = 0.;
		c1 = i*N;
		for(j = 0; j < i; ++j) {
			sum += y[j] * L[c1 + j];
		}
		y[i] = (b[i] - sum)/L[c1+i];
	}
	
	mtranspose(L,N,N,A);
	
	//Back Substitution
	
	x[N - 1] = y[N - 1]/A[N * N - 1];
	
	for (i = N - 2; i >= 0; i--) {
		sum = 0.;
		c1 = i*(N+1);
		l=0;
		for(j = i+1; j < N;j++) {
			l++;
			sum += A[c1 + l] * x[j];
		}
		x[i] = (y[i] - sum) / A[c1];
	}
	
	free(y);
	free(A);
}
Ejemplo n.º 5
0
int as154_seas(double *inp, int N, int optmethod, int p, int d, int q, int s, int P, int D, int Q,
	double *phi, double *theta, double *PHI, double *THETA, double *wmean,double *var,double *loglik,double *hess) {
	int i, pq, retval, length, offset,ret;
	double *b, *tf, *x,*inp2,*dx,*thess;
	int *ipiv;
	double maxstep;
	alik_seas_object obj;
	custom_function as154_min;
	obj = alik_seas_init(p, d, q, s, P, D, Q, N);
	inp2 = (double*)malloc(sizeof(double)* (N - s*D));
	pq = obj->pq;
	b = (double*)malloc(sizeof(double)* pq);
	tf = (double*)malloc(sizeof(double)* pq);
	thess = (double*)malloc(sizeof(double)* pq*pq);
	dx = (double*)malloc(sizeof(double)* pq);
	ipiv = (int*)malloc(sizeof(int)* pq);

	length = N;

	maxstep = 1.0;


	css_seas(inp, N, optmethod, p, d, q, s, P, D, Q, phi, theta, PHI, THETA, wmean, var,loglik,hess);

	/*

	*/

	if (D > 0) {
		N = diffs(inp, N, D, s, inp2);
	}
	else {
		for (i = 0; i < N; ++i) {
			inp2[i] = inp[i];
		}
	}

	x = (double*)malloc(sizeof(double)* (N - d));

	if (d > 0) {
		N = diff(inp2, N, d, x); // No need to demean x
	}
	else {
		for (i = 0; i < N; ++i) {
			x[i] = inp2[i];
		}
	}

	obj->N = N;

	offset = obj->offset;
	for (i = 0; i < p; ++i) {
		b[i] = phi[i];
	}
	for (i = 0; i < q; ++i) {
		b[p + i] = -theta[i];
	}
	for (i = 0; i < P; ++i) {
		b[p + q + i] = PHI[i];
	}
	for (i = 0; i < Q; ++i) {
		b[p + q + P + i] = -THETA[i];
	}

	if (obj->M == 1) {
		b[p + q + P + Q] = *wmean;
	}

	obj->mean = *wmean;

	//mdisplay(b, 1, p + q + P + Q);

	for (i = 0; i < N; ++i) {
		obj->x[offset + i] = obj->x[offset + 2 * N + i] = x[i];
	}
	for (i = N; i < 2 * N; ++i) {
		obj->x[offset + i] = 0.0;
	}
	//printf("\n %d %g ", pq,maxstep);

//	custom_function as154_min = { fas154_seas, obj };
	as154_min.funcpt = fas154_seas;
	as154_min.params = obj;

	retval = fminunc(&as154_min, NULL, pq, b, maxstep, optmethod, tf);
	if (retval == 0) {
		ret = 0;
	}
	else if (retval == 15) {
		ret = 15;
	}
	else if (retval == 4) {
		ret = 4;
	}
	else {
		ret = 1;
	}

	for (i = 0; i < pq; ++i) {
		dx[i] = 1.0;
	}

	hessian_fd(&as154_min, tf, pq, dx, obj->eps, hess);

	mtranspose(hess, pq, pq, thess);

	for (i = 0; i < pq*pq; ++i) {
		thess[i] = (N - d - s*D) * 0.5 * (hess[i] + thess[i]);
	}


	ludecomp(thess, pq, ipiv);
	minverse(thess, pq, ipiv, hess);

	for (i = 0; i < p; ++i) {
		phi[i] = tf[i];
	}
	for (i = 0; i < q; ++i) {
		theta[i] = -tf[p + i];
	}
	for (i = 0; i < P; ++i) {
		PHI[i] = tf[p + q + i];
	}
	for (i = 0; i < Q; ++i) {
		THETA[i] = -tf[p + q + P + i];
	}

	if (obj->M == 1) {
		*wmean = tf[p + q + Q + P];
	}
	else {
		*wmean = 0.0;
	}

	*var = (obj->ssq) / (double) N;
	*loglik = obj->loglik;
	//printf("MEAN %g \n", mean(obj->x+N,N));
	//mdisplay(obj->x + N, 1, N);

	free(b);
	free(tf);
	free(inp2);
	free(x);
	free(dx);
	free(thess);
	free(ipiv);
	free_alik_seas(obj);
	return ret;
}
Ejemplo n.º 6
0
int as154(double *inp, int N, int optmethod, int p, int d, int q, double *phi, double *theta, double *wmean, double *var,double *resid,double *loglik,double *hess) {
	int i,pq,retval,length,ret;
	double *b,*tf,*x,*dx,*thess;
	int *ipiv;
	double maxstep;
	alik_object obj;
	custom_function as154_min;

	x = (double*)malloc(sizeof(double)* (N - d));

	length = N;
	
	maxstep = 1.0;

	obj = alik_init(p, d, q, N);


	css(inp, N, optmethod, p, d, q, phi, theta, wmean, var,resid,loglik,hess);

	if (d > 0) {
		N = diff(inp, N, d, x); // No need to demean x
	}
	else {
		for (i = 0; i < N; ++i) {
			x[i] = inp[i];
		}
	}

	obj->N = N;
	obj->mean = *wmean;
	pq = obj->pq;
	b = (double*)malloc(sizeof(double)* pq);
	tf = (double*)malloc(sizeof(double)* pq);
	thess = (double*)malloc(sizeof(double)* pq*pq);
	dx = (double*)malloc(sizeof(double)* pq);
	ipiv = (int*)malloc(sizeof(int)* pq);

	for (i = 0; i < p; ++i) {
		b[i] = phi[i];
	}
	for (i = 0; i < q; ++i) {
		b[p + i] = -theta[i];
	}

	if (obj->M == 1) {
		b[p + q] = obj->mean;
	}


	for (i = 0; i < N; ++i) {
		obj->x[i] = obj->x[2 * N + i] = x[i];
	}
	for (i = N; i < 2 * N; ++i) {
		obj->x[i] = 0.0;
	}

	//20141121ÐÞ¸Ä

	//custom_function as154_min = { fas154, obj };
	as154_min.funcpt = fas154;
	as154_min.params = obj;

	retval = fminunc(&as154_min, NULL, pq, b,maxstep, optmethod, tf);

	if (retval == 0) {
		ret = 0;
	}
	else if (retval == 15) {
		ret = 15;
	}
	else if (retval == 4) {
		ret = 4;
	}
	else {
		ret = 1;
	}

	for (i = 0; i < pq; ++i) {
		dx[i] = 1.0;
	}

	hessian_fd(&as154_min, tf, pq, dx, obj->eps, hess);
	
	mtranspose(hess, pq, pq, thess);

	for (i = 0; i < pq*pq; ++i) {
		thess[i] = (N-d) * 0.5 * (hess[i] + thess[i]);
	}
	

	ludecomp(thess, pq, ipiv);
	minverse(thess, pq, ipiv, hess);


	for (i = 0; i < p; ++i) {
		phi[i] = tf[i];
	}
	for (i = 0; i < q; ++i) {
		theta[i] = -tf[p + i];
	}
	if (obj->M == 1) {
		*wmean = tf[p + q];
	}
	else {
		*wmean = 0.0;
	}
	/*
	wmean = 0.0;
	for (i = 0; i < N; ++i) {
		wmean += (obj->x[N + i] * obj->x[N + i]);
	}*/

	*var = (obj->ssq) / (double) N;
	for (i = 0; i < N - d; ++i) {
		resid[i] = obj->x[N + i];
	}
	*loglik = obj->loglik;
	//printf("MEAN %g \n", mean(obj->x+N,N));
	//mdisplay(obj->x + N, 1, N);

	free(b);
	free(tf);
	free(x);
	free(thess);
	free(ipiv);
	free(dx);
	free_alik(obj);
	return ret;
}
Ejemplo n.º 7
0
Archivo: nls.c Proyecto: rafat/optimc
void qrfac(double *A, int M, int N, int lda, int pivot, int *ipvt, int lipvt,double *rdiag, double *acnorm,double eps) {
    int i,j,jp1,k,kmax,minmn,t;
    double ajnorm,epsmch,one,p05,sum,temp,zero,temp2,pmaxval;
    double *AT,*wa,*wa2;

    /*
        * This routine is a C translation of Fortran Code by
        *     argonne national laboratory. minpack project. march 1980.
         burton s. garbow, kenneth e. hillstrom, jorge j. more
        *
        * M is a positive integer input variable set to the number
            of rows of a.

          N is a positive integer input variable set to the number
            of columns of a.

          A is an M by N array. on input a contains the matrix for
            which the qr factorization is to be computed. on output
            the strict upper trapezoidal part of a contains the strict
            upper trapezoidal part of r, and the lower trapezoidal
            part of a contains a factored form of q (the non-trivial
            elements of the u vectors described above).

          lda is a positive integer input variable not less than m
            which specifies the leading dimension of the array a.

          pivot is an integer input variable. if pivot is set to 1,
            then column pivoting is enforced. if pivot is set to 0,
            then no column pivoting is done.

          ipvt is an integer output array of length lipvt. ipvt
            defines the permutation matrix p such that a*p = q*r.
            column j of p is column ipvt(j) of the identity matrix.
            if pivot is false, ipvt is not referenced.

          lipvt is a positive integer input variable. if pivot is false,
            then lipvt may be as small as 1. if pivot is true, then
            lipvt must be at least n.

          rdiag is an output array of length N which contains the
            diagonal elements of r.

          acnorm is an output array of length N which contains the
            norms of the corresponding columns of the input matrix a.
            if this information is not needed, then acnorm can coincide
            with rdiag.
        *
        */


    if (pivot != 0 && pivot != 1) {
        printf("Pivot only takes binary values 0 and 1 \n");
        exit(-1);
    }

    AT = (double*) malloc(sizeof(double) *N*M);
    wa = (double*) malloc(sizeof(double) *N);
    wa2 = (double*) malloc(sizeof(double) *M);

    one = 1.0;
    zero = 0.0;
    p05 = 5.0e-02;
    epsmch = eps;

    mtranspose(A,M,N,AT);// AT is size NXM

    //compute the initial column norms and initialize several arrays.

    for(j = 0; j < N; ++j) {
        acnorm[j] = enorm(AT+j*M,M);
        rdiag[j] = acnorm[j];
        wa[j] = rdiag[j];
        if (pivot == 1) {
            ipvt[j] = j;
        }
    }

    //reduce a to r with householder transformations.

    if (M < N) {
        minmn = M;
    } else {
        minmn = N;
    }

    for (j = 0; j < minmn; ++j) {
        if (pivot == 1) {
            //bring the column of largest norm into the pivot position.
            kmax = j;
            for(k = j; k < N; ++k) {
                if (rdiag[k] > rdiag[kmax]) {
                    kmax = k;
                }
            }
            if (kmax != j) {
                for(i = 0; i < M; ++i) {
                    t = i * N;
                    temp = A[t+j];
                    A[t+j] = A[t+kmax];
                    A[t+kmax] = temp;
                }
                rdiag[kmax] = rdiag[j];
                wa[kmax] = wa[j];
                k = ipvt[j];
                ipvt[j] = ipvt[kmax];
                ipvt[kmax] = k;
            }
        }
        //        compute the householder transformation to reduce the
        //       j-th column of a to a multiple of the j-th unit vector.
        t = j * N + j;

        for(i = 0; i < M-j; ++i) {
            wa2[i] = A[t+i*N];
        }
        ajnorm = enorm(wa2,M-j);
        if (ajnorm != zero) {
            if (A[t] < zero) {
                ajnorm = - ajnorm;
            }
            for(i = j; i < M; ++i) {
                A[i*N+j] /= ajnorm;
            }
            A[t] += one;
            //        apply the transformation to the remaining columns
            //        and update the norms.

            jp1 = j + 1; // Breakpoint
            if (N >= jp1+1) {
                for(k = jp1; k < N; ++k) {
                    sum = zero;
                    for(i = j; i < M; ++i) {
                        sum += (A[i*N+j] * A[i*N+k]);
                    }
                    temp = sum / A[t];
                    for(i = j; i < M; ++i) {
                        A[i*N+k] -= (temp * A[i*N+j]);
                    }
                    // Breakpoint 2
                    if (pivot == 1 && rdiag[k] != zero) {
                        temp = A[j*N+k] / rdiag[k];
                        pmaxval = pmax(zero, one - temp*temp);
                        rdiag[k] = rdiag[k]*sqrt(pmaxval);
                        temp2 = (p05*(rdiag[k]/wa[k]));
                        temp2 = temp2 * temp2;
                        if (temp2 <= epsmch) {
                            for(i = 0; i < M-j-1; ++i) {
                                wa2[i] = A[jp1*N+k+i*N];
                            }
                            rdiag[k] = enorm(wa2,M-j-1);
                            wa[k] = rdiag[k];
                        }
                    }
                }
            }
        }
        rdiag[j] = -ajnorm;
    }

    free(AT);
    free(wa);
    free(wa2);
}
Ejemplo n.º 8
0
int mesh_out( const char*name)
{
	FILE*f=fopen( name , "wb");
	if(!f)
		return printf("File could not open: %s\n",name), -1;

	{
		//quick, non-optimal export
		int count = v_tri.size() / 3;
		std::vector<vertex_t>vertices;
		std::vector<u16>indices;
	
		for ( int i=0; i<count; ++i){
			int j=i*3;
			int vix = v_tri[j];
			int nix = v_tri[j+1];
			int tix = v_tri[j+2];
			
			vertex_t v={
				v_pos[vix*3],
				v_pos[vix*3+1],
				v_pos[vix*3+2],
				
				v_uv[tix*2],
				1.f-v_uv[tix*2+1],
				
				v_normal[nix*3],
				v_normal[nix*3+1],
				v_normal[nix*3+2],
				
				(float)v_jw[vix].joint_a,
				(float)v_jw[vix].joint_b,
				v_jw[vix].weight_a,
				v_jw[vix].weight_b
			};
			int index = vfind( &vertices[0], v, vertices.size() );
			if( index == (int)vertices.size() )
				vertices.push_back(v);
			indices.push_back( index );
		}

		//write
		int num_indices = indices.size();
		int num_vertices = vertices.size();
		int num_bone = v_keyframes.size();
		int num_frame = v_keyframes[0].size()/ 16;
		u8 num_child[16]={};
		u8 level[16]={};
		char stack_instr[16]={};
		std::vector<mat4_t>invm;
		int child_count = 0;

		assert( num_bone <= 16 && " TOO MANY BONES "[0] );
		assert( !v_keyframes.empty() && "KEYFRAMES EMPTY"[0]);
		//for( size_t i= 0 ;  i < joint_hier.size(); ++i)
		joint_ride(invm,num_child, joint_hier[0], level, child_count);
		
		for(int i = 0; i<num_bone-1; ++i)
			stack_instr[i]=(int)level[i+1]-(int)level[i];

		//WRITE header
		fwrite( &num_indices, sizeof( int ), 1, f );
		fwrite( &num_vertices, sizeof( int ), 1, f );
		fwrite( &num_bone, sizeof( int ) , 1, f);
		fwrite( &num_frame, sizeof( int ) , 1, f);
		//fwrite( num_child, sizeof(u8), 16, f);// hierarchy
		fwrite( stack_instr, sizeof(char), 16, f);// stack_instr
		
		//WRITE indices
		fwrite( &indices[0], sizeof( u16 ), num_indices, f );
		//WRITE vertices
		fwrite( &vertices[0], sizeof( vertex_t ), num_vertices, f );


		//transpose all matrices 
		mtranspose( &bind_shape_matrix[0] );

		for( int i=0; i<num_bone; ++i ){
			float res[16];
			mtranspose( res,invm[i].data );
			
			//PRE-MULTIPLY WITH BSM
			mmul(invm[i].data, res, &bind_shape_matrix[0]);
			
			for( int k=0;k<num_frame; ++k){
				mtranspose(&v_keyframes[i][k*16] );

			}
		}
		//fwrite( bind_shape_matrix.data(), sizeof(float),16,f);

		//WRITE inverse matrices
		fwrite( invm[0].data, sizeof(float),16*num_bone,f);

		//KEYFRAMES
		//create joints  ( pos , dir/quat ) from matrices 
		//WRITE keyframes (and reorder frame wise, not bone wise)
		//AND convert to joint quat
		jq_t jq={};
		std::vector<jq_t>jointquats;
		jointquats.reserve(num_frame*num_bone);
		for( int k=0;k<num_frame; ++k)
			for( int i=0;i<num_bone; ++i){
			quat_from_mat4(jq.quat,&v_keyframes[i][k*16]);

			jq.pos[0]=v_keyframes[i][k*16+12];
			jq.pos[1]=v_keyframes[i][k*16+13];
			jq.pos[2]=v_keyframes[i][k*16+14];

			jq.child[0]=jq.child[1]=jq.child[2]=jq.child[3]=0;
			jointquats.push_back(jq);
			//fwrite( &v_keyframes[i][k*16], sizeof(float),16,f);
			}
		fwrite(jointquats.data(), sizeof(jq_t),num_frame*num_bone,f);
		
		//done
		fclose(f);
		
		//print stuff...
		for( int i=0;i<16; ++i)
			printf( "m = %.4f\tinv = %.4f\n",v_keyframes[0][i],
					invm[0].data[i]);


		printf("\nOUTPUT\n");
		printf( "INDICES: %d\n", num_indices);
		printf( "VERTICES: %d\n", num_vertices);
		printf("BONES = %d\n", num_bone);
		printf("FRAMES = %d\n", num_frame);
		printf("STACK = \n\t");
		for( int i=0;i<num_bone; ++i)
			printf("%d, ",stack_instr[i]);
		printf("\nHIERARCHY = \n\t");
		for( int i=0;i<num_bone; ++i)
			printf("%d, ",num_child[i]);
		printf("\n");
	}

	return 0;
}
Ejemplo n.º 9
0
static int rbcholu(double *A,int N, int stride, double *UB, double *UT) {
	int bs,bb,i,j,Nb,t,k,u,v,w,sc;
	double *b,*x,*U12,*U12T;
	double sum;
	
	bs = (int) BLOCKSIZE;
	bb = bs*bs;
	
	if (N <= BLOCKSIZE) {
		sc = rcholu(A,N,stride,UB);
		if (sc == -1) {
			return -1;
		}
	} else {
		Nb = N - bs;
		x = (double*) malloc(sizeof(double) * bs);
		b = (double*) malloc(sizeof(double) * bs);
		U12T = (double*) malloc(sizeof(double) * Nb * bs);
		U12 = (double*) malloc(sizeof(double) * Nb * bs);
		rcholu(A,bs,stride,UB); // U11
		
		for (i =0; i < bs;++i) {
			t = i *stride;
			u = 0;
			for(j = 0; j < N;++j) {
				UT[u+i] = A[j+t];
				u += bs;
			}
		}
		
		for(k = 0; k < Nb;++k) {
			u = k * bs;
			for(i = 0; i < bs;++i) {
				b[i] = UT[bb+u+i];
				x[i] = 0.;
			}
			for (i = 0; i < bs;++i) {
				t = i*bs;
				sum = 0;
				for (j = 0; j < i;++j) {
					sum += UT[t+j] * x[j];
				}
				x[i] = (b[i] - sum) / UT[t+i];
			}
			v = bs + k;
			for(i = 0; i < bs;++i) {
				A[v] = x[i];
				U12T[u+i] = x[i];
				v += stride;
			}
		}
		
		mtranspose(U12T,Nb,bs,U12);
		mmult(U12T,U12,UT,Nb,bs,Nb);
		free(U12T);
		free(U12);
		free(b);
		free(x);
		for (i = 0; i < Nb; ++i) {
			u = bs * stride + bs + i * stride;
			w = i * Nb;
			for(j = i; j < Nb;j++) {
				A[j + u] -= UT[j + w];
			}
		}
		
		sc = rbcholu(A + bs * stride + bs,Nb,stride,UB,UT);
		if (sc == -1) {
			return -1;
		}
	}
	
	return sc;
}