示例#1
2
void draw_planet(HDC hDC, planetstruct *planet)
{
	HBRUSH hbrColor, hbrOld;
	gravstruct *gp = &gravs;
	double D;						// a DX variable to work with
	unsigned char cmpt;

	D = POS(X) * POS(X) + POS(Y) * POS(Y) + POS(Z) * POS(Z);
	if (D < COLLIDE)
		D = COLLIDE;
	D = sqrt(D);
	D = D * D * D;
	for (cmpt = X; cmpt < DIMENSIONS; cmpt++) {
		ACC(cmpt) = POS(cmpt) * GRAV / D;
		if (iDamping) {
			if (ACC(cmpt) > MaxA)
				ACC(cmpt) = MaxA;
			else if (ACC(cmpt) < -MaxA)
				ACC(cmpt) = -MaxA;
			VEL(cmpt) = VEL(cmpt) + ACC(cmpt);
			VEL(cmpt) *= DAMP;
		} else {
			// update velocity
			VEL(cmpt) = VEL(cmpt) + ACC(cmpt);
		}
		// update position
		POS(cmpt) = POS(cmpt) + VEL(cmpt);
	}

	gp->x = planet->xi;
	gp->y = planet->yi;

	if (POS(Z) > -ALMOST) {
		planet->xi = (unsigned int)
			((double) gp->width * (HALF + POS(X) / (POS(Z) + DIST)));
		planet->yi = (unsigned int)
			((double) gp->height * (HALF + POS(Y) / (POS(Z) + DIST)));
	}
	else
		planet->xi = planet->yi = -1;	

	// Mask
	hbrOld = (HBRUSH)SelectObject(hDC, (HBRUSH)GetStockObject(BLACK_BRUSH));
	Planet(gp->x, gp->y);

	if (iTrails)
		SetPixel(hDC, gp->x, gp->y, PALETTEINDEX(100));

	// Move
	gp->x = planet->xi;
	gp->y = planet->yi;
	planet->ri = RADIUS;

	if (iColorCycle) {
		if (planet->colors++ > (PALSIZE-21))
			planet->colors = 1;
	}

	// Redraw
	hbrColor = CreateSolidBrush(PALETTEINDEX(planet->colors));
	SelectObject(hDC, hbrColor);
	Planet(gp->x, gp->y);

	SelectObject(hDC, hbrOld);
	DeleteObject(hbrColor);
}
示例#2
0
/*
 * compute cross correlation between two stereo signals, but for one channel only. The results should be at most n1+n2 samples.
**/
static int compute_cross_correlation_interleaved(int16_t *s1, int n1, int16_t *s2, int n2, int64_t *xcorr, int xcorr_nsamples, MSAudioDiffProgressNotify func, void *user_data, int channel_num, int delta){
	int64_t acc;
	int64_t max=0;
	int max_index=0;
	int i,k;
	int completion=0;
	int prev_completion=0;

	for (i=0;i<delta;++i){
		completion=100*(i+(2*delta*channel_num))/(delta*4);
		acc=0;

		for(k=0; k<MIN(n1,n2-delta+i); k++){
			int k2=k+delta-i;
			ACC((2*k)+channel_num,(2*k2)+channel_num,0);
		}
		xcorr[i]=acc;
		acc = acc > 0 ? acc : -acc;
		if (acc>max) {
			max=acc;
			max_index=i;
		}
		if (func && completion>prev_completion)
			func(user_data,completion);
		prev_completion=completion;
	}
	for (i=delta;i<2*delta;++i){
		completion=100*(i+(2*delta*channel_num))/(delta*4);
		acc=0;

		for(k=0; k<MIN(n1+delta-i,n2); k++){
			int k1;
			k1=k+i-delta;
			ACC((2*k1)+channel_num,(2*k)+channel_num,0);
		}
		xcorr[i] = acc;
		acc = acc > 0 ? acc : -acc;
		if (acc>max) {
			max=acc;
			max_index=i;
		}
		if (func && completion>prev_completion)
			func(user_data,completion);
		prev_completion=completion;
	}
	return max_index;
}
示例#3
0
/*
 * compute cross correlation between two signals. The results should n1+n2 samples.
**/
static int compute_cross_correlation(int16_t *s1, int n1, int16_t *s2, int n2, int64_t *xcorr, int xcorr_nsamples, MSAudioDiffProgressNotify func, void *user_data, int delta){
	int64_t acc;
	int64_t max=0;
	int max_index=0;
	int i,k;
	int completion=0;
	int prev_completion=0;

#define STEP 4
#define ACC(k1,k2,s) acc+=(int64_t)( (int)s1[k1+s]*(int)s2[k2+s]);
	for(i=0;i<delta;++i){
		completion=100*i/(delta*2);
		acc=0;

		for(k=0; k<MIN(n1,n2-delta+i); k+=STEP){
			int k2=k+delta-i;
			ACC(k,k2,0);
			ACC(k,k2,1);
			ACC(k,k2,2);
			ACC(k,k2,3);
		}
		xcorr[i]=acc;
		acc = acc > 0 ? acc : -acc;
		if (acc>max) {
			max=acc;
			max_index=i;
		}
		if (func && completion>prev_completion)
			func(user_data,completion);
		prev_completion=completion;
	}
	for(i=delta;i<2*delta;++i){
		completion=100*i/(delta*2);
		acc=0;

		for(k=0; k<MIN(n1+delta-i,n2); k+=STEP){
			int k1;
			k1=k+i-delta;
			ACC(k1,k,0);
			ACC(k1,k,1);
			ACC(k1,k,2);
			ACC(k1,k,3);
		}
		xcorr[i] = acc;
		acc = acc > 0 ? acc : -acc;
		if (acc>max) {
			max=acc;
			max_index=i;
		}
		if (func && completion>prev_completion)
			func(user_data,completion);
		prev_completion=completion;
	}
	return max_index;
}
示例#4
0
void
gnm_complete_start (GnmComplete *complete, char const *text)
{
	g_return_if_fail (complete != NULL);
	g_return_if_fail (GNM_IS_COMPLETE (complete));
	g_return_if_fail (text != NULL);

	if (complete->text != text) {
		g_free (complete->text);
		complete->text = g_strdup (text);
	}

	if (complete->idle_tag == 0)
		complete->idle_tag = g_idle_add (complete_idle, complete);

	if (ACC(complete)->start_over)
		ACC(complete)->start_over (complete);
}
示例#5
0
static void
draw_planet(ModeInfo * mi, planetstruct * planet)
{
	Display    *display = MI_DISPLAY(mi);
	Window      window = MI_WINDOW(mi);
	GC          gc = MI_GC(mi);
	gravstruct *gp = &gravs[MI_SCREEN(mi)];
	double      D;		/* A distance variable to work with */
	register unsigned char cmpt;

	D = POS(X) * POS(X) + POS(Y) * POS(Y) + POS(Z) * POS(Z);
	if (D < COLLIDE)
		D = COLLIDE;
	D = sqrt(D);
	D = D * D * D;
	for (cmpt = X; cmpt < DIMENSIONS; cmpt++) {
		ACC(cmpt) = POS(cmpt) * GRAV / D;
		if (decay) {
			if (ACC(cmpt) > MaxA)
				ACC(cmpt) = MaxA;
			else if (ACC(cmpt) < -MaxA)
				ACC(cmpt) = -MaxA;
			VEL(cmpt) = VEL(cmpt) + ACC(cmpt);
			VEL(cmpt) *= DAMP;
		} else {
			/* update velocity */
			VEL(cmpt) = VEL(cmpt) + ACC(cmpt);
		}
		/* update position */
		POS(cmpt) = POS(cmpt) + VEL(cmpt);
	}

	gp->x = planet->xi;
	gp->y = planet->yi;

	if (POS(Z) > -ALMOST) {
		planet->xi = (int)
			((double) gp->width * (HALF + POS(X) / (POS(Z) + DIST)));
		planet->yi = (int)
			((double) gp->height * (HALF + POS(Y) / (POS(Z) + DIST)));
	} else
		planet->xi = planet->yi = -1;

	/* Mask */
	XSetForeground(display, gc, MI_BLACK_PIXEL(mi));
	Planet(gp->x, gp->y);
	if (trail) {
		XSetForeground(display, gc, planet->colors);
		XDrawPoint(display, MI_WINDOW(mi), gc, gp->x, gp->y);
	}
	/* Move */
	gp->x = planet->xi;
	gp->y = planet->yi;
	planet->ri = RADIUS;

	/* Redraw */
	XSetForeground(display, gc, planet->colors);
	Planet(gp->x, gp->y);
}
示例#6
0
static gint
complete_idle (gpointer data)
{
	GnmComplete *complete = data;

	g_return_val_if_fail (complete->idle_tag != 0, FALSE);

	if (ACC(complete)->search_iteration (complete))
		return TRUE;

	complete->idle_tag = 0;

	return FALSE;
}
示例#7
0
void Extraction(char *img_path, char *fv_path)
{
  CImage *cimg=NULL;
  Histogram *acc=NULL;

  cimg = ReadCImage(img_path);

  acc = ACC(cimg);

  WriteFileHistogram(acc,fv_path);
  DestroyHistogram(&acc);
  DestroyCImage(&cimg);

}
示例#8
0
int main(int argc, char** argv)
{
  CImage *cimg=NULL;
  Histogram *acc=NULL;

  if (argc != 3) {
    fprintf(stderr,"usage: acc_extraction <img_path> <fv_path>\n");
    exit(-1);
  }

  cimg = ReadCImage(argv[1]);

  acc = ACC(cimg);

  WriteFileHistogram(acc,argv[2]);
  DestroyHistogram(&acc);
  DestroyCImage(&cimg);

  return(0);
}
示例#9
0
int sys_fstat(int handle, struct stat * statbuf)
{
    struct file_time ft;
    unsigned long gmt;
    int info;

    if ((info = rm_ioctl_getattr(handle)) == -1)
	return -1;

    if (!(info & 0x80)) {	/* file */
	long oldpos;
	statbuf->st_mode = S_IFREG;
	oldpos = rm_lseek(handle, 0L, 1);
	statbuf->st_size = rm_lseek(handle, 0L, 2);
	rm_lseek(handle, oldpos, 0);
	statbuf->st_dev = 0;
    } else {			/* device */
	statbuf->st_mode = S_IFCHR;
	statbuf->st_size = 0L;
	statbuf->st_dev = (long) (info & 0x3F);
    }
    rm_getftime(handle, &ft.ft_date, &ft.ft_time);
    gmt = filetime2gmt(&ft);
    statbuf->st_atime = gmt;
    statbuf->st_ctime = gmt;
    statbuf->st_mtime = gmt;
    statbuf->st_mode |= ACC(S_IREAD | S_IWRITE);
    statbuf->st_uid = 0L;
    statbuf->st_gid = 0L;
    statbuf->st_rdev = statbuf->st_dev;
    statbuf->st_nlink = 1L;
    statbuf->st_ino = ino++;
    statbuf->st_attr = 0L;
    statbuf->st_reserved = 0L;
    return 0;
}
示例#10
0
int sys_stat(char *filename, struct stat * statbuf)
{
    struct find_t find;
    unsigned long gmt;

    if (filename == NULL)
	return -1;

    /* find : dir, system, hidden */
    if (rm_findfirst(filename, 0x16, &find) == -1) {
	/* error: must be a root dir  or illegal drive */
	char path[260];
	unsigned olddrive, newdrive;
	int ret;

	if (rm_get_exterror_code() == 0x53) /* fail on int24 */
	    return -1;

	olddrive = rm_getdrive();
	if (filename[1] == ':') {
	    newdrive = (unsigned) *filename;
	    if (newdrive >= 'a')
		newdrive -= 0x20;
	    if (newdrive < 'A' || newdrive > 'Z')
		return -1;
	    newdrive -= 'A';
	    if (newdrive != olddrive)
		rm_setdrive(newdrive);
	} else
	    newdrive = olddrive;
	rm_getcwd(0, path);
	ret = rm_chdir(filename);
	rm_chdir(path); 	/* restore path */
	if (newdrive != olddrive)	/* restore drive */
	    rm_setdrive(olddrive);
	if (ret == -1)
	    return -1;		/* no root, error */
	statbuf->st_mode = S_IFDIR | ACC(S_IREAD | S_IWRITE | S_IEXEC);
	statbuf->st_size = 0L;
	statbuf->st_dev = (long) newdrive;
	statbuf->st_attr = 0;
	gmt = 0;
    } else {
	struct file_time ft;

	statbuf->st_attr = (long) find.attrib;
	if (find.attrib & 0x10) {	/* directory */
	    statbuf->st_mode = S_IFDIR | ACC(S_IREAD | S_IWRITE | S_IEXEC);
	    statbuf->st_size = 0L;
	} else {
	    statbuf->st_mode = S_IFREG; /* file */
	    statbuf->st_mode |= ACC(S_IREAD);
	    if (!(find.attrib & 0x1))	/* test read only */
		statbuf->st_mode |= ACC(S_IWRITE);	/* Read-write access */
	    statbuf->st_size = (DWORD) find.size_hi << 16 | find.size_lo;
	}
	ft.ft_date = find.wr_date;
	ft.ft_time = find.wr_time;
	gmt = filetime2gmt(&ft);
	if (find.name[1] == ':')
	    statbuf->st_dev = (long) (find.name[0] - 'A');
	else
	    statbuf->st_dev = rm_getdrive();
    }
    /* all */
    statbuf->st_atime = gmt;
    statbuf->st_ctime = gmt;
    statbuf->st_mtime = gmt;
    statbuf->st_rdev = statbuf->st_dev;
    statbuf->st_uid = 0L;
    statbuf->st_gid = 0L;
    statbuf->st_ino = ino++;
    statbuf->st_nlink = 1L;
    statbuf->st_reserved = 0L;
    return 0;
}
示例#11
0
static VALUE
acc_timestamp(VALUE rcv, SEL sel)
{
    return DBL2NUM(ACC(rcv)->timestamp);
}
示例#12
0
static VALUE
acc_z(VALUE rcv, SEL sel)
{
    return DBL2NUM(ACC(rcv)->z);
}
示例#13
0
/**
 * Recursive Newton-Euler algorithm.
 *
 * @Note the parameter \p stride which is used to allow for input and output
 * arrays which are 2-dimensional but in column-major (Matlab) order.  We
 * need to access rows from the arrays.
 *
 */
