예제 #1
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
예제 #2
0
def distance(f: Face, p: Point) -> float:
    # http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.104.4264&rep=rep1&type=pdf
    # p point projection to plane which face f lays in
    # normal Np of P1P2P3
    np = cross(Vector(f.p1, f.p2), Vector(f.p1, f.p3))

    # the angle a between normal Np and P1P0
    cosa = dot(Vector(f.p1, p), np) / (np.length() * Vector(f.p1, p).length())

    # the length of P0P0'
    p0p0l = Vector(p, f.p1).length() * cosa

    # the vector P0P0'
    p0p0p = np * ((-1) * (p0p0l) / np.length())

    # point P0' from P0 and P0P0' vector
    p0p = p + p0p0p

    # compute baricentric coordinates
    area = cross(Vector(f.p1, f.p2), Vector(f.p1, f.p3)).length() / 2
    a = cross(Vector(p0p, f.p2), Vector(p0p, f.p3)).length() / (2 * area)
    b = cross(Vector(p0p, f.p3), Vector(p0p, f.p1)).length() / (2 * area)
    c = 1 - a - b

    if 0 <= a <= 1 and 0 <= b <= 1 and 0 <= c <= 1:
        return sqrt(dot(p0p0p, p0p0p))
    else:
        e1 = Edge(f.p1, f.p2)
        e2 = Edge(f.p2, f.p3)
        e3 = Edge(f.p1, f.p3)

        return min(distance(e1, p), distance(e2, p), distance(e3, p),
                   distance(f.p1, p), distance(f.p2, p), distance(f.p3, p))
예제 #3
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
예제 #4
0
    def arc_to(self, endpoint, center=None, start_slant=None, end_slant=None):
        """
        Draw an arc ending at the specified point, starting tangent to the
        current position and heading.
        """
        if points_equal(self._position, endpoint):
            return
        # Handle unspecified center.
        # We need to find the center of the arc, so we can find its radius. The
        # center of this arc is uniquely defined by the intersection of two
        # lines:
        # 1. The first line is perpendicular to the pen heading, passing
        #    through the pen position.
        # 2. The second line is the perpendicular bisector of the pen position
        #    and the target arc end point.
        v_pen = self._vector()
        v_perp = vec.perp(self._vector())
        v_chord = vec.vfrom(self._position, endpoint)
        if center is None:
            midpoint = vec.div(vec.add(self._position, endpoint), 2)
            v_bisector = vec.perp(v_chord)
            center = intersect_lines(
                self._position,
                vec.add(self._position, v_perp),
                midpoint,
                vec.add(midpoint, v_bisector),
            )

        # Determine true start heading. This may not be the same as the
        # original pen heading in some circumstances.
        assert not points_equal(center, self._position)
        v_radius_start = vec.vfrom(center, self._position)
        v_radius_perp = vec.perp(v_radius_start)
        if vec.dot(v_radius_perp, v_pen) < 0:
            v_radius_perp = vec.neg(v_radius_perp)
        start_heading = math.degrees(vec.heading(v_radius_perp))
        self.turn_to(start_heading)
        # Refresh v_pen and v_perp based on the new start heading.
        v_pen = self._vector()
        v_perp = vec.perp(self._vector())

        # Calculate the arc angle.
        # The arc angle is double the angle between the pen vector and the
        # chord vector. Arcing to the left is a positive angle, and arcing to
        # the right is a negative angle.
        arc_angle = 2 * math.degrees(vec.angle(v_pen, v_chord))
        radius = vec.mag(v_radius_start)
        # Check which side of v_pen the goes toward.
        if vec.dot(v_chord, v_perp) < 0:
            arc_angle = -arc_angle
            radius = -radius

        self._arc(
            center,
            radius,
            endpoint,
            arc_angle,
            start_slant,
            end_slant,
        )
예제 #5
0
def intersect_lines(a, b, c, d, segment=False):
    """
    Find the intersection of lines a-b and c-d.

    If the "segment" argument is true, treat the lines as segments, and check
    whether the intersection point is off the end of either segment.
    """
    # Reference:
    # http://geomalgorithms.com/a05-_intersect-1.html
    u = vec.vfrom(a, b)
    v = vec.vfrom(c, d)
    w = vec.vfrom(c, a)

    u_perp_dot_v = vec.dot(vec.perp(u), v)
    if float_equal(u_perp_dot_v, 0):
        return None  # We have collinear segments, no single intersection.

    v_perp_dot_w = vec.dot(vec.perp(v), w)
    s = v_perp_dot_w / u_perp_dot_v
    if segment and (s < 0 or s > 1):
        return None

    u_perp_dot_w = vec.dot(vec.perp(u), w)
    t = u_perp_dot_w / u_perp_dot_v
    if segment and (t < 0 or t > 1):
        return None

    return vec.add(a, vec.mul(u, s))
