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 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 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 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 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 __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)