void
newton_euler (
	Robot	*robot,		/*!< robot object  */
	double	*tau,		/*!< returned joint torques */
	double	*qd,		/*!< joint velocities */
	double	*qdd,		/*!< joint accelerations */
	double	*fext,		/*!< external force on manipulator tip */
	int	stride		/*!< indexing stride for qd, qdd */
) {
	Vect			t1, t2, t3, t4;
	Vect			qdv, qddv;
	Vect			F, N;
	Vect		z0 = {0.0, 0.0, 1.0};
	Vect		zero = {0.0, 0.0, 0.0};
	Vect		f_tip = {0.0, 0.0, 0.0};
	Vect		n_tip = {0.0, 0.0, 0.0};
	register int		j;
	double			t;
	Link			*links = robot->links;

	/*
	 * angular rate and acceleration vectors only have finite
	 * z-axis component
	 */
	qdv = qddv = zero;

	/* setup external force/moment vectors */
	if (fext) {
		f_tip.x = fext[0];
		f_tip.y = fext[1];
		f_tip.z = fext[2];
		n_tip.x = fext[3];
		n_tip.y = fext[4];
		n_tip.z = fext[5];
	}


/******************************************************************************
 * forward recursion --the kinematics
 ******************************************************************************/

	if (robot->dhtype == MODIFIED) {
	    /*
	     * MODIFIED D&H CONVENTIONS
	     */
	    for (j = 0; j < robot->njoints; j++) {

		/* create angular vector from scalar input */
		qdv.z = qd[j*stride]; 
		qddv.z = qdd[j*stride];

		switch (links[j].sigma) {
		case REVOLUTE:
			/* 
			 * calculate angular velocity of link j
			 */
			if (j == 0)
				*OMEGA(j) = qdv;
			else {
				rot_trans_vect_mult (&t1, ROT(j), OMEGA(j-1));
				vect_add (OMEGA(j), &t1, &qdv);
			}

			/*
			 * calculate angular acceleration of link j 
			 */
			if (j == 0) 
				*OMEGADOT(j) = qddv;
			else {
				rot_trans_vect_mult (&t3, ROT(j), OMEGADOT(j-1));
				vect_cross (&t2, &t1, &qdv);
				vect_add (&t1, &t2, &t3);
				vect_add (OMEGADOT(j), &t1, &qddv);
			}

			/*
			 * compute acc[j]
			 */
			if (j == 0) {
				t1 = *robot->gravity;
			} else {
				vect_cross(&t1, OMEGA(j-1), PSTAR(j));
				vect_cross(&t2, OMEGA(j-1), &t1);
				vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));
				vect_add(&t1, &t1, &t2);
				vect_add(&t1, &t1, ACC(j-1));
			}
			rot_trans_vect_mult(ACC(j), ROT(j), &t1);

			break;

		case PRISMATIC:
			/* 
			 * calculate omega[j]
			 */
			if (j == 0)
				*(OMEGA(j)) = qdv;
			else
				rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));

			/*
			 * calculate alpha[j] 
			 */
			if (j == 0)
				*(OMEGADOT(j)) = qddv;
			else
				rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));

			/*
			 * compute acc[j]
			 */
			if (j == 0) {
				*ACC(j) = *robot->gravity;
			} else {
				vect_cross(&t1, OMEGADOT(j-1), PSTAR(j));

				vect_cross(&t3, OMEGA(j-1), PSTAR(j));
				vect_cross(&t2, OMEGA(j-1), &t3);
				vect_add(&t1, &t1, &t2);
				vect_add(&t1, &t1, ACC(j-1));
				rot_trans_vect_mult(ACC(j), ROT(j), &t1);

				rot_trans_vect_mult(&t2, ROT(j), OMEGA(j-1));
				vect_cross(&t1, &t2, &qdv);
				scal_mult(&t1, &t1, 2.0);
				vect_add(ACC(j), ACC(j), &t1);

				vect_add(ACC(j), ACC(j), &qddv);
			}

			break;
		}

		/*
		 * compute abar[j]
		 */
		vect_cross(&t1, OMEGADOT(j), R_COG(j));
		vect_cross(&t2, OMEGA(j), R_COG(j));
		vect_cross(&t3, OMEGA(j), &t2);
		vect_add(ACC_COG(j), &t1, &t3);
		vect_add(ACC_COG(j), ACC_COG(j), ACC(j));