예제 #6
0
    def arc_to(self, endpoint, center=None, start_slant=None, end_slant=None):
        """
        Draw an arc ending at the specified point, starting tangent to the
        current position and heading.
        """
        if points_equal(self._position, endpoint):
            return
        # Handle unspecified center.
        # We need to find the center of the arc, so we can find its radius. The
        # center of this arc is uniquely defined by the intersection of two
        # lines:
        # 1. The first line is perpendicular to the pen heading, passing
        #    through the pen position.
        # 2. The second line is the perpendicular bisector of the pen position
        #    and the target arc end point.
        v_pen = self._vector()
        v_perp = vec.perp(self._vector())
        v_chord = vec.vfrom(self._position, endpoint)
        if center is None:
            midpoint = vec.div(vec.add(self._position, endpoint), 2)
            v_bisector = vec.perp(v_chord)
            center = intersect_lines(
                self._position,
                vec.add(self._position, v_perp),
                midpoint,
                vec.add(midpoint, v_bisector),
            )

        # Determine true start heading. This may not be the same as the
        # original pen heading in some circumstances.
        assert not points_equal(center, self._position)
        v_radius_start = vec.vfrom(center, self._position)
        v_radius_perp = vec.perp(v_radius_start)
        if vec.dot(v_radius_perp, v_pen) < 0:
            v_radius_perp = vec.neg(v_radius_perp)
        start_heading = math.degrees(vec.heading(v_radius_perp))
        self.turn_to(start_heading)
        # Refresh v_pen and v_perp based on the new start heading.
        v_pen = self._vector()
        v_perp = vec.perp(self._vector())

        # Calculate the arc angle.
        # The arc angle is double the angle between the pen vector and the
        # chord vector. Arcing to the left is a positive angle, and arcing to
        # the right is a negative angle.
        arc_angle = 2 * math.degrees(vec.angle(v_pen, v_chord))
        radius = vec.mag(v_radius_start)
        # Check which side of v_pen the goes toward.
        if vec.dot(v_chord, v_perp) < 0:
            arc_angle = -arc_angle
            radius = -radius

        self._arc(
            center,
            radius,
            endpoint,
            arc_angle,
            start_slant,
            end_slant,
        )
예제 #7
0
def intersect_lines(a, b, c, d, segment=False):
    """
    Find the intersection of lines a-b and c-d.

    If the "segment" argument is true, treat the lines as segments, and check
    whether the intersection point is off the end of either segment.
    """
    # Reference:
    # http://geomalgorithms.com/a05-_intersect-1.html
    u = vec.vfrom(a, b)
    v = vec.vfrom(c, d)
    w = vec.vfrom(c, a)

    u_perp_dot_v = vec.dot(vec.perp(u), v)
    if float_equal(u_perp_dot_v, 0):
        return None  # We have collinear segments, no single intersection.

    v_perp_dot_w = vec.dot(vec.perp(v), w)
    s = v_perp_dot_w / u_perp_dot_v
    if segment and (s < 0 or s > 1):
        return None

    u_perp_dot_w = vec.dot(vec.perp(u), w)
    t = u_perp_dot_w / u_perp_dot_v
    if segment and (t < 0 or t > 1):
        return None

    return vec.add(a, vec.mul(u, s))
예제 #8
0
def clip_line_plane(line, plane, small_z=0):
    p0 = line[0]
    p1 = line[1]

    n = plane.normal
    th = plane.threshold + small_z

    p0n = vec.dot(p0, n)
    p1n = vec.dot(p1, n)

    p0_safe = p0n >= th
    p1_safe = p1n >= th
    #if both vertices are behind, draw neither
    if (not p0_safe) and (not p1_safe):
        return None
    #both vertices in front
    if p0_safe and p1_safe:
        return line
    #if one of the vertices is behind the camera
    t_intersect = (p0n - th) / (p0n - p1n)
    intersect = vec.linterp(p0, p1, t_intersect)
    if (not p0_safe) and p1_safe:
        return Line(intersect, p1)
    else:
        return Line(p0, intersect)
예제 #9
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),
    )
