def __init__(self, min_shin_angle, base, base_x_offset, knee_rod, knee_connection_rod, knee_offset, hip, shin): super(HindLeg, self).__init__() self.min_shin_angle = min_shin_angle self.base = base self.base_x_offset = base_x_offset self.knee_rod = knee_rod self.knee_connection_rod = knee_connection_rod self.knee_offset = knee_offset self.hip = hip self.shin = shin self.knee_base = vec(0, 0) self.hip_base = self.knee_base + vec(-self.base_x_offset, -self.base)
def dataframe(df_path, subject_id, subject_template, in_mat, out_mat): # overwrite_dat=OVERWRITE_DF import os # Imports must be made inside Node Function. import pandas as pd from utils import compute_rmse, vec if not os.path.isfile(df_path): e = 'File %s does not exist. Please re-run initialisation script.' % df_path return print (e) else: df = pd.read_csv(df_path, index_col=0) cols_h = ['subject','template','matfile_name','px','py','pz','translation','rotation','total_length','resolution','noise','rmse'] cols_r = list(df) if not cols_h != cols_r: e = 'Column names do not match. Please check.' return print (e) matfile_id = in_mat.split('/')[-1] px, py, pz, vec_length = vec(in_mat,out_mat) rx, ry, rz = None, None, None trans, rot = matfile_id.split('_')[0], matfile_id.split('_')[1] vox_dim = '1' # find params of functions, to get vox_dim ## HERE rmse = compute_rmse(in_mat,out_mat) df_tmp = pd.DataFrame([[subject_id,matfile_id,px,py,pz,vec_length,trans,rot,vox_dim,rmse]], columns=cols_r) df = df.append(df_tmp, ignore_index=True) df.to_csv(df_path)
def __init__(self, start, startId, end, endId): self.start = start self.startId = startId self.end = end self.endId = endId self.defaultVector = vec(self.end.x - self.start.x, self.end.y - self.start.y).normalize()
def angles_generator(hip_min=0, hip_max=180, hip_step=5, knee_min=0, knee_max=180, knee_step=5): for hip_angle in np.arange(hip_min, hip_max, hip_step): for knee_angle in np.arange(knee_min, knee_max, knee_step): yield np.radians(vec(hip_angle, knee_angle))
def shading_test(self, p, n, v, l, r, I, material, scene): # test with shading at p with normal n and view/illum directions v/l # r is distance to light, I is intensity t = 1.3 # arbitrary value d = -2.3 * v # arbitrary scale ray = Ray(p - t * d, d) # ray consistent with hit hit = Hit(t, p, n, vec([0, 0]), material) light = PointLight(p + r * normalize(l), I) return light.illuminate(ray, hit, scene)
def test_simple(self): # A triangle on the xy plane and perpendicular rays tri = Triangle(np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]]), None) hit = tri.intersect(Ray(vec([0.3, 0.3, 1]), vec([0, 0, -1]))) self.assertAlmostEqual(hit.t, 1.) np.testing.assert_allclose(hit.point, [0.3, 0.3, 0]) np.testing.assert_allclose(hit.normal, [0, 0, 1]) hit = tri.intersect(Ray(vec([-0.3, 0.3, 1]), vec([0, 0, -1]))) self.assertEqual(hit.t, np.inf) hit = tri.intersect(Ray(vec([0.5, 0.55, 1]), vec([0, 0, -1]))) self.assertEqual(hit.t, np.inf) hit = tri.intersect(Ray(vec([0.2, -0.1, 1]), vec([0, 0, -1]))) self.assertEqual(hit.t, np.inf)
def predict(self, Y, verbose = True, iterations = 100): lasso = Lasso(self.D, self.lamb) lasso.fit(Y, iterations = iterations) X = lasso.coef_ E = np.zeros((self.C, Y.shape[1])) for i in range(self.C): Xi = utils.get_block_row(X, i, self.train_range) Di = utils.get_block_col(self.D, i, self.train_range) R = Y - np.dot(Di, Xi) E[i,:] = (R*R).sum(axis = 0) return utils.vec(np.argmin(E, axis = 0) + 1)
def test_1_cube(self): (i, p, n, t) = read_obj(open('models/cube.obj')) m = Mesh(i, p, None, None, None) hit = m.intersect(Ray(vec([0.3, 0.3, 0.3]), vec([1, 0, 0]))) self.assertAlmostEqual(hit.t, 0.7) np.testing.assert_allclose(hit.point, [1, 0.3, 0.3]) np.testing.assert_allclose(hit.normal, [1, 0, 0]) hit = m.intersect(Ray(vec([0.3, 0.3, 0.3]), vec([-1, 0, 0]))) self.assertAlmostEqual(hit.t, 1.3) np.testing.assert_allclose(hit.point, [-1, 0.3, 0.3]) np.testing.assert_allclose(hit.normal, [-1, 0, 0]) hit = m.intersect(Ray(vec([-1.3, 0.3, 0.7]), vec([1, 0, 0]))) self.assertLess(hit.t, np.inf) np.testing.assert_allclose(hit.point, [-1, 0.3, 0.7]) np.testing.assert_allclose(hit.normal, [-1, 0, 0]) hit = m.intersect(Ray(vec([1.3, 0.3, 0.7]), vec([-1, 0, 0]))) self.assertLess(hit.t, np.inf) np.testing.assert_allclose(hit.point, [1, 0.3, 0.7]) np.testing.assert_allclose(hit.normal, [1, 0, 0]) hit = m.intersect(Ray(vec([1, 1, 2]), vec([0.1, 0.1, -1]))) self.assertEqual(hit.t, np.inf)
def update_time(self): """ The idle function advances the time of day. The day has 24 hours, from sunrise to sunset and from sunrise to second sunset. The time of day is converted to degrees and then to radians. """ if not self.window.exclusive: return time_of_day = self.time_of_day if self.time_of_day < 12.0 \ else 24.0 - self.time_of_day if time_of_day <= 2.5: self.time_of_day += 1.0 / G.TIME_RATE time_of_day += 1.0 / G.TIME_RATE self.count += 1 else: self.time_of_day += 20.0 / G.TIME_RATE time_of_day += 20.0 / G.TIME_RATE self.count += 1.0 / 20.0 if self.time_of_day > 24.0: self.time_of_day = 0.0 time_of_day = 0.0 side = len(self.world.sectors) * 2.0 self.light_y = 2.0 * side * sin( time_of_day * self.hour_deg * G.DEG_RAD) self.light_z = 2.0 * side * cos( time_of_day * self.hour_deg * G.DEG_RAD) if time_of_day <= 2.5: ambient_value = 1.0 else: ambient_value = 1 - (time_of_day - 2.25) / 9.5 self.ambient = vec(ambient_value, ambient_value, ambient_value, 1.0) # Calculate sky colour according to time of day. sin_t = sin(pi * time_of_day / 12.0) self.bg_red = 0.1 * (1.0 - sin_t) self.bg_green = 0.9 * sin_t self.bg_blue = min(sin_t + 0.4, 0.8) if fmod(self.count / 2, G.TIME_RATE) == 0: if self.clock == 18: self.clock = 6 else: self.clock += 1 self.skydome.update_time_of_day(self.time_of_day)
def update_time(self): """ The idle function advances the time of day. The day has 24 hours, from sunrise to sunset and from sunrise to second sunset. The time of day is converted to degrees and then to radians. """ if not self.window.exclusive: return time_of_day = self.time_of_day if self.time_of_day < 12.0 \ else 24.0 - self.time_of_day if time_of_day <= 2.5: self.time_of_day += 1.0 / G.TIME_RATE time_of_day += 1.0 / G.TIME_RATE self.count += 1 else: self.time_of_day += 20.0 / G.TIME_RATE time_of_day += 20.0 / G.TIME_RATE self.count += 1.0 / 20.0 if self.time_of_day > 24.0: self.time_of_day = 0.0 time_of_day = 0.0 side = len(self.world.sectors) * 2.0 self.light_y = 2.0 * side * sin(time_of_day * self.hour_deg * G.DEG_RAD) self.light_z = 2.0 * side * cos(time_of_day * self.hour_deg * G.DEG_RAD) if time_of_day <= 2.5: ambient_value = 1.0 else: ambient_value = 1 - (time_of_day - 2.25) / 9.5 self.ambient = vec(ambient_value, ambient_value, ambient_value, 1.0) # Calculate sky colour according to time of day. sin_t = sin(pi * time_of_day / 12.0) self.bg_red = 0.1 * (1.0 - sin_t) self.bg_green = 0.9 * sin_t self.bg_blue = min(sin_t + 0.4, 0.8) if fmod(self.count / 2, G.TIME_RATE) == 0: if self.clock == 18: self.clock = 6 else: self.clock += 1 self.skydome.update_time_of_day(self.time_of_day)
def get_max_cloud_cross_sections(points, resolution, draw=False): x_min = np.min(points[:, 0]) x_max = np.max(points[:, 0]) y_min = np.min(points[:, 1]) y_max = np.max(points[:, 1]) N = int((x_max - x_min) / resolution) + 1 M = int((y_max - y_min) / resolution) + 1 matrix = np.zeros((M, N), dtype=np.int8) for point in points: x, y = point col = int((x - x_min) / resolution) row = M - int((y - y_min) / resolution) - 1 matrix[row, col] = 1 horizontal = max((get_max_sequence_length(row) for row in matrix)) vertical = max((get_max_sequence_length(col) for col in matrix.T)) if horizontal > 0: horizontal -= 1 if vertical > 0: vertical -= 1 if draw: corner = vec(x_min, y_min) + (resolution / 2) matrix_points = [] for x in xrange(N): for y in xrange(M): if matrix[M - y - 1, x]: matrix_points.append(corner + vec(x, y) * resolution) points = np.array(matrix_points) plt.plot(points[:, 0], points[:, 1], '.', markersize=8) return vec(horizontal, vertical) * resolution
def test_aspect(self): # A camera with a different aspect ratio: rays should be scaled in x aspect = 1.5 cam = Camera(aspect=aspect) # Center ray is still straight down the axis ray = cam.generate_ray(vec([0.5, 0.5])) np.testing.assert_almost_equal(ray.origin, vec([0, 0, 0])) assert_direction_matches(ray.direction, vec([0, 0, -1])) # FOV corner rays are scaled by aspect ray = cam.generate_ray(vec([0, 0])) assert_direction_matches(ray.direction, vec([-aspect, -1, -1])) ray = cam.generate_ray(vec([1, 1])) assert_direction_matches(ray.direction, vec([aspect, 1, -1]))
def find_left_triangle(a, b, c_s, c_e, get_b_global_angle=False): c_vec = (c_e - c_s) * vec(1, -1) c = np.linalg.norm(c_vec) if c == 0: c = np.nan d = (c**2 + b**2 - a**2) / (2 * c) b_global_angle = np.pi - (get_angle(c_vec) + np.arccos(d / b)) vertex = c_e + b * rotated(b_global_angle) return (vertex, b_global_angle) if get_b_global_angle else vertex
def test_fov(self): # A camera with a different fov: rays should be scaled in x and y vfov = 60 cam = Camera(vfov=vfov) s = np.tan(vfov / 2 * np.pi / 180) # Center ray is still straight down the axis ray = cam.generate_ray(vec([0.5, 0.5])) np.testing.assert_almost_equal(ray.origin, vec([0, 0, 0])) assert_direction_matches(ray.direction, vec([0, 0, -1])) # FOV corner rays are scaled by s ray = cam.generate_ray(vec([0, 0])) assert_direction_matches(ray.direction, vec([-s, -s, -1])) ray = cam.generate_ray(vec([1, 1])) assert_direction_matches(ray.direction, vec([s, s, -1]))
def test_unitsphere_misses(self): unit_sphere = Sphere(vec([0, 0, 0]), 1.0, None) # on axis miss hit = unit_sphere.intersect( Ray(vec([2.0, 3.0, 0.0]), vec([-1.0, 0.0, 0.0]))) self.assertEqual(hit.t, np.inf) # off axis miss hit = unit_sphere.intersect( Ray(vec([2.0, 3.0, 4.0]), vec([-1.0, -1.0, -1.0]))) self.assertEqual(hit.t, np.inf) # dead center miss, start past sphere hit = unit_sphere.intersect( Ray(vec([2.0, 0.0, 0.0]), vec([-1.0, 0.0, 0.0]), 3.1)) self.assertEqual(hit.t, np.inf)
def test_0_single(self): # Same as the triangle intersection test case, with just one triangle m = Mesh( np.array([[0, 1, 2]]), # indices np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]]), # positions None, # normals None, # uvs None) hit = m.intersect(Ray(vec([0.3, 0.3, 1]), vec([0, 0, -1]))) self.assertAlmostEqual(hit.t, 1.) np.testing.assert_allclose(hit.point, [0.3, 0.3, 0]) np.testing.assert_allclose(hit.normal, [0, 0, 1]) hit = m.intersect(Ray(vec([-0.3, 0.3, 1]), vec([0, 0, -1]))) self.assertEqual(hit.t, np.inf) hit = m.intersect(Ray(vec([0.5, 0.55, 1]), vec([0, 0, -1]))) self.assertEqual(hit.t, np.inf) hit = m.intersect(Ray(vec([0.2, -0.1, 1]), vec([0, 0, -1]))) self.assertEqual(hit.t, np.inf)
def reduced_glove(): counter = 0 with open(glove_reduced, "w") as f: pass from utils import vec for token in vocab.tokens(): if counter % 750 == 0: print(counter) res = token for fl in vec(token): res += " " + str(fl) with open(glove_reduced, "a") as f: f.write(res) f.write('\n') counter += 1 print(counter)
def test_arbitrary_frame(self): # A camera that lines up with nothing in particular eye = vec([3, 4, 5]) target = vec([6, 7, 8]) up = vec([1, 2, 3]) vfov = 47 cam = Camera(eye=eye, target=target, up=up, vfov=vfov) # Center ray points towards target ray = cam.generate_ray(vec([0.5, 0.5])) np.testing.assert_almost_equal(ray.origin, eye) assert_direction_matches(ray.direction, target - eye) # top-center ray coplanar with eye, target, up ray = cam.generate_ray(vec([0.5, 1])) np.testing.assert_allclose(ray.direction @ np.cross(target - eye, up), vec([0, 0, 0]), 1e-6, 1e-6) # top-center ray has the correct angle self.assertAlmostEqual( normalize(ray.direction) @ normalize(target - eye), np.cos(vfov / 2 * np.pi / 180), places=6)
WIDTH = 700 HEIGHT = 500 FPS = 60 # define colors WHITE = (255, 255, 255) BLACK = (0, 0, 0) RED = (255, 0, 0) GREEN = (0, 255, 0) BLUE = (0, 0, 255) CAR_RECT = Rect(0, 0, 5, 5) if __name__ == '__main__': SIMULATOR = Simulator([ vec(50, 40), vec(100, 44), vec(300, 36), vec(450, 40), vec(50, 160), vec(100, 180), vec(200, 160), vec(450, 160), vec(200, 10), vec(200, 40), vec(150, 172), vec(200, 400) ], [ [1], [9], [3], [7], [5], [10], [7], [11], [9], [10, 2], [11, 6], [7, 10, 6] ], { 0: TrafficLight(2, 1, 2), 1: TrafficLight(2, 1, 2), 2: TrafficLight(2, 1, 2), 3: TrafficLight(2, 1, 2), 4: TrafficLight(2, 1, 2), 5: TrafficLight(2, 1, 2), 6: TrafficLight(2, 1, 2) }, [
def setup(self): try: #Make sure the address they want to connect to works ipport = G.IP_ADDRESS.split(":") if len(ipport) == 1: ipport.append(1486) sock = socket.socket() sock.connect(tuple(ipport)) except socket.error as e: print "Socket Error:", e #Otherwise back to the main menu we go return False self.init_gl() sky_rotation = -20.0 # -20.0 print 'loading sky' self.skydome = Skydome( 'resources/skydome.jpg', 0.7, 100.0, sky_rotation, ) lux = 600.0 self.focus_block = Block(width=1.05, height=1.05) self.earth = vec(0.8, 0.8, 0.8, 1.0) self.white = vec(1.0, 1.0, 1.0, 1.0) self.ambient = vec(1.0, 1.0, 1.0, 1.0) self.polished = GLfloat(100.0) self.crack_batch = pyglet.graphics.Batch() #if G.DISABLE_SAVE and world_exists(G.game_dir, G.SAVE_FILENAME): # open_world(self, G.game_dir, G.SAVE_FILENAME) self.world = World() self.packetreceiver = PacketReceiver(self.world, self, sock) self.world.packetreceiver = self.packetreceiver self.packetreceiver.start() #Get our position from the server self.packetreceiver.request_spawnpos() #Since we don't know it yet, lets disable self.update, or we'll load the wrong chunks and fall self.update_disabled = self.update self.update = lambda dt: None #We'll re-enable it when the server tells us where we should be self.player = Player((0,0,0), (-20, 0), game_mode=G.GAME_MODE) print('Game mode: ' + self.player.game_mode) self.item_list = ItemSelector(self, self.player, self.world) self.inventory_list = InventorySelector(self, self.player, self.world) self.item_list.on_resize(self.window.width, self.window.height) self.inventory_list.on_resize(self.window.width, self.window.height) self.text_input = TextWidget(self.window, '', 0, 0, self.window.width, visible=False, font_name='Arial') self.text_input.push_handlers(on_toggled=self.on_text_input_toggled, key_released=self.text_input_callback) self.chat_box = TextWidget(self.window, '', 0, self.text_input.y + self.text_input.height + 50, self.window.width / 2, height=min(300, self.window.height / 3), visible=False, multi_line=True, readonly=True, font_size=14, font_name='Arial', background_color=(64,64,64,200)) self.camera = Camera3D(target=self.player) if G.HUD_ENABLED: self.label = pyglet.text.Label( '', font_name='Arial', font_size=8, x=10, y=self.window.height - 10, anchor_x='left', anchor_y='top', color=(255, 255, 255, 255)) pyglet.clock.schedule_interval_soft(self.world.process_queue, 1.0 / G.MAX_FPS) pyglet.clock.schedule_interval_soft(self.world.hide_sectors, 1.0, self.player) return True
def setup(self): if G.SINGLEPLAYER: try: print 'Starting internal server...' # TODO: create world menu G.SAVE_FILENAME = "world" start_server(internal=True) sock = socket.socket() sock.connect(("localhost", 1486)) except socket.error as e: print "Socket Error:", e #Otherwise back to the main menu we go return False except Exception as e: print 'Unable to start internal server' import traceback traceback.print_exc() return False else: try: #Make sure the address they want to connect to works ipport = G.IP_ADDRESS.split(":") if len(ipport) == 1: ipport.append(1486) sock = socket.socket() sock.connect((tuple(ipport))) except socket.error as e: print "Socket Error:", e #Otherwise back to the main menu we go return False self.init_gl() sky_rotation = -20.0 # -20.0 # TERRAIN_CHOICE = self.biome_generator.get_biome_type(sector[0], sector[2]) default_skybox = 'skydome.jpg' #if TERRAIN_CHOICE == G.NETHER: # default_skybox = 'skydome_nether.jpg' #else: # default_skybox = 'skybox.jpg' print 'loading ' + default_skybox self.skydome = Skydome( 'resources/' + default_skybox, #'resources/skydome.jpg', 0.7, 100.0, sky_rotation, ) self.player_ids = {} # Dict of all players this session, indexes are their ID's [0: first Player on server,] self.focus_block = B.Block(width=1.05, height=1.05) self.earth = vec(0.8, 0.8, 0.8, 1.0) self.white = vec(1.0, 1.0, 1.0, 1.0) self.ambient = vec(1.0, 1.0, 1.0, 1.0) self.polished = GLfloat(100.0) self.crack_batch = pyglet.graphics.Batch() #if G.DISABLE_SAVE and world_exists(G.game_dir, G.SAVE_FILENAME): # open_world(self, G.game_dir, G.SAVE_FILENAME) self.world = World() self.packetreceiver = PacketReceiver(self.world, self, sock) self.world.packetreceiver = self.packetreceiver G.CLIENT = self.packetreceiver self.packetreceiver.start() #Get our position from the server self.packetreceiver.request_spawnpos() #Since we don't know it yet, lets disable self.update, or we'll load the wrong chunks and fall self.update_disabled = self.update self.update = lambda dt: None #We'll re-enable it when the server tells us where we should be self.player = Player(game_mode=G.GAME_MODE) print('Game mode: ' + self.player.game_mode) self.item_list = ItemSelector(self, self.player, self.world) self.inventory_list = InventorySelector(self, self.player, self.world) self.item_list.on_resize(self.window.width, self.window.height) self.inventory_list.on_resize(self.window.width, self.window.height) self.text_input = TextWidget(self.window, '', 0, 0, self.window.width, visible=False, font_name=G.CHAT_FONT) self.text_input.push_handlers(on_toggled=self.on_text_input_toggled, key_released=self.text_input_callback) self.chat_box = TextWidget(self.window, '', 0, self.text_input.y + self.text_input.height + 50, self.window.width / 2, height=min(300, self.window.height / 3), visible=False, multi_line=True, readonly=True, font_name=G.CHAT_FONT, text_color=(255, 255, 255, 255), background_color=(0, 0, 0, 100), enable_escape=True) self.camera = Camera3D(target=self.player) if G.HUD_ENABLED: self.label = pyglet.text.Label( '', font_name='Arial', font_size=8, x=10, y=self.window.height - 10, anchor_x='left', anchor_y='top', color=(255, 255, 255, 255)) pyglet.clock.schedule_interval_soft(self.world.process_queue, 1.0 / G.MAX_FPS) pyglet.clock.schedule_interval_soft(self.world.hide_sectors, 10.0, self.player) return True
def test_default_camera(self): # A camera located at the origin facing the -z direction cam = Camera() # Center ray is straight down the axis ray = cam.generate_ray(vec([0.5, 0.5])) np.testing.assert_almost_equal(ray.origin, vec([0, 0, 0])) assert_direction_matches(ray.direction, vec([0, 0, -1])) # FOV is 90 degrees, so corner rays are centered in octants ray = cam.generate_ray(vec([0, 0])) assert_direction_matches(ray.direction, vec([-1, -1, -1])) ray = cam.generate_ray(vec([1, 0])) assert_direction_matches(ray.direction, vec([1, -1, -1])) ray = cam.generate_ray(vec([0, 1])) assert_direction_matches(ray.direction, vec([-1, 1, -1])) ray = cam.generate_ray(vec([1, 1])) assert_direction_matches(ray.direction, vec([1, 1, -1])) # Spot check something that is not a corner dir00 = cam.generate_ray(vec([0, 0])).direction dir10 = cam.generate_ray(vec([1, 0])).direction dir01 = cam.generate_ray(vec([0, 1])).direction dirab = cam.generate_ray(vec([0.2, 0.6])).direction assert_direction_matches(dirab, 0.2 * dir00 + 0.2 * dir10 + 0.6 * dir01)
def test_nonunit_hits(self): # all the same as the first case, but scaled by 3 and shifted by (-1, -5, -7) sphere = Sphere(vec([-1, -5, -7]), 3.0, None) hit = self.confirm_hit( sphere, Ray(vec([5.0, -5.0, -7.0]), vec([-3.0, 0.0, 0.0]))) self.assertAlmostEqual(hit.t, 1.0) hit = self.confirm_hit( sphere, Ray(vec([8.0, -5.0, -7.0]), vec([-6.0, 0.0, 0.0]))) self.assertAlmostEqual(hit.t, 1.0) hit = self.confirm_hit( sphere, Ray(vec([2.0, -3.5, -7.0]), vec([-3.0, 0.0, 0.0]))) self.assertAlmostEqual(hit.t, 1 - np.sin(np.pi / 3)) hit = self.confirm_hit( sphere, Ray(vec([5.0, 4.0, 5.0]), vec([-2.0, -3.0, -4.0]))) self.assertAlmostEqual(hit.t, 3 - 3 / np.sqrt(29)) hit = self.confirm_hit( sphere, Ray(vec([5.0, 4.0, 8.0]), vec([-2.0, -3.0, -4.0]))) self.assertAlmostEqual(hit.t, 3.0)
def test_square_frame(self): # A camera with a frame where up is equal to v cam = Camera(eye=vec([1, 2, 2]), target=vec([1, 4, 2]), up=vec([0, 0, 1])) # Center ray is straight down the y axis ray = cam.generate_ray(vec([0.5, 0.5])) np.testing.assert_almost_equal(ray.origin, vec([1, 2, 2])) assert_direction_matches(ray.direction, vec([0, 1, 0])) # corners are like default camera but (x,y) is (x, z) ray = cam.generate_ray(vec([0, 0])) assert_direction_matches(ray.direction, vec([-1, 1, -1])) ray = cam.generate_ray(vec([1, 0])) assert_direction_matches(ray.direction, vec([1, 1, -1])) ray = cam.generate_ray(vec([0, 1])) assert_direction_matches(ray.direction, vec([-1, 1, 1])) ray = cam.generate_ray(vec([1, 1])) assert_direction_matches(ray.direction, vec([1, 1, 1]))
# hip_torque = hip_drive_torque # # knee_torque = knee_drive_torque * knee_angle_delta / shin_angle_delta # torque_coeff = knee_angle_delta / shin_angle_delta # knee_torque = knee_drive_torque * torque_coeff # A * X = B # X = invA * B A = np.vstack([1.0 / hip_sholder, torque_coeff / knee_sholder]).T B = shin_force hip_drive_torque, knee_drive_torque = (np.matrix(A).I * np.matrix(B).T).A1 return state, hip_drive_torque, knee_drive_torque if __name__ == '__main__': model = optimal() ys = np.arange(Y_MIN, Y_MAX, STEP) torques = np.array( [get_torques(model, vec(X, y), vec(0.0, 2 * G * M))[1:] for y in ys]) torques[np.abs(torques) > MAX_TORQUE] = np.nan plt.plot(ys, torques) plt.show()
def test_unitsphere_hits(self): unit_sphere = Sphere(np.array([0, 0, 0]), 1.0, None) # dead center hit hit = self.confirm_hit( unit_sphere, Ray(vec([2.0, 0.0, 0.0]), vec([-1.0, 0.0, 0.0]))) self.assertAlmostEqual(hit.t, 1.0) # dead center with non-unit direction hit = self.confirm_hit( unit_sphere, Ray(vec([3.0, 0.0, 0.0]), vec([-2.0, 0.0, 0.0]))) self.assertAlmostEqual(hit.t, 1.0) # off center hit hit = self.confirm_hit( unit_sphere, Ray(vec([1.0, 0.5, 0.0]), vec([-1.0, 0.0, 0.0]))) self.assertAlmostEqual(hit.t, 1 - np.sin(np.pi / 3)) # center hit from off axis hit = self.confirm_hit( unit_sphere, Ray(vec([2.0, 3.0, 4.0]), vec([-2.0, -3.0, -4.0]))) self.assertAlmostEqual(hit.t, 1 - 1 / np.sqrt(29)) # off center, off axis hit hit = self.confirm_hit( unit_sphere, Ray(vec([2.0, 3.0, 5.0]), vec([-2.0, -3.0, -4.0]))) self.assertAlmostEqual(hit.t, 1.0) # dead center hit, start inside hit = self.confirm_hit( unit_sphere, Ray(vec([2.0, 0.0, 0.0]), vec([-1.0, 0.0, 0.0]), 1.1)) self.assertAlmostEqual(hit.t, 3.0)
def setup(self): if G.SINGLEPLAYER: try: print 'Starting internal server...' # TODO: create world menu G.SAVE_FILENAME = "world" start_server(internal=True) sock = socket.socket() sock.connect(("localhost", 1486)) except socket.error as e: print "Socket Error:", e #Otherwise back to the main menu we go return False except Exception as e: print 'Unable to start internal server' import traceback traceback.print_exc() return False else: try: #Make sure the address they want to connect to works ipport = G.IP_ADDRESS.split(":") if len(ipport) == 1: ipport.append(1486) sock = socket.socket() sock.connect((tuple(ipport))) except socket.error as e: print "Socket Error:", e #Otherwise back to the main menu we go return False self.init_gl() sky_rotation = -20.0 # -20.0 # TERRAIN_CHOICE = self.biome_generator.get_biome_type(sector[0], sector[2]) default_skybox = 'skydome.jpg' #if TERRAIN_CHOICE == G.NETHER: # default_skybox = 'skydome_nether.jpg' #else: # default_skybox = 'skybox.jpg' print 'loading ' + default_skybox self.skydome = Skydome( 'resources/' + default_skybox, #'resources/skydome.jpg', 0.7, 100.0, sky_rotation, ) self.player_ids = { } # Dict of all players this session, indexes are their ID's [0: first Player on server,] self.focus_block = Block(width=1.05, height=1.05) self.earth = vec(0.8, 0.8, 0.8, 1.0) self.white = vec(1.0, 1.0, 1.0, 1.0) self.ambient = vec(1.0, 1.0, 1.0, 1.0) self.polished = GLfloat(100.0) self.crack_batch = pyglet.graphics.Batch() #if G.DISABLE_SAVE and world_exists(G.game_dir, G.SAVE_FILENAME): # open_world(self, G.game_dir, G.SAVE_FILENAME) self.world = World() self.packetreceiver = PacketReceiver(self.world, self, sock) self.world.packetreceiver = self.packetreceiver G.CLIENT = self.packetreceiver self.packetreceiver.start() #Get our position from the server self.packetreceiver.request_spawnpos() #Since we don't know it yet, lets disable self.update, or we'll load the wrong chunks and fall self.update_disabled = self.update self.update = lambda dt: None #We'll re-enable it when the server tells us where we should be self.player = Player(game_mode=G.GAME_MODE) print('Game mode: ' + self.player.game_mode) self.item_list = ItemSelector(self, self.player, self.world) self.inventory_list = InventorySelector(self, self.player, self.world) self.item_list.on_resize(self.window.width, self.window.height) self.inventory_list.on_resize(self.window.width, self.window.height) self.text_input = TextWidget(self.window, '', 0, 0, self.window.width, visible=False, font_name=G.CHAT_FONT) self.text_input.push_handlers(on_toggled=self.on_text_input_toggled, key_released=self.text_input_callback) self.chat_box = TextWidget(self.window, '', 0, self.text_input.y + self.text_input.height + 50, self.window.width / 2, height=min(300, self.window.height / 3), visible=False, multi_line=True, readonly=True, font_name=G.CHAT_FONT, text_color=(255, 255, 255, 255), background_color=(0, 0, 0, 100), enable_escape=True) self.camera = Camera3D(target=self.player) if G.HUD_ENABLED: self.label = pyglet.text.Label('', font_name='Arial', font_size=8, x=10, y=self.window.height - 10, anchor_x='left', anchor_y='top', color=(255, 255, 255, 255)) #if G.DEBUG_TEXT_ENABLED: # self.debug_text = TextWidget(self.window, '', # 0, self.window.height - 300, # 500, 300, # visible=True, multi_line=True, readonly=True, # font_name='Arial', font_size=10, # text_color=(255, 255, 255, 255), # background_color=(0, 0, 0, 0)) pyglet.clock.schedule_interval_soft(self.world.process_queue, 1.0 / G.MAX_FPS) pyglet.clock.schedule_interval_soft(self.world.hide_sectors, 10.0, self.player) return True
def test_diffuse(self): # light directly overhead, unit distance and intensity np.testing.assert_allclose( self.shading_test( vec([0, 0, 0]), vec([0, 1, 0]), vec([1, 1, 0]), # p, n, v vec([0, 1, 0]), 1, vec([1, 1, 1]), # l, r, I Material(vec([0.2, 0.4, 0.6])), Scene([])), vec([0.2, 0.4, 0.6])) # light at 60 degrees, unit distance and intensity np.testing.assert_allclose( self.shading_test( vec([0, 0, 0]), vec([0, 1, 0]), vec([1, 1, 0]), # p, n, v vec([0, 1, np.sqrt(3)]), 1, vec([1, 1, 1]), # l, r, I Material(vec([0.2, 0.4, 0.6])), Scene([])), 0.5 * vec([0.2, 0.4, 0.6])) # light at 45 degrees, non-unit distance and intensity np.testing.assert_allclose( self.shading_test( vec([1, 2, 3]), vec([0, 0, 1]), vec([1, 1, 1]), # p, n, v vec([0, 1, 1]), 2, vec([0.6, 0.3, 0.2]), # l, r, I Material(vec([0.2, 0.4, 0.6])), Scene([])), np.sqrt(0.5) * vec([.12, .12, .12]) / 4, 1e-6, 1e-6) # light behind the surface np.testing.assert_allclose( self.shading_test( vec([0, 0, 0]), vec([0, 1, 0]), vec([1, 1, 0]), # p, n, v vec([0, -1, np.sqrt(3)]), 1, vec([1, 1, 1]), # l, r, I Material(vec([0.2, 0.4, 0.6])), Scene([])), vec([0., 0., 0.]))
def constructW(fea, bNormalized=False, NeighborMode='KNN', kKNN=5, bLDA=False, bSelfConnected=False, gnd=None, WeightMode='HeatKernel', t=1, bTrueKNN=False, SameCategoryWeight=1, bSemiSupervised=False, semiSplit=None): ''' % Usage: % W = constructW(fea,options) % % fea: Rows of vectors of data points. Each row is x_i % options: Struct value in Matlab. The fields in options that can be set: % % NeighborMode - Indicates how to construct the graph. Choices % are: [Default 'KNN'] % 'KNN' - k = 0 % Complete graph % k > 0 % Put an edge between two nodes if and % only if they are among the k nearst % neighbors of each other. You are % required to provide the parameter k in % the options. Default k=5. % 'Supervised' - k = 0 % Put an edge between two nodes if and % only if they belong to same class. % k > 0 % Put an edge between two nodes if % they belong to same class and they % are among the k nearst neighbors of % each other. % Default: k=0 % You are required to provide the label % information gnd in the options. % % WeightMode - Indicates how to assign weights for each edge % in the graph. Choices are: % 'Binary' - 0-1 weighting. Every edge receiveds weight % of 1. % 'HeatKernel' - If nodes i and j are connected, put weight % W_ij = exp(-norm(x_i - x_j)/2t^2). You are % required to provide the parameter t. [Default One] % 'Cosine' - If nodes i and j are connected, put weight % cosine(x_i,x_j). % % k - The parameter needed under 'KNN' NeighborMode. % Default will be 5. % gnd - The parameter needed under 'Supervised' % NeighborMode. Colunm vector of the label % information for each data point. % bLDA - 0 or 1. Only effective under 'Supervised' % NeighborMode. If 1, the graph will be constructed % to make LPP exactly same as LDA. Default will be % 0. % t - The parameter needed under 'HeatKernel' % WeightMode. Default will be 1 % bNormalized - 0 or 1. Only effective under 'Cosine' WeightMode. % Indicates whether the fea are already be % normalized to 1. Default will be 0 % bSelfConnected - 0 or 1. Indicates whether W(i,i) == 1. Default 0 % if 'Supervised' NeighborMode & bLDA == 1, % bSelfConnected will always be 1. Default 0. % bTrueKNN - 0 or 1. If 1, will construct a truly kNN graph % (Not symmetric!). Default will be 0. Only valid % for 'KNN' NeighborMode % % % Examples: % % fea = rand(50,15); % options = []; % options.NeighborMode = 'KNN'; % options.k = 5; % options.WeightMode = 'HeatKernel'; % options.t = 1; % W = constructW(fea,options); % % % fea = rand(50,15); % gnd = [ones(10,1);ones(15,1)*2;ones(10,1)*3;ones(15,1)*4]; % options = []; % options.NeighborMode = 'Supervised'; % options.gnd = gnd; % options.WeightMode = 'HeatKernel'; % options.t = 1; % W = constructW(fea,options); % % % fea = rand(50,15); % gnd = [ones(10,1);ones(15,1)*2;ones(10,1)*3;ones(15,1)*4]; % options = []; % options.NeighborMode = 'Supervised'; % options.gnd = gnd; % options.bLDA = 1; % W = constructW(fea,options); % % % For more details about the different ways to construct the W, please % refer: % Deng Cai, Xiaofei He and Jiawei Han, "Document Clustering Using % Locality Preserving Indexing" IEEE TKDE, Dec. 2005. % % % Written by Deng Cai (dengcai2 AT cs.uiuc.edu), April/2004, Feb/2006, % May/2007 ''' bSpeed = 1 NeighborMode_low = NeighborMode.lower() if NeighborMode_low == 'KNN'.lower(): # For simplicity, we include the data point itself in the kNN k = kKNN elif NeighborMode_low == 'Supervised'.lower(): k = kLDA # 0 assert gnd is not None, 'Label(gnd) should be provided under ' 'Supervised' ' NeighborMode!' if (fea is not None) and (len(gnd) != fea.shape[0]): raise ValueError('gnd doesn' 't match with fea!') else: raise NotImplementedError('NeighborMode does not exist!') bBinary = False bCosine = False WeightMode_low = WeightMode.lower() if WeightMode_low == 'Binary'.lower(): bBinary = True elif WeightMode_low == 'HeatKernel'.lower(): if t is None: nSmp = fea.shape[0] if nSmp > 3000: tmpInd = np.random.randint(0, nSmp, size=[3000]) D = EuDist2(fea[tuple(tmpInd), :]) else: D = EuDist2(fea) t = np.mean(D) elif WeightMode_low == 'Cosine'.lower(): bCosine = True else: raise ValueError('WeightMode does not exist!') if gnd is not None: nSmp = len(gnd) else: nSmp = fea.shape[0] maxM = 62500000 # 500M BlockSize = int(np.floor(float(maxM) / (nSmp * 3))) if NeighborMode_low == 'Supervised'.lower(): Label = set(gnd) nLabel = len(Label) if bLDA: G = np.zeros([nSmp, nSmp]) for idx in xrange(nLabel): classIdx = gnd == Label[idx] G[np.ix_(classIdx, classIdx)] = 1. / np.sum(classIdx) W = scipy.sparse.csr_matrix(G) ######################### return G if WeightMode_low == 'Binary'.lower(): if k > 0: G = np.zeros([nSmp * (k + 1), 3]) idNow = 0 for i in xrange(nLabel): classIdx = np.where(gnd == Label[i]) D = EuDist2(fea[classIdx, :], None, False) idx = np.argsort(D, axis=1) dump = np.sort(D, axis=1) # sort each row del D, dump idx = idx[:, :k + 1] nSmpClass = len(classIdx) * (k + 1) G[idNow:nSmpClass + idNow, 0] = np.tile(classIdx, [k + 1, 1]) G[idNow:nSmpClass + idNow, 1] = classIdx[idx] G[idNow:nSmpClass + idNow, 2] = 1 idNow = idNow + nSmpClass del idx G = scipy.sparse.coo_matrix((G[:, 0], [G[:, 1], G[:, 2]]), (nSmp, nSmp)) ###### G = G.tolil() #G = G.todense() G = np.maximum(G, G.T) else: G = np.zeros([nSmp, nSmp]) for i in xrange(nLabel): classIdx = np.where(gnd == Label[i]) G[np.ix_(classIdx, classIdx)] = 1. if not bSelfConnected: if isinstance(G, np.ndarray): np.fill_diagonal(G, 0) else: G.setdiag(0) W = scipy.sparse.coo_matrix( G) #################################### W = W.tolil() elif WeightMode_low == 'HeatKernel'.lower(): if k > 0: G = np.zeros([nSmp * (k + 1), 3]) idNow = 0 for i in xrange(nLabel): classIdx = np.where(gnd == Label[i]) D = EuDist2(fea[classIdx, :], None, False) idx = np.argsort(D, axis=1) dump = np.sort(D, axis=1) # sort each row del D idx = idx[:, :k + 1] dump = dump[:, :k + 1] dump = np.exp(-dump / (2. * (t**2.))) nSmpClass = len(classIdx) * (k + 1) G[idNow:nSmpClass + idNow, 0] = np.tile(classIdx, [k + 1, 1]) G[idNow:nSmpClass + idNow, 1] = classIdx(vec(idx)) G[idNow:nSmpClass + idNow, 2] = vec(dump) idNow += nSmpClass del dump, idx G = scipy.sparse.coo_matrix((G[:, 0], [G[:, 1], G[:, 2]]), (nSmp, nSmp)) G = G.tolil() else: G = np.zeros([nSmp, nSmp]) for i in xrange(nLabel): classIdx = np.where(gnd == Label[i]) D = EuDist2( fea[classIdx, :], None, False ) ######################################################## D = exp(-D / (2 * (t**2))) G[np.ix_(classIdx, classIdx)] = D if not bSelfConnected: if isinstance(G, np.ndarray): np.fill_diagonal(G, 0) else: G.setdiag(0) W = scipy.sparse.coo_matrix(np.maximum( G, G.T)) ###################### W = W.tolil() elif WeightMode_low == 'Cosine'.lower(): if not bNormalized: fea = NormalizeFea(fea) if k > 0: G = np.zeros([nSmp * (k + 1), 3]) idNow = 0 for i in xrange(nLabel): classIdx = np.where(gnd == Label[i]) D = np.dot(fea[classIdx, :], fea[classIdx, :].T) idx = np.argsort(-D, axis=1) dump = np.sort(-D, axis=1) # sort each row del D idx = idx[:, :k + 1] dump = -dump[:, :k + 1] nSmpClass = len(classIdx) * (k + 1) G[idNow:nSmpClass + idNow, 0] = np.tile(classIdx, [k + 1, 1]) G[idNow:nSmpClass + idNow, 1] = classIdx(vec(idx)) G[idNow:nSmpClass + idNow, 2] = vec(dump) idNow += nSmpClass del dump, idx G = scipy.sparse.coo_matrix((G[:, 0], [G[:, 1], G[:, 2]]), (nSmp, nSmp)) G = G.tolil() else: G = np.zeros([nSmp, nSmp]) for i in xrange(nLabel): classIdx = np.where(gnd == Label[i]) G[np.ix_(classIdx, classIdx)] = np.dot(fea[classIdx, :], fea[classIdx, :].T) if not bSelfConnected: if isinstance(G, np.ndarray): np.fill_diagonal(G, 0) else: G.setdiag(0) if isinstance(G, np.ndarray): W = np.maximum(G, G.T) W = scipy.sparse.csr_matrix(W) else: W = G.maximum(G.T) W = W.tolil() else: raise ValueError('WeightMode does not exist!') return W if (bCosine) and (not bNormalized): Normfea = NormalizeFea(fea) if (NeighborMode.lower() == 'KNN'.lower()) and (k > 0): if not (bCosine and bNormalized): G = np.zeros([nSmp * (k + 1), 3]) for i in xrange(int(np.ceil(float(nSmp) / BlockSize))): if (i == int(np.ceil(float(nSmp) / BlockSize)) - 1): smpIdx = range(i * BlockSize, nSmp) dist = EuDist2(fea[smpIdx, :], fea, False) if bSpeed: nSmpNow = len(smpIdx) dump = np.zeros([nSmpNow, k + 1]) idx = dump.copy() for j in xrange(k + 1): dump[:, j] = np.min(dist, axis=1) idx[:, j] = np.argmin(dist, axis=1) temp = idx[:, j] * nSmpNow + np.arange(nSmpNow) temp = temp.astype(np.int) shapeDist = dist.shape dist = dist.flatten(order='F') dist[temp] = 1e100 ############################## dist = reshape(dist, shapeDist) else: idx = np.argsort(dist, axis=1) dump = np.sort(dist, axis=1) # sort each row idx = idx[:, :k + 1] dump = dump[:, :k + 1] if not bBinary: if bCosine: dist = np.dot(Normfea[smpIdx, :], Normfea.T) dist = dist.todense( ) ######################################### linidx = range(idx.shape[0]) ##dump = dist(sub2ind(size(dist), linidx(:,ones(1,size(idx,2))), idx)); dumpInd = sub2ind( dist.shape, [linidx[:, tuple([1] * idx.shape[1]), idx]]) dump = dist[dumpInd] else: dump = np.exp(-dump / (2 * (t**2))) G[i * BlockSize * (k + 1):nSmp * (k + 1), 0] = np.array(smpIdx * (k + 1)) G[i * BlockSize * (k + 1):nSmp * (k + 1), 1] = vec(idx) if not bBinary: G[i * BlockSize * (k + 1):nSmp * (k + 1), 2] = vec(dump) else: G[i * BlockSize * (k + 1):nSmp * (k + 1), 2] = 1 else: smpIdx = range(i * BlockSize, (i + 1) * BlockSize) dist = EuDist2(fea[smpIdx, :], fea, False) if bSpeed: nSmpNow = len(smpIdx) dump = np.zeros([nSmpNow, k + 1]) idx = dump.copy() for j in xrange(k + 1): idx[:, j] = np.argmin(dist, axis=1) dump[:, j] = np.min(dist, axis=1) temp = idx[:, j] * nSmpNow + np.arange(nSmpNow) dist[tuple(temp)] = 1e100 else: idx = np.argsort(dist, axis=1) dump = np.sort(dist, axis=1) # sort each row idx = idx[:, :k + 1] dump = dump[:, :k + 1] if not bBinary: if bCosine: dist = np.dot(Normfea[smpIdx, :], Normfea.T) dist = dist.todense( ) ################################################33 linidx = range(idx.shape[0]) dumpInd = sub2ind( dist.shape, [np.tile(linidix, [1, idx.shape[1]]), idx]) dump = dist[dumpInd] else: dump = np.exp(-dump / (2 * (t**2))) G[i * BlockSize * (k + 1):(i + 1) * BlockSize * (k + 1), 0] = np.tile(smpIdx, [k + 1, 1]) ################## G[i * BlockSize * (k + 1):(i + 1) * BlockSize * (k + 1), 1] = vec(idx) if not bBinary: G[i * BlockSize * (k + 1):(i + 1) * BlockSize * (k + 1), 2] = vec(dump) else: G[i * BlockSize * (k + 1):(i + 1) * BlockSize * (k + 1), 2] = 1 W = scipy.sparse.coo_matrix((G[:, 0], [G[:, 1], G[:, 2]]), (nSmp, nSmp)) W = W.tolil() else: G = np.zeros([nSmp * (k + 1), 3]) for i in xrange(int(np.ceil(float(nSmp) / BlockSize))): if (i == int(np.ceil(float(nSmp) / BlockSize)) - 1): smpIdx = range(i * BlockSize, nSmp) dist = np.dot(fea[tuple(smpIdx), :], fea.T) dist = dist.todense( ) ########################################################################3 if bSpeed: nSmpNow = len(smpIdx) dump = np.zeros([nSmpNow, k + 1]) idx = dump.copy() for j in xrange(k + 1): idx[:, j] = np.argmax(dist, axis=1) dump[:, j] = np.max(dist, axis=1) temp = idx[:, j] * nSmpNow + np.arange(nSmpNow) dist[tuple(temp)] = 0 else: idx = np.argsort(-dist, axis=1) dump = np.sort(-dist, axis=1) # sort each row idx = idx[:, :k + 1] dump = -dump[:, :k + 1] G[i * BlockSize * (k + 1):nSmp * (k + 1), 0] = np.tile( smpIdx, [1, k + 1]) ################################## G[i * BlockSize * (k + 1):nSmp * (k + 1), 1] = vec(idx) G[i * BlockSize * (k + 1):nSmp * (k + 1), 2] = vec(dump) else: smpIdx = range(i * BlockSize, (i + 1) * BlockSize) dist = np.dot(fea[tuple(smpIdx), :], fea.T) dist = dist.todense() ###### if bSpeed: nSmpNow = len(smpIdx) dump = np.zeros([nSmpNow, k + 1]) idx = dump.copy() for j in xrange(k + 1): idx[:, j] = np.argmax(dist, axis=1) dump[:, j] = np.max(dist, axis=1) temp = idx[:, j] * nSmpNow + np.arange(nSmpNow) dist[tuple(temp)] = 0 else: idx = np.argsort(-dist, axis=1) dump = np.sort(-dist, axis=1) # sort each row idx = idx[:, :k + 1] dump = -dump[:, :k + 1] G[i * BlockSize * (k + 1):(i + 1) * BlockSize * (k + 1), 0] = np.tile(smpIdx, [1, k + 1]) G[i * BlockSize * (k + 1):(i + 1) * BlockSize * (k + 1), 1] = vec(idx) G[i * BlockSize * (k + 1):(i + 1) * BlockSize * (k + 1), 2] = vec(dump) W = scipy.sparse.coo_matrix((G[:, 0], [G[:, 1], G[:, 2]]), (nSmp, nSmp)) W = W.tolil() if bBinary: W[W != 0] = 1 if bSemiSupervised: tmpgnd = gnd(semiSplit) Label = set(tmpgnd) nLabel = len(Label) G = np.zeros([np.sum(semiSplit), np.sum(semiSplit)]) for idx in xrange(nLabel): classIdx = tmpgnd == Label[idx] G[np.ix_(classIdx, classIdx)] = 1 Wsup = scipy.sparse.coo_matrix( G ) ############################################################ Wsup = Wsup.tolil() W[np.ix_(semiSplit, semiSplit)] = ( Wsup > 0) * SameCategoryWeight ############################## if not bSelfConnected: if isinstance(W, np.ndarray): np.fill_diagonal(W, 0) else: W.setdiag(0) if not bTrueKNN: if isinstance(W, np.ndarray): W = np.maximum(W, W.T) W = scipy.sparse.csr_matrix(W) else: W = W.maximum(W.T) W = W.tolil() return W # strcmpi(options.NeighborMode,'KNN') & (options.k == 0) # Complete Graph if WeightMode_low == 'Binary'.lower(): raise ValueError('Binary weight can not be used for complete graph!') elif WeightMode_low == 'HeatKernel'.lower(): W = EuDist2(fea, bSqrt=False) W = np.exp(-W / (2 * (t**2))) elif WeightMode_low == 'Cosine'.lower(): W = np.dot(Normfea, Normfea.T) else: raise ValueError('WeightMode does not exist!') if not bSelfConnected: if isinstance(W, np.ndarray): np.fill_diagonal(W, 0) else: W.setdiag(0) if isinstance(W, np.ndarray): W = np.maximum(W, W.T) W = scipy.sparse.csr_matrix(W) else: W = W.maximum(W.T) W = W.tolil() return W
def get_max_deceleratable_speed(model, deceleration, robot_mass, max_angular_speed, max_torque, x, y_min, y_max, y_step, plot=False, each_nth_curve=1): y = np.arange(y_min, y_max, y_step) force = robot_mass * vec(0.0, deceleration) y, points, angles = get_limited_y_range(model, x, y, max_torque, force) heights = (y - y[0]) / 1000 # m das = np.diff(angles[:, :2], axis=0) min_times = np.max(np.abs(das), axis=1) / max_angular_speed dhs = np.diff(heights) max_v_speed = dhs / min_times # m/s start_v_speed = np.hstack( (np.arange(0, max_v_speed[0], max_v_speed[1] - max_v_speed[0]), max_v_speed)) dvs = -2 * deceleration * np.linspace(0, heights[-1], dhs.size) start_speed_matrix = stack_to_matrix(start_v_speed, dhs.size) speed_matrix = start_speed_matrix**2 + deceleration_matrix( dvs, start_v_speed.size) speed_matrix = np.sqrt(speed_matrix * (speed_matrix > 0)) max_speed_matrix = stack_to_matrix(max_v_speed, start_v_speed.size).T speed_matrix[speed_matrix > max_speed_matrix] = np.nan mask = speed_matrix[-1, :] == 0 possible_curves = speed_matrix[:, mask] best_curve = np.max(possible_curves, axis=1) height_mask = -np.isnan(best_curve) possible_heights = heights[:-1][height_mask] if plot: plt.plot(heights[:-1], max_v_speed) plt.plot(heights[:-1], speed_matrix[:, ::each_nth_curve]) plt.plot(heights[:-1], best_curve, linewidth=2) plt.show() max_possible_speed = best_curve[-np.isnan(best_curve)][0] start_height = min(possible_heights) * 1000 + y[0] stop_height = max(possible_heights) * 1000 + y[0] return max_possible_speed, start_height, stop_height