C******************************************************************* C Package LQPLAN C C Compute a least-square plane from a set of 3D coordinates. C C LQPLAN s calculate least-square plane from a set of points C LPDIST rf return distance from point to given plane C LPRMSD rf compute root-mean-square deviation of points to given plane C C Uses no separately compiled procedures. C C Copyright (C) 1988 Per Kraulis C C Per Kraulis, Dept Molecular Biology, Uppsala University Sweden. C 12-May-1988 first attempts C 13-May-1988 written C 16-May-1988 debugged C C C----------------------------------------------------------- SUBROUTINE LQPLAN (PLANE, CENTER, POINTS, NUM, ERRCOD) C INTEGER NUM, ERRCOD REAL PLANE (4), CENTER (3), POINTS (3, NUM) C C PLANE (Out) plane parameters: xyz of normal, and dist to origin C CENTER (Out) center of gravity of points C POINTS (In) coordinates of set of points C NUM (In) total number of points C ERRCOD (Out) error code C 0 = no error C 1 = too few points C 2 = ill conditioned; determinant of matrix close to zero C C MINDET minimum absolute value of determinant C REAL MINDET PARAMETER (MINDET = 1.0E-6) C C Help variables C INTEGER I, OFFSET INTEGER ORD (3) REAL FACTOR REAL XYZ (3), MTX (3, 3) C C Skip if too few points input C IF (NUM .LT. 3) THEN ERRCOD = 1 RETURN END IF C C No offset has been added to any direction C OFFSET = 0 C C Compute sum of coordinates C 100 XYZ (1) = 0.0 XYZ (2) = 0.0 XYZ (3) = 0.0 DO 200 I = 1, NUM XYZ (1) = XYZ (1) + POINTS (1, I) XYZ (2) = XYZ (2) + POINTS (2, I) XYZ (3) = XYZ (3) + POINTS (3, I) 200 CONTINUE C C Compute center of gravity, if not already done C IF (OFFSET .EQ. 0) THEN CENTER (1) = XYZ (1) / REAL (NUM) CENTER (2) = XYZ (2) / REAL (NUM) CENTER (3) = XYZ (3) / REAL (NUM) END IF C C Compute matrix elements C MTX (1, 1) = 0.0 MTX (1, 2) = 0.0 MTX (1, 3) = 0.0 MTX (2, 2) = 0.0 MTX (2, 3) = 0.0 MTX (3, 3) = 0.0 DO 300 I = 1, NUM MTX (1, 1) = MTX (1, 1) + POINTS (1, I) * POINTS (1, I) MTX (1, 2) = MTX (1, 2) + POINTS (1, I) * POINTS (2, I) MTX (1, 3) = MTX (1, 3) + POINTS (1, I) * POINTS (3, I) MTX (2, 2) = MTX (2, 2) + POINTS (2, I) * POINTS (2, I) MTX (2, 3) = MTX (2, 3) + POINTS (2, I) * POINTS (3, I) MTX (3, 3) = MTX (3, 3) + POINTS (3, I) * POINTS (3, I) 300 CONTINUE MTX (2, 1) = MTX (1, 2) MTX (3, 1) = MTX (1, 3) MTX (3, 2) = MTX (2, 3) C C Check if problem is ill conditioned C IF (ABS (MTX (1,1) * MTX (2,2) * MTX (3,3) + \$ MTX (1,2) * MTX (2,3) * MTX (3,1) + \$ MTX (1,3) * MTX (2,1) * MTX (3,2) - \$ MTX (1,1) * MTX (2,3) * MTX (3,2) - \$ MTX (1,2) * MTX (2,1) * MTX (3,3) - \$ MTX (1,3) * MTX (2,2) * MTX (3,1)) .LT. MINDET) THEN C C Try adding offset and recompute centre of gravity and matrix C cpjx type * , ' Ill conditioned' IF (OFFSET .LT. 3) THEN C C Remove old offset, if any C IF (OFFSET .GT. 0) THEN DO 400 I = 1, NUM POINTS (OFFSET, I) = POINTS (OFFSET, I) - 1.0 400 CONTINUE END IF C C Add offset, try first in x direction, then y, then z C OFFSET = OFFSET + 1 DO 450 I = 1, NUM POINTS (OFFSET, I) = POINTS (OFFSET, I) + 1.0 450 CONTINUE GOTO 100 C C If neither x, y nor z offset works, then really ill conditioned C ELSE ERRCOD = 2 RETURN END IF END IF C C Use Gauss elimination and row pivoting (or whatever it's called) C IF (ABS (MTX (1,1)) .GT. ABS (MTX (2,1))) THEN ORD (1) = 1 ELSE ORD (1) = 2 END IF IF (ABS (MTX (3,1)) .GT. ABS (MTX (ORD (1), 1))) ORD (1) = 3 DO 500 I = 1, 3 IF (I .NE. ORD (1)) THEN FACTOR = MTX (I, 1) / MTX (ORD (1), 1) MTX (I, 2) = MTX (I, 2) - FACTOR * MTX (ORD (1), 2) MTX (I, 3) = MTX (I, 3) - FACTOR * MTX (ORD (1), 3) XYZ (I) = XYZ (I) - FACTOR * XYZ (ORD (1)) END IF 500 CONTINUE C IF (ORD (1) .EQ. 1) THEN IF (ABS (MTX (2,2)) .GT. ABS (MTX (3,2))) THEN ORD (2) = 2 ORD (3) = 3 ELSE ORD (2) = 3 ORD (3) = 2 END IF ELSE IF (ORD (1) .EQ. 2) THEN IF (ABS (MTX (1,2)) .GT. ABS (MTX (3,2))) THEN ORD (2) = 1 ORD (3) = 3 ELSE ORD (2) = 3 ORD (3) = 1 END IF ELSE IF (ABS (MTX (1,2)) .GT. ABS (MTX (2,2))) THEN ORD (2) = 1 ORD (3) = 2 ELSE ORD (2) = 2 ORD (3) = 1 END IF END IF cpjx type * , ' denominator 1 = ',MTX (ORD (2), 2) FACTOR = MTX (ORD (3), 2) / MTX (ORD (2), 2) MTX (ORD (3), 3) = MTX (ORD (3), 3) - FACTOR * MTX (ORD (2), 3) XYZ (ORD (3)) = XYZ (ORD (3)) - FACTOR * XYZ (ORD (2)) C C Solve for the plane coefficients; backwards substitution C cpjx type * , ' denominator 2 = ',MTX (ORD (3), 3) PLANE (3) = XYZ (ORD (3)) / MTX (ORD (3), 3) cpjx type * , ' denominator 3 = ',MTX (ORD (2), 2) PLANE (2) = (XYZ (ORD (2)) - MTX (ORD (2), 3) * PLANE (3)) / \$ MTX (ORD (2), 2) cpjx type * , ' denominator 4 = ',MTX (ORD (1), 1) PLANE (1) = (XYZ (ORD (1)) - MTX (ORD (1), 2) * PLANE (2) - \$ MTX (ORD (1), 3) * PLANE (3)) / MTX (ORD (1), 1) PLANE (4) = SQRT (PLANE(1)**2 + PLANE(2)**2 + PLANE(3)**2) IF ( PLANE (4) .NE. 0 ) THEN PLANE (4) = 1.0 / PLANE (4) ELSE cpjx type * , ' Can''t compute plane (4)' return END IF PLANE (1) = PLANE (4) * PLANE (1) PLANE (2) = PLANE (4) * PLANE (2) PLANE (3) = PLANE (4) * PLANE (3) C C If offset added, remove it from plane parameter and coordinates C IF (OFFSET .NE. 0) THEN PLANE (4) = PLANE (4) - PLANE (OFFSET) DO 600 I = 1, NUM POINTS (OFFSET, I) = POINTS (OFFSET, I) - 1.0 600 CONTINUE END IF C ERRCOD = 0 RETURN END