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
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))
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
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,
)
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))
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,
)
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))
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)
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),
)
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 []
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
]
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)
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)
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 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
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()
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
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))
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)
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)
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]
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]})
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]
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]})
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})
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)
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()
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)
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)
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)
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])
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)
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})
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()
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
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())
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
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
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)
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]))
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
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
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)
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
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)
def forwardness(p):
v_p = vec.vfrom(base_middle, p)
return vec.dot(v_p, v_base),
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
def contains(self, point):
sign = vec.dot(
vec.vfrom(self.center, point),
self.normal,
)
return (sign >= 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)
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')
