diff options
Diffstat (limited to 'boids2d.c')
| -rw-r--r-- | boids2d.c | 962 |
1 files changed, 962 insertions, 0 deletions
diff --git a/boids2d.c b/boids2d.c new file mode 100644 index 0000000..520a3ae --- /dev/null +++ b/boids2d.c @@ -0,0 +1,962 @@ +/* + + boids2d 08/2005 a.sier / jasch adapted from boids by eric singer 1995-2003 eric l. singer + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#include "m_pd.h" +#include <stdlib.h> +#include <math.h> + +// constants +#define kAssistInlet 1 +#define kAssistOutlet 2 +#define kMaxLong 0xFFFFFFFF +#define kMaxNeighbors 4 + +// util +#define MAX(a,b) ((a)>(b)?(a):(b)) +#define CLIP(x,a,b) (x)=(x)<(a)?(a):(x)>(b)?(b):(x) + +// initial flight parameters +const short kNumBoids = 12; // number of boids +const short kNumNeighbors = 2; // must be <= kMaxNeighbors +const double kMinSpeed = 0.15; // boids' minimum speed +const double kMaxSpeed = 0.25; // boids' maximum speed +const double kCenterWeight = 0.25; // flock centering +const double kAttractWeight = 0.300;// attraction point seeking +const double kMatchWeight = 0.100;// neighbors velocity matching +const double kAvoidWeight = 0.10; // neighbors avoidance +const double kWallsWeight = 0.500;// wall avoidance [210] +const double kEdgeDist = 0.5; // vision distance to avoid wall edges [5] +const double kSpeedupFactor = 0.100;// alter animation speed +const double kInertiaFactor = 0.20; // willingness to change speed & direction +const double kAccelFactor = 0.100;// neighbor avoidance accelerate or decelerate rate +const double kPrefDist = 0.25; // preferred distance from neighbors +const double kFlyRectTop = 1.0; // fly rect boundaries +const double kFlyRectLeft = -1.0; +const double kFlyRectBottom = -1.0; +const double kFlyRectRight = 1.0; +// const double kFlyRectFront = 1.0; +// const double kFlyRectBack = -1.0; + + +// typedefs +typedef struct Velocity { + double x; + double y; +// double z; +} Velocity; + +typedef struct Point2d { + double x; + double y; +// double z; +} Point2d; + +typedef struct Box3D { + double left, right; + double top, bottom; +// double front, back; +} Box3D; + +typedef struct _Boid { + Point2d oldPos; + Point2d newPos; + Velocity oldDir; + Velocity newDir; + double speed; + short neighbor[kMaxNeighbors]; + double neighborDistSqr[kMaxNeighbors]; +} t_one_boid, *BoidPtr; + +typedef struct _FlockObject { + t_object ob; + void *out1, *out2; + short mode; + long numBoids; + long numNeighbors; + Box3D flyRect; + double minSpeed; + double maxSpeed; + double centerWeight; + double attractWeight; + double matchWeight; + double avoidWeight; + double wallsWeight; + double edgeDist; + double speedupFactor; + double inertiaFactor; + double accelFactor; + double prefDist; + double prefDistSqr; + Point2d centerPt; + Point2d attractPt; + BoidPtr boid; + double d2r, r2d; +} t_boids, *FlockPtr; + +// variables +// void *flock; +t_symbol *ps_nothing; + +// prototypes +void *boids2d_class; +void *Flock_new(t_symbol *s, long argc, t_atom *argv); +void Flock_free(t_boids *x); +void Flock_bang(t_boids *x); +void Flock_dump(t_boids *x); +void Flock_mode(t_boids *x, t_float arg); +void Flock_numNeighbors(t_boids *x, t_float arg); +void Flock_numBoids(t_boids *x, t_float arg); +void Flock_minSpeed(t_boids *x, t_float arg); +void Flock_maxSpeed(t_boids *x, t_float arg); +void Flock_centerWeight(t_boids *x, t_float arg); +void Flock_attractWeight(t_boids *x, t_float arg); +void Flock_matchWeight(t_boids *x, t_float arg); +void Flock_avoidWeight(t_boids *x, t_float arg); +void Flock_wallsWeight(t_boids *x, t_float arg); +void Flock_edgeDist(t_boids *x, t_float arg); +void Flock_speedupFactor(t_boids *x, t_float arg); +void Flock_inertiaFactor(t_boids *x, t_float arg); +void Flock_accelFactor(t_boids *x, t_float arg); +void Flock_prefDist(t_boids *x, t_float arg); +void Flock_flyRect(t_boids *x, t_symbol *msg, short argc, t_atom *argv); +void Flock_attractPt(t_boids *x, t_symbol *msg, short argc, t_atom *argv); +void Flock_reset(t_boids *x); +void Flock_resetBoids(t_boids *x); +void InitFlock(t_boids *x); +void FlightStep(t_boids *x); +Point2d FindFlockCenter(t_boids *x); +float MatchAndAvoidNeighbors(t_boids *x, short theBoid, Velocity *matchNeighborVel, Velocity *avoidNeighborVel); +Velocity SeekPoint(t_boids *x, short theBoid, Point2d seekPt); +Velocity AvoidWalls(t_boids *x, short theBoid); +int InFront(BoidPtr theBoid, BoidPtr neighbor); +void NormalizeVelocity(Velocity *direction); +double RandomInt(double minRange, double maxRange); +double DistSqrToPt(Point2d firstPoint, Point2d secondPoint); + +void boids2d_setup(void) +{ + boids2d_class = class_new(gensym("boids2d"), (t_newmethod)Flock_new, + (t_method)Flock_free, sizeof(t_boids), 0, A_GIMME, 0); + /* setup((t_messlist **) &flock, (method)Flock_new, (method)Flock_free, (short) sizeof(FlockObject), 0L, A_LONG, A_DEFLONG, 0); */ + class_addfloat(boids2d_class, (t_method) Flock_numBoids); + class_addbang(boids2d_class, (t_method) Flock_bang); + class_addmethod(boids2d_class, (t_method) Flock_numNeighbors, gensym("neighbors"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_numBoids, gensym("number"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_mode, gensym("mode"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_minSpeed, gensym("minspeed"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_maxSpeed, gensym("maxspeed"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_centerWeight, gensym("center"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_attractWeight, gensym("attract"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_matchWeight, gensym("match"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_avoidWeight, gensym("avoid"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_wallsWeight, gensym("repel"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_edgeDist, gensym("edgedist"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_speedupFactor, gensym("speed"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_inertiaFactor, gensym("inertia"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_accelFactor, gensym("accel"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_prefDist, gensym("prefdist"), A_FLOAT, 0); + class_addmethod(boids2d_class, (t_method) Flock_flyRect, gensym("flyrect"), A_GIMME, 0); + class_addmethod(boids2d_class, (t_method) Flock_attractPt, gensym("attractpt"), A_GIMME, 0); + class_addmethod(boids2d_class, (t_method) Flock_resetBoids, gensym("reset"), 0); + class_addmethod(boids2d_class, (t_method) Flock_reset, gensym("init"), 0); + class_addmethod(boids2d_class, (t_method) Flock_dump, gensym("dump"), 0); + + post("boids2d 2005-2006 a.sier / jasch 1995-2003 eric l. singer "__DATE__" "__TIME__); + ps_nothing = gensym(""); +} + + +void *Flock_new(t_symbol *s, long argc, t_atom *argv) +{ + t_boids *x = (t_boids *)pd_new(boids2d_class); + x->out1 = outlet_new(&x->ob, NULL); + x->out2 = outlet_new(&x->ob, NULL); + + x->numBoids = 16; + if((argc >= 1) && (argv[0].a_type == A_FLOAT)){ + x->numBoids = argv[0].a_w.w_float; + } + x->boid = (t_one_boid *)malloc(sizeof(t_one_boid) * x->numBoids); + + InitFlock(x); + + x->mode = 0; + if((argc >= 2) && (argv[1].a_type == A_FLOAT)){ + x->mode = (short)(CLIP(argv[1].a_w.w_float, 0, 2)); + } + + x->d2r = 3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068/180.0; + x->r2d = 180.0/3.141592653589793238462643383279502884197169399375105820974944592307816406286208998628034825342117068; + + return(x); +} + + +void Flock_free(t_boids *x) +{ + free(x->boid); +} + +void Flock_bang(t_boids *x) +{ + short i; + t_atom outlist[10]; + t_atom *out; + + double tempNew_x, tempNew_y; + double tempOld_x, tempOld_y; + double delta_x, delta_y; + double azi, speed; + // double tempspeed; + + out = outlist; + + FlightStep(x); + + + switch(x->mode) { // newpos + case 0: + for (i = 0; i < x->numBoids; i++){ + SETFLOAT(out+0, i); + SETFLOAT(out+1, x->boid[i].newPos.x); + SETFLOAT(out+2, x->boid[i].newPos.y); + // SETFLOAT(out+3, x->boid[i].newPos.z); + outlet_list(x->out1, 0L, 3, out); + } + break; + case 1: //newpos + oldpos + for (i = 0; i < x->numBoids; i++){ + SETFLOAT(out+0, i); + SETFLOAT(out+1, x->boid[i].newPos.x); + SETFLOAT(out+2, x->boid[i].newPos.y); + // SETFLOAT(out+3, x->boid[i].newPos.z); + SETFLOAT(out+3, x->boid[i].oldPos.x); + SETFLOAT(out+4, x->boid[i].oldPos.y); + // SETFLOAT(out+6, x->boid[i].oldPos.z); + outlet_list(x->out1, 0L, 5, out); + } + break; + case 2: + for (i = 0; i < x->numBoids; i++){ + tempNew_x = x->boid[i].newPos.x; + tempNew_y = x->boid[i].newPos.y; + // tempNew_z = x->boid[i].newPos.z; + tempOld_x = x->boid[i].oldPos.x; + tempOld_y = x->boid[i].oldPos.y; + // tempOld_z = x->boid[i].oldPos.z; + delta_x = tempNew_x - tempOld_x; + delta_y = tempNew_y - tempOld_y; + // delta_z = tempNew_z - tempOld_z; + azi = atan2(delta_y, delta_x) * x->r2d; + // ele = atan2(delta_y, delta_x) * x->r2d; + speed = sqrt(delta_x * delta_x + delta_y * delta_y);// + delta_z * delta_z); + SETFLOAT(out+0, i); + SETFLOAT(out+1, tempNew_x); + SETFLOAT(out+2, tempNew_y); + // SETFLOAT(out+3, tempNew_z); + SETFLOAT(out+3, tempOld_x); + SETFLOAT(out+4, tempOld_y); + // SETFLOAT(out+6, tempOld_z); + SETFLOAT(out+5, speed); + SETFLOAT(out+6, azi); + // SETFLOAT(out+9, ele); + outlet_list(x->out1, 0L, 7, out); + } + break; + } +} + +void Flock_dump(t_boids *x) +{ + t_atom outList[6]; + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->numNeighbors; + outlet_anything(x->out2, gensym("neighbors"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->minSpeed; + outlet_anything(x->out2, gensym("minspeed"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->maxSpeed; + outlet_anything(x->out2, gensym("maxspeed"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->centerWeight; + outlet_anything(x->out2, gensym("center"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->attractWeight; + outlet_anything(x->out2, gensym("attract"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->matchWeight; + outlet_anything(x->out2, gensym("match"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->avoidWeight; + outlet_anything(x->out2, gensym("avoid"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->wallsWeight; + outlet_anything(x->out2, gensym("repel"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->edgeDist; + outlet_anything(x->out2, gensym("edgedist"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->speedupFactor; + outlet_anything(x->out2, gensym("speed"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->inertiaFactor; + outlet_anything(x->out2, gensym("inertia"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->accelFactor; + outlet_anything(x->out2, gensym("accel"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->prefDist; + outlet_anything(x->out2, gensym("prefdist"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->flyRect.left; + outList[1].a_type = A_FLOAT; + outList[1].a_w.w_float = x->flyRect.top; + outList[2].a_type = A_FLOAT; + outList[2].a_w.w_float = x->flyRect.right; + outList[3].a_type = A_FLOAT; + outList[3].a_w.w_float = x->flyRect.bottom; + /*outList[4].a_type = A_FLOAT; + outList[4].a_w.w_float = x->flyRect.front; + outList[5].a_type = A_FLOAT; + outList[5].a_w.w_float = x->flyRect.back;*/ + outlet_anything(x->out2, gensym("flyrect"), 4, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->attractPt.x; + outList[1].a_type = A_FLOAT; + outList[1].a_w.w_float = x->attractPt.y; + /*outList[2].a_type = A_FLOAT; + outList[2].a_w.w_float = x->attractPt.z;*/ + outlet_anything(x->out2, gensym("attractpt"), 2, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->mode; + outlet_anything(x->out2, gensym("mode"), 1, outList); + + outList[0].a_type = A_FLOAT; + outList[0].a_w.w_float = x->numBoids; + outlet_anything(x->out2, gensym("number"), 1, outList); +} + + +void Flock_mode(t_boids *x, t_float arg) +{ + long m = (long)arg; + x->mode = CLIP(m, 0, 2); +} + +void Flock_numNeighbors(t_boids *x, t_float arg) +{ + x->numNeighbors = (long)arg; +} + +void Flock_numBoids(t_boids *x, t_float arg) +{ + x->boid = (t_one_boid *)realloc(x->boid, sizeof(t_one_boid) * (long)arg); + x->numBoids = (long)arg; + Flock_resetBoids(x); +} + +void Flock_minSpeed(t_boids *x, t_float arg) +{ + x->minSpeed = MAX(arg, 0.000001); +} + +void Flock_maxSpeed(t_boids *x, t_float arg) +{ + x->maxSpeed = (double)arg; +} + +void Flock_centerWeight(t_boids *x, t_float arg) +{ + x->centerWeight = (double)arg; +} + +void Flock_attractWeight(t_boids *x, t_float arg) +{ + x->attractWeight = (double)arg; +} + +void Flock_matchWeight(t_boids *x, t_float arg) +{ + x->matchWeight = (double)arg; +} + +void Flock_avoidWeight(t_boids *x, t_float arg) +{ + x->avoidWeight = (double)arg; +} + +void Flock_wallsWeight(t_boids *x, t_float arg) +{ + x->wallsWeight = (double)arg; +} + +void Flock_edgeDist(t_boids *x, t_float arg) +{ + x->edgeDist = (double)arg; +} + +void Flock_speedupFactor(t_boids *x, t_float arg) +{ + x->speedupFactor = (double)arg; +} + +void Flock_inertiaFactor(t_boids *x, t_float arg) +{ + if(arg == 0){ + x->inertiaFactor = 0.000001; + }else{ + x->inertiaFactor = (double)arg; + } +} + +void Flock_accelFactor(t_boids *x, t_float arg) +{ + x->accelFactor = (double)arg; +} + +void Flock_prefDist(t_boids *x, t_float arg) +{ + x->prefDist = (double)arg; +} + +void Flock_flyRect(t_boids *x, t_symbol *msg, short argc, t_atom *argv) +{ + double temp[4]; + short i; + if(argc == 4){ + for(i=0;i<4;i++) { + if(argv[i].a_type == A_FLOAT) { + temp[i] = (double)argv[i].a_w.w_float; + } + } + x->flyRect.left = temp[0]; + x->flyRect.top = temp[1]; + x->flyRect.right = temp[2]; + x->flyRect.bottom = temp[3]; + // x->flyRect.front = temp[4]; + // x->flyRect.back = temp[5]; + }else{ + error("boids2d: flyrect needs four values"); + } +} + +void Flock_attractPt(t_boids *x, t_symbol *msg, short argc, t_atom *argv) +{ + double temp[2]; + short i; + if(argc == 2){ + for(i=0;i<2;i++) { + if(argv[i].a_type == A_FLOAT) { + temp[i] = (double)argv[i].a_w.w_float; + } + } + x->attractPt.x = temp[0]; + x->attractPt.y = temp[1]; + // x->attractPt.z = temp[2]; + }else{ + error("boids2d: attractPt needs two values"); + } +} + +void Flock_reset(t_boids *x) +{ + InitFlock(x); +} + +void Flock_resetBoids(t_boids *x) +{ + long i, j; + double rndAngle; + + for (i = 0; i < x->numBoids; i++) { // init everything to 0.0 + x->boid[i].oldPos.x = 0.0; + x->boid[i].oldPos.y = 0.0; + // x->boid[i].oldPos.z = 0.0; + + x->boid[i].newPos.x = 0.0; + x->boid[i].newPos.y = 0.0; + // x->boid[i].newPos.z = 0.0; + + x->boid[i].oldDir.x = 0.0; + x->boid[i].oldDir.y = 0.0; + // x->boid[i].oldDir.z = 0.0; + + x->boid[i].newDir.x = 0.0; + x->boid[i].newDir.y = 0.0; + // x->boid[i].newDir.z = 0.0; + + x->boid[i].speed = 0.0; + + for(j=0; j<kMaxNeighbors;j++){ + x->boid[i].neighbor[j] = 0; + x->boid[i].neighborDistSqr[j] = 0.0; + } + } + for (i = 0; i < x->numBoids; i++) { // set the initial locations and velocities of the boids + x->boid[i].newPos.x = x->boid[i].oldPos.x = RandomInt(x->flyRect.right,x->flyRect.left); // set random location within flyRect + x->boid[i].newPos.y = x->boid[i].oldPos.y = RandomInt(x->flyRect.bottom, x->flyRect.top); + // x->boid[i].newPos.z = x->boid[i].oldPos.z = RandomInt(x->flyRect.back, x->flyRect.front); + rndAngle = RandomInt(0, 360) * x->d2r; // set velocity from random angle + x->boid[i].newDir.x = sin(rndAngle); + x->boid[i].newDir.y = cos(rndAngle); + // x->boid[i].newDir.z = (cos(rndAngle) + sin(rndAngle)) * 0.5; + x->boid[i].speed = (kMaxSpeed + kMinSpeed) * 0.5; + } +} + +void InitFlock(t_boids *x) +{ + x->numNeighbors = kNumNeighbors; + x->minSpeed = kMinSpeed; + x->maxSpeed = kMaxSpeed; + x->centerWeight = kCenterWeight; + x->attractWeight = kAttractWeight; + x->matchWeight = kMatchWeight; + x->avoidWeight = kAvoidWeight; + x->wallsWeight = kWallsWeight; + x->edgeDist = kEdgeDist; + x->speedupFactor = kSpeedupFactor; + x->inertiaFactor = kInertiaFactor; + x->accelFactor = kAccelFactor; + x->prefDist = kPrefDist; + x->prefDistSqr = kPrefDist * kPrefDist; + x->flyRect.top = kFlyRectTop; + x->flyRect.left = kFlyRectLeft; + x->flyRect.bottom = kFlyRectBottom; + x->flyRect.right = kFlyRectRight; + // x->flyRect.front = kFlyRectFront; + // x->flyRect.back = kFlyRectBack; + x->attractPt.x = (kFlyRectLeft + kFlyRectRight) * 0.5; + x->attractPt.y = (kFlyRectTop + kFlyRectBottom) * 0.5; + // x->attractPt.z = (kFlyRectFront + kFlyRectBack) * 0.5; + Flock_resetBoids(x); +} + +void FlightStep(t_boids *x) +{ + Velocity goCenterVel; + Velocity goAttractVel; + Velocity matchNeighborVel; + Velocity avoidWallsVel; + Velocity avoidNeighborVel; + float avoidNeighborSpeed; + const Velocity zeroVel = {0.0, 0.0};//, 0.0}; + short i; + + x->centerPt = FindFlockCenter(x); + for (i = 0; i < x->numBoids; i++) { // save position and velocity + x->boid[i].oldPos.x = x->boid[i].newPos.x; + x->boid[i].oldPos.y = x->boid[i].newPos.y; + // x->boid[i].oldPos.z = x->boid[i].newPos.z; + + x->boid[i].oldDir.x = x->boid[i].newDir.x; + x->boid[i].oldDir.y = x->boid[i].newDir.y; + // x->boid[i].oldDir.z = x->boid[i].newDir.z; + } + for (i = 0; i < x->numBoids; i++) { + if (x->numNeighbors > 0) { // get all velocity components + avoidNeighborSpeed = MatchAndAvoidNeighbors(x, i,&matchNeighborVel, &avoidNeighborVel); + } else { + matchNeighborVel = zeroVel; + avoidNeighborVel = zeroVel; + avoidNeighborSpeed = 0; + } + goCenterVel = SeekPoint(x, i, x->centerPt); + goAttractVel = SeekPoint(x, i, x->attractPt); + avoidWallsVel = AvoidWalls(x, i); + + // compute resultant velocity using weights and inertia + x->boid[i].newDir.x = x->inertiaFactor * (x->boid[i].oldDir.x) + + (x->centerWeight * goCenterVel.x + + x->attractWeight * goAttractVel.x + + x->matchWeight * matchNeighborVel.x + + x->avoidWeight * avoidNeighborVel.x + + x->wallsWeight * avoidWallsVel.x) / x->inertiaFactor; + x->boid[i].newDir.y = x->inertiaFactor * (x->boid[i].oldDir.y) + + (x->centerWeight * goCenterVel.y + + x->attractWeight * goAttractVel.y + + x->matchWeight * matchNeighborVel.y + + x->avoidWeight * avoidNeighborVel.y + + x->wallsWeight * avoidWallsVel.y) / x->inertiaFactor; + /*x->boid[i].newDir.z = x->inertiaFactor * (x->boid[i].oldDir.z) + + (x->centerWeight * goCenterVel.z + + x->attractWeight * goAttractVel.z + + x->matchWeight * matchNeighborVel.z + + x->avoidWeight * avoidNeighborVel.z + + x->wallsWeight * avoidWallsVel.z) / x->inertiaFactor;*/ + NormalizeVelocity(&(x->boid[i].newDir)); // normalize velocity so its length is unity + + // set to avoidNeighborSpeed bounded by minSpeed and maxSpeed + if ((avoidNeighborSpeed >= x->minSpeed) && + (avoidNeighborSpeed <= x->maxSpeed)) + x->boid[i].speed = avoidNeighborSpeed; + else if (avoidNeighborSpeed > x->maxSpeed) + x->boid[i].speed = x->maxSpeed; + else + x->boid[i].speed = x->minSpeed; + + // calculate new position, applying speedupFactor + x->boid[i].newPos.x += x->boid[i].newDir.x * x->boid[i].speed * (x->speedupFactor / 100.0); + x->boid[i].newPos.y += x->boid[i].newDir.y * x->boid[i].speed * (x->speedupFactor / 100.0); + // x->boid[i].newPos.z += x->boid[i].newDir.z * x->boid[i].speed * (x->speedupFactor / 100.0); + + } +} + +Point2d FindFlockCenter(t_boids *x) +{ + double totalH = 0, totalV = 0, totalD = 0; + Point2d centerPoint; + short i; + + for (i = 0 ; i < x->numBoids; i++) + { + totalH += x->boid[i].oldPos.x; + totalV += x->boid[i].oldPos.y; + // totalD += x->boid[i].oldPos.z; + } + centerPoint.x = (double)(totalH / x->numBoids); + centerPoint.y = (double)(totalV / x->numBoids); + // centerPoint.z = (double) (totalD / x->numBoids); + + return(centerPoint); +} + +float MatchAndAvoidNeighbors(t_boids *x, short theBoid, Velocity *matchNeighborVel, Velocity *avoidNeighborVel) +{ + short i, j, neighbor; + double distSqr; + double dist, distH, distV,distD; + double tempSpeed; + short numClose = 0; + Velocity totalVel = {0.0,0.0};//,0.0}; + + /**********************/ + /* Find the neighbors */ + /**********************/ + + /* special case of one neighbor */ + if (x->numNeighbors == 1) { + x->boid[theBoid].neighborDistSqr[0] = kMaxLong; + + for (i = 0; i < x->numBoids; i++) { + if (i != theBoid) { + distSqr = DistSqrToPt(x->boid[theBoid].oldPos, x->boid[i].oldPos); + + /* if this one is closer than the closest so far, then remember it */ + if (x->boid[theBoid].neighborDistSqr[0] > distSqr) { + x->boid[theBoid].neighborDistSqr[0] = distSqr; + x->boid[theBoid].neighbor[0] = i; + } + } + } + } + /* more than one neighbor */ + else { + for (j = 0; j < x->numNeighbors; j++) + x->boid[theBoid].neighborDistSqr[j] = kMaxLong; + + for (i = 0 ; i < x->numBoids; i++) { + /* if this one is not me... */ + if (i != theBoid) { + distSqr = DistSqrToPt(x->boid[theBoid].oldPos, x->boid[i].oldPos); + + /* if distSqr is less than the distance at the bottom of the array, sort into array */ + if (distSqr < x->boid[theBoid].neighborDistSqr[x->numNeighbors-1]) { + j = x->numNeighbors - 1; + + /* sort distSqr in to keep array in size order, smallest first */ + while ((distSqr < x->boid[theBoid].neighborDistSqr[j-1]) && (j > 0)) { + x->boid[theBoid].neighborDistSqr[j] = x->boid[theBoid].neighborDistSqr[j - 1]; + x->boid[theBoid].neighbor[j] = x->boid[theBoid].neighbor[j - 1]; + j--; + } + x->boid[theBoid].neighborDistSqr[j] = distSqr; + x->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 = x->boid[theBoid].speed; + + for (i = 0; i < x->numNeighbors; i++) { + neighbor = x->boid[theBoid].neighbor[i]; + + // calculate matchNeighborVel by averaging the neighbor velocities + matchNeighborVel->x += x->boid[neighbor].oldDir.x; + matchNeighborVel->y += x->boid[neighbor].oldDir.y; + // matchNeighborVel->z += x->boid[neighbor].oldDir.z; + + // if distance is less than preferred distance, then neighbor influences boid + distSqr = x->boid[theBoid].neighborDistSqr[i]; + if (distSqr < x->prefDistSqr) { + dist = sqrt(distSqr); + + distH = x->boid[neighbor].oldPos.x - x->boid[theBoid].oldPos.x; + distV = x->boid[neighbor].oldPos.y - x->boid[theBoid].oldPos.y; + // distD = x->boid[neighbor].oldPos.z - x->boid[theBoid].oldPos.z; + + if(dist == 0.0) dist = 0.0000001; + totalVel.x = totalVel.x - distH - (distH * ((float) x->prefDist / (dist))); + totalVel.y = totalVel.y - distV - (distV * ((float) x->prefDist / (dist))); + // totalVel.z = totalVel.z - distD - (distV * ((float) x->prefDist / (dist))); + + numClose++; + } + if (InFront(&(x->boid[theBoid]), &(x->boid[neighbor]))) { // adjust speed + if (distSqr < x->prefDistSqr) + tempSpeed /= (x->accelFactor / 100.0); + else + tempSpeed *= (x->accelFactor / 100.0); + } + else { + if (distSqr < x->prefDistSqr) + tempSpeed *= (x->accelFactor / 100.0); + else + tempSpeed /= (x->accelFactor / 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); +} + + +Velocity SeekPoint(t_boids *x, short theBoid, Point2d seekPt) +{ + Velocity tempDir; + tempDir.x = seekPt.x - x->boid[theBoid].oldPos.x; + tempDir.y = seekPt.y - x->boid[theBoid].oldPos.y; + // tempDir.z = seekPt.z - x->boid[theBoid].oldPos.z; + NormalizeVelocity(&tempDir); + return(tempDir); +} + + +Velocity AvoidWalls(t_boids *x, short theBoid) +{ + Point2d testPoint; + Velocity tempVel = {0.0, 0.0};//, 0.0}; + + /* calculate test point in front of the nose of the boid */ + /* distance depends on the boid's speed and the avoid edge constant */ + testPoint.x = x->boid[theBoid].oldPos.x + x->boid[theBoid].oldDir.x * x->boid[theBoid].speed * x->edgeDist; + testPoint.y = x->boid[theBoid].oldPos.y + x->boid[theBoid].oldDir.y * x->boid[theBoid].speed * x->edgeDist; + // testPoint.z = x->boid[theBoid].oldPos.z + x->boid[theBoid].oldDir.z * x->boid[theBoid].speed * x->edgeDist; + + /* if test point is out of the left (right) side of x->flyRect, */ + /* return a positive (negative) horizontal velocity component */ + if (testPoint.x < x->flyRect.left) + tempVel.x = fabs(x->boid[theBoid].oldDir.x); + else if (testPoint.x > x->flyRect.right) + tempVel.x = - fabs(x->boid[theBoid].oldDir.x); + + /* same with top and bottom */ + if (testPoint.y < x->flyRect.top) + tempVel.y = fabs(x->boid[theBoid].oldDir.y); + else if (testPoint.y > x->flyRect.bottom) + tempVel.y = - fabs(x->boid[theBoid].oldDir.y); + + /* same with front and back + if (testPoint.z < x->flyRect.front) + tempVel.z = fabs(x->boid[theBoid].oldDir.z); + else if (testPoint.z > x->flyRect.back) + tempVel.z = - fabs(x->boid[theBoid].oldDir.z); + */ + + return(tempVel); +} + + +int InFront(BoidPtr theBoid, BoidPtr neighbor) +{ + float grad, intercept; + int result; + +/* + +Find the gradient and y-intercept of a line passing through theBoid's oldPos +perpendicular to its direction of motion. Another boid is in front of theBoid +if it is to the right or left of this linedepending on whether theBoid is moving +right or left. However, if theBoid is travelling vertically then just compare +their vertical coordinates. + +*/ + // xy plane + + // if theBoid is not travelling vertically... + if (theBoid->oldDir.x != 0) { + // calculate gradient of a line _perpendicular_ to its direction (hence the minus) + grad = -theBoid->oldDir.y / theBoid->oldDir.x; + + // calculate where this line hits the y axis (from y = mx + c) + intercept = theBoid->oldPos.y - (grad * theBoid->oldPos.x); + + /* compare the horizontal position of the neighbor boid with */ + /* the point on the line that has its vertical coordinate */ + if (neighbor->oldPos.x >= ((neighbor->oldPos.y - intercept) / grad)) { + /* return true if the first boid's horizontal movement is +ve */ + result = (theBoid->oldDir.x > 0); + + if (result==0) return 0; + else goto next; + + } else { + /* return true if the first boid's horizontal movement is +ve */ + result = (theBoid->oldDir.x < 0); + if (result==0) return 0; + else goto next; + } + } + /* else theBoid is travelling vertically, so just compare vertical coordinates */ + else if (theBoid->oldDir.y > 0) { + result = (neighbor->oldPos.y > theBoid->oldPos.y); + if (result==0){ + return 0; + }else{ + goto next; + } + }else{ + result = (neighbor->oldPos.y < theBoid->oldPos.y); + if (result==0){ + return 0; + } else { + goto next; + } + } +next: +/* + // yz plane + + // if theBoid is not travelling vertically... + if (theBoid->oldDir.y != 0) { + // calculate gradient of a line _perpendicular_ to its direction (hence the minus) + grad = -theBoid->oldDir.z / theBoid->oldDir.y; + + // calculate where this line hits the y axis (from y = mx + c) + intercept = theBoid->oldPos.z - (grad * theBoid->oldPos.y); + + // compare the horizontal position of the neighbor boid with + // the point on the line that has its vertical coordinate + if (neighbor->oldPos.y >= ((neighbor->oldPos.z - intercept) / grad)) { + // return true if the first boid's horizontal movement is +ve + result = (theBoid->oldDir.y > 0); + if (result==0){ + return 0; + }else{ + goto next2; + } + } else { + // return true if the first boid's horizontal movement is +ve + result = (theBoid->oldDir.y < 0); + if (result==0){ + return 0; + }else{ + goto next2; + } + } + } + // else theBoid is travelling vertically, so just compare vertical coordinates + else if (theBoid->oldDir.z > 0) { + result = (neighbor->oldPos.z > theBoid->oldPos.z); + if (result==0){ + return 0; + }else{ + goto next2; + } + }else{ + result = (neighbor->oldPos.z < theBoid->oldPos.z); + if (result==0){ + return 0; + }else{ + goto next2; + } + } +next2: */ + return 1; +} + +void NormalizeVelocity(Velocity *direction) +{ + float my_hypot; + + my_hypot = sqrt(direction->x * direction->x + direction->y * direction->y);// + direction->z * direction->z ); + + if (my_hypot != 0.0) { + direction->x = direction->x / my_hypot; + direction->y = direction->y / my_hypot; + // direction->z = direction->z / my_hypot; + } +} + +double RandomInt(double minRange, double maxRange) +{ + unsigned short qdRdm; + double t, result; + + qdRdm = rand(); + t = (double)qdRdm / 65536.0; // now 0 <= t <= 1 + result = (t * (maxRange - minRange)) + minRange; + return(result); +} + +double DistSqrToPt(Point2d firstPoint, Point2d secondPoint) +{ + double a, b,c; + a = firstPoint.x - secondPoint.x; + b = firstPoint.y - secondPoint.y; + //c = firstPoint.z - secondPoint.z; + return(a * a + b * b); // + c * c); +}
\ No newline at end of file |