def move(self):
logger.info("[ActiveCar]: move car {0}".format(self.ID))
logger.info(" Route Length: " + str(len(self.Route)) +
" Next node: " + str(self.NextNodeIdx))
self.Position = Vector3(self.Position.X + self.Velocity.X,
self.Position.Y + self.Velocity.Y,
self.Position.Z + self.Velocity.Z)
if (self.Position.X >= self.Route[self.NextNodeIdx - 1][0]
and self.Position.X >= self.Route[self.NextNodeIdx][0]) or (
self.Position.X < self.Route[self.NextNodeIdx - 1][0]
and self.Position.X < self.Route[self.NextNodeIdx][0]):
self.Position = Vector3(self.Route[self.NextNodeIdx][0],
self.Route[self.NextNodeIdx][1], 40.5)
if self.NextNodeIdx + 1 == len(self.Route):
self.stop()
else:
self.NextNodeIdx += 1
dx = self.Route[self.NextNodeIdx][0] - self.Route[
self.NextNodeIdx - 1][0]
dy = self.Route[self.NextNodeIdx][1] - self.Route[
self.NextNodeIdx - 1][1]
dis = math.sqrt(dx * dx + dy * dy)
self.Velocity = Vector3(dx / dis * self.Speed,
dy / dis * self.Speed, 0)
logger.debug(
"[ActiveCar]: Current velocity: {0}, New position {1}".format(
self.Velocity, self.Position))
def __init__(self, uid=None):
self.Position = Vector3(0, 0, 40.5)
self.Velocity = Vector3(0, 0, 0)
self.Speed = 0
# self.Length = 0
# self.Width = 0
self.Name = "ArchitCars"
def __init__(self):
self.ID = str(uuid.uuid4())
self.Position = Vector3(0, 0, 40.5)
self.Velocity = Vector3(0, 0, 0)
self.Length = 0
self.Width = 0
self.Name = ""
def get_coordinates(self):
if self.Oneway == True:
return (self.Source.Location, self.Destination.Location)
elif hasattr(self, "_coordinates"):
return self._coordinates
else:
import numpy as np
(x0, y0) = self.Source.Location.X, self.Source.Location.Y
(x1, y1) = self.Destination.Location.X, self.Destination.Location.Y
# normalize and multiply by offset
vec = np.array((x1 - x0, y1 - y0))
norm_vec = np.linalg.norm(vec)
normal_vec = vec / norm_vec
# offset normal ortogonal of vector
ovec = np.array((normal_vec[1], -normal_vec[0])) * self._offset
source_coord = ovec + (x0, y0)
parallel_coord = (normal_vec * norm_vec) + source_coord
self._coordinates = (Vector3(source_coord[0], source_coord[1], 0),
Vector3(parallel_coord[0], parallel_coord[1],
0))
#print self._coordinates
return self._coordinates
def move(self):
r = self.Route
l = len(r)
cr = self.CurrentRoute
p = self.Position
v = self.Velocity
if l > cr > 0:
if p.X == r[l - 1]['X'] and p.Y == r[l - 1]['Y']:
self.stop()
else:
# Distance between current position and end of route
d = math.hypot(r[cr]['X'] - p.X, r[cr]['Y'] - p.Y)
if d > 0:
# Position after moving
fx = p.X + v.X
fy = p.Y + v.Y
fd = math.hypot(fx - p.X, fy - p.Y)
if fd < d:
if not (p.X == self.PrevPosition.X
and p.Y == self.PrevPosition.Y
and p.Z == self.PrevPosition.Z):
self.PrevPosition = Vector3(p.X, p.Y, p.Z)
self.Position = Vector3(fx, fy, p.Z)
else:
if not (p.X == self.PrevPosition.X
and p.Y == self.PrevPosition.Y
and p.Z == self.PrevPosition.Z):
self.PrevPosition = Vector3(p.X, p.Y, p.Z)
self.Position = Vector3(r[cr]['X'], r[cr]['Y'], p.Z)
self.CurrentRoute += 1
def stop(self):
self.Velocity = Vector3(0, 0, 0)
self.TopSpeed = 0
self.Position = Vector3(0, 0, 0)
self.PrevPosition = Vector3(0, 0, 0)
self.CurrentRoute = 0
self.Route = []
def update(self):
cars = self.frame.get(Vehicle)
for c in cars:
if self.step % 5:
px = (c.Velocity.X * math.cos(self.rotation) -
c.Velocity.Y * math.sin(self.rotation))
py = (c.Velocity.X * math.sin(self.rotation) +
c.Velocity.Y * math.cos(self.rotation))
c.Velocity = Vector3(px, py, c.Velocity.Z)
c.Position += c.Velocity * self.timestep
if self.step % 40 == 0:
# First route: from a random place to another random place
v = Vehicle()
v.Position = Vector3(500, 500, 25.5)
v.Velocity = Vector3(10, 10, 0)
v.Name = "[testcar] Car %s" % self.step
v.Lenght = 5
v.Width = 2
self.frame.add(v)
self.todelete.append(v.ID)
if len(self.todelete) > 360 / self.rotating_degree:
car = self.frame.get(Vehicle, self.todelete[0])
self.frame.delete(Vehicle, car)
del self.todelete[0]
self.step += 1
def updateVelocity(self):
#if(self.route is not None):
if(len(self._Waypoints) == 1):
self.numRoutesFinished += 1
logger.debug("Route Finished after {0} Ticks".format(self.ticks))
self.ticks = 0
self._Velocity = Vector3(0,0,0)
else:
d = 0.5
if((self._Waypoints[1]['Y'] - self._Waypoints[0]['Y']) == 0):
m = -1 #check this later, just to avoid division by 0
else:
m = (self._Waypoints[1]['X'] - self._Waypoints[0]['X'])/(self._Waypoints[1]['Y'] - self._Waypoints[0]['Y'])*-1
#x0 = math.sqrt(math.pow(d,2)/(1+math.pow(m,2))) + self._Waypoints[0]['X']
#x1 = math.sqrt(math.pow(d,2)/(1+math.pow(m,2))) + self._Waypoints[1]['X']
#y0 = m*(x0 - self._Waypoints[0]['X']) + self._Waypoints[0]['Y']
#y1 = m*(x1 - self._Waypoints[1]['X']) + self._Waypoints[1]['Y']
#temp = Vector3(x1 - x0, y1 - y0, self._Waypoints[1]['Z'] - self._Waypoints[0]['Z'])
temp = Vector3(self._Waypoints[1]['X'] - self._Waypoints[0]['X'], self._Waypoints[1]['Y'] - self._Waypoints[0]['Y'], self._Waypoints[1]['Z'] - self._Waypoints[0]['Z'])
#print("Original: X = {0}, Y = {1}; Novo: X = {2}, Y = {3}".format(self._Waypoints[0]['X'], self._Waypoints[0]['Y'], x0, y0))
#print("Original: X = {0}, Y = {1}; Novo: X = {2}, Y = {3}".format(self._Waypoints[1]['X'], self._Waypoints[1]['Y'], x1, y1))
module = math.sqrt(math.pow(temp.X, 2) + math.pow(temp.Y, 2) + math.pow(temp.Z, 2))
if(module != 0):
temp.X = (temp.X / module) * self.SPEED
temp.Y = (temp.Y / module) * self.SPEED
temp.Z = (temp.Z / module) * self.SPEED
else:
temp.X = 0
temp.Y = 0
temp.Z = 0
self._Velocity = temp
self._Waypoints.pop(0)
self._ticksToTarget = self.TicksToNextTarget()
def __init__(self):
self.Name = ""
self.Position = Vector3(0, 0, 0)
self.Route = ""
self.Target = ""
self.Velocity = Vector3(0, 0, 0)
self.Rotation = Quaternion(0, 0, 0, 0)
self.Type = ""
def __init__(self, uid=None):
self.ID = uid
self.Velocity = Vector3(0, 0, 0)
self.Name = "uberCar"
self.Position = Vector3(0, 0, 40.5)
self.Velocity = Vector3(0, 0, 0)
self.Length = 10
self.Width = 10
def __init__(self, uid = None):
self.ID = uid
self._Length = 30
self._Width = 10
self._ticksToTarget = 0
self._Velocity = Vector3(0,0,0)
self._Position = Vector3(0, 0, 40.5)
logger.debug("%s car created", LOG_HEADER)
def __init__(self, uid=None):
self.ID = uid
self.Position = Vector3(0, 0, 0)
self.Velocity = Vector3(0, 0, 0)
self.Length = 0
self.Width = 0
self.Name = ""
self.Aflag = True
self.Waypoints = []
self.FinalPosition = {}
def assign(self, route):
logger.debug("[FreeWPCar]: {0} assign route {1}".format(
self.ID, shorten_str(route)))
self.Route = list(route)
self.RouteSection = 0
self.IsAssigned = True
self.Position = Vector3(*route[0])
self.Velocity = Vector3(0.0, 0.0, 0.0)
self.LastTickTime = time.time()
self.AccelMode == "Cruising"
def __predicate__(c1, c2):
# pass
if c2.Position != Vector3(0, 0, 40.5) and c1.Position != Vector3(
0, 0, 40.5) and c1.ID != c2.ID:
if ((c2.Position.X < c1.Position.X + (2 * c1.Speed)) and
(c2.Position.X > c1.Position.X)) and (
(c2.Position.Y < c1.Position.Y +
(2 * c1.Speed)) and (c2.Position.Y > c1.Position.Y)):
return True
return False
def start(self, route, top_speed):
logger.debug("[InactiveCar]: {0} starting".format(self.ID))
self.TopSpeed = top_speed # top speed will be constant, speed will keep changing
self.Speed = top_speed
self.Velocity = Vector3(0, 0, 0)
# self.Position = Vector3(start_x, start_y, 0)
self.Route = route
# print "OUTPUT", type(self.Route)
# print "Contents", self.Route
self.Position = Vector3(self.Route[0]['X'], self.Route[0]['Y'], 40.5)
logger.debug(
"[InactiveCar][{0}]: Starting, New position {2}, Current velocity: {1}"
.format(self.ID, self.Velocity.X, self.Position))
self.CurrentSegment = 1
def start(self, route):
self.Route = route
dx = route[1][0] - route[0][0]
dy = route[1][1] - route[0][1]
if dx == 0 and dy == 0:
dx = route[2][0] - route[1][0]
dy = route[2][1] - route[1][1]
self.NextNodeIdx += 1
dis = math.sqrt(dx * dx + dy * dy)
self.Velocity = Vector3(dx / dis * self.Speed, dy / dis * self.Speed, 0)
self.Position = Vector3(route[0][0], route[0][1], 40.5)
self.NextNodeIdx += 1
logger.info("[InactiveCar]: {0} starting".format(self.ID))
logger.info("velocity: ")
logger.info(self.Velocity)
def move(self, node1, node2):
# by Jilin, based on road line from node1(x1, y1) to node2(x2, y2): y = ax + b,
node2 = [node2['X'], node2['Y']]
node1 = [node1['X'], node1['Y']]
a = (node2[1] - node1[1])/(node2[0] - node1[0])
b = 1
alpha = 1.0 # type int and float
if node2[0] > node1[0]:
newPositionX = self.Position.X + alpha * self.SPEED / math.sqrt(pow(a, 2) + 1)
if newPositionX > node2[0]:
newPositionX = node2[0]
elif node2[0] < node1[0]:
newPositionX = self.Position.X - alpha * self.SPEED/ math.sqrt(pow(a, 2) + 1)
if newPositionX < node2[0]:
newPositionX = node2[0]
else:
newPositionX = self.Position.X
if node2[1] > node1[1]:
newPositionY = self.Position.Y + abs(a) * alpha * self.SPEED / math.sqrt(pow(a, 2) + 1)
if newPositionY > node2[1]:
newPositionY = node2[1]
elif node2[1] < node1[1]:
newPositionY = self.Position.Y - abs(a) * alpha * self.SPEED / math.sqrt(pow(a, 2) + 1)
if newPositionY < node2[1]:
newPositionY = node2[1]
else:
newPositionY = self.Position.Y
self.Position = Vector3(newPositionX, newPositionY, 0)
# logger.debug("[ActiveCar] {2}: Current velocity: {0}, New position {1}".format(self.Velocity, self.Position, self.ID))
# End of ride
if (self.Position.X == self.FinalPosition[0]) or (self.Position.Y == self.FinalPosition[1]):
self.stop()
def HandleDeleteObjectEvent(self):
"""Handle the delete object event. In this case, rather than delete the
object from the scene completely, mothball it in a location well away from
the simulation.
"""
deleted = self.frame.get_deleted(MobdatVehicle)
for car in deleted:
vname = car.Name
self.__Logger.info("deleting car %s from OpenSim", vname)
if vname not in self.Vehicles2Sim:
self.__Logger.warn("attempt to delete unknown vehicle %s" %
(vname))
continue
sim = self.Vehicles2Sim[vname]
vehicle = sim["Vehicles"][car.Name]
sim["VehicleReuseList"][vehicle.VehicleType].append(vehicle)
mothball = OpenSimVehicleDynamics()
mothball.Position = Vector3(10.0, 10.0, 500.0)
mothball.UpdateTime = self.CurrentTime
vehicle.TweenUpdate = mothball
vehicle.LastUpdate = mothball
vehicle.InUpdateQueue = True
self.WorkQ.put([vehicle.VehicleName])
return True
def move(self):
self.Position = Vector3(self.Position.X - self.Velocity.X,
self.Position.Y + self.Velocity.Y,
self.Position.Z + self.Velocity.Z)
# logger.debug("[ActiveCar] {2}: Current velocity: {0}, New position {1}".format(self.Velocity, self.Position, self.ID))
# End of ride
if (self.Position.X <= self.FINAL_POSITION):
self.stop()
def start(self, route=[], v=10):
r = route
self.TopSpeed = v
self.Route = r
# Calculating position and velocity vector according to route points
if len(route) > 1:
self.CurrentRoute += 1
self.Position = Vector3(r[0]['X'], r[0]['Y'], self.Position.Z)
ddash = math.hypot(self.Position.X - r[1]['X'],
self.Position.Y - r[1]['Y'])
self.Velocity = Vector3(
(v / ddash) * (r[1]['X'] - self.Position.X),
(v / ddash) * (r[1]['Y'] - self.Position.Y), 0)
else:
self.Position = Vector3(0, 0, self.Position.Z)
self.Velocity = Vector3(0, 0, 0)
def move(self):
'''code with individual steps calculation'''
# end coordinates of the line segment
end = self.Route[self.CurrentSegment]
x2, y2 = end['X'], end['Y']
x, y, velx, vely = self.get_next_point(self.Position.X,
self.Position.Y, x2, y2,
self.Speed)
# logger.debug("velx:{0} vely:{1}".format(velx, vely))
if (x > x2 or y > y2):
''' We have exceeded the endpoint of this line segment
setting the position of the car as the end of line segment
increment the count so that we get endpoints for the next line segment
'''
x, y = x2, y2
self.CurrentSegment += 1
self.Position = Vector3(x, y, self.Position.Z)
self.Velocity = Vector3(velx, vely, 0)
logger.debug(
"[ActiveCar][{0}]: Speed: {1}, New position {3}, Current velocity: {2}"
.format(self.ID, self.Speed, self.Velocity, self.Position))
if self.Speed < self.TopSpeed:
self.Speed += round(0.34 * self.TopSpeed, 2)
# self.Speed = Vector3(self.Velocity.X, 0, 0)
if self.Speed > self.TopSpeed:
self.Speed = self.TopSpeed
lastx = self.Route[-1]['X']
lasty = self.Route[-1]['Y']
if (x == lastx and y == lasty):
''' reached the end of the route, stop car'''
logger.debug("X: {0}, LastX: {1}".format(x, self.Route[-1]['X']))
logger.debug("Y: {0}, LastY: {1}".format(y, self.Route[-1]['Y']))
self.stop()
def move(self):
self.ticks += 1
#logger.debug("[SilverCar]: Current velocity: {0}, New position {1}, TicketsToTarget {2}".format(self.Velocity, self.Position, self._ticksToTarget));
if(self._ticksToTarget <= 0):
self.updateVelocity()
if(self._ticksToTarget > 0):
self.Position = Vector3(self.Position.X + self.Velocity.X, self.Position.Y + self.Velocity.Y, self.Position.Z + self.Velocity.Z)
self._ticksToTarget -= 1
def __NormalizeVelocity(self, speed, heading) :
# i'm not at all sure why the coordinates for speed are off
# by 270 degrees... but this works
heading = (2.0 * (heading + 270.0) * math.pi) / 360.0
# the 0.9 multiplier just makes sure we dont overestimate
# the velocity because of the time shifting, experience
# is better if the car falls behind a bit rather than
# having to be moved back because it got ahead
x = self.VelocityFudgeFactor * self.TimeScale * speed * math.cos(heading)
y = self.VelocityFudgeFactor * self.TimeScale * speed * math.sin(heading)
return Vector3(x / self.XSize, y / self.YSize, 0.0)
def __predicate__(c1, c2):
dx = c2.Position.X - c1.Position.X
dy = c2.Position.Y - c1.Position.Y
dis = math.sqrt(dx * dx + dy * dy)
vx = c1.Velocity.X
vy = c1.Velocity.Y
if dis == 0:
return False
if c1.Velocity == Vector3(0, 0, 0):
return False
cosTheta = (dx * vx + dy * vy)/(math.sqrt(dx*dx+dy*dy) * math.sqrt(vx*vx+vy*vy))
if cosTheta > 0.9 and dis < 10:
return True
return False
def carToSegments(self):
px = self.Position.X
py = self.Position.Y
widthBy2 = self._Width / 2
lengthBy2 = self._Length / 2
seg1 = Segment()
x1 = px - widthBy2
x2 = px + widthBy2
y1 = py + lengthBy2
y2 = py + lengthBy2
seg1.p1 = Vector3(x1,y1,0)
seg1.p2 = Vector3(x2,y2,0)
seg2 = Segment()
x1 = px + widthBy2
x2 = px + widthBy2
y1 = py + lengthBy2
y2 = py - lengthBy2
seg2.p1 = Vector3(x1,y1,0)
seg2.p2 = Vector3(x2,y2,0)
seg3 = Segment()
x1 = px + widthBy2
x2 = px - widthBy2
y1 = py - lengthBy2
y2 = py - lengthBy2
seg3.p1 = Vector3(x1,y1,0)
seg3.p2 = Vector3(x2,y2,0)
seg4 = Segment()
x1 = px - widthBy2
x2 = px - widthBy2
y1 = py - lengthBy2
y2 = py + lengthBy2
seg4.p1 = Vector3(x1,y1,0)
seg4.p2 = Vector3(x2,y2,0)
return [seg1, seg2, seg3, seg4]
def move(self):
#temp fix inactive carstar failing
#self.Velocity = 4;#SPEED
#get what current index is
# End of ride -- if source index is dest
print("\n" + str(self.CurrentSourceIndex))
print("\n" + str(len(self.Lines) - 1) + "\n\n\n")
if (self.CurrentSourceIndex == (len(self.Lines) - 1)):
self.stop()
return
m = self.slope()
k = self.toadd(m)
#update positions based on formulas
self.Position = Vector3(self.Position.X + k, self.Position.Y + k * m,
self.Position.Z + 0)
#check if position is still in between two or not
pflag = self.validatePoint()
if (pflag == False):
a, b = str(self.Lines[self.CurrentSourceIndex]).split(',')
# as it implies overshoot
self.Position.X = float(a)
self.Position.Y = float(b)
self.CurrentSourceIndex += 1
#new pos reached
print(self.CurrentSourceIndex)
print("\n\n\n\n\n")
logger.debug("[ActiveCar_teja]: {0}".format(self.ID))
logger.debug(
"[ActiveCar_teja]: Current velocity: {0}, New position {1}".format(
self.Velocity, self.Position))
class Car_akshatp(Vehicle.Class()):
_Route = []
@dimension(list)
def Route(self):
return self._Route
@Route.setter
def Route(self, value):
self._Route = value
@dimension(float)
def RouteLength(self):
if len(self._Route) > 1:
l = 0
for i in range(1, len(self._Route)):
x1 = self._Route[i - 1]['X']
y1 = self._Route[i - 1]['Y']
x2 = self._Route[i]['X']
y2 = self._Route[i]['Y']
l += math.sqrt(((x2 - x1) * (x2 - x1)) + ((y2 - y1) *
(y2 - y1)))
return l
else:
return 0
@RouteLength.setter
def RouteLength(self, RouteLength):
pass
@dimension(list)
def CarRotatedBox(self):
p = self.Position
prevp = self.PrevPosition
cangle = complex(prevp.X - p.X, prevp.Y - p.Y)
cangle = 0 if abs(cangle) == 0 else (cangle / abs(cangle))
center = complex(p.X, p.Y)
x1 = p.X - self.Length / 2
y1 = p.Y - self.Width / 2
x2 = p.X + self.Length / 2
y2 = p.Y - self.Width / 2
x3 = p.X + self.Length / 2
y3 = p.Y + self.Width / 2
x4 = p.X - self.Length / 2
y4 = p.Y + self.Width / 2
xy1 = cangle * (complex(x1, y1) - center) + center
xy2 = cangle * (complex(x2, y2) - center) + center
xy3 = cangle * (complex(x3, y3) - center) + center
xy4 = cangle * (complex(x4, y4) - center) + center
return [
xy1.real, xy1.imag, xy2.real, xy2.imag, xy3.real, xy3.imag,
xy4.real, xy4.imag
]
@CarRotatedBox.setter
def CarRotatedBox(self, CarBox):
pass
@dimension(list)
def CarRotatedHitBox(self):
p = self.Position
prevp = self.PrevPosition
cangle = complex(prevp.X - p.X, prevp.Y - p.Y)
cangle = 0 if abs(cangle) == 0 else (cangle / abs(cangle))
center = complex(p.X, p.Y)
hx1 = p.X - self.Length / 2 - self.Velocity.X * 4
hy1 = p.Y - self.Width / 2 - self.Width / 2
hx2 = p.X - self.Length / 2
hy2 = p.Y - self.Width / 2 - self.Width / 2
hx3 = p.X - self.Length / 2
hy3 = p.Y + self.Width / 2 + self.Width / 2
hx4 = p.X - self.Length / 2 - self.Velocity.X * 4
hy4 = p.Y + self.Width / 2 + self.Width / 2
hxy1 = cangle * (complex(hx1, hy1) - center) + center
hxy2 = cangle * (complex(hx2, hy2) - center) + center
hxy3 = cangle * (complex(hx3, hy3) - center) + center
hxy4 = cangle * (complex(hx4, hy4) - center) + center
return [
hxy1.real, hxy1.imag, hxy2.real, hxy2.imag, hxy3.real, hxy3.imag,
hxy4.real, hxy4.imag
]
@CarRotatedHitBox.setter
def CarRotatedHitBox(self, CarBox):
pass
_CurrentRoute = 0
@dimension(int)
def CurrentRoute(self):
return self._CurrentRoute
@CurrentRoute.setter
def CurrentRoute(self, value):
self._CurrentRoute = value
_PrevPosition = Vector3(0, 0, 0)
@dimension(Vector3)
def PrevPosition(self):
return self._PrevPosition
@PrevPosition.setter
def PrevPosition(self, value):
self._PrevPosition = value
_TopSpeed = 0
@dimension(int)
def TopSpeed(self):
return self._TopSpeed
@TopSpeed.setter
def TopSpeed(self, value):
self._TopSpeed = value
def __init__(self):
self.ID = str(uuid.uuid4())
self.Position = Vector3(0, 0, 40.5)
self.Velocity = Vector3(0, 0, 0)
self.Length = 0
self.Width = 0
self.Name = ""
def __init__(self):
self.StartingPoint = Vector3(0, 0, 0)
self.EndPoint = Vector3(0, 0, 0)
self.Width = 0
self.Type = None
def slowDown(self):
self.Velocity = Vector3(0, 0, 0)
def __init__(self):
self.Center = Vector3(0, 0, 0)
self.Angle = 0
self.Name = ""
self.Width = 0
self.Rezcap = Capsule()
