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