// Recursive
int fastExp(int b, int p) {
	if (p == 0) return 1;
	if (p == 1) return b;
	int ans = fastExp(b, p / 2); ans *= ans;
	if (p % 2 == 1) ans *= b;
	return ans;
}
/**
* Applies a force that repels nearby particles.
 * \todo Need to write this so the query particle need not be in the
 * same particles as its "neighbors" organized in ParticleLocality.
 */
void ParticleRepulsionContour::applyForce() {
		
	unsigned int i, j;
	double	rij2,	// distance squared between particle and one of its neighbors
		sig2,	// particle's repulsion radius squared
		Eij,	// Energy term between particle and one of its neighbors
		dDidr;	// Derivative of D[i] wrt radius
	gmVector3 rij;	// Vector between particles i and j.
		
	for (i = 0; i < ps->size(); ++i) {
		
		/* Find this particle's neighbors.
		*/
		
		Neighbors nb;
		nb.n1 = nb.n2 = i; 
		contours->getNeighbors(i,nb);
		unsigned int neighbors[2] = {nb.n1, nb.n2};
					
		rep_data->D[i] = 0.0;
		dDidr = 0.0;
		unsigned int *nbr;
		nbr = neighbors;
		// Apply the force based on all neighbors
		
		for (int k = 0; k < 2; k++) {
			j = *nbr;
			
			if (i == j) continue;	// Don't repel self
			
			rij = position->getPosition(i) - position->getPosition(j);
			rij2 = rij.lengthSquared();
			sig2 = rep_data->r[i] * rep_data->r[i];
			
			/* Compute energy contribution
				* This is the adaptive repulsion formula in WH 4.3
				*/
			Eij = rep_data->alpha*fastExp(-0.5*rij2/sig2);
			
			/* Scale Eij by distance so it goes to zero when |rij|
				* nears sdmul*r[i].
				* This keeps the particles stable when they are near
				* the boundary of influence of other particles.
				*/
			Eij *= 1.0 - rij2/(sig2*rep_data->sdmul*rep_data->sdmul);
			
			/* Add energy into particle i's velocity (first half of WH (9))
				*/
			velocity->v[i] += rij*Eij;
			
			/* Add energy into particle j's velocity (second half of (9))
				* Negate because rij is in the opposite direction.
				* Note that the subtraction of the energy terms should actually
				* be an addition in (9). See Heckbert's erratum.
				*/
						
			velocity->v[j] -= (rep_data->r[j]*rep_data->r[j]/sig2)*rij*Eij;
			
			
			/* D[i] is the sum of the total energy.
				* Also defined as the portion of particle i's
				* energy directly affected by a change in its
				* own repulsion radius. [WH]
				*/
			rep_data->D[i] += Eij;
			
			/* dDidr is the derivative of Di wrt the repulsion radius.
				* See WH (13).
				*/
			dDidr += rij2 * Eij / (sig2*rep_data->r[i]);
			nbr++;
		} // end cache loop
		
		/* DiDot is the change in Di over time, used
			* as a feedback to control Di. See (10) in WH.
			*/
		double Didot = -rho*(rep_data->D[i] - rep_data->Ehat);
		
		/* dr[i] is the change in the particle i's radius.
			* Called \dot{sigma}^i in WH, see (12).
			*/
		rep_data->dr[i] = Didot / (dDidr + beta);		
	}
}