void baseMarcher::initalizeNarrow()
{
// for each point in the far field check if neighbor is frozen
// if so calculate distance and insert into heap
for (int i=0; i<size_; i++)
{
if (flag_[i] == Far)
{
for (int dim=0; dim<dim_; dim++)
{
for (int j=-1; j<2; j+=2) // each direction
{
int naddr = _getN(i,dim,j,Mask);
if (naddr!=-1 && flag_[naddr]==Frozen)
if (flag_[i]==Far)
{
flag_[i] = Narrow;
double d;
if (order_ == 2)
d = updatePointOrderTwo(i);
else
d = updatePointOrderOne(i);
distance_[i] = d;
heapptr_[i] = heap_->push(i,fabs(d));
}
} // for each direction
} // for each dimension
} // each far field point
}
}
double distanceMarcher::updatePointOrderOne(int i)
{
double a,b,c;
a=b=c=0;
int naddr=0;
for (int dim=0; dim<dim_; dim++)
{
double value = maxDouble;
for (int j=-1; j<2; j+=2) // each direction
{
naddr = _getN(i,dim,j,Mask);
if (naddr!=-1 && flag_[naddr]==Frozen)// && (!ignore_mask_ || !ignore_mask_[naddr]))
{
if (distance_[naddr]<value)
{
value = distance_[naddr];
}
}
}
if (value<maxDouble)
{
// printf("yay, %f, %f\n", value, maxDouble);
a+=idx2_[dim];
b-=idx2_[dim]*2*value;
c+=idx2_[dim]*pow(value,2);
}
}
return solveQuadratic(i,a,b,c);
}
double travelTimeMarcher::updatePointOrderTwo(int i)
{
double a,b,c;
a=b=c=0;
int naddr=0;
for (int dim=0; dim<dim_; dim++)
{
double value1 = maxDouble;
double value2 = maxDouble;
for (int j=-1; j<2; j+=2) // each direction
{
naddr = _getN(i,dim,j,Mask);
if (naddr!=-1 && flag_[naddr]==Frozen)
{
if (distance_[naddr]<value1)
{
value1 = distance_[naddr];
int naddr2 = _getN(i,dim,j*2,Mask);
if (naddr2!=-1 &&
flag_[naddr2]==Frozen &&
((distance_[naddr2]<=value1 && value1 >=0) ||
(distance_[naddr2]>=value1 && value1 <=0)))
{
value2=distance_[naddr2];
if (phi_[naddr2] * phi_[naddr] < 0)
value2 *= -1;
}
}
}
}
if (value2<maxDouble)
{
double tp = oneThird*(4*value1-value2);
a+=idx2_[dim]*aa;
b-=idx2_[dim]*2*aa*tp;
c+=idx2_[dim]*aa*pow(tp,2);
}
else if (value1<maxDouble)
{
a+=idx2_[dim];
b-=idx2_[dim]*2*value1;
c+=idx2_[dim]*pow(value1,2);
}
}
return solveQuadratic(i,a,b,c);
}
void distanceMarcher::initalizeFrozen()
{
//loop over phi to find zero values
// and mark them as frozen
for (int i=0; i<size_; i++)
{
if (flag_[i] != Mask && phi_[i]==0.0 && (!ignore_mask_ || !ignore_mask_[i]))
{
flag_[i]=Frozen;
distance_[i]=0.0;
}
}
//loop over all of phi and for each point check each direction
// to see if we cross the zero level set
// if so calculate the minimum distance to the zero level set
// mark as frozen.
for (int i=0; i<size_; i++)
if (flag_[i] == Far)
{
double ldistance[MaximumDimension];
bool borders=false;
for (int dim=0; dim<dim_; dim++)
{
ldistance[dim]=0;
for (int j=-1; j<2; j+=2) // each direction
{
int naddr = _getN(i,dim,j,Mask);
if (ignore_mask_ && (ignore_mask_[i] || ignore_mask_[naddr]))
continue;
if (naddr!=-1 && phi_[i] * phi_[naddr]<0)
{
// this cell and neighbor span the zero level set.
borders=true;
//calculate the distance to the zero level set.
double d = dx_[dim]*phi_[i]/(phi_[i]-phi_[naddr]);
if (ldistance[dim]==0 || ldistance[dim]>d)
{
ldistance[dim] = d;
}
}
} // for each direction
} // for each dimension
if (borders)
{
double dsum = 0;
for (int dim=0; dim<dim_; dim++)
if (ldistance[dim]>0) dsum += 1/ldistance[dim]/ldistance[dim];
if (phi_[i]<0)
distance_[i] = -sqrt(1/dsum);
else distance_[i] = sqrt(1/dsum);
flag_[i]=Frozen;
}
}// for each point in the far field
}
void extensionVelocityMarcher::finalizePoint(int i, double phi_i)
{
// set the extension velocity of this point
// find f_ext where grad f_ext . grad phi = 0
// as described in Adalsteinsson and Sethian
// technically we do not need to calculate this extension velocity
// until the point is frozen.
double ldistance[MaximumDimension];
double lspeed[MaximumDimension];
double numerator = 0.0;
double denominator = 0.0;
for (int dim=0; dim<dim_; dim++)
{
lspeed[dim]=0;
ldistance[dim]=0;
for (int j=-1; j<2; j+=2) // each direction
{
int naddr = _getN(i,dim,j,Mask);
if (naddr!=-1 && flag_[naddr]==Frozen)
{
//determine which direction, in this dimension, is nearest to
//the front. Calculate the distance to front in this direction
double d = distance_[i] - distance_[naddr];
if (ldistance[dim]==0 || ldistance[dim]>d)
{
ldistance[dim] = d;
lspeed[dim] = f_ext_[naddr];
}
}
} // for each direction
} // for each dimension
for (int dim=0; dim<dim_; dim++)
{
numerator += fabs(ldistance[dim])*lspeed[dim]*idx2_[dim];
denominator += fabs(ldistance[dim])*idx2_[dim];
}
if (denominator != 0.0)
{
f_ext_[i] = numerator/denominator;
}
else
{
throw std::runtime_error(
"div by zero error in scikit-fmm extension velocity");
}
}
// now we apply the fast marching algorithm main loop
// (1) take the smallest narrow band element and
// freeze it.
// (2) for each neighbor of the frozen point calculate distance based
// on frozen elements
// - mark each neighbor as narrow band and stick it
// into the heap
// - if the neighbor is already in the heap update the
// distance value.
void baseMarcher::solve()
{
int frozenCount=0;
for (int i=0; i<size_; i++)
if (flag_[i] == Frozen) frozenCount++;
if (!frozenCount)
{
error_ = 2;
return;
}
int i=0;
while (! heap_->empty())
{
i++;
double value = 0;
int addr = 0;
heap_->pop(&addr, &value);
flag_[addr]=Frozen;
finalizePoint(addr, value);
for (int dim=0; dim<dim_; dim++)
{
for (int j=-1; j<2; j+=2) // each direction
{
int naddr = _getN(addr,dim,j,Frozen);
if (naddr!=-1 && flag_[naddr]!=Frozen)
{
if (flag_[naddr]==Narrow)
{
double d;
if (order_ == 2)
d = updatePointOrderTwo(naddr);
else
d = updatePointOrderOne(naddr);
if (d)
{
heap_->set(heapptr_[naddr],fabs(d));
distance_[naddr]=d;
}
}
else if (flag_[naddr]==Far)
{
double d;
if (order_ == 2)
d = updatePointOrderTwo(naddr);
else
d = updatePointOrderOne(naddr);
if (d)
{
distance_[naddr]=d;
flag_[naddr]=Narrow;
heapptr_[naddr] = heap_->push(naddr,fabs(d));
}
}
}
//==========================================================
// update the far point in the second order stencil
// "jump" over a Frozen point if needed
if (order_ == 2)
{
int local_naddr = _getN(addr,dim,j,Mask);
if (local_naddr!=-1 && flag_[local_naddr]==Frozen)
{
int naddr2 = _getN(addr,dim,j*2,Frozen);
if (naddr2!=-1 && flag_[naddr2]==Narrow)
{
double d = updatePointOrderTwo(naddr2);
if (d)
{
heap_->set(heapptr_[naddr2], fabs(d));
distance_[naddr2]=d;
}
}
}
}
//==========================================================
} // for each direction
} // for each dimension
} // main loop of Fast Marching Method
// add back mask here. The python wrapper will look for elements
// equal to maxDouble and add the mask back
for (int i=0; i<size_; i++)
{
if (flag_[i] == Mask) distance_[i] = maxDouble;
if (flag_[i] == Far) distance_[i] = maxDouble;
}
error_ = 0;
return;
}
void extensionVelocityMarcher::initalizeFrozen()
{
//loop over phi to find zero values
// and mark them as frozen
for (int i=0; i<size_; i++)
{
if (flag_[i] != Mask && phi_[i]==0.0)
{
flag_[i]=Frozen;
distance_[i]=0.0;
f_ext_[i]=speed_[i];
}
}
//loop over all of phi and for each point check each direction
// to see if we cross the zero level set
// if so calculate the minimum distance to the zero level set
// mark as frozen.
for (int i=0; i<size_; i++)
if (flag_[i] == Far)
{
double ldistance[MaximumDimension];
double lspeed[MaximumDimension];
bool borders=false;
for (int dim=0; dim<dim_; dim++)
{
ldistance[dim]=0;
lspeed[dim]=0;
for (int j=-1; j<2; j+=2) // each direction
{
int naddr = _getN(i,dim,j,Mask);
if (naddr!=-1 && phi_[i] * phi_[naddr]<0)
{
// this cell and neighbor span the zero level set.
borders=true;
//calculate the distance to the zero level set.
double d = dx_[dim]*phi_[i]/(phi_[i]-phi_[naddr]);
if (ldistance[dim]==0 || ldistance[dim]>d)
{
ldistance[dim] = d;
if (ext_mask_[i])
lspeed[dim] = speed_[naddr];
else if (ext_mask_[naddr])
lspeed[dim] = speed_[i];
else
lspeed[dim] = speed_[i] + d / dx_[dim] * (speed_[naddr] - speed_[i]);
}
}
} // for each direction
} // for each dimension
if (borders)
{
double numerator = 0.0;
double denominator = 0.0;
for (int dim=0; dim<dim_; dim++)
{
if (ldistance[dim] != 0.0)
{
numerator += lspeed[dim]/pow(ldistance[dim],2);
denominator += 1/pow(ldistance[dim],2);
}
}
if (denominator != 0.0)
{
f_ext_[i] = numerator/denominator;
}
else
{
throw std::runtime_error(
"div by zero (flag=2) in scikit-fmm extension marcher");
}
double dsum = 0;
for (int dim=0; dim<dim_; dim++)
if (ldistance[dim]>0) dsum += 1/ldistance[dim]/ldistance[dim];
if (phi_[i]<0)
distance_[i] = -sqrt(1/dsum);
else distance_[i] = sqrt(1/dsum);
flag_[i]=Frozen;
}
}// for each point in the far field
}
// now we apply the fast marching algorithm main loop
// (1) take the smallest narrow band element and
// freeze it.
// (2) for each neighbor of the frozen point calculate distance based
// on frozen elements
// - mark each neighbor as narrow band and stick it
// into the heap
// - if the neighbor is already in the heap update the
// distance value.
void baseMarcher::solve()
{
int frozenCount=0;
for (int i=0; i<size_; i++)
if (flag_[i] == Frozen) frozenCount++;
if (!frozenCount)
{
error_ = 2;
return;
}
int i=0;
while (! heap_->empty())
{
i++;
double value = 0;
int addr = 0;
heap_->pop(&addr, &value);
flag_[addr]=Frozen;
finalizePoint(addr, value);
for (int dim=0; dim<dim_; dim++)
{
for (int j=-1; j<2; j+=2) // each direction
{
int naddr = _getN(addr,dim,j,Frozen);
if (naddr!=-1 && flag_[naddr]!=Frozen)
{
if (flag_[naddr]==Narrow)
{
double d;
if (order_ == 2)
d = updatePointOrderTwo(naddr);
else
d = updatePointOrderOne(naddr);
if (d)
{
heap_->set(heapptr_[naddr],fabs(d));
distance_[naddr]=d;
}
}
else if (flag_[naddr]==Far)
{
double d;
if (order_ == 2)
d = updatePointOrderTwo(naddr);
else
d = updatePointOrderOne(naddr);
if (d)
{
distance_[naddr]=d;
flag_[naddr]=Narrow;
heapptr_[naddr] = heap_->push(naddr,fabs(d));
}
}
}
}
}
}
// add back mask here. The python wrapper will look for elements
// equal to mexDouble and add the mask back
for (int i=0; i<size_; i++)
{
if (flag_[i] == Mask) distance_[i] = maxDouble;
if (flag_[i] == Far) distance_[i] = maxDouble;
}
error_ = 0;
return;
}