def slice_file(image_position_patient_array, image_orientation_patient, output_path=None, input_path=None): print("Status: Loading File.") model = STLModel(input_path) stats = model.stats() print(stats) print("Status: Calculating Slices") v1 = Point3D(image_orientation_patient[0][0], image_orientation_patient[0][1], image_orientation_patient[0][2]) v2 = Point3D(image_orientation_patient[1][0], image_orientation_patient[1][1], image_orientation_patient[1][2]) org = Point3D(0, 0, 0) for i, slice_loc in enumerate(image_position_patient_array): slice_loc = Point3D(slice_loc[0], slice_loc[1], slice_loc[2]) dwg = Drawing(output_path + str(i) + '.svg', profile='tiny') plane = Plane(v1 + slice_loc, v2 + slice_loc, org + slice_loc) x_axis = Line(org + slice_loc, v1 + slice_loc) y_axis = Line(org + slice_loc, v2 + slice_loc) pairs = model.slice_at_plane(plane, x_axis, y_axis) for pair in pairs: dwg.add(dwg.line(pair[0], pair[1], stroke=rgb(0, 0, 0, "%"))) dwg.save() print("Status: Finished Outputting Slices")
def drawArm3Segment(canvas, angles, segments, all=None): theta0 = angles[0] theta1 = angles[1] - 90 theta2 = angles[2] - (90 + 180 - angles[1]) s1 = segments[0] s2 = segments[1] s3 = segments[2] print(theta0, theta1, theta2) origin = Point3D(0, 0, 10) s1p = Point3D(0, 0, s1) s2p = Point3D(s1p.x + s2, s1p.y, s1p.z) s2p = rotatePoint(s2p, theta1, s1p) s3p = Point3D(s2p.x + s3, s2p.y, s2p.z) s3p = rotatePoint(s3p, theta2, s2p) s4p = Point3D(s3p.x + 35, s3p.y, s3p.z) canvas.addPoint(origin) canvas.addPoint(s1p) canvas.addPoint(s2p) canvas.addPoint(s3p) canvas.addPoint(s4p) canvas.plot() return True
def move_tcp_to_plane(self, offset=0): # make line out of 0,0,0 and point b = Point3D(0, 0, 0) tcp_point = Point3D(self.getl()[0:3]) line = Line3D(b, tcp_point) # project line onto plane plane_point = self.plane.intersection(line) print(plane_point) plane_point = plane_point[0] print(plane_point) # get distance between tcp point and base point dis_tcp = b.distance(tcp_point) # get distance between plane point and base point dis_plane = b.distance(plane_point) # put negative or positive sign if dis_tcp > dis_plane: sign = -1 else: sign = +1 # get distance to plane distance = self.plane.distance(tcp_point).evalf() distance = sign * distance # move tcp # location = self.plane.projection( tcp_point ) # print(self.plane.distance( tcp_point )) self.move_tcp((0, 0, distance - offset))
def rotatePoint(point, angle, centerPoint=Point3D(0, 0, 0)): """Rotates a point around another centerPoint. Angle is in degrees. Rotation is counter-clockwise""" angle = math.radians(angle) x, z = (point.x - centerPoint.x, point.z - centerPoint.z) x, z = (x * math.cos(angle) - z * math.sin(angle), x * math.sin(angle) + z * math.cos(angle)) x, z = x + centerPoint.x, z + centerPoint.z return Point3D(x, point.y, z)
def create_particle_line(theta, phi): """ odredujemo dvije tocke jedna je ishodiste druga je usmjerena sa theta i phi, a nalazi se na proizvoljnoj udaljenosti 1 """ p_0 = Point3D(0, 0, 0) p_1 = Point3D(spher_2_cart(1., theta, phi)) return Line3D(p_0, p_1)
def compute_intersections(self, *detectors): self._intersection_points = collections.OrderedDict() all_intersections = collections.OrderedDict() # go thru all detectors if len(detectors) == 0: dets = self._rays.keys() for det_name, det in self._rays.items(): self._intersection_points[det_name] = [] ray_dict = collections.OrderedDict() if det_name in dets: # now go through all rays for i, ray in enumerate(det): # now all components collision_info = collections.OrderedDict() collision_info["surface"] = [] collision_info["point"] = [] collision_info["distance"] = [] for name, component in self._spacecraft_components.items(): # intersect the volume with the rays component.intersect_ray(ray) plane, point, distance = component.intersection current_ray_origin = Point3D(ray.ray_origin) if plane is not None and current_ray_origin.distance( Point3D(point)) <= current_ray_origin.distance( Point3D(ray.detector_origin)): collision_info["surface"].append("%s %s" % (name, plane)) collision_info["point"].append(point) collision_info["distance"].append(distance) self._intersection_points[det_name].append(point) ray_dict[i] = collision_info all_intersections[det_name] = ray_dict return all_intersections
def makeSphere(p1,p2,p3,p4): sp1=Point3D(seq(p1)) sp2=Point3D(seq(p2)) sp3=Point3D(seq(p3)) sp4=Point3D(seq(p4)) # helper planes e12=Plane((sp1+sp2)/2,sp2-sp1) e23=Plane((sp2+sp3)/2,sp3-sp2) e14=Plane((sp1+sp4)/2,sp4-sp1) # intersect to find the circumcenter its=e12.intersection(e23) l=its[0] r=sympy.Ray3D(l) ins=sympy.intersection(e14,r) m=ins[0] # check the radius r1=m.distance(sp1) r1 m.distance(sp2) m.distance(sp3) r1.evalf() # visualize k1=App.ActiveDocument.addObject("Part::Sphere","Sphere") k1.Placement.Base=p1 k1.Radius=0.2 k1.ViewObject.ShapeColor=(1.0,0.0,0.0) k2=App.ActiveDocument.addObject("Part::Sphere","Sphere") k2.Placement.Base=p2 k2.Radius=0.2 k2.ViewObject.ShapeColor=(1.0,0.0,0.0) k3=App.ActiveDocument.addObject("Part::Sphere","Sphere") k3.Placement.Base=p3 k3.Radius=0.2 k3.ViewObject.ShapeColor=(1.0,0.0,0.0) k4=App.ActiveDocument.addObject("Part::Sphere","Sphere") k4.Placement.Base=p4 k4.Radius=0.2 k4.ViewObject.ShapeColor=(1.0,0.0,0.0) k=App.ActiveDocument.addObject("Part::Sphere","Sphere") k.Radius.Value=r1.evalf() k.Placement.Base=FreeCAD.Vector(m.x.evalf(),m.y.evalf(),m.z.evalf()) k.ViewObject.Transparency=80 App.activeDocument().recompute()
def ransac(self, triangulated_points): """ Feature rejection function :param triangulated_points: np array, (num_features, 3) all triangulated points :return bestinliers: (N, ), indices of the inliers, where N is the total number of inliers """ # we only expect to have the 5-10% outliers, so we'll set the minimum inliers # to be 92% of the total number of features. num_features = triangulated_points.shape[0] # tunable parameters mininliers = .92 * num_features alpha = 0.042 maxinliers = 0 sympy_points = np.ndarray((num_features, 3)) for j in range(num_features): sympy_points[j] = Point3D(triangulated_points[j, 0], triangulated_points[j, 1], triangulated_points[j, 2]) # make an array of possible indices that we will randomly draw from for i in range(1000): # select 3 random points from triangulated points to compute a plane rand_pts = np.random.choice(triangulated_points.shape[0], 3) normal, d = self.compute_plane(triangulated_points[rand_pts, :]) # make the points into a plane object plane = Plane(Point3D(triangulated_points[rand_pts[0], :]), normal_vector=normal) # compute the reprojection error for all the points reproj = np.ndarray((num_features, )) for j in range(num_features): reproj[j] = plane.distance(Point3D(sympy_points[j, :])) # print(reproj) inliers = reproj < alpha ninliers = np.sum(inliers) # update the max values if applicable if ninliers > maxinliers: maxinliers = ninliers bestinliers = inliers # check exit condition if maxinliers > mininliers: print('required', i + 1, 'RANSAC attempts to remove outliers.') break print('number of inliers: ' + str(maxinliers)) # returns the inlier indices, not the actual values return bestinliers
def get_angle_xy_plane_tcp(self): """ calculate the angle between the tcp and the xy plane of the Robot """ from sympy import Plane, Point3D, Line3D normal_point = Point3D(0, 0, 1) l_point: Point3D = Point3D(self.Robot.i.to_matrix(self.Tcp)) p: Plane = Plane((0, 0, 0), normal_vector=normal_point) projection = p.projection(l_point) param = p.parameter_value(projection, u, v) angle = atan2(param[v], param[u]) return angle
def get_trajectory(tr2d, R, M, T, F, Z_start=1, Z_end = 0): ''' compute 3D trajectory for 2D image points. :param tr2d: 2d trajectory [[x1,y1],[x2,y2],...] :param R: Rotation matrix :param M: Camera matrix :param T: Translation matrix :param F: [R^T|-R^T@T] :param Z_start: Hight of first point in 2D trajectory :param Z_end: Hight of last point in 2D trajectory :return: array of 3D world points ''' start_im = tr2d[0][1:3] end_im = tr2d[-1][1:3] start_3D = get_3D_position(imgpoint=start_im, R=R, M=M, T=T, F=F, objpoint={'X': None, 'Y': None, 'Z': Z_start}, point=True) end_3D = get_3D_position(imgpoint=end_im, R=R, M=M, T=T, F=F, objpoint={'X': None, 'Y': None, 'Z': Z_end}, point=True) print(start_3D, end_3D) # Get plane through the two points where Z is known X_ws, Y_ws, Z_ws = start_3D[0], start_3D[1], start_3D[2] X_we, Y_we, Z_we = end_3D[0], end_3D[1], end_3D[2] start = np.array([X_ws, Y_ws, Z_ws]) ende = np.array([X_we, Y_we, Z_we]) rvec = ende - start # normal nv = np.cross((rvec), [0, 0, 1]) plane = Plane(Point3D(X_ws, Y_ws, Z_ws), normal_vector=(nv[0], nv[1], nv[2])) # Get point on plane for each point on 2D trajectory tr3D = [] for point in tr2d: imgpoint = point[1:3] lfa1, lfa2 = np.array(get_3D_position(imgpoint=imgpoint, R=R, M=M, T=T, F=F, objpoint={'X': None, 'Y': None, 'Z': None}, point=False)) line = Line3D(Point3D(lfa1[0], lfa1[1], lfa1[2]), Point3D(lfa2[0], lfa2[1], lfa2[2])) # Calculate intersect res = plane.intersection(line) res = [float(x) for x in res[0]] tr3D += [res] return tr3D
def _distance(self, s1, s2, index1, index2): xCoord = s1.x_coord_list[index1] yCoord = s1.y_coord_list[index1] zCoord = s1.z_coord_list[index1] newPoint1 = Point3D(xCoord, yCoord, zCoord) xCoord = s2.x_coord_list[index2] yCoord = s2.y_coord_list[index2] zCoord = s2.z_coord_list[index2] newPoint2 = Point3D(xCoord, yCoord, zCoord) return newPoint1.distance(newPoint2)
def estimateCoordinate(A, alpha, beta, h, W, ximg, yimg): B = Point3D(0.0, 0.0, 0.0) C = Point3D(20.0, 0.0, 0.0) Z = Point3D(10.0, 10.0, 0.0) # pABC = CG3dPlanePN(A, B, C) scale = (2 * tan (beta/2) * h / sin(alpha)) / W # scale = 1 xL = (ximg*scale*sin(alpha)) yL = (yimg*scale) zL = (ximg*scale*cos(alpha)) L = Point3D(xL, yL, zL) LineAL = Line3D(A, L) planeOxy = Plane(B, C, Z) result = LineAL.intersection(planeOxy) return result
def _calculate_ray_origin(self): theta = np.deg2rad(90.0 - self._point_source.lat.value) phi = np.deg2rad(self._point_source.lon.value) x = Ray._R * np.cos(phi) * np.sin(theta) y = Ray._R * np.sin(phi) * np.sin(theta) z = Ray._R * np.cos(theta) # this is the "distant orgin of the ray" self._origin = np.array([x, y, z]) self._sympy_line = Line3D(Point3D(self._detector.mount_point), Point3D(self._origin)) self._plot_origin = self.point_on_ray(Ray._scale)
def Gdubins3D(start_x, start_y, start_z, goal_x, goal_y, goal_z, start_dir_x, start_dir_y, rad): start_dir_z = math.sqrt(1 - start_dir_x**2 - start_dir_y**2) vec = [start_dir_x, start_dir_y, start_dir_z] dir_x = start_x + start_dir_x dir_y = start_y + start_dir_y dir_z = start_z + start_dir_z P = Plane(Point3D(start_x, start_y, start_z), Point3D(dir_x, dir_y, dir_z), Point3D(goal_x, goal_y, goal_z)) nor_vec = P.normal_vector ang = angle(vec, [goal_x - start_x, goal_y - start_y, goal_z - start_z], True) a3D = np.array([[nor_vec[0], dir_x - start_x, goal_x - start_x], [nor_vec[1], dir_y - start_y, goal_y - start_y], [nor_vec[2], dir_z - start_z, goal_z - start_z]], dtype='float') # print(a3D) # print() dist_g = distance([goal_x - start_x, goal_y - start_y, goal_z - goal_z], [0, 0, 0]) # dist_a = dist_v = distance(nor_vec, [0, 0, 0]) a2D = np.array([[0, 1000, dist_g * cos(ang)], [0, 0, dist_g * sin(ang)], [dist_v, 0, 0]]) # print(a2D) mat2Dconvert3D = a3D.dot(lg.inv(a2D)) mat3Dconvert2D = a2D.dot(np.linalg.inv(a3D)) px, py, pz, dis_d, goal_dir = Gdubins3D(start_x, start_y, start_z, goal_x, goal_y, goal_z, math.pi, mat2Dconvert3D=mat2Dconvert3D, mat3Dconvert2D=mat3Dconvert2D) return px, py, pz, dis_d, goal_dir
def distance(self, o): """ From https://github.com/sympy/sympy/blob/58e1e9abade7c38c66871fd06bf76ebac8c1df78/sympy/geometry/plane.py#L246-L298 but returns signed Distance not absolut :param self: Plane :param o: Point :return: signed distance between Plane and Point """ if self.intersection(o) != []: return S.Zero if isinstance(o, (Segment3D, Ray3D)): a, b = o.p1, o.p2 pi, = self.intersection(Line3D(a, b)) if pi in o: return self.distance(pi) elif a in Segment3D(pi, b): return self.distance(a) else: assert isinstance(o, Segment3D) is True return self.distance(b) # following code handles `Point3D`, `LinearEntity3D`, `Plane` a = o if isinstance(o, Point3D) else o.p1 n = Point3D(self.normal_vector).unit d = (a - self.p1).dot(n) return d
def __init__(self, label="Shape", description=None, circumscribed_shape=None, inscribed_shape_list=[], position=None, surface_color=Color((200, 200, 200)), *args, **kwargs): self.label = label self.description = description if position is None: print("AbstractShape.position --> default") position = Point3D(0, 0, 0) self.circumscribed_shape = None if circumscribed_shape is not None: circumscribed_shape.append_shape(self) self.inscribed_shape_list = list() if inscribed_shape_list is not None: for inscribed_shape in inscribed_shape_list: self.append_shape(inscribed_shape) self.position = position print("AbstractShape.position:{}".format(self.position)) self.surface_color = surface_color
def append_shape(self, shape, position=Point3D(0, 0, 0)): """It is mandatory to use this method to append shapes on parent shapes, to guarantee proper referential integrity between parents and children. """ shape.circumscribed_shape = self if position is not None: shape.position = position self.inscribed_shape_list.append(shape)
def calc_gaze_points_dist(right_eye1, left_eye1, right_eye2, left_eye2, points2): l1 = Line3D(Point3D(right_eye1[0], right_eye1[1], right_eye1[2]), Point3D(right_eye2[0], right_eye2[1], right_eye2[2])) l2 = Line3D(Point3D(left_eye1[0], left_eye1[1], left_eye1[2]), Point3D(left_eye2[0], left_eye2[1], left_eye2[2])) for p2 in points2: point = Point3D(p2[0], p2[1], p2[2]) dist1 = l1.distance(point) dist2 = l2.distance(point) if (dist1 < 100 or dist2 < 100): return True return False
def main(p1, p2): """ The main function. """ lon1, lat1 = map(float, p1.split(",")) lon2, lat2 = map(float, p2.split(",")) x1, y1, z1 = convert_ll2xyz(lat1, lon1) x2, y2, z2 = convert_ll2xyz(lat2, lon2) slice_plane = Plane(Point3D(x1, y1, z1), Point3D(x2, y2, z2), Point3D(0, 0, 0)) axis = slice_plane.normal_vector # print out the result print(f"x: {float(axis[0])}") print(f"y: {float(axis[1])}") print(f"z: {float(axis[2])}") print(f"point1 {x1} {y1} {z1}") print(f"point2 {x2} {y2} {z2}")
def create_detector_plane(R, theta, phi): x, y, z = spher_2_cart(R, theta, phi) origin = Point3D(x, y, z) normal_vector = (x, y, z) return Plane(origin, normal_vector), origin, normal_vector
def calcular_partes(self): puntos = list(self.limites) partes = {'I': [], 'II': [], 'III': [], 'IV': []} plano = self.sympy for segmento in self.plano_vertical_bordes: interseccion = intersection(plano, segmento) if interseccion: if isinstance(interseccion[0], Segment3D): puntos.append(segmento.points[0].coordinates) puntos.append(segmento.points[1].coordinates) else: puntos.append(interseccion[0].coordinates) for segmento in self.plano_horizontal_bordes: interseccion = intersection(plano, segmento) if interseccion: if isinstance(interseccion[0], Segment3D): puntos.append(segmento.points[0].coordinates) puntos.append(segmento.points[1].coordinates) else: puntos.append(interseccion[0].coordinates) interseccion = intersection( Segment3D(Point3D(500, 0, 0), Point3D(-500, 0, 0)), plano) if interseccion: if isinstance(interseccion[0], Segment3D): puntos.append((500, 0, 0)) puntos.append((-500, 0, 0)) else: puntos.append(interseccion[0].coordinates) for punto in puntos: if punto[1] >= 0 and punto[2] >= 0: partes['I'].append(punto) if punto[1] <= 0 and punto[2] >= 0: partes['II'].append(punto) if punto[1] <= 0 and punto[2] <= 0: partes['III'].append(punto) if punto[1] >= 0 and punto[2] <= 0: partes['IV'].append(punto) # partes = dict((k, list(map(self.ordenar_vertices, v))) for k, v in partes.items()) partes['I'] = self.ordenar_vertices(partes['I']) partes['II'] = self.ordenar_vertices(partes['II']) partes['III'] = self.ordenar_vertices(partes['III']) partes['IV'] = self.ordenar_vertices(partes['IV']) return partes
def getNodesNormalVector(self, nodes, gravity_load=False): if gravity_load: return (0., 0., 1.) points = [] for node in nodes: point = Point3D(self.nodes_array[node].tolist()) points.append(point) return Plane(points[0], points[1], points[2]).normal_vector
def makePlane(p1, p2, p3, p4): sp1 = Point3D(seq(p1)) sp2 = Point3D(seq(p2)) sp3 = Point3D(seq(p3)) sp4 = Point3D(seq(p4)) mp = (sp1 + sp2 + sp3 + sp4) / 4 e4 = Plane(sp1, sp2, sp3) e3 = Plane(sp1, sp2, sp4) e2 = Plane(sp1, sp4, sp3) e1 = Plane(sp4, sp2, sp3) n = Point3D(e1.normal_vector) + Point3D(e2.normal_vector) + Point3D( e3.normal_vector) + Point3D(e3.normal_vector) e = Plane(mp, n) m = e.p1 fm = FreeCAD.Vector(m.x.evalf(), m.y.evalf(), m.z.evalf()) fn = FreeCAD.Vector(n[0], n[1], n[2]) x = fm.cross(fn).normalize() y = x.cross(fn).normalize() Draft.makeWire( [fm.add(x.multiply(10)), fm.add(y.multiply(10)), fm.sub(x), fm.sub(y)], closed=True, face=True) w = Draft.makeWire([p1, p2, p3], closed=True, face=True) w.ViewObject.Transparency = 80 w.ViewObject.ShapeColor = (0., 1., 0.) w = Draft.makeWire([p4, p2, p3], closed=True, face=True) w.ViewObject.Transparency = 80 w.ViewObject.ShapeColor = (0., 1., 0.) w = Draft.makeWire([p1, p4, p3], closed=True, face=True) w.ViewObject.Transparency = 80 w.ViewObject.ShapeColor = (0., 1., 0.) w = Draft.makeWire([p1, p2, p4], closed=True, face=True) w.ViewObject.Transparency = 80 w.ViewObject.ShapeColor = (0., 1., 0.) for p in [p1, p2, p3, p4]: k1 = App.ActiveDocument.addObject("Part::Sphere", "Sphere") k1.Placement.Base = p k1.Radius = 0.5 k1.ViewObject.ShapeColor = (1.0, 0.0, 0.0) App.activeDocument().recompute()
def get_gps(tyleX,tyleY,coordX,coordY,gps,cameraAngle,droneOrientation,height,gyroscope): angleX=(coordX/(tyleX/2)-1)*(cameraAngle[0]/2) angleY = (coordY / (tyleY / 2) - 1) * (cameraAngle[1]/2) hypotenuseX=height/cos(radians(angleX)) hypotenuseY=height/cos(radians(angleY)) posObject=Point3D((hypotenuseX*sin(radians(angleX))),(hypotenuseY*sin(radians(angleY))),-height) print("pos3",posObject.x.evalf(),posObject.y.evalf(),posObject.z.evalf()) lineX = np.array([0,-cos(gyroscope[0]),-sin(gyroscope[0])]) lineY = np.array([-cos(gyroscope[1]),0,-sin(gyroscope[1])]) CameraFocusLine=np.cross(lineX,lineY) value=atan2(CameraFocusLine[1],CameraFocusLine[0]) print(value) h=sqrt(pow(CameraFocusLine[0],2)+pow(CameraFocusLine[1],2)) value2=atan2(h,-CameraFocusLine[2]) droneOrientation = radians(droneOrientation) rmatrixZ2 = Matrix( [[cos(value+droneOrientation), sin(value+droneOrientation), 0, 0], [-sin(value+droneOrientation), cos(value+droneOrientation), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) rmatrixY = Matrix([[cos(-value2),0, -sin(-value2), 0],[0, 1, 0, 0], [ sin(-value2),0, cos(-value2), 0], [0, 0, 0, 1]]) value3=atan2(posObject.y,posObject.x) hypot=sqrt(pow(posObject.x,2)+pow(posObject.y,2)) posObject = Point3D(hypot*cos(-value+value3),hypot*sin(-value+value3),-height) print("pos1",posObject.x.evalf(),posObject.y.evalf(),posObject.z.evalf()) posObject = posObject.transform(rmatrixY) print("pos2",posObject.x.evalf(),posObject.y.evalf(),posObject.z.evalf()) posObject = posObject.transform(rmatrixZ2) print("pos3",posObject.x.evalf(),posObject.y.evalf(),posObject.z.evalf()) posObject=Point3D(posObject.x*(-height/posObject.z),posObject.y*(-height/posObject.z),-height) print("pos3",posObject.x.evalf(),posObject.y.evalf(),posObject.z.evalf()) value=atan2(posObject.y,posObject.x) value=-(value+pi/2) print(value+pi/2) print(atan2(posObject.y,posObject.x)) gps_location=destination(gps,value,sqrt(pow(posObject.x,2)+pow(posObject.y,2))/1000) return (gps_location.latitude,gps_location.longitude)
def lineplane_intersection(line, plane, la=0): bbox = ((min([x[0] for x in plane]), min([x[1] for x in plane]), min([x[2] for x in plane])), (max([x[0] for x in plane]), max([x[1] for x in plane]), max([x[2] for x in plane]))) if bbox[0][la-1]<=line[0][la-1]<=bbox[1][la-1] and bbox[0][la-2]<=line[0][la-2]<=bbox[1][la-2]: s_line = Line3D(Point3D(line[0]), Point3D(line[1])) s_plane = Plane(Point3D(plane[0]), Point3D(plane[1]), Point3D(plane[2])) itp = s_plane.intersection(s_line) in_range = False if len(itp) > 0: intpoint = itp[0] if min([x[la] for x in line])<=intpoint[la]<=max([x[la] for x in line]): if bbox[0][0]<=intpoint[0]<=bbox[1][0]: if bbox[0][1] <= intpoint[1] <= bbox[1][1]: if bbox[0][2] <= intpoint[2] <= bbox[1][2]: in_range = True if in_range: return [float(x) for x in intpoint] else: return None else: return None
def rot_SD_u_mean(SD_init, user, rot_angle): user = array(user) angle = radians(float(rot_angle)) #rot_axe = vector(rot_axe[0],rot_axe[1],rot_axe[2]) SD_rotated = empty(shape(SD_init)) i = 0 for sd in SD_init: sd_v = vector(sd[0], sd[1], sd[2]) p0 = Point3D(0.0, 0.0, 0.0) p1 = Point3D(sd[0], sd[1], sd[2]) p2 = Point3D(user[0], user[1], user[2]) plane_actual = Plane(p0, p1, p2) rot_axe = plane_actual.normal_vector rot_axe = [rot_axe[0].n(5),rot_axe[1].n(5),rot_axe[2].n(5)] #print(rot_axe) rot_axe = vector(float(rot_axe[0]), float(rot_axe[1]), float(rot_axe[2])) #print(angle) sd_r = rotate(sd_v, angle=angle, axis=rot_axe) SD_rotated[i] = array([sd_r.x, sd_r.y, sd_r.z]) i += 1 return SD_rotated
def isHitEdge(ego, sim, init_degree): # init_degree = ego.state.rotation.y lane_center = sim.map_point_on_lane(ego.state.transform.position) ego_x = ego.state.transform.position.x ego_y = ego.state.transform.position.y ego_z = ego.state.transform.position.z ego_point = Point3D(ego_x, ego_y, ego_z) mp_x = lane_center.position.x mp_y = lane_center.position.y mp_z = lane_center.position.z mp_point = Point3D(mp_x, mp_y, mp_z) # x1, y1, z1 = 160.809997558594, 10.1931667327881, 8.11004638671875 # x_e_1, y_e_1, z_e_1 = 101.646751403809, 10.1278858184814, 8.18318462371826 # x6, y6, z6 = 24.9999961853027, 10.1931667327881, -3.77267646789551 # x_e_6, y_e_6, z_e_6 = 84.163330078125, 10.1277523040771, -3.77213048934937 # l1 = Line3D(Point3D(x1, y1, z1), Point3D(x_e_1, y_e_1, z_e_1)) # l6 = Line3D(Point3D(x6, y6, z6), Point3D(x_e_6, y_e_6, z_e_6)) diagnal_length = pow(ego.bounding_box.size.z, 2) + pow( ego.bounding_box.size.x, 2) diagnal_length = math.sqrt(diagnal_length) rotate_degree = abs(ego.state.rotation.y - init_degree) + 23.86 ego_size_z = (diagnal_length / 2.0) * math.sin(math.radians(rotate_degree)) if (l6.distance(mp_point) <= 1): lane_bound = mp_z - 2.2 if (ego.state.transform.position.z - ego_size_z <= lane_bound): util.print_debug("--- Cross the boundary --- ") return True if (l1.distance(mp_point) <= 1): lane_bound = mp_z + 2.2 if (ego.state.transform.position.z + ego_size_z >= lane_bound): util.print_debug("--- Cross the boundary --- ") return True return False
def isHitYellowLine(ego, sim, init_degree): lane_center = sim.map_point_on_lane(ego.state.transform.position) ego_x = ego.state.transform.position.x ego_y = ego.state.transform.position.y ego_z = ego.state.transform.position.z ego_point = Point3D(ego_x, ego_y, ego_z) mp_x = lane_center.position.x mp_y = lane_center.position.y mp_z = lane_center.position.z mp_point = Point3D(mp_x, mp_y, mp_z) # x1, y1, z1 = 145.000030517578, 10.1931667327881, 4.20298147201538 # x_e_1, y_e_1, z_e_1 = 132.136016845703, 10.1280860900879, 4.20766830444336 # x6, y6, z6 = 24.9999923706055, 10.1931667327881, 0.026848778128624 # x_e_6, y_e_6, z_e_6 = 82.6629028320313, 10.1278924942017, 0.0420729108154774 # l1 = Line3D(Point3D(x1, y1, z1), Point3D(x_e_1, y_e_1, z_e_1)) # l6 = Line3D(Point3D(x6, y6, z6), Point3D(x_e_6, y_e_6, z_e_6)) diagnal_length = pow(ego.bounding_box.size.z, 2) + pow( ego.bounding_box.size.x, 2) diagnal_length = math.sqrt(diagnal_length) rotate_degree = abs(ego.state.rotation.y - init_degree) + 23.86 ego_size_z = (diagnal_length / 2.0) * math.sin(math.radians(rotate_degree)) if (l1.distance(mp_point) <= 1): lane_bound = mp_z - 2.2 if (ego.state.transform.position.z - ego_size_z <= lane_bound): util.print_debug(" --- Cross the yellow line") return True if (l6.distance(mp_point) <= 1): lane_bound = mp_z + 2.2 if (ego.state.transform.position.z + ego_size_z >= lane_bound): util.print_debug(" --- Cross the yellow line") return True return False
def is_coplanar(pyptlist): """ This function checks if the list of points are coplanar. Parameters ---------- pyptlist : a list of tuples The list of points to be checked. List of points to be converted. A pypt is a tuple that documents the xyz coordinates of a pt e.g. (x,y,z), thus a pyptlist is a list of tuples e.g. [(x1,y1,z1), (x2,y2,z2), ...] Returns ------- True or False : bool If True the list of points are coplanar. """ from sympy import Point3D sympt_list = [] for pypt in pyptlist: sympt = Point3D(pypt[0], pypt[1], pypt[2]) sympt_list.append(sympt) coplanar = Point3D.are_coplanar(*sympt_list) return coplanar
def check_intersect(p1, p3, p4): p1 = Point3D(p1) p2 = Point3D(999, 999, 999) p3 = Point3D(p3) p4 = Point3D(p4) print(p1, p2, p3, p4) l1 = Line3D(p1, p2) l2 = Line3D(p3, p4) print(l1, l2) x = l1.intersection(l2) print(x) if len(x) == 0: return {"result": "false"} else: x = x[0] if x in [p1]: return {"result": "boundary"} else: return {"result": "true", "point": [x.x, x.y, x.z]}