1 /*=============================================================================
5 * =============================================================================
7 * Copyright (C) Stanford University, 2006. All Rights Reserved.
10 * =============================================================================
12 * For the license of bayes/sort.h and bayes/sort.c, please see the header
15 * ------------------------------------------------------------------------
17 * For the license of kmeans, please see kmeans/LICENSE.kmeans
19 * ------------------------------------------------------------------------
21 * For the license of ssca2, please see ssca2/COPYRIGHT
23 * ------------------------------------------------------------------------
25 * For the license of lib/mt19937ar.c and lib/mt19937ar.h, please see the
26 * header of the files.
28 * ------------------------------------------------------------------------
30 * For the license of lib/rbtree.h and lib/rbtree.c, please see
31 * lib/LEGALNOTICE.rbtree and lib/LICENSE.rbtree
33 * ------------------------------------------------------------------------
35 * Unless otherwise noted, the following license applies to STAMP files:
37 * Copyright (c) 2007, Stanford University
38 * All rights reserved.
40 * Redistribution and use in source and binary forms, with or without
41 * modification, are permitted provided that the following conditions are
44 * * Redistributions of source code must retain the above copyright
45 * notice, this list of conditions and the following disclaimer.
47 * * Redistributions in binary form must reproduce the above copyright
48 * notice, this list of conditions and the following disclaimer in
49 * the documentation and/or other materials provided with the
52 * * Neither the name of Stanford University nor the names of its
53 * contributors may be used to endorse or promote products derived
54 * from this software without specific prior written permission.
56 * THIS SOFTWARE IS PROVIDED BY STANFORD UNIVERSITY ``AS IS'' AND ANY
57 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
58 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
59 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL STANFORD UNIVERSITY BE LIABLE
60 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
61 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
62 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
63 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
64 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
65 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
66 * THE POSSIBILITY OF SUCH DAMAGE.
68 * =============================================================================
75 Vector_t wallvectorPtr; /* contains source/destination pairs to route */
76 Vector_t srcVectorPtr; /* obstacles */
77 Vector_t dstVectorPtr; /* destinations */
80 /* =============================================================================
82 * =============================================================================
83 maze_t* maze_alloc ();
85 public static Maze alloc()
87 Maze mazePtr = new Maze();
90 mazePtr.gridPtr = null;
91 mazePtr.workQueuePtr = Queue.alloc(1024);
92 mazePtr.wallVectorPtr = Vector.alloc(1);
93 mazePtr.srcVectorPtr = Vector.alloc(1);
94 mazePtr.dstVectorPtr = Vector.alloc(1);
101 /* =============================================================================
103 * =============================================================================
104 void maze_free (maze_t* mazePtr);
106 public static void free(Maze m)
111 /* =============================================================================
113 * =============================================================================
115 private void addToGrid(Grid gridPtr,Vector_t vectorPtr,char[] type)
118 int n = vectorPtr.getSize();
120 for(i = 0; i < n; i++) {
121 Coordinate coordinatePtr = (Coordinate)vectorPtr.vector_at(i);
122 if(!gridPtr.isPointValid(coodinatePtr.x,coordinatePtr.y,coordinatePtr.z))
124 System.err.println("Error: " + type + " (" + coordinate.x +
125 ", " + coordinate.y +
126 ", " + coordinate.z);
130 gridPtr.addPath(vectorPtr);
132 /* =============================================================================
134 * -- Return number of path to route
135 * =============================================================================
136 long maze_read (maze_t* mazePtr, char* inputFileName);
138 public int read(String inputFileName)
140 BufferedReader in = new BufferedReader(new FileReader(inputFileName));
149 char[] line = new char[256];
150 boolean isParseError = false;
151 List_t workListPtr = List.alloc(1); // List.alloc(Coordinate.comparePair);
154 while(line = in.readLine()) {
157 int[] xy = new int[6]; // equivalent to x1,y1,z1,x2,y2,z2
160 StringTokenizer tok = new StringTokenizer(line);
162 if(numToken = tok.countTokens() < 1 ) {
166 code = (char)tok.nextElement();
168 for(i=0;i<numToken-1;i++) {
169 xy[i] = Integer.ParserInt(tok.nextToken());
176 }else if(code == 'd') {
177 /* dimensions (format: d x y z) */
185 if(width < 1 || height < 1 || depth <1)
188 }else if(code == 'p') { /* paths (format: p x1 y1 z1 x2 y2 z2) */
193 Coordinate srcPtr = Coordinate.alloc(xy[0],xy[1],xy[2]);
194 Coordinate dstPtr = Coordinate.alloc(xy[3],xy[4],xy[5]);
196 if(Coordinate.isEqual(srcPtr,dstPtr)) {
200 Pair coordinatePtr = Pair.alloc(srcPtr,dstPtr);
201 boolean status = workListPtr.insert((Object)coordinatePairPtr);
204 }else if(code == 'w') {
205 /* walls (format: w x y z) */
209 Coordinate wallPtr = Coordinate.alloc(xy[0],xy[1],xy[2]);
210 wallVectorPtr.vector_pushBack(wallPtr);
216 if(isParseError) {/* Error */
217 System.err.println("Error: line " + lineNumber + " of " + inputfileName + "invalid");
221 /* iterate over lines in put file */
224 * Initialize grid contents
226 if(width < 1 || height < 1 || depth < 1) {
227 System.err.println("Error : Invalid dimensions ( " + width + ", " + height + ", "+ depth + ")");
231 Grid gridPtr = Grid.alloc(width,height,depth);
232 mazePtr.gridPtr = gridPtr;
233 gridPtr.addToGrid(wallVectorPtr,"wall");
234 gridPtr.addToGrid(srcVectorPtr, "source");
235 grdPtr.addToGrid(dstVectorPtr, " destination");
236 System.out.println("Maze dimensions = " + width + " x " + height + " x " + depth);
237 System.out.println("Paths to route = " + workListPtr.getSize());
240 * Initialize work queue
242 Queue workQueuePtr = mazePtr.workQueuePtr;
243 List_Iter it = new List_Iter();
244 it.reset(workListPtr);
246 while(it.hasNext(wrokListPtr)) {
247 Pair coordinatePtr = (Pair)it.next(workListPtr);
248 workQueuePtr.queue_push((Object)coordinatePairPtr);
253 return srcVectorPtr.getSize();
257 /* =============================================================================
259 * =============================================================================
260 bool_t maze_checkPaths (maze_t* mazePtr, list_t* pathListPtr, bool_t doPrintPaths);
262 public boolean checkPaths(List_t pathListPtr,boolean doPrintPaths)
267 Grid testGridPtr = Grid.alloc(width,height,depth);
268 testGridPtr.addPath(wallVectorPtr);
271 int numSrc = srcVectorPtr.getSize();
272 for(i = 0; i < numSrc; i++) {
273 Coordinate srcPtr = (Coordinate)srcVectorPtr.vector_at(i);
274 testGridPtr.setPoint(srcPtr.x,srcPtr.y,srcPtr.z,0);
277 /* Mark destinations */
278 int numDst = destVectorPtr.getSize();
279 for(i = 0; i < numdst; i++) {
280 Coordinate dstPtr = (Coordinate)dstVector.vector_at(i);
281 testGridPtr.setPoint(dstPtr.x,dstPtr.y,dstPtr.z,0);
284 /* Make sure path is contiguous and does not overlap */
286 List_Iter it = new List_Iter();
287 it.reset(pathVectorListPtr);
288 while(it.hasNext(pathVectorListPtr)) {
289 Vector_t pathVectorPtr = it.next(pathVectorListPtr);
290 int numPath = pathVectorPtr.getSize();
292 for(i = 0; i < numPath; i++) {
294 Vector pointVectorPtr = pathVectorPtr.vector_at(i);
296 int prevGridPointIndex = pointVectorPtr.vector_at(0);
297 int[] x = new int[1];
298 int[] y = new int[1];
299 int[] z = new int[1];
300 gridPtr.getPointIndices(prevGridPointIndex,x,y,z);
301 if(testGridPtr.getPoint(x[0],y[0],z[0]) != 0) {
302 Grid.free(testGridPtr);
305 Coordinate prevCoordinate = new Coordinate();
306 int[] x = new int[1];
307 int[] y = new int[1];
308 int[] z = new int[1];
309 gridPtr.getpointIndices(prevGridPointIndex,x,y,z);
310 prevCoordinate.x = x[0];
311 prevCoordinate.y = y[0];
312 prevCoordinate.z = z[0];
314 int numPoiont = pointVectorPtr.getSize();
316 for(j = 1; j< (numPoint - 1) ;j++) { /* no need to check endpoints */
317 int currGridPointIndex = pointVectorPtr.vector_at(j);
318 Coordinate currCoordinate = new Coordinate();
319 int[] x = new int[1];
320 int[] y = new int[1];
321 int[] z = new int[1];
322 gridPtr.getPointIndices(currGridPointIndex,x,y,z);
323 currGridPoint.x = x[0];
324 currGridPoint.y = y[0];
325 currGridPoint.z = z[0];
327 if(Coordinate.areAdjacent(currCoordinate,preCoordinate)) {
328 Grid.free(testGridPtr);
332 prevCoordinate = currCoordinate;
333 int x = currCoordinate.x;
334 int y = currCoordinate.y;
335 int z = currCoordinate.z;
336 if(testGridPtr.getPoint(x,y,z) != GRID_POINT_EMPTY) {
337 Grid.free(testGridPtr);
340 testGridPtr.setPoint(x,y,z,id);
344 int lastGridPointIndex = pointVectorPtr.vector_at(j);
345 gridPtr.getPointindices(lastGridPointIndex,x,y,z);
346 if(testGridPtr.getPoint(x[0],y[0],z[0]) != 0) {
347 Grid.free(testGridPtr);
350 } /* iterate over pathVector */
351 } /* iterate over pathVectorList */
354 system.out.println("\nRouted Maze:");
358 Grid.free(testGridPtr);
364 /* =============================================================================
368 * =============================================================================