! Last change: A 9 Jul 2005 9:59 pm PROGRAM Triangulation !For MPDSS: Multi Ping Distance Sensor Systems ! by: Alex Beck. IMPLICIT NONE REAL(kind=8),PARAMETER::pi = 3.1415926535897932384626433832795D0, small = 0.1, tolerance = 0.0001 INTEGER::A,F,G,H,I,J,K,L REAL(kind=8)::distance0,distance1,distance2,distance3,S0X,S0Y,S0Z,S1X,S1Y,S1Z,S2X,S2Y,S2Z,S3X,S3Y,S3Z,TX,TY,TZ,B,C,E REAL(kind=8), DIMENSION(0:5)::D REAL(kind=8), DIMENSION(0:5)::X,Y,Z !distance0-3 is a sinuglar element of distance mesured from each sensor-node subsystem. D0-3 are temp variables for each sensor. !sensor-node subsystem data. S0-3X-Z represents the sensors (X,Y,Z) cartesian coordinate data received at WDT. X,Y,Z are temp. !WRITE (*,*) "distance0: " !READ (*,*) distance0 !WRITE (*,*) "distance1: " !READ (*,*) distance1 !WRITE (*,*) "distance2: " !READ (*,*) distance2 !WRITE (*,*) "distance3: " !READ (*,*) distance3 distance0 = 1466785.0D0 distance1 = 489634.0D0 distance2 = 752222.0D0 distance3 = 1010101.0D0 S0X = 0.0D0 S0Y = 0.0D0 S0Z = 0.0D0 S1X = 1000000.0D0 S1Y = 0.0D0 S1Z = 0.0D0 S2X = 500000.0D0 S2Y = 1000000.0D0 S2Z = 1000000.0D0 S3X = 0.0D0 S3Y = 1200000.0D0 S3Z = 0.0D0 X(0) = S0X + distance0 Y(0) = S0Y Z(0) = S0Z CALL distanceFinder(X(0),Y(0),Z(0), S1Y,S1X,S1Z,D(0)) X(1) = S0X Y(1) = S0Y + distance0 Z(1)= S0Z CALL distanceFinder(X(1),Y(1),Z(1), S1Y,S1X,S1Z,D(1)) X(2) = S0X - distance0 Y(2) = S0Y Z(2) = S0Z CALL distanceFinder(X(2),Y(2),Z(2), S1Y,S1X,S1Z,D(2)) X(3) = S0X Y(3) = S0Y - distance0 Z(3) = S0Z CALL distanceFinder(X(3),Y(3),Z(3), S1Y,S1X,S1Z,D(3)) IF (ABS(D(0)-distance1) < ABS(D(1)-distance1)) THEN A = 0 ELSE A = 1 END IF IF (ABS(D(2)-distance1) < ABS(D(A)-distance1)) THEN A = 2 END IF IF (ABS(D(3)-distance1) < ABS(D(A)-distance1)) THEN A = 3 END IF WRITE (*,*) "A: ",A X(0)=X(A) Y(0)=Y(A) Z(0)=Z(A) Z(1) = Z(0) + small Z(2) = Z(0) - small IF (A==0 .OR. A==2) THEN Y(1)=Y(0) Y(2)=Y(0) CALL Sphere(distance0, Y(1), Z(1), X(1)) X(2) = X(1) ELSE X(1)=X(0) X(2)=X(0) CALL Sphere(distance0, X(1), Z(1), Y(1)) Y(2) = Y(1) END IF CALL distanceFinder(X(0),Y(0),Z(0),S1Y,S1X,S1Z,D(0)) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2)-distance1 ) < ABS( D(1)-distance1 ) ) THEN !Checks which side of the equator is closer X(3) = X(2) Y(3) = Y(2) !OVERWRITES chosen variables into '1' array slot, or leaves them there Z(3) = Z(2) D(3) = D(2) ELSE X(3) = X(1) Y(3) = Y(1) !OVERWRITES chosen variables into '1' array slot, or leaves them there Z(3) = Z(1) D(3) = D(1) END IF IF (ABS(D(0)-distance1) < ABS(D(3)-distance1)) THEN !then equator point closer WRITE (*,*) "Take care of this occurance with below GOTO statement." !!GOTO END IF IF (A==0 .OR. A==2) THEN !inputs distances from other equator points near chosen point defined by A Y(1) = Y(0) + small !!! It could be that Y(0) in this instance is always 0 Z(1) = Z(0) CALL Sphere(distance0, Y(1), Z(1), X(1)) Y(2) = Y(0) - small !!! It could be that Y(0) in this instance is always 0 Z(2) = Z(0) X(2) = X(1) !!! It could be that this statment may need to be corrected. CALL distanceFinder(X(0),Y(0),Z(0),S1Y,S1X,S1Z,D(0)) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) C = ABS( D(3) - distance1) WRITE (*,*) C READ (*,*) E IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) Y(3) = (Y(1) - Y(2))/2.0D0 + Y(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) END DO X(1) = X(3) Y(1) = Y(3) Z(1) = Z(3) X(4) = X(1) - X(0) !Begin finding location of point along 90 degree arc on sphere Y(4) = Y(1) - Y(0) Z(4) = Z(1) - Z(0) B = -X(0)/X(4) X(2) = 0 Y(2) = Y(1)*B + Y(0) Z(2) = Z(1)*B + Z(0) CALL Sphere(distance0, Y(2), X(2), Z(3)) IF (ABS(Z(3) - Z(2)) < tolerance) THEN WRITE (*,*) "WORks" ELSE WRITE (*,*) "MIGHT BE A PROBLEM" END IF ELSE X(1) = X(0) + small !!! It could be that Y(0) in this instance is always 0 Z(1) = Z(0) CALL Sphere(distance0, X(1), Z(1), Y(1)) X(2) = X(0) - small !!! It could be that Y(0) in this instance is always 0 Z(2) = Z(0) Y(2) = Y(1) !!! It could be that this statment may need to be corrected. CALL distanceFinder(X(0),Y(0),Z(0),S1Y,S1X,S1Z,D(0)) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp C = ABS( D(3) - distance1) !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp WRITE (*,*) C !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp READ (*,*) E !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) X(3) = (X(1) - X(2))/2.0D0 + X(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) END DO X(1) = X(3) Y(1) = Y(3) Z(1) = Z(3) X(4) = X(1) - X(0) !Begin finding location of point along 90 degree arc on sphere Y(4) = Y(1) - Y(0) Z(4) = Z(1) - Z(0) B = -Y(0)/Y(4) Y(2) = 0 X(2) = X(1)*B + X(0) Z(2) = Z(1)*B + Z(0) CALL Sphere(distance0, Y(2), X(2), Z(3)) IF (ABS(Z(3) - Z(2)) < tolerance) THEN WRITE (*,*) "WORks" ELSE WRITE (*,*) "MIGHT BE A PROBLEM" END IF !!!!!!!!!!!!!!!!!!!!!!!!!!!11 !!!!!!!!!!!!!!!!! !!!!!!!!!!!!!!!!!!!LOOOK FOR ERROR IN BINARY SELECTOR END IF !!!!!!!!!!!!!!! ! NOT DENING NEW 1 POSITION PARAMETER !!!!!!!!!!!!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!! SHOOTING POINT AFTER AIMING WITH ABOVE DO LOOP!!!!!!!!!!!!!!!!!!!!1111 !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE IF (A==0 .OR. A==2) THEN !defines second point for finding cirlcle of overlap of distance0 and distance1 locks down bisection. X(2) = 0.0D0 C = Z(1)/Y(1) IF (Y(1) - Y(0) > 0.0D0) THEN Y(2) = + distance0/(SQRT(C*C+1.0D0)) ELSE Y(2) = - distance0/(SQRT(C*C+1.0D0)) CALL Sphere(distance0, X(2), Y(2), Z(2)) !!!! Z(4) = C*Y(2) IF (ABS(Z(4) - Z(2)) < small) THEN WRITE (*,*) "MAKE PROGRAM FASTER" ELSE WRITE (*,*) "WE COULD HAVE A PROBLEM" END IF !!!! END IF DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1) Z(3) = C*Y(3) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO !SHOULD be locked down on cirlce at this poiint ELSE Y(2) = 0.0D0 C = Z(1)/X(1) IF (X(1) - X(0) > 0.0D0) THEN X(2) = + distance0/(SQRT(C*C+1.0D0)) ELSE X(2) = - distance0/(SQRT(C*C+1.0D0)) CALL Sphere(distance0, Y(2), X(2), Z(2)) !!!! Z(4) = C*X(2) IF (ABS(Z(4) - Z(2)) < small) THEN WRITE (*,*) "MAKE PROGRAM FASTER" ELSE WRITE (*,*) "WE COULD HAVE A PROBLEM" END IF !!!! END IF DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) X(3) = (X(2)-X(1))/2.0D0 + X(1) Z(3) = C*X(3) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO END IF X(0) = X(3) !X,Y,Z(0) NOW REPRESENTS POINT ON CIRCLE WHERE d0 and d1 intersect Y(0) = Y(3) Z(0) = Z(3) X(1) = X(0) + small*10.0D0 !Defines 4 points around cirlce point Y(1) = Y(0) CALL Sphere(distance0, Y(1), X(1), Z(1)) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) X(2) = X(0) - small*10.0D0 Y(2) = Y(0) CALL Sphere(distance0, Y(2), X(2), Z(2)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) Y(3) = Y(0) + small*10.0D0 X(3) = X(0) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) Y(4) = Y(0) - small*10.0D0 X(4) = X(0) CALL Sphere(distance0, Y(4), X(4), Z(4)) CALL distanceFinder(X(4),Y(4),Z(4),S1Y,S1X,S1Z,D(4)) A = 1 IF ( ABS(D(2) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) A = 2 END IF IF ( ABS(D(3) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(3) Y(1) = Y(3) Z(1) = Z(3) A = 3 END IF IF ( ABS(D(4) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(4) Y(1) = Y(4) Z(1) = Z(4) A = 4 END IF IF (A==3 .OR. A==4) THEN X(2) = X(1) + small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) Y(2) = Y(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) X(3) = X(1) - small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) Y(3) = Y(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) END IF X(3) = (X(2)-X(1))/2.0D0 + X(1) Y(3) = Y(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) !Bisection method to lock down on another circle point CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) X(3) = (X(2)-X(1))/2.0D0 + X(1) Y(3) = Y(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO END IF !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE IF (A==1 .OR. A==2) THEN Y(2) = Y(1) + small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) X(2) = X(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) Y(3) = Y(1) - small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) X(3) = X(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) END IF Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1) X(3) = X(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) !Bisection method to lock down on another circle point CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1) X(3) = X(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO END IF X(4) = X(3) !temporarily stores second point in X,Y,Z(4) Y(4) = Y(3) Z(4) = Z(3) !!!!!!!!!!!!!!!!DOES ENTIRE ABOVE OPERATION OVER AGAIN TO DEDUCE THIRD POINT FROM SECOND POINT X(1) = X(4) + small*10.0D0 !Defines 4 points around cirlce point Y(1) = Y(4) CALL Sphere(distance0, Y(1), X(1), Z(1)) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) X(2) = X(4) - small*10.0D0 Y(2) = Y(4) CALL Sphere(distance0, Y(2), X(2), Z(2)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) Y(3) = Y(4) + small*10.0D0 X(3) = X(4) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) Y(4) = Y(4) - small*10.0D0 X(4) = X(4) CALL Sphere(distance0, Y(4), X(4), Z(4)) CALL distanceFinder(X(4),Y(4),Z(4),S1Y,S1X,S1Z,D(4)) A = 1 IF ( ABS(D(2) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) A = 2 END IF IF ( ABS(D(3) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(3) Y(1) = Y(3) Z(1) = Z(3) A = 3 END IF IF ( ABS(D(4) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(4) Y(1) = Y(4) Z(1) = Z(4) A = 4 END IF IF (A==3 .OR. A==4) THEN X(2) = X(1) + small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) Y(2) = Y(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) X(3) = X(1) - small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) Y(3) = Y(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) END IF X(3) = (X(2)-X(1))/2.0D0 + X(1) Y(3) = Y(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) !Bisection method to lock down on another circle point CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) X(3) = (X(2)-X(1))/2.0D0 + X(1) Y(3) = Y(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO END IF !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE IF (A==1 .OR. A==2) THEN Y(2) = Y(1) + small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) X(2) = X(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) Y(3) = Y(1) - small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) X(3) = X(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) END IF Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1) X(3) = X(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) !Bisection method to lock down on another circle point CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1) X(3) = X(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO END IF X(5) = X(3) !STORES third CIRCLE POINT IN X,Y,Z(5) Y(5) = Z(3) Y(5) = Z(3) !!!!!!!!!!!!!!!!DOES ENTIRE ABOVE OPERATION OVER AGAIN TO DEDUCE THIRD POINT FROM SECOND POINT X(1) = X(5) + small*10.0D0 !Defines 4 points around cirlce point Y(1) = Y(5) CALL Sphere(distance0, Y(1), X(1), Z(1)) CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) X(2) = X(5) - small*10.0D0 Y(2) = Y(5) CALL Sphere(distance0, Y(2), X(2), Z(2)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) Y(3) = Y(5) + small*10.0D0 X(3) = X(5) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) Y(4) = Y(5) - small*10.0D0 X(4) = X(5) CALL Sphere(distance0, Y(4), X(4), Z(4)) CALL distanceFinder(X(4),Y(4),Z(4),S1Y,S1X,S1Z,D(4)) A = 1 IF ( ABS(D(2) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) A = 2 END IF IF ( ABS(D(3) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(3) Y(1) = Y(3) Z(1) = Z(3) A = 3 END IF IF ( ABS(D(4) - distance1) < ABS(D(1) - distance1) ) THEN X(1) = X(4) Y(1) = Y(4) Z(1) = Z(4) A = 4 END IF IF (A==3 .OR. A==4) THEN X(2) = X(1) + small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) Y(2) = Y(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) X(3) = X(1) - small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) Y(3) = Y(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) END IF X(3) = (X(2)-X(1))/2.0D0 + X(1) Y(3) = Y(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) !Bisection method to lock down on another circle point CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) X(3) = (X(2)-X(1))/2.0D0 + X(1) Y(3) = Y(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO END IF !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE IF (A==1 .OR. A==2) THEN Y(2) = Y(1) + small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) X(2) = X(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) Y(3) = Y(1) - small*10.0D0 !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1) X(3) = X(1) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) END IF Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1) X(3) = X(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) DO WHILE ( ABS( D(3) - distance1 ) > tolerance ) !Bisection method to lock down on another circle point CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1)) CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2)) IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN X(1) = X(2) Y(1) = Y(2) Z(1) = Z(2) END IF X(2) = X(3) Y(2) = Y(3) Z(2) = Z(3) Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1) X(3) = X(2) CALL Sphere(distance0, Y(3), X(3), Z(3)) CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3)) END DO END IF ! X,Y,Z(0) X,Y,Z(1) X,Y,Z(2) X,Y,Z(3) STORES ALL THREE POINTS ON CIRLCE X(1) = X(4) Y(1) = Y(4) Z(1) = Z(4) X(2) = X(4) Y(2) = Y(4) Z(2) = Z(4) STOP CONTAINS SUBROUTINE distanceFinder (X, Y, Z, SX, SY, SZ, distance) !Determines distance from X,Y,Z coordinates to Sensor's location REAL(KIND=8), INTENT(IN) :: X, Y, Z, SX, SY, SZ REAL(KIND=8), INTENT(OUT) :: distance distance = SQRT((SX-X)*(SX-X) + (SY-Y)*(SY-Y) + (SZ-Z)*(SZ-Z)) RETURN END SUBROUTINE distanceFinder SUBROUTINE Sphere (R, X, Y, Z) !extrapolates the third cooridinate (always positive) given 2 coordinates and the radius. REAL(KIND=8), INTENT(IN) :: R, X, Y REAL(KIND=8), INTENT(OUT) :: Z Z = SQRT( -(X*X) - (Y*Y) + (R*R) ) RETURN END SUBROUTINE Sphere SUBROUTINE Circle (R, X, Y) !extrapolates the third cooridinate (always positive) given 2 coordinates and the radius. REAL(KIND=8), INTENT(IN) :: R, X REAL(KIND=8), INTENT(OUT) :: Y Y = SQRT( -(X*X) + (R*R) ) RETURN END SUBROUTINE Circle END PROGRAM Triangulation