/[escript]/trunk/esys2/finley/src/finleyC/Util.c
ViewVC logotype

Contents of /trunk/esys2/finley/src/finleyC/Util.c

Parent Directory Parent Directory | Revision Log Revision Log


Revision 113 - (show annotations)
Mon Feb 28 07:06:33 2005 UTC (14 years, 1 month ago) by jgs
File MIME type: text/plain
File size: 16919 byte(s)
*** empty log message ***

1 /* $Id$ */
2
3 /**************************************************************/
4
5 /* Some utility routines: */
6
7 /**************************************************************/
8
9 /* Copyrights by ACcESS Australia, 2003 */
10 /* author: gross@access.edu.au */
11 /* Version: $Id$ */
12
13 /**************************************************************/
14
15 #include "Common.h"
16 #include "Finley.h"
17 #include "Util.h"
18 #ifdef _OPENMP
19 #include <omp.h>
20 #endif
21
22 /**************************************************************/
23
24 /* gathers double values out from in by index: */
25
26 /* out(1:numData,1:len)=in(1:numData,index(1:len)) */
27
28 void Finley_Util_Gather_double(int len,maybelong* index,int numData,double* in, double * out){
29 int s,i;
30 for (s=0;s<len;s++) {
31 for (i=0;i<numData;i++) {
32 out[INDEX2(i,s,numData)]=in[INDEX2(i,index[s],numData)];
33 }
34 }
35 }
36
37 /**************************************************************/
38
39
40 /* gathers maybelong values out from in by index: */
41
42 /* out(1:numData,1:len)=in(1:numData,index(1:len)) */
43
44 void Finley_Util_Gather_int(int len,maybelong* index,int numData, maybelong* in, maybelong * out){
45 int s,i;
46 for (s=0;s<len;s++) {
47 for (i=0;i<numData;i++) {
48 out[INDEX2(i,s,numData)]=in[INDEX2(i,index[s],numData)];
49 }
50 }
51 }
52
53 /**************************************************************/
54
55 /* adds a vector in into out using and index. */
56
57 /* out(1:numData,index(1:len))+=in(1:numData,1:len) */
58
59 void Finley_Util_AddScatter(int len,maybelong* index,int numData,double* in,double * out){
60 int i,s;
61 for (s=0;s<len;s++) {
62 for(i=0;i<numData;i++) {
63 #pragma omp atomic
64 out[INDEX2(i,index[s],numData)]+=in[INDEX2(i,s,numData)];
65 }
66 }
67 }
68
69 /* multiplies two matrices */
70
71 /* A(1:A1,1:A2)=B(1:A1,1:B2)*C(1:B2,1:A2) */
72
73 void Finley_Util_SmallMatMult(int A1,int A2, double* A, int B2, double*B, double* C) {
74 int i,j,s;
75 for (i=0;i<A1*A2;i++) A[i]=0;
76 for (i=0;i<A1;i++) {
77 for (j=0;j<A2;j++) {
78 for (s=0;s<B2;s++) {
79 A[INDEX2(i,j,A1)]+=B[INDEX2(i,s,A1)]*C[INDEX2(s,j,B2)];
80 }
81 }
82 }
83 }
84
85 /* multiplies a two sets of matries: */
86
87 /* A(1:A1,1:A2,i)=B(1:A1,1:B2,i)*C(1:B2,1:A2,i) i=1,len */
88
89 void Finley_Util_SmallMatSetMult(int len,int A1,int A2, double* A, int B2, double*B, double* C) {
90 int q,i,j,s;
91 for (i=0;i<A1*A2*len;i++) A[i]=0;
92 for (q=0;q<len;q++) {
93 for (i=0;i<A1;i++) {
94 for (j=0;j<A2;j++) {
95 for (s=0;s<B2;s++) {
96 A[INDEX3(i,j,q,A1,A2)]+=B[INDEX3(i,s,q,A1,B2)]*C[INDEX3(s,j,q,B2,A2)];
97 }
98 }
99 }
100 }
101 }
102
103 /* calcultes the LU factorization for a small matrix dimxdim matrix A */
104 /* TODO: use LAPACK */
105
106 int Finley_Util_SmallMatLU(int dim,double* A,double *LU,int* pivot){
107 double D,A11,A12,A13,A21,A22,A23,A31,A32,A33;
108 int info=0;
109 /* LAPACK version */
110 /* memcpy(LU,A,sizeof(douple)); */
111 /* dgetf2_(&dim,&dim,A,&dim,LU,pivot,&info); */
112 switch(dim) {
113 case 1:
114 D=A[INDEX2(0,0,dim)];
115 if (ABS(D) >0. ){
116 LU[INDEX2(0,0,dim)]=1./D;
117 } else {
118 info=2;
119 }
120 break;
121
122 case 2:
123 A11=A[INDEX2(0,0,dim)];
124 A12=A[INDEX2(0,1,dim)];
125 A21=A[INDEX2(1,0,dim)];
126 A22=A[INDEX2(1,1,dim)];
127
128 D = A11*A22-A12*A21;
129 if (ABS(D) > 0 ){
130 D=1./D;
131 LU[INDEX2(0,0,dim)]= A22*D;
132 LU[INDEX2(1,0,dim)]=-A21*D;
133 LU[INDEX2(0,1,dim)]=-A12*D;
134 LU[INDEX2(1,1,dim)]= A11*D;
135 } else {
136 info=2;
137 }
138 break;
139
140 case 3:
141 A11=A[INDEX2(0,0,dim)];
142 A21=A[INDEX2(1,0,dim)];
143 A31=A[INDEX2(2,0,dim)];
144 A12=A[INDEX2(0,1,dim)];
145 A22=A[INDEX2(1,1,dim)];
146 A32=A[INDEX2(2,1,dim)];
147 A13=A[INDEX2(0,2,dim)];
148 A23=A[INDEX2(1,2,dim)];
149 A33=A[INDEX2(2,2,dim)];
150
151 D = A11*(A22*A33-A23*A32)+ A12*(A31*A23-A21*A33)+A13*(A21*A32-A31*A22);
152 if (ABS(D) > 0 ){
153 D=1./D;
154 LU[INDEX2(0,0,dim)]=(A22*A33-A23*A32)*D;
155 LU[INDEX2(1,0,dim)]=(A31*A23-A21*A33)*D;
156 LU[INDEX2(2,0,dim)]=(A21*A32-A31*A22)*D;
157 LU[INDEX2(0,1,dim)]=(A13*A32-A12*A33)*D;
158 LU[INDEX2(1,1,dim)]=(A11*A33-A31*A13)*D;
159 LU[INDEX2(2,1,dim)]=(A12*A31-A11*A32)*D;
160 LU[INDEX2(0,2,dim)]=(A12*A23-A13*A22)*D;
161 LU[INDEX2(1,2,dim)]=(A13*A21-A11*A23)*D;
162 LU[INDEX2(2,2,dim)]=(A11*A22-A12*A21)*D;
163 } else {
164 info=2;
165 }
166 break;
167 default:
168 info=1;
169 }
170 return info;
171 }
172
173 /* solves LUx=b where LU is a LU factorization calculated by an Finley_Util_SmallMatLU call */
174 void Finley_Util_SmallMatForwardBackwardSolve(int dim ,int nrhs,double* LU,int* pivot,double* x,double* b) {
175 int i;
176 switch(dim) {
177 case 1:
178 for (i=0;i<nrhs;i++) {
179 x[INDEX2(0,i,dim)]=LU[0]*b[INDEX2(0,i,dim)];
180 }
181 break;
182 case 2:
183 for (i=0;i<nrhs;i++) {
184 x[INDEX2(0,i,dim)]=LU[INDEX2(0,0,dim)]*b[INDEX2(0,i,dim)]+LU[INDEX2(0,1,dim)]*b[INDEX2(1,i,dim)];
185 x[INDEX2(1,i,dim)]=LU[INDEX2(1,0,dim)]*b[INDEX2(0,i,dim)]+LU[INDEX2(1,1,dim)]*b[INDEX2(1,i,dim)];
186 }
187 break;
188
189 case 3:
190 for (i=0;i<nrhs;i++) {
191 x[INDEX2(0,i,dim)]=LU[INDEX2(0,0,dim)]*b[INDEX2(0,i,dim)]+LU[INDEX2(0,1,dim)]*b[INDEX2(1,i,dim)]+LU[INDEX2(0,2,dim)]*b[INDEX2(2,i,dim)];
192 x[INDEX2(1,i,dim)]=LU[INDEX2(1,0,dim)]*b[INDEX2(0,i,dim)]+LU[INDEX2(1,1,dim)]*b[INDEX2(1,i,dim)]+LU[INDEX2(1,2,dim)]*b[INDEX2(2,i,dim)];
193 x[INDEX2(2,i,dim)]=LU[INDEX2(2,0,dim)]*b[INDEX2(0,i,dim)]+LU[INDEX2(2,1,dim)]*b[INDEX2(1,i,dim)]+LU[INDEX2(2,2,dim)]*b[INDEX2(2,i,dim)];
194 }
195 break;
196 }
197 return;
198 }
199 /* inverts the set of dim x dim matrices A(:,:,1:len) with dim=1,2,3 */
200 /* the determinante is returned. */
201
202 void Finley_Util_InvertSmallMat(int len,int dim,double* A,double *invA, double* det){
203 int q;
204 double D,A11,A12,A13,A21,A22,A23,A31,A32,A33;
205
206 switch(dim) {
207 case 1:
208 for (q=0;q<len;q++) {
209 D=A[INDEX3(0,0,q,dim,dim)];
210 if (ABS(D) > 0 ){
211 det[q]=D;
212 D=1./D;
213 invA[INDEX3(0,0,q,dim,dim)]=D;
214 } else {
215 Finley_ErrorCode=ZERO_DIVISION_ERROR;
216 sprintf(Finley_ErrorMsg,"Non-regular matrix");
217 return;
218 }
219 }
220 break;
221
222 case 2:
223 for (q=0;q<len;q++) {
224 A11=A[INDEX3(0,0,q,dim,dim)];
225 A12=A[INDEX3(0,1,q,dim,dim)];
226 A21=A[INDEX3(1,0,q,dim,dim)];
227 A22=A[INDEX3(1,1,q,dim,dim)];
228
229 D = A11*A22-A12*A21;
230 if (ABS(D) > 0 ){
231 det[q]=D;
232 D=1./D;
233 invA[INDEX3(0,0,q,dim,dim)]= A22*D;
234 invA[INDEX3(1,0,q,dim,dim)]=-A21*D;
235 invA[INDEX3(0,1,q,dim,dim)]=-A12*D;
236 invA[INDEX3(1,1,q,dim,dim)]= A11*D;
237 } else {
238 Finley_ErrorCode=ZERO_DIVISION_ERROR;
239 sprintf(Finley_ErrorMsg,"Non-regular matrix");
240 return;
241 }
242 }
243 break;
244
245 case 3:
246 for (q=0;q<len;q++) {
247 A11=A[INDEX3(0,0,q,dim,dim)];
248 A21=A[INDEX3(1,0,q,dim,dim)];
249 A31=A[INDEX3(2,0,q,dim,dim)];
250 A12=A[INDEX3(0,1,q,dim,dim)];
251 A22=A[INDEX3(1,1,q,dim,dim)];
252 A32=A[INDEX3(2,1,q,dim,dim)];
253 A13=A[INDEX3(0,2,q,dim,dim)];
254 A23=A[INDEX3(1,2,q,dim,dim)];
255 A33=A[INDEX3(2,2,q,dim,dim)];
256
257 D = A11*(A22*A33-A23*A32)+ A12*(A31*A23-A21*A33)+A13*(A21*A32-A31*A22);
258 if (ABS(D) > 0 ){
259 det[q] =D;
260 D=1./D;
261 invA[INDEX3(0,0,q,dim,dim)]=(A22*A33-A23*A32)*D;
262 invA[INDEX3(1,0,q,dim,dim)]=(A31*A23-A21*A33)*D;
263 invA[INDEX3(2,0,q,dim,dim)]=(A21*A32-A31*A22)*D;
264 invA[INDEX3(0,1,q,dim,dim)]=(A13*A32-A12*A33)*D;
265 invA[INDEX3(1,1,q,dim,dim)]=(A11*A33-A31*A13)*D;
266 invA[INDEX3(2,1,q,dim,dim)]=(A12*A31-A11*A32)*D;
267 invA[INDEX3(0,2,q,dim,dim)]=(A12*A23-A13*A22)*D;
268 invA[INDEX3(1,2,q,dim,dim)]=(A13*A21-A11*A23)*D;
269 invA[INDEX3(2,2,q,dim,dim)]=(A11*A22-A12*A21)*D;
270 } else {
271 Finley_ErrorCode=ZERO_DIVISION_ERROR;
272 sprintf(Finley_ErrorMsg,"Non-regular matrix");
273 return;
274 }
275 }
276 break;
277
278 }
279 return;
280 }
281
282 /* sets the derterminate of a set of dim x dim matrices A(:,:,1:len) with dim=1,2,3 */
283
284 void Finley_Util_DetOfSmallMat(int len,int dim,double* A, double* det){
285 int q;
286 double A11,A12,A13,A21,A22,A23,A31,A32,A33;
287
288 switch(dim) {
289 case 1:
290 for (q=0;q<len;q++) {
291 det[q]=A[INDEX3(0,0,q,dim,dim)];
292 }
293 break;
294
295 case 2:
296 for (q=0;q<len;q++) {
297 A11=A[INDEX3(0,0,q,dim,dim)];
298 A12=A[INDEX3(0,1,q,dim,dim)];
299 A21=A[INDEX3(1,0,q,dim,dim)];
300 A22=A[INDEX3(1,1,q,dim,dim)];
301
302 det[q] = A11*A22-A12*A21;
303 }
304 break;
305
306 case 3:
307 for (q=0;q<len;q++) {
308 A11=A[INDEX3(0,0,q,dim,dim)];
309 A21=A[INDEX3(1,0,q,dim,dim)];
310 A31=A[INDEX3(2,0,q,dim,dim)];
311 A12=A[INDEX3(0,1,q,dim,dim)];
312 A22=A[INDEX3(1,1,q,dim,dim)];
313 A32=A[INDEX3(2,1,q,dim,dim)];
314 A13=A[INDEX3(0,2,q,dim,dim)];
315 A23=A[INDEX3(1,2,q,dim,dim)];
316 A33=A[INDEX3(2,2,q,dim,dim)];
317
318 det[q] = A11*(A22*A33-A23*A32)+ A12*(A31*A23-A21*A33)+A13*(A21*A32-A31*A22);
319 }
320 break;
321
322 }
323 return;
324 }
325 /* returns the normalized vector Normal[dim,len] orthogonal to A(:,0,q) and A(:,1,q) in the case of dim=3 */
326 /* or the vector A(:,0,q) in the case of dim=2 */
327
328 void Finley_NormalVector(int len, int dim, int dim1, double* A,double* Normal) {
329 int q;
330 double A11,A12,CO_A13,A21,A22,CO_A23,A31,A32,CO_A33,length,invlength;
331
332 switch(dim) {
333 case 1:
334 for (q=0;q<len;q++) Normal[INDEX1(q)] =1;
335 break;
336 case 2:
337 for (q=0;q<len;q++) {
338 A11=A[INDEX3(0,0,q,dim,dim1)];
339 A21=A[INDEX3(1,0,q,dim,dim1)];
340 length = sqrt(A11*A11+A21*A21);
341 if (! length>0) {
342 Finley_ErrorCode=ZERO_DIVISION_ERROR;
343 sprintf(Finley_ErrorMsg,"area equals zero.");
344 return;
345 } else {
346 invlength=1./length;
347 Normal[INDEX2(0,q,dim)]=A21*invlength;
348 Normal[INDEX2(1,q,dim)]=-A11*invlength;
349 }
350 }
351 break;
352 case 3:
353 for (q=0;q<len;q++) {
354 A11=A[INDEX3(0,0,q,dim,dim1)];
355 A21=A[INDEX3(1,0,q,dim,dim1)];
356 A31=A[INDEX3(2,0,q,dim,dim1)];
357 A12=A[INDEX3(0,1,q,dim,dim1)];
358 A22=A[INDEX3(1,1,q,dim,dim1)];
359 A32=A[INDEX3(2,1,q,dim,dim1)];
360 CO_A13=A21*A32-A31*A22;
361 CO_A23=A31*A12-A11*A32;
362 CO_A33=A11*A22-A21*A12;
363 length=sqrt(CO_A13*CO_A13+CO_A23*CO_A23+CO_A33*CO_A33);
364 if (! length>0) {
365 Finley_ErrorCode=ZERO_DIVISION_ERROR;
366 sprintf(Finley_ErrorMsg,"area equals zero.");
367 return;
368 } else {
369 invlength=1./length;
370 Normal[INDEX2(0,q,dim)]=CO_A13*invlength;
371 Normal[INDEX2(1,q,dim)]=CO_A23*invlength;
372 Normal[INDEX2(2,q,dim)]=CO_A33*invlength;
373 }
374
375 }
376 break;
377
378 }
379 return;
380 }
381
382 /* return the length of the vector which is orthogonal to the vectors A(:,0,q) and A(:,1,q) in the case of dim=3 */
383 /* or the vector A(:,0,q) in the case of dim=2 */
384
385 void Finley_LengthOfNormalVector(int len, int dim, int dim1, double* A,double* length) {
386 int q;
387 double A11,A12,CO_A13,A21,A22,CO_A23,A31,A32,CO_A33;
388
389 switch(dim) {
390 case 1:
391 for (q=0;q<len;q++) length[INDEX1(q)] =1;
392 break;
393 case 2:
394 for (q=0;q<len;q++) {
395 A11=A[INDEX3(0,0,q,dim,dim1)];
396 A21=A[INDEX3(1,0,q,dim,dim1)];
397 length[q] = sqrt(A11*A11+A21*A21);
398 }
399 break;
400 case 3:
401 for (q=0;q<len;q++) {
402 A11=A[INDEX3(0,0,q,dim,dim1)];
403 A21=A[INDEX3(1,0,q,dim,dim1)];
404 A31=A[INDEX3(2,0,q,dim,dim1)];
405 A12=A[INDEX3(0,1,q,dim,dim1)];
406 A22=A[INDEX3(1,1,q,dim,dim1)];
407 A32=A[INDEX3(2,1,q,dim,dim1)];
408 CO_A13=A21*A32-A31*A22;
409 CO_A23=A31*A12-A11*A32;
410 CO_A33=A11*A22-A21*A12;
411 length[q]=sqrt(CO_A13*CO_A13+CO_A23*CO_A23+CO_A33*CO_A33);
412 }
413 break;
414
415 }
416 return;
417 }
418
419 /* inverts the map map of length len */
420 /* there is no range checking! */
421 /* at output Map[invMap[i]]=i for i=0:lenInvMap */
422
423 void Finley_Util_InvertMap(int lenInvMap, maybelong* invMap,int lenMap, maybelong* Map) {
424 int i;
425 for (i=0;i<lenInvMap;i++) invMap[i]=0;
426 for (i=0;i<lenMap;i++) {
427 if (Map[i]>=0) invMap[Map[i]]=i;
428 }
429 }
430
431 /* orders a Finley_Util_ValueAndIndex array by value */
432 /* it is assumed that n is large */
433
434 int Finley_Util_ValueAndIndex_compar(const void *arg1 , const void *arg2 ) {
435 Finley_Util_ValueAndIndex *e1,*e2;
436 e1=(Finley_Util_ValueAndIndex*) arg1;
437 e2=(Finley_Util_ValueAndIndex*) arg2;
438 if (e1->value < e2->value) return -1;
439 if (e1->value > e2->value) return 1;
440 return 0;
441 }
442 void Finley_Util_sortValueAndIndex(int n,Finley_Util_ValueAndIndex* array) {
443 /* OMP : needs parallelization !*/
444 qsort(array,n,sizeof(Finley_Util_ValueAndIndex),Finley_Util_ValueAndIndex_compar);
445 }
446
447
448 /**************************************************************/
449
450 /* calculates the minimum value from a dim X N integer array */
451
452 maybelong Finley_Util_getMinInt(int dim,int N,maybelong* values) {
453 maybelong i,j,out;
454 out=MAYBELONG_MAX;
455 if (values!=NULL && dim*N>0 ) {
456 /* OMP */
457 out=values[0];
458 for (j=0;j<N;j++) {
459 for (i=0;i<dim;i++) out=MIN(out,values[INDEX2(i,j,dim)]);
460 }
461 }
462 return out;
463 }
464
465 /* calculates the maximum value from a dim X N integer array */
466
467 maybelong Finley_Util_getMaxInt(int dim,int N,maybelong* values) {
468 maybelong i,j,out;
469 out=-MAYBELONG_MAX;
470 if (values!=NULL && dim*N>0 ) {
471 /* OMP */
472 out=values[0];
473 for (j=0;j<N;j++) {
474 for (i=0;i<dim;i++) out=MAX(out,values[INDEX2(i,j,dim)]);
475 }
476 }
477 return out;
478 }
479
480 /* set the index of the positive entries in mask. The length of index is returned. */
481
482 maybelong Finley_Util_packMask(maybelong N,maybelong* mask,maybelong* index) {
483 maybelong out,k;
484 out=0;
485 /*OMP */
486 for (k=0;k<N;k++) {
487 if (mask[k]>=0) {
488 index[out]=k;
489 out++;
490 }
491 }
492 return out;
493 }
494
495 /* returns true if array contains value */
496 int Finley_Util_isAny(maybelong N,maybelong* array,maybelong value) {
497 int out=FALSE;
498 maybelong i;
499 #pragma omp parallel for private(i) schedule(static) reduction(||:out)
500 for (i=0;i<N;i++) out=out || (array[i]==value);
501 return out;
502 }
503 /* calculates the cummultative sum in array and returns the total sum */
504 maybelong Finley_Util_cumsum(maybelong N,maybelong* array) {
505 maybelong out=0,tmp,i;
506 #ifdef _OPENMP
507 maybelong partial_sums[omp_get_max_threads()],sum;
508 #pragma omp parallel private(sum,i,tmp)
509 {
510 sum=0;
511 #pragma omp for
512 for (i=0;i<N;++i) {
513 tmp=sum;
514 sum+=array[i];
515 array[i]=tmp;
516 }
517 #pragma omp critical
518 partial_sums[omp_get_thread_num()]=sum;
519 #pragma omp master
520 {
521 out=0;
522 for (i=0;i<omp_get_max_threads();++i) {
523 tmp=out;
524 out+=partial_sums[i];
525 partial_sums[i]=tmp;
526 }
527 }
528 sum=partial_sums[omp_get_thread_num()];
529 #pragma omp for
530 for (i=0;i<N;++i) array[i]+=sum;
531 }
532 #else
533 for (i=0;i<N;++i) {
534 tmp=out;
535 out+=array[i];
536 array[i]=tmp;
537 }
538 #endif
539 return out;
540 }
541
542 void Finley_copyDouble(int n,double* source, double* target) {
543 int i;
544 for (i=0;i<n;i++) target[i]=source[i];
545 }

Properties

Name Value
svn:eol-style native
svn:keywords Author Date Id Revision

  ViewVC Help
Powered by ViewVC 1.1.26