Python vec3.vec3函数代码示例

    def calculate(self, boid):
        the_world = boid.world
        position = boid.position
        radius = boid.radius
        for other in the_world.boids(boid):
            offset = position - other.position
            distance = offset.length()
            radius_ij = radius + other.radius
            if distance < radius_ij:
                offset = offset.scale(radius_ij - distance)
                boid.position += offset
        wall_found = False
        checked = []
        for obstacle in the_world.obstacles(boid):
            if obstacle in checked: continue
            checked.append(obstacle)
            intersect, distance_to_line, normal = self.distance_from_line(position, obstacle)
            if intersect and distance_to_line < radius * 1.2:
                wall_found = True
                normal = normal.scale(radius * 1.2 - distance_to_line)
                boid.position += normal
        if not wall_found:
            checked = []
            for obstacle in the_world.obstacles(boid):
                if obstacle in checked: continue
                checked.append(obstacle)
                for point in (obstacle[0], obstacle[1]):
                    offset = position - vec3(point)
                    distance = offset.length()
                    if distance < radius * 1.2:
                        boid.position += offset.scale(radius * 1.2 - distance)

        return vec3()

    def calculate(self, boid):
        """ calculate boid behavior

        Parameters
        ----------

        boid

        Returns
        -------

        steering

        """


        the_world = boid.world
        others = the_world.boids(boid, 6.0)
        separation_distance = 6.0 * boid.velocity.length() / boid.max_speed
        acceleration = vec3()
        for other in others:
            local_position = the_world.to_local(boid, other.position)
            in_front = local_position[1] > -boid.radius
            if in_front and local_position.length() < separation_distance:
                separation = other.position - boid.position
                force = separation.scale(-1 / max(1e-9,separation.length() ** 2))
                # create orthogonal vector in order to make boids avoidance
                force2 = (-1**randint(0,1))*vec3(-force[1],force[0],0)
#                force2 = vec3(-force[1],force[0],0)
                acceleration += 0.5*force2 #3*force2
        return acceleration

    def distance_from_line(self, position, line):
        """ distance from line

        Parameters
        ----------

        position : 2D vector
        line : np.array

        Returns
        -------

        True , distance , vector
        or 
        False , None , None 

        """
        line_length = (vec3(line[1]) - vec3(line[0])).length()
        u = ((position.x - line[0][0]) * (line[1][0] - line[0][0])
           + (position.y - line[0][1]) * (line[1][1] - line[0][1])) \
            / line_length ** 2
        if 0.0 < u < 1.0:
            # point is tangent to line
            x = line[0][0] + u * (line[1][0] - line[0][0])
            y = line[0][1] + u * (line[1][1] - line[0][1])
            vector = position - vec3(x, y)
            distance = vector.length()
            return True, distance, vector
        return False, None, None

def station(start, mainline_speed, side, berths, decision, queue_berths=None):
    if side == 'left':
        side = -1
    else:
        side = 1
    if queue_berths is None:
        queue_berths = berths
    seconds = mainline_speed / PRT.max_acceleration
    average_ramp_speed = (mainline_speed + 0) / 2
    ramp_length = average_ramp_speed * seconds
    offline_guideway_straight_length = ramp_length * 2 - spline_length * 2 + (berths + queue_berths) * PRT.berth_length
    online_guideway_straight_length = offline_guideway_straight_length + spline_height * 2
    xx, yy = start
    d1 = decision
    d1.next = d2 = Diverge()
    d2.left = s1 = Straight((xx, yy), (xx, yy+online_guideway_straight_length))

    d2.right = sp1 = Spline((xx, yy), (xx, yy+spline_height / 2),
                            (xx+(1.8*side), yy+spline_height / 2), (xx+(1.8*side), yy+spline_height))
    yy_top = yy+spline_height+offline_guideway_straight_length
    sp1.next = s2 = Straight((xx+(1.8*side), yy+spline_height), (xx+(1.8*side), yy_top))
    s2.destinations = []
    for ii in range(0, berths + queue_berths):
        s2.destinations.append(vec3(xx+(1.8*side), yy_top - (ramp_length - spline_length) - ii * PRT.berth_length))
    s2.platform = (vec3(xx+(1.8+PRT.width/2)*side, yy_top - (ramp_length - spline_length) + PRT.berth_length / 2),
                   vec3(xx+(1.8+PRT.width/2+3)*side, yy_top - (ramp_length - spline_length) - berths * PRT.berth_length
                        + PRT.berth_length / 2))
    d1.destinations = s2.destinations
    d1.platform = s2.platform
    s2.next = sp2 = Spline((xx+(1.8*side), yy_top), (xx+(1.8*side), yy_top+spline_height/2),
                           (xx, yy_top+spline_height/2), (xx, yy_top+spline_height))
    return d1, s1, sp2, online_guideway_straight_length

 def distance_from_line(self, position, line):
     line_length = (vec3(line[1]) - vec3(line[0])).length()
     u = ((position.x - line[0][0]) * (line[1][0] - line[0][0])
          + (position.y - line[0][1]) * (line[1][1] - line[0][1])) \
         / line_length ** 2
     if 0.0 < u < 1.0:
         # point is tangent to line
         x = line[0][0] + u * (line[1][0] - line[0][0])
         y = line[0][1] + u * (line[1][1] - line[0][1])
         vector = position - vec3(x, y)
         distance = vector.length()
         return True, distance, vector
     return False, None, None

