// general purpose facet-drop which calls xy_normal_length(), normal_length(),
// and center_height() on the subclass
bool MillingCutter::facetDrop(CLPoint &cl, const Triangle &t) const { // Drop cutter at (cl.x, cl.y) against facet of Triangle t
Point normal = t.upNormal(); // facet surface normal
if ( isZero_tol( normal.z ) ) // vertical surface
return false; //can't drop against vertical surface
assert( isPositive( normal.z ) );
if ( ( isZero_tol(normal.x) ) && ( isZero_tol(normal.y) ) ) { // horizontal plane special case
CCPoint cc_tmp( cl.x, cl.y, t.p[0].z, FACET);
return cl.liftZ_if_inFacet(cc_tmp.z, cc_tmp, t);
} else { // general case
// plane containing facet: a*x + b*y + c*z + d = 0, so
// d = -a*x - b*y - c*z, where (a,b,c) = surface normal
double d = - normal.dot(t.p[0]);
normal.normalize(); // make length of normal == 1.0
Point xyNormal( normal.x, normal.y, 0.0);
xyNormal.xyNormalize();
// define the radiusvector which points from the cc-point to the cutter-center
Point radiusvector = this->xy_normal_length*xyNormal + this->normal_length*normal;
CCPoint cc_tmp = cl - radiusvector; // NOTE xy-coords right, z-coord is not.
cc_tmp.z = (1.0/normal.z)*(-d-normal.x*cc_tmp.x-normal.y*cc_tmp.y); // cc-point lies in the plane.
cc_tmp.type = FACET;
double tip_z = cc_tmp.z + radiusvector.z - this->center_height;
return cl.liftZ_if_inFacet(tip_z, cc_tmp, t);
}
}
// edge-drop function which calls the sub-class MillingCutter::singleEdgeDrop on each
// edge of the input Triangle t.
bool MillingCutter::edgeDrop(CLPoint &cl, const Triangle &t) const {
bool result = false;
for (int n=0;n<3;n++) { // loop through all three edges
int start=n; // index of the start-point of the edge
int end=(n+1)%3; // index of the end-point of the edge
const Point p1 = t.p[start];
const Point p2 = t.p[end];
if ( !isZero_tol( p1.x - p2.x) || !isZero_tol( p1.y - p2.y) ) {
const double d = cl.xyDistanceToLine(p1,p2);
if (d<=radius) // potential contact with edge
if ( this->singleEdgeDrop(cl,p1,p2,d) )
result=true;
}
}
return result;
}
/// return Point on p1-p2 line which is closest in XY-plane to this
Point Point::xyClosestPoint(const Point &p1, const Point &p2) const
{
// one explanation is here
// http://local.wasp.uwa.edu.au/~pbourke/geometry/pointline/
Point pt1 = p1; // this required because of "const" arguments above.
Point pt2 = p2;
Point v = pt2 - pt1;
if ( isZero_tol( v.xyNorm() ) ) { // if p1 and p2 do not make a line in the xy-plane
std::cout << "point.cpp: xyClosestPoint ERROR!: can't calculate closest point from \n";
std::cout << "point.cpp: xyClosestPoint ERROR!: *this ="<<*this <<" to line through\n";
std::cout << "point.cpp: xyClosestPoint ERROR!: p1="<<p1<<" and \n";
std::cout << "point.cpp: xyClosestPoint ERROR!: p2="<<p2<< "\n";
std::cout << "point.cpp: xyClosestPoint ERROR!: in the xy-plane\n";
assert(0);
return Point(0,0,0);
}
double u;
// vector notation:
// u = (p3-p1) dot v / (v dot v)
u = (this->x - p1.x) * (v.x) + (this->y - p1.y)*(v.y);
u = u/ (v.x*v.x + v.y*v.y);
// coordinates for closest point
double x = p1.x + u*v.x;
double y = p1.y + u*v.y;
return Point(x,y,0);
}
// this is the horizontal edge case
bool MillingCutter::horizEdgePush(const Fiber& f, Interval& i, const Point& p1, const Point& p2) const {
bool result=false;
double h = p1.z - f.p1.z; // height of edge above fiber
if ( (h > 0.0) ) {
if ( isZero_tol( p2.z-p1.z ) ) { // this is the horizontal-edge special case
double eff_radius = this->width( h ); // the cutter acts as a cylinder with eff_radius
// contact this cylinder/circle against edge in xy-plane
double qt; // fiber is f.p1 + qt*(f.p2-f.p1)
double qv; // line is p1 + qv*(p2-p1)
if (xy_line_line_intersection( p1 , p2, qv, f.p1, f.p2, qt ) ) {
Point q = p1 + qv*(p2-p1); // the intersection point
// from q, go v-units along tangent, then eff_r*normal, and end up on fiber:
// q + ccv*tangent + r*normal = p1 + clt*(p2-p1)
double ccv, clt;
Point xy_tang=p2-p1;
xy_tang.z=0;
xy_tang.xyNormalize();
Point xy_normal = xy_tang.xyPerp();
Point q1 = q+eff_radius*xy_normal;
Point q2 = q1+(p2-p1);
if ( xy_line_line_intersection( q1 , q2, ccv, f.p1, f.p2, clt ) ) {
double t_cl1 = clt;
double t_cl2 = qt + (qt - clt );
if ( calcCCandUpdateInterval(t_cl1, ccv, q, p1, p2, f, i, f.p1.z, EDGE_HORIZ) )
result = true;
if ( calcCCandUpdateInterval(t_cl2, -ccv, q, p1, p2, f, i, f.p1.z, EDGE_HORIZ) )
result = true;
}
}
}
}
//std::cout << " horizEdgePush = " << result << "\n";
return result;
}
bool BullCutter::generalEdgePush(const Fiber& f, Interval& i, const Point& p1, const Point& p2) const {
//std::cout << " BullCutter::generalEdgePush() \n";
bool result = false;
if ( isZero_tol( (p2-p1).xyNorm() ) ) { // this would be a vertical edge
return result;
}
if ( isZero_tol( p2.z-p1.z ) ) // this would be a horizontal edge
return result;
assert( fabs(p2.z-p1.z) > 0.0 ); // no horiz edges allowed hereafter
// p1+t*(p2-p1) = f.p1.z+radius2 =>
double tplane = (f.p1.z + radius2 - p1.z ) / (p2.z-p1.z); // intersect edge with plane at z = ufp1.z
Point ell_center = p1+tplane*(p2-p1);
assert( isZero_tol( fabs(ell_center.z - (f.p1.z+radius2)) ) );
Point major_dir = (p2-p1);
assert( major_dir.xyNorm() > 0.0 );
major_dir.z = 0;
major_dir.xyNormalize();
Point minor_dir = major_dir.xyPerp();
double theta = atan( (p2.z - p1.z) / (p2-p1).xyNorm() );
double major_length = fabs( radius2/sin(theta) ) ;
double minor_length = radius2;
AlignedEllipse e(ell_center, major_length, minor_length, radius1, major_dir, minor_dir );
if ( e.aligned_solver( f ) ) { // now we want the offset-ellipse point to lie on the fiber
Point pseudo_cc = e.ePoint1(); // pseudo cc-point on ellipse and cylinder
Point pseudo_cc2 = e.ePoint2();
CCPoint cc = pseudo_cc.closestPoint(p1,p2);
CCPoint cc2 = pseudo_cc2.closestPoint(p1,p2);
cc.type = EDGE_POS;
cc2.type = EDGE_POS;
Point cl = e.oePoint1() - Point(0,0,center_height);
assert( isZero_tol( fabs(cl.z - f.p1.z)) );
Point cl2 = e.oePoint2() - Point(0,0,center_height);
assert( isZero_tol( fabs(cl2.z - f.p1.z)) );
double cl_t = f.tval(cl);
double cl_t2 = f.tval(cl2);
if ( i.update_ifCCinEdgeAndTrue( cl_t, cc, p1, p2, true ) )
result = true;
if ( i.update_ifCCinEdgeAndTrue( cl_t2, cc2, p1, p2, true ) )
result = true;
}
//std::cout << " BullCutter::generalEdgePush() DONE result= " << result << "\n";
return result;
}
// check that s and t values are OK
bool EllipsePosition::isValid() const {
if ( isZero_tol( square(s) + square(t) - 1.0 ) )
return true;
else {
std::cout << " EllipsePosition=" << *this << "\n";
std::cout << " square(s) + square(t) - 1.0 = " << square(s) + square(t) - 1.0 << " !!\n";
return false;
}
}
// because this checks for contact with both the tip and the circular edge it is hard to move to the base-class
// we either hit the tip, when the slope of the plane is smaller than angle
// or when the slope is steep, the circular edge between the cone and the cylindrical shaft
bool ConeCutter::facetDrop(CLPoint &cl, const Triangle &t) const {
bool result = false;
Point normal = t.upNormal(); // facet surface normal
if ( isZero_tol( normal.z ) ) // vertical surface
return false; //can't drop against vertical surface
if ( (isZero_tol(normal.x)) && (isZero_tol(normal.y)) ) { // horizontal plane special case
CCPoint cc_tmp( cl.x, cl.y, t.p[0].z, FACET_TIP ); // so any vertex is at the correct height
return cl.liftZ_if_inFacet(cc_tmp.z, cc_tmp, t);
} else {
// define plane containing facet
// a*x + b*y + c*z + d = 0, so
// d = -a*x - b*y - c*z, where (a,b,c) = surface normal
double a = normal.x;
double b = normal.y;
double c = normal.z;
double d = - normal.dot(t.p[0]);
normal.xyNormalize(); // make xy length of normal == 1.0
// cylindrical contact point case
// find the xy-coordinates of the cc-point
CCPoint cyl_cc_tmp = cl - radius*normal;
cyl_cc_tmp.z = (1.0/c)*(-d-a*cyl_cc_tmp.x-b*cyl_cc_tmp.y);
double cyl_cl_z = cyl_cc_tmp.z - length; // tip positioned here
cyl_cc_tmp.type = FACET_CYL;
// tip contact with facet
CCPoint tip_cc_tmp(cl.x,cl.y,0.0);
tip_cc_tmp.z = (1.0/c)*(-d-a*tip_cc_tmp.x-b*tip_cc_tmp.y);
double tip_cl_z = tip_cc_tmp.z;
tip_cc_tmp.type = FACET_TIP;
result = result || cl.liftZ_if_inFacet( tip_cl_z, tip_cc_tmp, t);
result = result || cl.liftZ_if_inFacet( cyl_cl_z, cyl_cc_tmp, t);
return result;
}
}
// drop-cutter: Toroidal cutter edge-test
CC_CLZ_Pair BullCutter::singleEdgeDropCanonical( const Point& u1, const Point& u2 ) const {
if ( isZero_tol( u1.z - u2.z ) ) { // horizontal edge special case
return CC_CLZ_Pair( 0 , u1.z - height(u1.y) );
} else { // the general offset-ellipse case
double b_axis = radius2; // short axis of ellipse = radius2
double theta = atan( (u2.z - u1.z) / (u2.x-u1.x) ); // theta is the slope of the line
double a_axis = fabs( radius2/sin(theta) ); // long axis of ellipse = radius2/sin(theta)
Point ellcenter(0,u1.y,0);
Ellipse e = Ellipse( ellcenter, a_axis, b_axis, radius1);
int iters = e.solver_brent();
assert( iters < 200 );
e.setEllipsePositionHi(u1,u2); // this selects either EllipsePosition1 or EllipsePosition2 and sets it to EllipsePosition_hi
// pseudo cc-point on the ellipse/cylinder, in the CL=origo system
Point ell_ccp = e.ePointHi(); assert( fabs( ell_ccp.xyNorm() - radius1 ) < 1E-5); // ell_ccp should be on the cylinder-circle
Point cc_tmp_u = ell_ccp.closestPoint(u1,u2); // find real cc-point
return CC_CLZ_Pair( cc_tmp_u.x , e.getCenterZ()-radius2);
}
}
/// solve system Ax = y by inverting A
/// x = Ainv * y
/// returns false if det(A)==0, i.e. no solution found
bool two_by_two_solver( const double& a,
const double& b,
const double& c,
const double& d,
const double& e,
const double& f,
double& u,
double& v) {
// [ a b ] [u] = [ e ]
// [ c d ] [v] = [ f ]
// matrix inverse is
// [ d -b ]
// 1/det * [ -c a ]
// so
// [u] [ d -b ] [ e ]
// [v] = 1/det * [ -c a ] [ f ]
double det = a*d-c*b;
if (isZero_tol(det))
return false;
u = (1.0/det) * (d*e - b*f);
v = (1.0/det) * (-c*e + a*f);
return true;
}
// use linear interpolation of the distance-field between vertices idx1 and idx2
// to generate a new iso-surface point on the idx1-idx2 edge
Point MarchingCubes::interpolate(const Octnode* node, int idx1, int idx2) {
// p = p1 - f1 (p2-p1)/(f2-f1)
assert( !isZero_tol( node->f[idx2] - node->f[idx1] ) ); // sign of dist-field should change on the edge (avoid divide by zero)
return *(node->vertex[idx1]) - node->f[idx1]*( *(node->vertex[idx2])-(*(node->vertex[idx1])) ) *
(1.0/(node->f[idx2] - node->f[idx1]));
}
// cone is pushed along Fiber f into contact with edge p1-p2
bool ConeCutter::generalEdgePush(const Fiber& f, Interval& i, const Point& p1, const Point& p2) const {
bool result = false;
if ( isZero_tol(p2.z-p1.z) ) // guard agains horizontal edge
return result;
assert( (p2.z-p1.z) != 0.0 );
// idea: as the ITO-cone slides along the edge it will pierce a z-plane at the height of the fiber
// the shaped of the pierced area is either a circle if the edge is steep
// or a 'half-circle' + cone shape if the edge is shallow (ice-cream cone...)
// we can now intersect this 2D shape with the fiber and get the CL-points.
// how to get the CC-point? (point on edge closest to z-axis of cutter? closest to CL?)
// this is where the ITO cone pierces the plane
// edge-line: p1+t*(p2-p1) = zheight
// => t = (zheight - p1)/ (p2-p1)
double t_tip = (f.p1.z - p1.z) / (p2.z-p1.z);
if (t_tip < 0.0 )
t_tip = 0.0;
Point p_tip = p1 + t_tip*(p2-p1);
assert( isZero_tol( abs(p_tip.z-f.p1.z) ) ); // p_tip should be in plane of fiber
// this is where the ITO cone base exits the plane
double t_base = (f.p1.z+center_height - p1.z) / (p2.z-p1.z);
Point p_base = p1 + t_base*(p2-p1);
p_base.z = f.p1.z; // project to plane of fiber
//std::cout << "(t0, t1) (" << t0 << " , " << t1 << ") \n";
double L = (p_base-p_tip).xyNorm();
if ( L <= radius ) { // this is where the ITO-slice is a circle
// find intersection points, if any, between the fiber and the circle
// fiber is f.p1 - f.p2
// circle is centered at p_base and radius
double d = p_base.xyDistanceToLine(f.p1, f.p2);
if ( d <= radius ) {
// we know there is an intersection point.
// http://mathworld.wolfram.com/Circle-LineIntersection.html
// subtract circle center, math is for circle centered at (0,0)
double dx = f.p2.x - f.p1.x;
double dy = f.p2.y - f.p1.y;
double dr = sqrt( square(dx) + square(dy) );
double det = (f.p1.x-p_base.x) * (f.p2.y-p_base.y) - (f.p2.x-p_base.x) * (f.p1.y-p_base.y);
// intersection given by:
// x = det*dy +/- sign(dy) * dx * sqrt( r^2 dr^2 - det^2 ) / dr^2
// y = -det*dx +/- abs(dy) * sqrt( r^2 dr^2 - det^2 ) / dr^2
double discr = square(radius) * square(dr) - square(det);
assert( discr > 0.0 ); // this means we have an intersection
if ( discr == 0.0 ) { // tangent case
double x_tang = ( det*dy )/ square(dr);
double y_tang = -( det*dx )/ square(dr);
Point p_tang(x_tang+p_base.x, y_tang+p_base.y); // translate back from (0,0) system!
double t_tang = f.tval( p_tang );
if ( circle_CC( t_tang, p1, p2, f, i) )
result = true;
} else {
// two intersection points
double x_pos = ( det*dy + sign(dy)* dx * sqrt( discr ) ) / square(dr);
double y_pos = ( -det*dx + abs(dy) * sqrt( discr ) ) / square(dr);
Point p_pos(x_pos+p_base.x, y_pos+p_base.y);
double t_pos = f.tval( p_pos );
// the same with "-" sign:
double x_neg = ( det*dy - sign(dy) * dx * sqrt( discr ) ) / square(dr);
double y_neg = ( -det*dx - abs(dy) * sqrt( discr ) ) / square(dr);
Point p_neg(x_neg+p_base.x, y_neg+p_base.y);
double t_neg = f.tval( p_neg );
if ( circle_CC( t_pos, p1, p2, f, i) )
result = true;
if ( circle_CC( t_neg, p1, p2, f, i) )
result = true;
}
}
return result;
} else {
// ITO-slice is cone + half-circle
// lines from p_tip to tangent points
assert( L > radius );
// http://mathworld.wolfram.com/CircleTangentLine.html
// circle centered at x0, y0, radius a
// tangent through (0,0)
// t = +/- acos( -a*x0 +/- y0*sqrt(x0^2+y0^2-a^2) / (x0^2+y0^2) )
// translate so p_mid is at (0,0)
//Point c = p_base - p_mid;
//double cos1 = (-radius*c.x + c.y*sqrt(square(c.x)+square(c.y)+square(radius)) )/ (square(c.x) + square(c.y) );
//double cos2 = (-radius*c.x - c.y*sqrt(square(c.x)+square(c.y)+square(radius)) )/ (square(c.x) + square(c.y) );
return result;
}
}
// general purpose facetPush
bool MillingCutter::generalFacetPush(double normal_length,
double center_height,
double xy_normal_length,
const Fiber& fib,
Interval& i,
const Triangle& t)
const {
bool result = false;
Point normal = t.upNormal(); // facet surface normal, pointing up
if ( normal.zParallel() ) // normal points in z-dir
return result; //can't push against horizontal plane, stop here.
normal.normalize();
Point xy_normal = normal;
xy_normal.z = 0;
xy_normal.xyNormalize();
// find a point on the plane from which radius2*normal+radius1*xy_normal lands on the fiber+radius2*Point(0,0,1)
// (u,v) locates a point on the triangle facet v0+ u*(v1-v0)+v*(v2-v0) u,v in [0,1]
// t locates a point along the fiber: p1 + t*(p2-p1) t in [0,1]
//
// facet-point + r2 * n + r1* xy_n = fiber-point + r2*Point(0,0,1)
// =>
// v0+ u*(v1-v0)+v*(v2-v0) + r2 * n + r1* xy_n = p1 + t*(p2-p1) + r2*Point(0,0,1)
//
// v0x + u*(v1x-v0x) + v*(v2x-v0x) + r2*nx + r1*xy_n.x = p1x + t*(p2x-p1x) p2x-p1x==0 for Y-fiber
// v0y + u*(v1y-v0y) + v*(v2y-v0y) + r2*ny + r1*xy_n.y = p1y + t*(p2y-p1y) p2y-p1y==0 for X-fiber
// v0z + u*(v1z-v0z) + v*(v2z-v0z) + r2*nz = p1z + t*(p2z-p1z) + r2 (p2z-p1z)==0 for XY-fibers!!
// X-fiber:
// v0x + u*(v1x-v0x) + v*(v2x-v0x) + r2*nx + r1*xy_n.x = p1x + t*(p2x-p1x)
// v0y + u*(v1y-v0y) + v*(v2y-v0y) + r2*ny + r1*xy_n.y = p1y solve these two for (u,v)
// v0z + u*(v1z-v0z) + v*(v2z-v0z) + r2*nz = p1z + r2 and substitute above for t
// or
// [ (v1y-v0y) (v2y-v0y) ] [ u ] = [ -v0y - r2*ny - r1*xy_n.y + p1y ]
// [ (v1z-v0z) (v2z-v0z) ] [ v ] = [ -v0z - r2*nz + p1z + r2 ]
//
// Y-fiber:
// [ (v1x-v0x) (v2x-v0x) ] [ u ] = [ -v0x - r2*nx - r1*xy_n.x + p1x ]
double a;
double b;
double c = t.p[1].z - t.p[0].z;
double d = t.p[2].z - t.p[0].z;
double e;
double f = -t.p[0].z - normal_length*normal.z + fib.p1.z + center_height;
// note: the xy_normal does not have a z-component, so omitted here.
double u, v; // u and v are coordinates of the cc-point within the triangle facet
// a,b,e depend on the fiber:
if ( fib.p1.y == fib.p2.y ) { // XFIBER
a = t.p[1].y - t.p[0].y;
b = t.p[2].y - t.p[0].y;
e = -t.p[0].y - normal_length*normal.y - xy_normal_length*xy_normal.y + fib.p1.y;
if (!two_by_two_solver(a,b,c,d,e,f,u,v))
return result;
CCPoint cc = t.p[0] + u*(t.p[1]-t.p[0]) + v*(t.p[2]-t.p[0]);
cc.type = FACET;
if ( ! cc.isInside( t ) )
return result;
// v0x + u*(v1x-v0x) + v*(v2x-v0x) + r2*nx + r1*xy_n.x = p1x + t*(p2x-p1x)
// =>
// t = 1/(p2x-p1x) * ( v0x + r2*nx + r1*xy_n.x - p1x + u*(v1x-v0x) + v*(v2x-v0x) )
assert( !isZero_tol( fib.p2.x - fib.p1.x ) ); // guard against division by zero
double tval = (1.0/( fib.p2.x - fib.p1.x )) * ( t.p[0].x + normal_length*normal.x + xy_normal_length*xy_normal.x - fib.p1.x
+ u*(t.p[1].x-t.p[0].x)+v*(t.p[2].x-t.p[0].x) );
if ( tval < 0.0 || tval > 1.0 ) {
std::cout << "MillingCutter::facetPush() tval= " << tval << " error!?\n";
//std::cout << " cutter: " << *this << "\n";
std::cout << " triangle: " << t << "\n";
std::cout << " fiber: " << fib << "\n";
}
assert( tval > 0.0 && tval < 1.0 );
i.update( tval, cc );
result = true;
} else if (fib.p1.x == fib.p2.x) { // YFIBER
a = t.p[1].x - t.p[0].x;
b = t.p[2].x - t.p[0].x;
e = -t.p[0].x - normal_length*normal.x - xy_normal_length*xy_normal.x + fib.p1.x;
if (!two_by_two_solver(a,b,c,d,e,f,u,v))
return result;
CCPoint cc = t.p[0] + u*(t.p[1]-t.p[0]) + v*(t.p[2]-t.p[0]);
cc.type = FACET;
if ( ! cc.isInside( t ) )
return result;
assert( !isZero_tol( fib.p2.y - fib.p1.y ) );
double tval = (1.0/( fib.p2.y - fib.p1.y )) * ( t.p[0].y + normal_length*normal.y + xy_normal_length*xy_normal.y - fib.p1.y
+ u*(t.p[1].y-t.p[0].y)+v*(t.p[2].y-t.p[0].y) );
if ( tval < 0.0 || tval > 1.0 ) {
std::cout << "MillingCutter::facetPush() tval= " << tval << " error!?\n";
std::cout << " (most probably a user error, the fiber is too short compared to the STL model?)\n";
}
assert( tval > 0.0 && tval < 1.0 );
i.update( tval, cc );
result = true;
} else {
assert(0);
}
return result;
}
bool Point::yParallel() const {
if ( isZero_tol( x ) && isZero_tol( z ) )
return true;
return false;
}