예제 #10
0
def clip_line(line, boundaries):
    p0 = line[0]
    p1 = line[1]

    a = 0.
    b = 1.
    p0_all_safe, p1_all_safe = False, False

    for boundary in boundaries:

        n = boundary.normal
        th = boundary.threshold

        p0n = vec.dot(p0, n)
        p1n = vec.dot(p1, n)

        p0_safe = p0n >= th
        p1_safe = p1n >= th

        if p0_safe and p1_safe:
            a = 0
            b = 1
            p0_all_safe = True
            p1_all_safe = True
            break
        #print('p0,p1 safe',p0_safe,p1_safe)
        if p0_safe and (not p1_safe):
            t_intersect = (p0n - th) / (p0n - p1n)
            a = max(a, t_intersect)
            #print('move a to',a)
        if (not p0_safe) and p1_safe:
            t_intersect = (p0n - th) / (p0n - p1n)
            b = min(t_intersect, b)
            #print('move b to',b)
        p0_all_safe = (p0_all_safe or p0_safe)
        p1_all_safe = (p1_all_safe or p1_safe)

    #print('all_safe',p0_all_safe,p1_all_safe)
    #both endpoints visible
    if p0_all_safe and p1_all_safe:
        #return two lines if we've intersected the shape
        if a > 0 and b < 1:
            return [
                Line(p0, vec.linterp(p0, p1, a)),
                Line(vec.linterp(p0, p1, b), p1)
            ]
        else:
            #return entire line if we haven't intersected the shape
            return [line]
    if p0_all_safe and (not p1_all_safe):
        return [Line(p0, vec.linterp(p0, p1, a))]
    if (not p0_all_safe) and p1_all_safe:
        return [Line(vec.linterp(p0, p1, b), p1)]
    #if neither point is visible, don't draw the line
    return []
예제 #11
0
 def update(self, shape, rot_mat, update_face_fuzz=True):
     self.normal = vec.dot(rot_mat, self.normal_ref)
     self.center = vec.barycenter(
         [shape.verts[vi] for vi in self.get_verts(shape)])
     self.threshold = vec.dot(self.normal, self.center)
     #need to shift and scale points as well as rotate
     if update_face_fuzz:
         self.fuzz_points = [
             vec.dot(rot_mat, point * shape.scale) + shape.pos
             for point in self.fuzz_points_ref
         ]
예제 #12
0
def distance(e: Edge, p: Point) -> float:
    # http://geomalgorithms.com/a02-_lines.html#Distance-to-Ray-or-Segment
    v = Vector(e.p1, e.p2)
    w = Vector(e.p1, p)
    c1 = dot(w, v)
    c2 = dot(v, v)
    b = c1 / c2
    pb = Point(e.p1.x + b * v.x, e.p1.y + b * v.y, e.p1.z + b * v.z)

    if c1 <= 0:
        return distance(p, e.p1)
    if c2 <= c1:
        return distance(p, e.p2)
    return distance(p, pb)
예제 #13
0
def line_plane_intersect(line, plane):
    p0 = line[0]
    p1 = line[1]
    n = plane.normal
    th = plane.threshold
    p0n = vec.dot(p0, n)
    p1n = vec.dot(p1, n)
    #line is contained in plane
    if vec.isclose(p0n, 0) and vec.isclose(p1n, 0):
        return None
    #plane does not intersect line segment
    t = (p0n - th) / (p0n - p1n)
    if t < 0 or t > 1:
        return None
    return vec.linterp(p0, p1, t)
예제 #14
0
 def interpret_controls(self):
     if not hasattr(self.input, 'turn_direction'):
         # Interpret controls using x and y axis to pick a target direction,
         # then translate into turn direction and thrust.
         # If the player is pushing towards a direction and not braking,
         # then it is thrusting.
         self.do_brake = self.input.brake
         self.turn_direction = 0
         self.do_thrust = False
         self.intended_direction = (self.input.x_axis, -self.input.y_axis)
         if self.intended_direction != (0, 0):
             if not self.do_brake:
                 self.do_thrust = True
             # Determine which direction we should turn to come closer to the
             # correct one.
             side = vec.dot(self.intended_direction, vec.perp(self.direction))
             if (
                 vec.angle(self.intended_direction, self.direction) <
                 c.player_intended_turn_threshold
             ):
                 self.turn_direction = 0
             elif side < 0:
                 self.turn_direction = +1
             elif side > 0:
                 self.turn_direction = -1
     else:
         # Interpret controls using thrust, brake, and turn direction.
         self.turn_direction = self.input.turn_direction
         self.do_brake = self.input.brake
         self.do_thrust = self.input.thrust and not self.do_brake