def queue_steering_mind(boid):
    """Sum of all steering vectors, except Separation in some cases.

    The Separation steering vector will be ignored if any prior
    steering behavior gave a non-zero acceleration, typically
    Containment."""

    acceleration = vec3()
    for behavior in boid.behaviors:
#        if not isinstance(behavior, Separation) or acceleration.length() < 0.0001:
         acceleration += behavior.calculate(boid)
    return acceleration

 def __init__(self, interval=0.5):
     Process.__init__(self)
     self.world = world()
     self.interval = interval
     self.manager = None
     self.manager_args = []
     self.waypoints = []
     self.position = vec3()
     self.velocity = vec3()
     self.localx = vec3(1, 0)
     self.localy = vec3(0, 1)
     self.world.add_boid(self)
     # from Helbing, et al "Self-organizing pedestrian movement"
     self.max_speed = normalvariate(1.36, 0.26)
     self.desired_speed = self.max_speed
     self.radius = normalvariate(self.average_radius, 0.025) / 2
     self.intersection = vec3()
     self.arrived = False
     self.behaviors = []
     self.steering_mind = default_steering_mind
     self.cancelled = 0

 def test_intersection(self, boid, wall, position, vector, method = 'direct'):
     # From http://astronomy.swin.edu.au/~pbourke/geometry/lineline2d/
     point1, point2 = wall
     if method == 'direct':
         VR=vector
     elif method == 'uniform':
 ######## version uniform
         r=uniform(-pi/12.0,pi/12.0)
         v0=vector.ang0()
         vl=vector.length()
         VR = vec3(cos(v0+r)*vl,sin(v0+r)*vl,vector.z)
     elif method == 'gauss':
 ######## version gaussienne
         vl=vector.length()
         r=gauss(vector.ang0(),pi/12.0)
         VR = vec3(cos(r)*vl,sin(r)*vl,vector.z)
     elif method == 'ellipse':
 ######## version ellipse
         theta = gauss(vector.ang0(),sqrt(pi/6.0)) # random angle to test
         a = vector.length()                 # create an elipse... 
         b = a/1.5                            # ...to test  collision if b=a/1. ellipse =circle
         e = (sqrt(a**2-b**2))/a             # ...
         r = (b**2/a)/(1.0+e*cos(theta))     # ....
         VR = vec3(cos(theta)*r,sin(theta)*r,vector.z)
     denominator = ((VR.y * (point2[0] - point1[0]))
                 - (VR.x * (point2[1] - point1[1])))
     if denominator == 0.0:
         # parallel or coincident
         return False, 0.0, None
     u_a = (VR.x * (point1[1] - position.y)
            - (VR.y) * (point1[0] - position.x)) / denominator
     u_b = ((point2[0] - point1[0]) * (point1[1] - position.y)
            - (point2[1] - point1[1]) * (point1[0] - position.x)) / denominator
     intersect = 0.0 < u_a < 1.0 and 0.0 < u_b < 1.0
     if intersect:
         intersection = vec3(point1[0] + u_a * (point2[0] - point1[0]),
                             point1[1] + u_a * (point2[1] - point1[1]))
         boid.intersection = intersection
         distance_along_check = u_b
         wall_VR = vec3(point1) - vec3(point2)
         wall_VR_normal = vec3(-wall_VR.y, wall_VR.x).normalize()
         boid.intersection_normal = wall_VR_normal
         normal_point = intersection + wall_VR_normal
         local_normal_point = boid.world.to_local(boid, normal_point)
         if local_normal_point.x <= 0.0:
             direction = 'left'
         else:
             direction = 'right'
         return True, distance_along_check, direction
     else:
         return False, 0.0, None

