csmbase.c 48.5 KB
Newer Older
1
2
/*! \file csmbase.c
    \brief implement function table
3
    \author Götz Pfeiffer (goetz.pfeiffer\@helmholtz-berlin.de)
4
    \version 1.0
5

6
   This module implements one and two dimensional function tables.
7
   The tables can be specified by simple ascii files. The points
8
9
10
   defined in the file don't have to be in the same distance.
*/

11
/*____________________________________________________________*/
franksen's avatar
franksen committed
12
/*                      general Defines                       */
13
/*____________________________________________________________*/
franksen's avatar
franksen committed
14
15


16
17
18
#ifndef DOXYGEN_SKIP_THIS

/*! \internal \brief needed for POSIX compability */
franksen's avatar
franksen committed
19
20
#define _INCLUDE_POSIX_SOURCE

21
/*! \internal \brief use DBG module ? */
22
23
#define USE_DBG 0

24
/*! \internal \brief use epics errlogPrintf ? */
25
26
#define USE_ERRLOGPRINTF 1

27
28
29
/*! \internal \brief use semaphores at all ? */
#define USE_SEM 1

30
/*! \internal \brief use psem semaphore module ? */
31
#define USE_PSEM 0
32
33
34
35
36

/* the following macros affect the debug (dbg) module: */

#if USE_DBG

37
/*! \internal \brief compile DBG assertions */
38
#define DBG_ASRT_COMP 0
39

40
/*! \internal \brief compile DBG debug messages */
41
#define DBG_MSG_COMP 0
42

43
/*! \internal \brief compile DBG trace messages */
44
45
#define DBG_TRC_COMP  0

46
/*! \internal \brief use local switch-variables for the dbg module */
47
48
#define DBG_LOCAL_COMP 1

49
/*! \internal \brief use async. I/O with message passing in DBG module*/
50
#define DBG_ASYNC_COMP 1
51

franksen's avatar
franksen committed
52
/* the following defines are for sci-debugging, they do not influence
53
   any header-files but are placed here since they are usually changed
franksen's avatar
franksen committed
54
   together with the above macros.*/
55

56
/*! \internal \brief for debug-outputs, "stdout" means console */
franksen's avatar
franksen committed
57
#define CSMBASE_OUT_FILE "stdout"
58

59
/*! \internal \brief csm trace level
60
61
62
63

  \li level 6: dense traces for debugging
  \li level 5: enter/exit tracing
*/
franksen's avatar
franksen committed
64
#define CSMBASE_TRC_LEVEL 0
65

66
/*! \internal \brief flushmode, 0, 1 and 2 are allowed */
franksen's avatar
franksen committed
67
68
#define CSMBASE_FLUSHMODE 1

69
70
#else

71
72
73
74
#if !USE_ERRLOGPRINTF
#define errlogPrintf printf
#endif

75
/*! \internal \brief compability macro, needed when DBG is not used */
76
#define DBG_MSG_PRINTF2(f,x) errlogPrintf(f,x)
77

78
/*! \internal \brief compability macro, needed when DBG is not used */
79
#define DBG_MSG_PRINTF3(f,x,y) errlogPrintf(f,x,y)
80

81
/*! \internal \brief compability macro, needed when DBG is not used */
82
#define DBG_MSG_PRINTF4(f,x,y,z) errlogPrintf(f,x,y,z)
83

pfeiffer's avatar
pfeiffer committed
84
85
86
/*! \internal \brief compability macro, needed when DBG is not used */
#define DBG_MSG_PRINTF5(f,x,y,z,q) errlogPrintf(f,x,y,z,q)

87
88
#endif

89
90
91
92
93
94
95
96
97
#if !USE_SEM
#define SEM_TYPE int
#define SEMTAKE(x) 
#define SEMGIVE(x) 
#define SEMDELETE(x) 
#define SEMCREATE(x) 0 

#else

98
99
100
101
102
103
104
105
#if USE_PSEM
#define SEM_TYPE psem_mutex
#define SEMTAKE(x) psem_mutex_take(&(x))
#define SEMGIVE(x) psem_mutex_give(&(x))
#define SEMDELETE(x) psem_mutex_remove(&(x))
#define SEMCREATE(x) (!psem_mutex_create(&(x)))
#else
/*! \internal \brief compability macro for semaphores */
106
#define SEM_TYPE epicsMutexId
107
/*! \internal \brief compability macro for semaphores */
108
#define SEMTAKE(x) epicsMutexLock(x)
109
/*! \internal \brief compability macro for semaphores */
110
#define SEMGIVE(x) epicsMutexUnlock(x)
111
/*! \internal \brief compability macro for semaphores */
112
#define SEMDELETE(x) epicsMutexDestroy(x)
113
/*! \internal \brief compability macro for semaphores */
114
#define SEMCREATE(x) (NULL==(x= epicsMutexCreate()))
115
116
#endif

117
118
#endif

119
120
#endif

