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 is_coplanar(a, b, tolerance_in_radians=0):
a1, a2, a3 = a
b1, b2, b3 = b
plane_a = Plane(Point3D(a1), Point3D(a2), Point3D(a3))
plane_b = Plane(Point3D(b1), Point3D(b2), Point3D(b3))
if not tolerance_in_radians: # only accept exact results
return plane_a.is_coplanar(plane_b)
else:
angle = plane_a.angle_between(plane_b).evalf()
angle %= np.pi # make sure that angle is between 0 and np.pi
return (angle - tolerance_in_radians <= 0.) or \
((np.pi - angle) - tolerance_in_radians <= 0.)
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 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_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 get_intersection_line3D(
lhs_plane: plotly.graph_objs.Surface,
rhs_plane: plotly.graph_objs.Surface) -> sympy.Line3D:
lhs_points = Point3D(lhs_plane.x[0, 0], lhs_plane.y[0, 0], lhs_plane.z[0, 0]), \
Point3D(lhs_plane.x[-1, -1], lhs_plane.y[-1, -1], lhs_plane.z[-1, -1]), \
Point3D(lhs_plane.x[0, -1], lhs_plane.y[0, -1], lhs_plane.z[0, -1])
rhs_points = Point3D(rhs_plane.x[0, 0], rhs_plane.y[0, 0], rhs_plane.z[0, 0]), \
Point3D(rhs_plane.x[-1, -1], rhs_plane.y[-1, -1], rhs_plane.z[-1, -1]), \
Point3D(rhs_plane.x[0, -1], rhs_plane.y[0, -1], rhs_plane.z[0, -1])
lhs_plane = Plane(*lhs_points)
rhs_plane = Plane(*rhs_points)
return lhs_plane.intersection(rhs_plane)
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 evaluate(individual: Component, env: Environment):
road = Plane(Point(0, -500, 0), Point(1, -500, 1), Point(4, -500, -4))
road_intersections = compute_intersections_3d(individual.original_rays,
road)
print(len(road_intersections))
compute_reflections_multiple_planes(individual)
road_intersections = compute_intersections_3d(individual.original_rays,
road)
print(len(road_intersections))
return len(road_intersections)
if env.configuration == "two connected":
compute_reflections_two_segments(individual, env.reflective_factor)
if env.configuration == "multiple free":
compute_reflection_multiple_segments(individual)
if env.quality_criterion == "light pollution":
return light_pollution(individual.original_rays)
if env.quality_criterion == "glare reduction":
return glare_reduction(individual)
road_intersections = compute_intersections(individual.original_rays,
env.road, env.cosine_error)
if env.quality_criterion == "efficiency":
return efficiency(road_intersections, individual.original_rays)
if env.quality_criterion == "illuminance uniformity":
segments_intensity = compute_segments_intensity(
road_intersections, env.road_sections, env.road_start,
env.road_length)
individual.segments_intensity_proportional = compute_proportional_intensity(
segments_intensity)
return illuminance_uniformity(segments_intensity)
return efficiency(individual)
def generate_reflective_segments(number_of_segments: int, base_length: int,
base_width: int):
reflective_segments = []
reflective_plane = Plane(Point(10, 0, base_length / 2),
Point(10, 0, -base_length / 2),
Point(10, 10, -base_length / 2))
reflective_segments.append(reflective_plane)
return reflective_segments
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 compute_reflection_plane(plane: Plane, ray: Ray) -> Ray:
all_intersections = ray.intersection(plane)
if len(all_intersections) != 1:
return None
intersection = all_intersections[0]
orig_point = ray.p1
parallel = plane.parallel_plane(orig_point)
perpendicular = plane.perpendicular_line(intersection)
meet_point = parallel.intersection(perpendicular)[0]
x_diff = (meet_point.x - orig_point.x)
y_diff = (meet_point.y - orig_point.y)
new_point = Point(meet_point.x + x_diff, meet_point.y + y_diff)
new_point = Point(orig_point.x,
intersection.y + (intersection.y - orig_point.y),
intersection.z + (intersection.z - orig_point.z))
reflected_ray = Ray(intersection, new_point)
return reflected_ray
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 test_geometry():
def do_test(*g, s=GeometrySeries, **kwargs):
s1 = _build_series(*g, pt="g", **kwargs)
assert isinstance(s1, s)
# since the range could be None, it is imperative to test that label
# receive the correct value.
assert s1.label == str(g[0])
s2 = _build_series(*g, **kwargs)
assert isinstance(s2, s)
assert s2.label == str(g[0])
assert np.array_equal(s1.get_data(), s2.get_data(), equal_nan=True)
x, y, z = symbols("x, y, z")
do_test(Point2D(1, 2))
do_test(Point3D(1, 2, 3))
do_test(Ray((1, 2), (3, 4)))
do_test(Segment((1, 2), (3, 4)))
do_test(Line((1, 2), (3, 4)), (x, -5, 5))
do_test(Ray3D((1, 2, 3), (3, 4, 5)))
do_test(Segment3D((1, 2, 3), (3, 4, 5)))
do_test(Line3D((1, 2, 3), (3, 4, 5)))
do_test(Polygon((1, 2), 3, n=10))
do_test(Circle((1, 2), 3))
do_test(Ellipse((1, 2), hradius=3, vradius=2))
do_test(Plane((0, 0, 0), (1, 1, 1)), (x, -5, 5), (y, -4, 4), (z, -3, 3),
s=PlaneSeries)
# Interactive series. Note that GeometryInteractiveSeries is an instance of
# GeometrySeries
do_test(Point2D(x, y), params={x: 1, y: 2})
do_test(
Plane((x, y, z), (1, 1, 1)),
(x, -5, 5),
(y, -4, 4),
(z, -3, 3),
params={
x: 1,
y: 2,
z: 3
},
s=PlaneInteractiveSeries,
)
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_plane_gradient(p1, p2, p3):
# Equation of type: a x + b y + c z + d = 0.
# Return gradient as 2D point: (-a / c, -b / c)
# p1, p2, p3 are Point3D objects.
plane_equation = Plane(p1, p2, p3).equation()
plane_equation_poly = poly(str(plane_equation),
gens=sp.symbols(['x', 'y', 'z']))
a = plane_equation_poly.diff('x')
b = plane_equation_poly.diff('y')
c = plane_equation_poly.diff('z')
return (-a / c, -b / c)
def fitPointsToPlane(points):
# if not isinstance(points,np.ndarray):
# points = np.array(points)
nvs = []
planes = pointsToPlanes(points)
nvs = [vectorToUnitVector(p.normal_vector.args) for p in planes]
nvs = np.array(nvs).astype(float)
#nvs = nvs/((nvs**2).sum(1)**0.5)[...,None]
nv = nvs.mean(0)
uv = vectorToUnitVector(nv)
points = np.array(points)
point = points.mean(0)
plane = Plane(tuple(point), tuple(uv))
return plane
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 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 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 parse(self, ns):
surfaces = self.elem.findall('./gml:surfaceMember/gml:Polygon', ns)
for surf in surfaces:
exterior = surf.find('./gml:exterior/gml:LinearRing', ns)
interior = surf.find('./gml:interior', ns)
if interior:
print('found poly interior, cant handle, ignoring')
positions = exterior.findall('.//gml:pos', ns)
pointIndexes = [0, int(len(positions)/4), 2*int(len(positions)/4)]
points = []
for i in pointIndexes:
point = re.sub('\\s+', ',', positions[i].text).split(',')
points.append(Point3D(point))
self.planes.append(Plane(*points))
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 test_vectors():
x, y, z = symbols("x:z")
N = CoordSys3D("N")
v1 = x * N.i + y * N.j
v2 = z * N.i + x * N.j + y * N.k
m1 = v1.to_matrix(N)
m2 = v2.to_matrix(N)
l1 = list(m1)
# I need a 2D vector: delete the last component, which is zero
l1 = l1[:-1]
l2 = list(m2)
# 2D vectors
s = _build_series(v1, (x, -10, 10), (y, -5, 5))
assert isinstance(s, Vector2DSeries)
s = _build_series(m1, (x, -10, 10), (y, -5, 5))
assert isinstance(s, Vector2DSeries)
s = _build_series(l1, (x, -10, 10), (y, -5, 5))
assert isinstance(s, Vector2DSeries)
s = _build_series(v1, (x, -10, 10), (y, -5, 5), pt="v2d")
assert isinstance(s, Vector2DSeries)
s = _build_series(m1, (x, -10, 10), (y, -5, 5), pt="v2d")
assert isinstance(s, Vector2DSeries)
s = _build_series(l1, (x, -10, 10), (y, -5, 5), pt="v2d")
assert isinstance(s, Vector2DSeries)
s = _build_series(v2, (x, -10, 10), (y, -5, 5), (z, -8, 8))
assert isinstance(s, Vector3DSeries)
s = _build_series(m2, (x, -10, 10), (y, -5, 5), (z, -8, 8))
assert isinstance(s, Vector3DSeries)
s = _build_series(l2, (x, -10, 10), (y, -5, 5), (z, -8, 8))
assert isinstance(s, Vector3DSeries)
s = _build_series(v2, (x, -10, 10), (y, -5, 5), (z, -8, 8), pt="v3d")
assert isinstance(s, Vector3DSeries)
s = _build_series(m2, (x, -10, 10), (y, -5, 5), (z, -8, 8), pt="v3d")
assert isinstance(s, Vector3DSeries)
s = _build_series(l2, (x, -10, 10), (y, -5, 5), (z, -8, 8), pt="v3d")
assert isinstance(s, Vector3DSeries)
s = _build_series(l2, (x, -10, 10), (y, -5, 5), (z, -8, 8),
slice=Plane((-2, 0, 0), (1, 0, 0)))
assert isinstance(s, SliceVector3DSeries)
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 parse(self, lod, ns):
name = self.elem.find('./gml:name', ns)
if not name.text:
self.name = ''.join(random.choice(string.ascii_uppercase + string.digits) for _ in range(16))
else:
self.name = re.sub(' ', '_', name.text)
surfaces = self.elem.findall('./brid:lod{0}Geometry/gml:MultiSurface/gml:surfaceMember/gml:Polygon'.format(lod), ns)
for surf in surfaces:
exterior = surf.find('./gml:exterior/gml:LinearRing', ns)
interior = surf.find('./gml:interior', ns)
if interior:
print('found poly interior, cant handle, ignoring')
positions = exterior.findall('.//gml:pos', ns)
pointIndexes = [0, int(len(positions)/4), 2*int(len(positions)/4)]
points = []
for i in pointIndexes:
point = re.sub('\\s+', ',', positions[i].text).split(',')
points.append(Point3D(point))
self.planes.append(Plane(*points))
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
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 estimate(self, eye1, eye2):
point1 = self.to_3D(eye1.coordinates)
point2 = self.to_3D(eye2.coordinates) * Rational(
eye1.radius / eye2.radius)
normal = cross(eye1.gaze, eye2.gaze)
plane1 = Plane(point1, normal_vector=normal)
plane2 = Plane(point2, normal_vector=normal)
line1 = Line(point1, direction_ratio=eye1.gaze)
line2 = Line(point2, direction_ratio=eye2.gaze)
plane1_line1 = plane1.projection_line(line1)
plane1_line2 = plane1.projection_line(line2)
plane2_line1 = plane2.projection_line(line1)
plane2_line2 = plane2.projection_line(line2)
target1, = plane1_line1.intersection(plane1_line2)
target2, = plane2_line1.intersection(plane2_line2)
return self.to_2D(target1 + target2) / 2
def retimage(yaw_all, pitch_all,leftxyz_all,phileft_all,rightxyz_all, phiright_all, pmat):
#have to tie the record number to each para coordinate in the list so you can compare the stimulus on either eye with heading. i.e. if you want
#to say something about where the para was wrt heading,
transformed_plist_r = []
transformed_plist_l = []
para_wrt_heading = []
headingvec = []
for frame in range(pmat.shape[1]):
print(frame)
yaw = np.radians(yaw_all[frame])
pitch = np.radians(pitch_all[frame])
phileft = np.radians(phileft_all[frame])
phiright = np.radians(phiright_all[frame])
rightxyz = np.array(rightxyz_all[frame])
leftxyz = np.array(leftxyz_all[frame])
if np.isnan(yaw) or np.isnan(pitch) or np.isnan(phileft) or np.isnan(phiright) or np.any(np.isnan(rightxyz)) or np.any(np.isnan(leftxyz)):
transformed_plist_r.append([])
transformed_plist_l.append([])
continue
planepoint = (rightxyz+leftxyz) / 2 #this is the midpoint between the eyes. use as the base of the heading vector
ufish = np.array([np.cos(yaw)*np.cos(pitch), np.sin(yaw)*np.cos(pitch), np.sin(pitch)])
headingvec.append(ufish)
ur_par = np.array([np.cos(yaw + np.pi/2), np.sin(yaw + np.pi/2), 0]) #this is a vector pointing out of the right eye when parallel to the body.
u_perp = np.cross(ufish,ur_par) #this will yield a vector normal to the shearing plane of the fish.
shearingplane = Plane(Point3D(planepoint[0],planepoint[1],planepoint[2]), normal_vector = (int(u_perp[0]*100), int(u_perp[1]*100), int(u_perp[2]*100)))
#normal vector input to a Plane object must be an int. if you don't multiply by 100, decimals are meaningless and just get rounded to 1 by int()
ur_xy = np.array([np.cos(yaw+phiright-np.pi/2), np.sin(yaw+phiright-np.pi/2),0]) #xy unit vectors pointing out of eyes
ul_xy = np.array([np.cos(yaw-phileft+np.pi/2), np.sin(yaw-phileft+np.pi/2),0])
#project xy vecs onto the shearing plane to give each one a Z coordinate
r_xy = rightxyz+ur_xy #point values b/c you have to project a point.
l_xy = leftxyz+ul_xy
r_xyz = shearingplane.projection(Point3D(r_xy[0],r_xy[1],r_xy[2]))
l_xyz = shearingplane.projection(Point3D(l_xy[0],l_xy[1],l_xy[2]))
vec3D_r = np.array([r_xyz.x - rightxyz[0], r_xyz.y - rightxyz[1], r_xyz.z - rightxyz[2]]).astype(np.float32)
vec3D_l = np.array([l_xyz.x - leftxyz[0], l_xyz.y - leftxyz[1], l_xyz.z - leftxyz[2]]).astype(np.float32)
#need to float32 these b/c r_xyz and l_xyz are sympy float types which can't be read out by numpy sqrt function
ur_xyz = vec3D_r / np.sqrt(np.dot(vec3D_r, vec3D_r)) #great these are 3D unit vecs pointing out of eyes!
ul_xyz = vec3D_l / np.sqrt(np.dot(vec3D_l, vec3D_l))
yaxis_r = np.cross(ur_xyz,u_perp)
yaxis_l = np.cross(ul_xyz,u_perp) #NOTE THAT THESE Y AXIS WILL GO IN DIFFERENT DIRECTIONS!! RIGHT TOWARDS TIP OF HEAD, LEFT TOWARDS THE TAIL. ALSO SIMPLY MAKE SURE THAT THIS IS ALL CORRECT.
#ke sure all vectors w/ yaxis, ur,ul or uperp are unit at this point. critical for transformation of basis.
temp_plist_r = []
temp_plist_l = []
temp_plist_heading = []
#for par_index in range(0,pmat.shape[0], 3): #comment this back in once you figure out why ir times are one too long.
for par_index in range(0,pmat.shape[0], 3):
para_xyz = pmat[par_index:par_index+3,frame]
wrt_heading = np.array([para_xyz[0]-planepoint[0], para_xyz[1]-planepoint[1], para_xyz[2]-planepoint[2]])
wrt_right = np.array([para_xyz[0]-rightxyz[0], para_xyz[1]-rightxyz[1], para_xyz[2]-rightxyz[2]])
wrt_left = np.array([para_xyz[0]-leftxyz[0], para_xyz[1]-leftxyz[1], para_xyz[2]-leftxyz[2]])
# if wrt_right[0] > 0: #this is wrong. if fish eye is at 500 facing hte para at 200, dont want to eliminate!! eliminate at dot stage.
new_basis_right = [np.dot(wrt_right, ur_xyz),np.dot(wrt_right,yaxis_r),np.dot(wrt_right,u_perp)]
# else:
# new_basis_right = []
# if wrt_left[0] > 0:
new_basis_left = [np.dot(wrt_left, ul_xyz),np.dot(wrt_left,yaxis_l),np.dot(wrt_left,u_perp)]
# else:
# new_basis_left = []
if magvector(new_basis_right) < 200 and new_basis_right[0] > 0: temp_plist_r.append(new_basis_right)
if magvector(new_basis_left) < 200 and new_basis_left[0] > 0: temp_plist_l.append(new_basis_left)
if magvector(wrt_heading) < 200: temp_plist_heading.append(wrt_heading)
transformed_plist_r.append(temp_plist_r)
transformed_plist_l.append(temp_plist_l)
para_wrt_heading.append(temp_plist_heading)
return transformed_plist_r,transformed_plist_l,para_wrt_heading
def pointsToPlanes(points):
planes = []
f = lambda x: planes.append(Plane(*x))
choice(points, 3, f)
return planes
return float(ag)
toArray = lambda x: np.array(x).astype(float)
def degreeOfVictor(p1, p2):
p1, p2 = map(toArray, (p1, p2))
dotProduct = (p1 * p2).sum()
norm = lambda p: (p**2).sum()**0.5
angle = dotProduct / (norm(p1) * norm(p2))
arccosDegree = lambda x: np.degrees(np.arccos(x))
degree = arccosDegree(angle)
return degree
#%%
if __name__ == '__main__':
po = Point3D(0, 0, 0)
# print fitPointsToPlane(points)
a.distance(po)
l = Line3D((0, 0, 0), (1, 0, 0))
l2 = Line3D((0, 1, 0), (0, 1, 1))
a = Plane(Point3D(1, 1, 1), normal_vector=(1, 1, 1))
b = a.perpendicular_line(Point3D(0, 0, 0))
points = (1, 0, 0), (0, 1, 0), (0, 0, 1), (0.1, 0.1, 0.1)
a.distance(Point3D(0, 0, 0))
pass
ray_end = [float(ray_end_str.strip('unitary]').split(" ")[1]), \
float(ray_end_str.strip('unitary]').split(" ")[3]), \
float(ray_end_str.strip('unitary]').split(" ")[5])]
# hdulist, ray_start, ray_end = extract_spectra(ds, impact)
print "(impact/proper_box_size)/x_width = ", (impact/proper_box_size)/x_width
# start by setting up plots
fig = plt.figure(figsize=(12,6), dpi=100)
# creates grid on which the figure will be plotted
gs = gridspec.GridSpec(3, 2)
## this will be the slice
ax_slice = fig.add_subplot(gs[:,0])
# figure out the plane we want
pl = Plane(Point3D(tuple(ray_start)), Point3D(tuple(ray_end)), Point3D(tuple(halo_center)))
# slc = yt.SlicePlot(ds,'x',('gas','density'),center=halo_center,width=x_width)
# slc = yt.SlicePlot(ds,np.array(pl.normal_vector),('gas','density'),center=halo_center,width=x_width)
## this one is trying a cartesian slice that does not go through the halo center
# slc = yt.SlicePlot(ds,'x',('gas','density'),center=[halo_center[0], ray_start[1], halo_center[2]],width=x_width)
slc = ds.r[halo_center[0], ymin:ymax, zmin:zmax]
## slc = ds.r[xmin:xmax, ymin:ymax, halo_center[2]]
res = [1000,1000]
# frb = slc.frb(x_width, res)
frb = slc.to_frb(x_width, res)
image = np.array(frb['gas', 'density'])
# image = np.array(frb)
print np.min(image), np.max(image)
# extent = [float(x.in_units('code_length')) for x in (slc.xlim + slc.ylim)]
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)
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
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