def contains(self, point): """ Returns True if the point in inside the cylinder and False if it is on the surface or outside. >>> # Inside >>> cylinder = Cylinder(.5, 2) >>> cylinder.contains([.25, .25, 1]) True >>> # On surface >>> cylinder.contains([.0, .0, 2.]) False >>> # Outside >>> cylinder.contains([-1,-1,-1]) False """ if self.on_surface(point) == True: return False local_point = transform_point(point, tf.inverse_matrix(self.transform)) origin_z = 0. xydistance = np.sqrt(local_point[0]**2 + local_point[1]**2) if intervalcheckstrict(origin_z, local_point[2], self.length) == True and xydistance<self.radius: return True else: return False
def surface_normal(self, ray, acute=True): """ Return the surface normal for a ray arriving on the CSGsub surface. """ invtransform = tf.inverse_matrix(self.transform) localray = Ray() localray.position = transform_point(ray.position, invtransform) localray.direction = transform_direction(ray.direction, invtransform) bool1 = self.SUBplus.on_surface(localray.position) bool2 = self.SUBminus.on_surface(localray.position) assertbool = False if bool1 == True and self.SUBminus.contains(localray.position) == False: assertbool = True if bool2 == True and self.SUBplus.contains(localray.position) == True: assertbool = True assert assertbool == True if bool1 == True and self.SUBminus.contains(localray.position) == False: return self.SUBplus.surface_normal(ray, acute) if bool2 == True and self.SUBplus.contains(localray.position) == True: if acute: return self.SUBminus.surface_normal(ray,acute) else: normal = -1 * self.SUBminus.surface_normal(ray, acute=True) # Remove signed zeros for i in range(0,3): if normal[i] == 0.0: normal[i] = 0.0 return normal
def on_surface(self, point): """Returns True if the point is on the plane's surface and false otherwise.""" inv_transform = tf.inverse_matrix(self.transform) rpos = transform_point(ray.position, inv_transform) if cmp_floats(rpos, 0.) and (0. < rpos[0] <= self.length ) and (0. < rpos[1] <= self.width): return True return False
def surface_identifier(self, surface_point, assert_on_surface = True): local_point = transform_point(surface_point, tf.inverse_matrix(self.transform)) origin_z = 0. xydistance = np.sqrt(local_point[0]**2 + local_point[1]**2) """ Assert surface_point on surface """ assertbool = False if intervalcheck(origin_z, local_point[2], self.length) == True: if cmp_floats(xydistance, self.radius) == True: surfacename = 'hull' assertbool = True if smallerequalto(xydistance,self.radius): if cmp_floats(local_point[2], origin_z) == True: surfacename = 'base' assertbool = True if cmp_floats(local_point[2], self.length) == True: surfacename = 'cap' assertbool = True if assert_on_surface == True: assert assertbool, "The assert bool is wrong." return surfacename
def surface_identifier(self, surface_point, assert_on_surface = True): """ Returns surface-ID name if surface_point located on CSGint surface """ """ Ensure surface_point on CSGint surface """ invtransform = tf.inverse_matrix(self.transform) local_point = transform_point(surface_point, invtransform) bool1 = self.INTone.on_surface(local_point) bool2 = self.INTtwo.on_surface(local_point) assertbool = False if bool1 == True and self.INTtwo.contains(local_point) == True: assertbool = True if bool2 == True and self.INTone.contains(local_point) == True: assertbool = True if bool1 == bool2 == True: assertbool = True if assert_on_surface == True: assert assertbool == True if bool1 == True: return self.reference + "_INTone_" + self.INTone.surface_identifier(local_point) if bool2 == True: return self.reference + "_INTtwo_" + self.INTtwo.surface_identifier(local_point)
def surface_normal(self, ray, acute=True): """ Returns surface normal in point where ray hits CSGint surface """ """ Ensure surface_point on CSGint surface """ invtransform = tf.inverse_matrix(self.transform) localray = Ray() localray.position = transform_point(ray.position, invtransform) localray.direction = transform_direction(ray.direction, invtransform) bool1 = self.INTone.on_surface(localray.position) bool2 = self.INTtwo.on_surface(localray.position) assertbool = False if bool1 == True and self.INTtwo.contains(localray.position) == True: assertbool = True if bool2 == True and self.INTone.contains(localray.position) == True: assertbool = True if bool1 == bool2 == True: assertbool = True assert assertbool == True if bool1 == True: return self.INTone.surface_normal(ray, acute) else: return self.INTtwo.surface_normal(ray, acute)
def on_surface(self, point): """ Returns True or False dependent on whether point on CSGadd surface or not """ if self.contains(point): return False invtransform = tf.inverse_matrix(self.transform) local_point = transform_point(point, invtransform) bool1 = self.ADDone.on_surface(local_point) bool2 = self.ADDtwo.on_surface(local_point) if bool1 == True and self.ADDtwo.contains(local_point) == False: return True if bool2 == True and self.ADDone.contains(local_point) == False: return True if bool1 == bool2 == True: return True else: return False
def surface_normal(self, ray, acute=True): """ Return the surface normal for a ray on the shape surface. An assert error is raised if the ray is not on the surface. >>> cylinder = Cylinder(2, 2) >>> #Bottom cap in >>> ray = Ray([0,0,0], [0,0,1]) >>> cylinder.surface_normal(ray) array([ 0., 0., 1.]) >>> #Bottom cap out >>> ray = Ray([0,0,0], [0,0,-1]) >>> cylinder.surface_normal(ray) array([ 0., 0., -1.]) >>> # End cap in >>> ray = Ray([0,0,2], [0,0,-1]) >>> cylinder.surface_normal(ray) array([ 0., 0., -1.]) >>> # End cap out >>> ray = Ray([0,0,2], [0,0,1]) >>> cylinder.surface_normal(ray) array([ 0., 0., 1.]) >>> # Radial >>> ray = Ray([2, 0, 1], [1,0,0]) >>> cylinder.surface_normal(ray) array([ 1., 0., 0.]) """ assert self.on_surface(ray.position), "The ray is not on the surface." invtrans = tf.inverse_matrix(self.transform) rpos = transform_point(ray.position, invtrans) rdir = transform_direction(ray.direction, invtrans) # point on radius surface pt_radius = np.sqrt(rpos[0]**2 + rpos[1]**2) c0 = cmp_floats(pt_radius, self.radius) #point on end caps c1 = cmp_floats(rpos[2], .0) c2 = cmp_floats(rpos[2], self.length) # check radius first if c0 and (c1 == c2): normal = norm(np.array(rpos) - np.array([0,0,rpos[2]])) elif c1: normal = np.array([0,0,-1]) else: # Create a vector that points from the axis of the cylinder to the ray position, # this is the normal vector. normal = np.array([0,0,1]) if acute: if angle(normal, rdir) > np.pi*0.5: normal = normal * -1. return transform_direction(normal, self.transform)
def contains(self, point): """Returns True is the point is inside the box or False if it is not or is on the surface. >>> box = Box([1,1,1], [2,2,2]) >>> box.contains([2,2,2]) False >>> box = Box([1,1,1], [2,2,2]) >>> box.contains([3,3,3]) False >>> # This point is not the surface within rounding errors >>> box = Box([0,0,0], [1,1,1]) >>> box.contains([ 0.04223342, 0.99999999999999989 , 0.35692177]) False >>> box = Box([0,0,0], [1,1,1]) >>> box.contains([ 0.04223342, 0.5 , 0.35692177]) True """ #local_point = transform_point(point, tf.inverse_matrix(self.transform)) #for pair in zip((self.origin, local_point, self.extent)): # if not pair[0] < pair[1] < pair[2]: # return False #return True local_point = transform_point(point, tf.inverse_matrix(self.transform)) for i in range(0,3): #if not (self.origin[i] < local_point[i] < self.extent[i]): # Want to make this comparison: self.origin[i] < local_point[i] < self.extent[i] c1 = cmp_floats_range(self.origin[i], local_point[i]) #print self.origin[i], " is less than ", local_point[i] if c1 == -1: b1 = True else: b1 = False #print b1 c2 = cmp_floats_range(local_point[i], self.extent[i]) #print local_point[i], " is less than ", self.extent[i] if c2 == -1: b2 = True else: b2 = False #print b2 if not (b1 and b2): return False return True """ # Alternatively:
def contains(self, point): """ Returns True if ray contained by CSGint, False otherwise """ invtransform = tf.inverse_matrix(self.transform) point = transform_point(point, invtransform) bool1 = self.INTone.contains(point) bool2 = self.INTtwo.contains(point) if bool1 == bool2 == True: return True else: return False
def on_surface(self, point): """ >>> # On surface >>> cylinder = Cylinder(.5, 2.) >>> cylinder.on_surface([.0, .0, 2.]) True """ """ # !!! Old version !!! local_point = transform_point(point, tf.inverse_matrix(self.transform)) # xy-component is equal to radius pt_radius = np.sqrt(local_point[0]**2 + local_point[1]**2) c0 = cmp_floats(pt_radius, self.radius) #z-component is equal to zero or length c1 = cmp_floats(local_point[2], .0) c2 = cmp_floats(local_point[2], self.length) if c1 or c2: return True elif c0: return True else: return False """ local_point = transform_point(point, tf.inverse_matrix(self.transform)) origin_z = 0. xydistance = np.sqrt(local_point[0]**2 + local_point[1]**2) if intervalcheck(origin_z, local_point[2], self.length) == True: if cmp_floats(xydistance, self.radius) == True: return True if smallerequalto(xydistance,self.radius): if cmp_floats(local_point[2], origin_z) == True or cmp_floats(local_point[2], self.length) == True: return True return False
def contains(self, point): """ Returns True if ray contained by CSGsub, False otherwise """ invtransform = tf.inverse_matrix(self.transform) local_point = transform_point(point, invtransform) bool1 = self.SUBplus.contains(local_point) bool2 = self.SUBminus.contains(local_point) if bool1 == False: return False if bool2 == True: return False else: return True
def on_surface(self, point): """Returns True if the point is on the surface False otherwise. >>> box = Box([1,1,1], [2,2,2]) >>> box.on_surface([2,2,2]) True >>> box = Box([1,1,1], [2,2,2]) >>> box.on_surface([4,4,4]) False >>> box = Box(origin=(0, 0, 1.1000000000000001), extent=np.array([ 1. , 1. , 2.1])) >>> ray = Ray(position=(.5,.5, 2.1), direction=(0,0,1)) >>> box.on_surface(ray.position) True """ if self.contains(point) == True: return False # Get an axis-aligned point... then this is really easy. local_point = transform_point(point, tf.inverse_matrix(self.transform)) # the local point must have at least one common point with the surface definition points def_points = np.concatenate((np.array(self.origin), np.array(self.extent))) bool1 = False bool2 = False bool3 = False boolarray = [bool1, bool2, bool3] for i in range(0,3): if cmp_floats(def_points[i], local_point[i]): for j in range(0,3): if intervalcheck(def_points[j],local_point[j],def_points[j+3]): boolarray[j] = True if cmp_floats(def_points[i+3], local_point[i]): for j in range(0,3): if intervalcheck(def_points[j],local_point[j],def_points[j+3]): boolarray[j] = True if boolarray[0] == boolarray[1] == boolarray[2] == True: return True return False
def on_surface(self, point): """ Returns True if the point is on the outer or inner surface of the CSGsub, and False othewise. """ invtransform = tf.inverse_matrix(self.transform) local_point = transform_point(point, invtransform) bool1 = self.SUBplus.on_surface(local_point) bool2 = self.SUBminus.on_surface(local_point) if bool1 == True and self.SUBminus.contains(local_point) == False: return True if bool2 == True and self.SUBplus.contains(local_point) == True: return True else: return False """ Alternatively:
def contains(self, point): """ Returns True if ray contained by CSGadd, False otherwise """ invtransform = tf.inverse_matrix(self.transform) local_point = transform_point(point, invtransform) bool1 = self.ADDone.contains(local_point) bool2 = self.ADDtwo.contains(local_point) bool3 = self.ADDone.on_surface(local_point) bool4 = self.ADDtwo.on_surface(local_point) if bool1 or bool2: return True if bool3 and bool4: return True return False
def intersection(self, ray): """ Returns the intersection point of the ray with the plane. If no intersection occurs None is returned. >>> ray = Ray(position=[0.5, 0.5, -0.5], direction=[0,0,1]) >>> plane = Plane() >>> plane.intersection(ray) [array([ 0.5, 0.5, 0. ])] >>> ray = Ray(position=[0.5, 0.5, -0.5], direction=[0,0,1]) >>> plane = Plane() >>> plane.transform = tf.translation_matrix([0,0,1]) >>> plane.intersection(ray) [array([ 0.5, 0.5, 1. ])] >>> ray = Ray(position=[0.5, 0.5, -0.5], direction=[0,0,1]) >>> plane = Plane() >>> plane.append_transform(tf.translation_matrix([0,0,1])) >>> plane.append_transform(tf.rotation_matrix(np.pi,[1,0,0])) >>> plane.intersection(ray) [array([ 0.5, 0.5, 1. ])] """ # We need apply the anti-transform of the plane to the ray. This gets the ray in the local frame of the plane. inv_transform = tf.inverse_matrix(self.transform) rpos = transform_point(ray.position, inv_transform) rdir = transform_direction(ray.direction, inv_transform) # Ray is in parallel to the plane -- there is no intersection if rdir[2] == 0.0: return None t = -rpos[2]/rdir[2] # Intersection point is behind the ray if t < 0.0: return None # Convert local frame to world frame point = rpos + t*rdir return [transform_point(point, self.transform)]
def surface_identifier(self, surface_point, assert_on_surface = True): """ Returns a unique identifier for the surface location on the CSGsub. """ invtransform = tf.inverse_matrix(self.transform) local_point = transform_point(surface_point, invtransform) bool1 = self.SUBplus.on_surface(local_point) bool2 = self.SUBminus.on_surface(local_point) assertbool = False if bool1 == True and self.SUBminus.contains(local_point) == False: assertbool = True elif bool2 == True and self.SUBplus.contains(local_point) == True: assertbool = True if assert_on_surface == True: assert assertbool == True if bool1 == True and self.SUBminus.contains(local_point) == False: return self.reference + "_SUBplus_" + self.SUBplus.surface_identifier(local_point) if bool2 == True and self.SUBplus.contains(local_point) == True: return self.reference + "_SUBminus_" + self.SUBminus.surface_identifier(local_point)
def intersection(self, ray): """ Returns the intersection points of ray with CSGadd in global frame """ # We will need the invtransform later when we return the results..." invtransform = tf.inverse_matrix(self.transform) localray = Ray() localray.position = transform_point(ray.position, invtransform) localray.direction = transform_direction(ray.direction, invtransform) ADDone__intersections = self.ADDone.intersection(localray) ADDtwo__intersections = self.ADDtwo.intersection(localray) """ Cover the simpler cases """ if ADDone__intersections == None and ADDtwo__intersections == None: return None """ Change ..._intersections into tuples """ if ADDone__intersections != None: for i in range(0,len(ADDone__intersections)): point = ADDone__intersections[i] new_point = (point[0], point[1], point[2]) ADDone__intersections[i] = new_point if ADDtwo__intersections != None: for i in range(0,len(ADDtwo__intersections)): point = ADDtwo__intersections[i] new_point = (point[0],point[1],point[2]) ADDtwo__intersections[i] = new_point """ Only intersection points NOT containted in resp. other structure relevant """ ADDone_intersections = [] ADDtwo_intersections = [] if ADDone__intersections != None: for i in range(0,len(ADDone__intersections)): if self.ADDtwo.contains(ADDone__intersections[i]) == False: ADDone_intersections.append(ADDone__intersections[i]) if ADDtwo__intersections != None: for j in range(0,len(ADDtwo__intersections)): if self.ADDone.contains(ADDtwo__intersections[j]) == False: ADDtwo_intersections.append(ADDtwo__intersections[j]) """ => Convert to list """ ADDone_set = set(ADDone_intersections[:]) ADDtwo_set = set(ADDtwo_intersections[:]) combined_set = ADDone_set | ADDtwo_set combined_intersections = list(combined_set) """ Just in case... """ if len(combined_intersections) == 0: return None """ Sort by separation from ray origin """ intersection_separations = [] for point in combined_intersections: intersection_separations.append(separation(ray.position, point)) """ Convert into Numpy arrays in order to sort """ intersection_separations = np.array(intersection_separations) sorted_indices = intersection_separations.argsort() sorted_combined_intersections = [] for index in sorted_indices: sorted_combined_intersections.append(np.array(combined_intersections[index])) global_frame_intersections = [] for point in sorted_combined_intersections: global_frame_intersections.append(transform_point(point, self.transform)) global_frame_intersections_cleared = [] for point in global_frame_intersections: if self.on_surface(point) == True: """ This is only necessary if the two objects have an entire surface region in common, for example consider two boxes joined at one face. """ global_frame_intersections_cleared.append(point) if len(global_frame_intersections_cleared) == 0: return None return global_frame_intersections_cleared
def surface_identifier(self, surface_point, assert_on_surface = True): """ Returns an unique identifier that specifies the surface which holds the surface_points. self.on_surface(surface_point) must return True, otherwise an assert error is thrown. Example, for a Box with origin=(X,Y,Z), and size=(L,W,H) has the identifiers: "left":(X,y,z) "right":(X+L,y,z) "near":(x,Y,z) "far":(x,Y+W,z) "bottom":(x,y,H) "top":(x,y,Z+H) """ # Get an axis-aligned point... then this is really easy. local_point = transform_point(surface_point, tf.inverse_matrix(self.transform)) # the local point must have at least one common point with the surface definition points def_points = np.concatenate((np.array(self.origin), np.array(self.extent))) #surface_id[0]=0 => left #surface_id[1]=0 => near #surface_id[2]=0 => bottom #surface_id[3]=0 => right #surface_id[4]=0 => far #surface_id[5]=0 => top #import pdb; pdb.set_trace() surface_id_array = [0,0,0,0,0,0] boolarray = [False, False, False] for i in range(0,3): if cmp_floats(def_points[i], local_point[i]): for j in range(0,3): if intervalcheck(def_points[j],local_point[j],def_points[j+3]): surface_id_array[i] = 1 boolarray[j] = True if cmp_floats(def_points[i+3], local_point[i]): for j in range(0,3): if intervalcheck(def_points[j],local_point[j],def_points[j+3]): surface_id_array[i+3] = 1 boolarray[j] = True if assert_on_surface == True: assert boolarray[0] == boolarray[1] == boolarray[2] == True surface_name = [] if surface_id_array[0] == 1: surface_name.append('left') if surface_id_array[1] == 1: surface_name.append('near') if surface_id_array[2] == 1: surface_name.append('bottom') if surface_id_array[3] == 1: surface_name.append('right') if surface_id_array[4] == 1: surface_name.append('far') if surface_id_array[5] == 1: surface_name.append('top') """ The following helps to specify if the local_point is located on a corner or edge of the box. If that is not desired, simply return surface_name[0]. """ # return surface_name[0] return_id = '' for j in range(len(surface_name)): return_id = return_id + surface_name[j] + '' return return_id
def surface_normal(self, ray, acute=True): """ Returns the normalised vector of which is the acute surface normal (0<~ theta <~ 90) with respect to ray direction. If acute=False is specified the reflex normal is returned (0<~ theta < 360) The ray must be on the surface otherwise an error is raised. >>> box = Box([0,0,0], [1,1,1]) >>> ray = Ray([0.5,0.5,1], [0,0,1]) >>> box.surface_normal(ray) array([ 0., 0., 1.]) >>> box = Box([1,1,1], [2,2,2]) >>> ray = Ray([1.5,1.5,2], [0,0,1]) >>> box.surface_normal(ray) array([ 0., 0., 1.]) """ #pdb.set_trace() assert self.on_surface(ray.position), "The point is not on the surface of the box." invtrans = tf.inverse_matrix(self.transform) rpos = transform_point(ray.position, invtrans) rdir = transform_direction(ray.direction, invtrans) # To define a flat surface, 3 points are needed. common_index = None exit = False reference_point = list(self.origin) for ref in reference_point: if not exit: for val in rpos: #logging.debug(str((ref,val))) if cmp_floats(ref,val): #logging.debug("Common value found, " + str(val) + " at index" + str(list(rpos).index(val))) common_index = list(rpos).index(val) exit = True break exit = False if common_index == None: reference_point = list(self.extent) for ref in reference_point: if not exit: for val in rpos: #logging.debug(str((ref,val))) if cmp_floats(ref,val): #logging.debug("Common value found, " + str(val) + " at index" + str(list(rpos).index(val))) common_index = list(rpos).index(val) exit = True break assert common_index != None, "The intersection point %s doesn't share an element with either the origin %s or extent points %s (all points transformed into local frame)." % (rpos, self.origin, self.extent) normal = np.zeros(3) if list(self.origin) == list(reference_point): normal[common_index] = -1. else: normal[common_index] = 1. if acute: if angle(normal, rdir) > np.pi/2: normal = normal * -1.0 assert 0 <= angle(normal, rdir) <= np.pi/2, "The normal vector needs to be pointing in the same direction quadrant as the ray, so the angle between them is between 0 and 90" # remove signed zeros this just makes the doctest work. Signed zeros shouldn't really effect the maths but makes things neat. for i in range(0,3): if normal[i] == 0.0: normal[i] = 0.0 return transform_direction(normal, self.transform)
def intersection(self, ray): """ Returns the intersection points of ray with CSGint in global frame """ # We will need the invtransform later when we return the results..." invtransform = tf.inverse_matrix(self.transform) localray = Ray() localray.position = transform_point(ray.position, invtransform) localray.direction = transform_direction(ray.direction, invtransform) INTone__intersections = self.INTone.intersection(localray) INTtwo__intersections = self.INTtwo.intersection(localray) """ Cover the simpler cases """ if INTone__intersections == None and INTtwo__intersections == None: return None """ Change ..._intersections into tuples """ if INTone__intersections != None: for i in range(0,len(INTone__intersections)): point = INTone__intersections[i] new_point = (point[0], point[1], point[2]) INTone__intersections[i] = new_point if INTtwo__intersections != None: for i in range(0,len(INTtwo__intersections)): point = INTtwo__intersections[i] new_point = (point[0], point[1], point[2]) INTtwo__intersections[i] = new_point """ Only intersection points contained in resp. other structure relevant """ INTone_intersections = [] INTtwo_intersections = [] if INTone__intersections != None: for i in range(0,len(INTone__intersections)): if self.INTtwo.contains(INTone__intersections[i]) == True: INTone_intersections.append(INTone__intersections[i]) if INTtwo__intersections != None: for j in range(0,len(INTtwo__intersections)): if self.INTone.contains(INTtwo__intersections[j]) == True: INTtwo_intersections.append(INTtwo__intersections[j]) """ => Convert to list """ INTone_set = set(INTone_intersections[:]) INTtwo_set = set(INTtwo_intersections[:]) combined_set = INTone_set | INTtwo_set combined_intersections = list(combined_set) """ Just in case... """ if len(combined_intersections) == 0: return None """ Sort by separation from ray origin """ intersection_separations = [] for point in combined_intersections: intersection_separations.append(separation(ray.position, point)) """ Convert into Numpy arrays in order to sort """ intersection_separations = np.array(intersection_separations) sorted_indices = intersection_separations.argsort() sorted_combined_intersections = [] for index in sorted_indices: sorted_combined_intersections.append(np.array(combined_intersections[index])) global_frame_intersections = [] for point in sorted_combined_intersections: global_frame_intersections.append(transform_point(point, self.transform)) return global_frame_intersections
def intersection(self, ray): '''Returns an array intersection points with the ray and box. If no intersection occurs this function returns None. # Inside-out single intersection >>> ray = Ray(position=[0.5,0.5,0.5], direction=[0,0,1]) >>> box = Box() >>> box.intersection(ray) [array([ 0.5, 0.5, 1. ])] # Inside-out single intersection with translation >>> ray = Ray(position=[0.5,0.5,0.5], direction=[0,0,1]) >>> box = Box() >>> box.transform = tf.translation_matrix([0,0,1]) >>> box.intersection(ray) [array([ 0.5, 0.5, 1. ]), array([ 0.5, 0.5, 2. ])] >>> ray = Ray(position=[0.5,0.5,0.5], direction=[0,0,1]) >>> box = Box() >>> box.append_transform(tf.rotation_matrix(2*np.pi, [0,0,1])) >>> box.intersection(ray) [array([ 0.5, 0.5, 1. ])] >>> ray = Ray(position=[0.5,0.5,0.5], direction=[0,0,1]) >>> box = Box() >>> box.append_transform(tf.rotation_matrix(2*np.pi, norm([1,1,0]))) >>> box.append_transform(tf.translation_matrix([0,0,1])) >>> box.intersection(ray) [array([ 0.5, 0.5, 1. ]), array([ 0.5, 0.5, 2. ])] Here I am using the the work of Amy Williams, Steve Barrus, R. Keith Morley, and Peter Shirley, "An Efficient and Robust Ray-Box Intersection Algorithm" Journal of graphics tools, 10(1):49-54, 2005''' invtrans = tf.inverse_matrix(self.transform) rpos = transform_point(ray.position, invtrans) rdir = transform_direction(ray.direction, invtrans) #pts = [transform_point(self.points[0], self.transform), transform_point(self.points[1], self.transform)] pts = [np.array(self.points[0]), np.array(self.points[1])] rinvd = [1.0/rdir[0], 1.0/rdir[1], 1.0/rdir[2]] rsgn = [1.0/rinvd[0] < 0.0, 1.0/rinvd[1] < 0.0, 1.0/rinvd[2] < 0.0] tmin = (pts[rsgn[0]][0] - rpos[0]) * rinvd[0] tmax = (pts[1-rsgn[0]][0] - rpos[0]) * rinvd[0] tymin = (pts[rsgn[1]][1] - rpos[1]) * rinvd[1] tymax = (pts[1-rsgn[1]][1] - rpos[1]) * rinvd[1] #Bug here: this is the exit point with bug1.py if (tmin > tymax) or (tymin > tmax): return None if tymin > tmin: tmin = tymin if tymax < tmax: tmax = tymax tzmin = (pts[rsgn[2]][2] - rpos[2]) * rinvd[2] tzmax = (pts[1-rsgn[2]][2] - rpos[2]) * rinvd[2] if (tmin > tzmax) or (tzmin > tmax): return None if tzmin > tmin: tmin = tzmin if tzmax < tmax: tmax = tzmax # Calculate the hit coordinates then if the solution is in the forward direction append to the hit list. hit_coordinates = [] pt1 = rpos + tmin * rdir pt2 = rpos + tmax * rdir #pt1_sign = np.dot(pt1, rdir) #pt2_sign = np.dot(pt2, rdir) #print "tmin", tmin, "tmax", tmax if tmin >= 0.0: hit_coordinates.append(pt1) if tmax >= 0.0: hit_coordinates.append(pt2) #print hit_coordinates if len(hit_coordinates) == 0: return None # Convert hit coordinate back to the world frame hit_coords_world = [] for point in hit_coordinates: hit_coords_world.append(transform_point(point, self.transform)) return hit_coords_world
def intersection(self, ray): """ Returns all forward intersection points with ray and the capped cylinder. The intersection algoithm is taken from, "Intersecting a Ray with a Cylinder" Joseph M. Cychosz and Warren N. Waggenspack, Jr., in "Graphics Gems IV", Academic Press, 1994. >>> cld = Cylinder(1.0, 1.0) >>> cld.intersection(Ray([0.0,0.0,0.5], [1,0,0])) [array([ 1. , 0. , 0.5])] >>> cld.intersection(Ray([-5,0.0,0.5], [1,0,0])) [array([-1. , 0. , 0.5]), array([ 1. , 0. , 0.5])] >>> cld.intersection(Ray([.5,.5,-1], [0,0,1])) [array([ 0.5, 0.5, 1. ]), array([ 0.5, 0.5, 0. ])] >>> cld.intersection( Ray([0.0,0.0,2.0], [0,0,-1])) [array([ 0., 0., 1.]), array([ 0., 0., 0.])] >>> cld.intersection(Ray([-0.2, 1.2,0.5],[0.75498586, -0.53837322, 0.37436697])) [array([ 0.08561878, 0.99632797, 0.64162681]), array([ 0.80834999, 0.48095523, 1. ])] >>> cld.intersection(Ray(position=[ 0.65993112596983427575736414, -0.036309587083015459896273569, 1. ], direction=[ 0.24273873128664008591570678, -0.81399482405912471083553328, 0.52772183462341881732271531])) [array([ 0.65993113, -0.03630959, 1. ])] >>> cld.transform = tf.translation_matrix([0,0,1]) >>> cld.intersection(Ray([-5,0.0,1.5], [1,0,0])) [array([-1. , 0. , 1.5]), array([ 1. , 0. , 1.5])] >>> cld.transform = tf.identity_matrix() >>> cld.transform = tf.rotation_matrix(0.25*np.pi, [1,0,0]) >>> cld.intersection(Ray([-5,-.5,-0.25], [1,0,0])) [array([-0.84779125, -0.5 , -0.25 ]), array([ 0.84779125, -0.5 , -0.25 ])] """ # Inverse transform the ray to get it into the cylinders local frame inv_transform = tf.inverse_matrix(self.transform) rpos = transform_point(ray.position, inv_transform) rdir = transform_direction(ray.direction, inv_transform) direction = np.array([0,0,1]) normal = np.cross(rdir, direction) normal_magnitude = magnitude(normal) #print normal_magnitude, "Normal magnitude" if cmp_floats(normal_magnitude, .0): # Ray parallel to cylinder direction normal = norm(normal) #d = abs(np.dot(rpos, direction)) #D = rpos - d * np.array(direction) #if magnitude(D) <= self.radius: # Axis aligned ray inside the cylinder volume only hits caps #print "Inside axis aligned ray only hits caps" bottom = Plane() top = Plane() top.transform = tf.translation_matrix([0,0,self.length]) p0 = top.intersection(Ray(rpos, rdir)) p1 = bottom.intersection(Ray(rpos, rdir)) cap_intersections = [] if p0 != None: cap_intersections.append(p0) if p1 != None: cap_intersections.append(p1) points = [] for point in cap_intersections: if point[0] != None: point = point[0] point_radius = np.sqrt(point[0]**2 + point[1]**2) if point_radius <= self.radius: #print "Hit cap at point:" #print point #print "" points.append(point) if len(points) > 0: world_points = [] for pt in points: world_points.append(transform_point(pt, self.transform)) #print "Local points", points #print "World points", world_points return world_points return None # finish axis parallel branch #print "Not parallel to cylinder axis." #print "" normal = norm(normal) d = abs(np.dot(rpos, normal)) if d <= self.radius: #Hit quadratic surface O = np.cross(rpos, direction) t = - np.dot(O,normal) / normal_magnitude O = np.cross(normal, direction) O = norm(O) s = abs(np.sqrt(self.radius**2 - d**2) / np.dot(rdir, O)) t0 = t - s p0 = rpos + t0 * rdir t1 = t + s p1 = rpos + t1 * rdir points = [] if (t0 >= 0.0) and (.0 <= p0[2] <= self.length): points.append(p0) if (t1 >= 0.0) and (.0 <= p1[2] <= self.length): points.append(p1) #print "Hits quadratic surface with t0 and t1, ", t0, t1 #print "" #print "Intersection points:" #p0 = rpos + t0 * rdir #p1 = rpos + t1 * rdir # Check that hit quadratic surface in the length range #points = [] #if (.0 <= p0[2] <= self.length) and not Ray(rpos, rdir).behind(p0): # points.append(p0) # #if (.0 <= p1[2] <= self.length) and not Ray(rpos, rdir).behind(p1): # points.append(p1) #print points #Now compute intersection with end caps #print "Now to calculate caps intersections" bottom = Plane() top = Plane() top.transform = tf.translation_matrix([0,0,self.length]) p2 = top.intersection(Ray(rpos, rdir)) p3 = bottom.intersection(Ray(rpos, rdir)) cap_intersections = [] if p2 != None: cap_intersections.append(p2) if p3 != None: cap_intersections.append(p3) for point in cap_intersections: if point[0] != None: point = point[0] point_radius = np.sqrt(point[0]**2 + point[1]**2) if point_radius <= self.radius: #print "Hit cap at point:" #print point #print "" points.append(point) #print points if len(points) > 0: world_points = [] for pt in points: world_points.append(transform_point(pt, self.transform)) return world_points return None
def intersection(self, ray): """ Returns the intersection points of ray with CSGsub in global frame """ # We will need the invtransform later when we return the results..." invtransform = tf.inverse_matrix(self.transform) localray = Ray() localray.position = transform_point(ray.position, invtransform) localray.direction = transform_direction(ray.direction, invtransform) SUBplus__intersections = self.SUBplus.intersection(localray) SUBminus__intersections = self.SUBminus.intersection(localray) """ Cover the simpler cases """ if SUBplus__intersections == None and SUBminus__intersections == None: return None """ Change ..._intersections into tuples """ if SUBplus__intersections != None: for i in range(0,len(SUBplus__intersections)): point = SUBplus__intersections[i] new_point = (point[0], point[1], point[2]) SUBplus__intersections[i] = new_point if SUBminus__intersections != None: for i in range(0,len(SUBminus__intersections)): point = SUBminus__intersections[i] new_point = (point[0], point[1], point[2]) SUBminus__intersections[i] = new_point """ Valid intersection points: SUBplus intersections must lie outside SUBminus SUBminus intersections must lie inside SUBplus """ SUBplus_intersections = [] SUBminus_intersections = [] if SUBplus__intersections != None: for intersection in SUBplus__intersections: if not self.SUBminus.contains(intersection): SUBplus_intersections.append(intersection) if SUBminus__intersections != None: for intersection in SUBminus__intersections: if self.SUBplus.contains(intersection): SUBminus_intersections.append(intersection) # SUBplus_set = set(SUBplus_intersections[:]) # SUBminus_set = set(SUBminus_intersections[:]) # combined_set = SUBplus_set ^ SUBminus_set # combined_intersections = list(combined_set) combined_intersections = np.array(list(set(SUBplus_intersections+SUBminus_intersections))) # intersection_separations = combined_intersections[0]**2+combined_intersections[1]**2+combined_intersections[2]**2 """ Just in case... """ if len(combined_intersections) == 0: return None transposed_intersections = combined_intersections.transpose() intersection_vectors = transposed_intersections[0]-ray.position[0], transposed_intersections[1]-ray.position[1], transposed_intersections[2]-ray.position[2] # intersection_separations= [] # print combined_intersections, point, intersection_vectors intersection_separations = intersection_vectors[0]**2+intersection_vectors[1]**2+intersection_vectors[2]**2 # for point in combined_intersections: # intersection_separations.append(separation(ray.position, point)) # for i in range(len(intersection_separations)): # print intersection_separations[i], intersection_separations2[i] """ Sort by distance from ray origin => Use Numpy arrays """ # intersection_separations = np.array(intersection_separations) sorted_combined_intersections = combined_intersections[intersection_separations.argsort()] # sorted_combined_intersections = [] # for index in sorted_indices: # sorted_combined_intersections.append(np.array(combined_intersections[index])) # global_frame_intersections = [] # for point in sorted_combined_intersections: # global_frame_intersections.append(transform_point(point, self.transform)) global_frame_intersections = [transform_point(point, self.transform) for point in sorted_combined_intersections] return global_frame_intersections