GenericIA3D_monolithic.java

package GenericIA3D;

import processing.core.PApplet;

import java.util.List;
import java.util.ArrayList;
import java.util.function.Function;


import culebra.viz.*;
import toxi.processing.*;
import toxi.geom.*;


public class GenericIA3D extends PApplet {
	public static void main(String[] args) {
		String[] processingArgs = {"GenericIA3D"};
		GenericIA3D sketch = new GenericIA3D();
		PApplet.runSketch(processingArgs, sketch);
	}

	// Camera & Graphics
	ToxiclibsSupport gfx;
	Cameras cam;
	GenericAgent cam_target;

	// Sim Properties
	GenericAgentController controller;
	List<GenericAgent> group;
	String gfx_type;

	int DIM = 500;
	float NEIGHBORHOOD_SIZE_SQUARED = pow(150.0f, 2);

	int AGENT_COUNT = 1000;
	int AGENT_SIZE = 5;
	float MAX_SPEED = 4.0f;

	int MAX_TRAIL_SEGMENTS = 20;
	float TRAIL_SEG_LENGTH = 35.0f;
	float FORCE_VECTOR_LENGTH = 70.0f;

	static float SEP_STR = 0.5f;
	static float COH_STR = 0.5f;
	static float ALI_STR = 0.5f;

	// Sim Options
	boolean PAUSE = false;
	boolean DEBUG = false;
	boolean WRAPPING = true;

	boolean FOLLOW = false;
	boolean FOLLOW_TOGGLED = false;

	boolean OBSTACLES = false;

	boolean TRAILS = false;
	boolean SHOW_NEIGHBORS = false;
	boolean SHOW_FORCE_VECTORS = true;

	boolean FORCE1 = true;
	boolean FORCE2 = true;
	boolean FORCE3 = true;

	// color transformation matrix used to map XYZ position into RGB values
	Matrix4x4 colorMatrix = new Matrix4x4().scale(255f/(DIM*2)).translate(DIM, DIM, DIM);

	public void settings() {
		// Environment Setup
		//size(1280, 720, P3D);
		fullScreen(P3D);
		//frameRate(165.0f);
		smooth();  
	}

	public void setup() {
		background(0);

		gfx_type = "toxic";
		if (gfx_type == "toxic") gfx = new ToxiclibsSupport(this);

		// Agent Setup
		controller = new GenericAgentController();
		SimOptions so = new SimOptions();	  

		group = controller.build_group(AGENT_COUNT, so);
		controller.add_group(group);

		// Camera Setup
		cam = set_default_camera();
	}

	public void draw() {
		background(0);

		controller.update_groups(gfx_type, colorMatrix);

		// Select follow target and update follow cam if toggled.
		if (FOLLOW && FOLLOW_TOGGLED) {
			cam_target = group.get((int) random(0, AGENT_COUNT));
			FOLLOW_TOGGLED = false;
		}
		if (FOLLOW) { 
			int[] target = new int[] {(int) cam_target.position.x, 
					(int) cam_target.position.y, 
					(int) cam_target.position.z};

			cam.set3DCamera(50, 50, 100, target, false);
		}

		// Show HUD
		cam.beginHUD();
		textSize(12);
		stroke(0, 0, 0);
		text("FPS " + (frameRate), 20, 20);
		cam.endHUD();
	}

	public void keyPressed() {
		if (key == 'r') setup();
		if (key == 'p') PAUSE = !PAUSE;
		if (key == 't') TRAILS = !TRAILS;
		if (key == 'v') SHOW_FORCE_VECTORS = !SHOW_FORCE_VECTORS;
		if (key == 'n') SHOW_NEIGHBORS = !SHOW_NEIGHBORS;
		if (key == 'o') OBSTACLES = !OBSTACLES;
		if (key == 'd') DEBUG = !DEBUG;
		if (key == '1') FORCE1 = !FORCE1;
		if (key == '2') FORCE2 = !FORCE2;
		if (key == '3') FORCE3 = !FORCE3;
		if (key == 'f') {
			if (FOLLOW) {
				set_default_camera();
			} else {
				FOLLOW_TOGGLED = true;
			}
			FOLLOW = !FOLLOW; 
		} 
	}

	public Cameras set_default_camera() {
		Cameras cam = new Cameras(this);
		int[] lookat = new int[] { width / 2, height / 2, DIM / 2 };
		cam.set3DCamera(DIM / 10, 0, DIM / 2, lookat, false);

		return cam;
	}

	class Forces {};

