Commit | Line | Data |
---|---|---|
9e2bf874 KT |
1 | #include "apl.h" |
2 | ||
3 | ex_mdom() | |
4 | { | |
5 | register data *dp; | |
6 | register a; | |
7 | int i, j; | |
8 | struct item *p, *q; | |
9 | ||
10 | p = fetch1(); | |
11 | if(p->rank != 2) | |
12 | error("mdom C"); | |
13 | a = p->dim[0]; | |
14 | q = newdat(DA, 2, a*a); | |
15 | q->dim[0] = a; | |
16 | q->dim[1] = a; | |
17 | push(q); | |
18 | dp = q->datap; | |
19 | for(i=0; i<a; i++) | |
20 | for(j=0; j<a; j++) { | |
21 | datum = zero; | |
22 | if(i == j) | |
23 | datum = one; | |
24 | *dp++ = datum; | |
25 | } | |
26 | ex_ddom(); | |
27 | } | |
28 | ||
29 | ex_ddom() | |
30 | { | |
31 | struct item *p, *q; | |
32 | register a, b; | |
33 | register data *d1; | |
34 | int m, n, o; | |
35 | data *dmn, *dn1, *dn2, *dn3, *dm; | |
36 | int *in; | |
37 | ||
38 | p = fetch2(); | |
39 | q = sp[-2]; | |
40 | if(p->type != DA || q->type != DA) | |
41 | error("domino T"); | |
42 | if((p->rank != 1 && p->rank != 2) || q->rank != 2) | |
43 | error("domino C"); | |
44 | m = q->dim[0]; | |
45 | n = q->dim[1]; | |
46 | if(m < n || m != p->dim[0]) | |
47 | error("domino R"); | |
48 | o = 1; | |
49 | if(p->rank == 2) | |
50 | o = p->dim[1]; | |
51 | a = alloc(n*(SINT + SDAT*m + SDAT*3) + SDAT*m); | |
52 | if(a == -1) | |
53 | error("domino M"); | |
54 | dmn = a; | |
55 | dn1 = dmn + m*n; | |
56 | dn2 = dn1 + n; | |
57 | dn3 = dn2 + n; | |
58 | dm = dn3 + n; | |
59 | in = dm + m; | |
60 | d1 = q->datap; | |
61 | for(b=0; b<m; b++) | |
62 | for(a=0; a<n; a++) | |
63 | dmn[a*m+b] = *d1++; | |
64 | a = lsq(dmn, dn1, dn2, dn3, dm, in, m, n, o, p->datap, q->datap); | |
65 | afree(dmn); | |
66 | if(a) | |
67 | error("domino D"); | |
68 | sp--; | |
69 | pop(); | |
70 | push(p); | |
71 | p->dim[0] = n; | |
72 | p->size = n*o; | |
73 | } | |
74 | ||
75 | lsq(dmn, dn1, dn2, dn3, dm, in, m, n, p, d1, d2) | |
76 | data *dmn, *dn1, *dn2, *dn3, *dm; | |
77 | data *d1, *d2; | |
78 | int *in; | |
79 | { | |
80 | register data *dp1, *dp2; | |
81 | double f1, f2, f3, f4; | |
82 | int i, j, k, l; | |
83 | ||
84 | dp1 = dmn; | |
85 | dp2 = dn1; | |
86 | for(j=0; j<n; j++) { | |
87 | f1 = 0.; | |
88 | for(i=0; i<m; i++) { | |
89 | f2 = *dp1++; | |
90 | f1 =+ f2*f2; | |
91 | } | |
92 | *dp2++ = f1; | |
93 | in[j] = j; | |
94 | } | |
95 | for(k=0; k<n; k++) { | |
96 | f1 = dn1[k]; | |
97 | l = k; | |
98 | dp1 = dn1 + k+1; | |
99 | for(j=k+1; j<n; j++) | |
100 | if(f1 < *dp1++) { | |
101 | f1 = dp1[-1]; | |
102 | l = j; | |
103 | } | |
104 | if(l != k) { | |
105 | i = in[k]; | |
106 | in[k] = in[l]; | |
107 | in[l] = i; | |
108 | dn1[l] = dn1[k]; | |
109 | dn1[k] = f1; | |
110 | dp1 = dmn + k*m; | |
111 | dp2 = dmn + l*m; | |
112 | for(i=0; i<m; i++) { | |
113 | f1 = *dp1; | |
114 | *dp1++ = *dp2; | |
115 | *dp2++ = f1; | |
116 | } | |
117 | } | |
118 | f1 = 0.; | |
119 | dp1 = dmn + k*m + k; | |
120 | f3 = *dp1; | |
121 | for(i=k; i<m; i++) { | |
122 | f2 = *dp1++; | |
123 | f1 =+ f2*f2; | |
124 | } | |
125 | if(f1 == 0.) | |
126 | return(1); | |
127 | f2 = sqrt(f1); | |
128 | if(f3 >= 0.) | |
129 | f2 = -f2; | |
130 | dn2[k] = f2; | |
131 | f1 = 1./(f1 - f3*f2); | |
132 | dmn[k*m+k] = f3 - f2; | |
133 | for(j=k+1; j<n; j++) { | |
134 | f2 = 0.; | |
135 | dp1 = dmn + k*m + k; | |
136 | dp2 = dmn + j*m + k; | |
137 | for(i=k; i<m; i++) | |
138 | f2 =+ *dp1++ * *dp2++; | |
139 | dn3[j] = f1*f2; | |
140 | } | |
141 | for(j=k+1; j<n; j++) { | |
142 | dp1 = dmn + k*m + k; | |
143 | dp2 = dmn + j*m + k; | |
144 | f1 = dn3[j]; | |
145 | for(i=k; i<m; i++) | |
146 | *dp2++ =- *dp1++ * f1; | |
147 | f1 = dmn[j*m + k]; | |
148 | dn1[j] =- f1; | |
149 | } | |
150 | } | |
151 | for(k=0; k<p; k++) { | |
152 | dp1 = dm; | |
153 | l = k; | |
154 | for(i=0; i<m; i++) { | |
155 | *dp1++ = d1[l]; | |
156 | l =+ p; | |
157 | } | |
158 | solve(m, n, dmn, dn2, in, dm, dn3); | |
159 | l = k; | |
160 | dp1 = dm; | |
161 | for(i=0; i<m; i++) { | |
162 | f1 = -d1[l]; | |
163 | l =+ p; | |
164 | dp2 = dn3; | |
165 | for(j=0; j<n; j++) | |
166 | f1 =+ d2[i*n+j] * *dp2++; | |
167 | *dp1++ = f1; | |
168 | } | |
169 | solve(m, n, dmn, dn2, in, dm, dn1); | |
170 | f4 = 0.; | |
171 | f3 = 0.; | |
172 | dp1 = dn3; | |
173 | dp2 = dn1; | |
174 | for(i=0; i<n; i++) { | |
175 | f1 = *dp1++; | |
176 | f4 =+ f1*f1; | |
177 | f1 = *dp2++; | |
178 | f3 =+ f1*f1; | |
179 | } | |
180 | if(f3 > 0.0625*f4) | |
181 | return(1); | |
182 | loop: | |
183 | if(intflg) | |
184 | return(1); | |
185 | dp1 = dn3; | |
186 | dp2 = dn1; | |
187 | for(i=0; i<n; i++) | |
188 | *dp1++ =+ *dp2++; | |
189 | if(f3 <= 4.81e-35*f4) | |
190 | goto out; | |
191 | l = k; | |
192 | dp1 = dm; | |
193 | for(i=0; i<m; i++) { | |
194 | f1 = -d1[l]; | |
195 | l =+ p; | |
196 | dp2 = dn3; | |
197 | for(j=0; j<n; j++) | |
198 | f1 =+ d2[i*n+j] * *dp2++; | |
199 | *dp1++ = f1; | |
200 | } | |
201 | solve(m, n, dmn, dn2, in, dm, dn1); | |
202 | f2 = f3; | |
203 | f3 = 0.; | |
204 | dp1 = dn1; | |
205 | for(i=0; i<n; i++) { | |
206 | f1 = *dp1++; | |
207 | f3 =+ f1*f1; | |
208 | } | |
209 | if(f3 < 0.0625*f2) | |
210 | goto loop; | |
211 | out: | |
212 | l = k; | |
213 | dp1 = dn3; | |
214 | for(i=0; i<n; i++) { | |
215 | d1[l] = *dp1++; | |
216 | l =+ p; | |
217 | } | |
218 | } | |
219 | return(0); | |
220 | } | |
221 | ||
222 | solve(m, n, dmn, dn2, in, dm, dn1) | |
223 | data *dmn, *dn1, *dn2, *dm; | |
224 | int *in; | |
225 | { | |
226 | register data *dp1, *dp2; | |
227 | int i, j, k; | |
228 | double f1, f2; | |
229 | ||
230 | for(j=0; j<n; j++) { | |
231 | f1 = 0.; | |
232 | dp1 = dmn + j*m + j; | |
233 | dp2 = dm + j; | |
234 | f2 = *dp1; | |
235 | for(i=j; i<m; i++) | |
236 | f1 =+ *dp1++ * *dp2++; | |
237 | f1 =/ f2*dn2[j]; | |
238 | dp1 = dmn + j*m + j; | |
239 | dp2 = dm + j; | |
240 | for(i=j; i<m; i++) | |
241 | *dp2++ =+ f1 * *dp1++; | |
242 | } | |
243 | dp1 = dm + n; | |
244 | dp2 = dn2 + n; | |
245 | dn1[in[n-1]] = *--dp1 / *--dp2; | |
246 | for(i=n-2; i>=0; i--) { | |
247 | f1 = -*--dp1; | |
248 | k = (i+1)*m + i; | |
249 | for(j=i+1; j<n; j++) { | |
250 | f1 =+ dmn[k] * dn1[in[j]]; | |
251 | k =+ m; | |
252 | } | |
253 | dn1[in[i]] = -f1 / *--dp2; | |
254 | } | |
255 | } |