def update(self, elapsed_seconds, force=None, extra_drag=0):
        # Apply force if necessary.
        if force is not None:
            self.velocity = vec.add(
                self.velocity,
                vec.mul(force, (elapsed_seconds / self.mass)),
            )

        # Apply drag.
        # Decrease the magnitude of the velocity vector by
        # the amount of drag.
        drag = (self.drag_rate + extra_drag) * elapsed_seconds
        if drag > self.speed:
            self.velocity = (0, 0)
        elif drag != 0 and self.velocity != (0, 0):
            drag_vector = vec.norm(self.velocity, -drag)
            self.velocity = vec.add(
                self.velocity,
                drag_vector,
            )

        # Limit to maximum speed.
        if self.speed > c.max_speed:
            self.velocity = vec.norm(self.velocity, c.max_speed)

        # Update position based on velocity.
        self.last_pos = self.pos
        self.pos = vec.add(
            self.pos,
            vec.mul(self.velocity, elapsed_seconds),
        )
示例#2
0
    def draw_player(self, player):
        # Show whether or not the player is thrusting.
        if player.do_thrust:
            trailing_point = vec.add(
                player.pos,
                vec.norm(player.direction, -1.5 * player.radius),
            )
            self.line(THRUST_COLOR, player.pos, trailing_point,
                      .5 * player.radius)
        if player.boost_heavy_time_remaining > 0.0:
            trailing_point = vec.add(
                player.pos,
                vec.norm(player.direction, -2.0 * player.radius),
            )
            self.line(THRUST_COLOR, player.pos, trailing_point,
                      1.5 * player.radius)

        # Show braking and boosting charge up.
        if player.do_brake:
            # Redden the brake color as we charge.
            color = vec.add(
                BRAKE_COLOR,
                vec.mul(THRUST_COLOR,
                        player.boost_charge_time / c.player_boost_ready_time))
            # Vibrate if we are fully charged.
            r = 1.2
            if player.boost_charge_time == c.player_boost_ready_time:
                r += 0.1 * math.sin(6 *
                                    (2 * math.pi) * pg.time.get_ticks() / 1000)
            self.circle(color, player.pos, r)

        # Body.
        self.circle(PLAYER_COLORS[player.number], player.pos, player.radius)

        # Show the player pointing towards a direction.
        leading_point = vec.add(
            player.pos,
            vec.norm(player.direction, player.radius),
        )
        self.line(DIRECTION_COLOR, player.pos, leading_point,
                  .2 * player.radius)

        if SHOW_RUDDER_FORCE:
            point = vec.add(
                player.pos,
                vec.mul(player.rudder_force, .1),
            )
            self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
        if SHOW_INTENDED_DIRECTION and hasattr(player, 'intended_direction'):
            point = vec.add(
                player.pos,
                player.intended_direction,
            )
            self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
示例#3
0
    def eval_spike_rush(self, ball_pos):
        dist = norm(ball_pos - self.pos)
        if dist > 160:
            self.has_ball_spiked = False

        rel_pos = dot(ball_pos - self.pos, self.rot)
        self._ball_last_rel_poss[self._next_rel_pos_to_replace] = rel_pos
        self._next_rel_pos_to_replace = (self._next_rel_pos_to_replace + 1) % 3

        change = norm(self._ball_last_rel_poss[0] - self._ball_last_rel_poss[1]) + \
                 norm(self._ball_last_rel_poss[1] - self._ball_last_rel_poss[2])

        self.has_ball_spiked = change < 1 and dist < 200
