int computemaxpos(int level)
{
int i,result;
for(i=1,result=level+1;i<N;i++)
{
result*=10;
}
return findu(result);
}
int main(){
int n, m, i, j, n1, n2;
double cost;
while(scanf("%d", &n) != EOF){
for(i=1; i<=n; i++){
scanf("%d %d", &coord[i][0], &coord[i][1]);
parents[i] = i;
}
cost=0;
scanf("%d", &m);
for(i=0; i<m; i++){
scanf("%d %d", &n1, &n2);
if(!findu(n1, n2)){
uni(n1, n2);
}
}
priority_queue<Edge> q;
Edge ed;
for(i=1; i<=n; i++){
for(j=i+1; j<=n; j++){
ed.a = i;
ed.b = j;
ed.w = dist(ed.a, ed.b);
q.push(ed);
}
}
while(!q.empty()){
ed = q.top(); q.pop();
if(!findu(ed.a, ed.b)){
uni(ed.a, ed.b);
cost += dist(ed.a, ed.b);
}
}
printf("%.2lf\n", cost);
}
}
double qfc(double* lb1, double* nc1, int* n1, int *r1in, double *sigmain,
double *c1in, int *lim1in, double *accin, double* trace, int* ifault)
/* distribution function of a linear combination of non-central
chi-squared random variables :
input:
lb[j] coefficient of j-th chi-squared variable
nc[j] non-centrality parameter
n[j] degrees of freedom
j = 0, 2 ... r-1
sigma coefficient of standard normal variable
c point at which df is to be evaluated
lim maximum number of terms in integration
acc maximum error
output:
ifault = 1 required accuracy NOT achieved
2 round-off error possibly significant
3 invalid parameters
4 unable to locate integration parameters
5 out of memory
trace[0] absolute sum
trace[1] total number of integration terms
trace[2] number of integrations
trace[3] integration interval in final integration
trace[4] truncation point in initial integration
trace[5] s.d. of initial convergence factor
trace[6] cycles to locate integration parameters */
{
int r1 = r1in[0], lim1 = lim1in[0];
double sigma = sigmain[0], c1 = c1in[0], acc = accin[0];
int j, nj, nt, ntm; double acc1, almx, xlim, xnt, xntm;
double utx, tausq, sd, intv, intv1, x, up, un, d1, d2, lj, ncj;
double qfval = 0;
static int rats[]={1,2,4,8};
if (setjmp(env) != 0) { *ifault=4; goto endofproc; }
r=r1; lim=lim1; c=c1;
n=n1; lb=lb1; nc=nc1;
for ( j = 0; j<7; j++ ) trace[j] = 0.0;
*ifault = 0; count = 0;
intl = 0.0; ersm = 0.0;
qfval = -1.0; acc1 = acc; ndtsrt = TRUE; fail = FALSE;
xlim = (double)lim;
th=(int*)malloc(r*(sizeof(int)));
if (! th) { *ifault=5; goto endofproc; }
/* find mean, sd, max and min of lb,
check that parameter values are valid */
sigsq = square(sigma); sd = sigsq;
lmax = 0.0; lmin = 0.0; mean = 0.0;
for (j=0; j<r; j++ )
{
nj = n[j]; lj = lb[j]; ncj = nc[j];
if ( nj < 0 || ncj < 0.0 ) { *ifault = 3; goto endofproc; }
sd = sd + square(lj) * (2 * nj + 4.0 * ncj);
mean = mean + lj * (nj + ncj);
if (lmax < lj) lmax = lj ; else if (lmin > lj) lmin = lj;
}
if ( sd == 0.0 )
{ qfval = (c > 0.0) ? 1.0 : 0.0; goto endofproc; }
if ( lmin == 0.0 && lmax == 0.0 && sigma == 0.0 )
{ *ifault = 3; goto endofproc; }
sd = sqrt(sd);
almx = (lmax < - lmin) ? - lmin : lmax;
/* starting values for findu, ctff */
utx = 16.0 / sd; up = 4.5 / sd; un = - up;
/* truncation point with no convergence factor */
findu(&utx, .5 * acc1);
/* does convergence factor help */
if (c != 0.0 && (almx > 0.07 * sd))
{
tausq = .25 * acc1 / cfe(c);
if (fail) fail = FALSE ;
else if (truncation(utx, tausq) < .2 * acc1)
{
sigsq = sigsq + tausq;
findu(&utx, .25 * acc1);
trace[5] = sqrt(tausq);
}
}
trace[4] = utx; acc1 = 0.5 * acc1;
/* find RANGE of distribution, quit if outside this */
l1:
d1 = ctff(acc1, &up) - c;
if (d1 < 0.0) { qfval = 1.0; goto endofproc; }
d2 = c - ctff(acc1, &un);
if (d2 < 0.0) { qfval = 0.0; goto endofproc; }
/* find integration interval */
intv = 2.0 * pi / ((d1 > d2) ? d1 : d2);
/* calculate number of terms required for main and
auxillary integrations */
xnt = utx / intv; xntm = 3.0 / sqrt(acc1);
if (xnt > xntm * 1.5)
{
/* parameters for auxillary integration */
if (xntm > xlim) { *ifault = 1; goto endofproc; }
ntm = (int)floor(xntm+0.5);
intv1 = utx / ntm; x = 2.0 * pi / intv1;
if (x <= fabs(c)) goto l2;
/* calculate convergence factor */
tausq = .33 * acc1 / (1.1 * (cfe(c - x) + cfe(c + x)));
if (fail) goto l2;
acc1 = .67 * acc1;
/* auxillary integration */
integrate(ntm, intv1, tausq, FALSE );
xlim = xlim - xntm; sigsq = sigsq + tausq;
trace[2] = trace[2] + 1; trace[1] = trace[1] + ntm + 1;
/* find truncation point with new convergence factor */
findu(&utx, .25 * acc1); acc1 = 0.75 * acc1;
goto l1;
}
/* main integration */
l2:
trace[3] = intv;
if (xnt > xlim) { *ifault = 1; goto endofproc; }
nt = (int)floor(xnt+0.5);
integrate(nt, intv, 0.0, TRUE );
trace[2] = trace[2] + 1; trace[1] = trace[1] + nt + 1;
qfval = 0.5 - intl;
trace[0] = ersm;
/* test whether round-off error could be significant
allow for radix 8 or 16 machines */
up=ersm; x = up + acc / 10.0;
for (j=0;j<4;j++) { if (rats[j] * x == rats[j] * up) *ifault = 2; }
endofproc :
free((char*)th);
trace[6] = (double)count;
return qfval;
}
REAL8 cdfwchisq(qfvars *vars, REAL8 sigma, REAL8 acc, INT4 *ifault)
{
INT4 nt, ntm;
REAL8 acc1, almx, xlim, xnt, xntm;
REAL8 utx, tausq, wnstd, intv, intv1, x, up, un, d1, d2;
REAL8 qfval;
INT4 rats[] = {1, 2, 4, 8};
*ifault = 0;
vars->count = 0;
vars->integrationValue = 0.0;
vars->integrationError = 0.0;
qfval = -1.0;
acc1 = acc;
vars->arrayNotSorted = 1;
vars->fail = 0;
xlim = (REAL8)vars->lim;
/* find wnmean, wnstd, wnmax and wnmin of weights, check that parameter values are valid */
vars->sigsq = sigma*sigma; //Sigma squared
wnstd = vars->sigsq; //weights*noise standard deviation initial value
vars->wnmax = 0.0; //Initial value for weights*noise maximum
vars->wnmin = 0.0; //Initial value for weights*noise minimum
vars->wnmean = 0.0; //Initial value for weights*noise 'mean'
for (UINT4 ii=0; ii<vars->weights->length; ii++ ) {
if ( vars->dofs->data[ii] < 0 || vars->noncentrality->data[ii] < 0.0 ) {
*ifault = 3;
return qfval;
} /* return error if any degrees of freedom is less than 0 or noncentrality parameter is less than 0.0 */
wnstd += vars->weights->data[ii]*vars->weights->data[ii] * (2 * vars->dofs->data[ii] + 4.0 * vars->noncentrality->data[ii]);
vars->wnmean += vars->weights->data[ii] * (vars->dofs->data[ii] + vars->noncentrality->data[ii]);
//Find maximum and minimum values
if (vars->wnmax < vars->weights->data[ii]) {
vars->wnmax = vars->weights->data[ii];
} else if (vars->wnmin > vars->weights->data[ii]) {
vars->wnmin = vars->weights->data[ii];
}
}
if ( wnstd == 0.0 ) {
if (vars->c>0.0) qfval = 1.0;
else qfval = 0.0;
return qfval;
}
if ( vars->wnmin == 0.0 && vars->wnmax == 0.0 && sigma == 0.0 ) {
*ifault = 3;
return qfval;
}
wnstd = sqrt(wnstd);
//almx is absolute value maximum of weights
if (vars->wnmax < -vars->wnmin) almx = -vars->wnmin;
else almx = vars->wnmax;
/* starting values for findu, cutoff */
utx = 16.0/wnstd;
up = 4.5/wnstd;
un = -up;
/* truncation point with no convergence factor */
findu(vars, &utx, 0.5*acc1);
/* does convergence factor help */
if (vars->c!=0.0 && almx>0.07*wnstd) {
tausq = 0.25*acc1/coeff(vars, vars->c);
if (vars->fail) vars->fail = 0;
else if (truncation(vars, utx, tausq) < 0.2*acc1) {
vars->sigsq += tausq;
findu(vars, &utx, 0.25*acc1);
}
}
acc1 *= 0.5;
/* find RANGE of distribution, quit if outside this */
l1:
d1 = cutoff(vars, acc1, &up) - vars->c;
if (d1 < 0.0) {
qfval = 1.0;
return qfval;
}
d2 = vars->c - cutoff(vars, acc1, &un);
if (d2 < 0.0) {
qfval = 0.0;
return qfval;
}
/* find integration interval */
if (d1>d2) intv = LAL_TWOPI/d1;
else intv = LAL_TWOPI/d2;
/* calculate number of terms required for main and auxillary integrations */
xnt = utx/intv;
xntm = 3.0/sqrt(acc1);
if (xnt>xntm*1.5) {
//parameters for auxillary integration
if (xntm>xlim) {
*ifault = 1;
return qfval;
}
ntm = (INT4)round(xntm);
intv1 = utx/ntm;
x = LAL_TWOPI/intv1;
if (x<=fabs(vars->c)) goto l2;
//calculate convergence factor
REAL8 coeffvalplusx = coeff(vars, vars->c+x);
REAL8 coeffvalminusx = coeff(vars, vars->c-x);
tausq = (1.0/3.0)*acc1/(1.1*(coeffvalminusx + coeffvalplusx));
if (vars->fail) goto l2;
acc1 = (2.0/3.0)*acc1;
//auxillary integration
//fprintf(stderr,"Num terms in auxillary integration %d\n", ntm);
integrate(vars, ntm, intv1, tausq, 0);
xlim -= xntm;
vars->sigsq += tausq;
//find truncation point with new convergence factor
findu(vars, &utx, 0.25*acc1);
acc1 *= 0.75;
goto l1;
}
/* main integration */
l2:
if (xnt > xlim) {
*ifault = 1;
return qfval;
}
nt = (INT4)round(xnt);
//fprintf(stderr,"Num terms in main integration %d\n", nt);
integrate(vars, nt, intv, 0.0, 1);
qfval = 0.5 - vars->integrationValue;
/* test whether round-off error could be significant allow for radix 8 or 16 machines */
up = vars->integrationError;
x = up + 0.1*acc;
for (UINT4 ii=0; ii<4; ii++) {
if (rats[ii] * x == rats[ii] * up) *ifault = 2;
}
return qfval;
} /* cdfwchisq() */
/* Functions */
void Robot_drive_task(void)
{
Robot_PWM_init(); // Initialize motors
int32_t pid_coeff[3]; // PID Coefficients
int8_t e[3];
int8_t e2[3];
int32_t u;
int32_t u2;
pid_terms_calc(KP, KI, KD, F_SAMP, pid_coeff);
uint8_t blk_cnt[4]; // Consecutive BLK read counters
uint8_t num_iterations = 0;
fw_motors(5, 60); //Test :)
while(TRUE)
{
// Pend on semaphore - Light Sensor values updated
Semaphore_pend(Sema_lightsense, BIOS_WAIT_FOREVER);
switch(drivestate)
{
case DRIVESTATE_IDLE:
// Do nothing :)
break;
case DRIVESTATE_FWD:
linedetection(lightsnsr_val, blk_cnt, 0, 1, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
fwd_pid(u, u2, dutycycle);
break;
case DRIVESTATE_REV:
linedetection(lightsnsr_val, blk_cnt, 1, 0, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
rev_pid(u, u2, dutycycle);
break;
case DRIVESTATE_STRAFELEFT:
linedetection(lightsnsr_val, blk_cnt, 2, 3, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
tl_pid(u, u2, dutycycle);
break;
case DRIVESTATE_STRAFERIGHT:
linedetection(lightsnsr_val, blk_cnt, 3, 2, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
error_calc(lightsnsr_val[0], e);
error_calc(lightsnsr_val[1], e2);
u = findu(e, u, pid_coeff);
u2 = findu(e2, u2, pid_coeff);
tl_pid(u, u2, dutycycle);
break;
case DRIVESTATE_TURNCW:
intersectiondetect(lightsnsr_val, blk_cnt, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
break;
case DRIVESTATE_TURNCCW:
intersectiondetect(lightsnsr_val, blk_cnt, &num_iterations);
linecheck(&u, &u2, &num_iterations, num_moves);
break;
}
}
}