예제 #15
0
 def interpret_controls(self):
     if not hasattr(self.input, 'turn_direction'):
         # Interpret controls using x and y axis to pick a target direction,
         # then translate into turn direction and thrust.
         # If the player is pushing towards a direction and not braking,
         # then it is thrusting.
         self.do_brake = self.input.brake
         self.turn_direction = 0
         self.do_thrust = False
         self.intended_direction = (self.input.x_axis, -self.input.y_axis)
         if self.intended_direction != (0, 0):
             if not self.do_brake:
                 self.do_thrust = True
             # Determine which direction we should turn to come closer to the
             # correct one.
             side = vec.dot(self.intended_direction,
                            vec.perp(self.direction))
             if (vec.angle(self.intended_direction, self.direction) <
                     c.player_intended_turn_threshold):
                 self.turn_direction = 0
             elif side < 0:
                 self.turn_direction = +1
             elif side > 0:
                 self.turn_direction = -1
     else:
         # Interpret controls using thrust, brake, and turn direction.
         self.turn_direction = self.input.turn_direction
         self.do_brake = self.input.brake
         self.do_thrust = self.input.thrust and not self.do_brake
def nearestIntersection(c0, c1, q):
    p = c1.position
    v = c1.orientation

    rp = p - c0.position
    k0 = vec.lengthSq(rp) - c0.getRadius() * c0.getRadius()
    k1 = vec.dot(v, rp)

    roots = []
    k = k1 * k1 - k0
    if util.isAlmostZero(k):
        roots.append(-k1)
    elif 0.0 < k:
        kSqrt = math.sqrt(k)
        roots.append(-k1 - kSqrt)
        roots.append(-k1 + kSqrt)
        assert roots[0] < roots[1]

    vec.set(Inf, q)

    for root in roots:
        if util.isAlmostZero(root) or 0 < root:
            q = vec.scale(v, root, q)
            q = q + p
            break

    return q
예제 #17
0
    def myTimeToCollision(self):
        self.cp = self.myNextCollisionPoint()

        if Inf == vec.length(self.cp):
            return Inf  # No collisions detected.

        # vec.copy(self.myPosition(), gui.debugPt0)
        # vec.copy(self.cp, gui.debugPt1)

        self.rp = self.cp - self.myPosition()

        # With the current set of assumptions, me could not be on a
        # collision course in the first place if the following assert
        # fails.
        assert 0 <= vec.dot(self.rp, self.myVelocity())

        # TODO: To more accurately compute the time to collision we should
        # take into account the velocity of the collider.  But if we do
        # that here, we should have done that in the computation of the
        # nearest collider.  For example, if a collider is moving out of
        # the way faster than we are approaching it, then there is no
        # danger of collision after all.  But remember this whole method
        # is a percept and percepts don't have to be perfect as they are
        # meant to model how the NPC thinks about the world.  And in that
        # vein, using a stationary snapshot of the world is OK for now.
        # Especially so as the snapshot is regularly updated when the
        # percept is recalculated every time an action is selected.
        # colliderVel = vec.dot(self.rp, self.nextCollider)
        # relVel = myVel - colliderVel;

        return vec.length(self.rp)  # / self.myVelocity()
    def myTimeToCollision(self):
        self.cp = self.myNextCollisionPoint()

        if Inf == vec.length(self.cp):
            return Inf # No collisions detected.

        # vec.copy(self.myPosition(), gui.debugPt0)
        # vec.copy(self.cp, gui.debugPt1)

        self.rp = self.cp - self.myPosition()

        # With the current set of assumptions, me could not be on a
        # collision course in the first place if the following assert
        # fails.
        assert 0 <= vec.dot(self.rp, self.myVelocity())

        # TODO: To more accurately compute the time to collision we should
        # take into account the velocity of the collider.  But if we do
        # that here, we should have done that in the computation of the
        # nearest collider.  For example, if a collider is moving out of
        # the way faster than we are approaching it, then there is no
        # danger of collision after all.  But remember this whole method
        # is a percept and percepts don't have to be perfect as they are
        # meant to model how the NPC thinks about the world.  And in that
        # vein, using a stationary snapshot of the world is OK for now.
        # Especially so as the snapshot is regularly updated when the
        # percept is recalculated every time an action is selected.
        # colliderVel = vec.dot(self.rp, self.nextCollider)
        # relVel = myVel - colliderVel;

        return vec.length(self.rp) # / self.myVelocity()
예제 #19
0
def nearestIntersection(c0, c1, q):
    p = c1.position
    v = c1.orientation

    rp = p - c0.position
    k0 = vec.lengthSq(rp) - c0.getRadius() * c0.getRadius()
    k1 = vec.dot(v, rp)

    roots = []
    k = k1 * k1 - k0
    if util.isAlmostZero(k):
        roots.append(-k1)
    elif 0.0 < k:
        kSqrt = math.sqrt(k)
        roots.append(-k1 - kSqrt)
        roots.append(-k1 + kSqrt)
        assert roots[0] < roots[1]

    vec.set(Inf, q)

    for root in roots:
        if util.isAlmostZero(root) or 0 < root:
            q = vec.scale(v, root, q)
            q = q + p
            break

    return q
