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
