def get_x3d_cone(start: tuple, end: tuple, position: float, length: float, radius: float,
color: tuple):
# by default the X3D cone points along the y axis
original_vector = (0, 1, 0)
line_vector = psub(end, start)
line_normalized = pnormalized(line_vector)
# the cone position is at its bottom
bottom_position = padd(start, pmul(line_vector, position))
# handling of corner cases
if pnormsq(pcross(original_vector, line_normalized)) == 0:
# Both vectors are aligned - but maybe point into different directions.
rotation_axis = (1, 0, 0)
rotation_angle = math.pi if original_vector != line_normalized else 0
else:
# Both vectors are not aligned. Use a normal rotation.
# Rotate around the vector in the vector in the middle by 180 degrees.
rotation_axis = padd(original_vector, line_normalized)
rotation_angle = math.pi
yield ('<Transform translation="{:f} {:f} {:f}" rotation="{:f} {:f} {:f} {:f}">'
.format(*bottom_position, *rotation_axis, rotation_angle))
yield "<Shape>"
yield "<Appearance>"
yield ('<Material diffuseColor="{:f} {:f} {:f}" transparency="{:f}" />'
.format(color["red"], color["green"], color["blue"], 1 - color["alpha"]))
yield "</Appearance>"
yield '<Cone bottomRadius="{:f}" topRadius="0" height="{:f}" />'.format(radius, length)
yield "</Shape>"
yield "</Transform>"
def intersect_circle_point(center, axis, radius, radiussq, direction, point):
# take a plane through the base
plane = Plane(center, axis)
# intersect with line gives ccp
(ccp, l) = plane.intersect_point(direction, point)
# check if inside circle
if ccp and (pnormsq(psub(center, ccp)) < radiussq - epsilon):
return (ccp, point, -l)
return (None, None, INFINITE)
def intersect_sphere_edge(self, direction, edge, start=None):
(cl, ccp, cp, l) = self.intersect_sphere_line(direction, edge, start=start)
if cp:
# check if the contact point is between the endpoints
d = psub(edge.p2, edge.p1)
m = pdot(psub(cp, edge.p1), d)
if (m < -epsilon) or (m > pnormsq(d) + epsilon):
return (None, INFINITE, None)
return (cl, l, cp)
def intersect_sphere_point(center, radius, radiussq, direction, point):
# line equation
# (1) x = p_0 + \lambda * d
# sphere equation
# (2) (x-x_0)^2 = R^2
# (1) in (2) gives a quadratic in \lambda
p0_x0 = psub(center, point)
a = pnormsq(direction)
b = 2 * pdot(p0_x0, direction)
c = pnormsq(p0_x0) - radiussq
d = b * b - 4 * a * c
if d < 0:
return (None, None, INFINITE)
if a < 0:
l = (-b + sqrt(d)) / (2 * a)
else:
l = (-b - sqrt(d)) / (2 * a)
# cutter contact point
ccp = padd(point, pmul(direction, -l))
return (ccp, point, l)
def get_intersection(self, line, infinite_lines=False):
""" Get the point of intersection between two lines. Intersections
outside the length of these lines are ignored.
Returns (None, None) if no valid intersection was found.
Otherwise the result is (CollisionPoint, distance). Distance is between
0 and 1.
"""
x1, x2, x3, x4 = self.p1, self.p2, line.p1, line.p2
a = psub(x2, x1)
b = psub(x4, x3)
c = psub(x3, x1)
# see http://mathworld.wolfram.com/Line-LineIntersection.html (24)
try:
factor = pdot(pcross(c, b), pcross(a, b)) / pnormsq(pcross(a, b))
except ZeroDivisionError:
# lines are parallel
# check if they are _one_ line
if pnorm(pcross(a, c)) != 0:
# the lines are parallel with a distance
return None, None
# the lines are on one straight
candidates = []
if self.is_point_inside(x3):
candidates.append((x3, pnorm(c) / pnorm(a)))
elif self.is_point_inside(x4):
candidates.append((x4, pdist(line.p2, self.p1) / pnorm(a)))
elif line.is_point_inside(x1):
candidates.append((x1, 0))
elif line.is_point_inside(x2):
candidates.append((x2, 1))
else:
return None, None
# return the collision candidate with the lowest distance
candidates.sort(key=lambda collision: collision[1])
return candidates[0]
if infinite_lines or (-epsilon <= factor <= 1 + epsilon):
intersec = padd(x1, pmul(a, factor))
# check if the intersection is between x3 and x4
if infinite_lines:
return intersec, factor
elif ((min(x3[0], x4[0]) - epsilon <= intersec[0] <=
max(x3[0], x4[0]) + epsilon)
and (min(x3[1], x4[1]) - epsilon <= intersec[1] <=
max(x3[1], x4[1]) + epsilon)
and (min(x3[2], x4[2]) - epsilon <= intersec[2] <=
max(x3[2], x4[2]) + epsilon)):
return intersec, factor
else:
# intersection outside of the length of line(x3, x4)
return None, None
else:
# intersection outside of the length of line(x1, x2)
return None, None
def intersect_torus_plane(center, axis, majorradius, minorradius, direction,
triangle):
# take normal to the plane
n = triangle.normal
if pdot(n, direction) == 0:
return (None, None, INFINITE)
if pdot(n, axis) == 1:
return (None, None, INFINITE)
# find place on torus where surface normal is n
b = pmul(n, -1)
z = axis
a = psub(b, pmul(z, pdot(z, b)))
a_sq = pnormsq(a)
if a_sq <= 0:
return (None, None, INFINITE)
a = pdiv(a, sqrt(a_sq))
ccp = padd(padd(center, pmul(a, majorradius)), pmul(b, minorradius))
# find intersection with plane
(cp, l) = triangle.plane.intersect_point(direction, ccp)
return (ccp, cp, l)
def intersect_circle_line(center, axis, radius, radiussq, direction, edge):
# make a plane by sliding the line along the direction (1)
d = edge.dir
if pdot(d, axis) == 0:
if pdot(direction, axis) == 0:
return (None, None, INFINITE)
plane = Plane(center, axis)
(p1, l) = plane.intersect_point(direction, edge.p1)
(p2, l) = plane.intersect_point(direction, edge.p2)
pc = Line(p1, p2).closest_point(center)
d_sq = pnormsq(psub(pc, center))
if d_sq >= radiussq:
return (None, None, INFINITE)
a = sqrt(radiussq - d_sq)
d1 = pdot(psub(p1, pc), d)
d2 = pdot(psub(p2, pc), d)
ccp = None
cp = None
if abs(d1) < a - epsilon:
ccp = p1
cp = psub(p1, pmul(direction, l))
elif abs(d2) < a - epsilon:
ccp = p2
cp = psub(p2, pmul(direction, l))
elif ((d1 < -a + epsilon) and (d2 > a - epsilon)) \
or ((d2 < -a + epsilon) and (d1 > a - epsilon)):
ccp = pc
cp = psub(pc, pmul(direction, l))
return (ccp, cp, -l)
n = pcross(d, direction)
if pnorm(n) == 0:
# no contact point, but should check here if circle *always* intersects
# line...
return (None, None, INFINITE)
n = pnormalized(n)
# take a plane through the base
plane = Plane(center, axis)
# intersect base with line
(lp, l) = plane.intersect_point(d, edge.p1)
if not lp:
return (None, None, INFINITE)
# intersection of 2 planes: lp + \lambda v
v = pcross(axis, n)
if pnorm(v) == 0:
return (None, None, INFINITE)
v = pnormalized(v)
# take plane through intersection line and parallel to axis
n2 = pcross(v, axis)
if pnorm(n2) == 0:
return (None, None, INFINITE)
n2 = pnormalized(n2)
# distance from center to this plane
dist = pdot(n2, center) - pdot(n2, lp)
distsq = dist * dist
if distsq > radiussq - epsilon:
return (None, None, INFINITE)
# must be on circle
dist2 = sqrt(radiussq - distsq)
if pdot(d, axis) < 0:
dist2 = -dist2
ccp = psub(center, psub(pmul(n2, dist), pmul(v, dist2)))
plane = Plane(edge.p1, pcross(pcross(d, direction), d))
(cp, l) = plane.intersect_point(direction, ccp)
return (ccp, cp, l)