예제 #20
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))
예제 #21
0
def calc_boundary(face1, face2, origin):

    n1 = face1.normal
    n2 = face2.normal
    th1 = face1.threshold
    th2 = face2.threshold

    #k1 and k2 must be opposite signs
    k1 = vec.dot(n1, origin) - th1
    k2 = vec.dot(n2, origin) - th2

    t = k1 / (k1 - k2)

    n3 = vec.linterp(n1, n2, t)
    th3 = vec.linterp(th1, th2, t)

    return HyperPlane(n3, th3)
def find_error(e):
    """ Takes error syndrome and return error vector """
    v = gf2_to_decimal(e)
    err_vector = [0] * 7
    pos = dot(list2vec(v), list2vec([1, 2, 4]))
    if pos:
        pos -= 1  # array from 0
        err_vector[pos] = One()
    return list2vec(err_vector)
예제 #23
0
 def transform(self, **kwargs):
     #rotate and translate vertices from reference points
     self.verts = [
         vec.dot(self.frame, v * self.scale) + self.pos
         for v in self.verts_ref
     ]
     #update faces
     for face in self.faces:
         face.update(self, self.frame, **kwargs)
예제 #24
0
def basis(vlist):
    '''
    Input:
        - vlist: a list of Vecs
    Output:
        - a list of linearly independent Vecs with equal span to vlist
    '''
    from vec import dot
    return [v for v in orthogonalize(vlist) if dot(v,v) > 1e-20]
예제 #25
0
def vector_matrix_mul(v, M):
    "Returns the product of vector v and matrix M"
    assert M.D[0] == v.D
    import matutil
    from matutil import mat2coldict
    from vec import dot

    mat2col = mat2coldict(M)
    return Vec(M.D[1], {key: dot(v, mat2col[key]) for key in M.D[1]})
예제 #26
0
def subset_basis(vlist):
    '''
    Input:
        - vlist: a list of Vecs
    Output:
        - linearly independent subset of vlist with the same span as vlist[v]
    '''
    from vec import dot
    return [y for (x,y) in zip(orthogonalize(vlist),vlist) if dot(x,x)>1e-20]
예제 #27
0
def matrix_vector_mul(M, v):
    "Returns the product of matrix M and vector v"
    assert M.D[1] == v.D
    import matutil
    from matutil import mat2rowdict
    from vec import dot

    mat2row = mat2rowdict(M)
    return Vec(M.D[0], {key: dot(mat2row[key], v) for key in M.D[0]})
예제 #28
0
def dot_prod_mat_mat_mult(A, B):
    assert A.D[1] == B.D[0]
    Adict = mat2rowdict(A)
    v = mat2coldict(B)
    list_rc = [(r,c) for r in A.D[0] for c in B.D[1]]
    dict1 = {}
    for i,j in list_rc:
        dict1[i,j] = dot(Adict[i],v[j])
    result = Mat((A.D[0], B.D[1]), dict1)
    return Mat((result.D), {(i,j):result.f[i,j] for (i,j) in result.f if result.f[i,j] !=0})
예제 #29
0
    def collide_wall(self, p):
        restitution = self.restitution * p.restitution

        # First, check that we haven't crossed through the wall due to
        # extreme speed and low framerate.
        intersection = intersect_segments(self.p1, self.p2, p.last_pos, p.pos)
        if intersection:
            p.pos = p.last_pos
            p.rebound(self.normal, intersection, restitution)
            return

        # Find vectors to each endpoint of the segment.
        v1 = vec.vfrom(self.p1, p.pos)
        v2 = vec.vfrom(self.p2, p.pos)

        # Find a perpendicular vector from the wall to p.
        v_dist = vec.proj(v1, self.normal)

        # Test distance from the wall.
        radius2 = p.radius**2
        if vec.mag2(v_dist) > radius2:
            return

        # Test for collision with the endpoints of the segment.
        # Check whether p is too far off the end of the segment, by checking
        # the sign of the vector projection, then a radius check for the
        # distance from the endpoint.
        if vec.dot(v1, self.tangent) < 0:
            if vec.mag2(v1) <= radius2:
                p.rebound(v1, self.p1, restitution)
            return
        if vec.dot(v2, self.tangent) > 0:
            if vec.mag2(v2) <= radius2:
                p.rebound(v2, self.p2, restitution)
            return

        # Test that p is headed toward the wall.
        if vec.dot(p.velocity, v_dist) >= c.epsilon:
            return

        # We are definitely not off the ends of the segment, and close enough
        # that we are colliding.
        p.rebound(self.normal, vec.sub(p.pos, v_dist), restitution)
