def test_Vec2_subtraction_operator():
components1 = np.random.rand(2)
components2 = np.random.rand(2)
vec2_1py = vectormath.Vector2(*components1)
vec2_2py = vectormath.Vector2(*components2)
vec2_1cpp = NumCpp.Vec2(*components1)
vec2_2cpp = NumCpp.Vec2(*components2)
assert np.array_equal(
np.round(vec2_1py - vec2_2py, DECIMALS_TO_ROUND),
np.round((NumCpp.Vec2_minusVec2(vec2_1cpp,
vec2_2cpp)).toNdArray().flatten(),
DECIMALS_TO_ROUND))
components = np.random.rand(2)
scaler = np.random.rand(1).item()
vec2py = vectormath.Vector2(*components)
vec2cpp = NumCpp.Vec2(*components)
assert np.array_equal(
np.round(vec2py - scaler, DECIMALS_TO_ROUND),
np.round((NumCpp.Vec2_minusVec2Scaler(vec2cpp,
scaler)).toNdArray().flatten(),
DECIMALS_TO_ROUND))
components = np.random.rand(2)
scaler = np.random.rand(1).item()
vec2py = vectormath.Vector2(*components)
vec2cpp = NumCpp.Vec2(*components)
assert np.array_equal(
np.round(-vec2py + scaler, DECIMALS_TO_ROUND),
np.round((NumCpp.Vec2_minusScalerVec2(vec2cpp,
scaler)).toNdArray().flatten(),
DECIMALS_TO_ROUND))
def __init__(self, i):
self.pos = vmath.Vector2(self.xc + rr(-500, 500), rr(720))
self.i = i
self.vel = vmath.Vector2(rr(-10, 10), rr(-10, 10)).as_unit()
self.swiz = [gd3.RED, gd3.GREEN, gd3.BLUE]
random.shuffle(self.swiz)
self.rgb = [(0x3e, 0x86, 0x6e)[i - gd3.RED] for i in self.swiz]
self.swiz += [gd3.ALPHA]
def test_Vec2_angle():
components1 = np.random.rand(2)
components2 = np.random.rand(2)
vec2_1py = vectormath.Vector2(*components1)
vec2_2py = vectormath.Vector2(*components2)
vec2_1cpp = NumCpp.Vec2(*components1)
vec2_2cpp = NumCpp.Vec2(*components2)
assert round(vec2_1py.angle(vec2_2py), DECIMALS_TO_ROUND) == \
round(vec2_1cpp.angle(vec2_2cpp), DECIMALS_TO_ROUND)
def _updateScaledTriangle(nTriangle):
if DEBUG:
print("Update scaled triangle", nTriangle)
t = m_vTriangles[nTriangle] # need to return
vDeformedV0 = m_vDeformedVerts[t.nVerts[0]]
vDeformedV1 = m_vDeformedVerts[t.nVerts[1]]
vDeformedV2 = m_vDeformedVerts[t.nVerts[2]]
vDeformed = [vDeformedV0[0], vDeformedV0[1], vDeformedV1[0],
vDeformedV1[1], vDeformedV2[0], vDeformedV2[1]]
if DEBUG:
print("t.mC:", t.mC)
print("t.mF:", t.mF)
# print("m_vInitialVerts:", m_vInitialVerts)
mCVec = np.matmul(t.mC, vDeformed)
vSolution = np.matmul(t.mF, mCVec)
vFitted0 = vmath.Vector2(float(vSolution[0]), float(vSolution[1]))
vFitted1 = vmath.Vector2(float(vSolution[2]), float(vSolution[3]))
x01, y01 = t.vTriCoords[0][0], t.vTriCoords[0][1]
vFitted01 = vFitted1 - vFitted0
vFitted01Perp = vmath.Vector2(vFitted01[1], -vFitted01[0])
vFitted2 = vFitted0 + float(x01) * vFitted01 + float(y01) * vFitted01Perp
vOrig0 = vmath.Vector2(float(m_vInitialVerts[t.nVerts[0]][0]), float(
m_vInitialVerts[t.nVerts[0]][1]))
vOrig1 = vmath.Vector2(float(m_vInitialVerts[t.nVerts[1]][0]), float(
m_vInitialVerts[t.nVerts[1]][1]))
fScale = (vOrig1 - vOrig0).length / vFitted01.length
# print("vOrig0:", vOrig0 )
# print("vOrig1:", vOrig1 )
# print("vFitted01:", vFitted01)
# print("length:", (vOrig1 - vOrig0).length, vFitted01.length)
# now scale !
# find center of mass
vCentroid = vFitted0 + vFitted1 + vFitted2 / 3.0
# convert to vectors, scale and restore
vFitted0 -= vCentroid
vFitted0 *= fScale
vFitted0 += vCentroid
vFitted1 -= vCentroid
vFitted1 *= fScale
vFitted1 += vCentroid
vFitted2 -= vCentroid
vFitted2 *= fScale
vFitted2 += vCentroid
t.vScaled[0], t.vScaled[1], t.vScaled[2] = vFitted0, vFitted1, vFitted2
# print("updated scales:", vFitted0, vFitted1, vFitted2)
return t
def test_Vec2_distance():
components1 = np.random.rand(2)
components2 = np.random.rand(2)
vec2_1py = vectormath.Vector2(*components1)
vec2_2py = vectormath.Vector2(*components2)
vec2_1cpp = NumCpp.Vec2(*components1)
vec2_2cpp = NumCpp.Vec2(*components2)
assert round((vec2_2py - vec2_1py).length, DECIMALS_TO_ROUND) == \
round(vec2_1cpp.distance(vec2_2cpp), DECIMALS_TO_ROUND)
def get_lines(self):
v1 = vmath.Vector2(15, -7.5)
v1.x, v1.y = rotate(v1.x, v1.y, -self.angle)
v2 = vmath.Vector2(-15, -7.5)
v2.x, v2.y = rotate(v2.x, v2.y, -self.angle)
v3 = vmath.Vector2(15, 7.5)
v3.x, v3.y = rotate(v3.x, v3.y, -self.angle)
v4 = vmath.Vector2(-15, 7.5)
v4.x, v4.y = rotate(v4.x, v4.y, -self.angle)
return [[self.x + v1.x, self.y + v1.y, self.x + v2.x, self.y + v2.y],
[self.x + v3.x, self.y + v3.y, self.x + v4.x, self.y + v4.y]]
def get_handpos(self, hand='right'):
forward = vmath.Vector2(cos(self.htheta), sin(self.htheta))
if hand == 'right':
hdir = vmath.Vector2(forward.y, -forward.x)
hdir = hdir / hypot(hdir.x, hdir.y)
hand = self.hpos + hdir * self.hlength
elif hand == 'left':
hdir = vmath.Vector2(-forward.y, forward.x)
hdir = hdir / hypot(hdir.x, hdir.y)
hand = self.hpos + hdir * self.hlength
return hand
def updateMesh(pMesh, bRigid):
global m_vConstraints
if DEBUG:
print("UpdateDeformedMesh(): constraint size =", len(m_vConstraints))
# 1. calculate deformed (**CORE *)
_validateDeformedMesh(bRigid)
# 2. depends on conditions, we should use either initial mesh information or deformed one
vVerts = []
if len(m_vConstraints) > 1:
# make vVerts = m_vDeformedVerts
for i in range(len(m_vDeformedVerts)):
vVerts.append(m_vDeformedVerts[i])
else:
# make vVerts = m_vInitialVerts
for i in range(len(m_vInitialVerts)):
vVerts.append(m_vInitialVerts[i])
# 3. fill the container of the caller
nVerts = len(pMesh.vertices)
for i in range(nVerts):
vNewPos = vmath.Vector2(float(vVerts[i][0]), float(vVerts[i][1]))
_SetVertex(pMesh, i, vNewPos)
return pMesh
def circle_collision(x, y, r=0.5):
x, y = int(math.floor(x)), int(math.floor(y))
if self.map.is_wall(x, y):
center = vmath.Vector2(x, y) + (0.5, 0.5)
if (agent.location - center).length < (r + width / 2):
return center + (agent.location -
center).as_length(r + width / 2)
def __init__(self, x0, y0, max_vel):
self.x = x0
self.y = y0
self.max_vel = max_vel
self.angle = 0
self.keys = dict(left=False, right=False, up=True, down=False)
self.vel = vmath.Vector2(0, 0)
def test_Vec2_normalize():
components = np.random.rand(2)
vec2py = vectormath.Vector2(*components)
vec2cpp = NumCpp.Vec2(*components)
assert np.array_equal(
np.round(vec2py.normalize(), DECIMALS_TO_ROUND),
np.round(vec2cpp.normalize().toNdArray().flatten(), DECIMALS_TO_ROUND))
def update(self):
'''update this games logic'''
if not super(ConcreteGame, self).update():
return False
self.target.position = vectormath.Vector2(pygame.mouse.get_pos())
for event in self._events:
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_s:
for i in self.gameobjects:
i.targetagent = self.target
i.Wander = False
i.Flee = False
i.Seek = True
print "agent at (%d, %d)" % i.position.vec
if event.key == pygame.K_f:
for i in self.gameobjects:
i.targetagent = self.target
i.Wander = False
i.Seek = False
i.Flee = True
print "agent at (%d, %d)" % i.position.vec
if event.key == pygame.K_w:
for i in self.gameobjects:
i.Seek = False
i.Flee = False
i.Wander = True
print "agent at (%d, %d)" % i.position.vec
for i in self.gameobjects:
i.update(self.deltatime)
print self.deltatime
return True
def test_Vec2_multiplication_operator():
components = np.random.rand(2)
scaler = np.random.rand(1).item()
vec2py = vectormath.Vector2(*components)
vec2cpp = NumCpp.Vec2(*components)
assert np.array_equal(np.round(vec2py * scaler, DECIMALS_TO_ROUND),
np.round((NumCpp.Vec2_multVec2Scaler(vec2cpp, scaler)).toNdArray().flatten(),
DECIMALS_TO_ROUND))
components = np.random.rand(2)
scaler = np.random.rand(1).item()
vec2py = vectormath.Vector2(*components)
vec2cpp = NumCpp.Vec2(*components)
assert np.array_equal(np.round(vec2py * scaler, DECIMALS_TO_ROUND),
np.round((NumCpp.Vec2_multScalerVec2(vec2cpp, scaler)).toNdArray().flatten(),
DECIMALS_TO_ROUND))
def make(self):
def color():
rgb = [rr(0xe0, 0xff), rr(0x80, 0xf0), rr(0x40, 0xf0)]
random.shuffle(rgb)
return rgb
colors = [color(), color()]
x = self.xc + rr(-400, 400)
age = ru(0, 0.2)
v = vmath.Vector2(ru(-1, 1), ru(-11, -13))
return [
Spark(
vmath.Vector2(x, 715) + rot().as_length(4),
v + rot().as_length(.1), colors[i // 60], age)
for i in range(120)
]
def test_Vec2_division_assignment_operator():
components1 = np.random.rand(2)
scaler = np.random.rand(1).item()
vec2_1py = vectormath.Vector2(*components1)
vec2_1cpp = NumCpp.Vec2(*components1)
vec2_1cpp /= scaler
assert np.array_equal(np.round(vec2_1py / scaler, DECIMALS_TO_ROUND),
np.round(vec2_1cpp.toNdArray().flatten(), DECIMALS_TO_ROUND))
def get_closet_dis(point, line):
a = point
b, c = line
t = (a[0] - b[0]) * (c[0] - b[0]) + (a[1] - b[1]) * (c[1] - b[1])
t = t / ((c[0] - b[0])**2 + (c[1] - b[1])**2)
a = vmath.Vector2(a[0], a[1])
b = vmath.Vector2(b[0], b[1])
c = vmath.Vector2(c[0], c[1])
if 0 < t < 1:
pcoords = vmath.Vector2(t * (c.x - b.x) + b.x, t * (c.y - b.y) + b.y)
dmin = get_dis(a, pcoords)
return pcoords, dmin
elif t <= 0:
return b, get_dis(a, b)
elif 1 <= t:
return c, get_dis(a, c)
def setup_patrol_route(self, pa):
self.patrol_route = [pa[0], pa[1], (pa[1][0], pa[0][1]), (pa[0][1], pa[1][0]), pa[0]]
self.patrol_route = [Position(vmath.Vector2(patrol_point)) for patrol_point in self.patrol_route]
print('Guard', self.ID, 'Patrolling guard, route:', self.patrol_route)
self.patrol_point = self.patrol_route[self.patrol_idx]
self.location = Position((pa[0][0] + np.abs(pa[0][0] - pa[1][0])*random.random(),
pa[0][1] + np.abs(pa[0][1] - pa[1][1])*random.random()))
def test_Vec2_subtraction_assignment_operator():
components1 = np.random.rand(2)
components2 = np.random.rand(2)
vec2_1py = vectormath.Vector2(*components1)
vec2_2py = vectormath.Vector2(*components2)
vec2_1cpp = NumCpp.Vec2(*components1)
vec2_2cpp = NumCpp.Vec2(*components2)
vec2_1cpp -= vec2_2cpp
assert np.array_equal(np.round(vec2_1py - vec2_2py, DECIMALS_TO_ROUND),
np.round(vec2_1cpp.toNdArray().flatten(), DECIMALS_TO_ROUND))
components1 = np.random.rand(2)
scaler = np.random.rand(1).item()
vec2_1py = vectormath.Vector2(*components1)
vec2_1cpp = NumCpp.Vec2(*components1)
vec2_1cpp -= scaler
assert np.array_equal(np.round(vec2_1py - scaler, DECIMALS_TO_ROUND),
np.round(vec2_1cpp.toNdArray().flatten(), DECIMALS_TO_ROUND))
def CreateClusterableGraph():
G = nx.Graph()
pivots = [
vmath.Vector2(0,0),
vmath.Vector2(0,10),
vmath.Vector2(10,0),
vmath.Vector2(10,10)
]
i = 1
for p in pivots:
for a in range(0, 4):
x = round(math.cos(math.pi*a * 0.5))
y = round(math.sin(math.pi*a * 0.5))
pos = p + vmath.Vector2(x,y)
G.add_node(i, pos = pos)
i = i + 1
return G
def draw(self, gd, ships, addr):
v = self.vel.as_unit()
a = int(256 * math.atan2(-v.x, v.y) / (2 * math.pi)) & 0xff
slot = addr + self.i * SZ
gd.cmd_flashread(slot, 8192 + SZ * a, SZ)
gd.Begin(gd3.BITMAPS)
gd.BitmapSource(slot)
gd.BitmapSwizzle(*self.swiz)
gd.Vertex2f(*(self.pos - vmath.Vector2(D, D)))
def test_vector2(self):
with self.assertRaises(TypeError):
properties.Vector2('bad len', length='ten')
with self.assertRaises(TypeError):
properties.Vector2('bad len', length=0)
with self.assertRaises(TypeError):
properties.Vector2('bad len', length=-0.5)
class HasVec2(properties.HasProperties):
vec2 = properties.Vector2('simple vector')
hv2 = HasVec2()
hv2.vec2 = [1., 2.]
assert isinstance(hv2.vec2, vmath.Vector2)
hv2.vec2 = 'east'
assert np.allclose(hv2.vec2, [1., 0.])
with self.assertRaises(ValueError):
hv2.vec2 = 'up'
with self.assertRaises(ValueError):
hv2.vec2 = [1., 2., 3.]
with self.assertRaises(ValueError):
hv2.vec2 = [[1., 2.]]
class HasLenVec2(properties.HasProperties):
vec2 = properties.Vector2('length 5 vector', length=5)
hv2 = HasLenVec2()
hv2.vec2 = [0., 1.]
assert np.allclose(hv2.vec2, [0., 5.])
assert isinstance(properties.Vector2.from_json([5., 6.]),
vmath.Vector2)
with self.assertRaises(ZeroDivisionError):
hv2.vec2 = [0., 0.]
assert properties.Vector2('').equal(vmath.Vector2(1., 2.),
vmath.Vector2(1., 2.))
assert not properties.Vector2('').equal(vmath.Vector2(1., 2.),
np.array([1., 2.]))
def reset(self):
self.car.x = (self.lines[self.lines_len - 1][0] +
self.lines[self.lines_len * 2 - 1][0]) / 2
self.car.y = (self.lines[self.lines_len - 1][1] +
self.lines[self.lines_len * 2 - 1][1]) / 2
self.car.angle = np.arctan((self.lines[self.lines_len - 1][3] -
self.lines[self.lines_len - 1][1]) /
(self.lines[self.lines_len - 1][2] -
self.lines[self.lines_len - 1][0]))
self.car.vel = vmath.Vector2(0, 0)
self.reward_lines = self.reward_lines_backup.copy()
self.reward_amount = self.reward_amount_backup
def perceived_angle(self):
"""
Calculates the perceived angle towards the noise from the perspective of the `target_pos`
This also adds the uncertainty as described in the booklet
"""
diff = self._noise.location - self._observer.location
if diff.length > 1e-5:
angle = vmath.Vector2(0, 1).angle(diff, unit='deg')
true_angle = angle if diff.x > 0 else -angle
else:
true_angle = 0
uncertainty = 10
return random.gauss(true_angle, uncertainty)
def _validateDeformedMesh(bRigid):
# global m_vTriangles, m_vDeformedVerts, m_vConstraints
if DEBUG:
print("validateDeformedMesh()........................")
nConstraints = len(m_vConstraints)
if nConstraints < 2:
return
# if not precomputed, compute the static matrix
validateSetup()
# compute the deformed Mesh
vQ = np.zeros(nConstraints * 2, dtype=float)
k = 0
for i in range(len(m_vConstraints)):
c = m_vConstraints[i]
vQ[k * 2] = c.vConstrainedPos[0]
vQ[k * 2 + 1] = c.vConstrainedPos[1]
# print("c: k =", k, ", n =", c.nVertex, " ", c.vConstrainedPos[0], " ", c.vConstrainedPos[1])
x, y = vQ[k * 2], vQ[k * 2 + 1]
# print("vQ: k = ", k, ", ", x, " ", y)
k += 1
# step 1: computation using m_mFirstMatrix (G) precomputed in eqn (8)
vU = np.matmul(m_mFirstMatrix, vQ)
# print("vU", vU)
nVerts = len(m_vDeformedVerts)
for i in range(nVerts):
c = Constraint(i, [0.0, 0.0])
if m_vConstraints.count(c) > 0:
continue
nRow = m_vVertexMap[i]
fX, fY = vU[nRow * 2], vU[nRow * 2 + 1]
m_vDeformedVerts[i] = vmath.Vector2(float(fX), float(fY))
if DEBUG:
print("rigid: ", bRigid)
# step 2: scale triangles
if bRigid:
nTris = len(m_vTriangles)
for i in range(nTris):
m_vTriangles[i] = _updateScaledTriangle(i)
_applyFittingStep() # bug fix
def test_coerce(self):
class HasNoCoerceArray(properties.HasProperties):
arr = properties.Array('', coerce=False)
vec2 = properties.Vector2('', coerce=False)
@properties.Array('', coerce=False)
def dynamic_arr(self):
if getattr(self, '_dynamic_arr', None) is None:
self._dynamic_arr = np.ones(5)
return self._dynamic_arr
no_coerce = HasNoCoerceArray()
no_coerce.arr = np.array([1, 2, 3])
with self.assertRaises(properties.ValidationError):
no_coerce.arr = [1, 2, 3]
assert no_coerce.dynamic_arr[0] == 1
no_coerce.dynamic_arr[0] = 0
assert no_coerce.dynamic_arr[0] == 0
no_coerce.vec2 = vmath.Vector2(0., 0.)
with self.assertRaises(properties.ValidationError):
no_coerce.vec2 = [0., 0.]
def step(self, action):
self.input(action)
reward = -0.03
# if action == 2:
# reward = -0.1
done = False
self.car.move(self.acceleration, self.ang_increment)
self.car.update(self.drag)
color = (150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150, 150)
c_lines = self.car.get_lines()
intersect_test = False
for lin in self.lines:
if len(lin) == 4:
ta0 = ((lin[1] - lin[3]) * (c_lines[0][0] - lin[0]) +
(lin[2] - lin[0]) * (c_lines[0][1] - lin[1])) / (
(lin[2] - lin[0]) *
(c_lines[0][1] - c_lines[0][3]) -
(c_lines[0][0] - c_lines[0][2]) * (lin[3] - lin[1]))
tb0 = ((c_lines[0][1] - c_lines[0][3]) *
(c_lines[0][0] - lin[0]) +
(c_lines[0][2] - c_lines[0][0]) *
(c_lines[0][1] - lin[1])) / (
(lin[2] - lin[0]) *
(c_lines[0][1] - c_lines[0][3]) -
(c_lines[0][0] - c_lines[0][2]) * (lin[3] - lin[1]))
ta1 = ((lin[1] - lin[3]) * (c_lines[1][0] - lin[0]) +
(lin[2] - lin[0]) * (c_lines[1][1] - lin[1])) / (
(lin[2] - lin[0]) *
(c_lines[1][1] - c_lines[1][3]) -
(c_lines[1][0] - c_lines[1][2]) * (lin[3] - lin[1]))
tb1 = ((c_lines[1][1] - c_lines[1][3]) *
(c_lines[1][0] - lin[0]) +
(c_lines[1][2] - c_lines[1][0]) *
(c_lines[1][1] - lin[1])) / (
(lin[2] - lin[0]) *
(c_lines[1][1] - c_lines[1][3]) -
(c_lines[1][0] - c_lines[1][2]) * (lin[3] - lin[1]))
if (ta0 >= 0 and ta0 <= 1 and tb0 >= 0
and tb0 <= 1) or (ta1 >= 0 and ta1 <= 1 and tb1 >= 0
and tb1 <= 1):
intersect_test = True
if intersect_test:
done = True
reward = -1
intersect_test = False
if len(self.reward_lines) == 0:
self.reward_lines = self.reward_lines_backup.copy()
lin = self.reward_lines[0]
if len(lin) == 4:
ta0 = ((lin[1] - lin[3]) * (c_lines[0][0] - lin[0]) +
(lin[2] - lin[0]) * (c_lines[0][1] - lin[1])) / (
(lin[2] - lin[0]) * (c_lines[0][1] - c_lines[0][3]) -
(c_lines[0][0] - c_lines[0][2]) * (lin[3] - lin[1]))
tb0 = ((c_lines[0][1] - c_lines[0][3]) * (c_lines[0][0] - lin[0]) +
(c_lines[0][2] - c_lines[0][0]) *
(c_lines[0][1] - lin[1])) / (
(lin[2] - lin[0]) * (c_lines[0][1] - c_lines[0][3]) -
(c_lines[0][0] - c_lines[0][2]) * (lin[3] - lin[1]))
ta1 = ((lin[1] - lin[3]) * (c_lines[1][0] - lin[0]) +
(lin[2] - lin[0]) * (c_lines[1][1] - lin[1])) / (
(lin[2] - lin[0]) * (c_lines[1][1] - c_lines[1][3]) -
(c_lines[1][0] - c_lines[1][2]) * (lin[3] - lin[1]))
tb1 = ((c_lines[1][1] - c_lines[1][3]) * (c_lines[1][0] - lin[0]) +
(c_lines[1][2] - c_lines[1][0]) *
(c_lines[1][1] - lin[1])) / (
(lin[2] - lin[0]) * (c_lines[1][1] - c_lines[1][3]) -
(c_lines[1][0] - c_lines[1][2]) * (lin[3] - lin[1]))
if (ta0 >= 0 and ta0 <= 1 and tb0 >= 0
and tb0 <= 1) or (ta1 >= 0 and ta1 <= 1 and tb1 >= 0
and tb1 <= 1):
intersect_test = lin
if intersect_test != False:
self.reward_lines.remove(intersect_test)
reward = 2
self.reward_amount = self.reward_amount_backup
#print(reward, len(self.reward_lines_backup), len(self.reward_lines), self.reward_amount_backup)
else:
self.reward_amount *= self.reward_gradient
distances = []
for ang in self.vision_angles:
vect = vmath.Vector2(self.view_range, 0)
vect.x, vect.y = rotate(vect.x, vect.y, ang - self.car.angle)
c_line = [
self.car.x, self.car.y, self.car.x + vect.x,
self.car.y + vect.y
]
min_dist = self.view_range
for lin in self.lines:
ta = ((lin[1] - lin[3]) * (c_line[0] - lin[0]) +
(lin[2] - lin[0]) * (c_line[1] - lin[1])) / (
(lin[2] - lin[0]) * (c_line[1] - c_line[3] + 1e-10) -
(c_line[0] - c_line[2]) * (lin[3] - lin[1]))
tb = ((c_line[1] - c_line[3]) * (c_line[0] - lin[0]) +
(c_line[2] - c_line[0]) * (c_line[1] - lin[1])) / (
(lin[2] - lin[0]) * (c_line[1] - c_line[3] + 1e-10) -
(c_line[0] - c_line[2]) * (lin[3] - lin[1]))
if ta >= 0 and ta <= 1 and tb >= 0 and tb <= 1 and self.view_range * ta < min_dist:
min_dist = self.view_range * ta
distances.append(min_dist)
obs = np.zeros((0, 0))
for dist in distances:
obs = np.append(obs, mapp(dist, 0, self.view_range, 0, 1))
obs = np.append(obs,
mapp(self.car.vel.length, 0, self.car.max_vel, 0, 1))
obs = np.reshape(obs, (1, 11))
return obs, reward, done
def test_Vec2_norm():
components = np.random.rand(2)
vec2py = vectormath.Vector2(*components)
vec2cpp = NumCpp.Vec2(*components)
assert round(vec2py.length, DECIMALS_TO_ROUND) == \
round(vec2cpp.norm(), DECIMALS_TO_ROUND)
def main():
dataset = 2
totaldata, boundary = loadsalsa(isprint=False, dataset=dataset)
trajectorydata = readextractedpaths(dataset=dataset)
N = 200
obstacles1 = [(8, 1), (8, 2.5), (8.5, 2.5), (8.5, 1), (8, 1)]
obstacles2 = [(8, 3), (8, 4.5), (8.5, 4.5), (8.5, 3), (8, 3)]
obstacles3 = [(9.2, 6.9), (10.2, 6.9), (10.2, 6.7), (9.2, 6.7), (9.2, 6.9)]
obstacles4 = [(10.8, 6.9), (11.8, 6.9), (11.8, 6.7), (10.8, 6.7),
(10.8, 6.9)]
obstacles5 = [(12.4, 6.9), (13.4, 6.9), (13.4, 6.7), (12.4, 6.7),
(12.4, 6.9)]
obstacles6 = [(12.5, 1.1), (12.4, 1.2), (13.1, 1.9), (13.2, 1.8),
(12.5, 1.1)]
newboundary = [
boundary[2] - 1, boundary[3] - 1, boundary[0] + 1, boundary[1] + 1
]
obstacles = [
obstacles1, obstacles2, obstacles3, obstacles4, obstacles5, obstacles6
]
wall1 = Wall(obstacles1, newboundary)
wall2 = Wall(obstacles2, newboundary)
wall3 = Wall(obstacles3, newboundary)
wall4 = Wall(obstacles4, newboundary)
wall5 = Wall(obstacles5, newboundary)
wall6 = Wall(obstacles6, newboundary)
x, y = np.meshgrid(np.linspace(boundary[2] - 1, boundary[0] + 1, N),
np.linspace(boundary[3] - 1, boundary[1] + 1, N))
dx = min((boundary[0] - boundary[2] + 2) / (N - 1),
(boundary[1] - boundary[3] + 2) / (N - 1))
size = x.shape[0]
zwall = [[0.0] * size for i in range(size)]
mask = np.zeros_like(zwall)
for ix in range(size):
for iy in range(size):
posx = x[0][ix]
posy = y[iy][0]
if wall1.test_point_in_wall((posx, posy)) == 0 and wall2.test_point_in_wall((posx, posy)) == 0 and \
wall3.test_point_in_wall((posx, posy)) == 0 and wall4.test_point_in_wall((posx, posy)) == 0 and \
wall5.test_point_in_wall((posx, posy)) == 0 and wall6.test_point_in_wall((posx, posy)) == 0 and \
ix != 0 and ix != size - 1 and iy != 0 and iy != size - 1:
zwall[ix][iy] = max(
wall1.get_wall_interaction((posx, posy)),
wall2.get_wall_interaction((posx, posy)),
wall3.get_wall_interaction((posx, posy)),
wall4.get_wall_interaction((posx, posy)),
wall5.get_wall_interaction((posx, posy)),
wall6.get_wall_interaction((posx, posy)),
wall1.get_boundary_interaction((posx, posy)),
wall2.get_boundary_interaction((posx, posy)),
wall3.get_boundary_interaction((posx, posy)),
wall4.get_boundary_interaction((posx, posy)),
wall5.get_boundary_interaction((posx, posy)),
wall6.get_boundary_interaction((posx, posy)))
else:
mask[ix][iy] = 1
totalSdis = []
for id in range(len(trajectorydata)):
itrajectory = trajectorydata[id]
startframenum = int(itrajectory[1])
endframenum = int(itrajectory[2])
selectedagentid = itrajectory[0]
totalframes = int(endframenum - startframenum)
newstart = [0, 0]
target = [0, 0]
Groundtruthx = []
Groundtruthy = []
StraveledPathx = []
StraveledPathy = []
Sdis = 0
if totalframes == 1:
continue
for iframe in range(totalframes):
z = [[0.0] * size for i in range(size)]
z = np.array(z)
humans = []
startframe = startframenum * 3 + 0.2
endframe = endframenum * 3 + 0.2
nextframepos = [0, 0]
traveledlen = 0
grouplist = []
for ind, idata in enumerate(totaldata):
for k in range(len(idata)):
if startframe == idata[k][0]:
if selectedagentid != ind:
pos = vmath.Vector2(idata[k][1], idata[k][2])
angle = idata[k][4]
grouplist.append(idata[k][-1])
human = Human(pos, angle)
humans.append(human)
else:
newstart = [idata[k][1], idata[k][2]]
if iframe == 0:
Groundtruthx.append(newstart[0])
Groundtruthy.append(newstart[1])
StraveledPathx.append(newstart[0])
StraveledPathy.append(newstart[1])
nextframepos = [idata[k + 1][1], idata[k + 1][2]]
Groundtruthx.append(nextframepos[0])
Groundtruthy.append(nextframepos[1])
traveledlen = math.sqrt(
(newstart[0] - nextframepos[0])**2 +
(newstart[1] - nextframepos[1])**2)
if endframe == idata[k][0] and selectedagentid == ind:
target = [idata[k][1], idata[k][2]]
break
if iframe == 0:
Snewstart = [newstart[0], newstart[1]]
else:
Snewstart = [StraveledPathx[-1], StraveledPathy[-1]]
target = (target[0], target[1])
groups = []
groupslist = get_group_index(grouplist)
for igroup in groupslist:
if len(igroup) > 1:
group = [humans[ind] for ind in igroup]
groupinst = Group(group, stride=0.2)
groups.append(groupinst)
for ix in range(size):
for iy in range(size):
posx = x[0][ix]
posy = y[iy][0]
newpos = vmath.Vector2(posx, posy)
if mask[ix][iy] == 1:
z[ix, iy] = 0
else:
humanvalue = [
ihuman.basic_personal_space(newpos)
for ihuman in humans
]
z[ix, iy] = max(humanvalue)
groupvalue = [
igroup.group_interaction(newpos)
for igroup in groups
]
z[ix, iy] = max(max(humanvalue), max(groupvalue),
zwall[ix][iy])
z = np.transpose(z)
mask = np.transpose(mask)
maxvalue = max(max(x) for x in z)
r = 0.1
phi = (x - Snewstart[0])**2 + (y - Snewstart[1])**2 - r**2
phi = np.ma.MaskedArray(phi, mask)
speed = np.zeros_like(x)
for ix in range(size):
for iy in range(size):
speed[ix][iy] = (maxvalue - z[ix][iy]) / maxvalue * 1.4
try:
t = skfmm.travel_time(phi, speed, dx, order=1)
except:
mask = np.transpose(mask)
print("no zero contour")
break
px, py = minimal_path(t,
target,
dx,
boundary=newboundary,
steps=N,
N=1000)
pxy = []
newpxy = []
for ind, ix in enumerate(px):
pxy.append([ix, py[ind]])
for ixy in pxy:
if ixy not in newpxy:
newpxy.append(ixy)
px = [ixy[0] for ixy in newpxy]
py = [ixy[1] for ixy in newpxy]
px.reverse()
py.reverse()
Straveledlen = 0
Sdiffx = np.zeros_like(px)
Sdiffy = np.zeros_like(py)
Sdiffx[1:] = np.asarray(px[1:]) - np.asarray(px[:-1])
Sdiffy[1:] = np.asarray(py[1:]) - np.asarray(py[:-1])
Straveli = 1
while Straveledlen < traveledlen and Straveli < len(Sdiffx):
lens = math.sqrt((Sdiffx[Straveli])**2 + (Sdiffy[Straveli])**2)
Straveledlen += lens
Straveli += 1
px = px[:Straveli - 1]
py = py[:Straveli - 1]
StraveledPathx += px[1:]
StraveledPathy += py[1:]
if iframe != 0 and iframe != totalframes - 1:
if len(px) != 0:
Sdis += math.sqrt((nextframepos[0] - px[-1])**2 +
(nextframepos[1] - py[-1])**2)
z_min, z_max = -np.abs(z).max(), np.abs(z).max()
fig, ax = plt.subplots()
plt.axis('equal')
c = ax.contourf(x, y, z, cmap='RdBu', vmin=z_min, vmax=z_max)
# set the limits of the plot to the limits of the data
ax.axis([x.min(), x.max(), y.min(), y.max()])
fig.colorbar(c, ax=ax)
for obs in obstacles:
obstaclesx, obstaclesy = Polygon(obs).exterior.xy
ax.plot(obstaclesx, obstaclesy, color='blue')
plt.plot(Groundtruthx[0], Groundtruthy[0], 'oc')
plt.plot(target[0], target[1], 'om')
plt.plot(StraveledPathx, StraveledPathy, '-r', linewidth=2)
plt.plot(Groundtruthx, Groundtruthy, '-g', linewidth=2)
plt.savefig('figures/salsa' + str(dataset) + 'paths/' + str(id) +
'_' + str(startframenum) + str(endframenum) +
'simulated.png')
plt.close()
startframenum += 1
mask = np.transpose(mask)
totalSdis.append(Sdis)
with open(Sdissavepath, 'a', newline='') as Scsvfile:
writer = csv.writer(Scsvfile)
towrite = [Sdis]
writer.writerow(towrite)
Scsvfile.close()
print('Social-aware mean:', statistics.mean(totalSdis), 'Social-aware SD',
statistics.stdev(totalSdis))
def accelerate(self, acc):
self.vel = self.vel.__add__(
vmath.Vector2(acc * np.cos(self.angle), acc * np.sin(self.angle)))
if self.vel.length > self.max_vel:
self.vel = self.vel.as_length(self.max_vel)
from shapely.geometry import Point
# Initialize key variables
# Props to @user13993 and @Nisan.H on stackoverflow.com for this elegant answer. Obtains the working directory of the
# script upon runtime
working_dir = os.path.dirname(os.path.realpath(__file__))
# Origin is located in top-left corner
origin = {'X': 5661358, 'Y': 19628008}
len_x = 8000
len_y = 8000
num_nodes = 1500
min_dist = 160
outfile = 'refinement.txt'
# Input to feed point generating function
v1 = vmath.Vector2(8000, 0)
v2 = vmath.Vector2(0, 8000)
points = pandas.DataFrame()
node = 1
while len(points) < num_nodes:
a1 = random.random()
a2 = random.random()
point = a1 * v1 + a2 * v2
# Verify that the minimum distance is honored by the new point
if len(points) == 0:
points = pandas.DataFrame.from_dict(
{str(node):
[origin['X'] + point.x,
origin['Y'] - point.y,
Point(point.x, point.y)]