#ifdef	DEBUG
		vect_print("w", OMEGA(j));
		vect_print("wd", OMEGADOT(j));
		vect_print("acc", ACC(j));
		vect_print("abar", ACC_COG(j));
#endif
	    }
	} else {
	    /*
	     * STANDARD D&H CONVENTIONS
	     */
	    for (j = 0; j < robot->njoints; j++) {

		/* create angular vector from scalar input */
		qdv.z = qd[j*stride]; 
		qddv.z = qdd[j*stride];

		switch (links[j].sigma) {
		case REVOLUTE:
			/* 
			 * calculate omega[j]
			 */
			if (j == 0)
				t1 = qdv;
			else
				vect_add (&t1, OMEGA(j-1), &qdv);
			rot_trans_vect_mult (OMEGA(j), ROT(j), &t1);

			/*
			 * calculate alpha[j] 
			 */
			if (j == 0) 
				t3 = qddv;
			else {
				vect_add (&t1, OMEGADOT(j-1), &qddv);
				vect_cross (&t2, OMEGA(j-1), &qdv);
				vect_add (&t3, &t1, &t2);
			}
			rot_trans_vect_mult (OMEGADOT(j), ROT(j), &t3);

			/*
			 * compute acc[j]
			 */
			vect_cross(&t1, OMEGADOT(j), PSTAR(j));
			vect_cross(&t2, OMEGA(j), PSTAR(j));
			vect_cross(&t3, OMEGA(j), &t2);
			vect_add(ACC(j), &t1, &t3);
			if (j == 0) {
				rot_trans_vect_mult(&t1, ROT(j), robot->gravity);
			} else 
				rot_trans_vect_mult(&t1, ROT(j), ACC(j-1));
			vect_add(ACC(j), ACC(j), &t1);
			break;

		case PRISMATIC:
			/* 
			 * calculate omega[j]
			 */
			if (j == 0)
				*(OMEGA(j)) = zero;
			else
				rot_trans_vect_mult (OMEGA(j), ROT(j), OMEGA(j-1));

			/*
			 * calculate alpha[j] 
			 */
			if (j == 0)
				*(OMEGADOT(j)) = zero;
			else
				rot_trans_vect_mult (OMEGADOT(j), ROT(j), OMEGADOT(j-1));

			/*
			 * compute acc[j]
			 */
			if (j == 0) {
				vect_add(&qddv, &qddv, robot->gravity);
				rot_trans_vect_mult(ACC(j), ROT(j), &qddv);
			} else {
				vect_add(&t1, &qddv, ACC(j-1));
				rot_trans_vect_mult(ACC(j), ROT(j), &t1);

			}

			vect_cross(&t1, OMEGADOT(j), PSTAR(j));
			vect_add(ACC(j), ACC(j), &t1);

			rot_trans_vect_mult(&t1, ROT(j), &qdv);
			vect_cross(&t2, OMEGA(j), &t1);
			scal_mult(&t2, &t2, 2.0);
			vect_add(ACC(j), ACC(j), &t2);

			vect_cross(&t2, OMEGA(j), PSTAR(j));
			vect_cross(&t3, OMEGA(j), &t2);
			vect_add(ACC(j), ACC(j), &t3);
			break;
		}
		/*
		 * compute abar[j]
		 */
		vect_cross(&t1, OMEGADOT(j), R_COG(j));
		vect_cross(&t2, OMEGA(j), R_COG(j));
		vect_cross(&t3, OMEGA(j), &t2);
		vect_add(ACC_COG(j), &t1, &t3);
		vect_add(ACC_COG(j), ACC_COG(j), ACC(j));

#ifdef	DEBUG
		vect_print("w", OMEGA(j));
		vect_print("wd", OMEGADOT(j));
		vect_print("acc", ACC(j));
		vect_print("abar", ACC_COG(j));
#endif
	    }
	}