예제 #30
0
    def update_rot_matrix(self, axis1, axis2, angle):
        #rows of the frame are the vectors. so to transform the frame, we multiply on the right
        R = vec.rotation_matrix(self.frame[axis1], self.frame[axis2], angle)
        self.frame = vec.dot(self.frame, R)

        self.rot_matrix = self.frame.T

        self.rot_matrix_T = self.rot_matrix.T

        self.update_plane()
예제 #31
0
    def collide_wall(self, p):
        restitution = self.restitution * p.restitution

        # First, check that we haven't crossed through the wall due to
        # extreme speed and low framerate.
        intersection = intersect_segments(self.p1, self.p2, p.last_pos, p.pos)
        if intersection:
            p.pos = p.last_pos
            p.rebound(self.normal, intersection, restitution)
            return

        # Find vectors to each endpoint of the segment.
        v1 = vec.vfrom(self.p1, p.pos)
        v2 = vec.vfrom(self.p2, p.pos)

        # Find a perpendicular vector from the wall to p.
        v_dist = vec.proj(v1, self.normal)

        # Test distance from the wall.
        radius2 = p.radius**2
        if vec.mag2(v_dist) > radius2:
            return

        # Test for collision with the endpoints of the segment.
        # Check whether p is too far off the end of the segment, by checking
        # the sign of the vector projection, then a radius check for the
        # distance from the endpoint.
        if vec.dot(v1, self.tangent) < 0:
            if vec.mag2(v1) <= radius2:
                p.rebound(v1, self.p1, restitution)
            return
        if vec.dot(v2, self.tangent) > 0:
            if vec.mag2(v2) <= radius2:
                p.rebound(v2, self.p2, restitution)
            return

        # Test that p is headed toward the wall.
        if vec.dot(p.velocity, v_dist) >= c.epsilon:
            return

        # We are definitely not off the ends of the segment, and close enough
        # that we are colliding.
        p.rebound(self.normal, vec.sub(p.pos, v_dist), restitution)
예제 #32
0
def matrix_vector_mul(M, v):
    "Returns the product of matrix M and vector v"
    assert M.D[1] == v.D
    MM = mat2rowdict(M)
    row = M.D[0]
    col = v.D
    list_rc = [(r,c) for r in row for c in col]
    dict1 = {}
    for i in MM:
        dict1[i] = dot(MM[i],v)
    return Vec(M.D[0], dict1)
예제 #33
0
def vector_matrix_mul(v, M):
    "Returns the product of vector v and matrix M"
    assert M.D[0] == v.D
    MM = mat2coldict(M)
    col = M.D[1]
    row = v.D
    list_rc = [(r,c) for r in row for c in col]
    dict1 = {}
    for i in MM:
        dict1[i] = dot(MM[i],v)
    return Vec(M.D[1], dict1)
예제 #34
0
def clip_line_sphere(line, r):
    v0 = line[0]
    v1 = line[1]

    v0_in_sphere = vec.dot(v0, v0) < r * r
    v1_in_sphere = vec.dot(v1, v1) < r * r

    #print('v0_in_sphere',v0_in_sphere)
    #print('v1_in_sphere',v1_in_sphere)
    if v0_in_sphere and v1_in_sphere:
        return line
    intersect = sphere_line_intersect(line, r)
    if intersect is None:
        return None
    if (not v0_in_sphere) and (not v1_in_sphere):
        return intersect
    if (not v0_in_sphere) and v1_in_sphere:
        return Line(intersect[0], v1)
    else:
        return Line(v0, intersect[1])
예제 #35
0
def sphere_t_intersect(line, r):
    v0 = line[0]
    v1 = line[1]
    v0sq = vec.dot(v0, v0)
    v1sq = vec.dot(v1, v1)
    v0dotv1 = vec.dot(v0, v1)
    #quadratic parameters
    a = v0sq + v1sq - 2 * v0dotv1
    b = 2 * (-v0sq + v0dotv1)
    c = v0sq - r * r

    discr = b * b - 4 * a * c

    if discr < 0:
        return None

    sqrt_discr = math.sqrt(discr)
    tm = (-b - sqrt_discr) / (2 * a)
    tp = (-b + sqrt_discr) / (2 * a)

    return (tm, tp)
