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)