	class BoidForces extends Forces {
		private List<Float> strengths;

		public BoidForces(float sep_str, float coh_str, float ali_str) {
			strengths = new ArrayList<Float>();
			strengths.add(sep_str);
			strengths.add(coh_str);
			strengths.add(ali_str);
		};	 

		public Vec3D separation(List<GenericAgent> agents) {
			Vec3D separation = agents.get(0).position;
			agents.remove(0);

			for (GenericAgent partner : agents) {
				separation = separation.interpolateTo(partner.position, strengths.get(0));
			}
			return separation;
		}

		public Vec3D cohesion(List<GenericAgent> agents) {
			Vec3D cohesion = agents.get(0).position;
			agents.remove(0);

			for (GenericAgent partner : agents) {
				cohesion = cohesion.interpolateTo(partner.position, strengths.get(1)).getInverted();
			}

			return cohesion;
		}

		public Vec3D align(List<GenericAgent> agents) {
			//float rps = 1 / partners.size();
			Vec3D alignment = agents.get(0).position;
			agents.remove(0);
			for (GenericAgent partner : agents) {
				alignment = alignment.interpolateTo(partner.position, strengths.get(2));
			}
			return alignment;
		}
	}	

	class SimOptions {
		public SimOptions() {};

		int DIM = 500;
		float NEIGHBORHOOD_SIZE_SQUARED = pow(150.0f, 2);

		int AGENT_COUNT = 1000;
		int AGENT_SIZE = 5;
		float MAX_SPEED = 4.0f;

		int MAX_TRAIL_SEGMENTS = 20;
		float TRAIL_SEG_LENGTH = 35.0f;
		float FORCE_VECTOR_LENGTH = 70.0f;

		float SEP_STR = 0.5f;
		float COH_STR = 0.5f;
		float ALI_STR = 0.5f;


		// Sim Options
		boolean PAUSE = false;
		boolean DEBUG = false;
		boolean WRAPPING = true;

		boolean FOLLOW = false;
		boolean FOLLOW_TOGGLED = false;

		boolean OBSTACLES = false;

		boolean TRAILS = false;
		boolean SHOW_NEIGHBORS = false;
		boolean SHOW_FORCE_VECTORS = true;

		boolean FORCE1 = true;
		boolean FORCE2 = true;
		boolean FORCE3 = true;

		BoidForces forces;
	}

	class GenericAgentController {
		private List<List<GenericAgent>> groups;

		public GenericAgentController() {this.groups = new ArrayList<List<GenericAgent>>(); };

		public List<GenericAgent> build_group(int size, SimOptions so) {
			List<GenericAgent> agents = new ArrayList<GenericAgent>();
			for (int i = 0; i < size; i++) {
				Vec3D pos = new Vec3D(random(0, DIM), random(0, DIM), random(0, DIM));
				Vec3D vel = new Vec3D(random(-MAX_SPEED / 2, MAX_SPEED / 2), 
						random(-MAX_SPEED / 2, MAX_SPEED / 2), 
						random(-MAX_SPEED / 2, MAX_SPEED / 2));
				GenericAgent c = new GenericAgent(pos, vel, so); 
				agents.add(c);
			}
			return agents;
		}

		public void add_group(List<GenericAgent> group) { groups.add(group); }
		public void remove_group(int index) { groups.remove(index); }

		public void update_group(List<GenericAgent> group, String gfx_type, Matrix4x4 colour) {
			for (GenericAgent agent : group) {
				if (!PAUSE) agent.apply_forces(group, WRAPPING);
				agent.show(gfx_type, colour);
			}
		}

		public void update_groups(String gfx_type, Matrix4x4 colour) {
			for (List<GenericAgent> group : groups) update_group(group, gfx_type, colour);
		}
	}

	class GenericAgent {
		private Vec3D position, velocity, acceleration;
		private List<Function<List<GenericAgent>, Vec3D>> forces;
		private List<Vec3D> last_forces;
		private List<GenericAgent> last_neighbors;
		private List<Vec3D> trail;

		private SimOptions sim_options;

		public GenericAgent(Vec3D pos, Vec3D vel, SimOptions so) {
			position = pos;
			velocity = vel;
			forces = new ArrayList<Function<List<GenericAgent>, Vec3D>>();
			sim_options = so;

			for (Function<List<GenericAgent>, Vec3D> f : forces) {
				forces.add(f);
			}

			last_forces = new ArrayList<Vec3D>();
			trail = new ArrayList<Vec3D>();
			trail.add(new Vec3D(position));
		}