예제 #36
0
def matrix_matrix_mul(A, B):
    "Returns the product of A and B"
    assert A.D[1] == B.D[0]
    AA = mat2rowdict(A)
    BB = mat2coldict(B)
    row = A.D[0]
    col = B.D[1]
    list_rc = [(r,c) for r in row for c in col]
    dict1 = {}
    for i,j in list_rc:
        dict1[(i,j)]=dot(AA[i],BB[j])
    result = Mat((A.D[0], B.D[1]), dict1)
    return Mat((A.D[0],B.D[1]), {(r,c):result.f[r,c] for r in A.D[0] for c in B.D[1] if result.f[r,c] !=0})
예제 #37
0
def distance(e1: Edge, e2: Edge) -> float:
    # http://geomalgorithms.com/a07-_distance.html#dist3D_Segment_to_Segment()
    SMALL_NUM = 0.00000001
    u, v, w = Vector(e1.p1, e1.p2), Vector(e2.p1, e2.p2), Vector(e2.p1, e1.p1)
    a, b, c, d, e = dot(u, u), dot(u, v), dot(v, v), dot(u, w), dot(v, w)
    D = a * c - b * b
    sc, sN, sD = D, D, D
    tc, tN, tD = D, D, D

    if D < SMALL_NUM:
        sN, sD = 0.0, 1.0
        tN, tD = e, c
    else:
        sN, tN = (b * e - c * d), (a * e - b * d)
        if sN < 0.0:
            sN, tN, tD = 0.0, e, c
        elif sN > sD:
            sN, tN, tD = sD, e + b, c
    if tN < 0.0:
        tN = 0.0
        if -d < 0.0:
            sN = 0.0
        elif -d > a:
            sN = sD
        else:
            sN, sD = -d, a
    elif tN > tD:
        tN = tD
        if (-d + b) < 0.0:
            sN = 0
        elif (-d + b) > a:
            sN = sD
        else:
            sN, sD = (-d + b), a

    sc = sN / sD
    tc = tN / tD
    dP = w + (u * sc) - (v * tc)
    return dP.length()
예제 #38
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
예제 #39
0
def draw_circle(bot, center: Vec3, normal: Vec3, radius: float, pieces: int,
                color_func):
    # Construct the arm that will be rotated
    arm = normalize(cross(normal, center)) * radius
    angle = 2 * math.pi / pieces
    rotation_mat = axis_to_rotation(angle * normalize(normal))
    points = [center + arm]

    for i in range(pieces):
        arm = dot(rotation_mat, arm)
        points.append(center + arm)

    bot.renderer.draw_polyline_3d(points, color_func())
예제 #40
0
def collinear(*points):
    """
    Determine whether the given points are collinear in the order they were
    passed in.
    """
    # Find vectors between successive points, in a chain.
    vectors = []
    for a, b in pairwise(points):
        vectors.append(vec.vfrom(a, b))
    # Find the angles between successive vectors in the chain. Actually we skip
    # the inverse cosine calculation required to find angle, and just use ratio
    # instead. The ratio is the cosine of the angle between the vectors.
    for u, v in pairwise(vectors):
        ratio = vec.dot(u, v) / (vec.mag(u) * vec.mag(v))
        if ratio < 1.0 - epsilon:
            return False
    return True
예제 #41
0
def collinear(*points):
    """
    Determine whether the given points are collinear in the order they were
    passed in.
    """
    # Find vectors between successive points, in a chain.
    vectors = []
    for a, b in pairwise(points):
        vectors.append(vec.vfrom(a, b))
    # Find the angles between successive vectors in the chain. Actually we skip
    # the inverse cosine calculation required to find angle, and just use ratio
    # instead. The ratio is the cosine of the angle between the vectors.
    for u, v in pairwise(vectors):
        ratio = vec.dot(u, v) / (vec.mag(u) * vec.mag(v))
        if ratio < 1.0 - epsilon:
            return False
    return True
예제 #42
0
def distance_squared(M, x):
    '''
    Input:
        - M: matrix with orthonormal rows with M.D[1] == x.D
        - x: vector
    Output:
        - the square of the distance from x to the row-space of M
    Example:
        >>> from vecutil import list2vec
        >>> from matutil import listlist2mat
        >>> x = list2vec([1, 2, 3])
        >>> M = listlist2mat([[1, 0, 0], [0, 1, 0]])
        >>> distance_squared(M, x)
        9
        >>> M = listlist2mat([[3/5, 1/5, 1/5], [0, 2/3, 1/3]])
        >>> distance_squared(M, x)
        8.355575308641976
    '''
    return dot(x, x) - projection_length_squared(M, x)
