Annotation of OpenXM_contrib/PHC/Ada/Math_Lib/Supports/givens_rotations.adb, Revision 1.1.1.1
1.1 maekawa 1: with Standard_Mathematical_Functions; use Standard_Mathematical_Functions;
2:
3: package body Givens_Rotations is
4:
5: procedure Givens_Factors ( v: in Vector; i,j : in integer;
6: cosi,sine : out double_float ) is
7: norm : double_float;
8:
9: begin
10: norm := SQRT(v(i)*v(i) + v(j)*v(j));
11: cosi := v(i)/norm; sine := v(j)/norm;
12: end Givens_Factors;
13:
14: procedure Givens_Factors ( mat : in Matrix; i,j : in integer;
15: cosi,sine : out double_float ) is
16: norm : double_float;
17:
18: begin
19: norm := SQRT(mat(i,i)*mat(i,i) + mat(j,i)*mat(j,i));
20: cosi := mat(i,i)/norm; sine := mat(j,i)/norm;
21: end Givens_Factors;
22:
23: procedure Givens_Rotation ( v : in out Vector; i,j : in integer;
24: cosi,sine : in double_float ) is
25: temp : double_float;
26:
27: begin
28: temp := v(i);
29: v(i) := cosi*v(i) + sine*v(j);
30: v(j) := -sine*temp + cosi*v(j);
31: end Givens_Rotation;
32:
33: procedure Givens_Rotation ( mat : in out Matrix; i,j : in integer;
34: cosi,sine : in double_float ) is
35: temp : double_float;
36:
37: begin
38: -- for k in mat'range(2) loop -- certain angle preservation
39: for k in i..mat'last(2) loop -- only angle preservation if upper triangular
40: temp := mat(i,k);
41: mat(i,k) := cosi*mat(i,k) + sine*mat(j,k);
42: mat(j,k) := -sine*temp + cosi*mat(j,k);
43: end loop;
44: end Givens_Rotation;
45:
46: procedure Givens_Rotation ( mat : in out Matrix; lastcol,i,j : in integer;
47: cosi,sine : in double_float ) is
48: temp : double_float;
49:
50: begin
51: -- for k in mat'first(2)..lastcol loop -- certain angle preservation
52: for k in i..lastcol loop -- only angle preservation if upper triangular
53: temp := mat(i,k);
54: mat(i,k) := cosi*mat(i,k) + sine*mat(j,k);
55: mat(j,k) := -sine*temp + cosi*mat(j,k);
56: end loop;
57: end Givens_Rotation;
58:
59: procedure Givens_Rotation ( v : in out Vector; i,j : in integer ) is
60:
61: cosi,sine : double_float;
62:
63: begin
64: Givens_Factors(v,i,j,cosi,sine);
65: Givens_Rotation(v,i,j,cosi,sine);
66: end Givens_Rotation;
67:
68: procedure Givens_Rotation ( mat : in out Matrix; i,j : in integer ) is
69:
70: cosi,sine : double_float;
71:
72: begin
73: Givens_Factors(mat,i,j,cosi,sine);
74: Givens_Rotation(mat,i,j,cosi,sine);
75: end Givens_Rotation;
76:
77: procedure Givens_Rotation ( mat : in out Matrix;
78: lastcol,i,j : in integer ) is
79:
80: cosi,sine : double_float;
81:
82: begin
83: Givens_Factors(mat,i,j,cosi,sine);
84: Givens_Rotation(mat,lastcol,i,j,cosi,sine);
85: end Givens_Rotation;
86:
87: procedure Upper_Triangulate
88: ( row : in integer; mat : in out Matrix; tol : in double_float;
89: ipvt : in out Standard_Integer_Vectors.Vector;
90: pivot : out integer ) is
91:
92: piv,tpi : integer := 0;
93: tmp : double_float;
94:
95: begin
96: for j in row..mat'last(2) loop -- search pivot
97: if abs(mat(row,j)) > tol
98: then piv := j;
99: end if;
100: exit when (piv /= 0);
101: end loop;
102: if piv /= 0 -- zero row
103: then
104: if piv /= row -- interchange columns
105: then for k in mat'range(1) loop
106: tmp := mat(k,row); mat(k,row) := mat(k,piv); mat(k,piv) := tmp;
107: end loop;
108: tpi := ipvt(row); ipvt(row) := ipvt(piv); ipvt(piv) := tpi;
109: end if;
110: for k in row+1..mat'last(1) loop -- perform Givens rotations
111: if abs(mat(k,row)) > tol
112: then Givens_Rotation(mat,row,k);
113: end if;
114: end loop;
115: end if;
116: pivot := piv;
117: end Upper_Triangulate;
118:
119: procedure Upper_Triangulate ( mat : in out Matrix; tol : in double_float ) is
120:
121: pivot : integer;
122: temp : double_float;
123:
124: begin
125: for i in mat'range(1) loop -- mat(mat'first(1)..i-1,mat'first(2)..i-1)
126: pivot := 0; -- is already upper triangular
127: for j in i..mat'last(2) loop -- search pivot
128: if abs(mat(i,j)) > tol
129: then pivot := j;
130: end if;
131: exit when (pivot /= 0);
132: end loop;
133: exit when (pivot = 0); -- zero row
134: if pivot /= i -- interchange columns
135: then for k in mat'range(1) loop
136: temp := mat(k,i); mat(k,i) := mat(k,pivot); mat(k,pivot) := temp;
137: end loop;
138: end if;
139: for k in i+1..mat'last(1) loop -- perform Givens rotations
140: if abs(mat(k,i)) > tol
141: then Givens_Rotation(mat,i,k);
142: end if;
143: end loop; -- mat(mat'first(1)..i,mat'first(2)..i)
144: exit when i > mat'last(2); -- is upper triangular
145: end loop;
146: end Upper_Triangulate;
147:
148: procedure Upper_Triangulate ( mat : in out Matrix; tol : in double_float;
149: ipvt : out Standard_Integer_Vectors.Vector ) is
150:
151: pivot,tpi : integer;
152: pivots : Standard_Integer_Vectors.Vector(mat'range(2));
153: temp : double_float;
154:
155: begin
156: for i in pivots'range loop -- initialize vector of pivots
157: pivots(i) := i;
158: end loop;
159: for i in mat'range(1) loop -- mat(mat'first(1)..i-1,mat'first(2)..i-1)
160: pivot := 0; -- is already upper triangular
161: for j in i..mat'last(2) loop -- search pivot
162: if abs(mat(i,j)) > tol
163: then pivot := j;
164: end if;
165: exit when (pivot /= 0);
166: end loop;
167: exit when (pivot = 0); -- zero row
168: if pivot /= i -- interchange columns
169: then for k in mat'range(1) loop
170: temp := mat(k,i); mat(k,i) := mat(k,pivot); mat(k,pivot) := temp;
171: end loop;
172: tpi := pivots(i); pivots(i) := pivots(pivot); pivots(pivot) := tpi;
173: end if;
174: for k in i+1..mat'last(1) loop -- perform Givens rotations
175: if abs(mat(k,i)) > tol
176: then Givens_Rotation(mat,i,k);
177: end if;
178: end loop; -- mat(mat'first(1)..i,mat'first(2)..i)
179: exit when i > mat'last(2); -- is upper triangular
180: end loop;
181: ipvt := pivots;
182: end Upper_Triangulate;
183:
184: procedure Upper_Triangulate ( mat : in out Matrix; rhs : in out Vector;
185: tol : in double_float ) is
186:
187: pivot : integer;
188: temp,cosi,sine : double_float;
189:
190: begin
191: for i in mat'range(1) loop -- mat(mat'first(1)..i-1,mat'first(2)..i-1)
192: pivot := 0; -- is already upper triangular
193: for j in i..mat'last(2) loop -- search pivot
194: if abs(mat(i,j)) > tol
195: then pivot := j;
196: end if;
197: exit when (pivot /= 0);
198: end loop;
199: exit when (pivot = 0); -- zero row
200: if pivot /= i -- interchange columns
201: then for k in mat'range(1) loop
202: temp := mat(k,i); mat(k,i) := mat(k,pivot); mat(k,pivot) := temp;
203: end loop;
204: end if;
205: for k in i+1..mat'last(1) loop -- perform Givens rotations
206: if abs(mat(k,i)) > tol
207: then Givens_Factors(mat,i,k,cosi,sine);
208: Givens_Rotation(mat,i,k,cosi,sine);
209: Givens_Rotation(rhs,i,k,cosi,sine);
210: end if;
211: end loop; -- mat(mat'first(1)..i,mat'first(2)..i)
212: exit when i > mat'last(2); -- is upper triangular
213: end loop;
214: end Upper_Triangulate;
215:
216: procedure Upper_Triangulate ( mat : in out Matrix; rhs : in out Vector;
217: tol : in double_float;
218: ipvt : out Standard_Integer_Vectors.Vector ) is
219:
220: pivot,tpi : integer;
221: pivots : Standard_Integer_Vectors.Vector(mat'range(2));
222: temp,cosi,sine : double_float;
223:
224: begin
225: for i in pivots'range loop -- initialize vector of pivots
226: pivots(i) := i;
227: end loop;
228: for i in mat'range(1) loop -- mat(mat'first(1)..i-1,mat'first(2)..i-1)
229: pivot := 0; -- is already upper triangular
230: for j in i..mat'last(2) loop -- search pivot
231: if abs(mat(i,j)) > tol
232: then pivot := j;
233: end if;
234: exit when (pivot /= 0);
235: end loop;
236: exit when (pivot = 0); -- zero row
237: if pivot /= i -- interchange columns
238: then for k in mat'range(1) loop
239: temp := mat(k,i); mat(k,i) := mat(k,pivot); mat(k,pivot) := temp;
240: end loop;
241: tpi := pivots(i); pivots(i) := pivots(pivot); pivots(pivot) := tpi;
242: end if;
243: for k in i+1..mat'last(1) loop -- perform Givens rotations
244: if abs(mat(k,i)) > tol
245: then Givens_Factors(mat,i,k,cosi,sine);
246: Givens_Rotation(mat,i,k,cosi,sine);
247: Givens_Rotation(rhs,i,k,cosi,sine);
248: end if;
249: end loop; -- mat(mat'first(1)..i,mat'first(2)..i)
250: exit when i > mat'last(2); -- is upper triangular
251: end loop;
252: ipvt := pivots;
253: end Upper_Triangulate;
254:
255: procedure Solve ( mat : in Matrix; rhs : in Vector; tol : in double_float;
256: x : out Vector ) is
257:
258: rank : natural := 0;
259: sol : vector(mat'range(1)) := (mat'range(1) => 0.0);
260:
261: begin
262: for i in mat'range(1) loop -- determine the rank of the matrix
263: if abs(mat(i,i)) > tol
264: then rank := rank + 1;
265: end if;
266: exit when ((i >= mat'last(2)) or (abs(mat(i,i)) < tol));
267: end loop;
268: for i in reverse mat'first(1)..rank loop -- back substitution
269: for j in i+1..rank loop
270: sol(i) := sol(i) + mat(i,j)*sol(j);
271: end loop;
272: sol(i) := rhs(i) - sol(i);
273: if abs(mat(i,i)) > tol
274: then sol(i) := sol(i)/mat(i,i);
275: elsif abs(sol(i)) < tol
276: then sol(i) := 1.0;
277: else return;
278: end if;
279: end loop; -- invariant : sol(i..rank) computed
280: x := sol;
281: end Solve;
282:
283: procedure Solve ( mat : in Matrix; rhs : in Vector; tol : in double_float;
284: rank : in natural; x : out Vector ) is
285:
286: sol : vector(mat'range(1)) := (mat'range(1) => 0.0);
287:
288: begin
289: for i in reverse mat'first(1)..rank loop -- back substitution
290: for j in i+1..rank loop
291: sol(i) := sol(i) + mat(i,j)*sol(j);
292: end loop;
293: sol(i) := rhs(i) - sol(i);
294: if abs(mat(i,i)) > tol
295: then sol(i) := sol(i)/mat(i,i);
296: elsif abs(sol(i)) < tol
297: then sol(i) := 1.0;
298: else return;
299: end if;
300: end loop; -- invariant : sol(i..rank) computed
301: x := sol;
302: end Solve;
303:
304: end Givens_Rotations;
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>