path: root/boids2d/boids2d.c

/*

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