示例#4
0
def intersect_circles(center1, radius1, center2, radius2):
    radius1 = abs(radius1)
    radius2 = abs(radius2)

    if radius2 > radius1:
        return intersect_circles(center2, radius2, center1, radius1)

    transverse = vec.vfrom(center1, center2)
    dist = vec.mag(transverse)

    # Check for identical or concentric circles. These will have either
    # no points in common or all points in common, and in either case, we
    # return an empty list.
    if points_equal(center1, center2):
        return []

    # Check for exterior or interior tangent.
    radius_sum = radius1 + radius2
    radius_difference = abs(radius1 - radius2)
    if (float_equal(dist, radius_sum) or float_equal(dist, radius_difference)):
        return [
            vec.add(center1, vec.norm(transverse, radius1)),
        ]

    # Check for non intersecting circles.
    if dist > radius_sum or dist < radius_difference:
        return []

    # If we've reached this point, we know that the two circles intersect
    # in two distinct points.
    # Reference:
    # http://mathworld.wolfram.com/Circle-CircleIntersection.html

    # Pretend that the circles are arranged along the x-axis.
    # Find the x-value of the intersection points, which is the same for both
    # points. Then find the chord length "a" between the two intersection
    # points, and use vector math to find the points.
    dist2 = vec.mag2(transverse)
    x = (dist2 - radius2**2 + radius1**2) / (2 * dist)
    a = ((1 / dist) * sqrt(
        (-dist + radius1 - radius2) * (-dist - radius1 + radius2) *
        (-dist + radius1 + radius2) * (dist + radius1 + radius2)))
    chord_middle = vec.add(
        center1,
        vec.norm(transverse, x),
    )
    perp = vec.perp(transverse)
    return [
        vec.add(chord_middle, vec.norm(perp, a / 2)),
        vec.add(chord_middle, vec.norm(perp, -a / 2)),
    ]
    def draw_player(self, player):
        # Show whether or not the player is thrusting.
        if player.do_thrust:
            trailing_point = vec.add(
                player.pos,
                vec.norm(player.direction, -1.5 * player.radius),
            )
            self.line(THRUST_COLOR, player.pos, trailing_point, .5 * player.radius)
        if player.boost_heavy_time_remaining > 0.0:
            trailing_point = vec.add(
                player.pos,
                vec.norm(player.direction, -2.0 * player.radius),
            )
            self.line(THRUST_COLOR, player.pos, trailing_point, 1.5 * player.radius)

        # Show braking and boosting charge up.
        if player.do_brake:
            # Redden the brake color as we charge.
            color = vec.add(
                BRAKE_COLOR,
                vec.mul(THRUST_COLOR, player.boost_charge_time / c.player_boost_ready_time)
            )
            # Vibrate if we are fully charged.
            r = 1.2
            if player.boost_charge_time == c.player_boost_ready_time:
                r += 0.1 * math.sin(6 * (2*math.pi) * pg.time.get_ticks() / 1000)
            self.circle(color, player.pos, r)

        # Body.
        self.circle(PLAYER_COLORS[player.number], player.pos, player.radius)

        # Show the player pointing towards a direction.
        leading_point = vec.add(
            player.pos,
            vec.norm(player.direction, player.radius),
        )
        self.line(DIRECTION_COLOR, player.pos, leading_point, .2 * player.radius)

        if SHOW_RUDDER_FORCE:
            point = vec.add(
                player.pos,
                vec.mul(player.rudder_force, .1),
            )
            self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
        if SHOW_INTENDED_DIRECTION and hasattr(player, 'intended_direction'):
            point = vec.add(
                player.pos,
                player.intended_direction,
            )
            self.line(THRUST_COLOR, player.pos, point, .1 * player.radius)
示例#6
0
    def predict_hit_pos(self, from_pos):
        speed = SUPER_SPEED if self.next_is_super else NORMAL_SPEED
        ball_prediction = self.get_ball_prediction_struct()

        if ball_prediction is not None:
            TIME_PER_SLICES = 1 / 60.0
            SLICES = 360

            # Iterate to find the first location which can be hit
            for i in range(0, 360):
                time = i * TIME_PER_SLICES
                rlpos = ball_prediction.slices[i].physics.location
                pos = Vec3(rlpos.x, rlpos.y, rlpos.z)
                dist = norm(from_pos - pos)
                travel_time = dist / speed
                if time >= travel_time:
                    # Add small bias for aiming
                    tsign = -1 if self.team == 0 else 1
                    enemy_goal = Vec3(0, tsign * -5030, 300)
                    bias_direction = normalize(pos - enemy_goal)
                    pos = pos + bias_direction * GOAL_AIM_BIAS_AMOUNT
                    return pos, travel_time

            # Use last
            rlpos = ball_prediction.slices[SLICES - 1].physics.location
            return Vec3(rlpos.x, rlpos.y, rlpos.z), 6

        return Vec3(0, 0, 0), 5
