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]}
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