/** * Determine if one segment intersects another. * * @param _s1 the threshold edge * @param _s2 the segment * @param _i1 the possible intersection point * @param _i2 the possible intersection point * @return 0 if the segments do not intersect * 1 if one segment intersects another at precisely one point * 2 if one segment overlaps another, in which case there are two intersection points. * @throw CException should any of the following occur: * 1. zero v.y. */ unsigned short CThreshold::seg_intersection(seg& _s1, seg& _s2, pt& _i1, pt& _i2 ) { unsigned short intersect = 0; vec u, v, w, w2; double d, du, dv; double t, t0, t1; double si, ti; do { init_vec(_s1.p1, _s1.p2, u); init_vec(_s2.p1, _s2.p2, v); init_vec(_s2.p1, _s1.p1, w); d = cross_vec(u, v); if (fabs(d) < PARALLELISM_TOLERANCE) { // segments are parallel if (cross_vec(u, w) != 0 || cross_vec(v, w) != 0) { // not colinear break; } // they are colinear or degenerate // check if they are degenerate points du = dot_vec(u, u); dv = dot_vec(v, v); if (0 == du && 0 == dv) { // both segments are points if (pts_equal(_s1.p1, _s2.p1)) { // the same point intersect = 1; init_pt(_i1, _s1.p1); } break; } if (0 == du) { // s1 is a single point if (pt_within_seg(_s2, _s1.p1)) { // is within s2 intersect = 1; init_pt(_i1, _s1.p1); } break; } if (0 == dv) { // s2 a single point if (pt_within_seg(_s1, _s2.p1)) { // is within s1 intersect = 1; init_pt(_i1, _s2.p1); } break; } // colinear segments - get overlap (or not) init_vec(_s2.p1, _s1.p2, w2); if (0 != v.x) { t0 = w.x/v.x; t1 = w2.x/v.x; } else { if (0.0 == v.y) { throw CException("Error! Zero v.y."); } t0 = w.y/v.y; t1 = w2.y/v.y; } // must have t0 smaller than t1 if (t0 > t1) { // swap if not t = t0; t0 = t1; t1 = t; } if (t0 > 1 || t1 < 0) { break; // no overlap } t0 = t0 < 0 ? 0 : t0; // clip to min 0 t1 = t1 > 1 ? 1 : t1; // clip to max 1 if (t0 == t1) { // intersection is a pt intersect = 1; _i1.x = _s2.p1.x + (t0*v.x); _i1.y = _s2.p1.y + (t0*v.y); break; } // they overlap in a valid subseg intersect = 2; _i1.x = _s2.p1.x + (t0*v.x); _i1.y = _s2.p1.y + (t0*v.y); _i2.x = _s2.p1.x + (t1*v.x); _i2.y = _s2.p1.y + (t1*v.y); break; } // end of <if (fabs(d) < PARALLELISM_TOLERANCE)> clause // the segs are skew and may intersect in a pt // get the intersect parameter for s1 si = cross_vec(v, w)/d; if (si < 0 || si > 1) { // no intersect with s1 break; } // get the intersect parameter for S2 ti = cross_vec(u, w)/d; if (ti < 0 || ti > 1) { // no intersect with s2 break; } intersect = 1; _i1.x = _s1.p1.x + (si*u.x); _i1.y = _s1.p1.y + (si*u.y); } while (0); return intersect; }
real cos_vec(real* v1, real* v2, int vsize){ real nv1 = dot_vec(v1, v1, vsize); real nv2 = dot_vec(v2, v2, vsize); real d = dot_vec(v1, v2, vsize); return d / sqrt(nv1 * nv2); }
Vec* normalize_vec(Vec* v) { double length = sqrt(dot_vec(v, v)); return create_vec(v->x / length, v->y / length, v->z / length, v->w); }