def default_steering_mind(boid):
    """ Sum all steering vectors.

    Notes 
    -----

    This is the place where all acceleration from all behaviors are summed.

    """
    
    acceleration = vec3()

    for behavior in boid.behaviors:
        acceleration += behavior.calculate(boid)
    return acceleration

def conv_vecarr(vin,dim=2):
    """ convert vec3 => np.array and  np.array => vec3 

    Parameters
    ----------
    dim : int 
        default (2) 
    """
    if isinstance(vin,vec3):
        if dim == 2:
            return(np.array((vin[0],vin[1])))
        elif dim == 3:
            return(np.array((vin[0],vin[1],vin[2])))
        else :
             raise AttributeError('incorrect input vec3 or nd array dimension')
    elif isinstance(vin,np.ndarray): 
        if dim == 2:
            return(vec3((vin[0],vin[1],0.0)))
        elif dim == 3:
            return(vec3((vin[0],vin[1],vin[2])))
        else :
             raise AttributeError('incorrect input vec3 or nd array dimension')
    else : 
        raise TypeError('vin must be either a vec3 or a np.ndarray instance')

    def __init__(self, interval=0.5,roomId=0, L=[],sim=None):
        """
        boid is initialized in a room of Layout L 
        """                
        #GeomNetType = np.dtype([('Id',int), 
        #                ('time',int), 
        #		('p',float,(1,3)),
        #		('v',float,(1,3)),
        #		('a',float,(1,3))])
        Person2.npers +=1
        Process.__init__(self,sim=sim)
        self.L = L 
        self.world = world()
        self.interval = interval
        self.manager = None
        self.manager_args = []
        self.waypoints = []
        self.roomId    = roomId
        self.nextroomId   = int(np.floor(uniform(0,self.L.Gr.size())))
        while self.nextroomId == self.roomId : # test destination different de l'arrive
            self.nextroomId   = int(np.floor(uniform(0,self.L.Gr.size())))
            print "nextroom : ", self.nextroomId
            self.wp           =  self.L.waypoint(roomId,self.nextroomId)
            for tup in self.wp[1:]:
                self.waypoints.append(vec3(tup)) 
            try:
                self.position = vec3(L.Gr.pos[roomId])
            except:         
                self.position = vec3()
#		self.old_pos = vec3()
		    self.stuck = 0               
            self.destination = self.waypoints[0]
            self.velocity = vec3()
            self.localx = vec3(1, 0)
            self.localy = vec3(0, 1)
            self.world.add_boid(self)
               
                #        GeomNet = np.vstack((GeomNet,np.array((Nnodes,0,[list(self.position)],[list(self.velocity)],[list(self.velocity)]),dtype=GeomNetType)))

                # from Helbing, et al "Self-organizing pedestrian movement"
                self.max_speed = normalvariate(1.36, 0.26)
                self.desired_speed = self.max_speed
                self.radius = normalvariate(self.average_radius, 0.025) / 2
                self.intersection = vec3()
                self.arrived = False 
                self.endpoint = False 
                self.behaviors = []
                self.steering_mind = default_steering_mind
                self.cancelled = 0

