def find_bin(line): truth_cases = [] for i in range(len(pent_polygons)): plane0 = Plane(Point3D(pent_polygons[i][0]), Point3D(pent_polygons[i][1]), Point3D(pent_polygons[i][2])) search_line = Line3D(Point3D(line), Point3D(0,0,0)) plane_search_intersection = plane0.intersection(search_line)[0].evalf() point_plane_dist = plane0.distance(Point3D(line)).evalf() intersectionX = plane_search_intersection.x intersectionY = plane_search_intersection.y intersectionZ = plane_search_intersection.z intersection_line = (intersectionX, intersectionY, intersectionZ) print i true_or_false = check_projections(pent_polygons[i], intersection_line) print i for j in range(3): temp_string = 'xyz' if true_or_false[j] == 1: truth_cases.append((i,temp_string[j],point_plane_dist,'pent')) for i in range(len(hex_polygons)): plane0 = Plane(Point3D(hex_polygons[i][0]), Point3D(hex_polygons[i][1]), Point3D(hex_polygons[i][2])) search_line = Line3D(Point3D(line), Point3D(0,0,0)) plane_search_intersection = plane0.intersection(search_line)[0].evalf() point_plane_dist = plane0.distance(Point3D(line)).evalf() intersectionX = plane_search_intersection.x intersectionY = plane_search_intersection.y intersectionZ = plane_search_intersection.z intersection_line = (intersectionX, intersectionY, intersectionZ) print i true_or_false = check_projections(hex_polygons[i], intersection_line) print i for j in range(3): temp_string = 'xyz' if true_or_false[j] == 1: truth_cases.append((i,temp_string[j],point_plane_dist,'hex')) print truth_cases
def plane_ray_intersection(plane, p1, p2):
from sympy import Point3D, Plane, Line, Polygon
point1 = Point3D(plane[0][0], plane[0][1], plane[0][2])
point2 = Point3D(plane[1][0], plane[1][1], plane[1][2])
point3 = Point3D(plane[2][0], plane[2][1], plane[2][2])
point4 = Point3D(plane[3][0], plane[3][1], plane[3][2])
planeL = Plane(point1, point2, point3)
planeR = Plane(point2, point3, point4)
line = Line(Point3D(p1[0], p1[1], p1[2]), Point3D(p2[0], p2[1], p2[2]))
inter1 = planeL.intersection(line)
inter2 = planeR.intersection(line)
if len(inter1) > 0:
return inter1[0]
if len(inter2):
return inter2[0]
return None
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 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 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 compute_intersections_3d(rays: List[MyRay3D],
road: Plane) -> List[Tuple[Rational, float]]:
"""
Compute intersections of rays from LED and road below the lamp. Zip x coordinates of each intersection
with intensity of the ray. If cosine_error has value "Yes" multiply intensity by reduction caused by the angle
of incident ray and the road.
:param rays: List of rays directed from LED
:param road: Segment representing road that rays should fall on
:param cosine_error: Yes/No indicator whether to work with cosine error or not
:return: List of tuples (x-coord of road intersection, intensity of incident ray)
"""
inter_array = []
for ray in rays:
inter_point = road.intersection(ray.ray_array[-1])
if inter_point:
#print_point_3d(inter_point[0])
intensity = ray.intensity
inter_array.append((inter_point[0].x, inter_point[0].y, intensity))
ray.road_intersection = (inter_point[0].x, inter_point[0].y,
inter_point[0].z)
return inter_array
class Surface(object):
def __init__(self, name, vertices):
self._vertices = vertices
self._name = name
if "+" in name:
self._sign = 1.0
elif "-" in name:
self._sign = -1.0
else:
raise RuntimeError("the plane name is wrong")
self._calculate_origin()
def _calculate_origin(self):
"""
compute the origin of the plane
:return:
"""
if "x" in self._name:
assert len(np.unique(
self._vertices[:, 0])) == 1, "vertices are wrong!"
self._normal = np.array([1, 0, 0]) * self._sign
x_origin = self._vertices[0, 0]
y_origin = (min(self._vertices[:, 1]) +
max(self._vertices[:, 1])) / 2.0
z_origin = (min(self._vertices[:, 2]) +
max(self._vertices[:, 2])) / 2.0
# self._edges = [self.]
elif "y" in self._name:
assert len(np.unique(
self._vertices[:, 1])) == 1, "vertices are wrong!"
self._normal = np.array([0, 1.0, 0]) * self._sign
x_origin = (min(self._vertices[:, 0]) +
max(self._vertices[:, 0])) / 2.0
y_origin = self._vertices[0, 1]
z_origin = (min(self._vertices[:, 2]) +
max(self._vertices[:, 2])) / 2.0
elif "z" in self._name:
assert len(np.unique(
self._vertices[:, 2])) == 1, "vertices are wrong!"
self._normal = np.array([0, 0, 1]) * self._sign
x_origin = (min(self._vertices[:, 0]) +
max(self._vertices[:, 0])) / 2.0
y_origin = (min(self._vertices[:, 1]) +
max(self._vertices[:, 1])) / 2.0
z_origin = self._vertices[0, 2]
self._xmax = self._vertices[:, 0].max()
self._ymax = self._vertices[:, 1].max()
self._zmax = self._vertices[:, 2].max()
self._xmin = self._vertices[:, 0].min()
self._ymin = self._vertices[:, 1].min()
self._zmin = self._vertices[:, 2].min()
self._origin = np.array([x_origin, y_origin, z_origin])
# TODO: perhaps rewrite this to find the plane
self._sympy_plane = Plane(Point3D(self._origin),
normal_vector=self._normal)
def is_intersecting(self, ray):
"""
checks if ray intersects plane
:param ray:
:return: bool, array
"""
intersecting_point = self._sympy_plane.intersection(ray.sympy_line)[0]
if "x" in self._name:
if self._within_y_bounds(
intersecting_point.y) and self._within_z_bounds(
intersecting_point.z):
return (
True,
np.array(
map(
float,
[
intersecting_point.x,
intersecting_point.y,
intersecting_point.z,
],
)),
)
elif "y" in self._name:
if self._within_x_bounds(
intersecting_point.x) and self._within_z_bounds(
intersecting_point.z):
return (
True,
np.array(
map(
float,
[
intersecting_point.x,
intersecting_point.y,
intersecting_point.z,
],
)),
)
elif "z" in self._name:
if self._within_y_bounds(
intersecting_point.y) and self._within_x_bounds(
intersecting_point.x):
return (
True,
np.array(
map(
float,
[
intersecting_point.x,
intersecting_point.y,
intersecting_point.z,
],
)),
)
return False, None
def _within_x_bounds(self, x):
if x <= self._xmax and self._xmin <= x:
return True
else:
return False
def _within_y_bounds(self, y):
if y <= self._ymax and self._ymin <= y:
return True
else:
return False
def _within_z_bounds(self, z):
if z <= self._zmax and self._zmin <= z:
return True
else:
return False
@property
def origin(self):
return self._origin
def intersect_cube(starting_point, direction_x, direction_y, direction_z, cube_info):
top_point_x = cube_info['cube_origin']['x_origin']
top_point_y = cube_info['cube_origin']['y_origin']
top_point_z = cube_info['cube_origin']['z_origin']
bottom_point_x = cube_info['cube_origin']['x_origin'] + cube_info['size']
bottom_point_y = cube_info['cube_origin']['y_origin'] + cube_info['size']
bottom_point_z = cube_info['cube_origin']['z_origin'] + cube_info['size']
bottom_plane = Plane(Point3D(top_point_x, top_point_y, top_point_z), normal_vector=(0, 0, -1))
back_plane = Plane(Point3D(top_point_x, top_point_y, top_point_z), normal_vector=(0, -1, 0))
side_plane1 = Plane(Point3D(top_point_x, top_point_y, top_point_z), normal_vector=(-1, 0, 0))
top_plane = Plane(Point3D(bottom_point_x, bottom_point_y, bottom_point_z), normal_vector=(0, 0, 1))
front_plane = Plane(Point3D(bottom_point_x, bottom_point_y, bottom_point_z), normal_vector=(0, 1, 0))
side_plane2 = Plane(Point3D(bottom_point_x, bottom_point_y, bottom_point_z), normal_vector=(1, 0, 0))
vector = Line3D(Point3D(starting_point['x'], starting_point['y'], starting_point['z']),
Point3D(starting_point['x'] + direction_x , starting_point['y'] + direction_y , starting_point['z'] + direction_z))
top_intersection = top_plane.intersection(vector)
bottom_intersection = bottom_plane.intersection(vector)
side1_intersection = side_plane1.intersection(vector)
side2_intersection = side_plane2.intersection(vector)
front_intersection = front_plane.intersection(vector)
back_intersection = back_plane.intersection(vector)
if len(top_intersection) > 0 and type(top_intersection[0]) is Point3D:
point = top_intersection[0]
if point.x < bottom_point_x and point.x > top_point_x and point.y < bottom_point_y and point.y > top_point_y:
print cube_info
print "top intersected intersectiont point : " + str(float(point.x)) + ', ' + str(float(point.y)) + '\n\n\n'
return True
if len(bottom_intersection) > 0 and type(bottom_intersection[0]) is Point3D:
point = bottom_intersection[0]
if point.x < bottom_point_x and point.x > top_point_x and point.y < bottom_point_y and point.y > top_point_y:
print "bottom intersected"
return True
if len(side1_intersection) > 0 and type(side1_intersection[0]) is Point3D:
point = side1_intersection[0]
if point.z < bottom_point_z and point.z > top_point_z and point.y < bottom_point_y and point.y > top_point_y:
print "side1 intersected"
return True
if len(side2_intersection) > 0 and type(side2_intersection[0]) is Point3D:
point = side2_intersection[0]
if point.z < bottom_point_z and point.z > top_point_z and point.y < bottom_point_y and point.y > top_point_y:
print "side2 intersected"
return True
if len(front_intersection) > 0 and type(front_intersection[0]) is Point3D:
point = front_intersection[0]
if point.x < bottom_point_x and point.x > top_point_x and point.z < bottom_point_z and point.z > top_point_z:
print "front intersected"
return True
if len(back_intersection) > 0 and type(back_intersection[0]) is Point3D:
point = back_intersection[0]
if point.x < bottom_point_x and point.x > top_point_x and point.z < bottom_point_z and point.z > top_point_z:
print "back intersected"
return True
print "no intersection found returning false!"
return False
class Surface(object):
def __init__(self, name, vertices):
self._vertices = vertices
self._name = name
if '+' in name:
self._sign = 1.
elif '-' in name:
self._sign = -1.
else:
raise RuntimeError('the plane name is wrong')
self._calculate_origin()
def _calculate_origin(self):
"""
compute the origin of the plane
:return:
"""
if 'x' in self._name:
assert len(np.unique(self._vertices[:, 0])) == 1, 'vertices are wrong!'
self._normal = np.array([1, 0, 0]) * self._sign
x_origin = self._vertices[0, 0]
y_origin = (min(self._vertices[:, 1]) + max(self._vertices[:, 1])) / 2.
z_origin = (min(self._vertices[:, 2]) + max(self._vertices[:, 2])) / 2.
# self._edges = [self.]
elif 'y' in self._name:
assert len(np.unique(self._vertices[:, 1])) == 1, 'vertices are wrong!'
self._normal = np.array([0, 1., 0]) * self._sign
x_origin = (min(self._vertices[:, 0]) + max(self._vertices[:, 0])) / 2.
y_origin = self._vertices[0, 1]
z_origin = (min(self._vertices[:, 2]) + max(self._vertices[:, 2])) / 2.
elif 'z' in self._name:
assert len(np.unique(self._vertices[:, 2])) == 1, 'vertices are wrong!'
self._normal = np.array([0, 0, 1]) * self._sign
x_origin = (min(self._vertices[:, 0]) + max(self._vertices[:, 0])) / 2.
y_origin = (min(self._vertices[:, 1]) + max(self._vertices[:, 1])) / 2.
z_origin = self._vertices[0, 2]
self._xmax = self._vertices[:, 0].max()
self._ymax = self._vertices[:, 1].max()
self._zmax = self._vertices[:, 2].max()
self._xmin = self._vertices[:, 0].min()
self._ymin = self._vertices[:, 1].min()
self._zmin = self._vertices[:, 2].min()
self._origin = np.array([x_origin, y_origin, z_origin])
# TODO: perhaps rewrite this to find the plane
self._sympy_plane = Plane(Point3D(self._origin), normal_vector=self._normal)
def is_intersecting(self, ray):
"""
checks if ray intersects plane
:param ray:
:return: bool, array
"""
intersecting_point = self._sympy_plane.intersection(ray.sympy_line)[0]
if 'x' in self._name:
if self._within_y_bounds(intersecting_point.y) and self._within_z_bounds(intersecting_point.z):
return True, np.array(map(float, [intersecting_point.x, intersecting_point.y, intersecting_point.z]))
elif 'y' in self._name:
if self._within_x_bounds(intersecting_point.x) and self._within_z_bounds(intersecting_point.z):
return True, np.array(map(float, [intersecting_point.x, intersecting_point.y, intersecting_point.z]))
elif 'z' in self._name:
if self._within_y_bounds(intersecting_point.y) and self._within_x_bounds(intersecting_point.x):
return True, np.array(map(float, [intersecting_point.x, intersecting_point.y, intersecting_point.z]))
return False, None
def _within_x_bounds(self, x):
if x <= self._xmax and self._xmin <= x:
return True
else:
return False
def _within_y_bounds(self, y):
if y <= self._ymax and self._ymin <= y:
return True
else:
return False
def _within_z_bounds(self, z):
if z <= self._zmax and self._zmin <= z:
return True
else:
return False
@property
def origin(self):
return self._origin