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