示例#7
0
def sphere_line_intersectQ(line, r, pos):
    v0 = line[0]
    v1 = line[1]
    dv = v1 - v0
    dv_norm = vec.norm(dv)
    dv = dv / dv_norm

    v0_rel = v0 - pos
    v0r_dv = vec.dot(v0_rel, dv)

    discr = (v0r_dv)**2 - vec.dot(v0_rel, v0_rel) + r * r

    #print('discr',discr)
    #no intersection with line
    if discr < 0:
        return False

    sqrt_discr = math.sqrt(discr)
    tm = -v0r_dv - sqrt_discr
    tp = -v0r_dv + sqrt_discr

    #print('tm,tp',tm,tp)
    #no intersection with line segment
    if tm > dv_norm and tp > dv_norm:
        return False
    if tm < 0 and tp < 0:
        return False

    return True
示例#8
0
def sphere_line_intersect(line, r):

    v0 = line[0]
    v1 = line[1]
    dv = v1 - v0
    dv_norm = vec.norm(dv)
    dv = dv / dv_norm

    #in our case, sphere center is the origin
    v0_rel = v0  # - sphere_center
    v0r_dv = vec.dot(v0_rel, dv)

    discr = (v0r_dv)**2 - vec.dot(v0_rel, v0_rel) + r * r

    #print('discr',discr)
    #no intersection with line
    if discr < 0:
        return None

    sqrt_discr = math.sqrt(discr)
    tm = -v0r_dv - sqrt_discr
    tp = -v0r_dv + sqrt_discr

    #print('tm,tp',tm,tp)
    #no intersection with line segment
    if tm > dv_norm and tp > dv_norm:
        return None
    if tm < 0 and tp < 0:
        return None
    intersect_points = [v0 + tm * dv, v0 + tp * dv]

    return intersect_points