예제 #43
0
def projected_representation(M, x):
    '''
    Input:
        - M: a matrix with orthonormal rows with M.D[1] == x.D
        - x: a vector
    Output:
        - the projection of x onto the row-space of M
    Examples:
        >>> from vecutil import list2vec
        >>> from matutil import listlist2mat
        >>> x = list2vec([1, 2, 3])
        >>> M = listlist2mat([[1, 0, 0], [0, 1, 0]])
        >>> projected_representation(M, x)
        Vec({0, 1},{0: 1, 1: 2})
        >>> M = listlist2mat([[3/5, 1/5, 1/5], [0, 2/3, 1/3]])
        >>> projected_representation(M, x)
        Vec({0, 1},{0: 1.6, 1: 2.333333333333333})
    '''
    rowdicts = matutil.mat2rowdict(M)
    return Vec(M.D[0], dict((k, dot(rowdicts[k], x)) for k in M.D[0]))
예제 #44
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
예제 #45
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
예제 #46
0
    def make_middle_face(i, j):
        edgeis = [
            j + i * phi_pts, j + (i + 1) * phi_pts,
            phi_pts * theta_pts + j + i * phi_pts,
            phi_pts * theta_pts + (j + 1) % phi_pts + i * phi_pts
        ]

        vertis = unique(flatten([edges[ei] for ei in edgeis]))
        verts_in_face = [verts[vi] for vi in vertis]
        center = vec.barycenter(verts_in_face)
        rel_verts = [v - center for v in verts_in_face]
        #find vector perpendicular to all vertices
        #it is the column of V corresponding to the zero singular value
        #since numpy sorts singular value from largest to smallest,
        #it is the last row in V transpose.
        perp = Vec(svd(np.array(rel_verts))[-1][-1])
        #want our vector to be pointing outwards
        if vec.dot(perp, center) < 0:
            perp = -perp
        normal = vec.unit(perp)

        return Face(edgeis=edgeis, normal=normal)
예제 #47
0
def balls_collisions(balls):
    collided = False
    for b1 in balls:
        for b2 in balls:
            if b1 is b2:
                continue

            d = vec.sub(b1.pos, b2.pos)
            penetration = (b1.radius + b2.radius) - vec.len(d)
            if penetration < 0:
                continue

            collided = True
            n = vec.unit(d)
            b1.pos = vec.add(b1.pos, vec.scale(n, penetration / 2 + 1))
            b2.pos = vec.add(b2.pos, vec.scale(n, -penetration / 2 - 1))

            j = vec.dot(vec.sub(b2.speed, b1.speed), n)
            b1.speed = vec.add(b1.speed, vec.scale(n, j))
            b2.speed = vec.add(b2.speed, vec.scale(n, -j))

    return collided
예제 #48
0
    def calcAction(self):
        # TODO: make this settable
        soonThreshold = 50.0

        # TODO: consider re-factoring using BrainConditional (or something like it)
        # TODO: this is hardwired to only avoid static obstacles

        if soonThreshold < self.percepts.myTimeToCollision() or (self.percepts.myNextCollider() and Inf != self.percepts.nextCollider.mass):
             # No collision danger
             time = self.percepts.getTime()
             # How many milliseconds to wait after a potential collision was detected before
             # resuming with the default controller.  TODO: consider making a settable class variable.
             delay = 0.5
             if self.timeLastCollisionDetected < 0 or delay < time - self.timeLastCollisionDetected:
                 self.defaultBrain.calcAction()
                 self.action = self.defaultBrain.action
             else:
                 # Just continue with last action.
                 # TODO: consider some time discounted blend of default controller and
                 # avoidance vector.
                 pass

             return

        self.timeLastCollisionDetected = self.percepts.getTime()

        # Collision danger present so need to take evasive action.
        self.rp = vec.normalize(self.percepts.myNextCollisionPoint() - self.percepts.myPosition(), self.rp)

        self.tmp = util2D.perpendicularTo(self.rp, self.percepts.myNextCollider().normalTo(self.percepts.getMe(), self.tmp), self.tmp)[0]

        assert util.isAlmostZero(vec.dot(self.rp, self.tmp))

        self.action.setDirection(self.tmp)
        # TODO: modulate the speed based on time until collision and whatever the defaultControler
        # set it to.
        self.action.setSpeed(1.0)
예제 #49
0
 def forwardness(p):
     v_p = vec.vfrom(base_middle, p)
     return vec.dot(v_p, v_base),
예제 #50
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
예제 #51
0
 def contains(self, point):
     sign = vec.dot(
         vec.vfrom(self.center, point),
         self.normal,
     )
     return (sign >= 0)
예제 #52
0
def dot_product_mat_vec_mult(M, v):
    assert(M.D[1] == v.D)
    import vec, matutil
    vv = {i:vec.dot(matutil.mat2rowdict(M)[i], v) for i in M.D[0]}
    return vec.Vec(M.D[0], vv)
예제 #53
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')