void RGBb_to_YUVi(const uint8 *RGB,int *Y,int *U,int *V)
{
int R = RGB[0], G = RGB[1], B = RGB[2];
*Y = Y_RGB(R,G,B);
*U = U_RGB(R,G,B) + 127;
*V = V_RGB(R,G,B) + 127;
assert( isinrange(*Y,0,255) );
assert( isinrange(*U,0,255) );
assert( isinrange(*V,0,255) );
}
void RGBi_to_YUVi(int R,int G,int B,int *Y,int *U,int *V)
{
assert( isinrange(R,0,255) );
assert( isinrange(G,0,255) );
assert( isinrange(B,0,255) );
*Y = Y_RGB(R,G,B);
*U = U_RGB(R,G,B) + 127;
*V = V_RGB(R,G,B) + 127;
assert( isinrange(*Y,0,255) );
assert( isinrange(*U,0,255) );
assert( isinrange(*V,0,255) );
}
double DropCutter::EdgeTest(const Cutter &cu, const double *e, const double *p1, const double *p2)
{
// contact cutter against edge from p1 to p2
// translate segment so that cutter is at (0,0)
double start[3] = {p1[0] - e[0], p1[1] - e[1], p1[2]};
double end[3] = {p2[0] - e[0], p2[1] - e[1], p2[2]};
// find angle btw. segment and X-axis
double dx = end[0] - start[0];
double dy = end[1] - start[1];
double alfa;
if (fabs(dx) > 0.0000000000001)
alfa = atan(dy / dx);
else
alfa = 1.5707963267948966;
//alfa = -alfa;
// rotation matrix for rotation around z-axis:
// should probably implement a matrix class later
// rotate by angle alfa
// need copy of data that does not change as we go through each line:
double sx = start[0], sy = start[1], ex = end[0], ey = end[1];
start[0] = sx * cos(alfa) + sy * sin(alfa);
start[1] = -sx * sin(alfa) + sy * cos(alfa);
end[0] = ex * cos(alfa) + ey * sin(alfa);
end[1] = -ex * sin(alfa) + ey * cos(alfa);
// check if segment is below cutter
if (start[1] > 0)
{
alfa = alfa+3.1415926535897932;
start[0] = sx * cos(alfa) + sy * sin(alfa);
start[1] = -sx * sin(alfa) + sy * cos(alfa);
end[0] = ex * cos(alfa) + ey * sin(alfa);
end[1] = -ex * sin(alfa) + ey * cos(alfa);
}
if (fabs(start[1]-end[1])>0.0000000001)
{
wxMessageBox(wxString::Format(_T("EdgeTest ERROR! (start.y - end.y) = %lf"), start[1]-end[1]));
return -10000000.0;
}
double l = -start[1]; // distance from cutter to edge
if (l < -heeksCAD->GetTolerance())
wxMessageBox(_T("EdgeTest ERROR! l<0 !"));
// System.Console.WriteLine("l=" + l+" start.y="+start.y+" end.y="+end.y);
// now we have two different algorithms depending on the cutter:
if (fabs(cu.r) < heeksCAD->GetTolerance())
{
// this is the flat endmill case
// it is easier and faster than the general case, so we handle it separately
if (l > cu.R + heeksCAD->GetTolerance()) // edge is outside of the cutter
return -10000000.0;
else // we are inside the cutter
{
if(fabs(end[0] - start[0]) < 0.000000001)return -10000000.0; // instead of maths error below
// so calculate CC point
double xc1 = sqrt(pow(cu.R, 2) - pow(l, 2));
double xc2 = -xc1;
double zc1 = ((xc1 - start[0]) / (end[0] - start[0])) * (end[2] - start[2]) + start[2];
double zc2 = ((xc2 - start[0]) / (end[0] - start[0])) * (end[2] - start[2]) + start[2];
// choose the higher point
double zc,xc;
if (zc1 > zc2)
{
zc = zc1;
xc = xc1;
}
else
{
zc = zc2;
xc = xc2;
}
// now that we have a CC point, check if it's in the edge
if ((start[0] > xc + heeksCAD->GetTolerance()) && (xc + heeksCAD->GetTolerance()< end[0]))
return -10000000.0;
else if ((end[0] < xc - heeksCAD->GetTolerance()) && (xc + heeksCAD->GetTolerance() > start[0]))
return -10000000.0;
else
return zc;
}
// unreachable place (according to compiler)
} // end of flat endmill (r=0) case
else// if (cu.r > 0)
{
// System.Console.WriteLine("edgetest r>0 case!");
// this is the general case (r>0) ball-nose or bull-nose (spherical or toroidal)
// later a separate case for the ball-cutter might be added (for performance)
double xd=0, w=0, h=0, xd1=0, xd2=0, xc=0 , ze=0, zc=0;
if (l > cu.R + heeksCAD->GetTolerance()) // edge is outside of the cutter
return -10000000.0;
else if (((cu.R-cu.r)<l - heeksCAD->GetTolerance())&&(l<=cu.R + heeksCAD->GetTolerance()))
{ // toroidal case
xd=0; // center of ellipse
w=sqrt(pow(cu.R,2)-pow(l,2)); // width of ellipse
h=sqrt(pow(cu.r,2)-pow((l-(cu.R-cu.r)),2)); // height of ellipse
}
else if ((cu.R-cu.r)>=l)
{
// quarter ellipse case
xd1=sqrt( pow((cu.R-cu.r),2)-pow(l,2));
xd2=-xd1;
h=cu.r; // ellipse height
w=sqrt( pow(cu.R,2)-pow(l,2) )- sqrt( pow((cu.R-cu.r),2)-pow(l,2) ); // ellipse height
}
// now there is a special case where the theta calculation will fail if
// the segment is horziontal, i.e. start.z==end.z so we need to catch that here
if (fabs(start[2] - end[2]) < heeksCAD->GetTolerance())
{
if ((cu.R-cu.r)<l - heeksCAD->GetTolerance())
{
// half-ellipse case
xc=0;
h=sqrt(pow(cu.r,2)-pow((l-(cu.R-cu.r)),2));
ze = start[2] + h - cu.r;
}
else
{
// quarter ellipse case
xc = 0;
ze = start[2];
}
// now we have a CC point
// so we need to check if the CC point is in the edge
if (isinrange(start[0], end[0], xc))
return ze;
else
return -10000000.0;
} // end horizontal edge special case
// now the general case where the theta calculation works
double theta = atan( h*(start[0]-end[0])/(w*(start[2]-end[2])) );
if(fabs(end[0] - start[0]) < 0.000000001)return -10000000.0; // instead of maths error below
// based on this calculate the CC point
if (((cu.R - cu.r) < l - heeksCAD->GetTolerance()) && (cu.R <= l + heeksCAD->GetTolerance()))
{
// half-ellipse case
double xc1 = xd + fabs(w * cos(theta));
double xc2 = xd - fabs(w * cos(theta));
double zc1 = ((xc1 - start[0]) / (end[0] - start[0])) * (end[2] - start[2]) + start[2];
double zc2 = ((xc2 - start[0]) / (end[0] - start[0])) * (end[2] - start[2]) + start[2];
// select the higher point:
if (zc1 > zc2)
{
zc = zc1;
xc = xc1;
}
else
{
zc = zc2;
xc = xc2;
}
}
else
{
// quarter ellipse case
double xc1 = xd1 + fabs(w * cos(theta));
double xc2 = xd2 - fabs(w * cos(theta));
double zc1 = ((xc1 - start[0]) / (end[0] - start[0])) * (end[2] - start[2]) + start[2];
double zc2 = ((xc2 - start[0]) / (end[0] - start[0])) * (end[2] - start[2]) + start[2];
// select the higher point:
if (zc1 > zc2)
{
zc = zc1;
xc = xc1;
}
else
{
zc = zc2;
xc = xc2;
}
}
// now we have a valid xc value, so calculate the ze value:
ze = zc + fabs(h * sin(theta)) - cu.r;
// finally, check that the CC point is in the edge
if (isinrange(start[0],end[0],xc))
return ze;
else
return -10000000.0;
// this line is unreachable (according to compiler)
} // end of toroidal/spherical case
// if we ever get here it is probably a serious error!
wxMessageBox(_T("EdgeTest: ERROR: no case returned a valid ze!"));
return -10000000.0;
}
