1 files changed, 364 insertions, 0 deletions
diff --git a/hacks/glx/glschool_alg.c b/hacks/glx/glschool_alg.c
new file mode 100644
index 0000000..32b4a5f
--- /dev/null
+++ b/hacks/glx/glschool_alg.c
@@ -0,0 +1,364 @@
+/* school_alg.c, Copyright (c) 2005-2006 David C. Lambert <dcl@panix.com>
+ *
+ * Permission to use, copy, modify, distribute, and sell this software and its
+ * documentation for any purpose is hereby granted without fee, provided that
+ * the above copyright notice appear in all copies and that both that
+ * copyright notice and this permission notice appear in supporting
+ * documentation.  No representations are made about the suitability of this
+ * software for any purpose.  It is provided "as is" without express or 
+ * implied warranty.
+ */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#ifdef HAVE_CONFIG_H
+# include "config.h"
+#endif
+
+#include "yarandom.h"
+#include "glschool_alg.h"
+
+/* for xscreensaver */
+#undef drand48
+#define drand48()	frand(1.0)
+
+#define RAD2DEG		(180.0/3.1415926535)
+
+
+static inline double
+norm(double *dv)
+{
+	return sqrt(dv[0]*dv[0] + dv[1]*dv[1] + dv[2]*dv[2]);
+}
+
+
+static inline void
+addVector(double *v, double *d)
+{
+	v[0] += d[0];
+	v[1] += d[1];
+	v[2] += d[2];
+}
+
+
+static inline void
+clearVector(double *v)
+{
+	v[0] = v[1] = v[2] = 0.0;
+}
+
+
+static inline void
+scaleVector(double *v, double s)
+{
+	v[0] *= s;
+	v[1] *= s;
+	v[2] *= s;
+}
+
+
+static inline void
+addScaledVector(double *v, double *d, double s)
+{
+	v[0] += d[0]*s;
+	v[1] += d[1]*s;
+	v[2] += d[2]*s;
+}
+
+
+static inline void
+getDifferenceVector(double *v0, double *v1, double *diff)
+{
+	diff[0] = v0[0] - v1[0];
+	diff[1] = v0[1] - v1[1];
+	diff[2] = v0[2] - v1[2];
+}
+
+
+void
+glschool_initFish(Fish *f, double *mins, double *ranges)
+{
+	int i;
+
+	for(i = 0; i < 3; i++) {
+		FISH_IPOS(f, i) = mins[i] + drand48()*ranges[i];
+		FISH_IACC(f, i) = 0.0;
+		FISH_IVEL(f, i) = drand48();
+		FISH_IMAGIC(f, i) = 0.70 + 0.60*drand48();
+		FISH_IOLDVEL(f, i) = 0.0;
+	}
+}
+
+
+void
+glschool_initFishes(School *s)
+{
+	int		i;
+	Fish	*f = (Fish *)0;
+	int		nFish = SCHOOL_NFISH(s);
+	BBox	*bbox = &SCHOOL_BBOX(s);
+	double	*mins = BBOX_MINS(bbox);
+	double	*ranges = SCHOOL_BBRANGES(s);
+	Fish	*theFishes = SCHOOL_FISHES(s);
+
+	for(i = 0, f = theFishes; i < nFish; i++, f++)
+		glschool_initFish(f, mins, ranges);
+}
+
+
+static void
+applyFishMovements(Fish *f, BBox *bbox, double minVel, double maxVel, double momentum)
+{
+	int			i;
+	int			oob = 0;
+	double		vMag = 0.0;
+
+	for(i = 0; i < 3; i++) {
+		double	pos = FISH_IPOS(f, i);
+		oob = (pos > BBOX_IMAX(bbox, i) || pos < BBOX_IMIN(bbox, i));
+		if (oob == 0) FISH_IVEL(f, i) += FISH_IACC(f, i) * FISH_IMAGIC(f, i);
+		vMag += (FISH_IVEL(f, i) * FISH_IVEL(f, i));
+	}
+	vMag = sqrt(vMag);
+
+	if (vMag > maxVel)
+		scaleVector(FISH_VEL(f), maxVel/vMag);
+	else if (vMag < minVel)
+		scaleVector(FISH_VEL(f), minVel/vMag);
+
+	for(i = 0; i < 3; i++) {
+		FISH_IVEL(f, i) = momentum * FISH_IOLDVEL(f, i) + (1.0-momentum) * FISH_IVEL(f, i);
+		FISH_IPOS(f, i) += FISH_IVEL(f, i);
+		FISH_IOLDVEL(f, i) = FISH_IVEL(f, i);
+
+		if (FISH_IPOS(f, i) < BBOX_IMIN(bbox, i))
+			FISH_IPOS(f, i) = BBOX_IMAX(bbox, i);
+		else if (FISH_IPOS(f, i) > BBOX_IMAX(bbox, i))
+			FISH_IPOS(f, i) = BBOX_IMIN(bbox, i);
+	}
+}
+
+
+void
+glschool_applyMovements(School *s)
+{
+	int		i = 0;
+	Fish	*f = (Fish *)0;
+	int		nFish = SCHOOL_NFISH(s);
+	double	minVel = SCHOOL_MINVEL(s);
+	double	maxVel = SCHOOL_MAXVEL(s);
+	double	momentum = SCHOOL_MOMENTUM(s);
+	BBox	*bbox = &SCHOOL_BBOX(s);
+	Fish	*theFishes = SCHOOL_FISHES(s);
+
+	for(i = 0, f = theFishes; i < nFish; i++, f++)
+		applyFishMovements(f, bbox, minVel, maxVel, momentum);
+}
+
+
+School *
+glschool_initSchool(int nFish, double accLimit, double maxV, double minV, double distExp, double momentum,
+		   double minRadius, double avoidFact, double matchFact, double centerFact, double targetFact,
+		   double distComp)
+{
+	School	*s = (School *)0;
+
+	if ((s = (School *)malloc(sizeof(School))) == (School *)0) {
+		perror("initSchool School allocation failed: ");
+		return s;
+	}
+
+	if ((SCHOOL_FISHES(s) = (Fish *)malloc(sizeof(Fish)*nFish)) == (Fish *)0) {
+		perror("initSchool Fish array allocation failed: ");
+		free(s);
+		return (School *)0;
+	}
+
+	SCHOOL_NFISH(s) = nFish;
+	SCHOOL_ACCLIMIT(s) = accLimit;
+	SCHOOL_MAXVEL(s) = maxV;
+	SCHOOL_MINVEL(s) = minV;
+	SCHOOL_DISTEXP(s) = distExp;
+	SCHOOL_MOMENTUM(s) = momentum;
+	SCHOOL_MINRADIUS(s) = minRadius;
+	SCHOOL_MINRADIUSEXP(s) = pow(minRadius, distExp);
+	SCHOOL_MATCHFACT(s) = matchFact;
+	SCHOOL_AVOIDFACT(s) = avoidFact;
+	SCHOOL_CENTERFACT(s) = centerFact;
+	SCHOOL_TARGETFACT(s) = targetFact;
+	SCHOOL_DISTCOMP(s) = distComp;
+
+	return s;
+}
+
+void
+glschool_freeSchool(School *s)
+{
+	free(SCHOOL_FISHES(s));
+	free(s);
+}
+
+void
+glschool_setBBox(School *s, double xMin, double xMax, double yMin, double yMax, double zMin, double zMax)
+{
+	int		i;
+	BBox	*bbox = &SCHOOL_BBOX(s);
+
+	BBOX_XMIN(bbox) = xMin; BBOX_XMAX(bbox) = xMax;
+	BBOX_YMIN(bbox) = yMin; BBOX_YMAX(bbox) = yMax;
+	BBOX_ZMIN(bbox) = zMin; BBOX_ZMAX(bbox) = zMax;
+
+	for(i = 0; i < 3; i++) {
+		SCHOOL_IMID(s, i) = BBOX_IMID(bbox, i);
+		SCHOOL_IRANGE(s, i) = BBOX_IRANGE(bbox, i);
+	}
+}
+
+
+void
+glschool_newGoal(School *s)
+{
+	SCHOOL_IGOAL(s,0) = 0.85*(drand48()-0.5)*SCHOOL_IRANGE(s,0) + SCHOOL_IMID(s,0);
+	SCHOOL_IGOAL(s,1) = 0.40*(drand48()-0.5)*SCHOOL_IRANGE(s,1) + SCHOOL_IMID(s,1);
+	SCHOOL_IGOAL(s,2) = 0.85*(drand48()-0.5)*SCHOOL_IRANGE(s,2) + SCHOOL_IMID(s,2);
+}
+
+
+double
+glschool_computeNormalAndThetaToPlusZ(double *v, double *xV)
+{
+	double	x1 = 0.0;
+	double	y1 = 0.0;
+	double	z1 = 1.0;
+	double	x2 = v[0];
+	double	y2 = v[1];
+	double	z2 = v[2];
+	double	theta = 0.0;
+	double	xVNorm = 0.0;
+	double	sinTheta = 0.0;
+	double	v2Norm = norm(v);
+
+	if (v2Norm == 0.0) {
+		xV[1] = 1.0;
+		xV[0] = xV[2] = 0.0;
+		return theta;
+	}
+	xV[0] = (y1*z2 - z1*y2);
+	xV[1] = -(x1*z2 - z1*x2);
+	xV[2] = (x1*y2 - y1*x2);
+	xVNorm = norm(xV);
+
+	sinTheta = xVNorm/v2Norm;
+	return (asin(sinTheta) * RAD2DEG);
+}
+
+
+int
+glschool_computeGroupVectors(School *s, Fish *ref, double *avoidance, double *centroid, double *avgVel)
+{
+	int		i;
+	double	dist;
+	double	adjDist;
+	double	diffVect[3];
+	int		neighborCount = 0;
+	Fish	*test = (Fish *)0;
+	int		nFish = SCHOOL_NFISH(s);
+	double	distExp = SCHOOL_DISTEXP(s);
+	double	distComp = SCHOOL_DISTCOMP(s);
+	double	minRadiusExp = SCHOOL_MINRADIUSEXP(s);
+	Fish	*fishes = SCHOOL_FISHES(s);
+
+	for(i = 0, test = fishes; i < nFish; i++, test++) {
+		if (test == ref) continue;
+
+		getDifferenceVector(FISH_POS(ref), FISH_POS(test), diffVect);
+
+		dist = norm(diffVect) - distComp;
+		if (dist < 0.0) dist = 0.1;
+
+		adjDist = pow(dist, distExp);
+		if (adjDist > minRadiusExp) continue;
+
+		neighborCount++;
+
+		addVector(avgVel, FISH_VEL(test));
+		addVector(centroid, FISH_POS(test));
+
+		addScaledVector(avoidance, diffVect, 1.0/adjDist);
+	}
+	if (neighborCount > 0) {
+		scaleVector(avgVel, 1.0/neighborCount);
+		scaleVector(centroid, 1.0/neighborCount);
+	}
+	return neighborCount;
+}
+
+
+void
+glschool_computeAccelerations(School *s)
+{
+	int		i;
+	int		j;
+	int		neighborCount;
+	double	dist;
+	double	adjDist;
+	double	accMag;
+	double	avgVel[3];
+	double	diffVect[3];
+	double	centroid[3];
+	double	avoidance[3];
+	Fish	*ref = (Fish *)0;
+	int		nFish = SCHOOL_NFISH(s);
+	double	*goal = SCHOOL_GOAL(s);
+	double	distExp = SCHOOL_DISTEXP(s);
+	double	distComp = SCHOOL_DISTCOMP(s);
+	double	avoidFact = SCHOOL_AVOIDFACT(s);
+	double	matchFact = SCHOOL_MATCHFACT(s);
+	double	centerFact = SCHOOL_CENTERFACT(s);
+	double	targetFact = SCHOOL_TARGETFACT(s);
+	double	accLimit = SCHOOL_ACCLIMIT(s);
+	double	minRadius = SCHOOL_MINRADIUS(s);
+	Fish	*fishes = SCHOOL_FISHES(s);
+
+	for(i = 0, ref = fishes; i < nFish; i++, ref++) {
+		clearVector(avgVel);
+		clearVector(centroid);
+		clearVector(avoidance);
+		clearVector(FISH_ACC(ref));
+		neighborCount = glschool_computeGroupVectors(s, ref, avoidance, centroid, avgVel);
+
+		/* avoidanceAccel[] = avoidance[] * AvoidFact */
+		scaleVector(avoidance, avoidFact);
+		addVector(FISH_ACC(ref), avoidance);
+
+		accMag = norm(FISH_ACC(ref));
+		if (neighborCount > 0 && accMag < accLimit) {
+			for(j = 0; j < 3; j++) {
+				FISH_IAVGVEL(ref, j) = avgVel[j];
+				FISH_IACC(ref, j) += ((avgVel[j] - FISH_IVEL(ref, j)) * matchFact);
+			}
+
+			accMag = norm(FISH_ACC(ref));
+			if (accMag < accLimit) {
+				for(j = 0; j < 3; j++)
+					FISH_IACC(ref, j) += ((centroid[j] - FISH_IPOS(ref, j)) * centerFact);
+			}
+		}
+
+		accMag = norm(FISH_ACC(ref));
+		if (accMag < accLimit) {
+			getDifferenceVector(goal, FISH_POS(ref), diffVect);
+
+			dist = norm(diffVect) - distComp;
+			if (dist < 0.0) dist = 0.1;
+
+			/*adjDist = pow(dist, distExp);*/
+			if (dist > minRadius) {
+				adjDist = pow(dist, distExp);
+				for(j = 0; j < 3; j++)
+					FISH_IACC(ref, j) += (diffVect[j]*targetFact/adjDist);
+			}
+		}
+	}
+}