git.hungrycats.org Git - xscreensaver/blob - hacks/glx/glschool_alg.c

   1 /* school_alg.c, Copyright (c) 2005-2006 David C. Lambert <dcl@panix.com>
   2  *
   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
   9  * implied warranty.
  10  */
  11 #include <math.h>
  12 #include <stdio.h>
  13 #include <stdlib.h>
  14
  15 #ifdef HAVE_CONFIG_H
  16 # include "config.h"
  17 #endif
  18
  19 #include "yarandom.h"
  20 #include "glschool_alg.h"
  21
  22 /* for xscreensaver */
  23 #undef drand48
  24 #define drand48()       frand(1.0)
  25
  26 #define RAD2DEG         (180.0/3.1415926535)
  27
  28
  29 static inline double
  30 norm(double *dv)
  31 {
  32         return sqrt(dv[0]*dv[0] + dv[1]*dv[1] + dv[2]*dv[2]);
  33 }
  34
  35
  36 static inline void
  37 addVector(double *v, double *d)
  38 {
  39         v[0] += d[0];
  40         v[1] += d[1];
  41         v[2] += d[2];
  42 }
  43
  44
  45 static inline void
  46 clearVector(double *v)
  47 {
  48         v[0] = v[1] = v[2] = 0.0;
  49 }
  50
  51
  52 static inline void
  53 scaleVector(double *v, double s)
  54 {
  55         v[0] *= s;
  56         v[1] *= s;
  57         v[2] *= s;
  58 }
  59
  60
  61 static inline void
  62 addScaledVector(double *v, double *d, double s)
  63 {
  64         v[0] += d[0]*s;
  65         v[1] += d[1]*s;
  66         v[2] += d[2]*s;
  67 }
  68
  69
  70 static inline void
  71 getDifferenceVector(double *v0, double *v1, double *diff)
  72 {
  73         diff[0] = v0[0] - v1[0];
  74         diff[1] = v0[1] - v1[1];
  75         diff[2] = v0[2] - v1[2];
  76 }
  77
  78
  79 void
  80 initFish(Fish *f, double *mins, double *ranges)
  81 {
  82         int i;
  83
  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;
  90         }
  91 }
  92
  93
  94 void
  95 initFishes(School *s)
  96 {
  97         int             i;
  98         Fish    *f = (Fish *)0;
  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);
 104
 105         for(i = 0, f = theFishes; i < nFish; i++, f++)
 106                 initFish(f, mins, ranges);
 107 }
 108
 109
 110 void
 111 applyFishMovements(Fish *f, BBox *bbox, double minVel, double maxVel, double momentum)
 112 {
 113         int                     i;
 114         int                     oob = 0;
 115         double          vMag = 0.0;
 116
 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));
 122         }
 123         vMag = sqrt(vMag);
 124
 125         if (vMag > maxVel)
 126                 scaleVector(FISH_VEL(f), maxVel/vMag);
 127         else if (vMag < minVel)
 128                 scaleVector(FISH_VEL(f), minVel/vMag);
 129
 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);
 134
 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);
 139         }
 140 }
 141
 142
 143 void
 144 applyMovements(School *s)
 145 {
 146         int             i = 0;
 147         Fish    *f = (Fish *)0;
 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);
 154
 155         for(i = 0, f = theFishes; i < nFish; i++, f++)
 156                 applyFishMovements(f, bbox, minVel, maxVel, momentum);
 157 }
 158
 159
 160 School *
 161 initSchool(int nFish, double accLimit, double maxV, double minV, double distExp, double momentum,
 162                    double minRadius, double avoidFact, double matchFact, double centerFact, double targetFact,
 163                    double distComp)
 164 {
 165         School  *s = (School *)0;
 166
 167         if ((s = (School *)malloc(sizeof(School))) == (School *)0) {
 168                 perror("initSchool School allocation failed: ");
 169                 return s;
 170         }
 171
 172         if ((SCHOOL_FISHES(s) = (Fish *)malloc(sizeof(Fish)*nFish)) == (Fish *)0) {
 173                 perror("initSchool Fish array allocation failed: ");
 174                 free(s);
 175                 return (School *)0;
 176         }
 177
 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;
 191
 192         return s;
 193 }
 194
 195 void
 196 freeSchool(School *s)
 197 {
 198         free(SCHOOL_FISHES(s));
 199         free(s);
 200 }
 201
 202 void
 203 setBBox(School *s, double xMin, double xMax, double yMin, double yMax, double zMin, double zMax)
 204 {
 205         int             i;
 206         BBox    *bbox = &SCHOOL_BBOX(s);
 207
 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;
 211
 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);
 215         }
 216 }
 217
 218
 219 void
 220 newGoal(School *s)
 221 {
 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);
 225 }
 226
 227
 228 double
 229 computeNormalAndThetaToPlusZ(double *v, double *xV)
 230 {
 231         double  x1 = 0.0;
 232         double  y1 = 0.0;
 233         double  z1 = 1.0;
 234         double  x2 = v[0];
 235         double  y2 = v[1];
 236         double  z2 = v[2];
 237         double  theta = 0.0;
 238         double  xVNorm = 0.0;
 239         double  sinTheta = 0.0;
 240         double  v2Norm = norm(v);
 241
 242         if (v2Norm == 0.0) {
 243                 xV[1] = 1.0;
 244                 xV[0] = xV[2] = 0.0;
 245                 return theta;
 246         }
 247         xV[0] = (y1*z2 - z1*y2);
 248         xV[1] = -(x1*z2 - z1*x2);
 249         xV[2] = (x1*y2 - y1*x2);
 250         xVNorm = norm(xV);
 251
 252         sinTheta = xVNorm/v2Norm;
 253         return (asin(sinTheta) * RAD2DEG);
 254 }
 255
 256
 257 int
 258 computeGroupVectors(School *s, Fish *ref, double *avoidance, double *centroid, double *avgVel)
 259 {
 260         int             i;
 261         double  dist;
 262         double  adjDist;
 263         double  diffVect[3];
 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);
 271
 272         for(i = 0, test = fishes; i < nFish; i++, test++) {
 273                 if (test == ref) continue;
 274
 275                 getDifferenceVector(FISH_POS(ref), FISH_POS(test), diffVect);
 276
 277                 dist = norm(diffVect) - distComp;
 278                 if (dist < 0.0) dist = 0.1;
 279
 280                 adjDist = pow(dist, distExp);
 281                 if (adjDist > minRadiusExp) continue;
 282
 283                 neighborCount++;
 284
 285                 addVector(avgVel, FISH_VEL(test));
 286                 addVector(centroid, FISH_POS(test));
 287
 288                 addScaledVector(avoidance, diffVect, 1.0/adjDist);
 289         }
 290         if (neighborCount > 0) {
 291                 scaleVector(avgVel, 1.0/neighborCount);
 292                 scaleVector(centroid, 1.0/neighborCount);
 293         }
 294         return neighborCount;
 295 }
 296
 297
 298 void
 299 computeAccelerations(School *s)
 300 {
 301         int             i;
 302         int             j;
 303         int             neighborCount;
 304         double  dist;
 305         double  adjDist;
 306         double  accMag;
 307         double  avgVel[3];
 308         double  diffVect[3];
 309         double  centroid[3];
 310         double  avoidance[3];
 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);
 323
 324         for(i = 0, ref = fishes; i < nFish; i++, ref++) {
 325                 clearVector(avgVel);
 326                 clearVector(centroid);
 327                 clearVector(avoidance);
 328                 clearVector(FISH_ACC(ref));
 329                 neighborCount = computeGroupVectors(s, ref, avoidance, centroid, avgVel);
 330
 331                 /* avoidanceAccel[] = avoidance[] * AvoidFact */
 332                 scaleVector(avoidance, avoidFact);
 333                 addVector(FISH_ACC(ref), avoidance);
 334
 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);
 340                         }
 341
 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);
 346                         }
 347                 }
 348
 349                 accMag = norm(FISH_ACC(ref));
 350                 if (accMag < accLimit) {
 351                         getDifferenceVector(goal, FISH_POS(ref), diffVect);
 352
 353                         dist = norm(diffVect) - distComp;
 354                         if (dist < 0.0) dist = 0.1;
 355
 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);
 361                         }
 362                 }
 363         }
 364 }