def place_finger(obj,d,z,normal,distal,hand_normal):
"""
Function to generate Geometry3D.ConvexPolygon definitions of fingers (including finger geometry and pose)
Parameters
----------
obj: dict
Dictionary including information about each surface on the object in the following form:
Geometry3D ConvexPolygon , Geometry3D Vector , Geometry3D ConvexPolygon
{surface_no:(surface_polygon_definition, surface_normal_vector, goal_region_polygon(if available))}
d: float
Parameter d for given finger
z: float
Elevation z for given finger
normal: Geometry3D.Vector
Normal vector pointing out of the finger (backhand)
distal: Geometry3D.Vector
Distal vector pointing out of the finger (from palm to fingertip)
hand_normal: Geometry3D.Vector
Vector normal to the manipulation plane of the hand
Returns
----------
finger: Geometry3D.ConvexPolygon
Finger geometry defined as a convex polygon - including pose information (position and orientation)
"""
# Loop through the object surfaces to find the one that has the same (similar) normal vector with the finger
for surf in obj:
if obj[surf][1].angle(normal)<1e-3:
# Contact surface found
surface = obj[surf][0]
break
# Find object length as the distance between two corners of the surface in the distal direction
for point in surface.points:
for other_point in surface.points:
if point==other_point:
continue
else:
edge = Vector(point,other_point)
angle = edge.angle(distal)
if angle <1e-3:
obj_length = edge.length()
# Find finger center by translating the surface center along z and distal direction using given finger parameters
finger_center = translate_point(surface.center_point,(hand_normal*z - distal*(d+obj_length/2-finger_w/2)))
# Find corner 1-4 of the finger by translating the finger center according to given finger parameters
finger_p1 = translate_point(finger_center,(distal*(finger_w/2)+distal.cross(normal)*(finger_h/2)))
finger_p2 = translate_point(finger_center,(-distal*(finger_w/2)+distal.cross(normal)*(finger_h/2)))
finger_p3 = translate_point(finger_center,(-distal*(finger_w/2)-distal.cross(normal)*(finger_h/2)))
finger_p4 = translate_point(finger_center,(distal*(finger_w/2)-distal.cross(normal)*(finger_h/2)))
# Define Geometry3D.ConvexPolygon for the finger using finger corners
finger = ConvexPolygon((finger_p1,finger_p2,finger_p3,finger_p4))
return finger
def test_vector_cross_product(self):
a = Vector(2, 3, 5)
b = Vector(7, 11, 13)
self.assertEqual(
a.cross(b),
Vector(
3 * 13 - 5 * 11,
5 * 7 - 2 * 13,
2 * 11 - 3 * 7,
),
)
def primary_halfline(self, x, y):
"""
**Input:**
- x: int of the index of the array.
- y: int of the index of the array.
**Output:**
- Geometry3D.HalfLine of the primary ray of the (x,y) pixel.
**Illustration:**
- The figure is given in the root folder/camera.png
"""
width = self.resolution[1]
height = self.resolution[0]
x_step_vec = -self.x_vec * (
1 / width) # be careful here, don't confuse the x and y
y_step_vec = self.y_vec * (
1 / height) # be careful here, don't confuse the x and y
p = copy.deepcopy(self.main_point).move(
x_step_vec * (-width / 2 + x + 0.5)).move(y_step_vec *
(-height / 2 + y + 0.5))
return HalfLine(p, Vector(self.focus_point, p))
def pivot_finger(angle,poly,normal,center,distal):
"""
Given a finger polygon, corresponding vectors, pivoting angle and a center generate the pivoted finger polygon
Parameters
----------
angle: float
Pivoting angle
poly: Geometry3D.ConvexPolygon
Finger polygon to be pivoted
normal: Geometry3D.Vector
Normal vector of the finger
center: Geometry3D.Point
Pivoting center
distal: Geometry3D.Vector
Distal vector of the finger
Returns
----------
(nsurf, normal, ndistal): tuple
Transformed finger polygon (Geometry3D.ConvexPolygon), finger normal remains the same (Geometry3D.Vector), transformed distal vector (Geometry3D.Vector)
"""
# Compute the transformation
A,B,C = normal
L = np.sqrt(A**2 + B**2 + C**2)
V = np.sqrt(B**2 + C**2)
D = np.array([[1,0,0,-center.x],[0,1,0,-center.y],[0,0,1,-center.z],[0,0,0,1]])
if V == 0:
R_x = np.eye(4)
else:
R_x = np.array([[1,0,0,0],[0,C/V,-B/V,0],[0,B/V,C/V,0],[0,0,0,1]])
if L == 0:
R_y = np.eye(4)
else:
R_y = np.array([[V/L,0,-A/L,0],[0,1,0,0],[A/L,0,V/L,0],[0,0,0,1]])
R_z = np.array([[np.cos(angle),-np.sin(angle),0,0],
[np.sin(angle),np.cos(angle),0,0],
[0,0,1,0],[0,0,0,1]])
T = np.linalg.inv(D)@np.linalg.inv(R_x)@np.linalg.inv(R_y)@R_z@R_y@R_x@D
# Apply transformation
P_init = np.empty((4,0))
for point in poly.points:
point_vec = np.array([point.x,point.y,point.z,1]).reshape(4,1)
P_init = np.concatenate([P_init,point_vec],axis=1)
distal_vec = np.array([poly.points[-1].x+distal[0],poly.points[-1].y+distal[1],poly.points[-1].z+distal[2],1]).reshape(4,1)
P_init = np.concatenate([P_init,distal_vec],axis=1)
P_final = T@P_init
new_points = list()
for i in range(P_final.shape[1]-1):
new_points.append(gPoint(np.round(P_final[:3,i],decimals=3)))
ndistal = Vector(np.round(P_final[:3,-1]-P_final[:3,-2],decimals=3))
nsurf = ConvexPolygon((new_points))
return (nsurf,normal,ndistal)
def rotate_vector(vector,axis,angle):
"""
Function to rotate a given vector around the given axis by a given angle
Parameters
----------
vector: Geometry3D.Vector
Vector to be rotated
axis: Geometry3D.Vector
Rotation axis
angle: float
Rotation angle
Returns
----------
Geometry3D.Vector
Rotated vector
"""
# Compute the required transformation (rotation) matrix
A,B,C = axis
L = np.sqrt(A**2 + B**2 + C**2)
V = np.sqrt(B**2 + C**2)
if V == 0:
R_x = np.eye(4)
else:
R_x = np.array([[1,0,0,0],[0,C/V,-B/V,0],[0,B/V,C/V,0],[0,0,0,1]])
if L == 0:
R_y = np.eye(4)
else:
R_y = np.array([[V/L,0,-A/L,0],[0,1,0,0],[A/L,0,V/L,0],[0,0,0,1]])
R_z = np.array([[np.cos(angle),-np.sin(angle),0,0],
[np.sin(angle),np.cos(angle),0,0],
[0,0,1,0],[0,0,0,1]])
T = np.linalg.inv(R_x)@np.linalg.inv(R_y)@R_z@R_y@R_x
# Apply the transformation
C_init = np.array([vector[0],vector[1],vector[2],1]).reshape(4,1)
C_final = T@C_init
for i in range(C_final.shape[1]):
new_points = np.round(C_final[:3,i],decimals=3)
return Vector(new_points)
def find_contact_center(finger_poly,normal,distal,surface,d,z,obj_l):
"""
Given the finger polygon, corresponding vectors, finger and object parameters, determines the pivoting center for the finger
Parameters
----------
finger_poly: Geometry3D.ConvexPolygon
Finger polygon
normal: Geometry3D.Vector
Finger normal
distal: Geometry3D.Vector
Finger distal vector
surface: Geometry3D.ConvexPolygon
Polygon corresponding to the contact surface
d: float
Finger parameter d
z: float
Finger parameter z
obj_l: float
Object length along distal direction
Returns
----------
center: Geometry3D.Point
Pivoting center
"""
# Check if the object exceeds the tip of the finger
if finger_w - d - obj_l > 0:
# if not no translation is necessary along distal
v1 = distal*0
else:
v1 = (-(finger_w-d)/2 + (obj_l/2))*distal
# Determine the translation along the hand normal
v_temp = Vector(surface.center_point,finger_poly.center_point)
hand_normal = distal.cross(normal)
if v_temp.length() == 0:
b = 0
else:
ang = v_temp.angle(hand_normal)
if ang<np.pi/2:
b = v_temp.length()*abs(np.cos(ang))
else:
b = -v_temp.length()*abs(np.cos(ang))
v2 = b*hand_normal
# Generate the pivoting center point by translating the surface center using the computed translation vectors
center = translate_point(surface.center_point,v1+v2)
return center
def __init__(self,
focus_point,
main_point,
x_vec,
y_vec,
image_path=None,
resolution=(640, 480)):
self.focus_point = focus_point
self.main_point = main_point
self.x_vec = x_vec
self.y_vec = y_vec
self.resolution = resolution
self.z_vec = Vector(focus_point, main_point)
self.image = np.zeros((resolution[1], resolution[0], 3),
dtype=np.float32)
self.image_path = image_path
if not orthogonal(self.x_vec, self.y_vec):
raise ValueError('x_vec and y_vec are not orthogonal')
if not orthogonal(self.x_vec, self.z_vec):
raise ValueError('x_vec and z_vec are not orthogonal')
if not orthogonal(self.y_vec, self.z_vec):
raise ValueError('y_vec and z_vec are not orthogonal')
def test_vector_normalization(self):
self.assertAlmostEqual(abs(Vector(1, 1, 1).normalized()), 1)
def test_vector_addition(self):
self.assertEqual(
Vector(2, 3, 5) + Vector(7, 11, 13),
Vector(9, 14, 18),
)
def get_experimental_setup(obj, exp_no):
"""
For a given object and experiment number, provides initial finger states and goal region number
Parameters
----------
obj: string
Name of the object
exp_no: int
Number assigned to a initial and goal pair
Returns
----------
obj_type: string
Name of the object
goal_no: int
Number assigned to goal region
z: float
finger elevation
d_l: float
left finger d
d_r: float
right finger d
normal_l: Geometry3D Vector
Normal vector left finger
distal_l: Geometry3D Vector
Distal vector left finger
normal_r: Geometry3D Vector
Normal vector right finger
distal_r: Geometry3D Vector
Distal vector right finger
"""
if obj == "square_prism":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 10
d_r = 10
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 1, 0)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 1, 0)
elif exp_no == 2:
goal_no = 2
z = 2
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 1
z = 2.5
d_l = 10
d_r = 10
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 1, 0)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 1, 0)
elif exp_no == 4:
goal_no = 1
z = 1
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif obj == "rectangular_prism_small":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 2:
goal_no = 2
z = 0
d_l = 8
d_r = 8
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 0, -1)
elif exp_no == 3:
goal_no = 2
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, 1, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(0, -1, 0)
distal_r = Vector(0, 0, -1)
elif exp_no == 4:
goal_no = 4
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(0, 1, 0)
distal_r = Vector(0, 0, -1)
elif obj == "rectangular_prism_curved":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 10
d_r = 10
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 1, 0)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 1, 0)
elif exp_no == 2:
goal_no = 2
z = 2
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 3
z = 2.5
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 4:
goal_no = 1
z = 1
d_l = 9
d_r = 9
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif obj == "hexagonal_prism_small":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(0, 1, 0)
distal_r = Vector(0, 0, -1)
elif exp_no == 2:
goal_no = 2
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 3
z = 0
d_l = 10
d_r = 10
normal_l = Vector(0, 0, -1)
distal_l = Vector(0, -1, 0)
normal_r = Vector(0, 0, 1)
distal_r = Vector(0, -1, 0)
elif exp_no == 4:
goal_no = 4
z = 0
d_l = 8
d_r = 8
# normal_l=Vector(0,0,-1)
# distal_l=Vector(0,-1,0)
# normal_r=Vector(0,0,1)
# distal_r=Vector(0,-1,0)
normal_l = Vector(0, -1, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(0, 1, 0)
distal_r = Vector(0, 0, -1)
elif obj == "dome_tall":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 10
d_r = 10
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 1, 0)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 1, 0)
elif exp_no == 2:
goal_no = 2
z = 2
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 3
z = 2.5
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 4:
goal_no = 1
z = 1
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif obj == "round_prism":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, 1, 0)
distal_l = Vector(1, 0, 0)
normal_r = Vector(0, -1, 0)
distal_r = Vector(1, 0, 0)
elif exp_no == 2:
pass
elif exp_no == 3:
pass
elif exp_no == 4:
pass
elif obj == "cube":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 8
d_r = 8
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 1, 0)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 1, 0)
elif exp_no == 2:
pass
elif exp_no == 3:
pass
elif exp_no == 4:
pass
elif obj == "rectangular_prism_large":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 8
d_r = 8
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 1, 0)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 1, 0)
elif exp_no == 2:
goal_no = 2
z = 0
d_l = 10
d_r = 10
normal_l = Vector(0, 0, -1)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 0, 1)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 3
z = 0
d_l = 10
d_r = 10
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 0, -1)
elif exp_no == 4:
goal_no = 4
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(0, 1, 0)
distal_r = Vector(0, 0, -1)
elif obj == "dome_short":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, 1, 0)
distal_l = Vector(1, 0, 0)
normal_r = Vector(0, -1, 0)
distal_r = Vector(1, 0, 0)
elif exp_no == 2:
goal_no = 2
z = 0
d_l = 10
d_r = 10
normal_l = Vector(0, 0, -1)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 0, 1)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 3
z = 0
d_l = 10
d_r = 10
normal_l = Vector(-1, 0, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(1, 0, 0)
distal_r = Vector(0, 0, -1)
elif exp_no == 4:
goal_no = 4
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(0, 0, -1)
normal_r = Vector(0, 1, 0)
distal_r = Vector(0, 0, -1)
elif obj == "hexagonal_prism_large":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 1
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 2:
pass
elif exp_no == 3:
pass
elif exp_no == 4:
pass
elif obj == "rectangular_prism_2x4":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 2:
goal_no = 2
z = 2
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 1
z = 0
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 4:
goal_no = 1
z = 1
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif obj == "hexagonal_prism_tall":
obj_type = obj
if exp_no == 1:
goal_no = 1
z = 0
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 2:
goal_no = 2
z = 2
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 3:
goal_no = 1
z = 2.5
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 4:
goal_no = 3
z = 1
d_l = 8
d_r = 8
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
elif exp_no == 5:
goal_no = 1
z = 2.5
d_l = 10
d_r = 10
normal_l = Vector(0, -1, 0)
distal_l = Vector(-1, 0, 0)
normal_r = Vector(0, 1, 0)
distal_r = Vector(-1, 0, 0)
return obj_type, goal_no, z, d_l, d_r, normal_l, distal_l, normal_r, distal_r
def unfold_surface(surface_dict,
neighbors_dict,
surf_idx,
other,
neighbor,
show=False):
"""
Given the object surface dict and neighbors dict, generate an unfolded surface for the desired surface centering a selected surface
Parameters
----------
surface_dict: dict
Object dictionary including information of surface polygons, normal vectors, and goal regions
neighbors_dict: dict
A nested dictionary including information of surfaces and corresponding neighboring relationships
surf_idx: int
Number assigned to the center surface
other: tuple (Geometry3D.ConvexPolygon,Geometry3D.Vector,Geometry3D.ConvexPolygon)
information of folded surface to be unfolded - surface polygon, normal vector, goal region
neighbor: tuple (Geometry3D.ConvexPolygon,Geometry3D.Vector,Geometry3D.Point,float)
neighboring information - neighbor surface polygon, vector along intersection edge, intersecting corners, angle between surfaces
show: bool
generates a plot if True
Returns
----------
(nsurf,nnormal,ngoal): tuple
unfolded surface, unfolded normal, unfolded goal (if available)
"""
# Visualization
p = Renderer()
p.add((surface_dict[surf_idx][0], 'r', 1))
# Normal of the center surface
current_normal = surface_dict[surf_idx][1]
# Normal of the neighboring surface
candidate_normal = other[1]
# Angle between surfaces
angle = candidate_normal.angle(current_normal)
# Rotation calculations (Finding transformation matrix)
A, B, C = neighbor[1]
L = np.sqrt(A**2 + B**2 + C**2)
V = np.sqrt(B**2 + C**2)
D = np.array([[1, 0, 0, -neighbor[2][0]], [0, 1, 0, -neighbor[2][1]],
[0, 0, 1, -neighbor[2][2]], [0, 0, 0, 1]])
if V == 0:
R_x = np.eye(4)
else:
R_x = np.array([[1, 0, 0, 0], [0, C / V, -B / V, 0],
[0, B / V, C / V, 0], [0, 0, 0, 1]])
R_y = np.array([[V / L, 0, -A / L, 0], [0, 1, 0, 0], [A / L, 0, V / L, 0],
[0, 0, 0, 1]])
R_z = np.array([[np.cos(angle), -np.sin(angle), 0, 0],
[np.sin(angle), np.cos(angle), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
T = np.linalg.inv(D) @ np.linalg.inv(R_x) @ np.linalg.inv(
R_y) @ R_z @ R_y @ R_x @ D
# Applying transformation
P_init = np.empty((4, 0))
for point in other[0].points:
point_vec = np.array([point.x, point.y, point.z, 1]).reshape(4, 1)
P_init = np.concatenate([P_init, point_vec], axis=1)
normal_vec = np.array([
other[0].points[-1].x + candidate_normal[0],
other[0].points[-1].y + candidate_normal[1],
other[0].points[-1].z + candidate_normal[2], 1
]).reshape(4, 1)
P_init = np.concatenate([P_init, normal_vec], axis=1)
P_final = T @ P_init
new_points = list()
for i in range(P_final.shape[1] - 1):
new_points.append(gPoint(np.round(P_final[:3, i], decimals=3)))
# New normal vector
nnormal = Vector(P_final[:3, -1] - P_final[:3, -2])
# New surface definition as convex polygon
nsurf = ConvexPolygon((new_points))
# Transform goal region as well
if len(other) > 2:
G_init = np.empty((4, 0))
for point in other[2].points:
point_vec = np.array([point.x, point.y, point.z, 1]).reshape(4, 1)
G_init = np.concatenate([G_init, point_vec], axis=1)
G_final = T @ G_init
new_goal = list()
for i in range(G_final.shape[1]):
new_goal.append(gPoint(np.round(G_final[:3, i], decimals=3)))
ngoal = ConvexPolygon((new_goal))
p.add((nsurf, 'k', 1))
p.add((ngoal, 'k', 1))
if show:
p.add((other[0], 'k', 1))
p.show()
return (nsurf, nnormal, ngoal)
else:
p.add((nsurf, 'k', 1))
if show:
p.add((other[0], 'k', 1))
p.show()
return (nsurf, nnormal)
def test_vector_coordinate_setting(self):
v = Vector(2, 3, 5)
v[0] = 7
v[1] = 11
v[2] = 13
self.assertEqual(v, Vector(7, 11, 13))
def test_vector_inversion(self):
self.assertEqual(
-Vector(2, 3, 5),
Vector(-2, -3, -5),
)
def test_vector_multiply_vector_with_vector(self):
self.assertEqual(
Vector(2, 3, 5) * Vector(7, 11, 13),
2 * 7 + 3 * 11 + 5 * 13,
)
def test_vector_multiply_real_with_vector(self):
self.assertEqual(
2 * Vector(2, 3, 5),
Vector(4, 6, 10),
)
def test_vector_subtraction(self):
self.assertEqual(
Vector(9, 14, 18) - Vector(7, 11, 13),
Vector(2, 3, 5),
)
def test_vector_equality(self):
self.assertEqual(Vector(1, 2, 3), Vector(1, 2, 3))
self.assertNotEqual(Vector(1, 2, 3), Vector(1, 2, 4))
def get_finger_param(finger_poly,obj):
"""
Given the finger polygon and the object find the state parameters (finger parameters)
Parameters
----------
finger_poly: (surf, normal, distal): tuple
Finger polygon (Geometry3D.ConvexPolygon), finger normal(Geometry3D.Vector), distal vector (Geometry3D.Vector)
obj: dict
Dictionary including information about each surface on the object in the following form:
Geometry3D ConvexPolygon , Geometry3D Vector , Geometry3D ConvexPolygon
{surface_no:(surface_polygon_definition, surface_normal_vector, goal_region_polygon(if available))}
Returns
----------
d: float
Distance between finger joint and object start
z: float
Elevation of the finger on the object
"""
# Find finger center point
finger_center = finger_poly[0].center_point
# Find contact surface
for surf in obj:
if obj[surf][1].angle(finger_poly[1])<1e-3:
surface = obj[surf][0]
break
# Find object length alond finger distal vector
for point in surface.points:
for other_point in surface.points:
if point==other_point:
continue
else:
edge = Vector(point,other_point)
angle = edge.angle(finger_poly[2])
if angle <1e-3:
obj_length = edge.length()
# Find the center of the contact surface
obj_center = surface.center_point
# Center point on the proximal object edge
close_edge = translate_point(obj_center,(-finger_poly[2]*(obj_length/2)))
# Generate the vector connecting the proximal edge center and finger center
vec = Vector(finger_center,close_edge)
# If the vector has a length of 0, centers align, thus elevation is the same and d is half of the finger length
if vec.length()==0:
d = finger_w/2
z = 0
# Else compute the angle between distal vector and the generated vector and find the distance components to determine d and z
else:
ang = vec.angle(finger_poly[2])
if ang<np.pi/2:
b = vec.length()*abs(np.cos(ang))
else:
b = -vec.length()*abs(np.cos(ang))
d = np.round(b+finger_w/2,decimals=1)
q = obj_center.distance(finger_center)**2-(b+obj_length/2)**2
if q < 0 and abs(q) < 1e-3:
q = 0
if finger_poly[2].cross(finger_poly[1]).angle(vec)<np.pi/2:
z = -np.round(np.sqrt(q),decimals=1)
else:
z = np.round(np.sqrt(q),decimals=1)
# debugging material
if np.isnan(z):
print(obj_center.distance(finger_center))
print((b+obj_length/2))
print(obj_center)
print(finger_center)
print(b)
print(vec)
print(finger_poly[2])
return None
return d,z
def test_vector_zero(self):
self.assertEqual(
Vector.zero(),
Vector(0, 0, 0),
)
def generate_map(unfolded_surfaces,base,contact):
"""
Function to convert Geometry3D polygons for the object and fingers to shapely.geometry entities representing elements of the search map
Parameters
----------
unfolded_surfaces: dict
A dictionary similar to object dictionary, where the center surface remains the same but other surfaces are modified through unfolding.
base: int
Integer indicating the contact surface number (which is the base surface for the unfolding)
contact: Geometry3D.ConvexPolygon
Finger geometry defined as a convex polygon - including pose information (position and orientation)
Returns
----------
surf_map: dict
A dictionary that stores shapely.geometry.Polygon's for each surface of the object, modified through unfolding
goal_map: dict
A dictionary that stores shapely.geometry.Polygon's for each goal region on the surfaces of the object, modified through unfolding
contact_map: shapely.geometry.Polygon
Finger geometry defined in 2D through shapely Polygon
"""
# Initialize empty dictionaries for goal and surface polygons
goal_map = dict()
surf_map = dict()
# Main surface (center surface for the unfolding)
main = unfolded_surfaces[base]
# Transform (translate and project) the 3D polygon definitions onto xy-plane and convert to 2D for shapely
# Translation required assuming the center of the main surface is the origin
translation = [-main[0].center_point.x,-main[0].center_point.y,-main[0].center_point.z]
# Determine required axis and rotation angles for the transformation
axis = Vector(0,0,1).cross(main[1])
angle = Vector(0,0,1).angle(main[1])
A,B,C = axis
L = np.sqrt(A**2 + B**2 + C**2)
V = np.sqrt(B**2 + C**2)
if V == 0:
R_x = np.eye(4)
else:
R_x = np.array([[1,0,0,0],[0,C/V,-B/V,0],[0,B/V,C/V,0],[0,0,0,1]])
if L == 0:
R_y = np.eye(4)
else:
R_y = np.array([[V/L,0,-A/L,0],[0,1,0,0],[A/L,0,V/L,0],[0,0,0,1]])
R_z = np.array([[np.cos(angle),-np.sin(angle),0,0],
[np.sin(angle),np.cos(angle),0,0],
[0,0,1,0],[0,0,0,1]])
T = np.linalg.inv(R_x)@np.linalg.inv(R_y)@R_z@R_y@R_x
# Transform object surfaces and goal regions using the computed transformation matrix
for item in unfolded_surfaces:
surf_init = unfolded_surfaces[item][0]
P_init = np.empty((4,0))
for point in surf_init.points:
point_vec = np.array([point.x+translation[0],point.y+translation[1],point.z+translation[2],1]).reshape(4,1)
P_init = np.concatenate([P_init,point_vec],axis=1)
P_final = T@P_init
new_points = list()
for i in range(P_final.shape[1]):
new_points.append(list(P_final[:2,i]))
surfs = Polygon(new_points)
surf_map[item] = surfs
if len(unfolded_surfaces[item])>2:
goal_init = unfolded_surfaces[item][2]
P_init = np.empty((4,0))
for point in goal_init.points:
point_vec = np.array([point.x+translation[0],point.y+translation[1],point.z+translation[2],1]).reshape(4,1)
P_init = np.concatenate([P_init,point_vec],axis=1)
P_final = T@P_init
new_points = list()
for i in range(P_final.shape[1]):
new_points.append(list(P_final[:2,i]))
goal = Polygon(new_points)
goal_map[item] = goal
# Transform the finger polygon similarly
C_init = np.empty((4,0))
for point in contact.points:
point_vec = np.array([point.x+translation[0],point.y+translation[1],point.z+translation[2],1]).reshape(4,1)
C_init = np.concatenate([C_init,point_vec],axis=1)
C_final = T@C_init
new_points = list()
for i in range(C_final.shape[1]):
new_points.append(list(C_final[:2,i]))
contact_map = Polygon(new_points)
return surf_map,goal_map,contact_map
def test_vector_length(self):
self.assertEqual(
abs(Vector(3, 4, 0)),
5,
)
def get_OBJ(object_type, goal_no):
"""
Given the object name and goal region number provides geometry definitions for object and goal region
Parameters
----------
object_type: string
Name of the object
goal_no: int
Number assigned to goal region
Returns
----------
prism: dict
Dictionary including information about each surface on the object in the following form:
Geometry3D ConvexPolygon , Geometry3D Vector , Geometry3D ConvexPolygon
{surface_no:(surface_polygon_definition, surface_normal_vector, goal_region_polygon(if available))}
"""
if object_type == "square_prism":
####################################################### Square prism ########################################################################
##############################################################################################################################################
# a = 2.5 cm, h = 8 cm
p1 = gPoint(-1.25, -1.25, -4)
p2 = gPoint(-1.25, -1.25, 4)
p3 = gPoint(1.25, -1.25, 4)
p4 = gPoint(1.25, -1.25, -4)
p5 = gPoint(1.25, 1.25, -4)
p6 = gPoint(1.25, 1.25, 4)
p7 = gPoint(-1.25, 1.25, 4)
p8 = gPoint(-1.25, 1.25, -4)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-1.25, -1.25, 4)
g2 = gPoint(-1.25, -1.25, 2)
g3 = gPoint(1.25, -1.25, 2)
g4 = gPoint(1.25, -1.25, 4)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.25, 1.25, 4)
g2 = gPoint(-1.25, 1.25, 2)
g3 = gPoint(1.25, 1.25, 2)
g4 = gPoint(1.25, 1.25, 4)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
g1 = gPoint(1.25, -1.25, -4)
g2 = gPoint(1.25, -1.25, 2)
g3 = gPoint(1.25, 0.75, 2)
g4 = gPoint(1.25, 0.75, -4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.25, -1.25, -4)
g2 = gPoint(-1.25, -1.25, 2)
g3 = gPoint(-1.25, 1.25, 2)
g4 = gPoint(-1.25, 1.25, -4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 3:
g1 = gPoint(1.25, -1.25, 4)
g2 = gPoint(1.25, -1.25, 2)
g3 = gPoint(1.25, 1.25, 2)
g4 = gPoint(1.25, 1.25, 4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.25, -1.25, 4)
g2 = gPoint(-1.25, -1.25, 2)
g3 = gPoint(-1.25, 1.25, 2)
g4 = gPoint(-1.25, 1.25, 4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "rectangular_prism_small":
# Rectangular prism:
##################
# w = 2 cm, l = 3 cm, h = 4 cm
p1 = gPoint(-1.5, -1, -2)
p2 = gPoint(-1.5, -1, 2)
p3 = gPoint(1.5, -1, 2)
p4 = gPoint(1.5, -1, -2)
p5 = gPoint(1.5, 1, -2)
p6 = gPoint(1.5, 1, 2)
p7 = gPoint(-1.5, 1, 2)
p8 = gPoint(-1.5, 1, -2)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
goal2 = surf2
goal4 = surf4
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
goal5 = surf5
goal6 = surf6
prism = {
1: (surf1, n1),
2: (surf2, n2),
3: (surf3, n3),
4: (surf4, n4),
5: (surf5, n5, goal5),
6: (surf6, n6, goal6)
}
elif goal_no == 4:
g1 = gPoint(-1.5, -1, 0)
g2 = gPoint(-1.5, -1, 2)
g3 = gPoint(1.5, -1, 2)
g4 = gPoint(1.5, -1, 0)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, 1, 0)
g2 = gPoint(-1.5, 1, 2)
g3 = gPoint(1.5, 1, 2)
g4 = gPoint(1.5, 1, 0)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "rectangular_prism_curved":
# Rectangular prism tall with flanged corners:
##################
# w = 2 cm, l = 3 cm, h = 8 cm
p1 = gPoint(-1.5, -1, -4)
p2 = gPoint(-1.5, -1, 4)
p3 = gPoint(1.5, -1, 4)
p4 = gPoint(1.5, -1, -4)
p5 = gPoint(1.5, 1, -4)
p6 = gPoint(1.5, 1, 4)
p7 = gPoint(-1.5, 1, 4)
p8 = gPoint(-1.5, 1, -4)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-1.5, -1, 4)
g2 = gPoint(-1.5, -1, 2)
g3 = gPoint(1.5, -1, 2)
g4 = gPoint(1.5, -1, 4)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, 1, 4)
g2 = gPoint(-1.5, 1, 2)
g3 = gPoint(1.5, 1, 2)
g4 = gPoint(1.5, 1, 4)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
g1 = gPoint(1.5, -1, -4)
g2 = gPoint(1.5, -1, 2)
g3 = gPoint(1.5, 1, 2)
g4 = gPoint(1.5, 1, -4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, -1, -4)
g2 = gPoint(-1.5, -1, 2)
g3 = gPoint(-1.5, 1, 2)
g4 = gPoint(-1.5, 1, -4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 3:
g1 = gPoint(1.5, -1, 4)
g2 = gPoint(1.5, -1, 2)
g3 = gPoint(1.5, 1, 2)
g4 = gPoint(1.5, 1, 4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, -1, 4)
g2 = gPoint(-1.5, -1, 2)
g3 = gPoint(-1.5, 1, 2)
g4 = gPoint(-1.5, 1, 4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "hexagonal_prism_small":
# Hexagonal prism:
##################
# a = 2 cm, h = 3 cm
h = 1.5 * 2
p1 = gPoint(-1, -sqrt3, -h / 2)
p2 = gPoint(-1, -sqrt3, h / 2)
p3 = gPoint(1, -sqrt3, h / 2)
p4 = gPoint(1, -sqrt3, -h / 2)
p5 = gPoint(2, 0, h / 2)
p6 = gPoint(2, 0, -h / 2)
p7 = gPoint(1, sqrt3, h / 2)
p8 = gPoint(1, sqrt3, -h / 2)
p9 = gPoint(-1, sqrt3, h / 2)
p10 = gPoint(-1, sqrt3, -h / 2)
p11 = gPoint(-2, 0, h / 2)
p12 = gPoint(-2, 0, -h / 2)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p5, p6))
surf3 = ConvexPolygon((p6, p5, p7, p8))
surf4 = ConvexPolygon((p8, p7, p9, p10))
surf5 = ConvexPolygon((p10, p9, p11, p12))
surf6 = ConvexPolygon((p12, p11, p2, p1))
surf7 = ConvexPolygon((p2, p11, p9, p7, p5, p3))
surf8 = ConvexPolygon((p10, p12, p1, p4, p6, p8))
n1 = Vector(0, -1, 0)
n2 = Vector(1.5, -sqrt3 / 2, 0)
n3 = Vector(1.5, sqrt3 / 2, 0)
n4 = Vector(0, 1, 0)
n5 = Vector(-1.5, sqrt3 / 2, 0)
n6 = Vector(-1.5, -sqrt3 / 2, 0)
n7 = Vector(0, 0, 1)
n8 = Vector(0, 0, -1)
if goal_no == 1:
# ############ GOAL DEF 1 ############
goal7 = surf7
goal8 = surf8
prism = {
1: (surf1, n1),
2: (surf2, n2),
3: (surf3, n3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6),
7: (surf7, n7, goal7),
8: (surf8, n8, goal8)
}
elif goal_no == 2:
# ############ GOAL DEF 2 ############
goal2 = surf2
goal5 = surf5
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4),
5: (surf5, n5, goal5),
6: (surf6, n6),
7: (surf7, n7),
8: (surf8, n8)
}
elif goal_no == 3:
# ############ GOAL DEF 3 ############
g1 = gPoint(-1, -sqrt3, h / 2)
g2 = gPoint(-1, -sqrt3, 0)
g3 = gPoint(1, -sqrt3, h / 2)
g4 = gPoint(1, -sqrt3, 0)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1, sqrt3, h / 2)
g2 = gPoint(-1, sqrt3, 0)
g3 = gPoint(1, sqrt3, h / 2)
g4 = gPoint(1, sqrt3, 0)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6),
7: (surf7, n7),
8: (surf8, n8)
}
elif goal_no == 4:
g1 = gPoint(-2, 0, h / 2)
g2 = gPoint(-1, sqrt3, h / 2)
g3 = gPoint(1, sqrt3, h / 2)
g4 = gPoint(2, 0, h / 2)
goal7 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-2, 0, -h / 2)
g2 = gPoint(-1, sqrt3, -h / 2)
g3 = gPoint(1, sqrt3, -h / 2)
g4 = gPoint(2, 0, -h / 2)
goal8 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2),
3: (surf3, n3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6),
7: (surf7, n7, goal7),
8: (surf8, n8, goal8)
}
elif goal_no == 5:
pass
elif object_type == "dome_tall":
####################################################### Dome Tall ########################################################################
##############################################################################################################################################
# a = 3 cm, h = 8 cm
p1 = gPoint(-1.5, -1.5, -4)
p2 = gPoint(-1.5, -1.5, 4)
p3 = gPoint(1.5, -1.5, 4)
p4 = gPoint(1.5, -1.5, -4)
p5 = gPoint(1.5, 1.5, -4)
p6 = gPoint(1.5, 1.5, 4)
p7 = gPoint(-1.5, 1.5, 4)
p8 = gPoint(-1.5, 1.5, -4)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-1.5, -1.5, 4)
g2 = gPoint(-1.5, -1.5, 2)
g3 = gPoint(1.5, -1.5, 2)
g4 = gPoint(1.5, -1.5, 4)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, 1.5, 4)
g2 = gPoint(-1.5, 1.5, 2)
g3 = gPoint(1.5, 1.5, 2)
g4 = gPoint(1.5, 1.5, 4)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
g1 = gPoint(1.5, -1.5, -4)
g2 = gPoint(1.5, -1.5, 2)
g3 = gPoint(1.5, 1, 2)
g4 = gPoint(1.5, 1, -4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, -1.5, -4)
g2 = gPoint(-1.5, -1.5, 2)
g3 = gPoint(-1.5, 1.5, 2)
g4 = gPoint(-1.5, 1.5, -4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 3:
g1 = gPoint(1.5, -1.5, 4)
g2 = gPoint(1.5, -1.5, 2)
g3 = gPoint(1.5, 1.5, 2)
g4 = gPoint(1.5, 1.5, 4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, -1.5, 4)
g2 = gPoint(-1.5, -1.5, 2)
g3 = gPoint(-1.5, 1.5, 2)
g4 = gPoint(-1.5, 1.5, 4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "round_prism":
####################################################### Round prism ########################################################################
##############################################################################################################################################
# a = 4 cm, h = 6 cm
p1 = gPoint(-2, -2, -3)
p2 = gPoint(-2, -2, 3)
p3 = gPoint(2, -2, 3)
p4 = gPoint(2, -2, -3)
p5 = gPoint(2, 2, -3)
p6 = gPoint(2, 2, 3)
p7 = gPoint(-2, 2, 3)
p8 = gPoint(-2, 2, -3)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-1.5, -2, -3)
g2 = gPoint(-1.5, -2, 3)
g3 = gPoint(2, -2, 3)
g4 = gPoint(2, -2, -3)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, 2, -3)
g2 = gPoint(-1.5, 2, 3)
g3 = gPoint(2, 2, 3)
g4 = gPoint(2, 2, -3)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "cube":
####################################################### Cube ########################################################################
##############################################################################################################################################
# a = 5 cm
p1 = gPoint(-2.5, -2.5, -2.5)
p2 = gPoint(-2.5, -2.5, 2.5)
p3 = gPoint(2.5, -2.5, 2.5)
p4 = gPoint(2.5, -2.5, -2.5)
p5 = gPoint(2.5, 2.5, -2.5)
p6 = gPoint(2.5, 2.5, 2.5)
p7 = gPoint(-2.5, 2.5, 2.5)
p8 = gPoint(-2.5, 2.5, -2.5)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-1.5, -2.5, -2.5)
g2 = gPoint(-1.5, -2.5, 2.5)
g3 = gPoint(2.5, -2.5, 2.5)
g4 = gPoint(2.5, -2.5, -2.5)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, 2.5, -2.5)
g2 = gPoint(-1.5, 2.5, 2.5)
g3 = gPoint(2.5, 2.5, 2.5)
g4 = gPoint(2.5, 2.5, -2.5)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "rectangular_prism_large":
####################################################### Square prism ########################################################################
##############################################################################################################################################
# a = 3 cm, b = 4 cm, h = 5 cm
p1 = gPoint(-1.5, -2, -2.5)
p2 = gPoint(-1.5, -2, 2.5)
p3 = gPoint(1.5, -2, 2.5)
p4 = gPoint(1.5, -2, -2.5)
p5 = gPoint(1.5, 2, -2.5)
p6 = gPoint(1.5, 2, 2.5)
p7 = gPoint(-1.5, 2, 2.5)
p8 = gPoint(-1.5, 2, -2.5)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-1, -2, -2.5)
g2 = gPoint(-1, -2, 2.5)
g3 = gPoint(1.5, -2, 2.5)
g4 = gPoint(1.5, -2, -2.5)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1, 2, -2.5)
g2 = gPoint(-1, 2, 2.5)
g3 = gPoint(1.5, 2, 2.5)
g4 = gPoint(1.5, 2, -2.5)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
g1 = gPoint(1.5, -2, -1)
g2 = gPoint(1.5, -2, 2.5)
g3 = gPoint(1.5, 2, 2.5)
g4 = gPoint(1.5, 2, -1)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, -2, -1)
g2 = gPoint(-1.5, -2, 2.5)
g3 = gPoint(-1.5, 2, 2.5)
g4 = gPoint(-1.5, 2, -1)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 3:
goal5 = surf5
goal6 = surf6
prism = {
1: (surf1, n1),
2: (surf2, n2),
3: (surf3, n3),
4: (surf4, n4),
5: (surf5, n5, goal5),
6: (surf6, n6, goal6)
}
elif goal_no == 4:
g1 = gPoint(-1.5, -2, 0)
g2 = gPoint(-1.5, -2, 2.5)
g3 = gPoint(1.5, -2, 2.5)
g4 = gPoint(1.5, -2, 0)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, 2, 0)
g2 = gPoint(-1.5, 2, 2.5)
g3 = gPoint(1.5, 2, 2.5)
g4 = gPoint(1.5, 2, 0)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "dome_short":
####################################################### Dome Short ########################################################################
##############################################################################################################################################
# a = 3 cm, h = 4 cm
p1 = gPoint(-1.5, -1.5, -2)
p2 = gPoint(-1.5, -1.5, 2)
p3 = gPoint(1.5, -1.5, 2)
p4 = gPoint(1.5, -1.5, -2)
p5 = gPoint(1.5, 1.5, -2)
p6 = gPoint(1.5, 1.5, 2)
p7 = gPoint(-1.5, 1.5, 2)
p8 = gPoint(-1.5, 1.5, -2)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-1, -1.5, -2)
g2 = gPoint(-1, -1.5, 2)
g3 = gPoint(1.5, -1.5, 2)
g4 = gPoint(1.5, -1.5, -2)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1, 1.5, -2)
g2 = gPoint(-1, 1.5, 2)
g3 = gPoint(1.5, 1.5, 2)
g4 = gPoint(1.5, 1.5, -2)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
g1 = gPoint(1.5, -1.5, -1)
g2 = gPoint(1.5, -1.5, 2)
g3 = gPoint(1.5, 1.5, 2)
g4 = gPoint(1.5, 1.5, -1)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, -1.5, -1)
g2 = gPoint(-1.5, -1.5, 2)
g3 = gPoint(-1.5, 1.5, 2)
g4 = gPoint(-1.5, 1.5, -1)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 3:
goal5 = surf5
goal6 = surf6
prism = {
1: (surf1, n1),
2: (surf2, n2),
3: (surf3, n3),
4: (surf4, n4),
5: (surf5, n5, goal5),
6: (surf6, n6, goal6)
}
elif goal_no == 4:
g1 = gPoint(-1.5, -1.5, 0)
g2 = gPoint(-1.5, -1.5, 2)
g3 = gPoint(1.5, -1.5, 2)
g4 = gPoint(1.5, -1.5, 0)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, 1.5, 0)
g2 = gPoint(-1.5, 1.5, 2)
g3 = gPoint(1.5, 1.5, 2)
g4 = gPoint(1.5, 1.5, 0)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "hexagonal_prism_large":
# Hexagonal prism large:
##################
# a = 3 cm, h = 10 cm
h = 1.5 * 2
p1 = gPoint(-1, -sqrt3, -h / 2)
p2 = gPoint(-1, -sqrt3, h / 2)
p3 = gPoint(1, -sqrt3, h / 2)
p4 = gPoint(1, -sqrt3, -h / 2)
p5 = gPoint(2, 0, h / 2)
p6 = gPoint(2, 0, -h / 2)
p7 = gPoint(1, sqrt3, h / 2)
p8 = gPoint(1, sqrt3, -h / 2)
p9 = gPoint(-1, sqrt3, h / 2)
p10 = gPoint(-1, sqrt3, -h / 2)
p11 = gPoint(-2, 0, h / 2)
p12 = gPoint(-2, 0, -h / 2)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p5, p6))
surf3 = ConvexPolygon((p6, p5, p7, p8))
surf4 = ConvexPolygon((p8, p7, p9, p10))
surf5 = ConvexPolygon((p10, p9, p11, p12))
surf6 = ConvexPolygon((p12, p11, p2, p1))
surf7 = ConvexPolygon((p2, p11, p9, p7, p5, p3))
surf8 = ConvexPolygon((p10, p12, p1, p4, p6, p8))
n1 = Vector(0, -1, 0)
n2 = Vector(1.5, -sqrt3 / 2, 0)
n3 = Vector(1.5, sqrt3 / 2, 0)
n4 = Vector(0, 1, 0)
n5 = Vector(-1.5, sqrt3 / 2, 0)
n6 = Vector(-1.5, -sqrt3 / 2, 0)
n7 = Vector(0, 0, 1)
n8 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(1.5, -sqrt3, h / 2)
g2 = gPoint(1.5, -sqrt3, 2)
g3 = gPoint(3, 0, 2)
g4 = gPoint(3, 0, h / 2)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1.5, sqrt3, h / 2)
g2 = gPoint(-1.5, sqrt3, 2)
g3 = gPoint(-3, 0, 2)
g4 = gPoint(-3, 0, h / 2)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
g1 = gPoint(1.5, -sqrt3, -h / 2)
g2 = gPoint(1.5, -sqrt3, 2)
g3 = gPoint(2, sqrt3, 2)
g4 = gPoint(2, sqrt3, -h / 2)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-2, sqrt3, -h / 2)
g2 = gPoint(-2, sqrt3, 2)
g3 = gPoint(-3, 0, 2)
g4 = gPoint(-3, 0, -h / 2)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "rectangular_prism_2x4":
# a = 2 cm, b = 4 cm, h = 8 cm
p1 = gPoint(-2, -1, -4)
p2 = gPoint(-2, -1, 4)
p3 = gPoint(2, -1, 4)
p4 = gPoint(2, -1, -4)
p5 = gPoint(2, 1, -4)
p6 = gPoint(2, 1, 4)
p7 = gPoint(-2, 1, 4)
p8 = gPoint(-2, 1, -4)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p6, p5))
surf3 = ConvexPolygon((p5, p6, p7, p8))
surf4 = ConvexPolygon((p8, p7, p2, p1))
surf5 = ConvexPolygon((p2, p7, p6, p3))
surf6 = ConvexPolygon((p8, p1, p4, p5))
n1 = Vector(0, -1, 0)
n2 = Vector(1, 0, 0)
n3 = Vector(0, 1, 0)
n4 = Vector(-1, 0, 0)
n5 = Vector(0, 0, 1)
n6 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(-2, -1, 4)
g2 = gPoint(-2, -1, 2)
g3 = gPoint(2, -1, 2)
g4 = gPoint(2, -1, 4)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-2, 1, 4)
g2 = gPoint(-2, 1, 2)
g3 = gPoint(2, 1, 2)
g4 = gPoint(2, 1, 4)
goal3 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 2:
g1 = gPoint(2, -1, -4)
g2 = gPoint(2, -1, 2)
g3 = gPoint(2, 1, 2)
g4 = gPoint(2, 1, -4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-2, -1, -4)
g2 = gPoint(-2, -1, 2)
g3 = gPoint(-2, 1, 2)
g4 = gPoint(-2, 1, -4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif goal_no == 3:
g1 = gPoint(2, -1, 4)
g2 = gPoint(2, -1, 2)
g3 = gPoint(2, 1, 2)
g4 = gPoint(2, 1, 4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-2, -1, 4)
g2 = gPoint(-2, -1, 2)
g3 = gPoint(-2, 1, 2)
g4 = gPoint(-2, 1, 4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6)
}
elif object_type == "hexagonal_prism_tall":
# Hexagonal prism:
##################
# a = 2 cm, h = 8 cm
h = 4 * 2
p1 = gPoint(-1, -sqrt3, -h / 2)
p2 = gPoint(-1, -sqrt3, h / 2)
p3 = gPoint(1, -sqrt3, h / 2)
p4 = gPoint(1, -sqrt3, -h / 2)
p5 = gPoint(2, 0, h / 2)
p6 = gPoint(2, 0, -h / 2)
p7 = gPoint(1, sqrt3, h / 2)
p8 = gPoint(1, sqrt3, -h / 2)
p9 = gPoint(-1, sqrt3, h / 2)
p10 = gPoint(-1, sqrt3, -h / 2)
p11 = gPoint(-2, 0, h / 2)
p12 = gPoint(-2, 0, -h / 2)
surf1 = ConvexPolygon((p1, p2, p3, p4))
surf2 = ConvexPolygon((p4, p3, p5, p6))
surf3 = ConvexPolygon((p6, p5, p7, p8))
surf4 = ConvexPolygon((p8, p7, p9, p10))
surf5 = ConvexPolygon((p10, p9, p11, p12))
surf6 = ConvexPolygon((p12, p11, p2, p1))
surf7 = ConvexPolygon((p2, p11, p9, p7, p5, p3))
surf8 = ConvexPolygon((p10, p12, p1, p4, p6, p8))
n1 = Vector(0, -1, 0)
n2 = Vector(1.5, -sqrt3 / 2, 0)
n3 = Vector(1.5, sqrt3 / 2, 0)
n4 = Vector(0, 1, 0)
n5 = Vector(-1.5, sqrt3 / 2, 0)
n6 = Vector(-1.5, -sqrt3 / 2, 0)
n7 = Vector(0, 0, 1)
n8 = Vector(0, 0, -1)
if goal_no == 1:
g1 = gPoint(1, -sqrt3, 4)
g2 = gPoint(1, -sqrt3, 2)
g3 = gPoint(2, 0, 2)
g4 = gPoint(2, 0, 4)
goal2 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1, sqrt3, 4)
g2 = gPoint(-1, sqrt3, 2)
g3 = gPoint(-2, 0, 2)
g4 = gPoint(-2, 0, 4)
goal5 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2, goal2),
3: (surf3, n3),
4: (surf4, n4),
5: (surf5, n5, goal5),
6: (surf6, n6),
7: (surf7, n7),
8: (surf8, n8)
}
elif goal_no == 2:
g1 = gPoint(2, 0, -4)
g2 = gPoint(2, 0, 2)
g3 = gPoint(1, sqrt3, 2)
g4 = gPoint(1, sqrt3, -4)
goal3 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-2, 0, -4)
g2 = gPoint(-2, 0, 2)
g3 = gPoint(-1, -sqrt3, 2)
g4 = gPoint(-1, -sqrt3, -4)
goal6 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1),
2: (surf2, n2),
3: (surf3, n3, goal3),
4: (surf4, n4),
5: (surf5, n5),
6: (surf6, n6, goal6),
7: (surf7, n7),
8: (surf8, n8)
}
elif goal_no == 3:
g1 = gPoint(-1, -sqrt3, 4)
g2 = gPoint(-1, -sqrt3, 2)
g3 = gPoint(1, -sqrt3, 2)
g4 = gPoint(1, -sqrt3, 4)
goal1 = ConvexPolygon((g1, g2, g3, g4))
g1 = gPoint(-1, sqrt3, 4)
g2 = gPoint(-1, sqrt3, 2)
g3 = gPoint(1, sqrt3, 2)
g4 = gPoint(1, sqrt3, 4)
goal4 = ConvexPolygon((g1, g2, g3, g4))
prism = {
1: (surf1, n1, goal1),
2: (surf2, n2),
3: (surf3, n3),
4: (surf4, n4, goal4),
5: (surf5, n5),
6: (surf6, n6),
7: (surf7, n7),
8: (surf8, n8)
}
return prism
def test_vector_parallel(self):
self.assertTrue(Vector(2, 3, 5).parallel(Vector(4, 6, 10)))
self.assertTrue(Vector(1, 0, 0).parallel(Vector(10, 0, 0)))
self.assertFalse(Vector(2, 3, 5).parallel(Vector(4, 6, 11)))
self.assertFalse(Vector(1, 0, 0).parallel(Vector(0, 1, 0)))
def trace_ray(halfline,
trace_list,
ray_list,
face_list,
point_list,
point_all_list,
light_list,
depth,
current_face,
n=1):
"""
**Input:**
- halfline: Geometry3D.HalfLine of the input HalfLine
- trace_list: a list of the current path of the ray now
- ray_list: a list of trace list which will be used to calculate the intersity of light
- face_list: a list of Faces of the scene
- depth: the transformation depth remained
- n: a float of the refraction rate of the input material
"""
if depth == 0:
return
inter_point, d, face = inter_halfline_face_list(halfline,
face_list,
current_face=current_face)
if inter_point is None:
return
# print('call trace_ray with trace_list={},d={},point={},cpg={}\n'.format(trace_list,d,inter_point,face.cpg))
if n > 1: # when the ray come out of a transparent
refraction_halfline = get_refraction_halfline(halfline, n, 1, face.cpg)
if refraction_halfline is not None:
# only refraction ray
trace_ray(halfline=refraction_halfline,
trace_list=copy.deepcopy(trace_list) + [d, NO_LOSS],
ray_list=ray_list,
face_list=face_list,
point_list=copy.deepcopy(point_list) + [inter_point],
point_all_list=point_all_list,
light_list=light_list,
depth=depth - 1,
current_face=face,
n=1)
else: # n == 1
# refraction ray
if not (face.material.f_refract == np.zeros(3)
).all(): # there should be refraction of the material
refraction_halfline = get_refraction_halfline(
halfline, 1, face.material.n, face.cpg)
if refraction_halfline is not None:
trace_ray(halfline=refraction_halfline,
trace_list=copy.deepcopy(trace_list) +
[d, face.material.f_refract],
ray_list=ray_list,
face_list=face_list,
point_list=copy.deepcopy(point_list) + [inter_point],
point_all_list=point_all_list,
light_list=light_list,
depth=depth - 1,
current_face=face,
n=face.material.n)
# reflection ray
reflection_halfline = get_reflection_halfline(halfline, face.cpg)
trace_ray(halfline=reflection_halfline,
trace_list=copy.deepcopy(trace_list) +
[d, face.material.f_reflect],
ray_list=ray_list,
face_list=face_list,
point_list=copy.deepcopy(point_list) + [inter_point],
point_all_list=point_all_list,
light_list=light_list,
depth=depth - 1,
current_face=face,
n=face.material.n)
# light
for light in light_list:
if isinstance(light, AmbientLight):
# print('\033[0;32mpts list:\033[0m{}'.format(point_list))
ray_list.append(
copy.deepcopy(trace_list) + [d, face.material.ka, light])
point_all_list.append(point_list)
elif isinstance(light, PointLight):
# print('\033[0;32mpts list:\033[0m{}'.format(point_list))
light_i, light_d, light_f = inter_halfline_face_list(
HalfLine(inter_point, light.pos),
face_list,
current_face=face)
if light_i is not None:
if light_i == inter_point:
continue # deal with some cases
if not light.pos in Segment(light_i, inter_point):
# print('intersection point:{},light point:{}'.format(light_i,light.pos))
continue
L_vec = Vector(inter_point, light.pos).normalized()
N_vec = face.cpg.plane.n.normalized()
V_vec = halfline.vector.normalized()
R_vec = reflection_halfline.vector.normalized()
H_vec = 0.5 * (V_vec + L_vec).normalized()
i_s = np.zeros(3, dtype=np.float32)
for i in range(3):
i_s[i] = face.material.ks[i] * math.pow(
N_vec * H_vec / 2, face.material.alpha)
if i_s[i] < 0:
print('L_vec:{},N_vec:{},V_vec:{},R_vec:{},H_vec:{}'.
format(L_vec, N_vec, V_vec, R_vec, H_vec))
i_d = face.material.kd * (L_vec * N_vec)
if i_d[0] < 0:
print('\033[0;32mL:{},N:{}\033[0m'.format(L_vec, N_vec))
d_light = distance(inter_point, light.pos)
ray_list.append(
copy.deepcopy(trace_list) + [d, i_s + i_d, d_light, light])
point_all_list.append(point_list)
else:
raise TypeError('Unknown Light Type:{}'.format(type(light)))
def test_vector_orthogonal(self):
self.assertTrue(Vector(1, 0, 0).orthogonal(Vector(0, 1, 1)))
self.assertFalse(Vector(1, 0, 0).orthogonal(Vector(1, 0, 0)))
def test_vector_angle(self):
self.assertAlmostEqual(
Vector(1, 0, 0).angle(Vector(1, 0, 1)),
math.pi / 4, # 45 deg
)
def test_vector_coordinate_access(self):
v = Vector(1, 2, 3)
self.assertEqual(v[0], 1)
self.assertEqual(v[1], 2)
self.assertEqual(v[2], 3)
