void Controller::allNbrs()
{
for( size_t i = 0 ; i < _numVehs ; ++i )
_nbrs[i].clear();
for( int i = 0 ; i < _numVehs ; ++i ) {
for( int j = 0 ; j < _numVehs ; ++j ) {
if( j == i )
continue ;
for( int k = j+1 ; k < _numVehs ; ++k ) {
if( k == i )
continue ;
_nbrs[i].push_back( Neighbor(i,j) ) ;
_nbrs[i].push_back( Neighbor(j,i) ) ;
}
}
}
}
//--------------------------------------------------------------------------------------------------
// buildNeighbors
void BasicSPH::buildNeighbors()
{
// Reserve space and initialize neighbors' data
_neighbors.clear();
_neighbors.resize(_particles.size());
// Init spatial grid
Vec3d borders(_h*2.0, _h*2.0, _h*2.0);
Vec3d gridMin = _volumeMin;
gridMin -= borders;
Vec3d gridMax = _volumeMax;
gridMax += borders;
SpatialGrid<long> grid(_h, gridMin, gridMax);
// Insert particles into grid
for (long p=0; p<_particles.size(); ++p)
{
grid.insert(p, _particles[p].pos);
}
// Use grid to retrieve neighbor particles
double h2 = _h*_h;
std::vector<long*> nearbyParticles;
for (long p=0; p<_particles.size(); ++p)
{
const SPHParticle &particle = _particles[p];
// Get nearby particles
grid.getElements(particle.pos, _h, nearbyParticles);
// Find particles that are within smoothing radius
_neighbors[p].reserve(50);
for (long i=0; i<nearbyParticles.size(); ++i)
{
long nID = *nearbyParticles[i];
const SPHParticle &neighborParticle = _particles[nID];
// Skip current particle
if (nID==p)
continue;
Vec3d xij = particle.pos - neighborParticle.pos;
// Check if distance is lower than smoothing radius
double dist2 = xij.dot(xij);
if (dist2 < h2)
{
// Yup! Add the particle to the neighbors list along with
// some precomputed informations
_neighbors[p].push_back(Neighbor(nID, xij, sqrt(dist2)));
}
}
}
}
ubi_btNodePtr ubi_btPrev( ubi_btNodePtr P )
/* ------------------------------------------------------------------------ **
* Given the node indicated by P, find the (sorted order) Previous node in
* the tree.
* Input: P - a pointer to a node that exists in a binary tree.
* Output: A pointer to the "previous" node in the tree, or NULL if P
* pointed to the "first" node in the tree or was NULL.
* ------------------------------------------------------------------------ **
*/
{
return( Neighbor( P, ubi_trLEFT ) );
} /* ubi_btPrev */
ubi_btNodePtr ubi_btNext( ubi_btNodePtr P )
/* ------------------------------------------------------------------------ **
* Given the node indicated by P, find the (sorted order) Next node in the
* tree.
* Input: P - a pointer to a node that exists in a binary tree.
* Output: A pointer to the "next" node in the tree, or NULL if P pointed
* to the "last" node in the tree or was NULL.
* ------------------------------------------------------------------------ **
*/
{
return( Neighbor( P, ubi_trRIGHT ) );
} /* ubi_btNext */
bool ofxRemoteUINeigbors::gotPing(string ip, int port, string name, string binaryName){
bool updated = false;
std::ostringstream oss;
oss << port;
string myID = ip + ":" + oss.str();
if ( neigbhors.find(myID) == neigbhors.end() ){ //not found, add it
updated = true;
Neighbor n = Neighbor(ip, port, time, name, binaryName);
neigbhors[myID] = n;
}else{ //found!
neigbhors[myID].timeLastSeen = time; //update time last seen for this dude
}
return updated;
}
void ClusterBasedRule::AssignNeighbors(NeighborMap& neighbors,
const size_t cand_size,
const int client_cid)
{
const size_t NUM_PEERLIST = g_btte_args.get_num_peerlist();
for (size_t i = 0; i < cand_size; i++)
{
if (i >= NUM_PEERLIST) break;
const int pid = candidates_[i];
const int cid = g_peers.at(pid).get_cid();
float pg_delay = ComputePGDelayByCluster(client_cid, cid);
Neighbor nei_info = Neighbor(pg_delay);
neighbors.insert(std::pair<int, Neighbor>(pid, nei_info));
g_peers.at(pid).incr_neighbor_counts(1);
}
}
void StandardRule::AssignNeighbors(NeighborMap& neighbors,
const size_t cand_size)
{
const size_t NUM_PEERLIST = g_btte_args.get_num_peerlist();
for (size_t i = 0; i < NUM_PEERLIST; i++)
{
if (i < cand_size)
{
const int cand_pid = candidates_[i];
float pg_delay = Roll(RSC::STD_PEERSELECT, 0.01, 1.0);
Neighbor nei_info = Neighbor(pg_delay);
neighbors.insert(std::pair<int, Neighbor>(cand_pid, nei_info));
g_peers.at(cand_pid).incr_neighbor_counts(1);
}
else
break;
}
}
int
KNN::classificateData(Neighbor point, vector<Neighbor> neighbors)
{
double distance;
vector<Neighbor> neigh, nearest;
for (unsigned int i=0; i<neighbors.size(); i++){
distance = this->calculateDistance(point, neighbors[i]);
if(distance == 0)
return neighbors[i].getInstance().getClassification();
Neighbor n = Neighbor(neighbors[i].getInstance(), distance);
neigh.push_back(n);
}
nearest = this->getNearestNeighbors(neigh);
int predictedClassification = this->determineMajority(nearest);
return predictedClassification;
}
int* GenericTobogganVer2<T, OrderInvariant, Connectivity>::
GetLabelImage(const T* OldGradient, const int Width, const int Height)
// ** (OrderInvariant == false) **
{
TNeighbor<Connectivity> Neighbor(1, Width+2);
TQueue<int> Queue(Width*Height);
T *Gradient, level, min_level;
int *Label, *int_scan, LabelCount;
int offset, index, begin, end, pixel, label, neighbor;
if(OrderInvariant == true) {
throw "GenericToboggan::GetLabelImage => OrderInvariant must be false";
}
// Surrounding the gradient image with sentinesl.
Gradient = MakeNewGradient<T>(OldGradient, Width, Height);
// Memory allocation
Label = new int[(Width+2)*(Height+2)];
if(!Label) {
throw "GenericToboggan_OrderVariant => cannot allocate memory";
}
// Initialization
// Setting the sentinels as ridges
memset(Label+0, -1, sizeof(int)*(Width+2)); // RIDGE_LABEL == -1
offset = Width+2;
for(index=1; index<=Height; index++) {
memset(Label+offset+1, 0, sizeof(int)*Width); // NULL_LABEL == 0
Label[offset+0] = Label[offset+Width+1] = RIDGE_LABEL;
offset += Width+2;
}
memset(Label+offset, -1, sizeof(int)*(Width+2)); // RIDGE_LABEL == -1
LabelCount = 0;
Queue.Reset();
// Downward sliding
begin = Width+3;
end = (Width+2)*(Height+2)-(Width+3);
for(pixel=begin; pixel<end; pixel++) {
if(Label[pixel] != NULL_LABEL) {
// `pixel' is one of the sentinels.
continue;
}
label = NULL_LABEL;
min_level = Gradient[pixel];
// Search for the steepest direction
for(Neighbor.Begin(); !Neighbor.End(); Neighbor.Next()) {
neighbor = Neighbor.Where(pixel);
level = Gradient[neighbor];
if(level < min_level) {
// ************************* Trick ***********************************
// Values less than `-1' are used as the pointers to the pixels.
// (Remind that RIDGE_LABEL is defined as -1 in "sentinel.h".)
// Thus only the first (pointer=0) and second (pointer=1) pixel in
// the image are not accessible. It is safe for this function because
// the first two pixels must be the sentinels.
// *******************************************************************
label = - neighbor;
min_level = level;
}
}
if(label != NULL_LABEL) {
Queue.In(pixel);
Label[pixel] = label;
}
}
// Region growing from the lower boundaries
while(!Queue.IsEmpty()) {
Queue.Out(pixel);
level = Gradient[pixel];
for(Neighbor.Begin(); !Neighbor.End(); Neighbor.Next()) {
neighbor = Neighbor.Where(pixel);
if(Label[neighbor] != NULL_LABEL) {
// `neighbor' has been visited.
continue;
}
if(Gradient[neighbor] != level) {
// `neighbor' is not at the same level as `pixel'.
continue;
}
Label[neighbor] = - pixel; // Refer to the above trick.
Queue.In(neighbor);
}
}
// Finding the minimal regions
for(index=begin; index<end; index++) {
pixel = index;
if(Label[pixel] != NULL_LABEL) {
// `pixel' has been visited
continue;
}
Label[pixel] = label = ++ LabelCount;
Queue.Reset();
Queue.In(pixel);
while(!Queue.IsEmpty()) {
Queue.Out(pixel);
for(Neighbor.Begin(); !Neighbor.End(); Neighbor.Next()) {
neighbor = Neighbor.Where(pixel);
if(Label[neighbor] != NULL_LABEL) {
// `neighbor' has been visited.
continue;
}
Label[neighbor] = label;
Queue.In(neighbor);
}
}
}
// Resolving pointers
for(pixel=begin; pixel<end; pixel++) {
label = Label[pixel];
if(label >= RIDGE_LABEL) {
// `pixe' is a sentinel, or it has been labelled.
continue;
}
Queue.Reset();
for(;;) {
neighbor = - label;
label = Label[neighbor];
if(label >= RIDGE_LABEL) {
break;
}
Queue.In(neighbor);
}
Label[pixel] = label;
while(!Queue.IsEmpty()) {
Queue.Out(neighbor);
Label[neighbor] = label;
}
}
// Memory allocated by other functions
delete[] Gradient;
// Removing the sentinels
offset = Width + 3;
int_scan = Label;
for(index=0; index<Height; index++) {
memcpy(int_scan, int_scan+offset, sizeof(int) * Width);
offset += 2;
int_scan += Width;
}
return Label;
}
int* GenericToboggan<T, OrderInvariant, Connectivity>::
GetLabelImage(const T* OldGradient, const int Width, const int Height)
{
TNeighbor<Connectivity> Neighbor(1, Width+2);
TQueue<int> Queue(Width*Height);
struct {
int Offset;
int NextState;
} TransitionTable[1 << Connectivity];
T *Gradient, level, min_level;
int *Label, *Dist, *BacktrackState, *int_scan, LabelCount;
int offset, index, dist, begin, end, pixel, label, neighbor, state, sliding_list;
int NegativeMask;
if(sizeof(int)*8-2 < Connectivity) {
// See the trick below.
throw "GenericToboggan_OrderInvariant => the number of bits of integer is not large enough";
}
GetMin(NegativeMask); // Setting the most significant bit of `NegativeMask'.
// Surrounding the gradient image with sentinesl.
Gradient = MakeNewGradient<T>(OldGradient, Width, Height);
// Memory allocation
Label = new int[(Width+2)*(Height+2)];
if(OrderInvariant) { // ---> ADDITIONAL CODE FOR ORDER-INVARIANT ALGORITHM
Dist = new int[(Width+2)*(Height+2)];
} ////////////////////////////////////////////////////////////////////////
if(!Label || (!Dist && OrderInvariant)) {
throw "GenericToboggan_OrderInvariant => cannot allocate memory";
}
// Initialization
// Setting the sentinels as ridges
memset(Label+0, -1, sizeof(int)*(Width+2)); // RIDGE_LABEL == -1
offset = Width+2;
for(index=1; index<=Height; index++) {
memset(Label+offset+1, 0, sizeof(int)*Width); // NULL_LABEL == 0
Label[offset+0] = Label[offset+Width+1] = RIDGE_LABEL;
offset += Width+2;
}
memset(Label+offset, -1, sizeof(int)*(Width+2)); // RIDGE_LABEL == -1
if(OrderInvariant) { // ---> ADDITIONAL CODE FOR ORDER-INVARIANT ALGORITHM
memcpy(Dist, Label, sizeof(int)*(Width+2)*(Height+2));
// The memory allocated by `Queue' is shared with `BacktrackState'.
BacktrackState = Dist;
} ////////////////////////////////////////////////////////////////////////
LabelCount = 0;
Queue.Reset();
// Downward sliding
begin = Width+3;
end = (Width+2)*(Height+2)-(Width+3);
for(pixel=begin; pixel<end; pixel++) {
if(Label[pixel] != NULL_LABEL) {
// `pixel' is one of the sentinels.
continue;
}
sliding_list = 0;
min_level = Gradient[pixel];
// Search for the steepest direction
for(Neighbor.Begin(); !Neighbor.End(); Neighbor.Next()) {
neighbor = Neighbor.Where(pixel);
level = Gradient[neighbor];
if(level < min_level) {
// ************************* Trick ***********************************
// We set the i-th bit if the label of `pixel' depends on `neighbor',
// where i is the index of the direction from `pixel' to `neighbor'.
// The most significant bit of the value will be set, so that it can
// be identified as a sliding list.
// Notice that the second-most significant bit must be zero, so that
// it guarantees the values of sliding lists are less than RIDGE_LABEL.
// *******************************************************************
sliding_list = (1 << Neighbor.Direction());
min_level = level;
continue;
}
if(OrderInvariant) { // ---> ADDITIONAL CODE FOR ORDER-INVARIANT ALGORITHM
if(level == min_level && sliding_list != 0) {
sliding_list |= (1 << Neighbor.Direction());
}
} ////////////////////////////////////////////////////////////////////////
}
if(sliding_list != 0) {
Queue.In(pixel);
Label[pixel] = sliding_list | NegativeMask;
}
}
// Region growing from the lower boundaries
while(!Queue.IsEmpty()) {
Queue.Out(pixel);
level = Gradient[pixel];
dist = Dist[pixel] + 1;
for(Neighbor.Begin(); !Neighbor.End(); Neighbor.Next()) {
// Using `Backtrack' to ensure the correctness of the further use of `Direction'.
neighbor = Neighbor.Backtrack(pixel);
if(Gradient[neighbor] != level) {
// `neighbor' is not at the same level as `pixel'.
continue;
}
if(Label[neighbor] == NULL_LABEL) {
Label[neighbor] = (1 << Neighbor.Direction()) | NegativeMask; // See the above trick.
Queue.In(neighbor);
if(OrderInvariant) { // ---> ADDITIONAL CODE FOR ORDER-INVARIANT ALGORITHM
Dist[neighbor] = dist;
} ////////////////////////////////////////////////////////////////////////
continue;
}
if(OrderInvariant) { // ---> ADDITIONAL CODE FOR ORDER-INVARIANT ALGORITHM
// `neighbor' has been visited.
if(Dist[neighbor] == dist) {
// and there are multiple ways for it to slide downward.
Label[neighbor] |= (1 << Neighbor.Direction());
}
} ////////////////////////////////////////////////////////////////////////
}
}
// Finding the minimal regions
for(index=begin; index<end; index++) {
pixel = index;
if(Label[pixel] != NULL_LABEL) {
// `pixel' has been visited
continue;
}
Label[pixel] = label = ++ LabelCount;
Queue.Reset();
Queue.In(pixel);
while(!Queue.IsEmpty()) {
Queue.Out(pixel);
for(Neighbor.Begin(); !Neighbor.End(); Neighbor.Next()) {
neighbor = Neighbor.Where(pixel);
if(Label[neighbor] != NULL_LABEL) {
// `neighbor' has been visited.
continue;
}
Label[neighbor] = label;
Queue.In(neighbor);
}
}
}
// Initiation of the transition table
TransitionTable[0].Offset = 0;
TransitionTable[0].NextState = 0;
for(Neighbor.Begin(), index=1; !Neighbor.End(); Neighbor.Next(), index*=2) {
offset = Neighbor.Where(0);
for(state=index; state<(1<<Connectivity); state+=index*2) {
TransitionTable[state].Offset = offset;
TransitionTable[state].NextState = state - index;
}
}
// Resolving the sliding lists
for(index=begin; index<end; index++) {
pixel = index;
sliding_list = Label[pixel];
if(sliding_list >= RIDGE_LABEL) {
// `pixel' has been labeled.
continue;
}
if(OrderInvariant) { // ---> CODE FOR ORDER-INVARIANT ALGORITHM ////////////////////
// Depth-First-Search (Topological Sort)
// Non-recursion Implementation
// Initializing the variables of the top level
BacktrackState[pixel] = 0;
state = sliding_list ^ NegativeMask;
label = NULL_LABEL;
while(state) {
neighbor = pixel + TransitionTable[state].Offset;
sliding_list = Label[neighbor];
// Has the label of `neighbor' been resolved?
if(sliding_list < RIDGE_LABEL) {
// Starting a new level
// Saving the variables of the current level
Label[pixel] = label;
BacktrackState[neighbor] = state;
// Initializing the variables of the next level
pixel = neighbor;
label = NULL_LABEL;
state = sliding_list ^ NegativeMask;
continue;
}
// Resolving [label(neighbor) == sliding_list]
if(label == NULL_LABEL) {
label = sliding_list;
}
else if(label != sliding_list) {
label = RIDGE_LABEL;
state = 0; // Early-jump-out
}
// Transition
while((state = TransitionTable[state].NextState) == 0) {
// (state == 0) --> Backtracking
state = BacktrackState[pixel];
// Saving the result of this level
Label[pixel] = label;
if(state == 0) {
// Reaching the top level
break;
}
// Restoring the variables of the upper level
pixel -= TransitionTable[state].Offset;
// Resolving
if(Label[pixel] != NULL_LABEL) {
if(Label[pixel] != label) {
label = RIDGE_LABEL;
state = 0; // Early-jump-out
}
}
}
}
}
else { // ---> CODE FOR NON-ORDER-INVARIANT ALGORITHM ////////////////////
Queue.Reset();
state = sliding_list ^ NegativeMask;
for(;;) {
neighbor = pixel + TransitionTable[state].Offset;
sliding_list = Label[neighbor];
if(sliding_list >= RIDGE_LABEL) {
// label(neighbor) == sliding_list
break;
}
Queue.In(neighbor);
pixel = neighbor;
state = sliding_list ^ NegativeMask;
}
Label[index] = label = sliding_list;
while(!Queue.IsEmpty()) {
Queue.Out(pixel);
Label[pixel] = label;
}
} ////////////////////////////////////////////////////////////////////////
}
// Memory allocated by other functions
delete[] Gradient;
// Memory allocated by this function
if(OrderInvariant) { // ---> ADDITIONAL CODE FOR ORDER-INVARIANT ALGORITHM
delete[] Dist;
} ////////////////////////////////////////////////////////////////////////
// Removing the sentinels
offset = Width + 3;
int_scan = Label;
for(index=0; index<Height; index++) {
memcpy(int_scan, int_scan+offset, sizeof(int) * Width);
offset += 2;
int_scan += Width;
}
return Label;
}
// Attempts to add a neighbor to the neighborhood
// based on the parameterized ID, state, and relationships,
// returning true if successful, false otherwise.
bool Neighborhood::addNbr(const int id, const State s,
const Vector desired, const Vector actual)
{
return addNbr(Neighbor(id, s, desired, actual));
}
// Attempts to add a neighbor to the neighborhood
// based on the parameterized relationship and state,
// returning true if successful, false otherwise.
bool Neighborhood::addNbr(const Relationship r, const State s)
{
return addNbr(Neighbor(r, s));
}
template<typename PointT, typename PointLT> void
pcl::OrganizedEdgeDetection<PointT, PointLT>::compute (pcl::PointCloud<PointLT>& labels, std::vector<pcl::PointIndices>& label_indices) const
{
const unsigned invalid_label = std::numeric_limits<unsigned>::max ();
pcl::Label invalid_pt;
invalid_pt.label = std::numeric_limits<unsigned>::max ();
labels.points.resize (input_->points.size (), invalid_pt);
labels.width = input_->width;
labels.height = input_->height;
unsigned int clust_id = 0;
std::cout << "width: " << labels.width << std::endl;
std::cout << "height: " << labels.height << std::endl;
std::cout << "[done] OrganizedEdgeDetection::compute ()" << std::endl;
// fill lookup table for next points to visit
const int num_of_ngbr = 8;
Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1),
Neighbor(-1, -1, -labels.width - 1),
Neighbor( 0, -1, -labels.width ),
Neighbor( 1, -1, -labels.width + 1),
Neighbor( 1, 0, 1),
Neighbor( 1, 1, labels.width + 1),
Neighbor( 0, 1, labels.width ),
Neighbor(-1, 1, labels.width - 1)};
for (int row = 1; row < int(input_->height) - 1; row++)
{
for (int col = 1; col < int(input_->width) - 1; col++)
{
int curr_idx = row*int(input_->width) + col;
if (!pcl_isfinite (input_->points[curr_idx].z))
continue;
float curr_depth = fabs (input_->points[curr_idx].z);
// Calculate depth distances between current point and neighboring points
std::vector<float> nghr_dist;
nghr_dist.resize (8);
bool found_invalid_neighbor = false;
for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
{
int nghr_idx = curr_idx + directions[d_idx].d_index;
assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
if (!pcl_isfinite (input_->points[nghr_idx].z))
{
found_invalid_neighbor = true;
break;
}
nghr_dist[d_idx] = curr_depth - fabs (input_->points[nghr_idx].z);
}
if (!found_invalid_neighbor)
{
// Every neighboring points are valid
std::vector<float>::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ());
std::vector<float>::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ());
float nghr_dist_min = *min_itr;
float nghr_dist_max = *max_itr;
float dist_dominant = fabs (nghr_dist_min) > fabs (nghr_dist_max) ? nghr_dist_min : nghr_dist_max;
if (fabs (dist_dominant) > th_depth_discon_*fabs (curr_depth))
{
// Found a depth discontinuity
if (dist_dominant > 0.f)
labels[curr_idx].label = EDGELABEL_OCCLUDED;
else
labels[curr_idx].label = EDGELABEL_OCCLUDING;
}
}
else
{
// Some neighboring points are not valid (nan points)
// Search for corresponding point across invalid points
// Search direction is determined by nan point locations with respect to current point
int dx = 0;
int dy = 0;
int num_of_invalid_pt = 0;
for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++)
{
int nghr_idx = curr_idx + directions[d_idx].d_index;
assert (nghr_idx >= 0 && nghr_idx < input_->points.size ());
if (!pcl_isfinite (input_->points[nghr_idx].z))
{
dx += directions[d_idx].d_x;
dy += directions[d_idx].d_y;
num_of_invalid_pt++;
}
}
// Search directions
assert (num_of_invalid_pt > 0);
float f_dx = static_cast<float> (dx) / static_cast<float> (num_of_invalid_pt);
float f_dy = static_cast<float> (dy) / static_cast<float> (num_of_invalid_pt);
// Search for corresponding point across invalid points
float corr_depth = std::numeric_limits<float>::quiet_NaN ();
for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++)
{
int s_row = row + static_cast<int> (std::floor (f_dy*static_cast<float> (s_idx)));
int s_col = col + static_cast<int> (std::floor (f_dx*static_cast<float> (s_idx)));
if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width))
break;
if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z))
{
corr_depth = fabs (input_->points[s_row*int(input_->width)+s_col].z);
break;
}
}
if (!pcl_isnan (corr_depth))
{
// Found a corresponding point
float dist = curr_depth - corr_depth;
if (fabs (dist) > th_depth_discon_*fabs (curr_depth))
{
// Found a depth discontinuity
if (dist > 0.f)
labels[curr_idx].label = EDGELABEL_OCCLUDED;
else
labels[curr_idx].label = EDGELABEL_OCCLUDING;
}
}
else
{
// Not found a corresponding point, just nan boundary edge
labels[curr_idx].label = EDGELABEL_NAN_BOUNDARY;
}
}
}
}
// Assign label indices
label_indices.resize (3);
for (unsigned idx = 0; idx < input_->points.size (); idx++)
{
if (labels[idx].label != invalid_label)
{
label_indices[labels[idx].label].indices.push_back (idx);
}
}
}
int PCS_Model::SaveToPOF(std::string filename, AsyncProgress* progress)
{
PCS_Model::BSP_MAX_DEPTH = 0;
PCS_Model::BSP_NODE_POLYS = 1;
PCS_Model::BSP_TREE_TIME = 0;
PCS_Model::BSP_COMPILE_ERROR = false;
POF poffile;
unsigned int i,j,k,l;
progress->setTarget(6 + light_arrays.size() + ai_paths.size() + insignia.size() + shield_mesh.size() +
thrusters.size() + docking.size() + turrets.size() + weapons.size() + special.size() +
eyes.size() + model_info.size() + subobjects.size() + textures.size());
char cstringtemp[256];
// Update Progress
progress->incrementWithMessage("Writing Header Pt1");
// --------- convert cross sections ---------
std::vector<cross_section> sections;
sections.resize(header.cross_sections.size());
for (i = 0; i < header.cross_sections.size(); i++)
{
sections[i].depth = header.cross_sections[i].depth;
sections[i].radius = header.cross_sections[i].radius;
}
poffile.HDR2_Set_CrossSections(header.cross_sections.size(), sections);
// Update Progress
progress->incrementWithMessage("Writing Header Pt2");
// --------- ACEN ---------
poffile.ACEN_Set_acen(POFTranslate(autocentering));
// Update Progress
progress->incrementWithMessage("Writing Acen");
// --------- TXTR ---------
// Update Progress
progress->incrementWithMessage("Writing Textures");
for (i = 0; i < textures.size(); i++)
poffile.TXTR_AddTexture(textures[i]);
// --------- Sub object Consversion ---------
wxLongLong time = wxGetLocalTimeMillis();
bool bsp_compiled = false;
header.max_radius = 0.0f;
for (i = 0; i < subobjects.size(); i++)
{
// Update Progress
//if (subobjects[i].name == "debris08")
// sprintf(cstringtemp, "Submodel %d: %s SENTINAL!", i, subobjects[i].name.c_str());
//else
sprintf(cstringtemp, "Submodel %d: %s", i, subobjects[i].name.c_str());
progress->incrementWithMessage(cstringtemp);
//memset((char *)&obj, 0, sizeof(OBJ2)); this is NO LONGER ALLOWED - obj2 now contains an class w/ vtable
boost::scoped_ptr<OBJ2> obj(new OBJ2);
obj->submodel_number = i;
if (!PMFObj_to_POFObj2(i, *obj, bsp_compiled, header.max_radius))
{
return 2; // error occured in bsp splitting!
}
poffile.OBJ2_Add(*obj); // takes over object management - including pointers
}
time = wxGetLocalTimeMillis() - time;
// we succeeded in compiling - let's cache the result
can_bsp_cache = true;
bsp_cache.resize(subobjects.size());
for (i = 0; i < subobjects.size(); i++)
poffile.OBJ2_Get_BSPData(i, bsp_cache[i].bsp_data);
// --------- ---------------------- ---------
int idx = GetModelInfoCount();
char cstrtmp[256];
wxString strtmp = PCS2_VERSION;
sprintf(cstrtmp, "PMFSaveToPOF: Compiled on %s with %s\nmax BSP depth was %d\nmost polys in a single node was %d\nTotal Compile time was %ldms, tree generation time was %ldms", std::string(strtmp.mb_str()).c_str(), std::string(PCS2_COMP_VERSION.mb_str()).c_str(), PCS_Model::BSP_MAX_DEPTH,PCS_Model::BSP_NODE_POLYS, time.ToLong(), PCS_Model::BSP_TREE_TIME.ToLong());
bool found = false;
for (i = 0; i < model_info.size() && !found; i++)
{
if (strstr(model_info[i].c_str(), "PMFSaveToPOF") != NULL)
{
found = true;
if (bsp_compiled) // update the string
model_info[i] = cstrtmp;
}
}
if (!found)
AddModelInfo(cstrtmp);
j = 0;
for (i = 0; i < model_info.size(); i++)
j += model_info[i].length() + 1;
boost::scoped_ptr<char> pinf(new char[j]);
memset(pinf.get(), 0, j);
j = 0;
for (i = 0; i < model_info.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing String %d", i);
progress->incrementWithMessage(cstringtemp);
strncpy(pinf.get()+j, model_info[i].c_str(), model_info[i].length());
j+= model_info[i].length() + 1;
}
poffile.PINF_Set(pinf.get(), j);
if (found)
model_info.resize(idx); // back down to size
// --------- EYE ---------
for (i = 0; i < eyes.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Eye %d", i);
progress->incrementWithMessage(cstringtemp);
poffile.EYE_Add_Eye(eyes[i].sobj_number,
POFTranslate(eyes[i].sobj_offset),
POFTranslate(eyes[i].normal));
}
// --------- SPCL ---------
for (i = 0; i < special.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Special %d", i);
progress->incrementWithMessage(cstringtemp);
poffile.SPCL_AddSpecial(special[i].name, special[i].properties,
POFTranslate(special[i].point), special[i].radius);
}
k = l = 0;
// --------- weapons (GPNT/MPNT) ---------
for (i = 0; i < weapons.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Weapon %d", i);
progress->incrementWithMessage(cstringtemp);
if (weapons[i].type == GUN)
{
poffile.GPNT_AddSlot();
for (j = 0; j < weapons[i].muzzles.size(); j++)
{
poffile.GPNT_AddPoint(k, POFTranslate(weapons[i].muzzles[j].point),
POFTranslate(weapons[i].muzzles[j].norm));
}
k++;
}
else
{ poffile.MPNT_AddSlot();
for (j = 0; j < weapons[i].muzzles.size(); j++)
{
poffile.MPNT_AddPoint(l, POFTranslate(weapons[i].muzzles[j].point),
POFTranslate(weapons[i].muzzles[j].norm));
}
l++;
}
}
// --------- turrets TGUN/TMIS ---------
k = l = 0;
for (i = 0; i < turrets.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Turret %d", i);
progress->incrementWithMessage(cstringtemp);
if (turrets[i].type == GUN)
{
poffile.TGUN_Add_Bank(turrets[i].sobj_parent,
turrets[i].sobj_par_phys,
POFTranslate(turrets[i].turret_normal));
for (j = 0; j < turrets[i].fire_points.size(); j++)
{
poffile.TGUN_Add_FirePoint(k, POFTranslate(turrets[i].fire_points[j]));
}
k++;
}
else
{
poffile.TMIS_Add_Bank(turrets[i].sobj_parent,
turrets[i].sobj_par_phys,
POFTranslate(turrets[i].turret_normal));
for (j = 0; j < turrets[i].fire_points.size(); j++)
{
poffile.TMIS_Add_FirePoint(l, POFTranslate(turrets[i].fire_points[j]));
}
l++;
}
}
// --------- docking ---------
for (i = 0; i < docking.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Docking %d", i);
progress->incrementWithMessage(cstringtemp);
poffile.DOCK_Add_Dock(docking[i].properties);
for (j = 0; j < docking[i].dockpoints.size(); j++)
{
poffile.DOCK_Add_Point(i, POFTranslate(docking[i].dockpoints[j].point),
POFTranslate(docking[i].dockpoints[j].norm));
}
for (j = 0; j < docking[i].paths.size(); j++)
{
poffile.DOCK_Add_SplinePath(i, docking[i].paths[j]);
}
}
// --------- thrusters ---------
for (i = 0; i < thrusters.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Thruster %d", i);
progress->incrementWithMessage(cstringtemp);
poffile.FUEL_Add_Thruster(thrusters[i].properties);
for (j = 0; j < thrusters[i].points.size(); j++)
{
poffile.FUEL_Add_GlowPoint(i, thrusters[i].points[j].radius,
POFTranslate(thrusters[i].points[j].pos),
POFTranslate(thrusters[i].points[j].norm));
}
}
// --------- shield_mesh ---------
int fcs[3], nbs[3];
std::vector<vector3d> points(shield_mesh.size()*3);
vector3d shldtvect;
// make points list
l = 0;
for (i = 0; i < shield_mesh.size(); i++)
{
for (j = 0; j < 3; j++)
{
bool found_corner = false;
for (auto& p : points)
{
if (p == POFTranslate(shield_mesh[i].corners[j]))
{
found_corner = true;
break;
}
}
if (!found_corner)
{
if (l >= points.size())
points.resize(points.size()*2);
points[l] = POFTranslate(shield_mesh[i].corners[j]);
l++;
}
}
}
points.resize(l);
// translate shield mesh
for (i = 0; i < shield_mesh.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Shield Tri %d", i);
progress->incrementWithMessage(cstringtemp);
// adds points to list if need, determine fcs[]
for (j = 0; j < 3; j++)
{
shldtvect = POFTranslate(shield_mesh[i].corners[j]);
fcs[j] = FindInList(points, shldtvect);
}
// determine neighbors (nbs[])
j=0;
for (k = 0; k < shield_mesh.size() && j < 3; k++)
{
if (Neighbor(shield_mesh[i], shield_mesh[k]) && i != k)
{
nbs[j] = k;
j++;
}
}
// add
poffile.SHLD_Add_Face(POFTranslate(shield_mesh[i].face_normal), fcs, nbs);
}
// add points
// Update Progress
progress->incrementWithMessage("Writing Shield Points");
for (i = 0; i < points.size(); i++)
poffile.SHLD_Add_Vertex(points[i]);
progress->incrementWithMessage("Writing Shield Collision Tree");
// --------------- generate shield collision tree ----------------
if (poffile.SHLD_Count_Faces() > 0)
{
std::vector<pcs_polygon> shldmesh(poffile.SHLD_Count_Faces());
// make a pcs_polygon mesh from the shields
for (i = 0; i < shldmesh.size(); i++)
{
shldmesh[i].verts.resize(3);
poffile.SHLD_Get_Face(i, shldmesh[i].norm, fcs, nbs);
for (j = 0; j < 3; j++)
{
poffile.SHLD_Get_Vertex(fcs[j], shldmesh[i].verts[j].point);
shldmesh[i].verts[j].norm = shldmesh[i].norm;
}
shldmesh[i].centeroid = PolygonCenter(shldmesh[i]);
}
// make the tree
vector3d smin, smax;
std::unique_ptr<bsp_tree_node> shld_root = MakeTree(shldmesh, smax, smin);
// pack the tree
int sldc_size = CalcSLDCTreeSize(shld_root.get());
std::vector<char> sldc;
sldc.resize(sldc_size);
PackTreeInSLDC(shld_root.get(), 0, &sldc.front(), sldc_size);
poffile.SLDC_SetTree(std::move(sldc)); // POFHandler will steal our copy of the buffer
}
// --------- insignia ---------
vector3d uv, vv;
float *u = (float *)&uv, *v = (float *)&vv;
for (i = 0; i < insignia.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Insignia %d", i);
progress->incrementWithMessage(cstringtemp);
poffile.INSG_Add_insignia(insignia[i].lod, POFTranslate(insignia[i].offset));
for (j = 0; j < insignia[i].faces.size(); j++)
{
for (k = 0; k < 3; k++)
{
while ((l = poffile.INST_Find_Vert(i, POFTranslate(insignia[i].faces[j].verts[k]))) == (unsigned)-1)
{
poffile.INSG_Add_Insig_Vertex(i, POFTranslate(insignia[i].faces[j].verts[k]));
}
fcs[k] = l;
u[k] = insignia[i].faces[j].u[k];
v[k] = insignia[i].faces[j].v[k];
}
poffile.INSG_Add_Insig_Face(i, fcs, uv, vv);
}
}
// --------- ai_paths ---------
for (i = 0; i < ai_paths.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Path %d", i);
progress->incrementWithMessage(cstringtemp);
poffile.PATH_Add_Path(ai_paths[i].name, ai_paths[i].parent);
for (j = 0; j < ai_paths[i].verts.size(); j++)
{
poffile.PATH_Add_Vert(i, POFTranslate(ai_paths[i].verts[j].pos),
ai_paths[i].verts[j].radius);
}
}
// --------- light arrays ---------
pcs_glow_array *gla;
for (i = 0; i < light_arrays.size(); i++)
{
// Update Progress
sprintf(cstringtemp, "Writing Glow %d", i);
progress->incrementWithMessage(cstringtemp);
gla = &light_arrays[i];
poffile.GLOW_Add_LightGroup(gla->disp_time, gla->on_time, gla->off_time,
gla->obj_parent, gla->LOD, gla->type, gla->properties);
for (j = 0; j < gla->lights.size(); j++)
{
poffile.GLOW_Add_GlowPoint(i, gla->lights[j].radius,
POFTranslate(gla->lights[j].pos),
POFTranslate(gla->lights[j].norm));
}
}
// --------- HDR2 ---------
// let's make new BBoxes based on the subobjects
vector3d minbox, maxbox, tmpmin, tmpmax;
poffile.OBJ2_Get_BoundingMax(0, maxbox);
poffile.OBJ2_Get_BoundingMin(0, minbox);
for (i = 1; i < poffile.OBJ2_Count(); i++)
{
vector3d sobj_offset(POFTranslate(OffsetFromParent(i)));
poffile.OBJ2_Get_BoundingMax(i, tmpmax);
poffile.OBJ2_Get_BoundingMin(i, tmpmin);
ExpandBoundingBoxes(maxbox, minbox, tmpmax + sobj_offset);
ExpandBoundingBoxes(maxbox, minbox, tmpmin + sobj_offset);
}
for (i = 0; i < poffile.SHLD_Count_Vertices(); i++)
{
poffile.SHLD_Get_Vertex(i, tmpmax);
ExpandBoundingBoxes(maxbox, minbox, tmpmax);
}
// update our bounding boxes
//axis doesn't matter on the bounding boxes - it's all negative/positive values! i'm a faqing moron
poffile.HDR2_Set_MinBound(header.min_bounding_overridden ? header.min_bounding_override : minbox);
poffile.HDR2_Set_MaxBound(header.max_bounding_overridden ? header.max_bounding_override : maxbox);
this->header.max_bounding = minbox;
this->header.min_bounding = maxbox;
poffile.HDR2_Set_MaxRadius(header.max_radius_overridden ? header.max_radius_override : header.max_radius);
poffile.HDR2_Set_Details(header.detail_levels.size(), header.detail_levels);
poffile.HDR2_Set_Debris(header.debris_pieces.size(), header.debris_pieces);
poffile.HDR2_Set_Mass(header.mass);
poffile.HDR2_Set_MassCenter(POFTranslate(header.mass_center));
poffile.HDR2_Set_MomentInertia(header.MOI);
poffile.HDR2_Set_SOBJCount(GetSOBJCount());
std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary);
if (!poffile.SavePOF(out))
return 1;
return 0;
}
template<typename PointT, typename PointLT> void
pcl::OrganizedConnectedComponentSegmentation<PointT, PointLT>::findLabeledRegionBoundary (int start_idx, PointCloudLPtr labels, pcl::PointIndices& boundary_indices)
{
boundary_indices.indices.clear ();
int curr_idx = start_idx;
int curr_x = start_idx % labels->width;
int curr_y = start_idx / labels->width;
unsigned label = labels->points[start_idx].label;
// fill lookup table for next points to visit
Neighbor directions [8] = {Neighbor(-1, 0, -1),
Neighbor(-1, -1, -labels->width - 1),
Neighbor( 0, -1, -labels->width ),
Neighbor( 1, -1, -labels->width + 1),
Neighbor( 1, 0, 1),
Neighbor( 1, 1, labels->width + 1),
Neighbor( 0, 1, labels->width ),
Neighbor(-1, 1, labels->width - 1)};
// find one pixel with other label in the neighborhood -> assume thats the one we came from
int direction = -1;
int x;
int y;
int index;
for (unsigned dIdx = 0; dIdx < 8; ++dIdx)
{
x = curr_x + directions [dIdx].d_x;
y = curr_y + directions [dIdx].d_y;
index = curr_idx + directions [dIdx].d_index;
if (x >= 0 && x < int(labels->width) && y >= 0 && y < int(labels->height) && labels->points[index].label != label)
{
direction = dIdx;
break;
}
}
// no connection to outer regions => start_idx is not on the border
if (direction == -1)
return;
boundary_indices.indices.push_back (start_idx);
do {
unsigned nIdx;
for (unsigned dIdx = 1; dIdx <= 8; ++dIdx)
{
nIdx = (direction + dIdx) & 7;
x = curr_x + directions [nIdx].d_x;
y = curr_y + directions [nIdx].d_y;
index = curr_idx + directions [nIdx].d_index;
if (x >= 0 && y < int(labels->width) && y >= 0 && y < int(labels->height) && labels->points[index].label == label)
break;
}
// update the direction
direction = (nIdx + 4) & 7;
curr_idx += directions [nIdx].d_index;
curr_x += directions [nIdx].d_x;
curr_y += directions [nIdx].d_y;
boundary_indices.indices.push_back(curr_idx);
} while ( curr_idx != start_idx);
}
ubi_btNodePtr ubi_btLocate( ubi_btRootPtr RootPtr,
ubi_btItemPtr FindMe,
ubi_trCompOps CompOp )
/* ------------------------------------------------------------------------ **
* The purpose of ubi_btLocate() is to find a node or set of nodes given
* a target value and a "comparison operator". The Locate() function is
* more flexible and (in the case of trees that may contain dupicate keys)
* more precise than the ubi_btFind() function. The latter is faster,
* but it only searches for exact matches and, if the tree contains
* duplicates, Find() may return a pointer to any one of the duplicate-
* keyed records.
*
* Input:
* RootPtr - A pointer to the header of the tree to be searched.
* FindMe - An ubi_btItemPtr that indicates the key for which to
* search.
* CompOp - One of the following:
* CompOp Return a pointer to the node with
* ------ ---------------------------------
* ubi_trLT - the last key value that is less
* than FindMe.
* ubi_trLE - the first key matching FindMe, or
* the last key that is less than
* FindMe.
* ubi_trEQ - the first key matching FindMe.
* ubi_trGE - the first key matching FindMe, or the
* first key greater than FindMe.
* ubi_trGT - the first key greater than FindMe.
* Output:
* A pointer to the node matching the criteria listed above under
* CompOp, or NULL if no node matched the criteria.
*
* Notes:
* In the case of trees with duplicate keys, Locate() will behave as
* follows:
*
* Find: 3 Find: 3
* Keys: 1 2 2 2 3 3 3 3 3 4 4 Keys: 1 1 2 2 2 4 4 5 5 5 6
* ^ ^ ^ ^ ^
* LT EQ GT LE GE
*
* That is, when returning a pointer to a node with a key that is LESS
* THAN the target key (FindMe), Locate() will return a pointer to the
* LAST matching node.
* When returning a pointer to a node with a key that is GREATER
* THAN the target key (FindMe), Locate() will return a pointer to the
* FIRST matching node.
*
* See Also: ubi_btFind(), ubi_btFirstOf(), ubi_btLastOf().
* ------------------------------------------------------------------------ **
*/
{
register ubi_btNodePtr p;
ubi_btNodePtr parent;
char whichkid;
/* Start by searching for a matching node. */
p = TreeFind( FindMe,
RootPtr->root,
&parent,
&whichkid,
RootPtr->cmp );
if( NULL != p ) /* If we have found a match, we can resolve as follows: */
{
switch( CompOp )
{
case ubi_trLT: /* It's just a jump to the left... */
p = Border( RootPtr, FindMe, p, ubi_trLEFT );
return( Neighbor( p, ubi_trLEFT ) );
case ubi_trGT: /* ...and then a jump to the right. */
p = Border( RootPtr, FindMe, p, ubi_trRIGHT );
return( Neighbor( p, ubi_trRIGHT ) );
default:
p = Border( RootPtr, FindMe, p, ubi_trLEFT );
return( p );
}
}
/* Else, no match. */
if( ubi_trEQ == CompOp ) /* If we were looking for an exact match... */
return( NULL ); /* ...forget it. */
/* We can still return a valid result for GT, GE, LE, and LT.
* <parent> points to a node with a value that is either just before or
* just after the target value.
* Remaining possibilities are LT and GT (including LE & GE).
*/
if( (ubi_trLT == CompOp) || (ubi_trLE == CompOp) )
return( (ubi_trLEFT == whichkid) ? Neighbor( parent, whichkid ) : parent );
else
return( (ubi_trRIGHT == whichkid) ? Neighbor( parent, whichkid ) : parent );
} /* ubi_btLocate */
void bilinear_interp_elem(ErrorElem *elem11, ErrorElem *elem21, ErrorElem *elem12,
ErrorElem *elem22, ErrorElem *Curr_El) {
double *bi_state_vars = Curr_El->get_bilin_state();
double *bi_prev_state_vars = Curr_El->get_bilin_prev_state();
double *bi_adjoint = Curr_El->get_bilin_adj();
double *x = Curr_El->get_coord();
ErrorElem* temp[4] = { elem11, elem21, elem12, elem22 };
vector<Neighbor> input;
vector<Neighbor> interpolant;
for (int i = 0; i < 4; ++i)
if (temp[i])
input.push_back(Neighbor(temp[i]));
int add = 0;
for (int i = 0; i < input.size(); ++i) {
add = 1;
for (int j = 0; j < interpolant.size(); ++j)
if (input[i] == interpolant[j]) {
add = 0;
break;
}
if (add)
interpolant.push_back(input[i]);
}
switch (interpolant.size()) {
case 0:
cout << "ERROR: NO ELEMENT FOR BILINEAR INTERPOLATION " << endl;
break;
case 1:
//father is in corner so there is no element for interp
//we do not do any extrapolation and leave as it is,
//which in refinement constructor should be the value of father element
// cout << "WARNING: ONE ELEMENT FOR BILINEAR INTERPOLATION " << endl;
for (int i = 0; i < NUM_STATE_VARS; ++i) {
bi_state_vars[i] = *(Curr_El->get_state_vars() + i);
bi_prev_state_vars[i] = *(Curr_El->get_prev_state_vars() + i);
bi_adjoint[i] = *(Curr_El->get_adjoint() + i);
}
break;
case 2: {
// two++;
NeighLess customLess;
sort(interpolant.begin(), interpolant.end(), customLess);
int xdir = 0, ydir = 1;
if (interpolant[1].x - interpolant[0].x < 1e-12)
//interpolation in y
for (int i = 0; i < NUM_STATE_VARS; ++i) {
bi_state_vars[i] = lineary_interp_wrapper(interpolant, state_vars, i, x[1]);
bi_prev_state_vars[i] = lineary_interp_wrapper(interpolant, prev_state_vars, i, x[1]);
bi_adjoint[i] = lineary_interp_wrapper(interpolant, adjoint, i, x[1]);
}
else
for (int i = 0; i < NUM_STATE_VARS; ++i) {
bi_state_vars[i] = linearx_interp_wrapper(interpolant, state_vars, i, x[0]);
bi_prev_state_vars[i] = linearx_interp_wrapper(interpolant, prev_state_vars, i, x[0]);
bi_adjoint[i] = linearx_interp_wrapper(interpolant, adjoint, i, x[0]);
}
break;
}
case 3: {
// this comes out of refinement near the boundary, and it's not possible to find all of the neighbors
// we do “barycentric interpolation” a.k.a. “convex combination” a.k.a. “affine linear extension
// three++;
for (int i = 0; i < NUM_STATE_VARS; ++i) {
bi_state_vars[i] = barycentric_interpolation_wrapeper(interpolant, state_vars, i, x[0],
x[1]);
bi_prev_state_vars[i] = barycentric_interpolation_wrapeper(interpolant, prev_state_vars, i,
x[0], x[1]);
bi_adjoint[i] = barycentric_interpolation_wrapeper(interpolant, adjoint, i, x[0], x[1]);
}
break;
}
case 4: {
// four++;
// bilinear interpolation
for (int i = 0; i < NUM_STATE_VARS; ++i) {
bi_state_vars[i] = bilinear_interpolation_wrapper(interpolant, state_vars, i, x[0], x[1]);
bi_prev_state_vars[i] = bilinear_interpolation_wrapper(interpolant, prev_state_vars, i,
x[0], x[1]);
bi_adjoint[i] = bilinear_interpolation_wrapper(interpolant, adjoint, i, x[0], x[1]);
}
break;
}
default:
cout << "ERROR IN BILINEAR INTERPOLATION " << endl;
break;
}
for (int i=0;i<NUM_STATE_VARS;++i){
assert(!isinf(bi_state_vars[i]) && !isinf(bi_prev_state_vars[i]) && !isinf(bi_adjoint[i]));
assert(!isnan(bi_state_vars[i]) && !isnan(bi_prev_state_vars[i]) && !isnan(bi_adjoint[i]));
}
}
