def toLine(l):
if isinstance(l, sympy.geometry.line.Line3D):
return l
if isinstance(l, np.ndarray):
if l.shape == (2, 3):
return Line3D(tuple(l[0]), tuple(l[1]))
l = tuple(l)
if isinstance(l, (list, tuple, sympy.geometry.point.Point3D,
sympy.core.containers.Tuple)):
return Line3D((0, 0, 0), l)
def add_orthogonal_vertices(bpairs, coords, loc, outfolder, force):
vertex_file = osp.join(outfolder, 'orthogonal_vertices.npy')
pair_file = osp.join(outfolder, 'orthogonal_pairs.npy')
if osp.isfile(vertex_file) and osp.isfile(pair_file) and not force:
closest_ortho_verts = np.load(vertex_file).tolist()
closest_ortho_pairs = np.load(pair_file).tolist()
else:
closest_ortho_pairs = []
closest_ortho_verts = []
vert_radius = 5
for i in bpairs:
if loc.get_contact_type(i[0]) in ['G', 'S']:
c1 = np.array(
loc.get_contact_coordinate('fs',
i[0],
coordinate_type='corrected'))[0]
c2 = np.array(
loc.get_contact_coordinate('fs',
i[1],
coordinate_type='corrected'))[0]
b1 = (c1 + c2) / 2
l1 = Line3D(c1, b1)
verts_near_bipolar = []
verts_distances = []
for v in coords:
v1 = np.array(list(v))
vp_dist = np.linalg.norm(b1 - v1)
if vp_dist < vert_radius:
verts_near_bipolar.append(v1)
verts_distances.append(vp_dist)
if len(verts_near_bipolar) == 0:
for v in coords:
v1 = np.array(list(v))
vp_dist = np.linalg.norm(b1 - v1)
if vp_dist < 2 * vert_radius:
verts_near_bipolar.append(v1)
verts_distances.append(vp_dist)
print('Found', len(verts_near_bipolar),
'vertices within radius', vert_radius, 'of bipolar')
closest_verts = [
x for _, x in sorted(
zip(verts_distances, verts_near_bipolar))
]
closest_vert = [0, 0, 0]
for vv1 in closest_verts:
l2 = Line3D(vv1, b1)
if abs(l1.angle_between(l2) - 1.5708) < 0.1:
closest_vert = vv1
break
closest_ortho_pairs.append(i)
closest_ortho_verts.append(closest_vert)
np.save(vertex_file, closest_ortho_verts)
np.save(pair_file, closest_ortho_pairs)
loc.set_pair_infos('closest_ortho_vertex_coordinate', closest_ortho_pairs,
np.vstack(closest_ortho_verts).tolist())
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 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 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 edge_param(edge, v1, v2, source, receiver):
# source, receiver :: Point3D
# edge :: Line3D(Point3D(), Point3D())
# v1, v2: vectors describing the edge
proj_src_edge = edge.projection(source)
proj_rec_edge = edge.projection(receiver)
ray_edge_src = Line3D(proj_src_edge, source) # Proj(S, edge) ---> S
ray_edge_rec = Line3D(proj_rec_edge, receiver) # Proj(R, edge) ---> R
x = float(v1.angle_between(v2))
teta_w = max(2.0 * numpy.pi - x, x)
teta_S = float(ray_edge_src.angle_between(v1))
teta_R = float(ray_edge_rec.angle_between(v1))
# print(raddeg(teta_S), raddeg(teta_R))
return teta_w, teta_R, teta_S
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 Scan(self,
sky,
zeta=np.radians(5.),
phi=math.radians(360.),
deltaphi=math.radians(1.)):
'''
Calculates in the BCRS the angle between the plane of the satellite and the line from the centre of the satellite to the star.
This angle is - zeta_angle_star_plane.
'''
self.observations = []
self.measurements = []
self.times = []
self.indexes = []
for idx, star in enumerate(sky.elements):
star_point = vector_to_point(star.vector)
star_line = Line3D(self.xyplane.args[0], star_point)
arc_angle_star_xyplane = self.xyplane.angle_between(star_line)
if len(arc_angle_star_xyplane.args) == 2:
zeta_angle_star_plane = -float(arc_angle_star_xyplane.args[1])
if len(arc_angle_star_xyplane.args) == 1:
zeta_angle_star_plane = float(arc_angle_star_xyplane.args[0])
if -zeta / 2. < (zeta_angle_star_plane) < zeta / 2.:
self.indexes.append(idx)
proy_star_point = self.xyplane.projection(star_point)
proy_star_vector = point_to_vector(proy_star_point)
proy_star_vector_srs = SRS(self, proy_star_vector)
phi_angle_obs = np.arctan2(float(proy_star_vector_srs[1]),
float(proy_star_vector_srs[0]))
zeta_angle = np.arctan2(
float(proy_star_vector_srs[2]),
float(
np.sqrt((proy_star_vector_srs[0])**2 +
(proy_star_vector_srs[0])**2)))
if phi_angle_obs < 0.:
phi_angle_obs = phi_angle_obs + 2 * np.pi
observation = Observation(phi_angle_obs, zeta_angle)
self.observations.append(observation)
'''
Once observations are made, now we pass the scan to see at what times if the star in the detector's range
'''
#maybe change this to +- deltaphiphi/2 at some point? but careful that phi > 0
for i in np.arange(0, phi, deltaphi):
self.ViewLine(i, 0)
axis1phi = self.phi % (2 * np.pi)
axis2phi = (self.phi + deltaphi) % (2 * np.pi)
for observation in self.observations:
if axis1phi < observation.azimuth and observation.azimuth < axis2phi:
time = i % (np.pi * 2)
self.times.append(time)
def edge_quant(edge, source, receiver, res):
# the apex point optimaziation is not implemented
# source, receiver :: Point3D
# edge :: Line3D(Point3D(), Point3D())
edgelength = float(edge.p1.distance(edge.p2))
zaxis = numpy.linspace(res * 0.5, edgelength - res * 0.5,
int(edgelength / (res)))
rS, rR = float(edge.distance(source)), float(edge.distance(receiver))
_p1, _p2 = edge.p1, edge.p2
proj_src_edge = edge.projection(source)
proj_rec_edge = edge.projection(receiver)
# >>> determine relative z position of the source: >>>>>>>>>>>>>>>>>>>>>>>>
alpha = edge.angle_between(Line3D(_p1, proj_src_edge)).evalf()
if abs(alpha) < 0.01: # 0 degree
z_shift = float(proj_src_edge.distance(_p1))
elif abs(alpha - numpy.pi) < 0.01: # 180 degree
z_shift = -float(proj_src_edge.distance(_p1))
else:
raise ValueError('z_shift: alpha = %.3f Should be 0 or pi.' % alpha)
zaxis += -z_shift
# >>> z(receiver): >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
#
abs_zR = float(proj_src_edge.distance(proj_rec_edge))
if abs_zR < 0.001:
zR = 0.0
else:
alpha = edge.angle_between(Line3D(proj_src_edge,
proj_rec_edge)).evalf()
if abs(alpha) < 0.01: # 0 degree
zR = abs_zR
elif abs(alpha - numpy.pi) < 0.01: # 180 degree
zR = -abs_zR
else:
raise ValueError('zR: alpha = %.3f Should be 0 or pi.' % alpha)
m, l = (zaxis**2.0 + rS**2.0)**0.5, (zaxis**2.0 + rR**2.0)**0.5
return m, l, zaxis, rS, rR, zR
def angle(e1, e2):
tol = 0.001
alpha = e1.angle_between(e2).evalf()
if abs(alpha - 0.5 * numpy.pi) < tol:
arrangement = 'perpendicular'
elif abs(alpha - numpy.pi) < tol: # 180 degree
arrangement = 'paralell'
e2 = Line3D(e2.p2, e2.p1)
elif abs(alpha) < tol: # 0 degree
arrangement = 'paralell'
else:
arrangement = 'general'
return arrangement, e1, e2
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)
# We do not know the correct values for Borregas Ave map and Lincoln2017MKZ (Apollo 5.0) vehicle
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 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)
# We do not know the correct values for Borregas Ave map and Lincoln2017MKZ (Apollo 5.0) vehicle
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 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 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 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 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
# === rectangle model =====================================
h = 60.0
w = 45.0
th = 2.0
R1 = Point3D(-w * 0.5, -h * 0.5, 0.0, evaluate=geomeval)
R2 = Point3D(+w * 0.5, -h * 0.5, 0.0, evaluate=geomeval)
R3 = Point3D(+w * 0.5, +h * 0.5, 0.0, evaluate=geomeval)
R4 = Point3D(-w * 0.5, +h * 0.5, 0.0, evaluate=geomeval)
R10 = Point3D(-w * 0.5, -h * 0.5, -th, evaluate=geomeval)
R20 = Point3D(+w * 0.5, -h * 0.5, -th, evaluate=geomeval)
R30 = Point3D(+w * 0.5, +h * 0.5, -th, evaluate=geomeval)
R40 = Point3D(-w * 0.5, +h * 0.5, -th, evaluate=geomeval)
# edges [edge, v1, v2]:
e1 = [Line3D(R1, R2), Ray3D(R2, R3), Ray3D(R1, R10)]
e2 = [Line3D(R2, R3), Ray3D(R3, R4), Ray3D(R2, R20)]
e3 = [Line3D(R3, R4), Ray3D(R4, R1), Ray3D(R3, R30)]
e4 = [Line3D(R4, R1), Ray3D(R1, R2), Ray3D(R4, R40)]
edges = [e1, e2, e3, e4]
# === plot the geometry of the setup =======================
quiveropts = dict(headlength=0,
scale=1,
scale_units='xy',
headwidth=1,
headaxislength=0)
if rec_model == 'pointsource':
draw_xy([source], edges, receiver, quiveropts, DS)
elif rec_model == 'pointarray':
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
P1t = P1.subs(x == Lparam[_sage_const_0].right(),
y == Lparam[_sage_const_1].right(),
z == Lparam[_sage_const_2].right())
tt = solve(P1t, t)[_sage_const_0].right()
xLP1 = Lparam[_sage_const_0].subs(t=tt)
yLP1 = Lparam[_sage_const_1].subs(t=tt)
zLP1 = Lparam[_sage_const_2].subs(t=tt)
# запишем наши объекты через sympy
from sympy import Plane, Point, Point3D, Line3D
Psp = Plane(Point3D(pointP), normal_vector=n)
M1sp = Point3D(M1)
prtemp = Psp.projection(M1sp)
# проекция M1 на P
prM1P = vector([prtemp.x._sage_(), prtemp.y._sage_(), prtemp.z._sage_()])
# наша L через sympy
Lsp = Line3D(pointL, point2L)
prtemp = Lsp.projection(M1sp)
# проекция M1 на L
prM1L = vector([prtemp.x._sage_(), prtemp.y._sage_(), prtemp.z._sage_()])
# расстояние от M2 до P2
P2sp = Plane(Point3D(M1), normal_vector=n2)
dM2P2 = P2sp.distance(Point3D(M2))
# плоскость через M1 и L
P6 = Simp_Plane(Make_Plane(M1, pointL - M1, sL))
# проекция L1 на P
# уравнение L1 в параметрическом виде
pointL1 = vector([
M2[_sage_const_0] + a[_sage_const_0],
M2[_sage_const_1] + a[_sage_const_1],
M2[_sage_const_2] + a[_sage_const_2]
])
def edge_quant2(edge1, edge2, source, receiver, res):
# source, receiver :: Point3D
# edge :: Line3D(Point3D(), Point3D())
arrangement, edge1, edge2 = angle(edge1, edge2)
rS, rR = float(edge1.distance(source)), float(edge2.distance(receiver))
edge1length = float(edge1.p1.distance(edge1.p2))
edge2length = float(edge2.p1.distance(edge2.p2))
if arrangement == 'paralell':
# z axis: edge1 p1 -> p2
# z(source) = 0
P1 = edge1.projection(source)
P2 = edge2.projection(source)
P3 = edge2.projection(receiver)
r12 = float(edge1.distance(edge2.p1))
Dz1 = float(P1.distance(edge1.p1))
Dz2 = float(P2.distance(edge2.p1))
Dz3 = float(P3.distance(P2))
al1 = edge1.angle_between(Line3D(edge1.p1, P1)).evalf()
al2 = edge2.angle_between(Line3D(edge2.p1, P2)).evalf()
al3 = edge2.angle_between(Line3D(P1, P2)).evalf()
if Dz1 < 0.001: # --- zaxis1 shift -----------------------------------
z_shift1 = 0.0
else:
if al1 < 0.01: # 0 degree
z_shift1 = Dz1
elif abs(al1 - numpy.pi) < 0.01: # 180 degree
z_shift1 = -Dz1
else:
raise ValueError('z_shift1: alpha = %.3f Should be 0 or pi.' %
al1)
if Dz2 < 0.001: # --- zaxis2 shift -----------------------------------
z_shift2 = 0.0
else:
if al2 < 0.01: # 0 degree
z_shift2 = Dz2
elif abs(al2 - numpy.pi) < 0.01: # 180 degree
z_shift2 = -Dz2
else:
raise ValueError('z_shift2: alpha = %.3f Should be 0 or pi.' %
al1)
if Dz3 < 0.001: # --- z(receiver) _-----------------------------------
zrec = 0.0
else:
if al3 < 0.01: # 0 degree
zrec = Dz3
elif abs(al3 - numpy.pi) < 0.01: # 180 degree
zrec = -Dz3
else:
raise ValueError('z_shift3: alpha = %.3f Should be 0 or pi.' %
al1)
zaxis1 = numpy.linspace(res * 0.5 - z_shift1,
edge1length - res * 0.5 - z_shift1,
int(edge1length / (res)))
zaxis2 = numpy.linspace(res * 0.5 - z_shift2,
edge2length - res * 0.5 - z_shift2,
int(edge2length / (res)))
# >>> l1(n x 1), l2(n x m), l3(1 x m) >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
l1 = (zaxis1**2.0 + rS**2.0)**0.5
l2 = ((zaxis1[:, numpy.newaxis] - zaxis2[numpy.newaxis, :])**2.0 +
r12**2.0)**0.5
l3 = ((zaxis2 - zrec)**2.0 + rS**2.0)**0.5
elif arrangement == 'perpendicular':
pass
else:
raise ValueError(
'Only paralell and 90degree configurations are supported.')
return l1, l2, l3, zaxis1, zaxis2, rS, rR, r12, zrec
def alignChain(entry,
prec=1E-4,
seed_index=0,
supercell=2,
c_mag=50,
dist_from_line=0):
"""
Align a 2D material such that the 'c' vector is perpendicular to the
in-plane lattice vectors
inputs
--------
entry (list): A set of components necessary for the TSA.
Makes it easier to parallelize with this as
the input
--structure (Structure): pymatgen Structure object
--tol (float): The scaling for the atomic bonds
--mp_id (str): The label for the entry, commonly
the MaterialsProject ID
prec (float): The precision to compare magnitude of vectors
representing the bonds in the system
seed_index (int): The site to use as the starting point for the
TSA. Typically does not impact the results, but
will if the structure is a bipartide or has
mixed dimensionality
supercell (int): The supercell size to generate for
periodic networks
c_mag (float): The magnitude to make the non-periodic vectors
dist_from_line (float): Maximum distance an atom can be from the
line parallel to the nanowire. Is relevant
when the atoms in the nanowire are spread
across periodic boundary conditions in the
unit cell
returns
--------
list (list): -fractional coordinates in new lattice
-new lattice (a,b,c)
-species associated with each site
"""
new_struct = copy.deepcopy(entry[0])
new_latt = getNewLattice(entry, 1, prec, seed_index, supercell, c_mag)
print(new_latt)
v1, v2, perp = new_latt
new_latt = np.array(new_latt)
line1 = Line3D(new_latt[0], [0, 0, 0])
line = line1.parallel_line(new_struct.sites[0].coords)
trans = list(itertools.product([1, -1, 0], repeat=3))
print('WORKING1')
lat = np.array(new_struct.lattice.as_dict()['matrix'])
final_sites = []
i = 0
i = -1
for site in [x.coords for x in new_struct.sites]:
print(i, new_struct.num_sites)
i += 1
point = Point3D(site)
if line.distance(point) < dist_from_line:
final_sites.append(site)
else:
news = []
for t in trans:
point = Point3D(site + np.dot(lat.T, t))
news.append([float(line.distance(point)), t])
news.sort(key=lambda x: x[0])
final_sites.append(site + np.dot(lat.T, news[0][1]))
#new_latt = np.array([,perp2])
new_fracs = np.linalg.solve(new_latt.T, np.array(final_sites).T).T
species = new_struct.species
return ([species, new_fracs, new_latt])
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
