calcdest.cc

柯老师网站上找到的

	time_transition = 0.0;

	position.X = position.Y = position.Z = 0.0;
	destination.X = destination.Y = destination.Z = 0.0;
	direction.X = direction.Y = direction.Z = 0.0;
	speed = 0.0;

	neighbor = new Neighbor[NODES];
	if(neighbor == 0) {
		perror("new");
		exit(1);
	}

	LIST_INIT(&traj);

	for(i = 1; i < NODES; i++) {
		neighbor[i].index = i;
		neighbor[i].reachable = (index == i) ? 1 : 0;
		neighbor[i].time_transition = 0.0;
	}
}



void
Node::RandomPosition()
{
	position.X = uniform() * MAXX;
	position.Y = uniform() * MAXY;
	position.Z = 0.0;
}


void
Node::RandomDestination()
{
	destination.X = uniform() * MAXX;
	destination.Y = uniform() * MAXY;
	destination.Z = 0.0;
	assert(destination != position);
}

void
Node::RandomSpeed()
{
	speed = uniform() * MAXSPEED;

	assert(speed != 0.0);
}


void
Node::Update()
{
        struct setdest *setdest = traj.lh_first;

	position += (speed * (TIME - time_update)) * direction;

	if(TIME == time_arrival) {

	  if (NULL == setdest) 
	    {
	      destination = position;
	      direction.X = direction.Y = direction.Z = 0.0;
	      speed = 0.0;
	      time_arrival = MAXTIME + 1;
	    } 
	  else 
	    {
	      vector v;
	      destination.X = setdest->X;
	      destination.Y = setdest->Y;
	      speed = setdest->speed;
	      if (0.0 == speed)
		{ // it's a pause at the current location
		  if (LIST_NEXT(setdest,traj))
		    time_arrival = LIST_NEXT(setdest,traj)->time;
		  else
		    time_arrival = MAXTIME + 1;
		}
	      else 
		{ // we're moving somewhere, when do we get there?
		  v = destination - position;
		  direction = v / v.length();
		  time_arrival = TIME + v.length() / speed;
		}
	      LIST_REMOVE(setdest,traj);
	      free(setdest);
	    }
	}

	time_update = TIME;
	time_transition = 0.0;
}


