float BoidsManager::MatchAndAvoidNeighbors(short theBoid, Velocity *matchNeighborVel, Velocity *avoidNeighborVel)
{
short i, j, neighbor;
double distSqr;
double dist, distH, distV;
double tempSpeed;
short numClose = 0;
Velocity totalVel = {0.0,0.0};
/**********************/
/* Find the neighbors */
/**********************/
/* special case of one neighbor */
if (m_numNeighbors == 1) {
m_boids[theBoid].neighborDistSqr[0] = kMaxLong;
for (i = 0; i < m_numBoids; i++)
{
if (i != theBoid)
{
distSqr = DistSqrToPt(m_boids[theBoid].oldPos, m_boids[i].oldPos);
/* if this one is closer than the closest so far, then remember it */
if (m_boids[theBoid].neighborDistSqr[0] > distSqr)
{
m_boids[theBoid].neighborDistSqr[0] = distSqr;
m_boids[theBoid].neighbor[0] = i;
}
}
}
}
/* more than one neighbor */
else
{
for (j = 0; j < m_numNeighbors; j++)
m_boids[theBoid].neighborDistSqr[j] = kMaxLong;
for (i = 0 ; i < m_numBoids; i++)
{
/* if this one is not me... */
if (i != theBoid)
{
distSqr = DistSqrToPt(m_boids[theBoid].oldPos, m_boids[i].oldPos);
/* if distSqr is less than the distance at the bottom of the array, sort into array */
if (distSqr < m_boids[theBoid].neighborDistSqr[m_numNeighbors-1])
{
j = m_numNeighbors - 1;
/* sort distSqr in to keep array in size order, smallest first */
while ((distSqr < m_boids[theBoid].neighborDistSqr[j-1]) && (j > 0))
{
m_boids[theBoid].neighborDistSqr[j] = m_boids[theBoid].neighborDistSqr[j - 1];
m_boids[theBoid].neighbor[j] = m_boids[theBoid].neighbor[j - 1];
j--;
}
m_boids[theBoid].neighborDistSqr[j] = distSqr;
m_boids[theBoid].neighbor[j] = i;
}
}
}
}
/*********************************/
/* Match and avoid the neighbors */
/*********************************/
matchNeighborVel->x = 0;
matchNeighborVel->y = 0;
// set tempSpeed to old speed
tempSpeed = m_boids[theBoid].speed;
for (i = 0; i < m_numNeighbors; i++) {
neighbor = m_boids[theBoid].neighbor[i];
// calculate matchNeighborVel by averaging the neighbor velocities
matchNeighborVel->x += m_boids[neighbor].oldDir.x;
matchNeighborVel->y += m_boids[neighbor].oldDir.y;
// if distance is less than preferred distance, then neighbor influences boid
distSqr = m_boids[theBoid].neighborDistSqr[i];
if (distSqr < m_prefDistSqr)
{
dist = sqrt(distSqr);
distH = m_boids[neighbor].oldPos.x - m_boids[theBoid].oldPos.x;
distV = m_boids[neighbor].oldPos.y - m_boids[theBoid].oldPos.y;
if(dist == 0.0) dist = 0.0000001;
totalVel.x = totalVel.x - distH - (distH * ((float) m_prefDist / (dist)));
totalVel.y = totalVel.y - distV - (distV * ((float) m_prefDist / (dist)));
numClose++;
}
// adjust speed
if (InFront(&(m_boids[theBoid]), &(m_boids[neighbor])))
{
if (distSqr < m_prefDistSqr)
tempSpeed /= m_accelFactor;
//tempSpeed /= (m_accelFactor / 100.0);
else
tempSpeed *= m_accelFactor;
}
else
{
if (distSqr < m_prefDistSqr)
tempSpeed *= m_accelFactor;
else
tempSpeed /= m_accelFactor;
}
}
if (numClose)
{
avoidNeighborVel->x = totalVel.x / numClose;
avoidNeighborVel->y = totalVel.y / numClose;
NormalizeVelocity(matchNeighborVel);
}
else {
avoidNeighborVel->x = 0;
avoidNeighborVel->y = 0;
}
return(tempSpeed);
}
float MatchAndAvoidNeighbors(t_jit_boids2d *flockPtr, long theBoid, double *matchNeighborVel, double *avoidNeighborVel)
{
long i, j, neighbor;
double distSqr;
double dist, distH, distV,distD;
double tempSpeed;
short numClose = 0;
double totalVel[2] = {0.0,0.0};
/**********************/
/* Find the neighbors */
/**********************/
/* special case of one neighbor */
if (flockPtr->neighbors == 1) {
flockPtr->boid[theBoid].neighborDistSqr[0] = kMaxLong;
for (i = 0; i < flockPtr->number; i++) {
if (i != theBoid) {
distSqr = DistSqrToPt(flockPtr->boid[theBoid].oldPos, flockPtr->boid[i].oldPos);
/* if this one is closer than the closest so far, then remember it */
if (flockPtr->boid[theBoid].neighborDistSqr[0] > distSqr) {
flockPtr->boid[theBoid].neighborDistSqr[0] = distSqr;
flockPtr->boid[theBoid].neighbor[0] = i;
}
}
}
}
/* more than one neighbor */
else {
for (j = 0; j < flockPtr->neighbors; j++)
flockPtr->boid[theBoid].neighborDistSqr[j] = kMaxLong;
for (i = 0 ; i < flockPtr->number; i++) {
/* if this one is not me... */
if (i != theBoid) {
distSqr = DistSqrToPt(flockPtr->boid[theBoid].oldPos, flockPtr->boid[i].oldPos);
/* if distSqr is less than the distance at the bottom of the array, sort into array */
if (distSqr < flockPtr->boid[theBoid].neighborDistSqr[flockPtr->neighbors-1]) {
j = flockPtr->neighbors - 1;
/* sort distSqr in to keep array in size order, smallest first */
while ((distSqr < flockPtr->boid[theBoid].neighborDistSqr[j-1]) && (j > 0)) {
flockPtr->boid[theBoid].neighborDistSqr[j] = flockPtr->boid[theBoid].neighborDistSqr[j - 1];
flockPtr->boid[theBoid].neighbor[j] = flockPtr->boid[theBoid].neighbor[j - 1];
j--;
}
flockPtr->boid[theBoid].neighborDistSqr[j] = distSqr;
flockPtr->boid[theBoid].neighbor[j] = i;
}
}
}
}
/*********************************/
/* Match and avoid the neighbors */
/*********************************/
matchNeighborVel[x] = 0;
matchNeighborVel[y] = 0;
//matchNeighborVel[z] = 0;
// set tempSpeed to old speed
tempSpeed = flockPtr->boid[theBoid].speed;
for (i = 0; i < flockPtr->neighbors; i++) {
neighbor = flockPtr->boid[theBoid].neighbor[i];
// calculate matchNeighborVel by averaging the neighbor velocities
matchNeighborVel[x] += flockPtr->boid[neighbor].oldDir[x];
matchNeighborVel[y] += flockPtr->boid[neighbor].oldDir[y];
//matchNeighborVel[z] += flockPtr->boid[neighbor].oldDir[z];
// if distance is less than preferred distance, then neighbor influences boid
distSqr = flockPtr->boid[theBoid].neighborDistSqr[i];
if (distSqr < flockPtr->prefDistSqr) {
dist = jit_math_sqrt(distSqr);
distH = flockPtr->boid[neighbor].oldPos[x] - flockPtr->boid[theBoid].oldPos[x];
distV = flockPtr->boid[neighbor].oldPos[y] - flockPtr->boid[theBoid].oldPos[y];
//distD = flockPtr->boid[neighbor].oldPos[z] - flockPtr->boid[theBoid].oldPos[z];
if(dist == 0.0) dist = 0.0000001;
totalVel[x] = totalVel[x] - distH - (distH * ((float) flockPtr->prefdist / (dist)));
totalVel[y] = totalVel[y] - distV - (distV * ((float) flockPtr->prefdist / (dist)));
//totalVel[z] = totalVel[z] - distD - (distV * ((float) flockPtr->prefdist / (dist)));
numClose++;
}
if (InFront(&(flockPtr->boid[theBoid]), &(flockPtr->boid[neighbor]))) { // adjust speed
if (distSqr < flockPtr->prefDistSqr)
tempSpeed /= (flockPtr->accel / 100.0);
else
tempSpeed *= (flockPtr->accel / 100.0);
}
else {
if (distSqr < flockPtr->prefDistSqr)
tempSpeed *= (flockPtr->accel / 100.0);
else
tempSpeed /= (flockPtr->accel / 100.0);
}
}
if (numClose) {
avoidNeighborVel[x] = totalVel[x] / numClose;
avoidNeighborVel[y] = totalVel[y] / numClose;
//avoidNeighborVel[z] = totalVel[z] / numClose;
NormalizeVelocity(matchNeighborVel);
}
else {
avoidNeighborVel[x] = 0;
avoidNeighborVel[y] = 0;
//avoidNeighborVel[z] = 0;
}
return(tempSpeed);
}
