def BendMaker(self, t, yawrate, transition_duration, rw, speed, sp, x_dir):
"""function returns a bend edge"""
"""function returns a bend edge"""
x, y, bearing = cc.clothoid_curve(t, speed, yawrate,
transition_duration)
if x_dir < 0:
bearing[:] = [(2 * (np.pi) - b) for b in bearing[:]]
midline = np.array([((x * x_dir) + sp[0]), (y + sp[1])]).T
outside = np.array(cc.add_edge((x * x_dir), y, (rw / 2), sp)).T
inside = np.array(cc.add_edge((x * x_dir), y, -(rw / 2), sp)).T
#print(outside.shape)
#print(inside.shape)
viz.startlayer(self.Primitive)
for ins, out in zip(inside, outside):
#print(ins)
#print(ins.shape)
viz.vertex(ins[0], ABOVEGROUND, ins[1])
viz.vertexcolor(self.Colour)
#print(ins[0], ins[1])
viz.vertex(out[0], ABOVEGROUND, out[1])
viz.vertexcolor(self.Colour)
#print(out[0], out[1])
Bend = viz.endlayer()
return ([Bend, midline, inside, outside, bearing])
def EdgeMaker(self, startpos, bearing, primitive_width):
"""function returns a bend edge"""
"""function returns a bend edge"""
viz.startlayer(self.primitive)
for ins, out in zip(inside,outside):
viz.vertex(ins[0], ABOVEGROUND, ins[1])
viz.vertexcolor(self.colour)
viz.vertex(out[0], ABOVEGROUND, out[1])
viz.vertexcolor(self.colour)
"""
while i < self.Edge_Pts:
x1 = ((bearing - primitive_width)*np.cos(self.RoadArray[i])) + startpos[0]
z1 = self.Zdirection*((bearing - primitive_width)*np.sin(self.RoadArray[i])) + startpos[2]
#print (z1[i])
viz.vertex(x1, ABOVEGROUND, z1)
viz.vertexcolor(self.colour)
if self.primitive == viz.QUAD_STRIP:
x2 = ((rads + primitive_width)*np.cos(self.RoadArray[i])) + startpos[0]
z2 = self.Zdirection*((rads + primitive_width)*np.sin(self.RoadArray[i])) + startpos[2]
viz.vertex(x2, ABOVEGROUND, z2)
viz.vertexcolor(self.colour)
i += 1
"""
Bend = viz.endlayer()
return Bend
def createQuad(left_bot, left_top, right_bot, right_top):
viz.startlayer(viz.QUADS)
viz.vertexcolor(0, 0, 0)
viz.vertex(left_bot)
viz.vertex(left_top)
viz.vertex(right_top)
viz.vertex(right_bot)
return viz.endlayer()
def StraightMaker(self, startpos, bearing, length, primitive_width):
endpos = [(startpos[0] + (length * (np.sin(bearing)))),
(startpos[1] + (length * (np.cos(bearing))))]
viz.startlayer(self.Primitive)
#print ("Startpos: ", startpos)
# print ("Endpos: ", endpos)
start_left = ([
(startpos[0] + (primitive_width * (np.sin(bearing - np.pi / 2)))),
ABOVEGROUND,
(startpos[1] + (primitive_width * (np.cos(bearing - np.pi / 2))))
])
#print('1L', start_left)
viz.vertex(start_left)
viz.vertexcolor(self.Colour)
# start rightside
start_right = ([
(startpos[0] + (primitive_width * (np.sin(bearing + np.pi / 2)))),
ABOVEGROUND,
(startpos[1] + (primitive_width * (np.cos(bearing + np.pi / 2))))
])
#print('1R', start_right)
viz.vertex(start_right)
viz.vertexcolor(self.Colour)
# end leftside:
end_left = ([
(endpos[0] + (primitive_width * (np.sin(bearing - np.pi / 2)))),
ABOVEGROUND,
(endpos[1] + (primitive_width * (np.cos(bearing - np.pi / 2))))
])
#print('2L', end_left)
viz.vertex(end_left)
viz.vertexcolor(self.Colour)
# end rightside:
end_right = ([
(endpos[0] + (primitive_width * (np.sin(bearing + np.pi / 2)))),
ABOVEGROUND,
(endpos[1] + (primitive_width * (np.cos(bearing + np.pi / 2))))
])
#print('2R', end_right)
viz.vertex(end_right)
viz.vertexcolor(self.Colour)
straight = viz.endlayer()
return (straight, endpos)
def StraightMaker(x,
start_z,
end_z,
colour=[.8, .8, .8],
primitive=viz.QUAD_STRIP,
width=None):
"""returns a straight, given some starting coords and length"""
viz.startlayer(primitive)
if width is None:
if primitive == viz.QUAD_STRIP:
width = .05
elif primitive == viz.LINE_STRIP:
width = 2
viz.linewidth(width)
width = 0
viz.vertex(x - width, .1, start_z)
viz.vertexcolor(colour)
viz.vertex(x + width, .1, start_z)
viz.vertexcolor(colour)
viz.vertex(x - width, .1, end_z)
viz.vertexcolor(colour)
viz.vertex(x + width, .1, end_z)
straightedge = viz.endlayer()
return straightedge
def EdgeMaker(self, startpos, rads, primitive_width):
"""function returns a bend edge"""
i = 0
viz.startlayer(self.primitive)
while i < self.Edge_Pts:
x1 = ((rads-primitive_width)*np.cos(self.RoadArray[i])) + startpos[0]
z1 = self.Z_direction*((rads-primitive_width)*np.sin(self.RoadArray[i])) + startpos[2]
#print (z1[i])
viz.vertex(x1, ABOVEGROUND, z1)
viz.vertexcolor(self.colour)
if self.primitive == viz.QUAD_STRIP:
x2 = ((rads+primitive_width)*np.cos(self.RoadArray[i])) + startpos[0]
z2 = self.Z_direction*((rads+primitive_width)*np.sin(self.RoadArray[i])) + startpos[2]
viz.vertex(x2, ABOVEGROUND, z2)
viz.vertexcolor(self.colour)
i += 1
Bend = viz.endlayer()
return Bend
def StraightEdgeMaker(self, startpos, endpos, primitive_width):
"""function returns a bend edge"""
i = 0
viz.startlayer(self.primitive)
#print ("Startpos: ", startpos)
#print ("Endpos: ", endpos)
viz.vertex([startpos[0] - primitive_width, startpos[1], startpos[2]])
viz.vertexcolor(self.colour)
viz.vertex([startpos[0] + primitive_width, startpos[1], startpos[2]])
viz.vertexcolor(self.colour)
viz.vertex([endpos[0] - primitive_width, endpos[1], endpos[2]])
viz.vertexcolor(self.colour)
viz.vertex([endpos[0] + primitive_width, endpos[1], endpos[2]])
viz.vertexcolor(self.colour)
straightedge = viz.endlayer()
return (straightedge)
def build_curve_viz(t, yawrate, transition, rw=3, speed=8):
x, y, bearing = clothoid_curve(t, speed, yawrate, transition)
midline = add_edge(x, y, rw=0)
outside = add_edge(x, y, rw)
inside = add_edge(x, y, -rw)
viz.startlayer(viz.POINTS)
for mid, ins, out in zip(midline, inside, outside):
viz.vertex(mid[0], ABOVEGROUND, mid[1])
viz.vertexcolor(viz.RED)
viz.vertex(ins[0], ABOVEGROUND, ins[1])
viz.vertexcolor(viz.BLUE)
viz.vertex(out[0], ABOVEGROUND, out[1])
viz.vertexcolor(viz.GREEN)
return (np.array((x, y, bearing)))
def createSolidBox(min, max):
viz.startlayer(viz.QUADS, "top")
viz.vertexcolor(1.0, 0.5, 0.0)
# top
# viz.vertexcolor(0.0,1.0,0.0)
viz.normal(0, 1, 0)
viz.vertex(max[0], max[1], min[2])
viz.vertex(min[0], max[1], min[2])
viz.vertex(min[0], max[1], max[2])
viz.vertex(max[0], max[1], max[2])
viz.startlayer(viz.QUADS, "bottom")
viz.vertexcolor(0.5, 0.5, 0.0)
viz.linewidth(2.0)
# bottom
# viz.vertexcolor(1.0,0.5,0.0)
viz.normal(0, -1, 0)
viz.vertex(max[0], min[1], max[2])
viz.vertex(min[0], min[1], max[2])
viz.vertex(min[0], min[1], min[2])
viz.vertex(max[0], min[1], min[2])
viz.startlayer(viz.QUADS, "front")
viz.vertexcolor(0.5, 0.5, 0.0)
viz.linewidth(2.0)
# front
# viz.vertexcolor(1.0,0.0,0.0)
viz.normal(0, 0, 1)
viz.vertex(max[0], max[1], max[2])
viz.vertex(min[0], max[1], max[2])
viz.vertex(min[0], min[1], max[2])
viz.vertex(max[0], min[1], max[2])
viz.startlayer(viz.QUADS, "back")
viz.vertexcolor(0.5, 0.5, 0.0)
viz.linewidth(2.0)
# back
# viz.vertexcolor(1.0,1.0,0.0)
viz.normal(0, 0, -1)
viz.vertex(max[0], min[1], min[2])
viz.vertex(min[0], min[1], min[2])
viz.vertex(min[0], max[1], min[2])
viz.vertex(max[0], max[1], min[2])
viz.startlayer(viz.QUADS, "left")
viz.vertexcolor(0.5, 0.5, 0.0)
viz.linewidth(2.0)
# left
# viz.vertexcolor(0.0,0.0,1.0)
viz.normal(-1, 0, 0)
viz.vertex(min[0], max[1], max[2])
viz.vertex(min[0], max[1], min[2])
viz.vertex(min[0], min[1], min[2])
viz.vertex(min[0], min[1], max[2])
viz.startlayer(viz.QUADS, "right")
viz.vertexcolor(0.5, 0.5, 0.0)
viz.linewidth(2.0)
# right
# viz.vertexcolor(1.0,0.0,1.0)
viz.normal(1, 0, 0)
viz.vertex(max[0], max[1], min[2])
viz.vertex(max[0], max[1], max[2])
viz.vertex(max[0], min[1], max[2])
viz.vertex(max[0], min[1], min[2])
return viz.endlayer()
def createSolidBox(min,max): viz.startlayer(viz.QUADS, 'top') viz.vertexcolor(1.0,0.5,0.0) #top #viz.vertexcolor(0.0,1.0,0.0) viz.normal(0,1,0) viz.vertex( max[0], max[1], min[2]) viz.vertex( min[0], max[1], min[2]) viz.vertex( min[0], max[1], max[2]) viz.vertex( max[0], max[1], max[2]) viz.startlayer(viz.QUADS,'bottom') viz.vertexcolor(.5,.5,0.0) viz.linewidth(2.0) #bottom #viz.vertexcolor(1.0,0.5,0.0) viz.normal(0,-1,0) viz.vertex( max[0], min[1], max[2]) viz.vertex( min[0], min[1], max[2]) viz.vertex( min[0], min[1], min[2]) viz.vertex( max[0], min[1], min[2]) viz.startlayer(viz.QUADS,'front') viz.vertexcolor(.5,.5,0.0) viz.linewidth(2.0) #front #viz.vertexcolor(1.0,0.0,0.0) viz.normal(0,0,1) viz.vertex( max[0], max[1], max[2]) viz.vertex( min[0], max[1], max[2]) viz.vertex( min[0], min[1], max[2]) viz.vertex( max[0], min[1], max[2]) viz.startlayer(viz.QUADS,'back') viz.vertexcolor(.5,.5,0.0) viz.linewidth(2.0) #back #viz.vertexcolor(1.0,1.0,0.0) viz.normal(0,0,-1) viz.vertex( max[0], min[1], min[2]) viz.vertex( min[0], min[1], min[2]) viz.vertex( min[0], max[1], min[2]) viz.vertex( max[0], max[1], min[2]) viz.startlayer(viz.QUADS,'left') viz.vertexcolor(.5,.5,0.0) viz.linewidth(2.0) #left #viz.vertexcolor(0.0,0.0,1.0) viz.normal(-1,0,0) viz.vertex( min[0], max[1], max[2]) viz.vertex( min[0], max[1], min[2]) viz.vertex( min[0], min[1], min[2]) viz.vertex( min[0], min[1], max[2]) viz.startlayer(viz.QUADS,'right') viz.vertexcolor(.5,.5,0.0) viz.linewidth(2.0) #right #viz.vertexcolor(1.0,0.0,1.0) viz.normal(1,0,0) viz.vertex( max[0], max[1], min[2]) viz.vertex( max[0], max[1], max[2]) viz.vertex( max[0], min[1], max[2]) viz.vertex( max[0], min[1], min[2]) return viz.endlayer()
def __init__( self , av_type = 0):
self.in_quad = 0
if av_type == 0:
type = random.randrange(0,4)
if type == 0:
self.avatar = viz.add('male.cfg',viz.WORLD,scene=viz.MainScene)
elif type == 1:
self.avatar = viz.add('vcc_male.cfg',viz.WORLD,scene=viz.MainScene)
elif type == 2:
self.avatar = viz.add('female.cfg',viz.WORLD,scene=viz.MainScene)
else:
self.avatar = viz.add('vcc_female.cfg',viz.WORLD,scene=viz.MainScene)
else:
self.avatar = viz.add('vcc_male.cfg',viz.WORLD,scene=viz.MainScene)
#Add the hat model
self.hat = viz.add('tophat.3ds')
self.hat.setScale([1,5,1])
#Get the head bone of the avatar
head = self.avatar.getBone('Bip01 Head')
#Link the hat to the head
HatLink = viz.link(head,self.hat)
#Tweek the hat link so it fits snuggly on the head
HatLink.preTrans( [0,0.1,-0.0] )
HatLink.preEuler( [0,-10,0] )
#HatLink.([0,5,0])
self.avatar.setPosition(get_random_point())
self.next_point = get_quadrant(self.avatar.getPosition()).get_random_walk()
self.coll = 0
self.avatar.collideMesh()
self.avatar.enable(viz.COLLIDE_NOTIFY)
#setup the AR
viz.startlayer(viz.QUADS)
viz.vertexcolor(1,0,0)
#viz.pointsize(20)
viz.linewidth(20)
pos = self.avatar.getPosition()
#viz.vertex(pos[0], -20, pos[2])
#viz.vertex(pos[0], 20, pos[2])
#rx = 0.5
#ry = 1
#viz.vertex(-rx, 0, 0)
#viz.vertex(-rx, r, 0)
#viz.vertex(r, r, 0)
#viz.vertex(r, -r, 0)
#viz.vertex(0,0,0)
#viz.vertex(0,2,0)
viz.vertex(-0.3,0,0)
viz.vertex(0.3,0,0)
viz.vertex(0.3,2,0)
viz.vertex(-0.3,2,0)
self.pointAR = viz.endlayer(viz.WORLD, viz.Scene2)
self.set_AR(True)
#self.pointAR.alpha(0.3)
self.arev = vizact.ontimer(.01,self.move_AR)
node2D = viz.addRenderNode() node2D.setScene(viz.Scene3) node2D.setBuffer( viz.RENDER_FRAME_BUFFER ) node2D.setOrder( viz.POST_RENDER ) node2D.setInheritView(0) node2D.setSize( HMDwidth,HMDheight ) node2D.setProjectionMatrix(viz.Matrix.ortho2D(0,HMDwidth,0,HMDheight)) node2D.setClearMask(0) node2D.disable(viz.DEPTH_TEST) #viz.startlayer(viz.LINES) #viz.vertex([HMDwidth/2,0,0]) #viz.vertex([HMDwidth/2,HMDheight,0]) #viz.endlayer(viz.WORLD,viz.Scene3) viz.startlayer(viz.LINES) viz.vertexcolor([1,0,0]) viz.vertex([HMDwidth/2-10,HMDheight/2,0]) viz.vertex([HMDwidth/2+10,HMDheight/2,0]) viz.vertex([HMDwidth/2,HMDheight/2-10,0]) viz.vertex([HMDwidth/2,HMDheight/2+10,0]) viz.vertexcolor([1,1,1]) viz.endlayer(viz.WORLD,viz.Scene3) ringbuffer_len = 20
def __init__( self , speedMultiplier = 1.0, av_type = 0 ):
print "Creating a person"
self.speedMultiplier = speedMultiplier
self.save_path = path.Path()
self.in_quad = 0
if av_type == 0:
type = random.randrange(0,4)
if type == 0:
self.avatar = viz.add('male.cfg',viz.WORLD,scene=viz.MainScene)
elif type == 1:
self.avatar = viz.add('vcc_male.cfg',viz.WORLD,scene=viz.MainScene)
elif type == 2:
self.avatar = viz.add('female.cfg',viz.WORLD,scene=viz.MainScene)
else:
self.avatar = viz.add('vcc_female.cfg',viz.WORLD,scene=viz.MainScene)
else:
self.avatar = viz.add('vcc_male.cfg',viz.WORLD,scene=viz.MainScene)
#Add the hat model
self.hat = viz.add('tophat.3ds')
self.hat.setScale([1,5,1])
#Get the head bone of the avatar
head = self.avatar.getBone('Bip01 Head')
#Link the hat to the head
HatLink = viz.link(head,self.hat)
#Tweek the hat link so it fits snuggly on the head
HatLink.preTrans( [0,0.1,-0.0] )
HatLink.preEuler( [0,-10,0] )
self.avatar.setPosition(quadSet.get_random_point())
#self.save_path.setStart(self.avatar.getPosition())
self.save_path.addPoint(self.avatar.getPosition(), 0)
self.next_point = quadSet.get_quadrant(self.avatar.getPosition())[0].get_random_walk()
self.next_speed = get_next_speed()
self.save_path.addPoint(self.next_point, self.next_speed)
self.coll = 0
self.avatar.collideMesh()
self.avatar.enable(viz.COLLIDE_NOTIFY)
#setup the AR
viz.startlayer(viz.QUADS)
if av_type == 0:
viz.vertexcolor(1,0,0)
else:
viz.vertexcolor(1,0,0)
viz.linewidth(20)
pos = self.avatar.getPosition()
viz.vertex(-0.3,0,0)
viz.vertex(0.3,0,0)
viz.vertex(0.3,2,0)
viz.vertex(-0.3,2,0)
self.pointAR = viz.endlayer(viz.WORLD, viz.Scene2)
self.pointAR.alpha(0.3)
self.tracking_error = []
self.arev = vizact.ontimer(.01,self.move_AR)
self.myquadrants = [False,False,False,False,False,False,False,False]
self.lastTime = 0
self.timeNotVisible = 0
for s in subpart2: temp +=(s*0.5,) part3 = temp result = (part1[0]+part2[0]+part3[0],part1[1]+part2[1]+part3[1],part1[2]+part2[2]+part3[2]) return result wd = [0,0,0] wc = 0 grav = [0.0,9.81,0.0] bl = ([-4.970107078552246, 5.897994518280029, 1.4124407768249512],[-4.973110198974609, 6.1175079345703125, 2.1125693321228027],[-4.975081920623779, 6.5309247970581055, 2.8221869468688965],[-4.975322246551514, 6.759340286254883, 3.1336262226104736],[-4.975906848907471, 7.1793365478515625, 2.779799699783325],[-4.97451114654541, 5.308044910430908, 3.1625561714172363],[-4.973536014556885, 5.8962178230285645, 3.2122514247894287],[-4.9707231521606445, 5.660599231719971, 3.5753281116485596],[-4.966211795806885, 4.887547492980957, 3.882848024368286],[-2.7433903217315674, 5.715956211090088, 5.507270812988281],[-3.2320258617401123, 6.556395053863525, 5.514878273010254],[-3.798262596130371, 5.829957008361816, 5.529846668243408],[-2.0034732818603516, 7.698963642120361, 5.504480361938477],[-4.953855037689209, 6.041557788848877, 5.1223649978637695],[-1.3830612897872925, 5.617004871368408, -4.059776782989502],[-1.6367100477218628, 3.0266971588134766, -4.050085544586182],[-2.242931604385376, 2.975187301635742, -4.043353080749512],[-2.9772372245788574, 2.110835075378418, -4.028698921203613],[-1.6415677070617676, 2.3813564777374268, -4.04733943939209],[-0.3302052915096283, 1.9607874155044556, -4.049890518188477],[-4.9920268058776855, 5.900781631469727, -1.8857085704803467],[-4.989490985870361, 5.56298828125, -1.7206439971923828],[-4.996257305145264, 4.581329345703125, -2.1972899436950684],[-4.994059085845947, 4.407802581787109, -1.6545305252075195],[-4.992647171020508, 4.442633628845215, -1.1358085870742798],[-4.988626480102539, 4.823823928833008, 0.1453738659620285],[-4.988673210144043, 5.07926607131958, -0.048772379755973816],[-4.988332748413086, 5.744387149810791, -0.1320347636938095],[-4.9886860847473145, 6.315260410308838, -0.23182286322116852],[-4.986820697784424, 5.827799320220947, -0.9230077862739563],[-4.991076946258545, 4.644659042358398, -0.8074011206626892],[-4.9762864112854, 4.111218452453613, 3.3881337642669678],[-4.9752607345581055, 7.730223178863525, 2.4686429500579834],[0.26733750104904175, 8.535117149353027, 5.487018585205078],[1.2186188697814941, 7.103093147277832, 5.488711833953857],[-0.5966476202011108, 7.537855625152588, 5.486145973205566],[-0.8177675008773804, 7.79792594909668, 5.4877166748046875],[-1.8698996305465698, 8.362137794494629, 5.504770755767822],[-4.992286205291748, 2.577256441116333, 0.02785523794591427]) al = [] target = [1.1959341764450073, 7.428847789764404, 0.7743394374847412] masterpointlist = [] for b in bl: viz.startLayer(viz.LINE_STRIP) viz.vertexcolor(viz.BLUE) viz.vertex(b) x=0 al += ([target[0]-b[0],target[1]-b[1],target[2]-b[2]],) pointlist = [] while x <=5: point = formula(grav,wd,wc,x,al[bl.index(b)*-1],b) viz.vertex(point) pointlist += [point] x += 0.1 masterpointlist += [pointlist] viz.endLayer() area = [] z=0 print al zcount = len(masterpointlist)-1
