/** Approximate the distance between a point and an ellipse using Rosin
distance.
*/
double d_rosin( Ring *ering, double x, double y )
{
double xtmp, ytmp;
double ae2, be2, fe2;
double xp, yp, xp2, yp2, xx, yy;
double delta, A, ah, bh2, term;
double d[4];
xtmp = x - ering->cx;
ytmp = y - ering->cy;
ae2 = ering->ax * ering->ax;
be2 = ering->bx * ering->bx;
fe2 = ae2 - be2;
xp = xtmp * cos(-ering->theta) - ytmp * sin(-ering->theta);
yp = xtmp * sin(-ering->theta) + ytmp * cos(-ering->theta);
xp2 = xp * xp;
yp2 = yp * yp;
delta = (xp2 + yp2 + fe2) * (xp2 + yp2 + fe2) - 4 * xp2 * fe2;
A = (xp2 + yp2 + fe2 - sqrt(delta))/2.0;
ah = sqrt(A);
bh2 = fe2 - A;
term = (A * be2 + ae2 * bh2);
xx = ah * sqrt( ae2 * (be2 + bh2) / term );
yy = ering->bx * sqrt( bh2 * (ae2 - A) / term );
d[0] = dist( xp, yp, xx, yy);
d[1] = dist( xp, yp, xx,-yy);
d[2] = dist( xp, yp,-xx, yy);
d[3] = dist( xp, yp,-xx,-yy);
double dmin = min_array(d,4);
if( xp2+yp2 > xx*xx+yy*yy ) return dmin;
else return -dmin;
}
// Note that 'arraySize = 8*((3*8*6144)+12)';
void dlsch_LLR_quant(const short *llr, const unsigned int arraySize, const short Mlevel, short *llr_quant)
{
unsigned int i, j;
short max_llr, min_llr;
short llr_interval;
float quant_step;
float *transLevelArray;
min_llr = min_array(llr, arraySize);
max_llr = max_array(llr, arraySize);
llr_interval = (max_llr - min_llr);
quant_step = (float)llr_interval / Mlevel;
if ((Mlevel%2) != 0) {
printf("Mlevel should be mutiple of 2...\n");
exit(-1);
}
transLevelArray = (float *)malloc((Mlevel+1)*sizeof(float));
if (!transLevelArray) {
printf("Cannot allocate memory for transLevelArray...!\n");
exit(-1);
}
for(j=0; j < Mlevel+1; j++) {
transLevelArray[j] = min_llr + j*quant_step;
}
for (i=0; i < arraySize; i++) {
for(j=0; j < Mlevel; j++) {
if ((transLevelArray[j] <= llr[i]) && (llr[i] <= transLevelArray[j+1])) {
llr_quant[i] = (short)(transLevelArray[j] + quant_step/2); // mid-points are selected;
break;
}
if (transLevelArray[j+1] <= llr[i]) { // the last term;
llr_quant[i] = (short)(transLevelArray[j] + quant_step/2); // mid-points are selected;
}
}
}
#ifdef TEST_DEBUG
printf("min_llr: %d : max_llr: %d \n", min_llr, max_llr);
printf("llr_interval: %d : quant_step: %f \n", llr_interval, quant_step);
printf("transLevelArray = [");
for(j=0; j < Mlevel+1; j++)
printf("%f ", transLevelArray[j]);
printf("] \n\n");
#endif // TEST_DEBUG
}
char current_location(int* position)
{
// int buffer to hold blob data
unsigned int buffer[12];
// read wii camera blob data into int buffer
m_wii_read(buffer);
// points
int x1 = (int)buffer[0];
int y1 = (int)buffer[1];
int x2 = (int)buffer[3];
int y2 = (int)buffer[4];
int x3 = (int)buffer[6];
int y3 = (int)buffer[7];
int x4 = (int)buffer[9];
int y4 = (int)buffer[10];
// array of points
int x_points[4] = {x1, x2, x3, x4};
int y_points[4] = {y1, y2, y3, y4};
int i;
for(i = 0; i<=4;i++)
{
if (x_points[i]==1023 && y_points[i]==1023)
{
x_points[i] = NaN;
y_points[i] = NaN;
}
}
// calculate all possible distances
int d12 = calc_dist(x_points[0], y_points[0], x_points[1], y_points[1]);
int d13 = calc_dist(x_points[0], y_points[0], x_points[2], y_points[2]);
int d14 = calc_dist(x_points[0], y_points[0], x_points[3], y_points[3]);
int d23 = calc_dist(x_points[1], y_points[1], x_points[2], y_points[2]);
int d24 = calc_dist(x_points[1], y_points[1], x_points[3], y_points[3]);
int d34 = calc_dist(x_points[2], y_points[2], x_points[3], y_points[3]);
// store all distances in an array
int all_distances[6] = {d12,d13,d14,d23,d24,d34};
// store all pair
int all_pairs[6][2] = {{0,1},{0,2},{0,3},{1,2},{1,3},{2,3}};
// index and value of max and min distance
int max_dat[2];
int min_dat[2];
// fill min and max arrays
max_array(all_distances, 6, max_dat);
min_array(all_distances, 6, min_dat);
// index of max and min points
int max_index = max_dat[1];
int min_index = min_dat[1];
// array of pair indicies
int long_pair[2] = {all_pairs[max_index][0], all_pairs[max_index][1]};
int short_pair[2] = {all_pairs[min_index][0], all_pairs[min_index][1]};
int top_point[2];
int bottom_point[2];
// finds top and bottom points
if (long_pair[0] == short_pair[0] || long_pair[0] == short_pair[1])
{
top_point[0] = x_points[long_pair[0]];
top_point[1] = y_points[long_pair[0]];
bottom_point[0] = x_points[long_pair[1]];
bottom_point[1] = y_points[long_pair[1]];
}
else
{
top_point[0] =x_points[long_pair[1]];
top_point[1] =y_points[long_pair[1]];
bottom_point[0] =x_points[long_pair[0]];
bottom_point[1] =y_points[long_pair[0]];
}
// calculate center of rink
double x_center = ((double)top_point[0] + (double)bottom_point[0])/2;
double y_center = ((double)top_point[1] + (double)bottom_point[1])/2;
//double theta = acos(y_unit);
double x_diff = (double)top_point[0] - x_center;
double y_diff =(double)top_point[1] - y_center;
// calculate position of camera relative to camera coordinate axes
double camera_x = 512;
double camera_y = 384;
// calculate angle theta
double theta = atan2(x_diff, y_diff);
// rotated points
float rotated_x = ((((camera_x-x_center)*cos(theta)) + ((camera_y-y_center)*-sin(theta))) + camera_x);
float rotated_y = ((((camera_x-x_center)*sin(theta)) + ((camera_y-y_center)*cos(theta))) + camera_y);
// position and angle relative to rink axes
position[0] = (int) (10*((512 - (rotated_x)))/28);
position[1] = (int) (10*(((rotated_y) - 384))/28);
position[2] = (int) (theta*(180/M_PI));
return 1;
}
