//modified to take the returned velocity as an argument instead
void SeekPoint(t_jit_boids2d *flockPtr, long theBoid, double *seekPt, double* seekDir)
{
seekDir[x] = seekPt[x] - flockPtr->boid[theBoid].oldPos[x];
seekDir[y] = seekPt[y] - flockPtr->boid[theBoid].oldPos[y];
//seekDir[z] = seekPt[z] - flockPtr->boid[theBoid].oldPos[z];
NormalizeVelocity(seekDir);
}
Velocity BoidsManager::SeekPoint(short theBoid, Point2d seekPt)
{
Velocity tempDir;
tempDir.x = seekPt.x - m_boids[theBoid].oldPos.x;
tempDir.y = seekPt.y - m_boids[theBoid].oldPos.y;
NormalizeVelocity(&tempDir);
return(tempDir);
}
void BoidsManager::update() // FlightStep();
{
Velocity goCenterVel;
Velocity goAttractVel;
Velocity matchNeighborVel;
Velocity avoidWallsVel;
Velocity avoidNeighborVel;
float avoidNeighborSpeed;
const Velocity zeroVel = {0.0, 0.0};
short i;
m_centerPt = FindFlockCenter();
// save position and velocity
for (i = 0; i < m_numBoids; i++)
{
m_boids[i].oldPos.x = m_boids[i].newPos.x;
m_boids[i].oldPos.y = m_boids[i].newPos.y;
m_boids[i].oldDir.x = m_boids[i].newDir.x;
m_boids[i].oldDir.y = m_boids[i].newDir.y;
}
for (i = 0; i < m_numBoids; i++)
{
// get all velocity components
if (m_numNeighbors > 0)
{
avoidNeighborSpeed = MatchAndAvoidNeighbors(i,&matchNeighborVel, &avoidNeighborVel);
}
else
{
matchNeighborVel = zeroVel;
avoidNeighborVel = zeroVel;
avoidNeighborSpeed = 0;
}
goCenterVel = SeekPoint(i, m_centerPt);
goAttractVel = SeekPoint(i, m_attractPt);
avoidWallsVel = AvoidWalls(i);
// compute resultant velocity using weights and inertia
m_boids[i].newDir.x = m_inertiaFactor * (m_boids[i].oldDir.x) +
(m_centerWeight * goCenterVel.x +
m_attractWeight * goAttractVel.x +
m_matchWeight * matchNeighborVel.x +
m_avoidWeight * avoidNeighborVel.x +
m_wallsWeight * avoidWallsVel.x) / m_inertiaFactor;
m_boids[i].newDir.y = m_inertiaFactor * (m_boids[i].oldDir.y) +
(m_centerWeight * goCenterVel.y +
m_attractWeight * goAttractVel.y +
m_matchWeight * matchNeighborVel.y +
m_avoidWeight * avoidNeighborVel.y +
m_wallsWeight * avoidWallsVel.y) / m_inertiaFactor;
// normalize velocity so its length is unity
NormalizeVelocity(&(m_boids[i].newDir));
// set to avoidNeighborSpeed bounded by minSpeed and maxSpeed
if ((avoidNeighborSpeed >= m_minSpeed) &&
(avoidNeighborSpeed <= m_maxSpeed))
m_boids[i].speed = avoidNeighborSpeed;
else if (avoidNeighborSpeed > m_maxSpeed)
m_boids[i].speed = m_maxSpeed;
else
m_boids[i].speed = m_minSpeed;
// calculate new position, applying speedupFactor
m_boids[i].newPos.x += m_boids[i].newDir.x * m_boids[i].speed * m_speedupFactor;
m_boids[i].newPos.y += m_boids[i].newDir.y * m_boids[i].speed * m_speedupFactor;
}
}
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);
}
void FlightStep(t_jit_boids2d *flockPtr)
{
double goCenterVel[2];
double goAttractVel[2];
double matchNeighborVel[2];
double avoidWallsVel[2];
double avoidNeighborVel[2];
double avoidNeighborSpeed;
const double zeroVel[2] = {0.0, 0.0};
long i;
//now modifies flockPtr->centgerPt within the function instead of returning a value
FindFlockCenter(flockPtr);
for (i = 0; i < flockPtr->number; i++) { // save position and velocity
flockPtr->boid[i].oldPos[x] = flockPtr->boid[i].newPos[x];
flockPtr->boid[i].oldPos[y] = flockPtr->boid[i].newPos[y];
//flockPtr->boid[i].oldPos[z] = flockPtr->boid[i].newPos[z];
flockPtr->boid[i].oldDir[x] = flockPtr->boid[i].newDir[x];
flockPtr->boid[i].oldDir[y] = flockPtr->boid[i].newDir[y];
//flockPtr->boid[i].oldDir[z] = flockPtr->boid[i].newDir[z];
}
for (i = 0; i < flockPtr->number; i++) {
if (flockPtr->neighbors > 0) { // get all velocity components
avoidNeighborSpeed = MatchAndAvoidNeighbors(flockPtr, i, matchNeighborVel, avoidNeighborVel);
} else {
matchNeighborVel[x] = zeroVel[x];
matchNeighborVel[y] = zeroVel[y];
//matchNeighborVel[z] = zeroVel[z];
avoidNeighborVel[x] = zeroVel[x];
avoidNeighborVel[y] = zeroVel[y];
//avoidNeighborVel[z] = zeroVel[z];
avoidNeighborSpeed = 0;
}
//velocities passed in as argument
SeekPoint(flockPtr, i, flockPtr->centerPt, goCenterVel);
SeekPoint(flockPtr, i, flockPtr->attractpt, goAttractVel);
AvoidWalls(flockPtr, i, avoidWallsVel);
// compute resultant velocity using weights and inertia
flockPtr->boid[i].newDir[x] = flockPtr->inertia * (flockPtr->boid[i].oldDir[x]) +
(flockPtr->center * goCenterVel[x] +
flockPtr->attract * goAttractVel[x] +
flockPtr->match * matchNeighborVel[x] +
flockPtr->avoid * avoidNeighborVel[x] +
flockPtr->repel * avoidWallsVel[x]) / flockPtr->inertia;
flockPtr->boid[i].newDir[y] = flockPtr->inertia * (flockPtr->boid[i].oldDir[y]) +
(flockPtr->center * goCenterVel[y] +
flockPtr->attract * goAttractVel[y] +
flockPtr->match * matchNeighborVel[y] +
flockPtr->avoid * avoidNeighborVel[y] +
flockPtr->repel * avoidWallsVel[y]) / flockPtr->inertia;
/*flockPtr->boid[i].newDir[z] = flockPtr->inertia * (flockPtr->boid[i].oldDir[z]) +
(flockPtr->center * goCenterVel[z] +
flockPtr->attract * goAttractVel[z] +
flockPtr->match * matchNeighborVel[z] +
flockPtr->avoid * avoidNeighborVel[z] +
flockPtr->repel * avoidWallsVel[z]) / flockPtr->inertia;*/
NormalizeVelocity(flockPtr->boid[i].newDir); // normalize velocity so its length is unity
// set to avoidNeighborSpeed bounded by minspeed and maxspeed
if ((avoidNeighborSpeed >= flockPtr->minspeed) &&
(avoidNeighborSpeed <= flockPtr->maxspeed))
flockPtr->boid[i].speed = avoidNeighborSpeed;
else if (avoidNeighborSpeed > flockPtr->maxspeed)
flockPtr->boid[i].speed = flockPtr->maxspeed;
else
flockPtr->boid[i].speed = flockPtr->minspeed;
// calculate new position, applying speed
flockPtr->boid[i].newPos[x] += flockPtr->boid[i].newDir[x] * flockPtr->boid[i].speed * (flockPtr->speed / 100.0);
flockPtr->boid[i].newPos[y] += flockPtr->boid[i].newDir[y] * flockPtr->boid[i].speed * (flockPtr->speed / 100.0);
//flockPtr->boid[i].newPos[z] += flockPtr->boid[i].newDir[z] * flockPtr->boid[i].speed * (flockPtr->speed / 100.0);
}
}
