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 arc_left(
self, arc_angle, radius=None, center=None, start_slant=None, end_slant=None,
):
if (
(radius is None and center is None)
or (radius is not None and center is not None)
):
raise TypeError('You must specify exactly one of center or radius.')
arc_angle = Angle(arc_angle)
# Create a radius vector, which is a vector from the arc center to the
# current position. Subtract to find the center, then rotate the radius
# vector to find the arc end point.
if center is None:
if arc_angle < 0:
radius = -abs(radius)
v_radius = vec.neg(vec.perp(self._vector(radius)))
center = vec.sub(self._position, v_radius)
elif radius is None:
v_radius = vec.vfrom(center, self._position)
radius = vec.mag(v_radius)
if arc_angle < 0:
radius = -radius
endpoint = vec.add(center, vec.rotate(v_radius, arc_angle.rad))
self._arc(
center,
radius,
endpoint,
arc_angle,
start_slant=start_slant,
end_slant=end_slant,
)
def arc_left(
self, arc_angle, radius=None, center=None, start_slant=None, end_slant=None,
):
if (
(radius is None and center is None) or
(radius is not None and center is not None)
):
raise TypeError('You must specify exactly one of center or radius.')
arc_angle = Angle(arc_angle)
# Create a radius vector, which is a vector from the arc center to the
# current position. Subtract to find the center, then rotate the radius
# vector to find the arc end point.
if center is None:
if arc_angle < 0:
radius = -abs(radius)
v_radius = vec.neg(vec.perp(self._vector(radius)))
center = vec.sub(self._position, v_radius)
elif radius is None:
v_radius = vec.vfrom(center, self._position)
radius = vec.mag(v_radius)
if arc_angle < 0:
radius = -radius
endpoint = vec.add(center, vec.rotate(v_radius, arc_angle.rad))
self._arc(
center,
radius,
endpoint,
arc_angle,
start_slant=start_slant,
end_slant=end_slant,
)
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 draw(s, gs):
starstep = 100
# This would be nicer if it worked. But, screw it for now. More important things.
# nearx = int(gs.camerax - (gs.camerax % starstep)) - (gs.screenW / 2) + starstep
# farx = nearx + gs.screenW
# neary = int(gs.cameray - (gs.cameray % starstep)) - (gs.screenH / 2) + starstep
# fary = nearx + gs.screenH
# for i in range(nearx, farx, starstep):
# for j in range(neary, fary, starstep):
# dist = vec.mag(vec.new(i,j))
# relativeDist = dist / gs.universeSize
# brightness = 255 - min(255, int(255 * relativeDist))
# sc = gs.screenCoords(vec.new(i, j))
# pygame.draw.circle(surf, pygame.Color(brightness,brightness,brightness), sc, 2)
xoff = int(gs.camerax) % starstep
yoff = int(gs.cameray) % starstep
sprite = resource.getSprite('warpspark')
for i in range(0, gs.screenW + starstep, starstep):
for j in range(0, gs.screenH + starstep, starstep):
dist = vec.mag(vec.new(gs.camerax, gs.cameray))
relativeDist = dist / gs.universeSize
brightness = 255 - min(255, int(255 * relativeDist))
sprite.x = i - xoff
sprite.y = j - yoff
sprite.draw()
def rebound(self, *args, **kwargs):
v_initial = self.velocity
super(Player, self).rebound(*args, **kwargs)
v_final = self.velocity
# Apply damage based on the difference in momentum.
v_diff = vec.sub(v_final, v_initial)
self.damage += vec.mag(v_diff) * self.mass
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 rebound(self, *args, **kwargs):
v_initial = self.velocity
super(Player, self).rebound(*args, **kwargs)
v_final = self.velocity
# Apply damage based on the difference in momentum.
v_diff = vec.sub(v_final, v_initial)
self.damage += vec.mag(v_diff) * self.mass
def text(self):
lines = [
'{:.0f} fps'.format(self.display.fps),
]
for p in self.display.environment.players:
lines.append('player {} speed: {:.1f}'.format(
p.number + 1,
vec.mag(p.velocity),
))
return lines
def text(self):
lines = [
'{:.0f} fps'.format(self.display.fps),
]
for p in self.display.environment.players:
lines.append(
'player {} speed: {:.1f}'.format(
p.number + 1,
vec.mag(p.velocity),
)
)
return lines
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 intersect_circle_line(center, radius, line_start, line_end):
"""
Find the intersection of a circle with a line.
"""
radius = abs(radius)
# First check whether the line is too far away, or if we have a
# single point of contact.
# Reference:
# http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
r = vec.vfrom(center, line_start)
v = vec.perp(vec.vfrom(line_start, line_end))
d = vec.proj(r, v)
dist = vec.mag(d)
if float_equal(dist, radius):
# Single intersection point, because the circle and line are tangent.
point = vec.add(center, d)
return [point]
elif dist > radius:
return []
# Set up parametric equations for the line and the circle, and solve them.
# Reference:
# http://www.cs.cf.ac.uk/Dave/CM0268/PDF/circle_line_intersect_proof.pdf
xc, yc = center
x0, y0 = line_start
x1, y1 = line_end
line_x, line_y = (x1 - x0), (y1 - y0) # f, g
dx, dy = (x0 - xc), (y0 - yc)
a = line_x**2 + line_y**2
b = 2 * (line_x * dx + line_y * dy)
c = dx**2 + dy**2 - radius**2
t0, t1 = quadratic_formula(a, b, c)
return [
(
x0 + line_x * t0,
y0 + line_y * t0,
),
(
x0 + line_x * t1,
y0 + line_y * t1,
),
]
def intersect_circle_line(center, radius, line_start, line_end):
"""
Find the intersection of a circle with a line.
"""
radius = abs(radius)
# First check whether the line is too far away, or if we have a
# single point of contact.
# Reference:
# http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
r = vec.vfrom(center, line_start)
v = vec.perp(vec.vfrom(line_start, line_end))
d = vec.proj(r, v)
dist = vec.mag(d)
if float_equal(dist, radius):
# Single intersection point, because the circle and line are tangent.
point = vec.add(center, d)
return [point]
elif dist > radius:
return []
# Set up parametric equations for the line and the circle, and solve them.
# Reference:
# http://www.cs.cf.ac.uk/Dave/CM0268/PDF/circle_line_intersect_proof.pdf
xc, yc = center
x0, y0 = line_start
x1, y1 = line_end
line_x, line_y = (x1 - x0), (y1 - y0) # f, g
dx, dy = (x0 - xc), (y0 - yc)
a = line_x**2 + line_y**2
b = 2 * (line_x * dx + line_y * dy)
c = dx**2 + dy**2 - radius**2
t0, t1 = quadratic_formula(a, b, c)
return [
(
x0 + line_x * t0,
y0 + line_y * t0,
),
(
x0 + line_x * t1,
y0 + line_y * t1,
),
]
def to_omega(self, dt): m = vec.mag(self.v) #print self.s,s if m > 0.0: omega = 2.0 * math.asin(m) / dt * vec.normalize(self.v) else: omega = np.zeros(3) ver = False if ver: print 'dt ',dt print 'm ',m print 'r ',self.s,self.v print 'omega',omega return omega
def genCaptureImage(s):
"""Builds a line sprite of the right length to go to the capturing planet."""
# Some paranoid error checking
if not s.parent:
raise Exception(
"Tried to generate capture image with nonexistant parent!")
vecToParent = vec.sub(s.parent.loc, s.loc)
distanceToParent = vec.mag(vecToParent)
# It appears that we can blit image_data (software image data
# in main memory) to a texture (hardware image data on the GPU)
# but not any other way.
ropeImage = resource.getImage('line2').get_image_data()
img = pyglet.image.create(ropeImage.width,
int(distanceToParent)).get_texture()
img.anchor_x = int(img.width // 2)
img.anchor_y = int(img.height // 2)
# Now we have the image, we fill it up with the
# capture-rope images.
for i in range(0, int(distanceToParent), ropeImage.height):
#print 'foo', i, img.height, ropeImage.height
img.blit_into(ropeImage, 0, i, 0)
s.captureSprite = pyglet.sprite.Sprite(img)
def draw(self, pen):
#TODO: this is not very good.
def scythe_cap(pen, end):
start_heading = pen.heading
switch = False
if self.character.mirrored_x:
switch = not switch
if self.flipped:
switch = not switch
if not switch:
top = end
bottom = pen.position
else:
top = pen.position
bottom = end
# Trace the curves with a temporary pen.
temp_pen = pen.copy()
if not switch:
arc = temp_pen.arc_right
else:
arc = temp_pen.arc_left
temp_pen.move_to(top)
temp_pen.turn_to(start_heading)
outer_arcs = [
(2.4, 1.6),
(1.0, 2.8),
]
outer_points = []
for radius, distance in outer_arcs:
circumference = radius * 2 * math.pi
circle_ratio = distance / circumference
angle = circle_ratio * 360
arc(angle, radius)
outer_points.append(temp_pen.position)
outer_tip_angle = temp_pen.heading
temp_pen.move_to(bottom)
temp_pen.turn_to(start_heading)
temp_pen.move_forward(0.5)
inner_forward = temp_pen.position
temp_pen.arc_to(outer_points[-1])
inner_tip_angle = temp_pen.heading
# Draw with the real pen.
if not switch:
pen.line_to(inner_forward)
pen.arc_to(outer_points[-1])
pen.turn_to(outer_tip_angle + 180)
for p in reversed(outer_points[:-1]):
pen.arc_to(p)
pen.arc_to(top)
else:
for p in outer_points:
pen.arc_to(p)
pen.turn_to(inner_tip_angle + 180)
pen.arc_to(inner_forward)
pen.line_to(bottom)
pen.line_to_y(BOTTOM + pen.mode.width / 2)
pen.turn_to(0)
# See how far forward we have to go to make the top of the stroke
# zero-length.
temp_pen = pen.copy(paper=True)
temp_pen.line_forward(pen.mode.width, end_slant=90)
seg = temp_pen.last_segment()
extra_left = vec.mag(vec.vfrom(seg.a_left, seg.b_left))
extra_right = vec.mag(vec.vfrom(seg.a_right, seg.b_right))
extra = min(extra_left, extra_right)
dist = pen.mode.width - extra
pen.line_forward(dist, end_slant=90)
pen.last_segment().end_cap = scythe_cap
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 last_slant_width(self):
seg = self.last_segment()
return vec.mag(vec.vfrom(seg.b_left, seg.b_right))
def speed(self):
return vec.mag(self.velocity)
def step(self, ti, ti_0): dt = self.c.t[ti] - self.c.t[ti-1] q = self.c.q[ti] q_ref = self.q_ref[ti] self.e1[ti] = q_ref * q.conj() q_ref_0 = self.q_ref[ti-1] q_ref_1 = self.q_ref[ti-0] # q_refd if ti_0 > 1: r = q_ref_1 * q_ref_0.conj() q_refd_1 = r.to_omega(dt) #print 'r',r.s,r.v else: q_refd_1 = np.zeros(3) # clamp q_refd_1 = vec.clamparr(q_refd_1, -1.0, 1.0) self.q_refd[ti] = q_refd_1 # q_refdd if ti_0 > 2: q_refdd_1 = (q_refd_1 - self.q_refd[ti-1]) / dt #print 'r',r.s,r.v else: q_refdd_1 = np.zeros(3) self.q_refdd[ti] = q_refdd_1 # omega ref self.omega_ref[ti] = self.q_refd[ti] # omega error self.e2[ti] = self.omega_ref[ti] - self.c.omega[ti] # e1 mag d if ti_0 > 0: self.e1_mag_d[ti] = (vec.mag(self.e1[ti].v) - vec.mag(self.e1[ti-1].v)) / dt # check objective if ti_0 > 0: if self.obj: if self.obj.mode == control.ObjMode.normal: if (self.e1_mag_d[ti] < 0.0) and (self.e1_mag_d[ti] > -0.001): if (2.0 * math.asin(vec.mag(self.e1[ti].v))) < self.obj.thresh: self.obj.flag_complete = True # extras def prin(): print 'q_ref_1 ',q_ref_1.v print 'q_ref_0 ',q_ref_0.v print 'r ',r.v print 'q_refd_n',q_refd_1 if np.any(q_refd_1 > 1.0): prin() ver = False #ver = True if ver: prin()
def step(self, ti):
self.ti = ti
dt = self.t[ti] - self.t[ti-1]
# rotation
omega = self.omega[ti-1]
q = self.q[ti-1]
tau = self.get_tau_rotor_body(ti-1)
omegad = np.dot(self.Iinv, tau - np.cross(omega, np.dot(self.I, omega)))
#theta = self.theta[ti-1]
#thetap = self.get_thetad(ti-1)
omega_n = omega + omegad * dt
#self.theta[ti] = theta + thetap * dt
omega_n_magn = vec.mag(omega_n)
#print omega_magn
if omega_n_magn == 0.0:
r = qt.Quat()
else:
omega_n_norm = omega_n / omega_n_magn
r = qt.Quat(theta = omega_n_magn * dt, v = omega_n_norm)
qn = r * q
self.omega[ti] = omega_n
self.q[ti] = qn
ver = False
if ver:
print 'tau ',tau
print 'omegad ',omegad
print 'omega_n',omega_n
print 'r ',r.s,r.v
# position
f = self.get_force(ti-1)
if any(np.isnan(f)):
raise ValueError('f nan')
x = self.x[ti-1]
v = self.v[ti-1]
a = f / self.m
vn = v + a * dt
xn = x + vn * dt
self.x[ti] = xn
self.v[ti] = vn
if any(np.isnan(vn)):
raise ValueError('v nan')
if any(np.isnan(xn)):
raise ValueError('x nan')
def last_slant_width(self):
seg = self.last_segment()
return vec.mag(vec.vfrom(seg.b_left, seg.b_right))
def dist(a, b):
return vec.mag(vec.vfrom(a, b))
def speed(self):
return vec.mag(self.velocity)
