def surface_identifier(self, surface_point, assert_on_surface=True):
"""
Assert surface_point on cylinder surface
:param surface_point: Point to be checked on surface
:param assert_on_surface: Assert point on surface (die if false)
:return: name of surface
"""
local_point = transform_point(surface_point, tf.inverse_matrix(self.transform))
origin_z = 0.
xy_distance = np.sqrt(local_point[0] ** 2 + local_point[1] ** 2)
assert_bool = False
surface_name = None
if interval_check(origin_z, local_point[2], self.length):
if cmp_floats(xy_distance, self.radius):
surface_name = 'hull'
assert_bool = True
if smaller_equal_to(xy_distance, self.radius):
if cmp_floats(local_point[2], origin_z):
surface_name = 'base'
assert_bool = True
if cmp_floats(local_point[2], self.length):
surface_name = 'cap'
assert_bool = True
if assert_on_surface:
assert assert_bool, "The point "+str(local_point)+" is on the cylinder surface"
return surface_name
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):
"""
Return the surface normal for a ray arriving on the CSGsub surface.
"""
inv_transform = tf.inverse_matrix(self.transform)
local_ray = Ray()
local_ray.position = transform_point(ray.position, inv_transform)
local_ray.direction = transform_direction(ray.direction, inv_transform)
bool1 = self.SUBplus.on_surface(local_ray.position)
bool2 = self.SUBminus.on_surface(local_ray.position)
assert_bool = False
if bool1 is True and self.SUBminus.contains(local_ray.position) == False:
assert_bool = True
if bool2 is True and self.SUBplus.contains(local_ray.position) == True:
assert_bool = True
assert assert_bool is True
if bool1 is True and self.SUBminus.contains(local_ray.position) == False:
return self.SUBplus.surface_normal(ray, acute)
if bool2 is True and self.SUBplus.contains(local_ray.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 or False dependent on whether point on CSGadd surface or not
"""
if self.contains(point):
return False
inv_transform = tf.inverse_matrix(self.transform)
local_point = transform_point(point, inv_transform)
bool1 = self.ADDone.on_surface(local_point)
bool2 = self.ADDtwo.on_surface(local_point)
if bool1 is True and self.ADDtwo.contains(local_point) == False:
return True
if bool2 is True and self.ADDone.contains(local_point) == False:
return True
if bool1 == bool2 is True:
return True
else:
return False
def surface_normal(self, ray, acute=True):
"""
Returns surface normal in point where ray hits CSGint surface
"""
"""
Ensure surface_point on CSGint surface
"""
inv_transform = tf.inverse_matrix(self.transform)
local_ray = Ray()
local_ray.position = transform_point(ray.position, inv_transform)
local_ray.direction = transform_direction(ray.direction, inv_transform)
bool1 = self.INTone.on_surface(local_ray.position)
bool2 = self.INTtwo.on_surface(local_ray.position)
assert_bool = False
if bool1 is True and self.INTtwo.contains(local_ray.position) == True:
assert_bool = True
if bool2 is True and self.INTone.contains(local_ray.position) == True:
assert_bool = True
if bool1 is bool2 is True:
assert_bool = True
assert assert_bool is True
if bool1:
return self.INTone.surface_normal(ray, acute)
else:
return self.INTtwo.surface_normal(ray, acute)
def contains(self, point):
"""
Returns true if point is within cylinder (if on surface then false)
:param point: Point to be evaluated
:return: Boolean
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):
return False
local_point = transform_point(point, tf.inverse_matrix(self.transform))
origin_z = 0.
xy_distance = np.sqrt(local_point[0] ** 2 + local_point[1] ** 2)
if interval_check(origin_z, local_point[2], self.length, strict=True) and xy_distance < self.radius:
return True
else:
return False
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 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 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 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
"""
inv_transform = tf.inverse_matrix(self.transform)
local_point = transform_point(surface_point, inv_transform)
bool1 = self.INTone.on_surface(local_point)
bool2 = self.INTtwo.on_surface(local_point)
assert_bool = False
if bool1 is True and self.INTtwo.contains(local_point) == True:
assert_bool = True
if bool2 is True and self.INTone.contains(local_point) == True:
assert_bool = True
if bool1 == bool2 is True:
assert_bool = True
if assert_on_surface:
assert assert_bool is True
if bool1:
return self.reference + "_INTone_" + self.INTone.surface_identifier(local_point)
if bool2:
return self.reference + "_INTtwo_" + self.INTtwo.surface_identifier(local_point)
def on_surface(self, point):
"""
Returns True is the point is on the plane's surface and false otherwise.
:param point: 3D coordinates of the point to be checked for position on surface
:return: boolean
"""
inv_transform = tf.inverse_matrix(self.transform)
rpos = transform_point(point, inv_transform)
if cmp_floats(rpos, 0.):
return True
return False
def on_surface(self, point):
"""
Returns True if the point is on the plane's surface and false otherwise.
:param point: 3D coordinates of the point to be evaluated
:return: boolean
"""
inv_transform = tf.inverse_matrix(self.transform)
position = transform_point(point, inv_transform)
if cmp_floats(position[2], 0.) and (0. < position[0] <= self.length) and (0. < position[1] <= self.width):
return True
return False
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 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 is True:
return True
else:
return False
def on_surface(self, point):
"""
Checks whether the point is on cylinder's surface
:param point the point (3D coord) to be evaluated as on_surface()
"""
"""
>>> # 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.
xy_distance = np.sqrt(local_point[0] ** 2 + local_point[1] ** 2)
if interval_check(origin_z, local_point[2], self.length):
if cmp_floats(xy_distance, self.radius):
return True
if smaller_equal_to(xy_distance, self.radius):
if cmp_floats(local_point[2], origin_z) or cmp_floats(local_point[2], self.length) == True:
return True
return False
def on_surface(self, point):
"""
Returns True if the point is on the surface False otherwise.
:param point: 3D coordinates of the point to be evaluated
:return: boolean
"""
"""
>>> 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):
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)))
bool_array = [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 interval_check(def_points[j], local_point[j], def_points[j + 3]):
bool_array[j] = True
if cmp_floats(def_points[i + 3], local_point[i]):
for j in range(0, 3):
if interval_check(def_points[j], local_point[j], def_points[j + 3]):
bool_array[j] = True
if bool_array[0] == bool_array[1] == bool_array[2] is 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 intersection(self, ray):
"""
Returns the intersection point of the ray with the plane. If no intersection occurs None is returned.
:param ray: The ray to be evaluated for intersection
:return: boolean
"""
"""
>>> 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)
ray_pos = transform_point(ray.position, inv_transform)
ray_dir = transform_direction(ray.direction, inv_transform)
# Ray is in parallel to the plane -- there is no intersection
if ray_dir[2] == 0.0:
return None
t = -ray_pos[2] / ray_dir[2]
# Intersection point is behind the ray
if t < 0.0:
return None
# Convert local frame to world frame
point = ray_pos + t * ray_dir
return [transform_point(point, self.transform)]
def contains(self, point):
"""
Returns True if ray contained by CSGsub, False otherwise
"""
inv_transform = tf.inverse_matrix(self.transform)
local_point = transform_point(point, inv_transform)
bool1 = self.SUBplus.contains(local_point)
bool2 = self.SUBminus.contains(local_point)
if not bool1:
return False
if bool2:
return False
else:
return True
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 contains(self, point):
"""
Returns True if ray contained by CSGadd, False otherwise
"""
inv_transform = tf.inverse_matrix(self.transform)
local_point = transform_point(point, inv_transform)
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 on_surface(self, point):
"""
Returns True if the point is on the outer or inner surface of the CSGsub, and False othewise.
"""
inv_transform = tf.inverse_matrix(self.transform)
local_point = transform_point(point, inv_transform)
bool1 = self.SUBplus.on_surface(local_point)
bool2 = self.SUBminus.on_surface(local_point)
if bool1 is True and self.SUBminus.contains(local_point) == False:
return True
if bool2 is 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 surface_identifier(self, surface_point, assert_on_surface=True):
"""
Returns a unique identifier for the surface location on the CSGsub.
"""
inv_transform = tf.inverse_matrix(self.transform)
local_point = transform_point(surface_point, inv_transform)
bool1 = self.SUBplus.on_surface(local_point)
bool2 = self.SUBminus.on_surface(local_point)
assert_bool = False
if bool1 is True and self.SUBminus.contains(local_point) == False:
assert_bool = True
elif bool2 is True and self.SUBplus.contains(local_point) == True:
assert_bool = True
if assert_on_surface:
assert assert_bool is True
if bool1 is True and self.SUBminus.contains(local_point) == False:
return self.reference + "_SUBplus_" + self.SUBplus.surface_identifier(local_point)
if bool2 is True and self.SUBplus.contains(local_point) == True:
return self.reference + "_SUBminus_" + self.SUBminus.surface_identifier(local_point)
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 is not None:
cap_intersections.append(p0)
if p1 is not None:
cap_intersections.append(p1)
points = []
for point in cap_intersections:
if point[0] is not 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 is not None:
cap_intersections.append(p2)
if p3 is not None:
cap_intersections.append(p3)
for point in cap_intersections:
if point[0] is not 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 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 is None and INTtwo__intersections is 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 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
""" Alternative implementation:
local_point = transform_point(point, tf.inverse_matrix(self.transform))
def_points = np.concatenate((np.array(self.origin), np.array(self.extent)))
contain_bool = True
for i in range(0,3):
if interval_check_strict(def_points[i],local_point[i],def_points[i+3]) == False:
contain_bool = False
return contain_bool
"""
return True
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.
:param ray: Ray (position, direction)
:param acute: Boolean weather acute angle
:return: bool
"""
"""
>>> 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.\n" \
"This can be caused by two object sharing a face" \
"(Position: " + str(ray.position) + ")"
inv_trans = tf.inverse_matrix(self.transform)
ray_pos = transform_point(ray.position, inv_trans)
ray_dir = transform_direction(ray.direction, inv_trans)
# To define a flat surface, 3 points are needed.
common_index = None
exit_loop = False
reference_point = list(self.origin)
for ref in reference_point:
if not exit_loop:
for val in ray_pos:
# logging.debug(str((ref,val)))
if cmp_floats(ref, val):
# logging.debug("Common value found, " + str(val) + " at index" + str(list(ray_pos).index(val)))
common_index = list(ray_pos).index(val)
exit_loop = True
break
exit_loop = False
if common_index is None:
reference_point = list(self.extent)
for ref in reference_point:
if not exit_loop:
for val in ray_pos:
# logging.debug(str((ref,val)))
if cmp_floats(ref, val):
# logging.debug("Common value found, " + str(val) + " at " + str(list(ray_pos).index(val)))
common_index = list(ray_pos).index(val)
exit_loop = True
break
assert common_index is not 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)."\
% (ray_pos, 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, ray_dir) > np.pi / 2:
normal *= -1.0
assert 0 <= angle(normal, ray_dir) <= 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 surface_identifier(self, surface_point, assert_on_surface=True):
"""
Returns an unique identifier that specifies the box surface which holds the surface_points.
:param surface_point: Point to be checked on surface
:param assert_on_surface: Assert point on surface (die if false)
:return: name of surface
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]
bool_array = [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 interval_check(def_points[j], local_point[j], def_points[j + 3]):
surface_id_array[i] = 1
bool_array[j] = True
if cmp_floats(def_points[i + 3], local_point[i]):
for j in range(0, 3):
if interval_check(def_points[j], local_point[j], def_points[j + 3]):
surface_id_array[i + 3] = 1
bool_array[j] = True
if assert_on_surface:
assert bool_array[0] == bool_array[1] == bool_array[2] is 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 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"""
inv_trans = tf.inverse_matrix(self.transform)
ray_pos = transform_point(ray.position, inv_trans)
ray_dir = transform_direction(ray.direction, inv_trans)
# 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])]
# When ray direction components are 0 (e.g. [0,0,1]) then the inverse (1/ray_dir[x/y])
# might result in division by zero.
# This results into infinity and it's apparently handled correctly, so just ignore the error HERE
np.seterr(divide='ignore')
ray_inv_dir = [1.0 / ray_dir[0], 1.0 / ray_dir[1], 1.0 / ray_dir[2]]
ray_sign = [1.0 / ray_inv_dir[0] < 0.0, 1.0 / ray_inv_dir[1] < 0.0, 1.0 / ray_inv_dir[2] < 0.0]
tmin = (pts[ray_sign[0]][0] - ray_pos[0]) * ray_inv_dir[0]
tmax = (pts[1 - ray_sign[0]][0] - ray_pos[0]) * ray_inv_dir[0]
tymin = (pts[ray_sign[1]][1] - ray_pos[1]) * ray_inv_dir[1]
tymax = (pts[1 - ray_sign[1]][1] - ray_pos[1]) * ray_inv_dir[1]
np.seterr(divide='warn')
# 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[ray_sign[2]][2] - ray_pos[2]) * ray_inv_dir[2]
tzmax = (pts[1 - ray_sign[2]][2] - ray_pos[2]) * ray_inv_dir[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 = ray_pos + tmin * ray_dir
pt2 = ray_pos + tmax * ray_dir
# pt1_sign = np.dot(pt1, ray_dir)
# pt2_sign = np.dot(pt2, ray_dir)
# 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 the intersection points of ray with CSGsub in global frame
"""
# We will need the invtransform later when we return the results..."
inv_transform = tf.inverse_matrix(self.transform)
local_ray = Ray()
local_ray.position = transform_point(ray.position, inv_transform)
local_ray.direction = transform_direction(ray.direction, inv_transform)
SUBplus__intersections = self.SUBplus.intersection(local_ray)
SUBminus__intersections = self.SUBminus.intersection(local_ray)
"""
Cover the simpler cases
"""
if SUBplus__intersections is None and SUBminus__intersections is None:
return None
"""
Change ..._intersections into tuples
"""
if SUBplus__intersections is not 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 is not 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 is not None:
for intersection in SUBplus__intersections:
if not self.SUBminus.contains(intersection):
SUBplus_intersections.append(intersection)
if SUBminus__intersections is not 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
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 is None and SUBminus__intersections is 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
def intersection(self, ray):
"""
Returns the intersection points of ray with CSGint in global frame
"""
# We will need the inv_transform later when we return the results..."
inv_transform = tf.inverse_matrix(self.transform)
local_ray = Ray()
local_ray.position = transform_point(ray.position, inv_transform)
local_ray.direction = transform_direction(ray.direction, inv_transform)
INTone__intersections = self.INTone.intersection(local_ray)
INTtwo__intersections = self.INTtwo.intersection(local_ray)
"""
Cover the simpler cases
"""
if INTone__intersections is None and INTtwo__intersections is None:
return None
"""
Change ..._intersections into tuples
"""
if INTone__intersections is not 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 is not 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 is not None:
for i in range(0, len(INTone__intersections)):
if self.INTtwo.contains(INTone__intersections[i]):
INTone_intersections.append(INTone__intersections[i])
if INTtwo__intersections is not None:
for j in range(0, len(INTtwo__intersections)):
if self.INTone.contains(INTtwo__intersections[j]):
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 the intersection points of ray with CSGadd in global frame
"""
# We will need the inv_transform later when we return the results..."
inv_transform = tf.inverse_matrix(self.transform)
local_ray = Ray()
local_ray.position = transform_point(ray.position, inv_transform)
local_ray.direction = transform_direction(ray.direction, inv_transform)
ADDone__intersections = self.ADDone.intersection(local_ray)
ADDtwo__intersections = self.ADDtwo.intersection(local_ray)
"""
Cover the simpler cases
"""
if ADDone__intersections is None and ADDtwo__intersections is None:
return None
"""
Change ..._intersections into tuples
"""
if ADDone__intersections is not 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 is not 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 contained in resp. other structure relevant
"""
ADDone_intersections = []
ADDtwo_intersections = []
if ADDone__intersections is not None:
for i in range(0, len(ADDone__intersections)):
if not self.ADDtwo.contains(ADDone__intersections[i]):
ADDone_intersections.append(ADDone__intersections[i])
if ADDtwo__intersections is not None:
for j in range(0, len(ADDtwo__intersections)):
if not self.ADDone.contains(ADDtwo__intersections[j]):
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):
"""
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 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 is None and ADDtwo__intersections is 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_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.
:param ray: Ray (position, direction)
:param acute: Boolean weather acute angle
:return: bool
"""
"""
>>> 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 *= -1
return transform_direction(normal, self.transform)
