source: trunk/fsource/fellipse.for @ 770

Last change on this file since 770 was 770, checked in by vondreele, 9 years ago

defin e variables in fellipse.for

File size: 9.9 KB
Line 
1      SUBROUTINE FELLIPSE(NMAX,X,Y,BB,TAU,RNORM)
2
3Cf2py intent(in) NMAX
4Cf2py intent(in) X,Y
5Cf2py intent(in) Y
6Cf2py depend(NMAX) X,Y
7Cf2py intent(in)  TAU
8Cf2py intent(out) BB
9Cf2py intent(out) RNORM
10
11C
12C     NMAX is the number of data points
13C     A, B : work arrays
14C     X, Y : data points coordinates
15C
16C     Daniel Pfenniger, Geneva Observatory, 6/1991
17C
18C
19      REAL*8    X(NMAX),Y(NMAX)
20      REAL*4    A(5,NMAX),B(NMAX),BB(0:4),TAU
21      INTEGER*4 KRANK,NMAX
22      REAL*4    RNORM
23C
24C
25C     
26      CALL FITQDR(NMAX,NMAX,A,B,X,Y,TAU,KRANK,RNORM)
27
28C     Coefficients of the quadratic form
29C
30      BB(0) = B(1)
31      BB(1) = B(2)
32      BB(2) = B(3)
33      BB(3) = B(4)
34      BB(4) = B(5)
35C
36      RETURN
37      END
38C-----------------------------------------------------------------------
39C
40      SUBROUTINE FITQDR(N,NMAX,A,B,X,Y,TAU,KRANK,RNORM)
41C
42C     Fortran subroutine using the linear least squares routine HFTI
43C     finding the parameters  a, c, d, e, f  defining the best
44C     quadratic form
45C
46C            (1+a) X^2 + (1-a) Y^2 + c X Y + d X + e Y + f = 0
47C
48C     passing through a set of data points {Xi,Yi} i=1,..N
49C     The result is invariant by rotation and translation.
50C     If N<6, this program finds an exact solution passing through the
51C             points
52C     If N>5, it finds the quadratic form which minimizes
53C
54C            SUM(i=1,N)  [ (1+a)X^2 + (1-a)Y^2 + c X Y + d X + e Y + f ]^2
55C
56C     N     : Actual number of data points
57C     NMAX  : Maximum number of data points
58C     NPAR  : Number of parameters to find (5)
59C     A, B  : Work arrays
60C     X, Y  : Data point coordinates
61C     KRANK : Rank of the data matrix A (should be 5)
62C     RNORM : Norm of the residual
63C
64C     TAU   : tolerance for a zero in HFTI
65C
66C     On outpout  a,c,d,e,f  are in B(1),B(2), ... B(5) respectively
67C
68C     Daniel Pfenniger, Geneva Observatory, 6/1991
69C
70      PARAMETER (NPAR=5)
71C
72      INTEGER*4 NMAX,KRANK
73      REAL*4 A(NMAX,NPAR), B(NMAX),TAU, RNORM
74      REAL*8 X(NMAX), Y(NMAX)
75C
76C     H, G, IP : Work arrays for HFTI
77C
78      INTEGER*4 H(NPAR), IP(NPAR),I,N
79      REAL*4    G(NPAR)
80C
81C     Building the matrices A and B
82C
83      DO 10 I = 1, N
84        A(I,1) = X(I)**2-Y(I)**2
85        A(I,2) = X(I)*Y(I)
86        A(I,3) = X(I)
87        A(I,4) = Y(I)
88        A(I,5) = 1. 
89   10   B(I) = -(X(I)**2+Y(I)**2)
90C
91C     Solving the LS problem: ||A Z - B|| minimum
92C     Call HFTI, the result Z is in the first NPAR rows of B
93C     TAU is the tolerance for zero
94C     KRANK is the rank of A (should be NPAR)
95C     RNORM is the norm of the residual
96C     See Lawson & Hanson (1974) for detail
97      CALL HFTI(A,NMAX,N,NPAR,B,NMAX,1,TAU,KRANK,RNORM,H,G,IP)
98C
99      RETURN
100      END
101
102C     Some LSQ routines from Lawson & Hanson
103C
104      SUBROUTINE HFTI (A,MDA,M,N,B,MDB,NB,TAU,KRANK,RNORM,H,G,IP)
105C     C.L.LAWSON & R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12
106C     TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL 1974
107C     SOLVE LEAST SQUARES PROBLEM USING ALGORITHM HFTI.
108C
109      INTEGER*4 MDA,M,N,MDB,NB,KRANK,H
110      REAL*4 A,B,TAU,RNORM,G
111      DIMENSION A(MDA,N),B(MDB,NB),H(N),G(N),RNORM(NB)
112      INTEGER IP(N)
113      DOUBLE PRECISION SM,DZERO
114      PARAMETER (SZERO=0., DZERO=0.D0, FACTOR = 0.001 )
115C
116      K = 0
117      LDIAG = MIN0(M,N)
118      IF (LDIAG.LE.0) GOTO 270
119        DO 80 J=1,LDIAG
120          IF (J.EQ.1) GOTO 20
121C
122C     UPDATE SQUARED COLUMN LENGTHS AND FIND LMAX
123C
124          LMAX = J
125            DO 10 L = J,N
126              H(L) = H(L) - A(J-1,L)**2
127              IF (H(L).GT.H(LMAX)) LMAX = L
128   10       CONTINUE
129          IF (DIFF(HMAX+FACTOR*H(LMAX),HMAX)) 20,20,50
130C
131C     COMPUTE SQUARED COLUMN LENGTHS AND FIND LMAX
132C
133   20     LMAX = J
134            DO 40 L = J,N
135              H(L) = SZERO
136              DO 30 I = J,M
137   30           H(L) = H(L) + A(I,L)**2
138              IF (H(L).GT.H(LMAX)) LMAX = L
139   40       CONTINUE
140          HMAX = H(LMAX)
141C
142C     LMAX HAS BEEN DETERMINED
143C
144C     DO COLUMN INTERCHANGES IF NEEDED.
145C
146   50     CONTINUE
147          IP(J) = LMAX
148          IF (IP(J).EQ.J) GOTO 70
149          DO 60 I = 1,M
150            TMP = A(I,J)
151            A(I,J) = A(I,LMAX)
152   60       A(I,LMAX) = TMP
153          H(LMAX) = H(J)
154C
155C     COMPUTE THE J-TH TRANSFORMATION AND APPLY IT TO A AND B.
156C
157   70     CALL H12 (1,J,J+1,M,A(1,J),1,H(J),A(1,J+1),1,MDA,N-J)
158   80     CALL H12 (2,J,J+1,M,A(1,J),1,H(J),B,1,MDB,NB)
159C
160C     DETERMINE THE PSEUDORANK, K, USING THE TOLERANCE, TAU.
161C
162          DO 90 J = 1,LDIAG
163            IF (ABS(A(J,J)).LE.TAU) GOTO 100
164   90     CONTINUE
165      K = LDIAG
166      GOTO 110
167  100 K = J - 1
168  110 KP1 = K + 1
169C
170C     COMPUTE THE NORMS OF THE RESIDUAL VECTORS.
171C
172      IF (NB.LE.0) GOTO 140
173        DO 130 JB = 1,NB
174          TMP = SZERO
175          IF (KP1.GT.M) GOTO 130
176          DO 120 I = KP1,M
177  120       TMP = TMP + B(I,JB)**2
178  130     RNORM(JB) = SQRT(TMP)
179  140 CONTINUE
180C     SPECIAL FOR PSEUDORANK = 0
181      IF (K.GT.0) GOTO 160
182      IF (NB.LE.0) GOTO 270
183        DO 150 JB = 1,NB
184          DO 150 I = 1,N
185  150       B(I,JB) = SZERO
186      GOTO 270
187C
188C     IF THE PSEUDORANK IS LESS THAN N COMPUTE HOUSHOLDER
189C     DECOMPOSITION OF FIRST K ROWS.
190C
191  160 IF (K.EQ.N) GOTO 180
192        DO 170 II = 1,K
193          I = KP1 - II
194  170     CALL H12 (1,I,KP1,N,A(I,1),MDA,G(I),A,MDA,1,I-1)
195  180 CONTINUE
196C
197C
198      IF (NB.LE.0) GOTO 270
199        DO 260 JB = 1,NB
200C
201C     SOLVE THE K BY K TRIANGULAR SYSTEM
202C
203          DO 210 L = 1,K
204            SM = DZERO
205            I = KP1 - L
206            IF (I.EQ.K) GOTO 200
207            IP1 = I + 1
208            DO 190 J = IP1,K
209  190         SM = SM + A(I,J)*DBLE(B(J,JB))
210  200       SM1 = SM
211  210       B(I,JB) = (B(I,JB)-SM1)/A(I,I)
212C
213          IF (K.EQ.N) GOTO 240
214            DO 220 J = KP1,N
215  220         B(J,JB) = SZERO
216            DO 230 I = 1,K
217  230         CALL H12 (2,I,KP1,N,A(I,1),MDA,G(I),B(1,JB),1,MDB,1)
218C
219C     RE-ORDER THE SOLUTION VECTOR TO COMPENSATE FOR THE
220C     COLUMN INTERCHANGES.
221C
222C     COMPLETE COMPUTATION OF SOLUTION VECTOR
223C
224  240     DO 250 JJ = 1,LDIAG
225            J = LDIAG + 1 - JJ
226            IF (IP(J).EQ.J) GOTO 250
227            L = IP(J)
228            TMP = B(L,JB)
229            B(L,JB) = B(J,JB)
230            B(J,JB) = TMP
231  250     CONTINUE
232  260   CONTINUE
233C
234C     THE SOLUTION VECTORS, X, ARE NOW
235C     IN THE FIRST N ROWS OF THE ARRAY B(.).
236C
237  270 KRANK = K
238      RETURN
239      END
240C-----------------------------------------------------------------------
241C
242C     SUBROUTINE H12 (MODE,LPIVOT,L1,M,U,IUE,UP,C,ICE,ICV,NCV)
243C     C.L.LAWSON & R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUN 12
244C     TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974
245C
246C     CONSTRUCTION AND/OR APPLICATION OF A SINGLE
247C     HOUSHOLDER TRANSFORMATION:  Q = I + U*(U**T)/B
248C
249C     MODE   = 1 OR 2  TO SELECT ALGORITHM H1 OR H2
250C     LPIVOT IS THE INDEX OF THE PIVOT ELEMENT.
251C     L1,M   IF L1 .LE. M  THE TRANSFORMATION WILL BE CONSTRUCTED TO
252C            ZERO ELEMENTS INDEXED FROM L1 THROUGH M.  IF L1 .GT. M
253C            THE SUBROUTINE DOES AN IDENTITY TRANSFORMATION.
254C     U(),IUE,UP    ON ENTRY TO H1 U() CONTAINS THE PIVOT VECTOR.
255C                   IUE IS THE STORAGE INCREMENT BETWEEN ELEMENTS.
256C                   ON EXIT FROM H1 U() AND UP
257C                   CONTAIN QUANTITES DEFINING THE VECTOR U OF THE
258C                   HOUSHOLDER TRANSFORMATION.  ON ENTRY TO H2 U()
259C                   AND UP SHOULD CONTAIN QUATITIES PREVIOUSLY COMPUTED
260C                   BY H1. THESE WILL NOT BE MODIFIED BY H2.
261C     C()    ON ENTRY TO H1 OR H2 C() CONTAINS A MATRIX WHICH WILL BE
262C            REGARDED AS A SET OF VECTORS TO WHICH THE HOUSHOLDER
263C            TRANSFORMATION IS TO BE APPLIED. ON EXIT C() CONTAINS THE
264C            SET OF TRANSFORMED VECTORS.
265C     ICE    STORAGE INCREMENT BETWEEN ELEMENTS OF VECTORS IN C().
266C     ICV    STORAGE INCREMENT BETWEEN VECTORS IN C().
267C     NCV    NUMBER OF VECTORS IN C() TO BE TRANSFORMED. IF NCV .LE. 0
268C            NO OPERATIONS WILL BE DONE ON C().
269C
270      SUBROUTINE H12 (MODE,LPIVOT,L1,M,U,IUE,UP,C,ICE,ICV,NCV)
271      INTEGER*4 MODE,LPIVOT,L1,M,IUE,ICE,ICV,NCV
272      REAL*4 U,UP,C,X,Y
273      DIMENSION U(IUE,M),C(*)
274      DOUBLE PRECISION SM,B
275      PARAMETER (ONE = 1.)
276C
277      IF (0.GE.LPIVOT.OR.LPIVOT.GE.L1.OR.L1.GT.M) RETURN
278      CL = ABS(U(1,LPIVOT))
279      IF (MODE.EQ.2) GOTO 60
280C     *** CONSTRUCT THE TRANSFORMATION ***
281      DO 10 J = L1,M
282   10   CL = AMAX1(ABS(U(1,J)),CL)
283      IF (CL) 130,130,20
284   20 CLINV = ONE/CL
285      SM = (DBLE(U(1,LPIVOT))*CLINV)**2
286      DO 30 J = L1,M
287   30   SM = SM + (DBLE(U(1,J))*CLINV)**2
288C       CONVERT DBLE PREC SM TO SNGL. PREC. SM1
289      SM1 = SM
290      CL = CL*SQRT(SM1)
291      IF (U(1,LPIVOT)) 50,50,40
292   40 CL = -CL
293   50 UP = U(1,LPIVOT) - CL
294      U(1,LPIVOT) = CL
295      GOTO 70
296C     *** APPLY THE TRANSFORMATION I + U*(U**T)*B TO C ***
297C
298   60 IF (CL) 130,130,70
299   70 IF (NCV.LE.0) RETURN
300      B = DBLE(UP)*U(1,LPIVOT)
301C       B MUST BE NONPOSITIVE HERE. IF B=0.,RETURN
302C
303      IF (B) 80,130,130
304   80 B = ONE/B
305      I2 = 1 - ICV + ICE*(LPIVOT-1)
306      INCR = ICE*(L1-LPIVOT)
307      DO 120 J = 1,NCV
308        I2 = I2 +ICV
309        I3 = I2 + INCR
310        I4 = I3
311        SM = C(I2)*DBLE(UP)
312        DO 90 I = L1,M
313          SM = SM + C(I3)*DBLE(U(1,I))
314   90     I3 = I3 + ICE
315          IF (SM) 100,120,100
316  100     SM = SM*B
317          C(I2) = C(I2) + SM*DBLE(UP)
318          DO 110 I = L1,M
319            C(I4) = C(I4) + SM*DBLE(U(1,I))
320  110       I4 = I4 + ICE
321  120 CONTINUE
322  130 RETURN
323      END
324C--------------------------- this routine is not a joke!
325C
326      FUNCTION DIFF(X,Y)
327C     C.L.LAWSON & R.J.HANSON, JET PROPULSION LABORATORY, 1973 JUNE 7
328C     TO APPEAR IN 'SOLVING LEAST SQUARES PROBLEMS', PRENTICE-HALL, 1974
329      REAL X,Y
330      DIFF = X-Y
331      RETURN
332      END
Note: See TracBrowser for help on using the repository browser.