swf_tabulation.c (4768B)
1 /* Copyright (C) 2024 |Méso|Star> (contact@meso-star.com) 2 * 3 * This program is free software: you can redistribute it and/or modify 4 * it under the terms of the GNU General Public License as published by 5 * the Free Software Foundation, either version 3 of the License, or 6 * (at your option) any later version. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * along with this program. If not, see <http://www.gnu.org/licenses/>. */ 15 16 #include "swf.h" 17 #include "swf_tabulation.h" 18 19 #include <rsys/algorithm.h> 20 #include <rsys/mem_allocator.h> 21 22 /******************************************************************************* 23 * Helper functions 24 ******************************************************************************/ 25 static void 26 release_tabulation(ref_T* ref) 27 { 28 struct swf_tabulation* tab = CONTAINER_OF(ref, struct swf_tabulation, ref); 29 ASSERT(ref); 30 darray_item_release(&tab->items); 31 MEM_RM(tab->allocator, tab); 32 } 33 34 static FINLINE int 35 cmp_fx(const void* a, const void* b) 36 { 37 const double f_a = *((const double*)a); 38 const double f_b = ((const struct item*)b)->fx; 39 return f_a < f_b ? -1 : (f_a > f_b ? 1 : 0); 40 } 41 42 static INLINE double 43 linear_interpolation 44 (const double y, 45 const struct item* a, 46 const struct item* b) 47 { 48 double u, x; 49 ASSERT(a && b); 50 u = (y - a->fx) / (b->fx - a->fx); 51 x = u*(b->x - a->x) + a->x; 52 return x; 53 } 54 55 /* H(x) = H(x0) + H'(x0)*(x-x0) + 0.5*H"(x0)*(x-x0)^2 56 * x = x0 + (-H'(x0) +/-) sqrt(H'(x0)^2 - 2*H"(x0)*(H(x0) - H(x)))) / H"(x0)) */ 57 static INLINE double 58 quadratic_extrapolation 59 (const double fx, 60 const struct item* a, 61 const struct item* b) 62 { 63 double x; 64 double d0, d1, x0, x1, y0, y1; 65 ASSERT(a && b); 66 67 d0 = a->dfx*a->dfx - 2*a->d2fx*(a->fx - fx); 68 d1 = b->dfx*b->dfx - 2*b->d2fx*(b->fx - fx); 69 ASSERT(d0 && d1); 70 71 y0 = (-a->dfx + sqrt(d0)) / (a->d2fx); 72 y1 = (-b->dfx + sqrt(d1)) / (b->d2fx); 73 x0 = a->x + y0; 74 x1 = b->x + y1; 75 76 x = x0 - a->x > b->x - x1 ? x1 : x0; 77 ASSERT(a->x <= x && x <= b->x); 78 79 return x; 80 } 81 82 /******************************************************************************* 83 * Exported symbols 84 ******************************************************************************/ 85 res_T 86 swf_tabulation_ref_get(struct swf_tabulation* tab) 87 { 88 if(!tab) return RES_BAD_ARG; 89 ref_get(&tab->ref); 90 return RES_OK; 91 } 92 93 res_T 94 swf_tabulation_ref_put(struct swf_tabulation* tab) 95 { 96 if(!tab) return RES_BAD_ARG; 97 ref_put(&tab->ref, release_tabulation); 98 return RES_OK; 99 } 100 101 double 102 swf_tabulation_inverse 103 (const struct swf_tabulation* tab, 104 const enum swf_prediction prediction, 105 const double y) 106 { 107 const struct item* items = NULL; 108 const struct item* find = NULL; 109 double x = 0; /* Argument corresponding to input value y */ 110 size_t nitems = 0; 111 ASSERT(tab && y >= 0 && y < 1); 112 ASSERT(prediction == SWF_LINEAR || prediction == SWF_QUADRATIC); 113 114 items = darray_item_cdata_get(&tab->items); 115 nitems = darray_item_size_get(&tab->items); 116 ASSERT(nitems); 117 118 find = search_lower_bound(&y, items, nitems, sizeof(*items), cmp_fx); 119 120 if(y == 0) return 0; 121 122 /* Input y is not in the tabulated range: returns the nearest x */ 123 if(!find) return items[nitems-1].x; 124 if(find == items) return find->x; 125 126 ASSERT(find->fx >= y); 127 128 /* The input y correspond exactly to a tabulated argument */ 129 if(find->fx == y) return find->x; 130 131 ASSERT(find > items && find[-1].fx < y); 132 133 /* Predict x values from of tabulated arguments */ 134 switch(prediction) { 135 case SWF_LINEAR: 136 x = linear_interpolation(y, &find[-1], &find[0]); 137 break; 138 case SWF_QUADRATIC: 139 x = quadratic_extrapolation(y, &find[-1], &find[0]); 140 break; 141 default: FATAL("Unreachable code\n"); break; 142 } 143 144 return x; 145 } 146 147 /******************************************************************************* 148 * Local functions 149 ******************************************************************************/ 150 res_T 151 tabulation_create 152 (struct mem_allocator* mem_allocator, 153 struct swf_tabulation** out_tab) 154 { 155 struct swf_tabulation* tab = NULL; 156 struct mem_allocator* allocator = NULL; 157 res_T res = RES_OK; 158 ASSERT(out_tab); 159 160 allocator = mem_allocator ? mem_allocator : &mem_default_allocator; 161 tab = MEM_CALLOC(allocator, 1, sizeof(*tab)); 162 if(!tab) { res = RES_MEM_ERR; goto error; } 163 ref_init(&tab->ref); 164 tab->allocator = allocator; 165 darray_item_init(allocator, &tab->items); 166 167 exit: 168 *out_tab = tab; 169 return res; 170 error: 171 if(tab) { 172 SWF(tabulation_ref_put(tab)); 173 tab = NULL; 174 } 175 goto exit; 176 }