def get_point_of_intersection(plane, line):
# http://en.wikipedia.org/wiki/Line-plane_intersection
# a point on the plane
p0 = plane.points()[0]
# normal vector of plane
n = normal(plane)
# point on the line
l0, l1 = line.points()
l_vector = subtract(l1, l0)
# p0 - l0 dot n = numerator
numerator = geo.dot(subtract(p0, l0), n)
denominator = geo.dot(n, l_vector)
# print "numerator: %f" %numerator
# print "denominator: %f" %denominator
# denominator is 0, numerator is nonzero --> no intersection
if abs(denominator) < 1E-20 and numerator != 0:
return []
# both are zero, parallel - intersect at every point along line segment
elif denominator == 0 and numerator == 0:
# todo(ndunn): how to handle this case
return []
else:
d = numerator / denominator
# point of intersection = d*l + l_0
x = l_vector[0] * d + l0.coordinates()[0]
y = l_vector[1] * d + l0.coordinates()[1]
z = l_vector[2] * d + l0.coordinates()[2]
return geo.Point(x,y,z)
def line_risk(T_list, line):
risk = 0
for threat in T_list:
point = threat[0:2]
r = threat[2]
start = line[0]
end = line[1]
dx, dy = end[0]-start[0], end[1]-start[1] # vector(dx,dy)
#distance from starting point/ending point to circle center
dist_start = ((start[0]-point[0])**2 + (start[1]-point[1])**2)**0.5
dist_end = ((end[0]-point[0])**2 + (end[1]-point[1])**2)**0.5
# if line segment is in the circle, return distance of line segment
if dist_start <= r and dist_end <= r:
risk += (dx**2+dy**2)**0.5
else:
# parametric equation 參數式
# s=start, t=parameter, v=vector, q=circle center
# |s + t*v - q| = r, square both side
# t^2*(v*v) + 2t(v*(s−q)) + (s*s + q*q − 2s*q − r^2)=0
a = geo.dot((dx, dy), (dx, dy))
b = 2*(geo.dot((dx, dy), (start[0]-point[0],start[1]-point[1])))
c = geo.dot(start,start) + geo.dot(point,point) - 2*geo.dot(start,point) - r**2
disc = b**2 - 4*a*c #discriminant
# no intersections whether the line extends => no risk
if disc <= 0:
continue
else:
t1 = (-b + disc**0.5) / (2*a)
t2 = (-b - disc**0.5) / (2*a)
intersect1 = (start[0]+dx*t1, start[1]+dy*t1)
intersect2 = (start[0]+dx*t2, start[1]+dy*t2)
#print("intersections:", intersect1, intersect2)
#print("t1,t2:", t1, t2)
# no intersections in line segment
if not (0 < t1 < 1 or 0 < t2 < 1):
continue
# 2 intersections, risk = |intersection1-intersection2|
elif (0 < t1 < 1 and 0 < t2 < 1):
risk += ((intersect1[0]-intersect2[0])**2 + (intersect1[1]-intersect2[1])**2)**0.5
# only 1 intersection, risk = |inner_point-intersection|
else:
if dist_start <= r:
if 0 <= t1 <= 1:
risk += ((intersect1[0]-start[0])**2 + (intersect1[1]-start[1])**2)**0.5
else:
risk += ((intersect2[0]-start[0])**2 + (intersect2[1]-start[1])**2)**0.5
elif dist_end <= r:
if 0 <= t1 <= 1:
risk += ((intersect1[0]-end[0])**2 + (intersect1[1]-end[1])**2)**0.5
else:
risk += ((intersect2[0]-end[0])**2 + (intersect2[1]-end[1])**2)**0.5
return risk
def find_triangles_which_intersect(mesh, plane):
intersecting_triangles = []
# for each triangle, check whether any of the three line segments intersects the plane
for geom in mesh.scene.objects('geometry'):
for prim in geom.primitives():
try:
for tri in prim.triangles():
p1, p2, p3 = [geo.Point(vertex) for vertex in tri.vertices]
pairs = ( (p1, p2), (p1, p3), (p2, p3) )
for pair in pairs:
# if the plane separates the two points, then the line segment between them
# intersects with the plane
intersects = plane.separates(pair[0], pair[1])
coplanar = geo.dot(pair[0].r - plane.r, plane.n) * geo.dot(pair[1].r - plane.r, plane.n) == 0
if intersects or coplanar:
intersecting_triangles.append(tri)
break
except AttributeError:
# not a triangle set
pass
return intersecting_triangles
def collision_detect_angle(rs, v):
rsv_dot = geo.dot(rs, v)
if rsv_dot < 0 :
return None
elif rsv_dot == 0:
return math.pi/2
rs_value = geo.distance([0,0],rs)
v_value = geo.distance([0,0],v)
value = rsv_dot/(rs_value*v_value)
if value <=1 :
return_v = math.acos(value)
else:
return_v = math.acos(1)
return return_v
