00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include <footstep_planner/PlanningState.h>
00028
00029
00030 namespace footstep_planner
00031 {
00032 PlanningState::PlanningState(double x, double y, double theta, Leg leg,
00033 double cell_size, int num_angle_bins,
00034 int max_hash_size)
00035 : ivX(state_2_cell(x, cell_size)),
00036 ivY(state_2_cell(y, cell_size)),
00037 ivTheta(angle_state_2_cell(theta, num_angle_bins)),
00038 ivLeg(leg),
00039 ivId(-1),
00040 ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size))
00041 {}
00042
00043
00044 PlanningState::PlanningState(int x, int y, int theta, Leg leg,
00045 int max_hash_size)
00046 : ivX(x),
00047 ivY(y),
00048 ivTheta(theta),
00049 ivLeg(leg),
00050 ivId(-1),
00051 ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size))
00052 {}
00053
00054
00055 PlanningState::PlanningState(const State& s, double cell_size,
00056 int num_angle_bins, int max_hash_size)
00057 : ivX(state_2_cell(s.getX(), cell_size)),
00058 ivY(state_2_cell(s.getY(), cell_size)),
00059 ivTheta(angle_state_2_cell(s.getTheta(), num_angle_bins)),
00060 ivLeg(s.getLeg()),
00061 ivId(-1),
00062 ivHashTag(calc_hash_tag(ivX, ivY, ivTheta, ivLeg, max_hash_size))
00063 {}
00064
00065
00066 PlanningState::PlanningState(const PlanningState& s)
00067 : ivX(s.getX()),
00068 ivY(s.getY()),
00069 ivTheta(s.getTheta()),
00070 ivLeg(s.getLeg()),
00071 ivId(s.getId()),
00072 ivHashTag(s.getHashTag())
00073 {}
00074
00075
00076 PlanningState::~PlanningState()
00077 {}
00078
00079
00080 bool
00081 PlanningState::operator ==(const PlanningState& s2)
00082 const
00083 {
00084
00085
00086 if (ivHashTag != s2.getHashTag())
00087 return false;
00088
00089 return (ivX == s2.getX() && ivY == s2.getY() &&
00090 ivTheta == s2.getTheta() && ivLeg == s2.getLeg());
00091 }
00092
00093
00094 bool
00095 PlanningState::operator !=(const PlanningState& s2)
00096 const
00097 {
00098 return ivHashTag != s2.getHashTag();
00099 }
00100
00101
00102 State
00103 PlanningState::getState(double cell_size, int num_angle_bins)
00104 const
00105 {
00106 return State(cell_2_state(ivX, cell_size),
00107 cell_2_state(ivY, cell_size),
00108 angles::normalize_angle(
00109 angle_cell_2_state(ivTheta, num_angle_bins)),
00110 ivLeg);
00111 }
00112 }