void
Node::UpdateNeighbors()
{
	static Node *n2;
	static Neighbor *m1, *m2;
	static vector D, B, v1, v2;
	static double a, b, c, t1, t2, Q;
	static u_int32_t i, reachable;

	v1 = speed * direction;

	/*
	 *  Only need to go from INDEX --> N for each one since links
	 *  are symmetric.
	 */
	for(i = index+1; i < NODES; i++) {

		m1 = &neighbor[i];
		n2 = &NodeList[i];
		m2 = &n2->neighbor[index];

		assert(i == m1->index);
		assert(m1->index == n2->index);
		assert(index == m2->index);
                assert(m1->reachable == m2->reachable);

                reachable = m1->reachable;

		/* ==================================================
		   Determine Reachability
		   ================================================== */
		{	vector d = position - n2->position;

			if(d.length() < RANGE) {
#ifdef SANITY_CHECKS
				if(TIME > 0.0 && m1->reachable == 0)
					assert(RANGE - d.length() < ROUND_ERROR);
#endif
				m1->reachable = m2->reachable = 1;
			}
			// Boundary condition handled below.
			else {
#ifdef SANITY_CHECKS
				if(TIME > 0.0 && m1->reachable == 1)
					assert(d.length() - RANGE < ROUND_ERROR);
#endif
				m1->reachable = m2->reachable = 0;
			}
#ifdef DEBUG
                        fprintf(stdout, "# %.6f (%d, %d) %.2fm\n",
                                TIME, index, m1->index, d.length());
#endif
		}

		/* ==================================================
		   Determine Next Event Time
		   ================================================== */
		v2 = n2->speed * n2->direction;

		D = v2 - v1;
		B = n2->position - position;

		a = (D.X * D.X) + (D.Y * D.Y) + (D.Z * D.Z);
		b = 2 * ((D.X * B.X) + (D.Y * B.Y) + (D.Z * B.Z));
		c = (B.X * B.X) + (B.Y * B.Y) + (B.Z * B.Z) - (RANGE * RANGE);

		if(a == 0.0) {
			/*
			 *  No Finite Solution
			 */
			m1->time_transition= 0.0;
			m2->time_transition= 0.0;
			goto  next;
		}

		Q = b * b - 4 * a * c;
		if(Q < 0.0) {
			/*
			 *  No real roots.
			 */
			m1->time_transition = 0.0;
			m2->time_transition = 0.0;
			goto next;
		}
		Q = sqrt(Q);

		t1 = (-b + Q) / (2 * a);
		t2 = (-b - Q) / (2 * a);

		// Stupid Rounding/Boundary Cases
		if(t1 > 0.0 && t1 < ROUND_ERROR) t1 = 0.0;
		if(t1 < 0.0 && -t1 < ROUND_ERROR) t1 = 0.0;
		if(t2 > 0.0 && t2 < ROUND_ERROR) t2 = 0.0;
		if(t2 < 0.0 && -t2 < ROUND_ERROR) t2 = 0.0;

		if(t1 < 0.0 && t2 < 0.0) {
			/*
			 *  No "future" time solution.
			 */
			m1->time_transition = 0.0;
			m2->time_transition = 0.0;
			goto next;
		}

		/*
		 * Boundary conditions.
		 */
		if((t1 == 0.0 && t2 > 0.0) || (t2 == 0.0 && t1 > 0.0)) {
			m1->reachable = m2->reachable = 1;
			m1->time_transition = m2->time_transition = TIME + max(t1, t2);
		}
		else if((t1 == 0.0 && t2 < 0.0) || (t2 == 0.0 && t1 < 0.0)) {
			m1->reachable = m2->reachable = 0;
			m1->time_transition = m2->time_transition = 0.0;
		}

		/*
		 * Non-boundary conditions.
		 */
		else if(t1 > 0.0 && t2 > 0.0) {
			m1->time_transition = TIME + min(t1, t2);
			m2->time_transition = TIME + min(t1, t2);
		}
		else if(t1 > 0.0) {
			m1->time_transition = TIME + t1;
			m2->time_transition = TIME + t1;
		}
		else {
			m1->time_transition = TIME + t2;
			m2->time_transition = TIME + t2;
		}

		/* ==================================================
		   Update the transition times for both NODEs.
		   ================================================== */
		if(time_transition == 0.0 || (m1->time_transition &&
		   time_transition > m1->time_transition)) {
			time_transition = m1->time_transition;
		}
		if(n2->time_transition == 0.0 || (m2->time_transition &&
		   n2->time_transition > m2->time_transition)) {
			n2->time_transition = m2->time_transition;
		}
        next:
                if(reachable != m1->reachable && TIME > 0.0) {
                        LinkChangeCount++;
                        link_changes++;
                        n2->link_changes++;
                }
	}
}

void
Node::Dump()
{
	Neighbor *m;
	u_int32_t i;

	fprintf(stdout,
		"Node: %d\tpos: (%.2f, %.2f, %.2f) dst: (%.2f, %.2f, %.2f)\n",
		index, position.X, position.Y, position.Z,
		destination.X, destination.Y, destination.Z);
	fprintf(stdout, "\tdir: (%.2f, %.2f, %.2f) speed: %.2f\n",
		direction.X, direction.Y, direction.Z, speed);
	fprintf(stdout, "\tArrival: %.2f, Update: %.2f, Transition: %.2f\n",
		time_arrival, time_update, time_transition);

	for(i = 1; i < NODES; i++) {
		m = &neighbor[i];
		fprintf(stdout, "\tNeighbor: %d (%x), Reachable: %d, Transition Time: %.2f\n",
			m->index, (int) m, m->reachable, m->time_transition);
	}
}


