bfs_3d.h

Go to the documentation of this file.

/*
 * Copyright (c) 2009, Maxim Likhachev
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 * 
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University of Pennsylvania nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <iostream>
#include <vector>
#include <string>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <queue>
#include <sbpl/config.h>
#include <distance_field/propagation_distance_field.h>

using namespace std;

#ifndef _BFS3D_
#define _BFS3D_

#define SMALL_NUM  0.00000001  
#define INFINITE_COST 1000000000 
 
#define DEBUG_TIME 0 
#define DIRECTIONS3D 26 
#define GOAL_TOLERANCE 0 

typedef struct
{
  unsigned int g;
  short unsigned int iterationclosed;
  short unsigned int x;
  short unsigned int y;
  short unsigned int z;
} State3D;


class BFS3D
{
  public:

    BFS3D(int dim_x, int dim_y, int dim_z, int radius, int cost_per_cell);

    ~BFS3D();

    void init();
    
    bool setGoal(std::vector<short unsigned int> goal);
    
    bool setGoals(std::vector<std::vector<short unsigned int> > goals);

    bool runBFS();

    int getDist(int x, int y, int z);

    void printGrid();

    void printCostToGoal();

    bool getShortestPath(std::vector<short unsigned int> start, std::vector<std::vector<int> > &path);

    void configDistanceField(bool enable, const distance_field::PropagationDistanceField* df);

    void printConfig(FILE* fOut);

  private:

    short unsigned int dimX_;
    short unsigned int dimY_;
    short unsigned int dimZ_;
  
    short unsigned int radius_;
    double radius_m_;

    std::vector<std::vector<short unsigned int> > goal_;

    int cost_1_move_;
    int cost_sqrt2_move_;
    int cost_sqrt3_move_;

    bool enable_df_;
    const distance_field::PropagationDistanceField* df_;

    unsigned char*** grid3D_;

    int dist_length_;
    std::vector<int> dist_;

    void reInitializeState3D(State3D* state);
    void initializeState3D(State3D* state, short unsigned int x, short unsigned int y, short unsigned int z);
    void create3DStateSpace(State3D**** statespace3D);
    void delete3DStateSpace(State3D**** statespace3D);
    inline int xyzToIndex(int x, int y, int z);
    void search3DwithQueue(State3D*** statespace);
    bool isGoal(const std::vector<int> &state);
    bool isValidCell(const int x, const int y, const int z);
};

inline int BFS3D::xyzToIndex(int x, int y, int z)
{
  int ret = x + y*dimX_ + z*dimX_*dimY_;
  if(ret < dist_length_)
    return ret;
  else
  {
    SBPL_WARN("[BFS3D] out of bounds (%d %d %d) (index: %d  size: %d)\n", x,y,z,ret,dist_length_);
    return 0;
  }
}

inline int BFS3D::getDist(int x, int y, int z)
{
  return dist_[xyzToIndex(x,y,z)];
}

#endif

---

sbpl_arm_planner
Author(s): Benjamin Cohen/bcohen@seas.upenn.edu
autogenerated on Wed Feb 29 11:46:16 2012