121
122
123
124
125
/*! \brief for type csm_bool */
#define CSM_TRUE 1

/*! \brief for type csm_bool */
#define CSM_FALSE 0
franksen's avatar
franksen committed
126
127
128
129
130

/*____________________________________________________________*/
/*			 Include-Files			      */
/*____________________________________________________________*/

131
#if USE_SEM
132

133
134
135
#if USE_PSEM
#include <psem.h>
#else
136
#include <epicsMutex.h>
137
#endif
franksen's avatar
franksen committed
138

139
#endif
140

141
#if USE_DBG
142
#include <dbg.h>
143
144
145
#endif

#if USE_ERRLOGPRINTF
146
#include <errlog.h> /* epics error printf */
147
#endif
franksen's avatar
franksen committed
148

pfeiffer's avatar
pfeiffer committed
149
#include <ctype.h>
franksen's avatar
franksen committed
150
#include <stdlib.h>
151
#include <stdio.h>
franksen's avatar
franksen committed
152
153
#include <string.h>

154
155
156
/* math.h for definition of NAN */
#include <math.h>

157
#include "csmbase.h"
158

159
160
161
/*____________________________________________________________*/
/*                         Defines                            */
/*____________________________________________________________*/
162
163
164
165
166
167

#ifndef NAN
/*! \internal \brief define NAN if it is not yet defined */
#define NAN (-(0.0/0.0))
#endif

168
/*____________________________________________________________*/
franksen's avatar
franksen committed
169
/*                          Types                             */
170
/*____________________________________________________________*/
171

172
173
174
      /*................................................*/
      /*          the coordinate type (private)         */
      /*................................................*/
175

176
/*! \internal \brief a single coordinate value */
franksen's avatar
franksen committed
177
typedef struct
178
  {
179
180
    double value; /*!< the floating point value of the coordinate */
    int    index; /*!< the index-number within this list of coordinates */
franksen's avatar
franksen committed
181
  } csm_coordinate;
182
183

/*! \internal \brief a list of coordinates */
franksen's avatar
franksen committed
184
typedef struct
185
  { csm_coordinate *coordinate;  /*!< pointer to coordinate array */
franksen's avatar
franksen committed
186
    int            no_of_elements;
187
                                 /*!< number of elements in the coordinate
188
189
190
				      array */
    int            a_last;       /*!< last left index */
    int            b_last;       /*!< last right index */
191
  } csm_coordinates;
franksen's avatar
franksen committed
192

193
194
      /*................................................*/
      /*        the linear function type (private)      */
franksen's avatar
franksen committed
195
196
      /*................................................*/

197
/*! \internal \brief a linear function y = a + b*x */
franksen's avatar
franksen committed
198
typedef struct
199
200
  { double a;         /*!< a in y= a+b*x */
    double b;         /*!< b in y= a+b*x */
201
202
203
204
205
206
  } csm_linear_function;

      /*................................................*/
      /*           the 1d table type (private)          */
      /*................................................*/

207
/*! \internal \brief one-dimenstional function table
208

209
  This is a list of x-y pairs that define a one-dimensional
210
  function. The function can be inverted, if it is monotone.
211
*/
212
typedef struct
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
213
214
215
216
217
  { csm_coordinates x;     /*!< list of x-coordinates */
    csm_coordinates y;     /*!< list of y-coordinates */
    double x_last;         /*!< last x value looked up */
    double y_last;         /*!< last y value looked up */
    csm_bool value_cached; /*!< 1 if x_last, y_last valid */
218
219
  } csm_1d_functiontable;

220
221
222
223
      /*................................................*/
      /*          the 2d table type (private)           */
      /*................................................*/

224
/*! \internal \brief two-dimenstional function table
225

226
  This is a list of x-y pairs and corresponding z-values that
227
  define a two-dimensional function table. The function cannot be inverted.
228
*/
franksen's avatar
franksen committed
229
typedef struct
230
231
232
  { csm_coordinates x;   /*!< list of x-coordinates */
    csm_coordinates y;   /*!< list of y-coordinates */
    double          *z;  /*!< list of z-values (function values) */
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
233
234
235
236
    double x_last;         /*!< last x value looked up */
    double y_last;         /*!< last y value looked up */
    double z_last;         /*!< last z value looked up */
    csm_bool value_cached; /*!< 1 if x_last, y_last valid */
237
238
  } csm_2d_functiontable;

239
240
      /*................................................*/
      /*           the function-type (private)          */
franksen's avatar
franksen committed
241
242
      /*................................................*/

243
#ifndef DOXYGEN_SKIP_THIS
244
245
246
247
248
/*! \internal \brief typedef-enum: types of functions known in this module */
typedef enum { CSM_NOTHING,  /*!< kind of NULL value */
               CSM_LINEAR,   /*!< linear y=a+b*x */
               CSM_1D_TABLE, /*!< one-dimensional function table */
	       CSM_2D_TABLE  /*!< two-dimensional function table */
249
	     } csm_func_type;
250
#endif
251

252
/*! \internal \brief compound type for functions */
253
struct csm_Function
254
  { SEM_TYPE semaphore;   /*!< semaphore to lock access to csm_function */
255
256
257
258
259
    double   last_x;      /*!< last x value that was calculated */
    double   last_y;      /*!< last y value that was calculated */
    double   last_dx;     /*!< last dx value that was calculated */
    double   last_dy;     /*!< last dy value that was calculated */
    double   last_z;      /*!< last z value that was calculated */
260
    csm_bool on_hold;     /*!< when TRUE, just return last */
261
    csm_func_type type;   /*!< the type of the function */
262

263
264
265
266
    union { csm_linear_function    lf;   /*!< linear function */
            csm_1d_functiontable   tf_1; /*!< 1d function table */
            csm_2d_functiontable   tf_2; /*!< 2d function table */
	  } f;            /*!< union that holds the actual data */
267
268

  };
franksen's avatar
franksen committed
269

270
271
272
/*____________________________________________________________*/
/*			  Variables			      */
/*____________________________________________________________*/
273

274
275
276
277
      /*................................................*/
      /*           initialization (private)             */
      /*................................................*/

278
279
280
281
282
/*! \internal \brief initialization flag [static]

  This variable is used to remember, wether the module
  was already initialized.
*/
283
static csm_bool initialized= CSM_FALSE;
franksen's avatar
franksen committed
284
285
286

/*____________________________________________________________*/
/*                        Functions                           */
287
/*____________________________________________________________*/
franksen's avatar
franksen committed
288
289
290


    /*----------------------------------------------------*/
291
    /*             debug - managment (private)            */
franksen's avatar
franksen committed
292
293
    /*----------------------------------------------------*/

294
295
#if USE_DBG
/*! \internal \brief initialize the DBG module [static]
franksen's avatar
franksen committed
296

297
298
  This internal function is called by csmbase_init
*/
franksen's avatar
franksen committed
299

300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
static void csm_dbg_init(void)
  { static csm_bool csmbase_init= CSM_FALSE;

    if (csmbase_init)
      return;
    csmbase_init= CSM_TRUE;
    /* initialize the debug (dbg) module: use stdout for error-output */
    DBG_SET_OUT(CSMBASE_OUT_FILE);
    DBG_TRC_LEVEL= CSMBASE_TRC_LEVEL;
                           /* level 6: dense traces for debugging
                              level 5: enter/exit tracing
                              level 2: output less severe errors and warnings
                              level 1: output of severe errors
                              level 0: no output */
    DBG_FLUSHMODE(CSMBASE_FLUSHMODE);
  }
316

317
#endif
franksen's avatar
franksen committed
318
319

    /*----------------------------------------------------*/
320
    /*        utilities for csm-coordinates (private)     */
franksen's avatar
franksen committed
321
322
323
324
    /*----------------------------------------------------*/

        /*              initialization                */

325
326
/*! \internal \brief initialize a csm_coordinates structure [static]

327
328
  This function initializes a csm_coordinates structure. This function
  should be called immediately after a variable of the type
329
330
331
332
  csm_coordinates was created.
  \param c pointer to the csm_coordinates array
*/
static void init_coordinates(csm_coordinates *c)
333
  {
334
335
336
337
338
339
340
341
    c->a_last=-1;
    c->b_last=-1;
    c->coordinate= NULL;
    c->no_of_elements=0;
  }

/*! \internal \brief initialize a csm_coordinates structure [static]

342
343
  This function allocates memory for a
  csm_coordinates structure.
344
345
346
347
348
  \param c pointer to the csm_coordinates structure
  \param elements number of elements in the structure
  \return returns CSM_FALSE in case of an error, CSM_TRUE otherwise
*/
static csm_bool alloc_coordinates(csm_coordinates *c, int elements)
franksen's avatar
franksen committed
349
350
  { int i;
    csm_coordinate *co;
351

pfeiffer's avatar
pfeiffer committed
352
    if (c->no_of_elements==0) /* 1st time memory allocation */
353
      c->coordinate= malloc(sizeof(csm_coordinate)*elements);
354
    else
355
356
      c->coordinate= realloc(c->coordinate,
                             sizeof(csm_coordinate)*elements);
357

358
    if (c->coordinate==NULL) /* allocation error */
359
      { DBG_MSG_PRINTF2("error in csm:alloc_coordinates line %d,\n"
360
361
362
363
364
                	"allocation failed!\n", __LINE__);
	init_coordinates(c);
	return(CSM_FALSE);
      };

365
366
    for(i=c->no_of_elements, co= (c->coordinate + c->no_of_elements);
        i<elements;
367
368
369
	i++, co++)
      { co->value=0;
	co->index=i;
franksen's avatar
franksen committed
370
      };
371

372
373
    c->no_of_elements= elements;
    return(CSM_TRUE);
374
  }
franksen's avatar
franksen committed
375

376
377
378
379
380
/*! \internal \brief resize a csm_coordinates structure [static]

  This function resizes a csm_coordinates structure. This means that
  the size of allocated memory is adapted to a new, different number
  of elements. Note that the existing data is not changed. If additional
381
  memory is allocated, it is initialized with zeroes.
382
383
384
385
386
  \param c pointer to the csm_coordinates structure
  \param elements new number of elements in the structure
  \return returns CSM_FALSE in case of an error, CSM_TRUE otherwise
*/
static csm_bool resize_coordinates(csm_coordinates *c, int elements)
franksen's avatar
franksen committed
387
  { if (elements==c->no_of_elements)
388
      return(CSM_TRUE);
389

franksen's avatar
franksen committed
390
391
392
    c->no_of_elements= elements;
    if (NULL== (c->coordinate= realloc(c->coordinate,
                                       sizeof(csm_coordinate)*elements)))
393
      { DBG_MSG_PRINTF2("error in csm:resize_coordinates line %d,\n" \
394
395
                        "realloc failed!\n", __LINE__);
        return(CSM_FALSE);
franksen's avatar
franksen committed
396
      };
397
    return(CSM_TRUE);
398
  }
399

400
/*! \internal
401
402
403
404
    \brief re-initialize a csm_coordinates structure [static]

  This re-initializes a csm_coordinates structure. This function
  is similar to init_coordinates, but already allocated memory is
405
  freed before the structure is filled with it's default values.
406
407
408
409
410
  \param c pointer to the csm_coordinates array
*/
static void reinit_coordinates(csm_coordinates *c)
  { if (c->no_of_elements==0)
      return;
411

412
413
    free(c->coordinate);
    init_coordinates(c);
414
  }
franksen's avatar
franksen committed
415

416
417
418
419
420
/*! \internal \brief initialize a matrix of double values [static]

  This function allocates an array of doubles in a way that it
  has enough room to hold all values of a rows*columns matrix.
  \param z address of pointer to array of double-numbers (output)
421
422
  \param rows number of rows
  \param columns number of columns
423
424
425
  \return returns CSM_FALSE in case of an error, CSM_TRUE otherwise
*/
static csm_bool init_matrix(double **z, int rows, int columns)
franksen's avatar
franksen committed
426
427
  { int i;
    double *zp;
428

franksen's avatar
franksen committed
429
    if (NULL== (zp= malloc((sizeof(double))*rows*columns)))
430
      { DBG_MSG_PRINTF2("error in csm:init_matrix line %d,\n" \
431
432
                        "malloc failed!\n", __LINE__);
        return(CSM_FALSE);
franksen's avatar
franksen committed
433
434
435
      };
    *z = zp;
    for (i= rows*columns; i>0; i--, *(zp++)=0);
436
    return(CSM_TRUE);
437
  }
franksen's avatar
franksen committed
438
439
440

        /*            x-compare for qsort             */

441
/*! \internal
442
    \brief compare two values within a csm_coordinate structure [static]
franksen's avatar
franksen committed
443

444
445
446
447
448
449
450
451
  This function compares two values within a cms_coordinate structure.
  It is needed in order to apply quicksort.
  \param t1 this is the pointer to the first value. It should be of the
            type csm_coordinate*.
  \param t2 this is the pointer to the second value. It should be of the
            type csm_coordinate*.
  \return returns -1 if *t1 < *t2, 0 if *t1 = *t2, 1 if *t1 > *t2
*/
franksen's avatar
franksen committed
452
453
454
static int coordinate_cmp(const void *t1, const void *t2)
  { double x1= ((csm_coordinate *)(t1))->value;
    double x2= ((csm_coordinate *)(t2))->value;
455

franksen's avatar
franksen committed
456
457
458
459
460
461
462
463
464
    if (x1 < x2)
      return(-1);
    if (x1 > x2)
      return( 1);
    return(0);
  }

        /*              coordinate-sort               */

465
466
467
468
469
470
/*! \internal \brief sort coordinates in a csm_coordinates structure [static]

  This function sorts the coordinates within a
  csm_coordinates structure according to their value field.
  \param coords pointer to the csm_coordinates structure
*/
franksen's avatar
franksen committed
471
472
static void coordinate_sort( csm_coordinates *coords)
  {
473
    qsort( coords->coordinate, coords->no_of_elements,
franksen's avatar
franksen committed
474
475
476
           sizeof(csm_coordinate), coordinate_cmp);
  }

477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
/*! \internal \brief update backlinks from on csm_coordinates structure to another [static]

  Two or more csm_coordinates structures can be connected by the
  index fields of their elements. The index field of each element
  in the first structure is then the index of the corresponding
  element in the second structure and vice versa. If the order
  of the element in one of the structures was changed, the index
  field in all elements of the second structure have to be updated,
  which is what this function does. It is usually called after
  sorting the first structure with coordinate_sort.
  \param coords1 pointer to the first csm_coordinates structure which
         was re-ordered.
  \param coords2 pointer to the second csm_coordinates structure which
         has to be updated.
*/
franksen's avatar
franksen committed
492
static void coordinate_update_backlinks( csm_coordinates *coords1,
493
				         csm_coordinates *coords2)
franksen's avatar
franksen committed
494
495
  { int i,l;
    csm_coordinate *c,*d;
496
497

    l= coords1->no_of_elements;
franksen's avatar
franksen committed
498
    for (i=0, c= coords1->coordinate, d= coords2->coordinate; i<l; i++, c++)
499
      { d[c->index].index = i; };
franksen's avatar
franksen committed
500
501
502
503
  }

        /*      lookup an index in coordinates        */

504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
/*! \internal \brief lookup nearest boundaries within a csm_coordinates structure [static]

  This searches for the nearest boundaries to a given x within a given
  csm_coordinates structure. It looks up the indices of two
  elements. For the first one,it is guaranteed that its value is
  smaller or equal to x and that no other element with a larger value
  has this property. For the second one, it is guaranteed that its value
  is bigger or equal to x and that no other element with a smaller value
  has this property. so
     coords->coordinate[a].value <= x <= coords->coordinate[b].value
  holds. If a or b cannot be found since x is at the border of the
  list of coordinates, they are set to -1.
  \param coords pointer to the first csm_coordinates structure
  \param x the value to find
  \param a pointer to the returned index of the smaller element
  \param b pointer to the returned index of the bigger element
  \return 2 if x was found, a is the corresponding index
          1 x was not found, but a and b define the closest interval around x
          0 x was not found at it is outside the closest interval a and b
            a or b may be -1
         -1 error
*/
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
526
527
528
529
530
531
532
533

#define ret(idx_a, idx_b, ret_code) \
  *a= idx_a;\
  *b= idx_b;\
  coords->a_last= idx_a;\
  coords->b_last= idx_b;\
  return(ret_code)
  
534
static int coordinate_lookup_index(csm_coordinates *coords,double x,
535
				   int *a, int *b)
franksen's avatar
franksen committed
536
537
538
539
540
541
542
543
544
  /* 2: x was found, it is returned in a
     1: x was not found, but a and b define the closest interval around x
     0: x was not found at it is outside the closest interval a and b
    -1: error
  */
  /* the table should be x-sorted */
  { int i, ia, ib;
    double xt;
    csm_coordinate *c= coords->coordinate;
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
545
546
547
548
    double x_boundary;
    int last_idx= coords->no_of_elements-1;
    int a_last, b_last;
    int tries;
549

franksen's avatar
franksen committed
550
551
    if (coords->no_of_elements<2)
      { if (coords->no_of_elements<1)
552
          {
553
            DBG_MSG_PRINTF2("error in csm:coordinate_lookup_index %d,\n" \
554
                            "table has less than 1 element!\n", __LINE__);
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
555
            ret(0,0,-1);
franksen's avatar
franksen committed
556
           };
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
557
        coords->a_last= 0; coords->b_last= 0; 
franksen's avatar
franksen committed
558
	if (x== c[0].value)
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
559
560
561
562
563
564
565
          {
            ret(0,0,2);
          }
        else
          {
            ret(0,0,0);
          }
franksen's avatar
franksen committed
566
567
      };

Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
    a_last= coords->a_last;
    b_last= coords->b_last;
    if (a_last<0)
      a_last= 0;
    if (b_last<0)
      b_last= last_idx;

    for(tries=1; tries<=3; tries++)
      {
        switch(tries)
          {
            case 1:
              break;
            case 2:
              b_last= a_last;
              a_last= a_last-1;
              break;
            case 3:
              b_last= a_last;
              a_last= 0;
              break;
          }

        x_boundary= c[a_last].value;
        if (x>x_boundary)
          break;
        if (x==x_boundary)
          {
            ret(a_last,a_last,2);
          }
        if (a_last==0)
          { /* left of area */
            ret(0,1, 0);
          }
      }

    for(tries=1; tries<=3; tries++)
      {
        switch(tries)
          {
            case 1:
              break;
            case 2:
              a_last= b_last;
              b_last= b_last+1;
              break;
            case 3:
              a_last= b_last;
              b_last= last_idx;
              break;
          }

        x_boundary= c[b_last].value;
        if (x<x_boundary)
          break;
        if (x==x_boundary)
          {
            ret(b_last,b_last,2);
          }
        if (b_last==last_idx)
          { /* right of area */
            ret(last_idx-1,last_idx, 0);
          }
      }
632

Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
633
634
635
636
637
    ia= a_last;
    ib= b_last;
    /* necessary condition here:
       c[ia].value<= x <= c[ib].value
     */
franksen's avatar
franksen committed
638
639
640
641
    for (; (ib>ia+1); )
      { i= (ib+ia)/2;
        xt= c[i].value;
        if (xt<x)
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
642
643
644
645
646
647
          {
            ia= i;
            continue;
          }
        if (xt>x)
          {
franksen's avatar
franksen committed
648
            ib= i;
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
649
650
651
652
            continue;
          }
        /* exact match */
        ret(i,i,2);
franksen's avatar
franksen committed
653
      };
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
654
    ret(ia,ib,1);
655
656
  }

657
658
659
660
661
    /*----------------------------------------------------*/
    /*     utilities for csm_linear_function (private)    */
    /*----------------------------------------------------*/

        /*               calculation                  */
franksen's avatar
franksen committed
662

663
664
665
/*! \internal \brief compute y from a given x [static]

  This function computes y when x is given for a linear function.
666
  A linear function is a
667
  \param p pointer to the linear function structure
668
  \param x the value of x
669
670
671
672
673
674
675
676
*/
static double linear_get_y(csm_linear_function *p, double x)
  { return( (p->a) + (p->b)*x ); }

/*! \internal \brief compute x from a given y [static]

  This function computes x when y is given for a linear function
  \param p pointer to the linear function structure
677
  \param y the value of y
678
679
680
681
682
683
684
685
*/
static double linear_get_x(csm_linear_function *p, double y)
  { return( (y - (p->a))/(p->b) ); }

/*! \internal \brief compute delta-y from a given delta-x [static]

  This function computes delta-y when delta-x is given for a linear function
  \param p pointer to the linear function structure
686
  \param x the value of x
687
688
689
690
691
692
693
694
*/
static double linear_delta_get_y(csm_linear_function *p, double x)
  { return( (p->b)*x ); }

/*! \internal \brief compute delta-x from a given delta-y [static]

  This function computes delta-x when delta-y is given for a linear function
  \param p pointer to the linear function structure
695
  \param y the value of y
696
697
698
699
700
701
702
703
704
705
*/
static double linear_delta_get_x(csm_linear_function *p, double y)
  { return( y/(p->b) ); }

    /*----------------------------------------------------*/
    /*    utilities for csm_1d_functiontable (private)    */
    /*----------------------------------------------------*/

        /*              initialization                */

706
707
708
709
710
711
712
/*! \internal \brief initialize a csm_1d_functiontable structure [static]

  This function initializes all members of the csm_1d_functiontable
  structure. This function should be called immediately after a
  variable of the type csm_1d_functiontable was created.
  \param ft pointer to the csm_1d_functiontable structure
*/
713
714
715
static void init_1d_functiontable(csm_1d_functiontable *ft)
  { init_coordinates(&(ft->x));
    init_coordinates(&(ft->y));
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
716
    ft->value_cached= CSM_FALSE;
717
718
  }

719
720
721
722
723
724
725
726
/*! \internal \brief reinitialize a csm_1d_functiontable structure [static]

  This function re-initializes the csm_coordinates members of the
  csm_1d_functiontable structure. This is similar to init_1d_functiontable,
  but already allocated memory is freed before the structures is filled
  with their default values.
  \param ft pointer to the csm_1d_functiontable structure
*/
727
728
729
static void reinit_1d_functiontable(csm_1d_functiontable *ft)
  { reinit_coordinates(&(ft->x));
    reinit_coordinates(&(ft->y));
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
730
    ft->value_cached= CSM_FALSE;
731
  }
732

733
        /*        lookup a value in the table         */
franksen's avatar
franksen committed
734

735
736
737
738
739
740
741
742
743
744
745
746
747
748
/*! \internal \brief lookup a value in a csm_1d_functiontable structure [static]

  This function looks up the function value y for a given x in a
  csm_1d_functiontable structure which is a one-dimensional function
  table. The two points x1 and x2 that are
  nearest to x are searched. A linear interpolation between the
  corresponding y1 and y2 values is done and this value is returned.
  If inverted lookup is wanted. the function looks up y1 and y2 for a
  given y and does a linear interpolation between x1 and x2.
  \param ft pointer to the csm_1d_functiontable structure
  \param x the value to look up
  \param invert if CSM_TRUE, an inverted lookup is done
  \return the value that fits best to x is returned.
*/
franksen's avatar
franksen committed
749
static double lookup_1d_functiontable(csm_1d_functiontable *ft, double x,
750
				      csm_bool invert)
franksen's avatar
franksen committed
751
752
753
  { int res,a,b;
    double xa,xb,ya,yb;
    csm_coordinate c;
754
    csm_coordinates *xcoords;
franksen's avatar
franksen committed
755
756
757
    csm_coordinates *ycoords;

    if (!invert)
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
758
759
760
761
      { 
        if ((ft->value_cached) && (ft->x_last==x))
          return(ft->y_last);
        xcoords= &(ft->x);
franksen's avatar
franksen committed
762
763
764
        ycoords= &(ft->y);
      }
    else
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
765
766
767
768
      { 
        if ((ft->value_cached) && (ft->y_last==x))
          return(ft->x_last);
        xcoords= &(ft->y);
franksen's avatar
franksen committed
769
770
        ycoords= &(ft->x);
      };
771

franksen's avatar
franksen committed
772
773
774
775
    res= coordinate_lookup_index(xcoords, x, &a, &b);


    if (res==-1) /* error */
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
776
777
778
779
      {
        ft->value_cached= CSM_FALSE;
        return(NAN);
      }
780
781
782
783
784
    if ((res== 2)||(a==b)) /* exact match or table with just a single value */
      { 
        /* if the table has just a single value (only one [x,y] pair) we return
           the corresponding y value. */
        c= (xcoords->coordinate)[a];
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
785
786
787
788
789
790
791
792
793
794
795
        ft->value_cached= CSM_TRUE;
        if (!invert)
          {
            ft->x_last= x;
            return(ft->y_last= (ycoords->coordinate)[c.index].value);
          }
        else
          {
            ft->y_last= x;
            return(ft->x_last= (ycoords->coordinate)[c.index].value);
          }
796
797
      };

franksen's avatar
franksen committed
798
799
800
    c= (xcoords->coordinate)[a];
    xa= c.value;
    ya= (ycoords->coordinate)[c.index].value;
801

franksen's avatar
franksen committed
802
803
804
    c= (xcoords->coordinate)[b];
    xb= c.value;
    yb= (ycoords->coordinate)[c.index].value;
805

Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
    ft->value_cached= CSM_TRUE;

    if (!invert)
      {
        ft->x_last= x;
        /* y= ya + (x-xa)/(xb-xa)*(yb-ya) */
        return(ft->y_last= ya + (x-xa)/(xb-xa) * (yb-ya));
      }
    else
      {
        ft->y_last= x;
        /* y= ya + (x-xa)/(xb-xa)*(yb-ya) */
        return(ft->x_last= ya + (x-xa)/(xb-xa) * (yb-ya));
      }

franksen's avatar
franksen committed
821
822
  }

823
824
825
    /*----------------------------------------------------*/
    /*    utilities for csm_2d_functiontable (private)    */
    /*----------------------------------------------------*/
franksen's avatar
franksen committed
826

827
828
        /*              initialization                */

829
830
831
832
833
834
835
/*! \internal \brief initialize a csm_2d_functiontable structure [static]

  This function initializes all members of the csm_2d_functiontable
  structure. This function should be called immediately after a
  variable of the type csm_2d_functiontable was created.
  \param ft pointer to the csm_2d_functiontable structure
*/
836
837
838
static void init_2d_functiontable(csm_2d_functiontable *ft)
  { init_coordinates(&(ft->x));
    init_coordinates(&(ft->y));
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
839
    ft->value_cached= CSM_FALSE;
840
841
  }

842
843
844
845
846
847
848
849
/*! \internal \brief reinitialize a csm_2d_functiontable structure [static]

  This function re-initializes the csm_coordinates members of the
  csm_2d_functiontable structure. This is similar to init_2d_functiontable,
  but already allocated memory is freed before the structures is filled
  with their default values.
  \param ft pointer to the csm_2d_functiontable structure
*/
850
851
852
853
854
855
static void reinit_2d_functiontable(csm_2d_functiontable *ft)
  { reinit_coordinates(&(ft->x));
    reinit_coordinates(&(ft->y));
    if (ft->z!=NULL)
      free(ft->z);
    ft->z= NULL;
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
856
    ft->value_cached= CSM_FALSE;
857
858
859
  }

        /*       lookup a value in the xy-table       */
franksen's avatar
franksen committed
860

861
862
863
864
865
866
867
868
869
870
871
872
/*! \internal \brief lookup a value in a csm_2d_functiontable structure [static]

  This function looks up the function value z for a given x and y in a
  csm_2d_functiontable structure which is a two-dimensional function
  table. The four points (x1,y1), (x1,y2), (x2,y1) and (x2,y2) that are
  closest to (x,y) are searched. A two-dimensional linear interpolation
  between four corresponding z values is done and this value is returned.
  \param ft pointer to the csm_2d_functiontable structure
  \param x x coordinate of the point to look up
  \param y y coordinate of the point to look up
  \return the value that fits best to (x,y) is returned.
*/
873
static double lookup_2d_functiontable(csm_2d_functiontable *ft,
874
				      double x, double y)
franksen's avatar
franksen committed
875
876
877
878
879
880
881
  { int res,xia,xib,yia,yib;
    double xa,xb,ya,yb;
    double zaa,zab,zba,zbb;
    double *z= ft->z;
    int no_of_columns;

    csm_coordinate cxa,cxb,cya,cyb;
882
    csm_coordinates *xcoords= &(ft->x);
franksen's avatar
franksen committed
883
884
    csm_coordinates *ycoords= &(ft->y);

Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
885
886
887
888
889
890
    if (ft->value_cached)
      {
        if ((ft->x_last==x) && (ft->y_last==y))
          return(ft->z_last);
      }

franksen's avatar
franksen committed
891
892
893
894
    no_of_columns= ycoords->no_of_elements;

    res= coordinate_lookup_index(xcoords, x, &xia, &xib);
    if (res==-1) /* error */
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
895
896
897
898
      {
        ft->value_cached= CSM_FALSE;
        return(NAN);
      }
franksen's avatar
franksen committed
899
900
901

    res= coordinate_lookup_index(ycoords, y, &yia, &yib);
    if (res==-1) /* error */
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
902
903
904
905
      {
        ft->value_cached= CSM_FALSE;
        return(NAN);
      }
franksen's avatar
franksen committed
906
907
908
909
910
911
912

    /* res==2 with exact match is currently not treated separately */

    cxa= (xcoords->coordinate)[xia];
    cxb= (xcoords->coordinate)[xib];
    cya= (ycoords->coordinate)[yia];
    cyb= (ycoords->coordinate)[yib];
913

franksen's avatar
franksen committed
914
915
916
917
    xa= cxa.value; /* with exact match, xa==xb or ya==yb is possible */
    xb= cxb.value;
    ya= cya.value;
    yb= cyb.value;
918

franksen's avatar
franksen committed
919
920
921
922
923
924
925
926
927
928
929
930
931
    zaa= z[ cxa.index * no_of_columns + cya.index ];
    zab= z[ cxa.index * no_of_columns + cyb.index ];
    zba= z[ cxb.index * no_of_columns + cya.index ];
    zbb= z[ cxb.index * no_of_columns + cyb.index ];

#if 0
printf("x:%f y:%f xa:%f xb:%f ya:%f yb:%f\n",x,y,xa,xb,ya,yb);
printf("zaa:%f zab:%f zba:%f zbb:%f\n",zaa,zab,zba,zbb);
#endif
#if 1
    { double alpha = (xb==xa) ? 0 : (x-xa)/(xb-xa);
      double beta  = (yb==ya) ? 0 : (y-ya)/(yb-ya);
      if (alpha==0)               /* xb==xa ? */
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
932
933
934
935
936
937
938
        { 
          ft->value_cached= CSM_TRUE;
          ft->x_last= x;
          ft->y_last= y;
          if (beta==0)            /* yb==ya ? */
	    return(ft->z_last= zaa);
	  return(ft->z_last= zaa+ beta *(zab-zaa) );
franksen's avatar
franksen committed
939
940
	};
      if (beta==0)                /* yb==ya ? */
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
941
942
943
944
945
946
        {
          ft->value_cached= CSM_TRUE;
          ft->x_last= x;
          ft->y_last= y;
          return(ft->z_last= zaa+ alpha*(zba-zaa) );
        }
947

franksen's avatar
franksen committed
948
949
#if 0
printf(" ret: %f\n",
950
       zaa + beta*(zab-zaa)+alpha*(zba-zaa+beta*(zbb-zba-zab+zaa)));
franksen's avatar
franksen committed
951
#endif
Pfeiffer, Götz's avatar
Pfeiffer, Götz committed
952
953
954
955
956
      ft->value_cached= CSM_TRUE;
      ft->x_last= x;
      ft->y_last= y;
      return(ft->z_last= 
              zaa + beta*(zab-zaa)+alpha*(zba-zaa+beta*(zbb-zba-zab+zaa)) );
franksen's avatar
franksen committed
957
    };
958
#endif
franksen's avatar
franksen committed
959
960
961
962
963
964
965
966
967
968
969
970
971

#if 0
    { /* alternative according to formula given by J.Bahrdt */
      /* doesn't work when yb==ya or xb==xa */
      double delta= (yb-ya)*(xb-xa);
      double yb_  = yb - y;
      double xb_  = xb - x;
      double y_a  = y  - ya;
      double x_a  = x  - xa;
      return((zaa*yb_*xb_ + zab*y_a*xb_ + zba*yb_*x_a + zbb*y_a*x_a)/delta);
#endif

 }
972

franksen's avatar
franksen committed
973
    /*----------------------------------------------------*/
974
    /*              generic functions (public)            */
franksen's avatar
franksen committed
975
976
    /*----------------------------------------------------*/

977
double csm_x(csm_function *func, double y)
978
  { double ret;
979

980
    SEMTAKE(func->semaphore);
981
982

    if (func->on_hold)
983
984
985
      { ret= func->last_x;
        SEMGIVE(func->semaphore);
        return(ret);
986
987
988
989
      };

    switch (func->type)
      { case CSM_NOTHING:
990
               func->last_x= 0;
991
992
	       break;
	case CSM_LINEAR:
993
994
	       func->last_x= linear_get_x(&(func->f.lf), y);
	       break;
995
	case CSM_1D_TABLE:
996
	       func->last_x= lookup_1d_functiontable(&(func->f.tf_1),y,
997
	       					   CSM_TRUE);
998
	       break;
999
        default:
1000
	       func->last_x=0;
For faster browsing, not all history is shown. View entire blame