//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); } }