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