![[BACK]](/icons/cvsweb/back.gif){kind=icon}
Return to generic_floating_linear_solvers.adb CVS log ![[TXT]](/icons/cvsweb/text.gif){kind=icon}
![[DIR]](/icons/cvsweb/dir.gif){kind=icon}
Up to [local] / OpenXM_contrib / PHC / Ada / Math_Lib / Matrices|
Return to generic_floating_linear_solvers.adb CVS log | Up to [local] / OpenXM_contrib / PHC / Ada / Math_Lib / Matrices |
1.1 maekawa 1: -- debugging
2: --with text_io; use text_io;
3: --with Standard_Natural_Vectors_io; use Standard_Natural_Vectors_io;
4:
5: package body Generic_Floating_Linear_Solvers is
6:
7: use Field,Vectors;
8:
9: -- AUXILIARIES :
10:
11: function csign ( x,y : number ) return number is
12:
13: res : number;
14:
15: begin
16: if y > zero
17: then res := AbsVal(x);
18: else res := AbsVal(x);
19: Min(res);
20: end if;
21: return res;
22: -- return AbsVal(x) * y / AbsVal(y);
23: end csign;
24:
25: function Inverse_Abs_Sum ( z : Vectors.Vector ) return number is
26:
27: -- DESCRIPTION :
28: -- Returns the reciprocal of the sum of the absolute values in z.
29:
30: res,sum,acc : number;
31:
32: begin
33: Copy(zero,sum);
34: for i in z'range loop
35: acc := AbsVal(z(i));
36: Add(sum,acc);
37: Clear(acc);
38: end loop;
39: res := one/sum;
40: Clear(sum);
41: return res;
42: end Inverse_Abs_Sum;
43:
44: -- STATIC TRIANGULATORS :
45:
46: procedure lufac ( a : in out matrix; n : in natural;
47: ipvt : out Standard_Natural_Vectors.Vector;
48: info : out natural ) is
49:
50: kp1,l,nm1 : natural;
51: smax,temp,acc : number;
52:
53: begin
54: info := 0;
55: nm1 := n - 1;
56: if nm1 >= 1
57: then for k in 1..nm1 loop
58: kp1 := k + 1;
59: l := k;
60: smax := AbsVal(a(k,k)); -- find the pivot index l
61: for i in kp1..n loop
62: acc := AbsVal(a(i,k));
63: if acc > smax
64: then l := i;
65: Copy(acc,smax);
66: end if;
67: Clear(acc);
68: end loop;
69: ipvt(k) := l;
70: if Equal(smax,zero)
71: then info := k; -- this column is already triangulated
72: else if l /= k -- interchange if necessary
73: then Copy(a(l,k),temp);
74: Copy(a(k,k),a(l,k));
75: Copy(temp,a(k,k));
76: end if;
77: acc := one/a(k,k); -- compute multipliers
78: Min(acc);
79: for i in kp1..n loop
80: Mul(a(i,k),acc);
81: end loop;
82: Clear(acc);
83: for j in kp1..n loop -- row elimination
84: Copy(a(l,j),temp);
85: if l /= k
86: then Copy(a(k,j),a(l,j));
87: Copy(temp,a(k,j));
88: end if;
89: for i in kp1..n loop
90: acc := temp*a(i,k);
91: Add(a(i,j),acc);
92: Clear(acc);
93: end loop;
94: end loop;
95: Clear(temp);
96: end if;
97: Clear(smax);
98: end loop;
99: end if;
100: ipvt(n) := n;
101: if Equal(a(n,n),zero)
102: then info := n;
103: end if;
104: end lufac;
105:
106: procedure lufco ( a : in out Matrix; n : in natural;
107: ipvt : out Standard_Natural_Vectors.Vector;
108: rcond : out number ) is
109:
110: -- NOTE :
111: -- rcond = 1/(norm(a)*(estimate of norm(inverse(a))))
112: -- estimate = norm(z)/norm(y) where a*z = y and ctrans(a)*y = e.
113: -- ctrans(a) is the conjugate transpose of a.
114: -- The components of e are chosen to cause maximum local
115: -- growth in teh elements of w where ctrans(u)*w = e.
116: -- The vectors are frequently rescaled to avoid overflow.
117:
118: z : Vectors.Vector(1..n);
119: info,kb,kp1,l : natural;
120: s,sm,sum,anorm,ynorm,ek,t,wk,wkm,acc,absacc1,absacc2 : number;
121: ipvtt : Standard_Natural_Vectors.Vector(1..n);
122:
123: begin
124: Copy(zero,anorm); -- compute 1-norm of a
125: for j in 1..n loop
126: Copy(zero,sum);
127: for i in 1..n loop
128: acc := AbsVal(a(i,j));
129: Add(sum,acc);
130: Clear(acc);
131: end loop;
132: if sum > anorm
133: then Copy(sum,anorm);
134: end if;
135: end loop;
136: Clear(sum);
137: lufac(a,n,ipvtt,info); -- factor
138: -- put_line("Factorization completed");
139: -- put("ipvtt : "); put(ipvtt); new_line;
140: for i in 1..n loop
141: ipvt(i) := ipvtt(i);
142: end loop;
143: Copy(one,ek); -- solve ctrans(u)*w = e
144: for j in 1..n loop
145: Copy(zero,z(j));
146: end loop;
147: -- put_line("At start of first k loop");
148: for k in 1..n loop
149: acc := AbsVal(z(k));
150: if not Equal(acc,zero)
151: then Clear(acc);
152: acc := -z(k);
153: ek := csign(ek,acc);
154: Clear(acc);
155: end if;
156: acc := ek-z(k);
157: absacc1 := AbsVal(acc);
158: absacc2 := AbsVal(a(k,k));
159: if absacc1 > absacc2
160: then s := absacc2/absacc1;
161: Mul(z,s);
162: Mul(ek,s);
163: Clear(s);
164: end if;
165: Clear(absacc1); Clear(absacc2);
166: Clear(acc);
167: wk := ek - z(k);
168: wkm := -ek;
169: Clear(ek);
170: Sub(wkm,z(k));
171: s := AbsVal(wk);
172: sm := AbsVal(wkm);
173: acc := AbsVal(a(k,k));
174: if Equal(acc,zero)
175: then Copy(one,wk);
176: Copy(one,wkm);
177: else wk := wk / a(k,k);
178: wkm := wkm / a(k,k);
179: end if;
180: Clear(acc);
181: kp1 := k + 1;
182: if kp1 <= n
183: then for j in kp1..n loop
184: acc := wkm*a(k,j);
185: Add(acc,z(j));
186: absacc1 := AbsVal(acc);
187: Add(sm,absacc1);
188: Clear(acc); Clear(absacc1);
189: acc := wk*a(k,j);
190: Add(z(j),acc);
191: Clear(acc);
192: absacc1 := AbsVal(z(j));
193: Add(s,absacc1);
194: end loop;
195: if s < sm
196: then t := wkm - wk;
197: Copy(wkm,wk);
198: for j in kp1..n loop
199: acc := t*a(k,j);
200: Add(z(j),acc);
201: Clear(acc);
202: end loop;
203: Clear(t);
204: end if;
205: end if;
206: Copy(wk,z(k)); Clear(wk); Clear(wkm);
207: Clear(s); Clear(sm);
208: end loop;
209: -- put_line("Gone through first k loop");
210: s := Inverse_Abs_Sum(z);
211: Mul(z,s); Clear(s);
212: -- put_line("At start of second k loop");
213: for k in 1..n loop -- solve ctrans(l)*y = w
214: kb := n+1-k;
215: if kb < n
216: then Copy(zero,t);
217: for i in (kb+1)..n loop
218: acc := a(i,kb)*z(i);
219: Add(t,acc);
220: Clear(acc);
221: end loop;
222: Add(z(kb),t); Clear(t);
223: end if;
224: -- put_line("In the middle of second k loop");
225: acc := AbsVal(z(kb));
226: if acc > one
227: then s := one / acc;
228: Mul(z,s); Clear(s);
229: end if;
230: Clear(acc);
231: l := ipvtt(kb);
232: -- put_line("Just before copying in second k loop");
233: if l /= kb
234: then Copy(z(l),t);
235: Copy(z(kb),z(l));
236: Copy(t,z(kb)); Clear(t);
237: end if;
238: -- put_line("At end of one stage of second k loop");
239: end loop;
240: -- put_line("At end of second k loop");
241: s := Inverse_Abs_Sum(z);
242: Mul(z,s); Clear(s);
243: Copy(one,ynorm);
244: -- put_line("At start of third k loop");
245: for k in 1..n loop -- solve l*v = y
246: l := ipvtt(k);
247: if l /= k
248: then Copy(z(l),t);
249: Copy(z(k),z(l));
250: Copy(t,z(k));
251: else Copy(z(l),t);
252: end if;
253: if k < n
254: then for i in (k+1)..n loop
255: acc := t*a(i,k);
256: Add(z(i),acc);
257: Clear(acc);
258: end loop;
259: end if;
260: Clear(t);
261: acc := AbsVal(z(k));
262: if acc > one
263: then s := one / acc;
264: Mul(z,s);
265: Mul(ynorm,s); Clear(s);
266: end if;
267: Clear(acc);
268: end loop;
269: -- put_line("At end of third k loop");
270: s := Inverse_Abs_Sum(z);
271: Mul(z,s);
272: Mul(ynorm,s); Clear(s);
273: -- put_line("At start of fourth k loop");
274: for k in 1..n loop -- solve u*z = v
275: kb := n+1-k;
276: absacc1 := AbsVal(z(kb));
277: absacc2 := AbsVal(a(kb,kb));
278: if absacc1 > absacc2
279: then s := absacc2 / absacc1;
280: Mul(z,s);
281: Mul(ynorm,s); Clear(s);
282: end if;
283: Clear(absacc1);
284: if Equal(absacc2,zero)
285: then Copy(one,z(kb));
286: else Div(z(kb),a(kb,kb));
287: end if;
288: Clear(absacc2);
289: t := -z(kb);
290: for i in 1..(kb-1) loop
291: acc := t*a(i,kb);
292: Add(z(i),acc);
293: Clear(acc);
294: end loop;
295: Clear(t);
296: end loop;
297: s := Inverse_Abs_Sum(z); -- make znorm = 1.0
298: Mul(z,s);
299: Mul(ynorm,s); Clear(s);
300: if Equal(anorm,zero)
301: then Copy(zero,rcond);
302: else rcond := ynorm/anorm;
303: end if;
304: end lufco;
305:
306: procedure lusolve ( a : in matrix; n : in natural;
307: ipvt : in Standard_Natural_Vectors.Vector;
308: b : in out Vectors.vector ) is
309:
310: l,nm1,kb : integer;
311: temp,acc : number;
312:
313: begin
314: nm1 := n-1;
315: if nm1 >= 1 -- solve l*y = b
316: then for k in 1..nm1 loop
317: l := ipvt(k);
318: Copy(b(l),temp);
319: if l /= k
320: then Copy(b(k),b(l));
321: Copy(temp,b(k));
322: end if;
323: for i in (k+1)..n loop
324: acc := temp*a(i,k);
325: Add(b(i),acc);
326: Clear(acc);
327: end loop;
328: Clear(temp);
329: end loop;
330: end if;
331: for k in 1..n loop -- solve u*x = y
332: kb := n+1-k;
333: Div(b(kb),a(kb,kb));
334: temp := -b(kb);
335: for j in 1..(kb-1) loop
336: acc := temp*a(j,kb);
337: Add(b(j),acc);
338: Clear(acc);
339: end loop;
340: Clear(temp);
341: end loop;
342: end lusolve;
343:
344: procedure Triangulate ( a : in out Matrix; n,m : in integer ) is
345:
346: pivot,k,kcolumn : integer;
347: max,temp,acc : number;
348:
349: begin
350: k := 1;
351: kcolumn := 1;
352: while (k <= n) and (kcolumn <= m) loop
353: max := zero; -- find pivot
354: for l in k..n loop
355: if AbsVal(a(l,kcolumn)) > max
356: then max := AbsVal(a(l,kcolumn));
357: pivot := l;
358: end if;
359: end loop;
360: if Equal(max,zero)
361: then kcolumn := kcolumn + 1;
362: else if pivot /= k -- interchange if necessary
363: then for i in 1..m loop
364: temp := a(pivot,i);
365: a(pivot,i) := a(k,i);
366: a(k,i) := temp;
367: end loop;
368: end if;
369: for j in (kcolumn+1)..m loop -- triangulate a
370: Div(a(k,j),a(k,kcolumn));
371: end loop;
372: Copy(one,a(k,kcolumn));
373: for i in (k+1)..n loop
374: for j in (kcolumn+1)..m loop
375: acc := a(i,kcolumn)*a(k,j);
376: Sub(a(i,j),acc);
377: Clear(acc);
378: end loop;
379: end loop;
380: for j in (k+1)..n loop
381: Copy(zero,a(j,kcolumn));
382: end loop;
383: k := k + 1;
384: kcolumn := kcolumn + 1;
385: end if;
386: end loop;
387: end Triangulate;
388:
389: procedure Diagonalize ( a : in out Matrix; n,m : in integer ) is
390:
391: max,temp,acc : number;
392: pivot,k,kcolumn : integer;
393:
394: begin
395: k := 1;
396: kcolumn := 1;
397: while (k <= n) and (kcolumn <= m) loop
398: max := zero; -- find pivot
399: for l in k..n loop
400: if AbsVal(a(l,kcolumn)) > max
401: then max := AbsVal(a(l,kcolumn));
402: pivot := l;
403: end if;
404: end loop;
405: if Equal(max,zero)
406: then kcolumn := kcolumn + 1;
407: else if pivot /= k -- interchange if necessary
408: then for i in 1..m loop
409: temp := a(pivot,i);
410: a(pivot,i) := a(k,i);
411: a(k,i) := temp;
412: end loop;
413: end if;
414: for j in (kcolumn+1)..m loop -- diagonalize a
415: Div(a(k,j),a(k,kcolumn));
416: end loop;
417: Copy(one,a(k,kcolumn));
418: for i in 1..(k-1) loop
419: for j in (kcolumn+1)..m loop
420: acc := a(i,kcolumn)*a(k,j);
421: Sub(a(i,j),acc);
422: Clear(acc);
423: end loop;
424: end loop;
425: for i in (k+1)..n loop
426: for j in (kcolumn+1)..m loop
427: acc := a(i,kcolumn)*a(k,j);
428: Sub(a(i,j),acc);
429: Clear(acc);
430: end loop;
431: end loop;
432: for j in 1..(k-1) loop
433: Copy(zero,a(j,kcolumn));
434: end loop;
435: for j in (k+1)..n loop
436: Copy(zero,a(j,kcolumn));
437: end loop;
438: k := k + 1;
439: kcolumn := kcolumn + 1;
440: end if;
441: end loop;
442: end Diagonalize;
443:
444: -- DYNAMIC TRIANGULATORS :
445:
446: procedure Upper_Triangulate
447: ( row : in natural; mat : in out Matrix; tol : in number;
448: ipvt : in out Standard_Natural_Vectors.Vector;
449: pivot : out integer ) is
450:
451: factor,tmp,max,acc : number;
452: piv,tpi : integer := 0;
453:
454: begin
455: for j in mat'first(1)..row-1 loop
456: if AbsVal(mat(row,j)) > tol -- make mat(row,j) zero
457: then factor := mat(row,j)/mat(j,j);
458: for k in j..mat'last(2) loop
459: acc := factor*mat(j,k);
460: Sub(mat(row,k),acc);
461: Clear(acc);
462: end loop;
463: end if;
464: end loop;
465: for j in row..ipvt'last loop -- search pivot
466: tmp := AbsVal(mat(row,j));
467: if tmp > tol
468: then if piv = 0
469: then max := tmp; piv := j;
470: elsif tmp > max
471: then max := tmp; piv := j;
472: end if;
473: end if;
474: end loop;
475: pivot := piv;
476: if piv /= 0 -- zero row
477: then if piv /= row -- interchange columns
478: then for k in mat'range(1) loop
479: tmp := mat(k,row); mat(k,row) := mat(k,piv);
480: mat(k,piv) := tmp;
481: end loop;
482: tpi := ipvt(row); ipvt(row) := ipvt(piv); ipvt(piv) := tpi;
483: end if;
484: end if;
485: end Upper_Triangulate;
486:
487: procedure Upper_Triangulate
488: ( roweli : in natural; elim : in Matrix; tol : in number;
489: rowmat : in natural; mat : in out Matrix ) is
490:
491: factor,acc : number;
492:
493: begin
494: if AbsVal(mat(rowmat,roweli)) > tol
495: then factor := mat(rowmat,roweli)/elim(roweli,roweli);
496: for i in roweli..mat'last(2) loop
497: acc := factor*elim(roweli,i);
498: Sub(mat(rowmat,i),acc);
499: Clear(acc);
500: end loop;
501: end if;
502: end Upper_Triangulate;
503:
504: procedure Upper_Triangulate
505: ( roweli : in natural; elim : in Matrix; tol : in number;
506: firstrow,lastrow : in natural; mat : in out Matrix ) is
507: begin
508: for i in firstrow..lastrow loop
509: Upper_Triangulate(roweli,elim,tol,i,mat);
510: end loop;
511: end Upper_Triangulate;
512:
513: procedure Switch ( ipvt : in Standard_Natural_Vectors.Vector;
514: row : in integer; mat : in out Matrix ) is
515:
516: tmp : Vectors.Vector(mat'range(2));
517:
518: begin
519: for k in tmp'range loop
520: tmp(k) := mat(row,k);
521: end loop;
522: for k in ipvt'range loop
523: mat(row,k) := tmp(ipvt(k));
524: end loop;
525: for k in ipvt'last+1..mat'last(2) loop
526: mat(row,k) := tmp(k);
527: end loop;
528: end Switch;
529:
530: procedure Switch ( k,pivot,first,last : in integer; mat : in out Matrix ) is
531:
532: tmp : number;
533:
534: begin
535: if k /= pivot
536: then for i in first..last loop
537: tmp := mat(i,k);
538: mat(i,k) := mat(i,pivot);
539: mat(i,pivot) := tmp;
540: end loop;
541: end if;
542: end Switch;
543:
544: function Solve ( mat : Matrix; tol : number;
545: ipvt : Standard_Natural_Vectors.Vector )
546: return Vectors.Vector is
547:
548: res,x : Vectors.Vector(mat'range(2)) := (mat'range(2) => zero);
549: index : integer;
550: acc : number;
551:
552: begin
553: for i in mat'range(1) loop
554: index := i;
555: exit when i > mat'last(2);
556: exit when AbsVal(mat(i,i)) < tol;
557: end loop;
558: if (AbsVal(mat(index,index)) > tol) and then (index < mat'last(2))
559: then index := index + 1;
560: end if;
561: Copy(one,x(index));
562: for i in reverse mat'first(1)..(index-1) loop
563: x(i) := -mat(i,index);
564: for j in i+1..index-1 loop
565: acc := mat(i,j)*x(j);
566: Sub(x(i),acc);
567: Clear(acc);
568: end loop;
569: Div(x(i),mat(i,i));
570: end loop;
571: for k in ipvt'range loop
572: res(ipvt(k)) := x(k);
573: end loop;
574: for k in ipvt'last+1..res'last loop
575: res(k) := x(k);
576: end loop;
577: return res;
578: end Solve;
579:
580: function Solve ( n,col : natural; mat : Matrix )
581: return Vectors.Vector is
582:
583: res : Vectors.Vector(1..n+1);
584: acc : number;
585:
586: begin
587: Copy(one,res(n+1));
588: for i in reverse 1..n loop
589: res(i) := -mat(i,col);
590: for j in i+1..n loop
591: acc := mat(i,j)*res(j);
592: Sub(res(i),acc);
593: Clear(acc);
594: end loop;
595: Div(res(i),mat(i,i));
596: end loop;
597: return res;
598: end Solve;
599:
600: end Generic_Floating_Linear_Solvers;