I run this company, do you expect me to do this?

1 files changed, 80 insertions, 8 deletions

diff --git a/src/spatial.c b/src/spatial.c
index 1c09be7..02a4bf3 100644
--- a/src/spatial.c
+++ b/src/spatial.c
@@ -12,25 +12,69 @@ static inline float dist(xas_spatial_coord a, xas_spatial_coord b) {
               + powf(b.z - a.z, 2.0f), 0.5f);
 }
 
-static int point_within_cone(xas_spatial_coord coord,
-                             xas_spatial_cone cone) {
-    float radius;
+static void rotate(xas_spatial_rotation rotation,
+                   xas_spatial_coord *coord) {
+    double cosY = cos(rotation.y),
+           sinY = sin(rotation.y),
+           cosZ = cos(rotation.z),
+           sinZ = sin(rotation.z),
+           cosX = cos(rotation.x),
+           sinX = sin(rotation.x);
+
+    double Rx, Ry, Py, Pz, Yx, Yz;
+
+    /*
+     * Adjust vertex for Z axis rotation
+     */
+    Rx = coord->x * cosZ - coord->y * sinZ;
+    Ry = coord->y * cosZ + coord->x * sinZ;
 
     /*
-     * XXX Perform an affine transformation of the point along with the cone,
-     * fixing the point of the cone at 0, 0, 0 with the open end facing to the
-     * right on the X axis
+     * Adjust vertex for X axis rotation
      */
+    Py = Ry * cosX - coord->z * sinX;
+    Pz = coord->z * cosX - Ry * sinX;
+
+    /*
+     * Adjust vertex for Y axis rotation
+     */
+    Yx = Rx * cosY - Pz * sinY;
+    Yz = Pz * cosY - Rx * sinY;
+
+    coord->x = Yx;
+    coord->y = Py;
+    coord->z = Yz;
+}
+
+static int within_cone(xas_spatial_coord coord,
+                       xas_spatial_cone cone) {
+    xas_spatial_rotation rotation = {
+        .pitch = -cone.rotation.x / 2.0f,
+        .roll  = -cone.rotation.y / 2.0f,
+        .yaw   = -cone.rotation.z / 2.0f
+    };
+
+    float radius;
+
+    coord.x -= cone.coord.x;
+    coord.y -= cone.coord.y;
+    coord.z -= cone.coord.z;
+
+    rotate(rotation, &coord);
+
+    cone.coord.x = 0;
+    cone.coord.y = 0;
+    cone.coord.z = 0;
 
     /*
      * If the point is to the left of the cone point, then the point is not
      * within the cone.
      */
-    if (coord.x < cone.coord.x) {
+    if (coord.x < 0) {
         return 0;
     }
 
-    radius = tanf(cone.angle / 2.0f) * cone.coord.x;
+    radius = tanf(cone.angle / 2.0f) * coord.x;
 
     /*
      * If the given point is outside the cone radius at its X coordinate, then
@@ -178,6 +222,34 @@ ssize_t scene_fill(xas_spatial_scene *scene,
             size_t index_l,
                    index_r;
 
+            xas_spatial_cone cone_l = {
+                .coord    = scene->speaker_l,
+                .rotation = {
+                    .x = 1.75f * M_PI,
+                    .y = 0.0f,
+                    .z = 0.0f
+                },
+
+                .angle = M_PI / 4.0f
+            }, cone_r = {
+                .coord    = scene->speaker_r,
+                .rotation = {
+                    .x = 0.25f * M_PI,
+                    .y = 0.0f,
+                    .z = 0.0f
+                },
+
+                .angle = M_PI / 4.0f
+            };
+
+            if (!within_cone(obj->coord, cone_l)) {
+                value_l /= 2.0f;
+            }
+
+            if (!within_cone(obj->coord, cone_r)) {
+                value_r /= 2.0f;
+            }
+
             if (buffer->index == buffer->size) {
                 buffer->index = 0;
             }