1 /*
2  * neighbor.c
3  *
4  *  Created on: 17.01.2017
5  *      Author: hoene
6  */
7 
8 #include "mysofa.h"
9 #include "mysofa_export.h"
10 #include "tools.h"
11 #include <stdio.h>
12 #include <stdlib.h>
13 #include <string.h>
14 
15 /* #define VDEBUG */
16 
mysofa_interpolate(struct MYSOFA_HRTF * hrtf,float * cordinate,int nearest,int * neighborhood,float * fir,float * delays)17 MYSOFA_EXPORT float *mysofa_interpolate(struct MYSOFA_HRTF *hrtf,
18                                         float *cordinate, int nearest,
19                                         int *neighborhood, float *fir,
20                                         float *delays) {
21   int i, use[6];
22   float d, d6[6];
23   float weight;
24   int size = hrtf->N * hrtf->R;
25 
26   d = distance(cordinate, hrtf->SourcePosition.values + nearest * hrtf->C);
27   if (fequals(d, 0)) {
28     if (hrtf->DataDelay.elements > hrtf->R) {
29       delays[0] = hrtf->DataDelay.values[nearest * hrtf->R];
30       delays[1] = hrtf->DataDelay.values[nearest * hrtf->R + 1];
31     } else {
32       delays[0] = hrtf->DataDelay.values[0];
33       delays[1] = hrtf->DataDelay.values[1];
34     }
35     float *ret = hrtf->DataIR.values + nearest * size;
36     copyFromFloat(fir, ret, size);
37     return ret;
38   }
39 
40   for (i = 0; i < 6; i++) {
41     use[i] = 0;
42     d6[i] = 1;
43   }
44 
45   if (neighborhood[0] >= 0 && neighborhood[1] >= 0) {
46     d6[0] = distance(cordinate,
47                      hrtf->SourcePosition.values + neighborhood[0] * hrtf->C);
48     d6[1] = distance(cordinate,
49                      hrtf->SourcePosition.values + neighborhood[1] * hrtf->C);
50 
51     if (!fequals(d6[0], d6[1])) {
52       if (d6[0] < d6[1])
53         use[0] = 1;
54       else
55         use[1] = 1;
56     }
57   } else if (neighborhood[0] >= 0) {
58     d6[0] = distance(cordinate,
59                      hrtf->SourcePosition.values + neighborhood[0] * hrtf->C);
60     use[0] = 1;
61   } else if (neighborhood[1] >= 0) {
62     d6[1] = distance(cordinate,
63                      hrtf->SourcePosition.values + neighborhood[1] * hrtf->C);
64     use[1] = 1;
65   }
66 
67   if (neighborhood[2] >= 0 && neighborhood[3] >= 0) {
68     d6[2] = distance(cordinate,
69                      hrtf->SourcePosition.values + neighborhood[2] * hrtf->C);
70     d6[3] = distance(cordinate,
71                      hrtf->SourcePosition.values + neighborhood[3] * hrtf->C);
72     if (!fequals(d6[2], d6[3])) {
73       if (d6[2] < d6[3])
74         use[2] = 1;
75       else
76         use[3] = 1;
77     }
78   } else if (neighborhood[2] >= 0) {
79     d6[2] = distance(cordinate,
80                      hrtf->SourcePosition.values + neighborhood[2] * hrtf->C);
81     use[2] = 1;
82   } else if (neighborhood[3] >= 0) {
83     d6[3] = distance(cordinate,
84                      hrtf->SourcePosition.values + neighborhood[3] * hrtf->C);
85     use[3] = 1;
86   }
87 
88   if (neighborhood[4] >= 0 && neighborhood[5] >= 0) {
89     d6[4] = distance(cordinate,
90                      hrtf->SourcePosition.values + neighborhood[4] * hrtf->C);
91     d6[5] = distance(cordinate,
92                      hrtf->SourcePosition.values + neighborhood[5] * hrtf->C);
93     if (!fequals(d6[4], d6[5])) {
94       if (d6[4] < d6[5])
95         use[4] = 1;
96       else
97         use[5] = 1;
98     }
99   } else if (neighborhood[4] >= 0) {
100     d6[4] = distance(cordinate,
101                      hrtf->SourcePosition.values + neighborhood[4] * hrtf->C);
102     use[4] = 1;
103   } else if (neighborhood[5] >= 0) {
104     d6[5] = distance(cordinate,
105                      hrtf->SourcePosition.values + neighborhood[5] * hrtf->C);
106     use[5] = 1;
107   }
108 
109   weight = 1 / d;
110   copyArrayWeighted(fir, hrtf->DataIR.values + nearest * size, size, weight);
111   if (hrtf->DataDelay.elements > hrtf->R) {
112     delays[0] = hrtf->DataDelay.values[nearest * hrtf->R] * weight;
113     delays[1] = hrtf->DataDelay.values[nearest * hrtf->R + 1] * weight;
114   } else {
115     delays[0] = hrtf->DataDelay.values[0] * weight;
116     delays[1] = hrtf->DataDelay.values[1] * weight;
117   }
118 #ifdef VDEBUG
119   printf("%d ! %f ", nearest, d);
120 #endif
121   for (i = 0; i < 6; i++) {
122     if (use[i]) {
123       float w = 1 / d6[i];
124 #ifdef VDEBUG
125       printf("%d - %f ", neighborhood[i], d6[i]);
126 #endif
127       addArrayWeighted(fir, hrtf->DataIR.values + neighborhood[i] * size, size,
128                        w);
129       weight += w;
130       if (hrtf->DataDelay.elements > hrtf->R) {
131         delays[0] += hrtf->DataDelay.values[neighborhood[i] * hrtf->R] * w;
132         delays[1] += hrtf->DataDelay.values[neighborhood[i] * hrtf->R + 1] * w;
133       }
134     }
135   }
136 #ifdef VDEBUG
137   printf("\n");
138 #endif
139   weight = 1 / weight;
140   scaleArray(fir, size, weight);
141   delays[0] *= weight;
142   delays[1] *= weight;
143   return fir;
144 }
145