def intersect_torus_point(center, axis, majorradius, minorradius,
majorradiussq, minorradiussq, direction, point):
dist = 0
if (direction.x == 0) and (direction.y == 0):
# drop
minlsq = (majorradius - minorradius)**2
maxlsq = (majorradius + minorradius)**2
l_sq = (point.x - center.x)**2 + (point.y - center.y)**2
if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
return (None, None, INFINITE)
l = sqrt(l_sq)
z_sq = minorradiussq - (majorradius - l)**2
if z_sq < 0:
return (None, None, INFINITE)
z = sqrt(z_sq)
ccp = Point(point.x, point.y, center.z - z)
dist = ccp.z - point.z
elif direction.z == 0:
# push
z = point.z - center.z
if abs(z) > minorradius - epsilon:
return (None, None, INFINITE)
l = majorradius + sqrt(minorradiussq - z * z)
n = axis.cross(direction)
d = n.dot(point) - n.dot(center)
if abs(d) > l - epsilon:
return (None, None, INFINITE)
a = sqrt(l * l - d * d)
ccp = center.add(n.mul(d).add(direction.mul(a)))
ccp.z = point.z
dist = point.sub(ccp).dot(direction)
else:
# general case
x = point.sub(center)
v = direction.mul(-1)
x_x = x.dot(x)
x_v = x.dot(v)
x1 = Point(x.x, x.y, 0)
v1 = Point(v.x, v.y, 0)
x1_x1 = x1.dot(x1)
x1_v1 = x1.dot(v1)
v1_v1 = v1.dot(v1)
R2 = majorradiussq
r2 = minorradiussq
a = 1.0
b = 4 * x_v
c = 2 * (x_x + 2 * x_v**2 + (R2 - r2) - 2 * R2 * v1_v1)
d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
e = (x_x)**2 + 2 * x_x * (R2 - r2) + (R2 - r2)**2 - 4 * R2 * x1_x1
r = poly4_roots(a, b, c, d, e)
if not r:
return (None, None, INFINITE)
else:
l = min(r)
ccp = point.add(direction.mul(-l))
dist = l
return (ccp, point, dist)
def intersect_torus_point(center, axis, majorradius, minorradius, majorradiussq,
minorradiussq, direction, point):
dist = 0
if (direction.x == 0) and (direction.y == 0):
# drop
minlsq = (majorradius - minorradius) ** 2
maxlsq = (majorradius + minorradius) ** 2
l_sq = (point.x-center.x) ** 2 + (point.y - center.y) ** 2
if (l_sq < minlsq + epsilon) or (l_sq > maxlsq - epsilon):
return (None, None, INFINITE)
l = sqrt(l_sq)
z_sq = minorradiussq - (majorradius - l) ** 2
if z_sq < 0:
return (None, None, INFINITE)
z = sqrt(z_sq)
ccp = Point(point.x, point.y, center.z - z)
dist = ccp.z - point.z
elif direction.z == 0:
# push
z = point.z - center.z
if abs(z) > minorradius - epsilon:
return (None, None, INFINITE)
l = majorradius + sqrt(minorradiussq - z * z)
n = axis.cross(direction)
d = n.dot(point) - n.dot(center)
if abs(d) > l - epsilon:
return (None, None, INFINITE)
a = sqrt(l * l - d * d)
ccp = center.add(n.mul(d).add(direction.mul(a)))
ccp.z = point.z
dist = point.sub(ccp).dot(direction)
else:
# general case
x = point.sub(center)
v = direction.mul(-1)
x_x = x.dot(x)
x_v = x.dot(v)
x1 = Point(x.x, x.y, 0)
v1 = Point(v.x, v.y, 0)
x1_x1 = x1.dot(x1)
x1_v1 = x1.dot(v1)
v1_v1 = v1.dot(v1)
R2 = majorradiussq
r2 = minorradiussq
a = 1.0
b = 4 * x_v
c = 2 * (x_x + 2 * x_v ** 2 + (R2 - r2) - 2 * R2 * v1_v1)
d = 4 * (x_x * x_v + x_v * (R2 - r2) - 2 * R2 * x1_v1)
e = (x_x) ** 2 + 2 * x_x * (R2 - r2) + (R2 - r2) ** 2 - 4 * R2 * x1_x1
r = poly4_roots(a, b, c, d, e)
if not r:
return (None, None, INFINITE)
else:
l = min(r)
ccp = point.add(direction.mul(-l))
dist = l
return (ccp, point, dist)
