def rr_int(p1, v1, p2, v2): """Intersect ray though p1 direction v1 with ray through p2 direction v2. Returns a list of zero or one solutions """ diag_print("rr_int " + str(p1) + str(v1) + str(p2) + str(v2), "intersections") s = ll_int(p1, v1, p2, v2) if len(s) > 0 and tol_gte(dot(s[0] - p2, v2), 0) and tol_gte( dot(s[0] - p1, v1), 0): return s else: return []
def is_right_handed(p1, p2, p3, p4): """return True if tetrahedron p1 p2 p3 p4 is right handed""" u = p2 - p1 v = p3 - p1 uv = cross(u, v) w = p4 - p1 return dot(uv, w) > 0
def angle_3p(p1, p2, p3): """Returns the angle, in radians, rotating vector p2p1 to vector p2p3. arg keywords: p1 - a vector p2 - a vector p3 - a vector returns: a number In 2D, the angle is a signed angle, range [-pi,pi], corresponding to a clockwise rotation. If p1-p2-p3 is clockwise, then angle > 0. In 3D, the angle is unsigned, range [0,pi] """ d21 = norm(p2 - p1) d23 = norm(p3 - p2) if tol_eq(d21, 0) or tol_eq(d23, 0): return None # degenerate angle v21 = (p1 - p2) / d21 v23 = (p3 - p2) / d23 t = dot(v21, v23) # / (d21 * d23) if t > 1.0: # check for floating point error t = 1.0 elif t < -1.0: t = -1.0 angle = math.acos(t) if len(p1) == 2: # 2D case if is_counterclockwise(p1, p2, p3): angle = -angle return angle
def cr_int(p1, r, p2, v): """ Intersect a circle (p1,r) with ray (p2,v) (a half-line) where p1, p2 and v are 2-vectors, r is a scalar Returns a list of zero, one or two solutions. """ sols = [] all = cl_int(p1, r, p2, v) for s in all: if tol_gte(dot(s - p2, v), 0): # gt -> gte 30/6/2006 sols.append(s) return sols
def test_rr_int(): """test random ray-ray intersection. returns True iff succesful""" # generate tree points A,B,C an two rays AC, BC. # then calculate the intersection of the two rays # and check that it equals C p_a = randvec(2, 0.0, 10.0, 1.0) p_b = randvec(2, 0.0, 10.0, 1.0) p_c = randvec(2, 0.0, 10.0, 1.0) # print p_a, p_b, p_c if tol_eq(norm(p_c - p_a), 0) or tol_eq(norm(p_c - p_b), 0): return True # ignore this case v_ac = (p_c - p_a) / norm(p_c - p_a) v_bc = (p_c - p_b) / norm(p_c - p_b) s = rr_int(p_a, v_ac, p_b, v_bc) if tol_eq(math.fabs(dot(v_ac, v_bc)), 1.0): return len(s) == 0 else: if len(s) > 0: p_s = s[0] return tol_eq(p_s[0], p_c[0]) and tol_eq(p_s[1], p_c[1]) else: return False
def sss_int(p1, r1, p2, r2, p3, r3): """Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively. Returns a list of zero, one or two solution points. """ solutions = [] # plane though p1, p2, p3 n = cross(p2 - p1, p3 - p1) n = n / norm(n) # intersect circles in plane cp1 = vector([0.0, 0.0]) cp2 = vector([norm(p2 - p1), 0.0]) cpxs = cc_int(cp1, r1, cp2, r2) if len(cpxs) == 0: return [] # px, rx, nx is circle px = p1 + (p2 - p1) * cpxs[0][0] / norm(p2 - p1) rx = abs(cpxs[0][1]) # plane of intersection cicle nx = p2 - p1 nx = nx / norm(nx) # print "px,rx,nx:",px,rx,nx # py = project p3 on px,nx dy3 = dot(p3 - px, nx) py = p3 - (nx * dy3) if tol_gt(dy3, r3): return [] ry = math.sin(math.acos(abs(dy3 / r3))) * r3 # print "py,ry:",py,ry cpx = vector([0.0, 0.0]) cpy = vector([norm(py - px), 0.0]) cp4s = cc_int(cpx, rx, cpy, ry) for cp4 in cp4s: p4 = px + (py - px) * cp4[0] / norm(py - px) + n * cp4[1] solutions.append(p4) return solutions
def is_flat(p1, p2, p3): """ returns True iff triangle p1,p2,p3 is flat (neither clockwise of counterclockwise oriented)""" u = p2 - p1 v = p3 - p2 perp_u = vector([-u[1], u[0]]) return tol_eq(dot(perp_u, v), 0)
def is_counterclockwise(p1, p2, p3): """ returns True iff triangle p1,p2,p3 is counterclockwise oriented""" u = p2 - p1 v = p3 - p2 perp_u = vector([-u[1], u[0]]) return tol_gt(dot(perp_u, v), 0)