/* ************************************************************************* */
HessianFactor::HessianFactor() : info_(matrix_) {
// The empty HessianFactor has only a constant error term of zero
FastVector<size_t> dims;
dims.push_back(1);
info_.resize(dims.begin(), dims.end(), false);
info_(0,0)(0,0) = 0.0;
assertInvariants();
}
void GmGridMapView::AddNeighbor(int node, float d, FastVector<AStar::EdgeInfo<int,float>>& neighbors)
{
if (IsCellTraversable(node))
{
neighbors.push_back(AStar::EdgeInfo<int,float>(node, d));
}
}
void GmGridMapView::GetNeighbors(int node, FastVector<AStar::EdgeInfo<int,float>>& neighbors)
{
neighbors.clear();
neighbors.resize(8);
float cost = GetCellCost(node);
float step1 = 1 * cost;
float stepD = SQRT_2 * cost;
AddNeighbor( GetNodeDxDy(node, 1, 0) , step1, neighbors);
AddNeighbor( GetNodeDxDy(node, - 1, 0) , step1, neighbors);
AddNeighbor( GetNodeDxDy(node, 0, 1) , step1, neighbors);
AddNeighbor( GetNodeDxDy(node, 0, -1) , step1, neighbors);
AddNeighbor( GetNodeDxDy(node, 1, 1), stepD, neighbors);
AddNeighbor( GetNodeDxDy(node, 1, -1), stepD, neighbors);
AddNeighbor( GetNodeDxDy(node, -1, 1), stepD, neighbors);
AddNeighbor( GetNodeDxDy(node, -1, -1), stepD, neighbors);
}