/******************************************************************************
 * backward recursion part --the kinetics
 ******************************************************************************/

	if (robot->dhtype == MODIFIED) {
	    /*
	     * MODIFIED D&H CONVENTIONS
	     */
	    for (j = robot->njoints - 1; j >= 0; j--) {

		/*
		 * compute F[j]
		 */
		scal_mult (&F, ACC_COG(j), M(j));

		/*
		 * compute f[j]
		 */
		if (j == (robot->njoints-1))
			t1 = f_tip;
		else
			rot_vect_mult (&t1, ROT(j+1), f(j+1));
		vect_add (f(j), &t1, &F);

		 /*
		  * compute N[j]
		  */
		mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
		mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
		vect_cross(&t4, OMEGA(j), &t3);
		vect_add(&N, &t2, &t4);

		 /*
		  * compute n[j]
		  */
		if (j == (robot->njoints-1))
			t1 = n_tip;
		else {
			rot_vect_mult(&t1, ROT(j+1), n(j+1));
			rot_vect_mult(&t4, ROT(j+1), f(j+1));
			vect_cross(&t3, PSTAR(j+1), &t4);

			vect_add(&t1, &t1, &t3);
		}

		vect_cross(&t2, R_COG(j), &F);
		vect_add(&t1, &t1, &t2);
		vect_add(n(j), &t1, &N);

#ifdef	DEBUG
		vect_print("f", f(j));
		vect_print("n", n(j));
#endif
	    }

	} else {
	    /*
	     * STANDARD D&H CONVENTIONS
	     */
	    for (j = robot->njoints - 1; j >= 0; j--) {

		/*
		 * compute f[j]
		 */
		scal_mult (&t4, ACC_COG(j), M(j));
		if (j != (robot->njoints-1)) {
			rot_vect_mult (&t1, ROT(j+1), f(j+1));
			vect_add (f(j), &t4, &t1);
		} else
			vect_add (f(j), &t4, &f_tip);

		 /*
		  * compute n[j]
		  */

			/* cross(pstar+r,Fm(:,j)) */
		vect_add(&t2, PSTAR(j), R_COG(j));
		vect_cross(&t1, &t2, &t4);

		if (j != (robot->njoints-1)) {
			/* cross(R'*pstar,f) */
			rot_trans_vect_mult(&t2, ROT(j+1), PSTAR(j));
			vect_cross(&t3, &t2, f(j+1));

			/* nn += R*(nn + cross(R'*pstar,f)) */
			vect_add(&t3, &t3, n(j+1));
			rot_vect_mult(&t2, ROT(j+1), &t3);
			vect_add(&t1, &t1, &t2);
		} else {
			/* cross(R'*pstar,f) */
			vect_cross(&t2, PSTAR(j), &f_tip);

			/* nn += R*(nn + cross(R'*pstar,f)) */
			vect_add(&t1, &t1, &t2);
			vect_add(&t1, &t1, &n_tip);
		}

		mat_vect_mult(&t2, INERTIA(j), OMEGADOT(j));
		mat_vect_mult(&t3, INERTIA(j), OMEGA(j));
		vect_cross(&t4, OMEGA(j), &t3);
		vect_add(&t2, &t2, &t4);

		vect_add(n(j), &t1, &t2);
#ifdef	DEBUG
		vect_print("f", f(j));
		vect_print("n", n(j));
#endif
	    }
	}

	/*
	 *  Compute the torque total for each axis
	 *
	 */
	for (j=0; j < robot->njoints; j++) {
		double	t;
		Link	*l = &links[j];

		if (robot->dhtype == MODIFIED)
			t1 = z0;
		else
			rot_trans_vect_mult(&t1, ROT(j), &z0);

		switch (l->sigma) {
		case REVOLUTE:
			t = vect_dot(n(j), &t1);
			break;
		case PRISMATIC:
			t = vect_dot(f(j), &t1);
			break;
		}

		/*
		 * add actuator dynamics and friction
		 */
		t +=   l->G * l->G * l->Jm * qdd[j*stride]; // inertia
        t += l->G * l->G * l->B * qd[j*stride];    // viscous friction
        t += fabs(l->G) * (
			(qd[j*stride] > 0 ? l->Tc[0] : 0.0) +    // Coulomb friction
			(qd[j*stride] < 0 ? l->Tc[1] : 0.0)
		);
		tau[j*stride] = t;
	}
}
示例#14
0
文件: vhsfix.c 项目: GNOME/gegl
static gboolean
process (GeglOperation       *operation,
         GeglBuffer          *input,
         GeglBuffer          *output,
         const GeglRectangle *result,
         gint                 level)
{
  GeglProperties *o;
  Priv           *p;

  o = GEGL_PROPERTIES (operation);
  p = (Priv*)o->user_data;
  if (p == NULL)
    init (o);
  p = (Priv*)o->user_data;

  {
    gint pixels = result->width * result->height;
    gfloat *acc[TEMP_BUFS];
    gfloat *buf;
    gint i;
    gfloat totdiff = 0.0;
    int x, y;
    int last_set[8192]={0,};
    for (i = 0; i < TEMP_BUFS; i++)
      acc[i] = g_new (gfloat, pixels * 4);
    buf = g_new (gfloat, pixels * 4);

    if (p->uninitialized)
    {
      gegl_buffer_get (input, result, 1.0, babl_format ("RGBA float"), buf, GEGL_AUTO_ROWSTRIDE, GEGL_ABYSS_NONE);
      for (i = 0; i<TEMP_BUFS; i++)
      {
        gegl_buffer_set (p->acc[i], result, 0, babl_format ("RGBA float"), buf, GEGL_AUTO_ROWSTRIDE);
        gegl_buffer_get (input, result, 1.0, babl_format ("RGBA float"), acc[i], GEGL_AUTO_ROWSTRIDE, GEGL_ABYSS_NONE);
      }
      p->uninitialized = 0;
    }
    else
    {

    for (i = TEMP_BUFS-1; i > 0; i--)
    {
      gegl_buffer_get (p->acc[i-1], result, 1.0, babl_format ("RGBA float"), acc[i], GEGL_AUTO_ROWSTRIDE, GEGL_ABYSS_NONE);
      gegl_buffer_set (p->acc[i], result, 0, babl_format ("RGBA float"), acc[i], GEGL_AUTO_ROWSTRIDE);
    }

    gegl_buffer_get (input, result, 1.0, babl_format ("RGBA float"), acc[0], GEGL_AUTO_ROWSTRIDE, GEGL_ABYSS_NONE);
    gegl_buffer_get (input, result, 1.0, babl_format ("RGBA float"), buf, GEGL_AUTO_ROWSTRIDE, GEGL_ABYSS_NONE);
    gegl_buffer_set (p->acc[0], result, 0, babl_format ("RGBA float"), acc[0], GEGL_AUTO_ROWSTRIDE);
    }

    for (i=0;i<pixels;i++)
      {
        gint c;
        for (c=0;c<4;c++)
        {
          int t;
          float avg[4] = {0,0,0,0};
          float avg2[4] = {0,0,0,0};

          for (t = 2; t < TEMP_BUFS; t++)
            avg[c] += acc[t][i*4+c];
          avg[c] /= (TEMP_BUFS);

          for (t = 1; t < TEMP_BUFS-2; t++)
            avg2[c] += acc[t][i*4+c];
          avg2[c] /= (TEMP_BUFS - 1 - 2);

#define ACC(a) acc[a][i*4+c]
          if (fabs (ACC(0) - avg[c]) > 0.05)
            ACC(0) = avg2[c];
        }
      }

    i = 0;

    for (y = result->y; y < result->y + result->height-1; y++)
      for (x = result->x; x < result->x + result->width; x++)
      {
        int c;
        float diff = 0;
        for (c = 0; c < 3; c++)
          if (acc[0][i * 4 + c] != buf[i * 4 + c])
          {
            diff+=fabs(acc[0][i * 4 + c] - buf[i * 4 + c]) *
                  fabs(acc[0][i * 4 + c] - buf[i * 4 + c]);
          }
        diff = sqrtf (diff);
        if (diff > 0.02f)
        {
          if (last_set[x] < 1) /* permit up to two pixels wide artifacts */
          {
            if (y > 1)
            for (c = 0; c < 4; c++)
              buf[i * 4 + c] = acc[0][i * 4 + c] * 0.2 + buf[(i-result->width) * 4 + c] * 0.4 + buf[(i-result->width) * 4 + c] * 0.4;
            else
            for (c = 0; c < 4; c++)
              buf[i * 4 + c] = acc[0][i * 4 + c];
            last_set[x]++;
          }
          else if (last_set[x] > 1)
          {
            if (y > 1)
            for (c = 0; c < 4; c++)
              buf[i * 4 + c] = acc[0][i * 4 + c] * 0.2 + buf[(i-result->width) * 4 + c] * 0.4 + buf[(i-result->width) * 4 + c] * 0.4;

            else
            for (c = 0; c < 4; c++)
              buf[i * 4 + c] = acc[0][i * 4 + c];
            last_set[x] = 0;
          }
        }
        else
        {
          last_set[x]++;
        }
        i++;
      }

    /* if reset, refetch buf .. and set scene-change flag */

    totdiff = 0;
    i = 0;
    for (y = result->y; y < result->y + result->height; y++)
      for (x = result->x; x < result->x + result->width; x++)
      {
        int c;
        float diff = 0;
        for (c = 0; c < 3; c++)
          diff+=fabs(acc[0][i * 4 + c] - acc[1][i * 4 + c]) *
                fabs(acc[0][i * 4 + c] - acc[1][i * 4 + c]);
        if (diff != 0.0)
          totdiff += sqrtf (diff);
        i++;
      }
    totdiff /= i;

    if (totdiff > 0.04)
    {
      gegl_buffer_get (input, result, 1.0, babl_format ("RGBA float"), buf, GEGL_AUTO_ROWSTRIDE, GEGL_ABYSS_NONE);
      p->uninitialized = 1;
    }

    gegl_buffer_set (output, result, 0, babl_format ("RGBA float"), buf, GEGL_AUTO_ROWSTRIDE);
    for (i = 0; i < TEMP_BUFS; i++)
      g_free (acc[i]);
    g_free (buf);
  }

  return  TRUE;
}