It's surprising this stuff compiles, but fuck it

2 files changed, 208 insertions, 19 deletions

diff --git a/src/Makefile b/src/Makefile
index b9e9150..7c26ac9 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -7,9 +7,9 @@ CC		 = $(CROSS)cc
 CFLAGS		+= -I$(INCLUDE_PATH)
 LDFLAGS		+=
 
-HEADERS		= audio.h riff.h mixer.h synth.h vox.h bank.h
+HEADERS		= audio.h riff.h mixer.h synth.h vox.h bank.h spatial.h
 
-OBJS		= audio.o riff.o mixer.o synth.o vox.o bank.o
+OBJS		= audio.o riff.o mixer.o synth.o vox.o bank.o spatial.o
 
 VERSION_MAJOR	= 0
 VERSION_MINOR	= 0.1
diff --git a/src/spatial.c b/src/spatial.c
index 809fce1..18173aa 100644
--- a/src/spatial.c
+++ b/src/spatial.c
@@ -10,24 +10,146 @@ static inline float dist(xas_spatial_coord a, xas_spatial_coord b) {
               + powf(b.z - a.z, 2.0f), 0.5f);
 }
 
-static int buf_realloc(xas_spatial_scene *scene, void *buf) {
-    float width = scene->observer.width,
-          speed = scene->speed;
+static int buffer_realloc(xas_spatial_scene *scene,
+                          xas_spatial_buffer *buffer) {
+    float seconds = scene->radius / scene->speed;
 
     size_t sample_rate = scene->format.sample_rate,
            stride      = scene->format.channels * scene->format.sample_size,
-           count       = floorf(width / (speed / sample_rate));
+           count       = sample_rate * ceilf(seconds),
+           total       = sizeof(xas_spatial_buffer) + stride * count;
 
-    if ((buf = realloc(buf, stride * count)) == NULL) {
-        goto error_realloc;
+    if ((buffer = realloc(buffer, total)) == NULL) {
+        goto error_realloc_buffer;
     }
 
-    scene->buf    = buf;
-    scene->buflen = count;
+    memset(buffer + 1, '\0', stride * count);
+
+    scene->buffer        = buffer;
+    scene->buffer->index = 0;
+    scene->buffer->size  = count;
 
     return 0;
 
-error_realloc:
+error_realloc_buffer:
+    return -1;
+}
+
+static inline size_t sample_index(xas_spatial_scene *scene, float distance) {
+    size_t index = floorf(distance / scene->speed / scene->format.sample_rate);
+
+    return (scene->buffer->index + index) % scene->buffer->size;
+}
+
+static inline int16_t sample_scale(int16_t value, float distance) {
+    return (int16_t)roundf((float)value * (1.0f / powf(distance, 2.0)));
+}
+
+static void buffer_zero(xas_spatial_buffer *buffer, size_t count) {
+    int16_t *dest = (int16_t *)buffer + 1;
+    size_t i = buffer->index;
+
+    if (count > buffer->size) {
+        count = buffer->size;
+    }
+
+    while (count--) {
+        if (i == buffer->size) {
+            i = 0;
+        }
+
+        dest[i*XAS_AUDIO_STEREO]   = 0;
+        dest[i*XAS_AUDIO_STEREO+1] = 0;
+
+        i++;
+    }
+}
+
+static void buffer_copy(xas_spatial_buffer *buffer,
+                        int16_t *dest,
+                        size_t count) {
+    int16_t *src = (int16_t *)buffer + 1;
+    size_t i = buffer->index;
+
+    if (count > buffer->size) {
+        count = buffer->size;
+    }
+
+    while (count--) {
+        if (i == buffer->size) {
+            i = 0;
+        }
+
+        dest[i*XAS_AUDIO_STEREO]   = src[i*XAS_AUDIO_STEREO];
+        dest[i*XAS_AUDIO_STEREO+1] = src[i*XAS_AUDIO_STEREO+1];
+    }
+}
+
+ssize_t scene_fill(xas_spatial_scene *scene,
+                   int16_t *output,
+                   size_t count,
+                   xas_audio_stream *source) {
+    xas_spatial_buffer *buffer = scene->buffer;
+    xas_spatial_object *obj    = scene->first;
+
+    size_t index_old = buffer->index;
+
+    int16_t *dest = (int16_t *)(scene->buffer + 1);
+
+    buffer_zero(buffer, count);
+
+    while (obj) {
+        int16_t *src;
+
+        float distance_l = dist(scene->speaker_l, obj->coord),
+              distance_r = dist(scene->speaker_r, obj->coord);
+
+        size_t index_l,
+               index_r;
+
+        ssize_t readlen,
+                i;
+
+        if (distance_l > scene->radius || distance_r > scene->radius) {
+            continue;
+        }
+
+        if ((readlen = xas_audio_stream_read(obj->source,
+                                               (void **)&src,
+                                               count)) < 0) {
+            goto error_audio_stream_read;
+        }
+
+        if (readlen > (ssize_t)buffer->size) {
+            readlen = buffer->size;
+        }
+
+        index_l = sample_index(scene, distance_l);
+        index_r = sample_index(scene, distance_r);
+
+        for (i=0; i<readlen; i++) {
+            int16_t value_l = sample_scale(src[i], distance_l),
+                    value_r = sample_scale(src[i], distance_r);
+
+            if (buffer->index == buffer->size) {
+                buffer->index = 0;
+            }
+
+            dest[XAS_AUDIO_STEREO*index_l+i]   += value_l;
+            dest[XAS_AUDIO_STEREO*index_r+i+1] += value_r;
+
+            buffer->index++;
+        }
+
+        buffer->index = index_old;
+        obj           = obj->next;
+    }
+
+    buffer_copy(buffer, output, count);
+
+    return count;
+
+error_audio_stream_read:
     return -1;
 }
 
@@ -47,13 +169,13 @@ xas_spatial_scene *xas_spatial_scene_new(xas_audio_format format,
     scene->speaker_r = speaker_r;
     scene->speed     = XAS_SPATIAL_DEFAULT_SPEED;
 
-    if (buf_realloc(scene, NULL) < 0) {
-        goto error_buf_realloc;
+    if (buffer_realloc(scene, NULL) < 0) {
+        goto error_buffer_realloc;
     }
 
     return scene;
 
-error_buf_realloc:
+error_buffer_realloc:
     free(scene);
 
 error_malloc_scene:
@@ -61,19 +183,28 @@ error_malloc_scene:
 }
 
 void xas_spatial_scene_destroy(xas_spatial_scene *scene) {
-    free(scene->buf);
+    xas_spatial_object *object = scene->first;
+
+    while (object) {
+        xas_spatial_object *next = object->next;
+
+        free(object);
+
+        object = next;
+    }
+
+    free(scene->buffer);
     free(scene);
 }
 
-int xas_spatial_scene_set_observer(xas_spatial_scene *scene,
+
+void xas_spatial_scene_set_observer(xas_spatial_scene *scene,
                                       xas_spatial_coord coord,
                                       xas_spatial_rotation rotation,
                                       float width) {
     scene->observer.coord    = coord;
     scene->observer.rotation = rotation;
     scene->observer.width    = width;
-
-    return buf_realloc(scene, scene->buf);
 }
 
 void xas_spatial_scene_set_speaker_coords(xas_spatial_scene *scene,
@@ -83,6 +214,64 @@ void xas_spatial_scene_set_speaker_coords(xas_spatial_scene *scene,
     scene->speaker_r = speaker_r;
 }
 
-void xas_spatial_scene_set_speed(xas_spatial_scene *scene, float speed) {
+int xas_spatial_scene_set_speed(xas_spatial_scene *scene, float speed) {
     scene->speed = speed;
+
+    return buffer_realloc(scene, scene->buffer);
+}
+
+int xas_spatial_scene_set_radius(xas_spatial_scene *scene, float radius) {
+    scene->radius = radius;
+
+    return buffer_realloc(scene, scene->buffer);
+}
+
+xas_spatial_object *xas_spatial_scene_add_object(xas_spatial_scene *scene,
+                                                     xas_spatial_coord coord,
+                                                     xas_audio_stream *source) {
+    xas_spatial_object *object;
+
+    if ((object = malloc(sizeof(*object))) == NULL) {
+        goto error_malloc_object;
+    }
+
+    object->coord  = coord;
+    object->source = source;
+    object->next   = NULL;
+
+    if (scene->first == NULL) {
+        scene->first = object;
+    }
+
+    if (scene->last) {
+        scene->last->next = object;
+    }
+
+    scene->last = object;
+
+    return object;
+
+error_malloc_object:
+    return NULL;
+}
+
+void xas_spatial_object_get_coord(xas_spatial_object *object,
+                                    xas_spatial_coord *coord) {
+    coord->x = object->coord.x;
+    coord->y = object->coord.y;
+    coord->z = object->coord.z;
+}
+
+void xas_spatial_object_set_coord(xas_spatial_object *object,
+                                    xas_spatial_coord coord) {
+    object->coord = coord;
+}
+
+xas_audio_stream *xas_spatial_scene_new_stream(xas_spatial_scene *scene,
+                                                   size_t buffer_size) {
+    return xas_audio_stream_new_source((xas_audio_fill)scene_fill,
+                                         NULL,
+                                         scene->format,
+                                         buffer_size,
+                                         scene);
 }