ssp_ranst_piecewise_linear.c (7697B)
1 /* Copyright (C) 2015-2025 |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 "ssp_rng_c.h" 17 18 #include <rsys/mem_allocator.h> 19 #include <rsys/ref_count.h> 20 21 #include <algorithm> 22 23 using piecewise_dist = RAN_NAMESPACE::piecewise_linear_distribution<double>; 24 25 using piecewise_dist_float = RAN_NAMESPACE::piecewise_linear_distribution<float>; 26 27 struct ssp_ranst_piecewise_linear { 28 ref_T ref; 29 struct mem_allocator* allocator; 30 piecewise_dist* distrib; 31 }; 32 33 struct ssp_ranst_piecewise_linear_float { 34 ref_T ref; 35 struct mem_allocator* allocator; 36 piecewise_dist_float* distrib; 37 }; 38 39 /******************************************************************************* 40 * Helper function 41 ******************************************************************************/ 42 static void 43 piecewise_release(ref_T* ref) 44 { 45 ssp_ranst_piecewise_linear* ran; 46 ASSERT(ref); 47 ran = CONTAINER_OF(ref, ssp_ranst_piecewise_linear, ref); 48 if(ran->distrib) { 49 ran->distrib->~piecewise_dist(); 50 MEM_RM(ran->allocator, ran->distrib); 51 } 52 MEM_RM(ran->allocator, ran); 53 } 54 55 static void 56 piecewise_float_release(ref_T* ref) 57 { 58 ssp_ranst_piecewise_linear_float* ran; 59 ASSERT(ref); 60 ran = CONTAINER_OF(ref, ssp_ranst_piecewise_linear_float, ref); 61 if(ran->distrib) { 62 ran->distrib->~piecewise_dist_float(); 63 MEM_RM(ran->allocator, ran->distrib); 64 } 65 MEM_RM(ran->allocator, ran); 66 } 67 68 /******************************************************************************* 69 * Exported functions 70 ******************************************************************************/ 71 res_T 72 ssp_ranst_piecewise_linear_create 73 (struct mem_allocator* allocator, 74 struct ssp_ranst_piecewise_linear** out_ran) 75 { 76 struct ssp_ranst_piecewise_linear* ran = nullptr; 77 void* mem = nullptr; 78 res_T res = RES_OK; 79 80 if(!out_ran) { 81 res = RES_BAD_ARG; 82 goto error; 83 } 84 85 allocator = allocator ? allocator : &mem_default_allocator; 86 87 ran = static_cast<ssp_ranst_piecewise_linear*> 88 (MEM_CALLOC(allocator, 1, sizeof(ssp_ranst_piecewise_linear))); 89 if(!ran) { 90 res = RES_MEM_ERR; 91 goto error; 92 } 93 ref_init(&ran->ref); 94 95 mem = MEM_ALLOC(allocator, sizeof(piecewise_dist)); 96 if(!mem) { 97 res = RES_MEM_ERR; 98 goto error; 99 } 100 ran->allocator = allocator; 101 ran->distrib = static_cast<piecewise_dist*>(new(mem) piecewise_dist); 102 if(out_ran) *out_ran = ran; 103 104 exit: 105 return res; 106 error: 107 if(ran) { 108 SSP(ranst_piecewise_linear_ref_put(ran)); 109 ran = nullptr; 110 } 111 goto exit; 112 } 113 114 res_T 115 ssp_ranst_piecewise_linear_float_create 116 (struct mem_allocator* allocator, 117 struct ssp_ranst_piecewise_linear_float** out_ran) 118 { 119 struct ssp_ranst_piecewise_linear_float* ran = nullptr; 120 void* mem = nullptr; 121 res_T res = RES_OK; 122 123 if(!out_ran) { 124 res = RES_BAD_ARG; 125 goto error; 126 } 127 128 allocator = allocator ? allocator : &mem_default_allocator; 129 130 ran = static_cast<ssp_ranst_piecewise_linear_float*> 131 (MEM_CALLOC(allocator, 1, sizeof(ssp_ranst_piecewise_linear_float))); 132 if(!ran) { 133 res = RES_MEM_ERR; 134 goto error; 135 } 136 ref_init(&ran->ref); 137 138 mem = MEM_ALLOC(allocator, sizeof(piecewise_dist_float)); 139 if(!mem) { 140 res = RES_MEM_ERR; 141 goto error; 142 } 143 ran->allocator = allocator; 144 ran->distrib 145 = static_cast<piecewise_dist_float*>(new(mem) piecewise_dist_float); 146 if (out_ran) *out_ran = ran; 147 148 exit: 149 return res; 150 error: 151 if(ran) { 152 SSP(ranst_piecewise_linear_float_ref_put(ran)); 153 ran = nullptr; 154 } 155 goto exit; 156 } 157 158 res_T 159 ssp_ranst_piecewise_linear_setup 160 (struct ssp_ranst_piecewise_linear* ran, 161 const double* intervals, 162 const double* weights, 163 const size_t size) 164 { 165 size_t i; 166 if(!ran || !intervals || !weights || size < 2) 167 return RES_BAD_ARG; 168 169 /* Checking param validity to avoid an assert when using ran */ 170 for(i=0; i < size-1; i++) { 171 if(weights[i] < 0) return RES_BAD_ARG; 172 if(intervals[i+1] <= intervals[i]) return RES_BAD_ARG; 173 } 174 if (intervals[size-1] - intervals[size-2] <= 0) return RES_BAD_ARG; 175 piecewise_dist::param_type p{intervals, intervals + size, weights}; 176 ran->distrib->param(p); 177 return RES_OK; 178 } 179 180 res_T 181 ssp_ranst_piecewise_linear_float_setup 182 (struct ssp_ranst_piecewise_linear_float* ran, 183 const float* intervals, 184 const float* weights, 185 const size_t size) 186 { 187 size_t i; 188 if(!ran || !intervals || !weights || size < 2) 189 return RES_BAD_ARG; 190 191 /* Checking param validity to avoid an assert when using ran */ 192 for(i=0; i < size-1; i++) { 193 if(weights[i] < 0) return RES_BAD_ARG; 194 if(intervals[i+1] <= intervals[i]) return RES_BAD_ARG; 195 } 196 if (weights[size-1] < 0) return RES_BAD_ARG; 197 piecewise_dist_float::param_type p{ intervals, intervals + size, weights }; 198 ran->distrib->param(p); 199 return RES_OK; 200 } 201 202 res_T 203 ssp_ranst_piecewise_linear_ref_get 204 (struct ssp_ranst_piecewise_linear* ran) 205 { 206 if(!ran) return RES_BAD_ARG; 207 ref_get(&ran->ref); 208 return RES_OK; 209 } 210 211 res_T 212 ssp_ranst_piecewise_linear_float_ref_get 213 (struct ssp_ranst_piecewise_linear_float* ran) 214 { 215 if(!ran) return RES_BAD_ARG; 216 ref_get(&ran->ref); 217 return RES_OK; 218 } 219 220 res_T 221 ssp_ranst_piecewise_linear_ref_put 222 (struct ssp_ranst_piecewise_linear* ran) 223 { 224 if(!ran) return RES_BAD_ARG; 225 ref_put(&ran->ref, piecewise_release); 226 return RES_OK; 227 } 228 229 res_T 230 ssp_ranst_piecewise_linear_float_ref_put 231 (struct ssp_ranst_piecewise_linear_float* ran) 232 { 233 if(!ran) return RES_BAD_ARG; 234 ref_put(&ran->ref, piecewise_float_release); 235 return RES_OK; 236 } 237 238 double 239 ssp_ranst_piecewise_linear_get 240 (const struct ssp_ranst_piecewise_linear* ran, 241 struct ssp_rng* rng) 242 { 243 return wrap_ran(*rng, *ran->distrib); 244 } 245 246 float 247 ssp_ranst_piecewise_linear_float_get 248 (const struct ssp_ranst_piecewise_linear_float* ran, 249 struct ssp_rng* rng) 250 { 251 return wrap_ran(*rng, *ran->distrib); 252 } 253 254 double 255 ssp_ranst_piecewise_linear_pdf 256 (const struct ssp_ranst_piecewise_linear *ran, 257 double x) 258 { 259 ASSERT(ran); 260 if(x<ran->distrib->min() || x>ran->distrib->max()) 261 return 0; 262 263 const auto& inter = ran->distrib->intervals(); 264 const auto& dens = ran->distrib->densities(); 265 auto b = std::lower_bound(inter.begin(), inter.end(), x); 266 size_t idx = b - inter.begin(); 267 if (x == *b) return dens[idx]; 268 idx--; 269 ASSERT(idx < inter.size() - 1); 270 return (dens[idx+1] * (x - inter[idx]) + dens[idx] * (inter[idx+1] - x)) 271 / (inter[idx+1]- inter[idx]); 272 } 273 274 float 275 ssp_ranst_piecewise_linear_float_pdf 276 (const struct ssp_ranst_piecewise_linear_float *ran, 277 float x) 278 { 279 ASSERT(ran); 280 if (x<ran->distrib->min() || x>ran->distrib->max()) 281 return 0; 282 283 const auto& inter = ran->distrib->intervals(); 284 /* std::piecewise_linear_distribution<float>::densities() 285 * should be a std::vector<float>, but is a std::vector<double> with gcc 286 * => use explicit casts */ 287 const auto& dens = ran->distrib->densities(); 288 auto b = std::lower_bound(inter.begin(), inter.end(), x); 289 size_t idx = b - inter.begin(); 290 if (x == *b) return (float)dens[idx]; 291 idx--; 292 ASSERT(idx < inter.size() - 1); 293 return 294 ((float)dens[idx+1] * (x-inter[idx]) + (float)dens[idx] * (inter[idx+1]-x)) 295 / (inter[idx+1] - inter[idx]); 296 }