Exemplo n.º 1
0
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
Exemplo n.º 3
0
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()
Exemplo n.º 4
0
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
Exemplo n.º 5
0
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
Exemplo n.º 7
0
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
Exemplo n.º 9
0
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