int ADPlanner::SetSearchGoalState(int SearchGoalStateID, ADSearchStateSpace_t* pSearchStateSpace)
{
if(pSearchStateSpace->searchgoalstate == NULL ||
pSearchStateSpace->searchgoalstate->StateID != SearchGoalStateID)
{
pSearchStateSpace->searchgoalstate = GetState(SearchGoalStateID, pSearchStateSpace);
//recompute heuristic for the heap if heuristics is used
#if USE_HEUR
int i;
//TODO - should get rid of and instead use iteration to re-compute h-values online as needed
for(i = 0; i < (int)pSearchStateSpace->searchMDP.StateArray.size(); i++)
{
CMDPSTATE* MDPstate = pSearchStateSpace->searchMDP.StateArray[i];
ADState* state = (ADState*)MDPstate->PlannerSpecificData;
state->h = ComputeHeuristic(MDPstate, pSearchStateSpace);
}
#if DEBUG
printf("re-evaluated heuristic values for %d states\n", i);
#endif
pSearchStateSpace->bReevaluatefvals = true;
#endif
}
return 1;
}
//initialization of a state
void ADPlanner::InitializeSearchStateInfo(ADState* state, ADSearchStateSpace_t* pSearchStateSpace)
{
state->g = INFINITECOST;
state->v = INFINITECOST;
state->iterationclosed = 0;
state->callnumberaccessed = pSearchStateSpace->callnumber;
state->bestnextstate = NULL;
state->costtobestnextstate = INFINITECOST;
state->heapindex = 0;
state->listelem[AD_INCONS_LIST_ID] = NULL;
state->numofexpands = 0;
#if AD_SEARCH_FORWARD == 1
state->bestpredstate = NULL;
#endif
//compute heuristics
#if USE_HEUR
if(pSearchStateSpace->searchgoalstate != NULL)
state->h = ComputeHeuristic(state->MDPstate, pSearchStateSpace);
else
state->h = 0;
#else
state->h = 0;
#endif
}
//check a node and add it to open list if necessary
void AstarExe::CheckNode(int _i, int _j, int _cost, Node * _parent) {
//out of bounds
if (_i < 0 || _i > m_grid->GetWidth() - 1 || _j < 0 || _j > m_grid->GetHeight() - 1)
return;
//Get Node at the position in the grid
Node * node = &( *m_grid->GetNodeAt(_i, _j) );
//don't process walls
if (node->type == Grid::S_WALL_TYPE ||node->type == Grid::S_START_TYPE)
return;
//Set cost if not done yet
if( node->cost == 0)
node->cost = _cost;
//Find in OPEN & CLOSE
bool inOpen = BetterExistsInList(m_open,node);
bool inClosed = BetterExistsInList(m_closed, node);
//if not in any list or no better exists
if (!inOpen && !inClosed) {
node->parent = _parent;
ComputeHeuristic(node);
printf("[OPEN] Node (%d,%d) added (h:%d)\n", _i, _j, node->heuristic);
m_open.push_back(node);
}
}
vec3 color(const ray& r, hitable *world, int depth)
{
depth = 0;
vec3 clearclr(0.0f, 0.0f, 0.0f);
vec3 coeff(1.0f, 1.0f, 1.0f);
ray _ray = r;
vec3 ret(0.0f, 0.0f, 0.0f);
float pdfb = 0.0f;
xz_rect ligth_shape(213, 343, 227, 332, 554, nullptr);
#ifdef ENABLE_RUSSIAN_ROULETTE
for (;;) {
#else
while (depth < g_maxdepth) {
#endif
hit_record rec;
if (world->hit(_ray, 0.001f, 10000, rec)) {
if (rec.mat_ptr->isEmssive()) {
// Random direction hits light.
float w = 1.0f;
if (depth > 0) {
// Compute PDF of area light.
hitable_pdf p0(&ligth_shape, _ray.org);
float pdfl = p0.value(_ray.dir);
// Compute weight by using PDF of this ray which the previous hitted object fired.
// ライトにヒットしたレイを飛ばしてきたオブジェクトのマテリアル特性によるレイの拡散に関するPDFを使ってウエイトを計算する.
//w = pdfb / (pdfb + pdfl);
w = ComputeHeuristic(pdfb, pdfl);
}
auto emit = rec.mat_ptr->emit(rec.u, rec.v, rec.p);
ret += coeff * emit * w;
return ret;
}
else {
vec3 rr(1.0f, 1.0f, 1.0f);
#ifdef ENABLE_RUSSIAN_ROULETTE
if (depth > g_rrdepth) {
#if 1
auto c = unit_vector(coeff);
auto p = math::CMath::Max(c.r(), math::CMath::Max(c.g(), c.b()));
auto r = drand48();
if (r > p) {
#else
auto r = drand48();
if (r > 0.5f) {
#endif
r = 1.0f / r;
rr = vec3(r, r, r);
}
else {
break;
}
}
#endif
// Just only getting albedo color.
ray scattered;
vec3 albedo;
float pdf;
rec.mat_ptr->scatter(_ray, rec, albedo, scattered, pdf);
{
// Only for lambert material.
cosine_pdf p1(rec.normal);
// Generate next ray.
auto direction = p1.generate();
scattered = ray(rec.p, direction);
// Compute PDF of next ray by this object's material.
pdfb = p1.value(scattered.dir);
// Compute BRDF of this object's material.
auto brdf = rec.mat_ptr->scattering_pdf(r, rec, scattered);
coeff *= rr * albedo * brdf / pdfb;
}
{
hitable_pdf p0(&ligth_shape, rec.p);
// Generate direction to light.
auto lightdir = p0.generate();
ray toLight(rec.p, lightdir);
hit_record rec_tolight;
if (world->hit(toLight, 0.001f, 10000, rec_tolight)) {
if (rec_tolight.mat_ptr->isEmssive()) {
// Compute PDF of area light.
float pdfl = p0.value(toLight.dir);
//float w = pdfl / (pdfb + pdfl);
float w = w = ComputeHeuristic(pdfl, pdfb);
auto emit = rec_tolight.mat_ptr->emit(rec_tolight.u, rec_tolight.v, rec_tolight.p);
ret += coeff * emit / pdfl * w;
}
}
}
// Set next ray.
_ray = scattered;
}
}
else {
return clearclr;
}
depth++;
}
return ret;
}
static void errorCallback(int error, const char* description)
{
PRINTF("Error: %s\n", description);
}
//: QueryForPath
//----------------------------------------------------------------------------------------
//
// Process this cell using the A* heuristic
//
//-------------------------------------------------------------------------------------://
bool NavigationCell::QueryForPath(NavigationHeap* pHeap, NavigationCell* Caller, float arrivalcost)
{
if (m_SessionID!=pHeap->SessionID())
{
// this is a new session, reset our internal data
m_SessionID = pHeap->SessionID();
if (Caller)
{
m_Open = true;
ComputeHeuristic(pHeap->Goal());
m_ArrivalCost = arrivalcost;
// remember the side this caller is entering from
if (Caller == m_Link[0])
{
m_ArrivalWall = 0;
}
else if (Caller == m_Link[1])
{
m_ArrivalWall = 1;
}
else if (Caller == m_Link[2])
{
m_ArrivalWall = 2;
}
}
else
{
// we are the cell that contains the starting location
// of the A* search.
m_Open = false;
m_ArrivalCost = 0;
m_Heuristic = 0;
m_ArrivalWall = 0;
}
// add this cell to the Open heap
pHeap->AddCell(this);
return(true);
}
else if (m_Open)
{
// m_Open means we are already in the Open Heap.
// If this new caller provides a better path, adjust our data
// Then tell the Heap to resort our position in the list.
if ((arrivalcost + m_Heuristic) < (m_ArrivalCost + m_Heuristic))
{
m_ArrivalCost = arrivalcost;
// remember the side this caller is entering from
if (Caller == m_Link[0])
{
m_ArrivalWall = 0;
}
else if (Caller == m_Link[1])
{
m_ArrivalWall = 1;
}
else if (Caller == m_Link[2])
{
m_ArrivalWall = 2;
}
// ask the heap to resort our position in the priority heap
pHeap->AdjustCell(this);
return(true);
}
}
// this cell is closed
return(false);
}