def truncate(self, max):
    """
    Parameters 
    ----------

    max

    References 
    ----------

    "near collision avoidance" inspired from
    http://people.revoledu.com/kardi/publication/Kouchi2001.pdf

    """
    if self.length() > max:
        return self.normalize() * max
    else:
        return vec3(self)

    def move(self):
        while True:
            while self.cancelled:
                yield passivate, self
                print "Person.move: activated after being cancelled"

            checked = []
            for zone in self.world.zones(self):
                if zone not in checked:
                    checked.append(zone)
                    zone(self)

            acceleration = self.steering_mind(self)

            acceleration = acceleration.truncate(self.max_acceleration)
            self.acceleration = acceleration

            velocity = self.velocity + acceleration * self.interval
            self.velocity = velocity.truncate(self.max_speed)
            if velocity.length() > 0.2:
                # record direction only when we've really had some
                self.localy = velocity.normalize()
                self.localx = vec3(self.localy.y, -self.localy.x)

            self.position = self.position + self.velocity * self.interval

            self.update()
            self.world.update_boid(self)

            if self.arrived:
                self.arrived = False
                if self.manager:
                    if self.manager(self, *self.manager_args):
                        yield passivate, self
                else:
                    yield passivate, self
            else:
                yield hold, self, self.interval

def copy(self):
    return vec3(self)

    def move(self):
        """ Move the Person

        """
        if self.pdshow:
            fig =plt.gcf()
            fig,ax=self.L.showG('w',labels=False,alphacy=0.,edges=False,fig=fig)
            plt.draw()
            plt.ion()
        while True:
            if self.moving:
                if self.sim.verbose:
                    print 'meca: updt ag ' + self.ID + ' @ ',self.sim.now()
                
                # if np.allclose(conv_vecarr(self.destination)[:2],self.L.Gw.pos[47]):
                #     import ipdb
                #     ipdb.set_trace()


                while self.cancelled:
                    yield passivate, self
                    print "Person.move: activated after being cancelled" 
                checked = []
                for zone in self.world.zones(self):
                    if zone not in checked:
                        checked.append(zone)
                        zone(self)

                # updating acceleration
                acceleration = self.steering_mind(self)
                acceleration = acceleration.truncate(self.max_acceleration)
                self.acceleration = acceleration

                # updating velocity
                velocity = self.velocity + acceleration * self.interval
                self.velocity = velocity.truncate(self.max_speed)


                if velocity.length() > 0.2:
                    # record direction only when we've really had some
                    self.localy = velocity.normalize()
                    self.localx = vec3(self.localy.y, -self.localy.x)

                # updating position
                self.position = self.position + self.velocity * self.interval
#                self.update()
                self.position.z=0
                self.world.update_boid(self)

                self.net.update_pos(self.ID,conv_vecarr(self.position),self.sim.now())
                p=conv_vecarr(self.position).reshape(3,1)
                v=conv_vecarr(self.velocity).reshape(3,1)
                a=conv_vecarr(self.acceleration).reshape(3,1)

                # fill panda dataframe 2D trajectory
                self.df = self.df.append(pd.DataFrame({'t':pd.Timestamp(self.sim.now(),unit='s'),
                'x':p[0],
                'y':p[1],
                'vx':v[0],
                'vy':v[1],
                'ax':a[0],
                'ay':a[1]},
                columns=['t','x','y','vx','vy','ax','ay']))

                if self.pdshow:
                    ptmp =np.array([p[:2,0],p[:2,0]+v[:2,0]])

                    if hasattr(self, 'pl'):
                        self.pl[0].set_data(self.df['x'].tail(1),self.df['y'].tail(1))
                        self.pla[0].set_data(ptmp[:,0],ptmp[:,1])
                    else :
                        self.pl = ax.plot(self.df['x'].tail(1),self.df['y'].tail(1),'o',color=self.color,ms=self.radius*10)
                        self.pla = ax.plot(ptmp[:,0],ptmp[:,1],'r')

                    # try:
                    #     fig,ax=plu.displot(p[:2],p[:2]+v[:2],'r')
                    # except:
                    #     pass
                    # import ipdb
                    # ipdb.set_trace()
                    plt.draw()
                    plt.pause(0.0001) 
                if 'mysql' in self.save:
                    self.db.writemeca(self.ID,self.sim.now(),p,v,a)

                if 'txt' in self.save:
                    pyu.writemeca(self.ID,self.sim.now(),p,v,a)

                # new target when arrived in poi

                if self.arrived and\
                    (self.L.pt2ro(self.position) ==\
                        self.L.Gw.node[self.rooms[1]]['room']):

                    self.arrived = False
                    if self.endpoint:
                        self.endpoint=False
                        self.roomId = self.nextroomId
                        # remove the remaining waypoint which correspond 
                        # to current room position
#.........这里部分代码省略.........