/**
* Prints graph node in stderr.
*/
void vf_Graph::DumpNode(vf_NodeHandle node) // a graph node
{
#if _VF_DEBUG
// print node incoming edges
vf_EdgeHandle edge;
for (edge = node->m_inedge; edge; edge = edge->m_innext) {
VF_DEBUG(" [" << GetNodeNum(edge->m_start) << "] -->");
}
switch (node->m_type) {
case VF_NODE_START_ENTRY:
VF_DEBUG("node[" << GetNodeNum(node) << "]: start node");
break;
case VF_NODE_END_ENTRY:
VF_DEBUG("node[" << GetNodeNum(node) << "]: end node");
break;
case VF_NODE_HANDLER:
VF_DEBUG("node[" << GetNodeNum(node) << "]: exception handler");
break;
default:
DumpNodeInternal(node);
}
// print node outcoming edges
for (edge = node->m_outedge; edge; edge = edge->m_outnext) {
VF_DEBUG(" --> [" << GetNodeNum(edge->m_end) << "]");
}
VF_DEBUG("");
#endif // _VF_DEBUG
} // vf_Graph::DumpNode
int GetNodeNum(BSTree T)
{
if (T == NULL){
return 0;
}
return GetNodeNum(T->lchild) + GetNodeNum(T->rchild) + 1;
}
int CheckVertical (vec3_t start, float x, float y, float z, int max_steps_up, int max_steps_down)
{
int i, n, max_steps;
vec3_t v;
VectorCopy(start, v);
max_steps = max_steps_up + max_steps_down;
v[0] += x; // left-right
v[1] += y; // forward-backward
// step up from this location until we find a node
for (i=0; i<=max_steps; i++)
{
if ((n = GetNodeNum(v)) != -1)
return n;
if (i <= max_steps_up)
v[2] += z;
else
v[2] -= z;
}
return -1;
}
REAL QPBO<REAL>::ComputeTwiceEnergy(int option)
{
REAL E = 2*zero_energy, E1[2], E2[2][2];
int i, j, e;
int xi, xj;
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E1[0], E1[1]);
if (option == 0) xi = (nodes[0][i].label < 0) ? 0 : nodes[0][i].label;
else xi = nodes[0][i].user_label;
code_assert(xi==0 || xi==1);
E += E1[xi] - E1[0];
}
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E2[0][0], E2[0][1], E2[1][0], E2[1][1]);
if (option == 0)
{
xi = (nodes[0][i].label < 0) ? 0 : nodes[0][i].label;
xj = (nodes[0][j].label < 0) ? 0 : nodes[0][j].label;
}
else
{
xi = nodes[0][i].user_label;
xj = nodes[0][j].user_label;
}
E += E2[xi][xj] - E2[0][0];
}
return E;
}
/**
* Dumps graph node instruction in file stream in DOT format.
*/
void vf_Graph::DumpDotNodeInternal(vf_NodeHandle node, // graph node number
char *next_node, // separator between nodes in stream
char *next_instr, // separator between instructions in stream
ofstream &out) // output file stream
{
#if _VF_DEBUG
// print node header
out << " [label=\"";
out << "Node " << GetNodeNum(node) << next_node
<< "Stack mod: " << node->m_stack << next_node;
// get code instructions
unsigned count = node->m_end - node->m_start + 1;
vf_InstrHandle instr = node->m_start;
// print node instructions
for (unsigned index = 0; index < count; index++, instr++) {
out << index << ": " << ((instr->m_stack < 0) ? "[" : "[ ")
<< instr->m_stack << "\\| " << instr->m_minstack << "] "
<< vf_opcode_names[*(instr->m_addr)] << next_instr;
}
out << "\"" << endl;
if (node->m_sub) {
out << ", color=\"#B2F" << hex
<< (vf_get_sub_num(node->m_sub, m_ctx) % 16) << dec << "EE\"";
}
out << "]" << endl;
#endif // _VF_DEBUG
} // vf_Graph::DumpDotNodeInternal
// get position for current boundary conditions
// assumes nodal BC, override for particle BC
void BoundaryCondition::GetPosition(double *xpos,double *ypos,double *zpos,double *rot)
{ int i=GetNodeNum();
*xpos=nd[i]->x;
*ypos=nd[i]->y;
*zpos=nd[i]->z;
*rot=0.;
}
/**
* Dumps graph node in file in DOT format.
*/
void vf_Graph::DumpDotNode(vf_NodeHandle node, // graph node number
ofstream &out) // output file stream
{
#if _VF_DEBUG
// print node to dot file
out << "node" << GetNodeNum(node);
if (VF_NODE_START_ENTRY == node->m_type) { // start node
out << " [label=\"START\", color=limegreen]" << endl;
} else if (VF_NODE_END_ENTRY == node->m_type) { // end node
out << " [label=\"END\", color=orangered]" << endl;
} else if (VF_NODE_HANDLER == node->m_type) { // handler node
out << " [label=\"Handler #"
<< GetNodeNum(node) << "\", shape=ellipse, color=";
if (node->m_sub) {
out << "\"#B7FFE" << hex
<< (vf_get_sub_num(node->m_sub, m_ctx) % 16) << dec << "\"";
} else {
out << "aquamarine";
}
out << "]" << endl;
} else { // other nodes
DumpDotNodeInternal(node, "\\n---------\\n", "\\l", out);
}
// print node outcoming edges to dot file
for (vf_EdgeHandle edge = node->m_outedge; edge; edge = edge->m_outnext) {
out << "node" << GetNodeNum(node) << " -> "
<< "node" << GetNodeNum(edge->m_end);
if (VF_NODE_HANDLER == edge->m_end->m_type) {
out << "[color=red]" << endl;
} else if (vf_is_jsr_branch(edge, m_ctx)) {
out << "[color=blue]" << endl;
}
out << ";" << endl;
}
#endif // _VF_DEBUG
} // vf_Graph::DumpDotNode
REAL QPBO<REAL>::ComputeTwiceEnergy(int* solution)
{
REAL E = 2*zero_energy, E1[2], E2[2][2];
int i, j, e;
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E1[0], E1[1]);
if (solution[i] == 1) E += E1[1];
}
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E2[0][0], E2[0][1], E2[1][0], E2[1][1]);
E += E2[(solution[i] == 1) ? 1 : 0][(solution[j] == 1) ? 1 : 0] - E2[0][0];
}
return E;
}
REAL QPBO<REAL>::ComputeTwiceLowerBound()
{
REAL lowerBound = 2*zero_energy, E0, E1, E00, E01, E10, E11;
int i, j, e;
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E0, E1);
if (E0 > E1) lowerBound += E1 - E0;
}
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E00, E01, E10, E11);
lowerBound -= E00;
}
return lowerBound;
}
/**
* Prints graph node instruction in stream.
*/
void vf_Graph::DumpNodeInternal(vf_NodeHandle node) // graph node number
{
#if _VF_DEBUG
// print node header
VF_DEBUG("Node #" << GetNodeNum(node));
VF_DEBUG("Stack mod: " << node->m_stack);
DumpSub(node->m_sub);
// get code instructions
unsigned count = node->m_end - node->m_start + 1;
vf_InstrHandle instr = node->m_start;
// print node instructions
for (unsigned index = 0; index < count; index++, instr++) {
VF_DEBUG(index << ": " << ((instr->m_stack < 0) ? "[" : "[ ")
<< instr->m_stack << "| " << instr->m_minstack << "] "
<< vf_opcode_names[*(instr->m_addr)]);
}
#endif // _VF_DEBUG
} // vf_Graph::DumpNodeInternal
/// 求二叉树中的结点个数
/// 思路: 如果二叉树为空，结点个数为0
/// 如果二叉树不为空，二叉树结点个数 = 左子树结点个数 + 右子树结点个数 + 1
int GetNodeNum(TreeNode* root)
{
if(root == NULL)
return 0;
return GetNodeNum(root->left) + GetNodeNum(root->right) + 1;
}
//=========================================
int FindPath(vec3_t start, vec3_t destination) {
node_t *StartNode;
node_t *BestNode;
node_t *tNode;
int NodeNumD;
int NodeNumS;
int g,c,i;
float h;
vec3_t tstart,tdest;
VectorCopy(start,tstart);
VectorCopy(destination,tdest);
// Get NodeNum of start vector
NodeNumS=GetNodeNum(tstart);
if (NodeNumS==-1) {
//gi.dprintf("bad nodenum at start\n");
return 0; // ERROR
}
// Get NodeNum of destination vector
NodeNumD=GetNodeNum(tdest);
if (NodeNumD==-1)
{
// gi.dprintf("bad nondenum at end\n");
return 0; // ERROR
}
// Allocate OPEN/CLOSED list pointers..
OPEN=(node_t *)V_Malloc(sizeof(node_t), TAG_LEVEL);
// OPEN=(node_t *)malloc(sizeof(node_t));
OPEN->NextNode=NULL;
CLOSED=(node_t *)V_Malloc(sizeof(node_t), TAG_LEVEL);
//CLOSED=(node_t *)malloc(sizeof(node_t));
CLOSED->NextNode=NULL;
//================================================
// This is our very first NODE! Our start vector
//================================================
StartNode=(node_t *)V_Malloc(sizeof(node_t), TAG_LEVEL);
//StartNode=(node_t *)malloc(sizeof(node_t));
StartNode->nodenum=NodeNumS; // starting position nodenum
StartNode->g=g=0; // we haven't gone anywhere yet
StartNode->h=h=distance(start, destination);//fabs(vDiff(start,destination)); // calculate remaining distance (heuristic estimate) GHz - changed to fabs()
StartNode->f=g+h; // total cost from start to finish
for (c=0;c < NUMCHILDS;c++)
StartNode->Child[c]=NULL; // no children for search pattern yet
StartNode->NextNode=NULL;
StartNode->PrevNode=NULL;
//================================================
// next node in open list points to our starting node
OPEN->NextNode=BestNode=StartNode; // First node on OPEN list..
//GHz - need to free these nodes too!
//NodeList[NodeCount++] = OPEN;
// NodeList[NodeCount++] = CLOSED;
NodeCount+=2;
for (;;) {
tNode=BestNode; // Save last valid node
BestNode=(node_t *)NextBestNode(NodeNumS, NodeNumD); // Get next node from OPEN list
if (!BestNode) {
//gi.dprintf("ran out of nodes to search\n");
return 0;//GHz
// BestNode=tNode; // Last valid node..
// break;
}
if (BestNode->nodenum==NodeNumD) break;// we there yet?
ComputeSuccessors(BestNode,NodeNumD);} // Search from here..
//================================================
RemoveDuplicates(BestNode, CLOSED);//FIXME: move this up before the start==end crash check
// gi.dprintf("%d: processed %d nodes\n", level.framenum,NodeCount);
if (BestNode==StartNode) { // Start==End??
FreeStack(StartNode);//FIXME: may cause crash
//gi.dprintf("start==end\n");
return 0; }
//gi.dprintf("Start = %d End = %d\n", NodeNumS, NodeNumD);
// gi.dprintf("Printing tNode (in reverse):\n");
// PrintNodes(BestNode, true);
// gi.dprintf("Printing OPEN list:\n");
//PrintNodes(OPEN, false);
//gi.dprintf("Printing CLOSED list:\n");
// PrintNodes(CLOSED, false);
BestNode->NextNode=NULL; // Must tie this off!
// How many nodes we got?
tNode=BestNode;
i=0;
while (tNode) {
i++; // How many nodes?
tNode=tNode->PrevNode; }
if (i <= 2) { // Only nodes are Start and End??
FreeStack(BestNode);//FIXME: may cause crash
//gi.dprintf("only start and end nodes\n");
return 0; }
// Let's allocate our own stuff...
//CLOSED->NextNode = NULL;//GHz - only needs to be null if we are using freestack()
numpts=i;
//GHz - free old memory
//V_Free(Waypoint);
Waypoint=(int *)V_Malloc(numpts*sizeof(int), TAG_LEVEL);
//Waypoint=(int *)malloc(numpts*sizeof(int));
// Now, we have to assign the nodenum's along
// this path in reverse order because that is
// the way the A* algorithm finishes its search.
// The last best node it visited was the END!
// So, we copy them over in reverse.. No biggy..
tNode=BestNode;
while (BestNode) {
Waypoint[--i]=BestNode->nodenum;//GHz: how/when is this freed?
BestNode=BestNode->PrevNode; }
// NOTE: At this point, if our numpts returned is not
// zero, then a path has been found! To follow this
// path we simply follow node[Waypoint[i]].origin
// because Waypoint array is filled with indexes into
// our node[i] array of valid vectors in the map..
// We did it!! Now free the stack and exit..
//================================================
//++++++++++ GHz NOTES +++++++++++++
// FreeStack() is flawed because the lists have nodes that point to nodes on other lists
// so if you free one list, then the next list will crash when it encounters a node with
// an invalid pointer (node was freed in last list)
//++++++++++++++++++++++++++++++++++
FreeStack(tNode); // Release ALL resources!!
//GHz: cleanup test/debugging
//for (i=0;i<NodeCount;i++)
//{
// V_Free(NodeList[i]);
// }
// OPEN = NULL;
//CLOSED = NULL;
NodeCount = 0;
//TODO: performance... cpu usage is still very high
//TODO: grid editor, save grid to disk
//TODO: need some way of handling manually edited grid
// because NextNode() only searches within a specific 32x32 pattern
// gi.dprintf("%d: found %d\n",level.framenum,numpts);
return (numpts);
}
bool QPBO<REAL>::Save(char* filename, int format)
{
int e;
int edge_num = 0;
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e)) edge_num ++;
if (format == 0)
{
FILE* fp;
REAL E0, E1, E00, E01, E10, E11;
int i, j;
char* type_name;
char* type_format;
char FORMAT_LINE[64];
int factor = (stage == 0) ? 2 : 1;
get_type_information(type_name, type_format);
fp = fopen(filename, "w");
if (!fp) return false;
fprintf(fp, "nodes=%d\n", GetNodeNum());
fprintf(fp, "edges=%d\n", edge_num);
fprintf(fp, "labels=2\n");
fprintf(fp, "type=%s\n", type_name);
fprintf(fp, "\n");
sprintf(FORMAT_LINE, "n %%d\t%%%s %%%s\n", type_format, type_format);
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E0, E1);
REAL delta = (E0 < E1) ? E0 : E1;
fprintf(fp, FORMAT_LINE, i, (E0-delta)/factor, (E1-delta)/factor);
}
sprintf(FORMAT_LINE, "e %%d %%d\t%%%s %%%s %%%s %%%s\n", type_format, type_format, type_format, type_format);
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E00, E01, E10, E11);
fprintf(fp, FORMAT_LINE, i, j, E00/factor, E01/factor, E10/factor, E11/factor);
}
fclose(fp);
return true;
}
if (format == 1)
{
FILE* fp;
REAL E0, E1, E00, E01, E10, E11;
int i, j;
Arc* a;
char* type_name;
char* type_format;
if (stage == 0) Solve();
get_type_information(type_name, type_format);
if (type_format[0] != 'd') return false;
fp = fopen(filename, "w");
if (!fp) return false;
fprintf(fp, "p %d %d\n", GetNodeNum(), GetNodeNum() + edge_num);
for (i=0; i<GetNodeNum(); i++)
{
GetTwiceUnaryTerm(i, E0, E1);
REAL delta = E1 - E0;
for (a=nodes[0][i].first; a; a=a->next)
{
if (IsNode0(a->head)) delta += a->sister->r_cap + GetMate(a)->sister->r_cap;
else delta -= a->r_cap + GetMate(a)->r_cap;
}
fprintf(fp, "1 %d %d\n", i+1, delta);
}
for (e=GetNextEdgeId(-1); e>=0; e=GetNextEdgeId(e))
{
GetTwicePairwiseTerm(e, i, j, E00, E01, E10, E11);
fprintf(fp, "2 %d %d %d\n", i+1, j+1, E00 + E11 - E01 - E10);
}
fclose(fp);
return true;
}
return false;
}
