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)