示例#9
0
    def __init__(self, filename):
        craftFile = open(filename,'r')
        data = json.load(craftFile)
        
        self.thrusterLocations = data[0]
        self.thrusterOrientations = data[1]
        self.accelerationMax = data[2]
        self.angularAccelerationMax = data[3]
        # each thruster provides accelerationMin = 0, and angularAccelerationMin = 0 (can't push backward)
        
        self.radius = data[4] #until a more sophisticated collision algorithm is implemented, use a sphere

        self.numThr = len(data[0])
        
        self.accelMatrix = []
        self.alphaMatrix = []

        self.totalAccelMax = 0 # the maximum acceleration magnitude the spacecraft can reach
        self.totalAlphaMax = 0 # as above for angular acceleration

        
        for i in range(2**self.numThr):
                      
            thrusterFlags = [ (i//(2**j))%2 for j in range(self.numThr)] # creates a list w/length numThr
                                                                          # filled with 1's and 0's, unique for
                                                                          # each i, covering all possible
                                                                          # combinations.

            accel = [
                sum(
                    [self.accelerationMax[j][k]*thrusterFlags[j] for j in range(self.numThr)]) 
                for k in range(3)
            ]
            if vec.norm(accel) > self.totalAccelMax:
                self.totalAccelMax = vec.norm(accel)
            
            alpha = [
                sum(
                    [self.angularAccelerationMax[j][k]*thrusterFlags[j] for j in range(self.numThr)])
                for k in range(3)
            ]
            if vec.norm(alpha) > self.totalAlphaMax:
                self.totalAlphaMax = vec.norm(alpha)

            self.accelMatrix.append(accel)
            self.alphaMatrix.append(alpha)
示例#10
0
def collide_particles(p1, p2):
    restitution = p1.restitution * p2.restitution

    # Don't collide immovable particles.
    if p1.immovable and p2.immovable:
        return

    # Test if p1 and p2 are actually intersecting.
    if not intersect(p1, p2):
        return

    # If one particle is immovable, make it the first one.
    if not p1.immovable and p2.immovable:
        p1, p2 = p2, p1

    # Vector spanning between the centers, normal to contact surface.
    v_span = vec.vfrom(p1.pos, p2.pos)

    # Split into normal and tangential components and calculate
    # initial velocities.
    normal = vec.norm(v_span)
    tangent = vec.perp(normal)
    v1_tangent = vec.proj(p1.velocity, tangent)
    v2_tangent = vec.proj(p2.velocity, tangent)
    p1_initial = vec.dot(p1.velocity, normal)
    p2_initial = vec.dot(p2.velocity, normal)

    # Don't collide if particles were actually moving away from each other, so
    # they don't get stuck inside one another.
    if p1_initial - p2_initial < 0:
        return

    # Handle immovable particles specially.
    if p1.immovable:
        p2_final = -p2_initial * restitution
        p2.velocity = vec.add(
            v2_tangent,
            vec.mul(normal, p2_final),
        )
        return

    # Elastic collision equations along normal component.
    m1, m2 = p1.mass, p2.mass
    m1plusm2 = (m1 + m2) / restitution
    p1_final = (p1_initial * (m1 - m2) / m1plusm2 + p2_initial *
                (2 * m2) / m1plusm2)
    p2_final = (p2_initial * (m2 - m1) / m1plusm2 + p1_initial *
                (2 * m1) / m1plusm2)

    # Tangential component is unchanged, recombine.
    p1.velocity = vec.add(
        v1_tangent,
        vec.mul(normal, p1_final),
    )
    p2.velocity = vec.add(
        v2_tangent,
        vec.mul(normal, p2_final),
    )
示例#11
0
def test_bumper():
    northwest = vec.norm((-1, -1))
    northeast = vec.norm((1, -1))

    env = Environment()
    env.load_level([
        (Bumper, dict(pos=(0,0), velocity=(0,0), radius=3)),
        (Particle, dict(pos=(6, 0), velocity=vec.norm(northwest, c.max_speed), mass=1, radius=3.1)),
    ])

    # The particle starts out heading northwest.
    bumper, particle = env.particles
    assert_vectors_equal(vec.norm(particle.velocity), northwest)

    # After it bounces, it goes northeast.
    for _ in range(10):
        env.update(1)
        assert_vectors_equal(vec.norm(particle.velocity), northeast)
示例#12
0
    def calc_rudder_force(self):
        # We continuously bring the direction of the player's movement to be
        # closer in line with the direction it is facing.
        target_velocity = vec.norm(self.direction, self.speed)
        force = vec.vfrom(self.velocity, target_velocity)
        if force == (0, 0):
            return (0, 0)

        # The strength of the rudder is highest when acting perpendicular to
        # the direction of movement.
        v_perp = vec.norm(vec.perp(self.velocity))
        angle_multiplier = abs(vec.dot(v_perp, self.direction))
        strength = self.speed * c.player_rudder_strength
        strength = min(strength, c.player_max_rudder_strength)
        strength *= angle_multiplier
        if strength == 0:
            return (0, 0)

        force = vec.norm(force, strength)
        return force
示例#13
0
    def calc_rudder_force(self):
        # We continuously bring the direction of the player's movement to be
        # closer in line with the direction it is facing.
        target_velocity = vec.norm(self.direction, self.speed)
        force = vec.vfrom(self.velocity, target_velocity)
        if force == (0, 0):
            return (0, 0)

        # The strength of the rudder is highest when acting perpendicular to
        # the direction of movement.
        v_perp = vec.norm(vec.perp(self.velocity))
        angle_multiplier = abs(vec.dot(v_perp, self.direction))
        strength = self.speed * c.player_rudder_strength
        strength = min(strength, c.player_max_rudder_strength)
        strength *= angle_multiplier
        if strength == 0:
            return (0, 0)

        force = vec.norm(force, strength)
        return force
示例#14
0
def draw_graph(graph):
    gap_size = 0.25

    p = canoepaddle.Pen()
    p.stroke_mode(0.1, 'black')
    for a, b in graph_edges(graph):
        gap = vec.norm(vec.vfrom(a, b), gap_size)
        p.move_to(vec.add(a, gap))
        p.line_to(vec.sub(b, gap))

    return p.paper
示例#15
0
    def update(self, elapsed_seconds, force=None, extra_drag=0):
        # Apply force if necessary.
        if force is not None:
            self.velocity = vec.add(self.velocity, vec.mul(force, (elapsed_seconds / self.mass)))

        # Apply drag.
        # Decrease the magnitude of the velocity vector by
        # the amount of drag.
        drag = (self.drag_rate + extra_drag) * elapsed_seconds
        if drag > self.speed:
            self.velocity = (0, 0)
        elif drag != 0 and self.velocity != (0, 0):
            drag_vector = vec.norm(self.velocity, -drag)
            self.velocity = vec.add(self.velocity, drag_vector)

        # Limit to maximum speed.
        if self.speed > c.max_speed:
            self.velocity = vec.norm(self.velocity, c.max_speed)

        # Update position based on velocity.
        self.last_pos = self.pos
        self.pos = vec.add(self.pos, vec.mul(self.velocity, elapsed_seconds))
示例#16
0
 def rebound(self, normal, point=None, restitution=1):
     # Split into normal and tangential components.
     tangent = vec.perp(normal)
     v_tangent = vec.proj(self.velocity, tangent)
     v_normal = vec.proj(self.velocity, normal)
     # Invert normal component and recombine, with restitution.
     v_normal = vec.neg(v_normal)
     self.velocity = vec.add(v_tangent, vec.mul(v_normal, restitution))
     # If the particle is partially inside the wall, move it out.
     if point is not None:
         v = vec.vfrom(point, self.pos)
         if vec.mag2(v) < self.radius ** 2:
             v = vec.norm(v, self.radius)
             self.pos = vec.add(point, v)
示例#17
0
    def join_with_arc(self, other):
        # Special case coincident arcs.
        if points_equal(self.center, other.center):
            if not (
                float_equal(self.radius, other.radius) and
                float_equal(self.width, other.width)
            ):
                self.end_joint_illegal = True
                other.start_joint_illegal = True
                return

            r = vec.vfrom(self.center, self.b)
            if self.radius < 0:
                r = vec.neg(r)
            v_left = vec.norm(r, self.radius - self.width / 2)
            self.b_left = other.a_left = Point(*vec.add(self.center, v_left))
            v_right = vec.norm(r, self.radius + self.width / 2)
            self.b_right = other.a_right = Point(*vec.add(self.center, v_right))
            return

        c1, r1 = self.offset_circle_left()
        c2, r2 = other.offset_circle_left()
        points_left = intersect_circles(c1, r1, c2, r2)
        if len(points_left) > 0:
            p = Point(*closest_point_to(self.b, points_left))
            self.b_left = other.a_left = p

        c1, r1 = self.offset_circle_right()
        c2, r2 = other.offset_circle_right()
        points_right = intersect_circles(c1, r1, c2, r2)
        if len(points_right) > 0:
            p = Point(*closest_point_to(self.b, points_right))
            self.b_right = other.a_right = p

        if len(points_left) == 0 or len(points_right) == 0:
            self.end_joint_illegal = True
            other.start_joint_illegal = True
示例#18
0
    def join_with_arc(self, other):
        # Special case coincident arcs.
        if points_equal(self.center, other.center):
            if not (
                float_equal(self.radius, other.radius)
                and float_equal(self.width, other.width)
            ):
                self.end_joint_illegal = True
                other.start_joint_illegal = True
                return

            r = vec.vfrom(self.center, self.b)
            if self.radius < 0:
                r = vec.neg(r)
            v_left = vec.norm(r, self.radius - self.width / 2)
            self.b_left = other.a_left = Point(*vec.add(self.center, v_left))
            v_right = vec.norm(r, self.radius + self.width / 2)
            self.b_right = other.a_right = Point(*vec.add(self.center, v_right))
            return

        c1, r1 = self.offset_circle_left()
        c2, r2 = other.offset_circle_left()
        points_left = intersect_circles(c1, r1, c2, r2)
        if len(points_left) > 0:
            p = Point(*closest_point_to(self.b, points_left))
            self.b_left = other.a_left = p

        c1, r1 = self.offset_circle_right()
        c2, r2 = other.offset_circle_right()
        points_right = intersect_circles(c1, r1, c2, r2)
        if len(points_right) > 0:
            p = Point(*closest_point_to(self.b, points_right))
            self.b_right = other.a_right = p

        if len(points_left) == 0 or len(points_right) == 0:
            self.end_joint_illegal = True
            other.start_joint_illegal = True
示例#19
0
def collide_particles(p1, p2):
    restitution = p1.restitution * p2.restitution

    # Don't collide immovable particles.
    if p1.immovable and p2.immovable:
        return

    # Test if p1 and p2 are actually intersecting.
    if not intersect(p1, p2):
        return

    # If one particle is immovable, make it the first one.
    if not p1.immovable and p2.immovable:
        p1, p2 = p2, p1

    # Vector spanning between the centers, normal to contact surface.
    v_span = vec.vfrom(p1.pos, p2.pos)

    # Split into normal and tangential components and calculate
    # initial velocities.
    normal = vec.norm(v_span)
    tangent = vec.perp(normal)
    v1_tangent = vec.proj(p1.velocity, tangent)
    v2_tangent = vec.proj(p2.velocity, tangent)
    p1_initial = vec.dot(p1.velocity, normal)
    p2_initial = vec.dot(p2.velocity, normal)

    # Don't collide if particles were actually moving away from each other, so
    # they don't get stuck inside one another.
    if p1_initial - p2_initial < 0:
        return

    # Handle immovable particles specially.
    if p1.immovable:
        p2_final = -p2_initial * restitution
        p2.velocity = vec.add(v2_tangent, vec.mul(normal, p2_final))
        return

    # Elastic collision equations along normal component.
    m1, m2 = p1.mass, p2.mass
    m1plusm2 = (m1 + m2) / restitution
    p1_final = p1_initial * (m1 - m2) / m1plusm2 + p2_initial * (2 * m2) / m1plusm2
    p2_final = p2_initial * (m2 - m1) / m1plusm2 + p1_initial * (2 * m1) / m1plusm2

    # Tangential component is unchanged, recombine.
    p1.velocity = vec.add(v1_tangent, vec.mul(normal, p1_final))
    p2.velocity = vec.add(v2_tangent, vec.mul(normal, p2_final))
示例#20
0
 def rebound(self, normal, point=None, restitution=1):
     # Split into normal and tangential components.
     tangent = vec.perp(normal)
     v_tangent = vec.proj(self.velocity, tangent)
     v_normal = vec.proj(self.velocity, normal)
     # Invert normal component and recombine, with restitution.
     v_normal = vec.neg(v_normal)
     self.velocity = vec.add(
         v_tangent,
         vec.mul(v_normal, restitution),
     )
     # If the particle is partially inside the wall, move it out.
     if point is not None:
         v = vec.vfrom(point, self.pos)
         if vec.mag2(v) < self.radius**2:
             v = vec.norm(v, self.radius)
             self.pos = vec.add(point, v)
示例#21
0
    def join_with_line(self, other):
        v_self = self._vector()
        v_other = other._vector()

        # Check turn angle.
        self_heading = Heading.from_rad(vec.heading(v_self))
        other_heading = Heading.from_rad(vec.heading(v_other))
        turn_angle = self_heading.angle_to(other_heading)

        # Special case equal widths.
        if(
            abs(turn_angle) <= MAX_TURN_ANGLE
            and float_equal(self.width, other.width)
        ):
            # When joints between segments of equal width are straight or
            # almost straight, the line-intersection method becomes very
            # numerically unstable, so use another method instead.

            # For each segment, get a vector perpendicular to the
            # segment, then add them. This is an angle bisector for
            # the angle of the joint.
            w_self = self._width_vector()
            w_other = other._width_vector()
            v_bisect = vec.add(w_self, w_other)

            # Make the bisector have the correct length.
            half_angle = vec.angle(v_other, v_bisect)
            v_bisect = vec.norm(
                v_bisect,
                (self.width / 2) / math.sin(half_angle)
            )

            # Determine the left and right joint spots.
            p_left = vec.add(self.b, v_bisect)
            p_right = vec.sub(self.b, v_bisect)
        else:
            a, b = self.offset_line_left()
            c, d = other.offset_line_left()
            p_left = intersect_lines(a, b, c, d)

            a, b = self.offset_line_right()
            c, d = other.offset_line_right()
            p_right = intersect_lines(a, b, c, d)

        # Make sure the joint points are "forward" from the perspective
        # of each segment.
        if p_left is not None:
            if vec.dot(vec.vfrom(self.a_left, p_left), v_self) < 0:
                p_left = None
        if p_right is not None:
            if vec.dot(vec.vfrom(self.a_right, p_right), v_self) < 0:
                p_right = None

        # Don't join the outer sides if the turn angle is too steep.
        if abs(turn_angle) > MAX_TURN_ANGLE:
            if turn_angle > 0:
                p_right = None
            else:
                p_left = None

        if p_left is not None:
            self.b_left = other.a_left = Point(*p_left)
        if p_right is not None:
            self.b_right = other.a_right = Point(*p_right)

        if p_left is None or p_right is None:
            self.end_joint_illegal = True
            other.start_joint_illegal = True
示例#22
0
    def heuristic(self, state, spacecraft):
        # print('placeholder4')

        
        
        # DEFINE HEURISTIC HERE
        # Don't refer to self.firstNode or self.firstHeuristic
        
        
        # See notes file part 1
        
        if not rotation:

            amax = spacecraft.totalAccelMax
            alpmax = spacecraft.totalAlphaMax
            v0 = state.v
            th0 = state.th
#            worstTime = 0

            # There is a critical speed along an axis such that if the spacecraft decelerates at its maximum
            # rate it will stop exactly at the goal. The first contribution to the heuristic will be how
            # different the spacecraft's actual velocity v0 is to the critical velocity, vc. The second
            # contribution is the time it will take to decelerate to the goal if its speed were equal to the
            # critical velocity



            deltaP = []
            
            deltaP.append(self.finalState.p[0] - state.p[0])
            deltaP.append(self.finalState.p[1] - state.p[1])
            deltaP.append(self.finalState.p[2] - state.p[2])

            deltaTh = []
            deltaTh.append(self.finalState.th[0] - state.th[0])
            deltaTh.append(self.finalState.th[1] - state.th[1])
            deltaTh.append(self.finalState.th[2] - state.th[2])
            # h = []
            
            # for i in range(3):
            #     if abs(deltaP[i]) < self.dp:
            #         h.append( v0[i]**4 / amax**4)
            #         # if h > worstTime:
            #         #     worstTime = h
            #         #     continue
            #         continue
                
            #     c1 = deltaP[i]**2 / (amax*amax)
            #     c2 = v0[i]*abs(v0[i]) / (amax*deltaP[i])
            #     h.append(c1*(c2**2 + 4*c2 + 8))
            #     # if h > worstTime:
            #     #     worstTime = h
            # # return worstTime

            normDeltaP = vec.norm(deltaP)

            normal = vec.smul(1/normDeltaP,deltaP)

            normalV = vec.dot(v0,normal)

            normV = vec.norm(v0)

            tangentialV = math.sqrt(normV**2 - normalV**2)
            
            vcrit = math.sqrt(2*amax*normDeltaP)

            t1 = 0
            
            if normalV < 0: #spacraft is heading the wrong way
                tstop = -normalV/amax #time required to stop
                t1 = tstop + self.ramp((1/2)*amax*(tstop**2) + normDeltaP,amax)
            elif normalV > vcrit: #spacecraft is moving too fast and will overshoot
                tstop = normalV/amax
                t1 = tstop + self.ramp((1/2)*amax*(tstop**2) - normDeltaP,amax)
            else: #spacecraft is heading the right way, slow enough to stop in time
                negTStop = -normalV/amax #represents the time in the past the ramp would have started
                t1 = negTStop + self.ramp((1/2)*amax*(negTStop**2) + normDeltaP,amax)
            # t1 is the heuristic if there is no tangential velocity
            # at the very least, the tangential velocity needs to be cancelled (taking extra time)
            h = t1 + tangentialV/amax + math.sqrt((deltaTh[0]/alpmax)**2 + (deltaTh[1]/alpmax)**2 + (deltaTh[2]/alpmax)**2)

            return h
        raise NotImplementedError('Rotating Spacecraft Heuristic Not Implemented')
示例#23
0
 def _width_vector(self):
     v = self._vector()
     v = vec.perp(v)
     v = vec.norm(v, self.width / 2)
     return v
示例#24
0
def intersect_circles(center1, radius1, center2, radius2):
    radius1 = abs(radius1)
    radius2 = abs(radius2)

    if radius2 > radius1:
        return intersect_circles(center2, radius2, center1, radius1)

    transverse = vec.vfrom(center1, center2)
    dist = vec.mag(transverse)

    # Check for identical or concentric circles. These will have either
    # no points in common or all points in common, and in either case, we
    # return an empty list.
    if points_equal(center1, center2):
        return []

    # Check for exterior or interior tangent.
    radius_sum = radius1 + radius2
    radius_difference = abs(radius1 - radius2)
    if (
        float_equal(dist, radius_sum) or
        float_equal(dist, radius_difference)
    ):
        return [
            vec.add(
                center1,
                vec.norm(transverse, radius1)
            ),
        ]

    # Check for non intersecting circles.
    if dist > radius_sum or dist < radius_difference:
        return []

    # If we've reached this point, we know that the two circles intersect
    # in two distinct points.
    # Reference:
    # http://mathworld.wolfram.com/Circle-CircleIntersection.html

    # Pretend that the circles are arranged along the x-axis.
    # Find the x-value of the intersection points, which is the same for both
    # points. Then find the chord length "a" between the two intersection
    # points, and use vector math to find the points.
    dist2 = vec.mag2(transverse)
    x = (dist2 - radius2**2 + radius1**2) / (2 * dist)
    a = (
        (1 / dist) *
        sqrt(
            (-dist + radius1 - radius2) *
            (-dist - radius1 + radius2) *
            (-dist + radius1 + radius2) *
            (dist + radius1 + radius2)
        )
    )
    chord_middle = vec.add(
        center1,
        vec.norm(transverse, x),
    )
    perp = vec.perp(transverse)
    return [
        vec.add(chord_middle, vec.norm(perp, a / 2)),
        vec.add(chord_middle, vec.norm(perp, -a / 2)),
    ]
示例#25
0
    def join_with_line(self, other):
        v_self = self._vector()
        v_other = other._vector()

        # Check turn angle.
        self_heading = Heading.from_rad(vec.heading(v_self))
        other_heading = Heading.from_rad(vec.heading(v_other))
        turn_angle = self_heading.angle_to(other_heading)

        # Special case equal widths.
        if(
            abs(turn_angle) <= MAX_TURN_ANGLE and
            float_equal(self.width, other.width)
        ):
            # When joints between segments of equal width are straight or
            # almost straight, the line-intersection method becomes very
            # numerically unstable, so use another method instead.

            # For each segment, get a vector perpendicular to the
            # segment, then add them. This is an angle bisector for
            # the angle of the joint.
            w_self = self._width_vector()
            w_other = other._width_vector()
            v_bisect = vec.add(w_self, w_other)

            # Make the bisector have the correct length.
            half_angle = vec.angle(v_other, v_bisect)
            v_bisect = vec.norm(
                v_bisect,
                (self.width / 2) / math.sin(half_angle)
            )

            # Determine the left and right joint spots.
            p_left = vec.add(self.b, v_bisect)
            p_right = vec.sub(self.b, v_bisect)
        else:
            a, b = self.offset_line_left()
            c, d = other.offset_line_left()
            p_left = intersect_lines(a, b, c, d)

            a, b = self.offset_line_right()
            c, d = other.offset_line_right()
            p_right = intersect_lines(a, b, c, d)

        # Make sure the joint points are "forward" from the perspective
        # of each segment.
        if p_left is not None:
            if vec.dot(vec.vfrom(self.a_left, p_left), v_self) < 0:
                p_left = None
        if p_right is not None:
            if vec.dot(vec.vfrom(self.a_right, p_right), v_self) < 0:
                p_right = None

        # Don't join the outer sides if the turn angle is too steep.
        if abs(turn_angle) > MAX_TURN_ANGLE:
            if turn_angle > 0:
                p_right = None
            else:
                p_left = None

        if p_left is not None:
            self.b_left = other.a_left = Point(*p_left)
        if p_right is not None:
            self.b_right = other.a_right = Point(*p_right)

        if p_left is None or p_right is None:
            self.end_joint_illegal = True
            other.start_joint_illegal = True
示例#26
0
 def _width_vector(self):
     v = self._vector()
     v = vec.perp(v)
     v = vec.norm(v, self.width / 2)
     return v