1 /* school_alg.c, Copyright (c) 2005-2006 David C. Lambert <dcl@panix.com>
3 * Permission to use, copy, modify, distribute, and sell this software and its
4 * documentation for any purpose is hereby granted without fee, provided that
5 * the above copyright notice appear in all copies and that both that
6 * copyright notice and this permission notice appear in supporting
7 * documentation. No representations are made about the suitability of this
8 * software for any purpose. It is provided "as is" without express or
20 #include "glschool_alg.h"
22 /* for xscreensaver */
24 #define drand48() frand(1.0)
26 #define RAD2DEG (180.0/3.1415926535)
32 return sqrt(dv[0]*dv[0] + dv[1]*dv[1] + dv[2]*dv[2]);
37 addVector(double *v, double *d)
46 clearVector(double *v)
48 v[0] = v[1] = v[2] = 0.0;
53 scaleVector(double *v, double s)
62 addScaledVector(double *v, double *d, double s)
71 getDifferenceVector(double *v0, double *v1, double *diff)
73 diff[0] = v0[0] - v1[0];
74 diff[1] = v0[1] - v1[1];
75 diff[2] = v0[2] - v1[2];
80 glschool_initFish(Fish *f, double *mins, double *ranges)
84 for(i = 0; i < 3; i++) {
85 FISH_IPOS(f, i) = mins[i] + drand48()*ranges[i];
86 FISH_IACC(f, i) = 0.0;
87 FISH_IVEL(f, i) = drand48();
88 FISH_IMAGIC(f, i) = 0.70 + 0.60*drand48();
89 FISH_IOLDVEL(f, i) = 0.0;
95 glschool_initFishes(School *s)
99 int nFish = SCHOOL_NFISH(s);
100 BBox *bbox = &SCHOOL_BBOX(s);
101 double *mins = BBOX_MINS(bbox);
102 double *ranges = SCHOOL_BBRANGES(s);
103 Fish *theFishes = SCHOOL_FISHES(s);
105 for(i = 0, f = theFishes; i < nFish; i++, f++)
106 glschool_initFish(f, mins, ranges);
111 applyFishMovements(Fish *f, BBox *bbox, double minVel, double maxVel, double momentum)
117 for(i = 0; i < 3; i++) {
118 double pos = FISH_IPOS(f, i);
119 oob = (pos > BBOX_IMAX(bbox, i) || pos < BBOX_IMIN(bbox, i));
120 if (oob == 0) FISH_IVEL(f, i) += FISH_IACC(f, i) * FISH_IMAGIC(f, i);
121 vMag += (FISH_IVEL(f, i) * FISH_IVEL(f, i));
126 scaleVector(FISH_VEL(f), maxVel/vMag);
127 else if (vMag < minVel)
128 scaleVector(FISH_VEL(f), minVel/vMag);
130 for(i = 0; i < 3; i++) {
131 FISH_IVEL(f, i) = momentum * FISH_IOLDVEL(f, i) + (1.0-momentum) * FISH_IVEL(f, i);
132 FISH_IPOS(f, i) += FISH_IVEL(f, i);
133 FISH_IOLDVEL(f, i) = FISH_IVEL(f, i);
135 if (FISH_IPOS(f, i) < BBOX_IMIN(bbox, i))
136 FISH_IPOS(f, i) = BBOX_IMAX(bbox, i);
137 else if (FISH_IPOS(f, i) > BBOX_IMAX(bbox, i))
138 FISH_IPOS(f, i) = BBOX_IMIN(bbox, i);
144 glschool_applyMovements(School *s)
148 int nFish = SCHOOL_NFISH(s);
149 double minVel = SCHOOL_MINVEL(s);
150 double maxVel = SCHOOL_MAXVEL(s);
151 double momentum = SCHOOL_MOMENTUM(s);
152 BBox *bbox = &SCHOOL_BBOX(s);
153 Fish *theFishes = SCHOOL_FISHES(s);
155 for(i = 0, f = theFishes; i < nFish; i++, f++)
156 applyFishMovements(f, bbox, minVel, maxVel, momentum);
161 glschool_initSchool(int nFish, double accLimit, double maxV, double minV, double distExp, double momentum,
162 double minRadius, double avoidFact, double matchFact, double centerFact, double targetFact,
165 School *s = (School *)0;
167 if ((s = (School *)malloc(sizeof(School))) == (School *)0) {
168 perror("initSchool School allocation failed: ");
172 if ((SCHOOL_FISHES(s) = (Fish *)malloc(sizeof(Fish)*nFish)) == (Fish *)0) {
173 perror("initSchool Fish array allocation failed: ");
178 SCHOOL_NFISH(s) = nFish;
179 SCHOOL_ACCLIMIT(s) = accLimit;
180 SCHOOL_MAXVEL(s) = maxV;
181 SCHOOL_MINVEL(s) = minV;
182 SCHOOL_DISTEXP(s) = distExp;
183 SCHOOL_MOMENTUM(s) = momentum;
184 SCHOOL_MINRADIUS(s) = minRadius;
185 SCHOOL_MINRADIUSEXP(s) = pow(minRadius, distExp);
186 SCHOOL_MATCHFACT(s) = matchFact;
187 SCHOOL_AVOIDFACT(s) = avoidFact;
188 SCHOOL_CENTERFACT(s) = centerFact;
189 SCHOOL_TARGETFACT(s) = targetFact;
190 SCHOOL_DISTCOMP(s) = distComp;
196 glschool_freeSchool(School *s)
198 free(SCHOOL_FISHES(s));
203 glschool_setBBox(School *s, double xMin, double xMax, double yMin, double yMax, double zMin, double zMax)
206 BBox *bbox = &SCHOOL_BBOX(s);
208 BBOX_XMIN(bbox) = xMin; BBOX_XMAX(bbox) = xMax;
209 BBOX_YMIN(bbox) = yMin; BBOX_YMAX(bbox) = yMax;
210 BBOX_ZMIN(bbox) = zMin; BBOX_ZMAX(bbox) = zMax;
212 for(i = 0; i < 3; i++) {
213 SCHOOL_IMID(s, i) = BBOX_IMID(bbox, i);
214 SCHOOL_IRANGE(s, i) = BBOX_IRANGE(bbox, i);
220 glschool_newGoal(School *s)
222 SCHOOL_IGOAL(s,0) = 0.85*(drand48()-0.5)*SCHOOL_IRANGE(s,0) + SCHOOL_IMID(s,0);
223 SCHOOL_IGOAL(s,1) = 0.40*(drand48()-0.5)*SCHOOL_IRANGE(s,1) + SCHOOL_IMID(s,1);
224 SCHOOL_IGOAL(s,2) = 0.85*(drand48()-0.5)*SCHOOL_IRANGE(s,2) + SCHOOL_IMID(s,2);
229 glschool_computeNormalAndThetaToPlusZ(double *v, double *xV)
239 double sinTheta = 0.0;
240 double v2Norm = norm(v);
247 xV[0] = (y1*z2 - z1*y2);
248 xV[1] = -(x1*z2 - z1*x2);
249 xV[2] = (x1*y2 - y1*x2);
252 sinTheta = xVNorm/v2Norm;
253 return (asin(sinTheta) * RAD2DEG);
258 glschool_computeGroupVectors(School *s, Fish *ref, double *avoidance, double *centroid, double *avgVel)
264 int neighborCount = 0;
265 Fish *test = (Fish *)0;
266 int nFish = SCHOOL_NFISH(s);
267 double distExp = SCHOOL_DISTEXP(s);
268 double distComp = SCHOOL_DISTCOMP(s);
269 double minRadiusExp = SCHOOL_MINRADIUSEXP(s);
270 Fish *fishes = SCHOOL_FISHES(s);
272 for(i = 0, test = fishes; i < nFish; i++, test++) {
273 if (test == ref) continue;
275 getDifferenceVector(FISH_POS(ref), FISH_POS(test), diffVect);
277 dist = norm(diffVect) - distComp;
278 if (dist < 0.0) dist = 0.1;
280 adjDist = pow(dist, distExp);
281 if (adjDist > minRadiusExp) continue;
285 addVector(avgVel, FISH_VEL(test));
286 addVector(centroid, FISH_POS(test));
288 addScaledVector(avoidance, diffVect, 1.0/adjDist);
290 if (neighborCount > 0) {
291 scaleVector(avgVel, 1.0/neighborCount);
292 scaleVector(centroid, 1.0/neighborCount);
294 return neighborCount;
299 glschool_computeAccelerations(School *s)
311 Fish *ref = (Fish *)0;
312 int nFish = SCHOOL_NFISH(s);
313 double *goal = SCHOOL_GOAL(s);
314 double distExp = SCHOOL_DISTEXP(s);
315 double distComp = SCHOOL_DISTCOMP(s);
316 double avoidFact = SCHOOL_AVOIDFACT(s);
317 double matchFact = SCHOOL_MATCHFACT(s);
318 double centerFact = SCHOOL_CENTERFACT(s);
319 double targetFact = SCHOOL_TARGETFACT(s);
320 double accLimit = SCHOOL_ACCLIMIT(s);
321 double minRadius = SCHOOL_MINRADIUS(s);
322 Fish *fishes = SCHOOL_FISHES(s);
324 for(i = 0, ref = fishes; i < nFish; i++, ref++) {
326 clearVector(centroid);
327 clearVector(avoidance);
328 clearVector(FISH_ACC(ref));
329 neighborCount = glschool_computeGroupVectors(s, ref, avoidance, centroid, avgVel);
331 /* avoidanceAccel[] = avoidance[] * AvoidFact */
332 scaleVector(avoidance, avoidFact);
333 addVector(FISH_ACC(ref), avoidance);
335 accMag = norm(FISH_ACC(ref));
336 if (neighborCount > 0 && accMag < accLimit) {
337 for(j = 0; j < 3; j++) {
338 FISH_IAVGVEL(ref, j) = avgVel[j];
339 FISH_IACC(ref, j) += ((avgVel[j] - FISH_IVEL(ref, j)) * matchFact);
342 accMag = norm(FISH_ACC(ref));
343 if (accMag < accLimit) {
344 for(j = 0; j < 3; j++)
345 FISH_IACC(ref, j) += ((centroid[j] - FISH_IPOS(ref, j)) * centerFact);
349 accMag = norm(FISH_ACC(ref));
350 if (accMag < accLimit) {
351 getDifferenceVector(goal, FISH_POS(ref), diffVect);
353 dist = norm(diffVect) - distComp;
354 if (dist < 0.0) dist = 0.1;
356 /*adjDist = pow(dist, distExp);*/
357 if (dist > minRadius) {
358 adjDist = pow(dist, distExp);
359 for(j = 0; j < 3; j++)
360 FISH_IACC(ref, j) += (diffVect[j]*targetFact/adjDist);