demos/Koules/KoulesPlayback.py
00001 #!/usr/bin/env python
00002 
00003 ######################################################################
00004 # Software License Agreement (BSD License)
00005 #
00006 #  Copyright (c) 2010, Rice University
00007 #  All rights reserved.
00008 #
00009 #  Redistribution and use in source and binary forms, with or without
00010 #  modification, are permitted provided that the following conditions
00011 #  are met:
00012 #
00013 #   * Redistributions of source code must retain the above copyright
00014 #     notice, this list of conditions and the following disclaimer.
00015 #   * Redistributions in binary form must reproduce the above
00016 #     copyright notice, this list of conditions and the following
00017 #     disclaimer in the documentation and/or other materials provided
00018 #     with the distribution.
00019 #   * Neither the name of the Rice University nor the names of its
00020 #     contributors may be used to endorse or promote products derived
00021 #     from this software without specific prior written permission.
00022 #
00023 #  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 #  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 #  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 #  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 #  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 #  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 #  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 #  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 #  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 #  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 #  POSSIBILITY OF SUCH DAMAGE.
00035 ######################################################################
00036 
00037 # Author: Beck Chen, Mark Moll
00038 
00039 from sys import argv, stdout
00040 from os.path import basename, splitext
00041 from math import cos, sin, atan2, pi, sqrt, ceil
00042 import matplotlib.pyplot as plt
00043 from matplotlib.path import Path
00044 from matplotlib import patches
00045 import matplotlib.animation as animation
00046 
00047 targetFrameRate = 30 # desired number of frames per second
00048 speedUp = 1.
00049 # the parameters will be read from file
00050 sideLength = 0
00051 shipRadius = 0
00052 kouleRadius = 0
00053 propagationStepSize = 0
00054 shipAcceleration = 0
00055 shipRotVel = 0
00056 shipDelta = 0
00057 shipEps = 0
00058 
00059 fig = plt.figure(figsize=(6, 6))
00060 ax = plt.axes(xlim=(0, 1), ylim=(0, 1))
00061 fig.subplots_adjust(left=0, bottom=0, right=1, top=1, wspace=None, hspace=None)
00062 handle, = ax.plot([], [])
00063 path = None
00064 
00065 def normalizeAngle(theta):
00066     if theta < -pi:
00067         return theta + 2. * pi
00068     if theta > pi:
00069         return theta - 2. * pi
00070     return theta
00071 
00072 def plotShip(x, u):
00073     pos = (x[0], x[1])
00074     theta = x[4]
00075     (cs,ss) = (shipRadius*cos(theta), shipRadius*sin(theta))
00076     v = [ u[0] - x[2], u[1] - x[3] ]
00077     deltaTheta = normalizeAngle(atan2(v[1], v[0]) - theta)
00078     if v[0]*v[0] + v[1]*v[1] >= shipDelta * shipDelta:
00079         if abs(deltaTheta) < shipEps:
00080             # accelerate forward, draw thruster on the back
00081             ax.add_patch(plt.Circle((pos[0] - cs, pos[1] - ss), .3 * shipRadius, color = "red"))
00082         elif deltaTheta > 0:
00083             # rotate counterclockwise, draw thruster on right side
00084             ax.add_patch(plt.Circle((pos[0] + ss, pos[1] - cs), .3 * shipRadius, color = "red"))
00085         else:
00086             # rotate clockwise, draw thruster on left side
00087             ax.add_patch(plt.Circle((pos[0] - ss, pos[1] + cs), .3 * shipRadius, color = "red"))
00088     # draw ship
00089     ax.add_patch(plt.Circle(x[:2], shipRadius, color = "yellow"))
00090     # draw two blue "eyes"
00091     ax.add_patch(plt.Circle((pos[0] + .7*shipRadius*cos(theta + .75),
00092         pos[1] + .7*shipRadius*sin(theta + .75)), .2 * shipRadius, color = "blue"))
00093     ax.add_patch(plt.Circle((pos[0] + .7*shipRadius*cos(theta - .75),
00094         pos[1] + .7*shipRadius*sin(theta - .75)), .2 * shipRadius, color = "blue"))
00095 
00096 def plotKoules(state):
00097     numKoules = int(len(state)/4)
00098     for i in range(numKoules):
00099         ax.add_patch(plt.Circle((state[4 * i], state[4 * i + 1]), kouleRadius, color = "red"))
00100 
00101 def plotSystem(index):
00102     ax.clear()
00103     ax.add_patch(plt.Rectangle((0, 0), 1, 1, color='black'))
00104     plotKoules(path[index][5:-3])
00105     plotShip(path[index][0:5], path[index][-3:])
00106     if index % 10 == 0:
00107         stdout.write('.')
00108         stdout.flush()
00109     return handle,
00110 
00111 def makeMovie(fname):
00112     with open(fname, 'r') as f:
00113         global sideLength, shipRadius, kouleRadius, propagationStepSize, shipAcceleration, \
00114             shipRotVel, shipDelta, shipEps, path
00115         sideLength, shipRadius, kouleRadius, propagationStepSize, shipAcceleration, \
00116             shipRotVel, shipDelta, shipEps = [float(x) for x in next(f).split()]
00117         path = [[float(x) for x in line.split(' ')] for line in f]
00118         if len(path) == 0:
00119             print('Error: %s contains no solution path' % fname)
00120             return
00121         step = int(ceil(speedUp / (propagationStepSize * targetFrameRate)))
00122         path = path[0:len(path):step]
00123         print('Creating a movie with %d frames...' % len(path))
00124         print('Printing a \'.\' for every 10th frame:')
00125         ani = animation.FuncAnimation(fig, plotSystem, frames = len(path),
00126             interval = 1000. / step, blit = True)
00127         (base,ext) = splitext(basename(fname))
00128         outfname = base + '.mp4'
00129         ani.save(outfname, bitrate = 300, fps = targetFrameRate, writer='mencoder')
00130         print('')
00131 
00132 if __name__ == '__main__':
00133     if len(argv) == 1:
00134         print('Usage: KoulesPlayback.py <filename> [<filename2> ...]')
00135     else:
00136         for fname in argv[1:]:
00137             makeMovie(fname)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines