/[ascend]/trunk/models/johnpye/datareader/dr.c
ViewVC logotype

Contents of /trunk/models/johnpye/datareader/dr.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 2504 - (show annotations) (download) (as text)
Sat Oct 1 06:18:31 2011 UTC (12 years, 9 months ago) by jpye
File MIME type: text/x-csrc
File size: 23689 byte(s)
Working on adding support for EnergyPlus/ESP-r 'E/E' weather data format. Ongoing.
1 /* ASCEND modelling environment
2 Copyright (C) 2006 Carnegie Mellon University
3
4 This program is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2, or (at your option)
7 any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with this program; if not, write to the Free Software
16 Foundation, Inc., 59 Temple Place - Suite 330,
17 Boston, MA 02111-1307, USA.
18 */
19 #include <stdlib.h>
20 #include <errno.h>
21 #include <math.h>
22
23 #include "dr.h"
24 #include "tmy.h"
25 #include "acdb.h"
26 #include "csv.h"
27 #include "ee.h"
28
29 #include <ascend/utilities/config.h>
30 #include <ascend/general/ospath.h>
31 #include <ascend/general/ascMalloc.h>
32 #include <ascend/utilities/error.h>
33 #include <ascend/general/panic.h>
34 #include <ascend/utilities/ascEnvVar.h>
35
36 #define DR_DEBUG 0
37
38 /*------------------------------------------------------------------------------
39 FORWARD DECLARATIONS
40 */
41
42 /*
43 declare the possible formats that we will accept ('format' child in the
44 DATA instance of the external relation
45 */
46
47 #define FMTS(D,X) D(TMY2) X D(ACDB) X D(CSV) X D(EE) X D(TDV)
48
49 #define ENUM(F_) DATAREADER_FORMAT_##F_
50 #define COMMA ,
51 typedef enum {
52 FMTS(ENUM, COMMA),
53 DATAREADER_INVALID_FORMAT
54 } DataReaderFormat;
55 #undef ENUM
56 #undef COMMA
57
58 /*------------------------------------------------------------------------------
59 API FUNCTIONS
60 */
61
62 /**
63 Create a data reader object, with the filename specified. The filename
64 will be searched for in a specified path, eg ASCENDLIBRARY.
65 */
66 DataReader *datareader_new(const char *fn, int noutputs) {
67 DataReader *d;
68 int i;
69
70 d = ASC_NEW(DataReader);
71 d->fn = fn;
72 d->fp = NULL;
73 d->f = NULL;
74 d->noutputs = noutputs; //maybe this is not the right place to put this!
75
76 //create a data allocation for the parameter list
77 d->cols = (int *)ascmalloc(noutputs*sizeof(int));
78 d->interp_t = (interp_t *)ascmalloc(noutputs*sizeof(interp_t));
79 //initialise param lists with default values. In case user doesn't declare params
80 for (i=0;i<noutputs;i++) {
81 d->cols[i] = i+1;
82 d->interp_t[i] = default_interp;
83 }
84 d->a0 = (double *)ascmalloc(noutputs*sizeof(double));
85 d->a1 = (double *)ascmalloc(noutputs*sizeof(double));
86 d->a2 = (double *)ascmalloc(noutputs*sizeof(double));
87 d->a3 = (double *)ascmalloc(noutputs*sizeof(double));
88
89 d->datafn = NULL;
90 d->headerfn = NULL;
91 d->eoffn = NULL;
92
93 CONSOLE_DEBUG("Datareader created...");
94 return d;
95 }
96 /**read the parameter token and return an interpolation type.
97 @param interpToken the interpolation string, as declared in the model
98 @return interp_t the model type
99 **/
100
101 interp_t datareader_int_type(const char *interpToken) {
102 if (strcmp(interpToken,"default")==0) {
103 return default_interp; //user has declared default interpolation
104 }
105 if (strcmp(interpToken,"linear")==0) {
106 return linear; //user has declared linear interpolation
107 }
108 if (strcmp(interpToken,"cubic")==0) {
109 return cubic; //usen has declared cubic interpolation
110 }
111 if (strcmp(interpToken,"sun")==0) {
112 return sun; //user has declared sun interpolation
113 }
114 /* if we got here, used did not declare a valid interpolation type */
115 return default_interp;
116 }
117
118 /** Set datareader parameters.
119 This function reads the parameters as declared in the model file and
120 fills out the cols and interp_t fields in the datareader structure.
121
122 This process happens after datareader creation so that the right amount
123 of memory is allocated for cols and interp_t. The other reason to create
124 a separate function is that an integer with the result of this operation
125 can be returned
126 @param d the datareader object
127 @param par the parameter string passed from the drconfig model
128 @return 0 on sucess
129 **/
130 int datareader_set_parameters(DataReader *d, const char *parameters) {
131 char *partok = NULL;
132 int parcount = 0;
133 boolean LastTokWasNumeric = FALSE;//keep track of token types
134 partok = strtok(parameters,",:");
135 while (partok !=NULL) { //cycle through the parameter string
136 if (strpbrk(partok,"1234567890")!=NULL) { //if the token is numeric
137 if (LastTokWasNumeric) parcount++; //last was numeric, no interp_t declared
138 d->cols[parcount] = atoi(partok);//assign to corresponding element array
139 if (d->cols[parcount] > d->nmaxoutputs) {
140 CONSOLE_DEBUG("col %d, max %d",d->cols[parcount],d->nmaxoutputs);
141 ERROR_REPORTER_HERE(ASC_USER_ERROR,
142 "Requested Column out of range!,check your data file and model declaration");
143 return 1; //failed due to column out of range
144 }
145 LastTokWasNumeric =TRUE; //keep track of numeric tokens
146 }
147 else {
148 d->interp_t[parcount] = datareader_int_type(partok); //get type from token
149 if (LastTokWasNumeric && (parcount +1 < d->noutputs)) parcount++;//last par was column number
150 LastTokWasNumeric = FALSE;//keep track of numeric tokens
151 }
152 partok = strtok(NULL,",:"); //reread parameter string for next token
153 }
154 CONSOLE_DEBUG("parcount: %d,noutoputs: %d",parcount,d->noutputs);
155 if (parcount+1 != d->noutputs) {
156 ERROR_REPORTER_HERE(ASC_USER_ERROR,
157 "Number of Columns in parameters and Model dont match, check model declaration");
158 return 1;
159 }
160 /*Check for model variables exceeding the allowable by data file.
161 This condition will check that the user has declared a number of model
162 variables less or equal to the variables allowed by the data file. This is
163 different from the parameter check where the user is requesting a column out
164 of range.
165
166 One could argue that the user might like to declare several model variables
167 linked to the same data column, but a decision has been made by the developer
168 to restrict this, as it is thought as superfluous for most modelling scenarios.
169
170 */
171 if (d->noutputs > d->nmaxoutputs) {
172 ERROR_REPORTER_HERE(ASC_USER_ERROR,"Numbef of model variables exceeds number of data colums, check your model");
173 return 1;
174 }
175 return 0; //sucess.
176 }
177
178 /**
179 Set data file format
180 @return 0 on success
181 */
182 int datareader_set_format(DataReader *d, const char *format) {
183
184 #define STR(N) #N
185 #define COMMA ,
186 const char *fmts[] = { FMTS(STR, COMMA) };
187 #undef STR
188 #undef COMMA
189
190 int i;
191
192 CONSOLE_DEBUG("FORMAT '%s'", format);
193
194 DataReaderFormat found = DATAREADER_INVALID_FORMAT;
195 for (i = 0; i < DATAREADER_INVALID_FORMAT; ++i) {
196 if (strcmp(format, fmts[i]) == 0) {
197 found = (DataReaderFormat)i;
198 break;
199 }
200 }
201
202 CONSOLE_DEBUG("FOUND DATA FORMAT %d", found);
203
204 switch (found) {
205 case DATAREADER_FORMAT_TMY2:
206 d->headerfn = &datareader_tmy2_header;
207 d->datafn = &datareader_tmy2_data;
208 d->eoffn = &datareader_tmy2_eof;
209 d->indepfn = &datareader_tmy2_time;
210 d->valfn = &datareader_tmy2_vals;
211 break;
212 case DATAREADER_FORMAT_ACDB:
213 d->headerfn = &datareader_acdb_header;
214 d->datafn = &datareader_acdb_data;
215 d->eoffn = &datareader_acdb_eof;
216 d->indepfn = &datareader_acdb_time;
217 d->valfn = &datareader_acdb_vals;
218 break;
219 case DATAREADER_FORMAT_CSV:
220 d->headerfn = &datareader_csv_header;
221 d->datafn = &datareader_csv_data;
222 d->eoffn = &datareader_csv_eof;
223 d->indepfn = &datareader_csv_time;
224 d->valfn = &datareader_csv_vals;
225 break;
226 case DATAREADER_FORMAT_EE:
227 d->headerfn = &datareader_ee_header;
228 d->datafn = &datareader_ee_data;
229 d->eoffn = &datareader_ee_eof;
230 d->indepfn = &datareader_ee_time;
231 d->valfn = &datareader_ee_vals;
232 break;
233 case DATAREADER_FORMAT_TDV:
234 ERROR_REPORTER_HERE(ASC_USER_ERROR, "Tab delimited values (TDV) format not yet implemenented.");
235 return 1;
236 default:
237 ERROR_REPORTER_HERE(ASC_USER_ERROR, "Unknown file format '%s' specified", format);
238 return 1;
239 }
240
241 return 0;
242 }
243
244 typedef struct DataFileSearchData_struct {
245 struct FilePath *fp; /**< the relative path we're searching for */
246 ospath_stat_t buf; /**< saves memory allocation in the 'test' fn */
247 int error; /**< error code (in case stat or fopen failed) */
248 struct FilePath *fp_found; /**< the full path we found */
249 } DataFileSearchData;
250
251 FilePathTestFn datareader_searchpath_test;
252
253 /**
254 @return 1 on success
255 */
256 int datareader_searchpath_test(struct FilePath *path, void *searchdata) {
257 struct FilePath *fp1;
258 DataFileSearchData *sd;
259
260 sd = (DataFileSearchData *)searchdata;
261 assert(sd != NULL);
262 assert(sd->fp != NULL);
263
264 fp1 = ospath_concat(path, sd->fp);
265 if (fp1 == NULL) {
266 CONSOLE_DEBUG("Couldn't concatenate path");
267 return 0;
268 }
269
270 if (ospath_stat(fp1, &sd->buf)) {
271 sd->error = errno;
272 ospath_free(fp1);
273 return 0;
274 }
275
276 sd->fp_found = fp1;
277 return 1;
278 };
279
280
281 /**
282 Initialise the datareader: open the file, check the number of columns, etc.
283 @return 0 on success
284
285 @TODO search for the file in the ASCENDLIBRARY if not found immediately
286 */
287 int datareader_init(DataReader *d) {
288 ospath_stat_t s;
289 char *tmp;
290 struct FilePath **sp1, *fp2;
291 DataFileSearchData sd;
292
293 d->fp = ospath_new(d->fn);
294 if (d->fp == NULL) {
295 ERROR_REPORTER_HERE(ASC_USER_ERROR, "Invalid filepath");
296 return 1;
297 }
298
299 if (ospath_stat(d->fp, &s)) {
300 if (errno == ENOENT) {
301 /* file doesn't exist: check the search path instead */
302 tmp = Asc_GetEnv(ASC_ENV_LIBRARY);
303 if (tmp == NULL) {
304 ERROR_REPORTER_HERE(ASC_PROG_ERROR, "No paths to search (is env var '%s' set?)", ASC_ENV_LIBRARY);
305 return 1;
306 }
307
308 sp1 = ospath_searchpath_new(tmp);
309 if (sp1 == NULL) {
310 ERROR_REPORTER_HERE(ASC_PROG_ERROR, "Unable to process %s value '%s'", ASC_ENV_LIBRARY, tmp);
311 /* memory error */
312 ascfree(tmp);
313 return -3;
314 }
315 ascfree(tmp);
316
317 sd.fp = d->fp;
318
319 fp2 = ospath_searchpath_iterate(sp1, &datareader_searchpath_test, &sd);
320
321 if (fp2 == NULL) {
322 ERROR_REPORTER_HERE(ASC_USER_ERROR, "File '%s' not found in search path (error %d)", d->fn, sd.error);
323 ospath_searchpath_free(sp1);
324 return -1;
325 }
326
327 ospath_searchpath_free(sp1);
328
329 /* replace our relative path with an absolute one */
330 ospath_free(d->fp);
331 d->fp = sd.fp_found;
332
333 } else {
334 ERROR_REPORTER_HERE(ASC_USER_ERROR, "The file '%s' cannot be accessed.\n"
335 "Check the file privileges, or try specifying an absolute path.", d->fn
336 );
337 return 1;
338 }
339 }
340 #if DR_DEBUG
341 CONSOLE_DEBUG("About to open the data file");
342 #endif
343 d->f = ospath_fopen(d->fp, "r");
344 if (d->f == NULL) {
345 ERROR_REPORTER_HERE(ASC_USER_ERROR, "Unable to open file '%s' for reading.", d->fn);
346 return 1;
347 }
348 #if DR_DEBUG
349 CONSOLE_DEBUG("Data file open ok");
350 #endif
351 asc_assert(d->headerfn);
352 asc_assert(d->eoffn);
353 asc_assert(d->datafn);
354
355 if ((*d->headerfn)(d)) {
356 ERROR_REPORTER_HERE(ASC_PROG_ERR, "Error processing file header in '%s'", d->fn);
357 fclose(d->f);
358 return 1;
359 }
360
361 while (! (*d->eoffn)(d)) {
362 if ((*d->datafn)(d)) {
363 ERROR_REPORTER_HERE(ASC_PROG_ERR, "Error reading file data in '%s'", d->fn);
364 fclose(d->f);
365 return 1;
366 }
367 }
368 #if DR_DEBUG
369 CONSOLE_DEBUG("Done retrieving data");
370 #endif
371 fclose(d->f);
372 #if DR_DEBUG
373 CONSOLE_DEBUG("Closed file");
374 #endif
375
376 d->i = 0; /* set current position to zero */
377
378 /* these values are set to ensure that the polynomial coefficients
379 are at least computed once*/
380 d->prev_i_val = -1;
381 d->prev_i_der = -1;
382 return 0;
383 }
384
385 /**
386 Shut down the data reader and deallocate any memory owned by it, then
387 free the memory at d.
388 */
389 int datareader_delete(DataReader *d) {
390 if (d->fp) {
391 ospath_free(d->fp);
392 d->fp = NULL;
393 }
394 if (d->f) {
395 fclose(d->f);
396 d->f = NULL;
397 }
398 ASC_FREE(d);
399 return 0;
400 }
401
402 /**
403 Return the number of inputs (independent variables) supplied in the
404 DataReader's current file. Can only be 1 at this stage.
405 */
406 int datareader_num_inputs(const DataReader *d) {
407 return d->ninputs;
408 }
409
410 /**
411 Return the number of outputs (dependent variables) found in the DataReader's
412 current file. Should be one or more.
413 */
414 int datareader_num_outputs(const DataReader *d) {
415 return d->noutputs;
416 }
417
418
419 int datareader_locate(DataReader *d, double t, double *t1, double *t2) {
420 (*d->indepfn)(d, t1);
421 if (*t1 > t && d->i > 0) {
422 /* start of current interval is too late */
423 do {
424 --(d->i);
425 (*d->indepfn)(d, t1);
426 /*CONSOLE_DEBUG("STEPPING BACK (d->i = %d currently), t1=%lf",d->i, *t1); */
427 } while (*t1 > t && d->i > 0);
428 }
429 /* now either d->i==0 or t1 < t*/
430 /* CONSOLE_DEBUG("d->i==%d, t1=%lf",d->i,*t1); */
431 ++d->i;
432 (*d->indepfn)(d, t2);
433 if (*t2 <= t) {
434 /* end of current interface is too early */
435 do {
436 /* CONSOLE_DEBUG("STEPPING FORWARD (d->i = %d currently), t1=%lf, t2=%lf",d->i,*t1,*t2); */
437 (*d->indepfn)(d, t1);
438 ++(d->i);
439 (*d->indepfn)(d, t2);
440 } while (*t2 < t && d->i < d->ndata);
441 }
442 #if DR_DEBUG
443 CONSOLE_DEBUG("d->i==%d, t1[0] = %lf, t2[0] = %lf", d->i, t1[0], t2[0]);
444 #endif
445
446 if (d->i == d->ndata || d->i == 0) {
447 return 1;
448 }
449
450 /* CONSOLE_DEBUG("INTERVAL OK"); */
451 return 0;
452 }
453
454 /**
455 Return an interpolated set of output values for the given input values.
456 This is computed according to user defined parameters.
457
458 The required memory for the inputs and outputs must be allocated by the
459 caller, and indicated by the pointers 'inputs' and 'outputs'.
460
461 @see datareader_deriv
462 @TODO implement this
463 */
464 int datareader_func(DataReader *d, double *inputs, double *outputs) {
465 boolean AtStart, AtEnd; //keep track of dataset ends, they affect constrained spline calculations
466 int i,j;
467 double t1[1], t2[1];
468 double v0[d->nmaxoutputs], v1[d->nmaxoutputs], v2[d->nmaxoutputs], v3[d->nmaxoutputs];
469 /** this is a fixed size...
470 the maximum size determined by the format handler, which is included in the datareader struct.
471 @TODO the TmyDataPoint has the output variables declared as float, why?**/
472
473 double t = inputs[0];
474
475 #if DR_DEBUG
476 CONSOLE_DEBUG("EVALUATING AT t = %lf", inputs[0]);
477 #endif
478
479 asc_assert(d->indepfn);
480
481 if (datareader_locate(d, t, t1, t2)) {
482 CONSOLE_DEBUG("LOCATION ERROR");
483 ERROR_REPORTER_HERE(ASC_USER_ERROR, "Time value t=%f is out of range", t);
484 return 1;
485 }
486
487 #if DR_DEBUG
488 CONSOLE_DEBUG("LOCATED AT t1 = %lf, t2 = %lf", t1[0], t2[0]);
489 #endif
490 if (d->i < d->ndata-1) {
491 ++d->i; //go one step forward
492 (*d->valfn)(d, v3); //take a data sample at t1+2
493 --d->i; //go back one step
494 AtEnd = FALSE; //index is not at the end of the dataset
495 }
496 else AtEnd = TRUE;
497
498 (*d->valfn)(d, v2);
499 --d->i;
500 (*d->valfn)(d, v1);
501
502 if (d->i > 0) {
503 --d->i; //go one step backward
504 (*d->valfn)(d, v0); //take a data sample at t1-1
505 ++d->i; //should be positioned at v1 t1
506 AtStart = FALSE;
507 }
508 else AtStart = TRUE;
509
510 #if DR_DEBUG
511 CONSOLE_DEBUG("LOCATED OK, d->i = %d, t1 = %lf, t2 = %lf, v1=%lf, v2=%lf", d->i, t1[0], t2[0], v1[0], v2[0]);
512 #endif
513
514 for (i = 0;i < d->noutputs;++i) {
515 j = d->cols[i]-1;
516 switch (d->interp_t[i]) {
517 case linear:
518 outputs[i] = dr_linearinterp(t,t1,t2,v1[j],v2[j]);
519 break;
520 case default_interp:
521 case sun: //to be implemented as a refinement of the cubic spline
522 case cubic:
523 outputs[i] = dr_cubicinterp(d,i,t,t1,t2,v0[j],v1[j],v2[j],v3[j]);
524
525 break;
526 }
527 #if DR_DEBUG
528 CONSOLE_DEBUG("[%d]: START = %lf, END = %lf, VALUE=%lf", i, v1[j],v2[j], outputs[i]);
529 #endif
530 }
531
532 return 0;
533 }
534
535 /**
536 Return an interpolated set of output derivatives for the given input
537 values. These can be smooth if the cubic interpolation method is selected.
538 */
539 int datareader_deriv(DataReader *d, double *inputs, double *jacobian) {
540 boolean AtStart, AtEnd; //keep track of dataset ends, they affect constrained spline calculations
541 int i,j;
542 double t1[1], t2[1];
543 double v0[d->nmaxoutputs], v1[d->nmaxoutputs], v2[d->nmaxoutputs], v3[d->nmaxoutputs];
544 /** this is a fixed size...
545 the maximum size determined by the format handler, which is included in the datareader struct.
546 @TODO the TmyDataPoint has the output variables declared as float, why?**/
547
548 double t = inputs[0];
549
550 #if DR_DEBUG
551 CONSOLE_DEBUG("EVALUATING AT t = %lf", inputs[0]);
552 #endif
553
554 asc_assert(d->indepfn);
555
556 if (datareader_locate(d, t, t1, t2)) {
557 CONSOLE_DEBUG("LOCATION ERROR");
558 ERROR_REPORTER_HERE(ASC_USER_ERROR, "Time value t=%f is out of range", t);
559 return 1;
560 }
561
562 #if DR_DEBUG
563 CONSOLE_DEBUG("LOCATED AT t1 = %lf, t2 = %lf", t1[0], t2[0]);
564 #endif
565 if (d->i < d->ndata-1) {
566 ++d->i; //go one step forward
567 (*d->valfn)(d, v3); //take a data sample at t1+2
568 --d->i; //go back one step
569 AtEnd = FALSE; //index is not at the end of the dataset
570 }
571 else AtEnd = TRUE;
572
573 (*d->valfn)(d, v2);
574 --d->i;
575 (*d->valfn)(d, v1);
576
577 if (d->i > 0) {
578 --d->i; //go one step backward
579 (*d->valfn)(d, v0); //take a data sample at t1-1
580 ++d->i; //should be positioned at v1 t1
581 AtStart = FALSE;
582 }
583 else AtStart = TRUE;
584
585 #if DR_DEBUG
586 CONSOLE_DEBUG("LOCATED OK, d->i = %d, t1 = %lf, t2 = %lf, v1=%lf, v2=%lf", d->i, t1[0], t2[0], v1[0], v2[0]);
587 #endif
588
589 for (i = 0;i < d->noutputs;++i) {
590 j = d->cols[i]-1;
591 switch (d->interp_t[i]) {
592 case linear:
593 jacobian[i] = dr_linearderiv(t,t1,t2,v1[j],v2[j]);
594 break;
595 case default_interp:
596 case sun: //to be implemented as a refinement of the cubic spline
597 case cubic:
598 jacobian[i] = dr_cubicderiv(d,i,t,t1,t2,v0[j],v1[j],v2[j],v3[j]);
599
600 break;
601 }
602 #if DR_DEBUG
603 CONSOLE_DEBUG("[%d]: START = %lf, VALUE=%lf", i, v1[j], jacobian[i]);
604 #endif
605 }
606
607 return 0;
608 }
609
610 double dr_linearinterp(double t, double *t1, double *t2, double v1, double v2) {
611 double g, dt;
612 dt = (*t2)-(*t1);
613 g = (v2 - v1) / dt;
614 return v1 + g * (t - (*t1));
615 }
616 double dr_cubicinterp(DataReader *d, int j, double t, double *t1, double *t2, double v0, double v1, double v2, double v3) {
617 //constrained cubic spline by CJC Kruger http://www.korf.co.uk
618 boolean AtStart, AtEnd;
619 double dt;
620 double k0,k1,k2,k3; //gradients for cubic spline interpolation
621 double a0,a1,a2,a3;//polynomial coefficients for second spline segment
622 dt = t2[0] - t1[0];
623
624 if (d->i != d->prev_i_val) {
625 /*if we are at a new interval, still wait for all coefficients to be
626 calculated before updating prev_i_val index */
627 if (j == d->noutputs-1) d->prev_i_val = d->i;
628
629 if (d->i < d->ndata-1) AtEnd = FALSE; //index is not at the end of the dataset
630 else AtEnd = TRUE; //index is at the end of the dataset
631 if (d->i > 0) AtStart = FALSE; //index is not at the beggining of the dataset
632 else AtStart = TRUE; //index is at the begginintg of the dataset
633
634 if ((dt/(v3-v2)+dt/(v2-v1))==0 || (v3-v2)*(v2-v1)<0) k2 = 0;
635 else {
636 if (AtEnd) k2 = 3*(v2-v1)/2-1/(dt/(v2-v1)+dt/(v1-v0));
637 else k2 = 2/(dt/(v3-v2)+dt/(v2-v1));
638 }
639 if ((dt/(v2-v1)+dt/(v1-v0))==0 || (v2-v1)*(v1-v0)<0) k1 = 0;
640 else {
641 if (AtStart) k1 = 3*(v2-v1)/2 - k2/2;
642 else k1 = 2/(dt/(v2-v1)+dt/(v1-v0));
643 }
644 k0 = -2*(k2+2*k1)/dt+6*(v2-v1)/pow(dt,2); //used as second derivative at t1
645 k3 = 2*(2*k2+k1)/dt-6*(v2-v1)/pow(dt,2); //used as second derivative at t2
646
647 //calculate polynomial coefficients
648 a3 = (k3-k0)/(6*dt);
649 a2 = (t2[0]*k0-t1[0]*k3)/(2*dt);
650 a1 = ((v2-v1)-a2*(pow(t2[0],2)-pow(t1[0],2))-a3*(pow(t2[0],3)-pow(t1[0],3)))/dt;
651 a0 = v1-a1*t1[0]-a2*pow(t1[0],2)-a3*pow(t1[0],3);
652
653 //store coefficients in the datareader structure
654 d->a0[j] = a0;
655 d->a1[j] = a1;
656 d->a2[j] = a2;
657 d->a3[j] = a3;
658 #if DR_DEBUG
659 if (j == 1 )CONSOLE_DEBUG("Cubic spline coefficients recalculated");
660 #endif
661 }
662 //calculate output value
663 #if DR_DEBUG
664 CONSOLE_DEBUG("v[%d]:%lf",j,d->a0[j] + d->a1[j]*t + d->a2[j]*pow(t,2) + d->a3[j]*pow(t,3));
665 #endif
666 return d->a0[j] + d->a1[j]*t + d->a2[j]*pow(t,2) + d->a3[j]*pow(t,3);
667
668 }
669
670 double dr_linearderiv(double t, double *t1, double *t2, double v1, double v2) {
671 return (v2 - v1) / ((*t2)-(*t1));
672 }
673 double dr_cubicderiv(DataReader *d, int j, double t, double *t1, double *t2, double v0, double v1, double v2, double v3) {
674 //constrained cubic spline by CJC Kruger http://www.korf.co.uk
675 boolean AtStart, AtEnd;
676 double dt;
677 double k0,k1,k2,k3; //gradients for cubic spline interpolation
678 double a0,a1,a2,a3;//polynomial coefficients for second spline segment
679 dt = t2[0] - t1[0];
680
681 if (d->i != d->prev_i_der) {
682 /*if we are at a new interval, still wait for all coefficients to be
683 calculated before updating prev_i_val index */
684 if (j == d->noutputs-1) d->prev_i_der = d->i;
685
686 if (d->i < d->ndata-1) AtEnd = FALSE; //index is not at the end of the dataset
687 else AtEnd = TRUE; //index is at the end of the dataset
688 if (d->i > 0) AtStart = FALSE; //index is not at the beggining of the dataset
689 else AtStart = TRUE; //index is at the begginintg of the dataset
690
691 if ((dt/(v3-v2)+dt/(v2-v1))==0 || (v3-v2)*(v2-v1)<0) k2 = 0;
692 else {
693 if (AtEnd) k2 = 3*(v2-v1)/2-1/(dt/(v2-v1)+dt/(v1-v0));
694 else k2 = 2/(dt/(v3-v2)+dt/(v2-v1));
695 }
696 if ((dt/(v2-v1)+dt/(v1-v0))==0 || (v2-v1)*(v1-v0)<0) k1 = 0;
697 else {
698 if (AtStart) k1 = 3*(v2-v1)/2 - k2/2;
699 else k1 = 2/(dt/(v2-v1)+dt/(v1-v0));
700 }
701 k0 = -2*(k2+2*k1)/dt+6*(v2-v1)/pow(dt,2); //used as second derivative at t1
702 k3 = 2*(2*k2+k1)/dt-6*(v2-v1)/pow(dt,2); //used as second derivative at t2
703
704 //calculate polynomial coefficients
705 a3 = (k3-k0)/(6*dt);
706 a2 = (t2[0]*k0-t1[0]*k3)/(2*dt);
707 a1 = ((v2-v1)-a2*(pow(t2[0],2)-pow(t1[0],2))-a3*(pow(t2[0],3)-pow(t1[0],3)))/dt;
708 a0 = v1-a1*t1[0]-a2*pow(t1[0],2)-a3*pow(t1[0],3);
709
710 //store coefficients in the datareader structure
711 d->a0[j] = a0;
712 d->a1[j] = a1;
713 d->a2[j] = a2;
714 d->a3[j] = a3;
715 #if DR_DEBUG
716 if (j == 1 )CONSOLE_DEBUG("Cubic spline derivatives recalculated");
717 #endif
718 }
719 //calculate output value
720 return d->a1[j] + 2*d->a2[j]*t + 3*d->a3[j]*pow(t,2);
721
722 }

john.pye@anu.edu.au
ViewVC Help
Powered by ViewVC 1.1.22