My simple particle swarm optimization

5 files changed, 484 insertions, 0 deletions

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0e957d7
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+pso
\ No newline at end of file
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..ea72a7e
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,13 @@
+CC=gcc
+CFLAGS=-Wall -Werror -g -O3
+LDFLAGS=-lm -lpthread
+
+.PHONY: all clean
+
+all: pso
+
+pso: pso.c pso.h problems.h
+	$(CC) $(CFLAGS) $(LDFLAGS) -o $@ pso.c
+
+clean:
+	rm -rf pso
diff --git a/problems.h b/problems.h
new file mode 100644
index 0000000..d612f8a
--- /dev/null
+++ b/problems.h
@@ -0,0 +1,82 @@
+#pragma once
+
+#include "pso.h"
+
+typedef struct problem {
+	char* name;
+	interval_t interval;
+	double(*func)(particle_t*);
+} problem_t;
+
+static const interval_t sphere_interval = {-500, 500};
+static double sphere_func(particle_t* particle) {
+	size_t size = particle->x->size;
+	double sum = 0;
+
+	for (size_t i = 0; i < size; ++i) {
+		double x = particle->x->elements[i];
+		if (x < sphere_interval.start || x > sphere_interval.end)
+			return INFINITY;
+
+		sum += pow(x, 2);
+	}
+	return sum;
+}
+static const problem_t sphere = {"sphere", sphere_interval, sphere_func};
+
+
+static const interval_t rosenbrock_interval = {-30, 30};
+static double rosenbrock_func(particle_t* particle) {
+	size_t size = particle->x->size;
+	double sum = 0;
+
+	for (size_t i = 0; i < size - 1; ++i) {
+		double x = particle->x->elements[i];
+		double next_x = particle->x->elements[i + 1];
+
+		if (x < rosenbrock_interval.start || x > rosenbrock_interval.end
+		        || next_x < rosenbrock_interval.start || next_x > rosenbrock_interval.end)
+			return INFINITY;
+
+		sum += 100 * pow(next_x - pow(x, 2), 2) + pow((1 - x), 2);
+	}
+	return sum;
+}
+static const problem_t rosenbrock = {"rosenbrock", rosenbrock_interval, rosenbrock_func};
+
+static const interval_t rastrigin_interval = {-5.12, 5.12};
+static double rastrigin_func(particle_t* particle) {
+	size_t size = particle->x->size;
+	double sum = 0;
+
+	for (size_t i = 0; i < size; ++i) {
+		double x = particle->x->elements[i];
+
+		if (x < rastrigin_interval.start || x > rastrigin_interval.end)
+			return INFINITY;
+
+		sum += pow(x, 2) - 10 * cos(2 * M_PI * x);
+	}
+	return 10 * size + sum;
+}
+static const problem_t rastrigin = {"rastrigin", rastrigin_interval, rastrigin_func};
+
+static const interval_t schwefel_interval = {-500, 500};
+static double schwefel_func(particle_t* particle) {
+	size_t size = particle->x->size;
+	double sum = 0;
+
+	for (size_t i = 0; i < size; ++i) {
+		double x = particle->x->elements[i];
+
+		if (x < schwefel_interval.start || x > schwefel_interval.end)
+			return INFINITY;
+
+		sum += -1 * x * sin(sqrt(fabs(x)));
+	}
+	return sum;
+}
+static const problem_t schwefel = {"schwefel", schwefel_interval, schwefel_func};
+
+#define NUM_PROBLEMS 4
+static problem_t problems[NUM_PROBLEMS] = {sphere, rosenbrock, rastrigin, schwefel};
diff --git a/pso.c b/pso.c
new file mode 100644
index 0000000..1ece9b4
--- /dev/null
+++ b/pso.c
@@ -0,0 +1,159 @@
+#include <assert.h>
+#include <math.h>
+#include <pthread.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "pso.h"
+#include "problems.h"
+
+// defaults
+static size_t problem = 0;
+static size_t dimensions = 2;
+static size_t threads = 1;
+static size_t num_particles = 100;
+static size_t iterations = 100;
+static double a = 0.72984;
+static double b_loc = 1.496172;
+static double b_glob = 1.496172;
+
+static particle_t** particles;
+static vec_t* p_glob;
+static double p_glob_val;
+
+static pthread_barrier_t iteration_barrier;
+static pthread_barrier_t p_glob_barrier;
+
+static void* thread_func(void* arg) {
+	size_t tid = (size_t)arg;
+
+	size_t particles_per_thread = num_particles / threads;
+	size_t first_particle = tid * particles_per_thread;
+	size_t last_particle = first_particle + particles_per_thread;
+
+	init_random();
+
+	// iteration loop
+	for (size_t iteration = 0; iteration < iterations; ++iteration) {
+		// particle loop
+		for (size_t i = first_particle; i < last_particle; ++i) {
+			step(a, b_loc, b_glob, particles[i], p_glob);
+			evaluate_particle(particles[i], problems[problem].func);
+		}
+
+		pthread_barrier_wait(&iteration_barrier);
+		if (tid == 0) {
+			// update p_glob
+			vec_t* optimum_location;
+			find_min(particles, num_particles, &optimum_location, &p_glob_val);
+			copy_vec(optimum_location, p_glob);
+
+			// reset barrier
+			pthread_barrier_destroy(&iteration_barrier);
+			pthread_barrier_init(&iteration_barrier, NULL, threads);
+		}
+
+		// wait till p_glob is updated
+		pthread_barrier_wait(&p_glob_barrier);
+		if (tid == 0) {
+			// reset barrier
+			pthread_barrier_destroy(&p_glob_barrier);
+			pthread_barrier_init(&p_glob_barrier, NULL, threads);
+		}
+	}
+
+	return NULL;
+}
+
+int main(int argc, char* argv[]) {
+	// TODO: read parameters from command line
+	for(size_t i = 1; i < argc; i += 2) {
+		if (strcmp(argv[i], "-p") == 0) {
+			num_particles = atol(argv[i + 1]);
+		} else if (strcmp(argv[i], "-i") == 0) {
+			iterations = atol(argv[i + 1]);
+		} else if (strcmp(argv[i], "-t") == 0) {
+			threads = atol(argv[i + 1]);
+		} else if (strcmp(argv[i], "-f") == 0) {
+			problem = atol(argv[i + 1]);
+		} else if (strcmp(argv[i], "-b") == 0) {
+			b_loc = strtod(argv[i + 1], NULL);
+			b_glob = b_loc;
+		} else if (strcmp(argv[i], "-a") == 0) {
+			a = strtod(argv[i + 1], NULL);
+		} else if (strcmp(argv[i], "-h") == 0) {
+			printf("Usage: %s [OPTIONS]\n", argv[0]);
+			printf("OPTIONS:\n");
+			printf("\t -p number of particles\n");
+			printf("\t -h print this help and exit\n");
+			printf("\t -t number of threads\n");
+			printf("\t -i iterations\n");
+			printf("\t -f the function to optimize\n");
+			printf("\t     0 - sphere function\n");
+			printf("\t     1 - rosenbrock function\n");
+			printf("\t     2 - rastrigin function\n");
+			printf("\t     3 - schwefel function\n");
+			printf("\t -a the a parameter\n");
+			printf("\t -b the b parameter\n");
+			exit(1);
+		}
+	}
+
+	assert(num_particles % threads == 0);
+	assert(problem < NUM_PROBLEMS);
+
+	printf("threads: %ld, particles: %ld, iterations: %ld, function: %s\n",
+	       threads, num_particles, iterations, problems[problem].name);
+	printf("a: %lf, b_loc: %lf b_glob: %lf\n",
+	       a, b_loc, b_glob);
+
+	pthread_t *threads_array = check_malloc(sizeof(pthread_t*) * threads);
+
+	if (pthread_barrier_init(&iteration_barrier, NULL, threads) != 0) {
+		perror("pthread_barrier_init");
+		exit(1);
+	}
+
+	if (pthread_barrier_init(&p_glob_barrier, NULL, threads) != 0) {
+		perror("pthread_barrier_init");
+		exit(1);
+	}
+
+	init_random();
+	particles = check_malloc(sizeof(particle_t*) * num_particles);
+	for (size_t i = 0; i < num_particles; ++i) {
+		particles[i] = new_particle(dimensions, problems[problem].interval);
+	}
+
+	vec_t* optimum_location;
+	find_min(particles, num_particles, &optimum_location, &p_glob_val);
+	p_glob = new_vec(dimensions);
+	copy_vec(optimum_location, p_glob);
+
+	for (size_t i = 0; i < threads; ++i) {
+		if (pthread_create(&threads_array[i], NULL, thread_func, (void*)i) != 0) {
+			perror("pthread_create");
+			exit(1);
+		}
+	}
+
+	for (size_t i = 0; i < threads; ++i) {
+		if (pthread_join(threads_array[i], NULL) != 0) {
+			perror("pthread_join");
+			exit(1);
+		}
+	}
+
+	printf("Found optimum %g at ", p_glob_val);
+	print_vec(p_glob);
+	printf(" after step %ld\n", iterations);
+
+	for (size_t i = 0; i < num_particles; ++i) {
+		destroy_particle(particles[i]);
+	}
+
+	free(particles);
+	free(threads_array);
+	return 0;
+}
diff --git a/pso.h b/pso.h
new file mode 100644
index 0000000..e5e90c3
--- /dev/null
+++ b/pso.h
@@ -0,0 +1,229 @@
+#pragma once
+
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+__thread struct drand48_data *random_data;
+
+static void* check_malloc(size_t size) {
+	void *mem = malloc(size);
+	if (mem == NULL) {
+		perror("malloc");
+		exit(1);
+	}
+	return mem;
+}
+
+static void init_random() {
+	if (random_data == NULL) {
+		random_data = check_malloc(sizeof(struct drand48_data));
+		srand48_r(time(NULL), random_data);
+	}
+}
+
+typedef struct interval {
+	double start, end;
+} interval_t;
+
+typedef struct vector {
+	size_t size;
+	double elements[];
+} vec_t;
+
+static vec_t* new_vec(size_t size) {
+	vec_t* vec = malloc(sizeof(size_t) * sizeof(double) * size);
+	if (vec == NULL) {
+		perror("malloc");
+		exit(1);
+	}
+
+	vec->size = size;
+	return vec;
+}
+
+static void print_vec(vec_t* vec) { 
+	if (vec->size == 1) { 
+		printf("{%g}", vec->elements[0]); 
+	} 
+
+	printf("{%g", vec->elements[0]); 
+	for (int i = 1; i < vec->size; i++) { 
+		printf(", %g", vec->elements[i]); 
+	} 
+
+	printf("}"); 
+} 
+
+static void copy_vec(vec_t* src, vec_t* dest) {
+	assert(src->size == dest->size);
+	memcpy(dest->elements, src->elements, sizeof(double) * src->size);
+}
+
+static vec_t* rand_vec(size_t size, vec_t* vec) {
+	if (vec == NULL) {
+		vec = new_vec(size);
+	}
+
+	for (size_t i = 0; i < size; ++i) {
+		drand48_r(random_data, &vec->elements[i]);
+	}
+	return vec;
+}
+
+static vec_t* rand_vec_in_interval(size_t size, interval_t interval, vec_t* vec) {
+	vec = rand_vec(size, vec);
+
+	for (size_t i = 0; i < size; ++i) {
+		double r;
+		drand48_r(random_data, &r);
+		// get uniform sample from standard uniform distribution
+		// https://en.wikipedia.org/wiki/Uniform_distribution_(continuous)#Computational_methods
+		vec->elements[i] = interval.start + (interval.end - interval.start) * r;
+	}
+	return vec;
+}
+
+static void member_prod(vec_t* v1, vec_t* v2, vec_t* res) {
+	assert(v1->size == v2->size && v2->size == res->size);
+
+	for (size_t i = 0; i < v1->size; ++i) {
+		res->elements[i] = v1->elements[i] * v2->elements[i];
+	}
+}
+
+static void member_sum(vec_t* v1, vec_t* v2, vec_t* res) {
+	assert(v1->size == v2->size && v2->size == res->size);
+
+	for (size_t i = 0; i < v1->size; ++i) {
+		res->elements[i] = v1->elements[i] + v2->elements[i];
+	}
+}
+
+static void member_diff(vec_t* v1, vec_t* v2, vec_t* res) {
+	assert(v1->size == v2->size && v2->size == res->size);
+
+	for (size_t i = 0; i < v1->size; ++i) {
+		res->elements[i] = v1->elements[i] - v2->elements[i];
+	}
+}
+
+static void scalar_prod(vec_t* v1, double scalar, vec_t* res) {
+	assert(v1->size == res->size);
+
+	for (size_t i = 0; i < v1->size; ++i) {
+		res->elements[i] = v1->elements[i] * scalar;
+	}
+}
+
+typedef struct particle {
+	double p_val;
+	vec_t *v, *x, *p, *r_loc, *r_glob;
+} particle_t;
+
+static particle_t* new_particle(size_t size, interval_t interval) {
+	particle_t* particle = check_malloc(sizeof(particle_t));
+
+	particle->p_val = INFINITY;
+
+	particle->v = new_vec(size);
+	memset(&particle->v->elements[0], 0, sizeof(double) * size);
+
+	particle->x = new_vec(size);
+	rand_vec_in_interval(size, interval, particle->x);
+
+	particle->p = new_vec(size);
+	copy_vec(particle->x, particle->p);
+
+	particle->r_loc = new_vec(size);
+	particle->r_glob = new_vec(size);
+
+	return particle;
+}
+
+static void destroy_particle(particle_t* particle) {
+	free(particle->x);
+	free(particle->v);
+	free(particle->p);
+	free(particle->r_loc);
+	free(particle->r_glob);
+	free(particle);
+}
+static void print_particle(particle_t* particle) __attribute__((unused));
+
+static void print_particle(particle_t* particle) {
+	printf("Particle at: ");
+	print_vec(particle->x);
+	printf(" with v: ");
+	print_vec(particle->v);
+	printf(" and local best %lf at ", particle->p_val);
+	print_vec(particle->p);
+	printf("\n");
+}
+
+static void evaluate_particle(particle_t* particle, double(*evaluation_func)(particle_t*)) {
+	double res = evaluation_func(particle);
+
+	if (particle->p_val == INFINITY || res < particle->p_val) {
+		// update local optimum
+		particle->p_val = res;
+		copy_vec(particle->x, particle->p);
+	}
+}
+
+static void find_min(particle_t** particles, size_t num_particles, vec_t** optimum_location, double* optimum) {
+	particle_t* particle = particles[0];
+	*optimum = particle->p_val;
+	*optimum_location = particle->p;
+
+	for (size_t i = 1; i < num_particles; ++i) {
+		particle = particles[i];
+		if (particle->p_val < *optimum) {
+			*optimum = particle->p_val;
+			*optimum_location = particle->p;
+		}
+	}
+}
+
+static void step(double a, double b_loc, double b_glob,
+                 particle_t* particle, vec_t* p_glob) {
+	assert(particle->v->size == particle->p->size && particle->p->size == p_glob->size);
+
+	vec_t *v = particle->v;
+	vec_t *x = particle->x;
+	vec_t *p = particle->p;
+	vec_t *r_loc = particle->r_loc;
+	vec_t *r_glob = particle->r_glob;
+
+	size_t dimensions = x->size;
+	// get new random vectors
+	rand_vec(dimensions, r_loc);
+	rand_vec(dimensions, r_glob);
+
+	// old speed momentum
+	scalar_prod(v, a, v);
+
+	// local optimum attraction
+	vec_t* local_opt_attraction = new_vec(dimensions);
+	// random local attraction part
+	scalar_prod(r_loc, b_loc, r_loc);
+	member_diff(p, x, local_opt_attraction);
+	member_prod(local_opt_attraction, r_loc, local_opt_attraction);
+
+	// global optimum attraction
+	vec_t* global_opt_attraction = new_vec(dimensions);
+	// random global attraction part
+	scalar_prod(r_glob, b_glob, r_glob);
+	member_diff(p_glob, x, global_opt_attraction);
+	member_prod(global_opt_attraction, r_glob, global_opt_attraction);
+
+	member_sum(v, local_opt_attraction, v);
+	free(local_opt_attraction);
+
+	// new speed
+	member_sum(v, global_opt_attraction, v);
+	free(global_opt_attraction);
+
+	// new location
+	member_sum(x, v, x);
+}