		// Forces
		public void apply_forces(List<GenericAgent> group, boolean wrapping) {
			acceleration = new Vec3D();
			last_neighbors = this.neighborhood(group);
			last_forces = new ArrayList<Vec3D>();
			if (last_neighbors.size() < 1) return;

			for (int i = 0; i < forces.size() - 1; i++){
				if (i == 0 && !FORCE1 || i == 1 && !FORCE2 || i == 2 && !FORCE3) continue;
				Function<List<GenericAgent>, Vec3D> force_op = forces.get(i);
				Vec3D force = force_op.apply(group);

				force.normalizeTo(MAX_SPEED * sim_options.forces.strengths.get(i));
				last_forces.add(force);
				acceleration.addSelf(force);
			}

			velocity.addSelf(acceleration.normalizeTo(MAX_SPEED));
			velocity.limit(MAX_SPEED);
			position.addSelf(velocity);
			if (wrapping) wrap();
		}

		// Force Helpers
		private List<GenericAgent> neighborhood(List<GenericAgent> group) {
			float dist_squared;
			List<GenericAgent> neighbors = new ArrayList<GenericAgent>();

			neighbors.add(this); // ensure the first item is always the agent being acted on.
			for (GenericAgent neighbor : group) {
				dist_squared = position.magSquared() + neighbor.position.magSquared();
				if (dist_squared < NEIGHBORHOOD_SIZE_SQUARED && 
						!position.sub(neighbor.position).isZeroVector()) neighbors.add(neighbor);
			}
			return neighbors;
		}

		private void wrap() {
			boolean wrapped = false;
			if (position.x < -DIM) { position.x = DIM; wrapped = true; }
			if (position.y < -DIM) { position.y = DIM; wrapped = true; }
			if (position.z < -DIM) { position.z = DIM; wrapped = true; }
			if (position.y > DIM) { position.y = -DIM; wrapped = true; }
			if (position.x > DIM) { position.x = -DIM; wrapped = true; }
			if (position.z > DIM) { position.z = -DIM; wrapped = true; }
			if (wrapped) { trail = new ArrayList<Vec3D>(); trail.add(new Vec3D(position)); }
		}

		// Graphics
		public void show(String gfx_type, Matrix4x4 colorMatrix) {
			Vec3D col = colorMatrix.applyTo(position.add(velocity).add(acceleration));
			fill(col.x, col.y, col.z);
			if (TRAILS) update_trail(TRAIL_SEG_LENGTH);
			if (gfx_type == "toxic") toxic_show(col);
		}

		private void update_trail(float seg_length_squared) {
			if (trail.get(trail.size() - 1).sub(position).magSquared() > seg_length_squared) {
				trail.add(new Vec3D(position));
			}
			if (trail.size() > 2 * MAX_TRAIL_SEGMENTS) {
				trail.remove(0);
			}
		}

		// Toxic Support
		private void toxic_show(Vec3D colour) {
			pushMatrix();
			fill(colour.x, colour.y, colour.z);
			stroke(colour.y, colour.z, colour.x);

			gfx.cone(new Cone(position, velocity, 0, AGENT_SIZE, AGENT_SIZE*4),5,false);

			if (TRAILS) this.toxic_show_trail(); 

			for (GenericAgent neighbor : last_neighbors) {  
				if (SHOW_NEIGHBORS) { stroke(100, 50, 200); gfx.line(position, neighbor.position); }
			}

			if (SHOW_FORCE_VECTORS) this.toxic_show_force_vectors();
			popMatrix();
		}

		private void toxic_show_force_vectors() {
			int num_forces = this.last_forces.size();
			for (int i = 0; i < num_forces - 1; i++) {
				Vec3D force = this.last_forces.get(i);
				Vec3D colour = force.normalizeTo(255);

				if (!force.isZeroVector()) {
					stroke(colour.x, colour.y, colour.z);
					gfx.line(position, position.add(force.normalizeTo(FORCE_VECTOR_LENGTH)));
				}
			}
			gfx.line(position, position.add(acceleration.normalizeTo(FORCE_VECTOR_LENGTH)));
			stroke(0, 0, 0);
		}

		private void toxic_show_trail() {
			if (trail.size() < 2) { return; }
			for (int i = 0; i < trail.size() - 1; i += 1) {
				gfx.line(trail.get(i), trail.get(i+1));
			}
		}
	}
}

// TODO: move force strengths into Forces make Forces into a Ruleset pass ruleset to controller
// TODO: fix force strengths and normalization
// TODO: why is it only neighboring central particles and not all?