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