/* ======================================================================
   Dijkstra's Shortest Path Algoritm
   ====================================================================== */
void dumpall()
{
	u_int32_t i;

	fprintf(stdout, "\nTime: %.2f\n", TIME);

	for(i = 1; i < NODES; i++) {
		NodeList[i].Dump();
	}
}

void
ComputeW()
{
	u_int32_t i, j;
	u_int32_t *W = D2;

	memset(W, '\xff', sizeof(int) * NODES * NODES);

	for(i = 1; i < NODES; i++) {
		for(j = i; j < NODES; j++) {
			Neighbor *m = &NodeList[i].neighbor[j];
			if(i == j)
				W[i*NODES + j] = W[j*NODES + i] = 0;
			else
				W[i*NODES + j] = W[j*NODES + i] = m->reachable ? 1 : INFINITY;	
		}
	}
}

void
floyd_warshall()
{
	u_int32_t i, j, k;

	ComputeW();	// the connectivity matrix

	for(i = 1; i < NODES; i++) {
		for(j = 1; j < NODES; j++) {
			for(k = 1; k < NODES; k++) {
				D2[j*NODES + k] = min(D2[j*NODES + k], D2[j*NODES + i] + D2[i*NODES + k]);
			}
		}
	}

#ifdef SANITY_CHECKS
	for(i = 1; i < NODES; i++)
		for(j = 1; j < NODES; j++) {
			assert(D2[i*NODES + j] == D2[j*NODES + i]);
			assert(D2[i*NODES + j] <= INFINITY);
		}
#endif
}


/*
 *  Write the actual GOD entries to a TCL script.
 */
void
show_diffs()
{
	u_int32_t i, j;

	for(i = 1; i < NODES; i++) {
		for(j = i + 1; j < NODES; j++) {
			if(D1[i*NODES + j] != D2[i*NODES + j]) {

				if(D2[i*NODES + j] == INFINITY)
					DestUnreachableCount++;

                                if(TIME > 0.0) {
                                        RouteChangeCount++;
                                        NodeList[i].route_changes++;
                                        NodeList[j].route_changes++;
                                }

				if(TIME == 0.0) {
					fprintf(out_file, GOD_FORMAT2,
						i, j, D2[i*NODES + j]);
#ifdef SHOW_SYMMETRIC_PAIRS
					fprintf(out_file, GOD_FORMAT2,
						j, i, D2[j*NODES + i]);
#endif
				}
				else {
					fprintf(out_file, GOD_FORMAT, 
						TIME, i, j, D2[i*NODES + j]);
#ifdef SHOW_SYMMETRIC_PAIRS
					fprintf(out_file, GOD_FORMAT, 
						TIME, j, i, D2[j*NODES + i]);
#endif
				}
			}
		}
	}

	memcpy(D1, D2, sizeof(int) * NODES * NODES);
}


void
show_routes()
{
	u_int32_t i, j;

	fprintf(stdout, "#\n# TIME: %.12f\n#\n", TIME);
	for(i = 1; i < NODES; i++) {
		fprintf(stdout, "# %2d) ", i);
		for(j = 1; j < NODES; j++)
			fprintf(stdout, "%3d ", D2[i*NODES + j] & 0xff);
		fprintf(stdout, "\n");
	}
	fprintf(stdout, "#\n");
}

void
show_counters()
{
	u_int32_t i;

	fprintf(out_file, "#\n# Destination Unreachables: %d\n#\n",
		DestUnreachableCount);
	fprintf(out_file, "# Route Changes: %d\n#\n", RouteChangeCount);
        fprintf(out_file, "# Link Changes: %d\n#\n", LinkChangeCount);
        fprintf(out_file, "# Node | Route Changes | Link Changes\n");
	for(i = 1; i < NODES; i++)
		fprintf(out_file, "# %4d |          %4d |         %4d\n",
                        i, NodeList[i].route_changes,
                        NodeList[i].link_changes);
	fprintf(out_file, "#\n");
}