hdiff output

r31921/gjk.f90 2017-02-15 22:30:11.361668762 +0000 r31920/gjk.f90 2017-02-15 22:30:11.585671777 +0000
223:         INTEGER, INTENT(INOUT) :: LEN_SIMPLEX223:         INTEGER, INTENT(INOUT) :: LEN_SIMPLEX
224:         LOGICAL, INTENT(OUT) :: RESULT224:         LOGICAL, INTENT(OUT) :: RESULT
225: 225: 
226:         ! O: the origin226:         ! O: the origin
227:         ! AB, AC, AD, AO, ABPERP, ACPERP, ADPERP, ABCPERP, ACDPERP, ADBPERP:227:         ! AB, AC, AD, AO, ABPERP, ACPERP, ADPERP, ABCPERP, ACDPERP, ADBPERP:
228:         !     storage for plane tests and search direction calculation228:         !     storage for plane tests and search direction calculation
229:         ! TOL: For checking whether things are zero229:         ! TOL: For checking whether things are zero
230:         DOUBLE PRECISION :: AB(DIMS), AC(DIMS), AD(DIMS), AO(DIMS)230:         DOUBLE PRECISION :: AB(DIMS), AC(DIMS), AD(DIMS), AO(DIMS)
231:         DOUBLE PRECISION :: ABPERP(DIMS), ACPERP(DIMS), ADPERP(DIMS)231:         DOUBLE PRECISION :: ABPERP(DIMS), ACPERP(DIMS), ADPERP(DIMS)
232:         DOUBLE PRECISION :: ABCPERP(DIMS), ACDPERP(DIMS), ADBPERP(DIMS)232:         DOUBLE PRECISION :: ABCPERP(DIMS), ACDPERP(DIMS), ADBPERP(DIMS)
233:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06233:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-010
234:         ! OVERABC, OVERACD, OVERADB: Results of plane tests234:         ! OVERABC, OVERACD, OVERADB: Results of plane tests
235:         LOGICAL :: OVERABC, OVERACD, OVERADB235:         LOGICAL :: OVERABC, OVERACD, OVERADB
236: 236: 
237:         ! Initialise output237:         ! Initialise output
238:         SEARCH_DIRECTION(:) = 1.D100238:         SEARCH_DIRECTION(:) = 1.D100
239:         RESULT = .FALSE.239:         RESULT = .FALSE.
240: 240: 
241:         ! The point referred to as A is always the mose recent added to the241:         ! The point referred to as A is always the mose recent added to the
242:         ! simplex, B the second most recent, etc.242:         ! simplex, B the second most recent, etc.
243:         ! No point can be the closest to the origin. In the GJK loop we've243:         ! No point can be the closest to the origin. In the GJK loop we've
475: 475: 
476:         IMPLICIT NONE476:         IMPLICIT NONE
477: 477: 
478:         TYPE(POLYHEDRON), INTENT(IN) :: SHAPE1, SHAPE2478:         TYPE(POLYHEDRON), INTENT(IN) :: SHAPE1, SHAPE2
479:         TYPE(SUPPORT_POINT), INTENT(IN) :: START_SIMPLEX(DIMS+1)479:         TYPE(SUPPORT_POINT), INTENT(IN) :: START_SIMPLEX(DIMS+1)
480:         DOUBLE PRECISION, INTENT(OUT) :: OVERLAP_DIST, OVERLAP_VECT(DIMS)480:         DOUBLE PRECISION, INTENT(OUT) :: OVERLAP_DIST, OVERLAP_VECT(DIMS)
481:         DOUBLE PRECISION, INTENT(OUT) :: WITNESS1(DIMS), WITNESS2(DIMS)481:         DOUBLE PRECISION, INTENT(OUT) :: WITNESS1(DIMS), WITNESS2(DIMS)
482: 482: 
483:         ! FACES: Stores all the current faces483:         ! FACES: Stores all the current faces
484:         ! TEMP_FACES: Used for reallocating the array if necessary484:         ! TEMP_FACES: Used for reallocating the array if necessary
485:         ! TEMP_FACE: Used for checking the normal485:         TYPE(FACE), ALLOCATABLE :: FACES(:), TEMP_FACES(:)
486:         TYPE(FACE), ALLOCATABLE :: FACES(:), TEMP_FACES(:), TEMP_FACE 
487: 486: 
488:         ! CULL_EDGES: Stores edges while checking for culling487:         ! CULL_EDGES: Stores edges while checking for culling
489:         TYPE(EDGE), ALLOCATABLE :: CULL_EDGES(:)488:         TYPE(EDGE), ALLOCATABLE :: CULL_EDGES(:)
490: 489: 
491:         ! NEW_POINT: the new point on the surface of the Minkowski difference490:         ! NEW_POINT: the new point on the surface of the Minkowski difference
492:         TYPE(SUPPORT_POINT) :: NEW_POINT491:         TYPE(SUPPORT_POINT) :: NEW_POINT
493: 492: 
494:         ! CLOSEST_BARY: Barycentric coordinates of the origin projected onto the493:         ! CLOSEST_BARY: Barycentric coordinates of the origin projected onto the
495:         !               closest triangle494:         !               closest triangle
496:         ! MIN_DISTANCE: Keep track of the closest distance to the origin495:         ! MIN_DISTANCE: Keep track of the closest distance to the origin
497:         ! TOL: Tolerance used for deciding if we've moved closer to the origin496:         ! TOL: Tolerance used for deciding if we've moved closer to the origin
498:         DOUBLE PRECISION :: CLOSEST_BARY(DIMS), MIN_DISTANCE497:         DOUBLE PRECISION :: CLOSEST_BARY(DIMS), MIN_DISTANCE
499:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06498:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-10
500: 499: 
501:         ! CLOSEST_INDEX: Index of the closest face to the origin500:         ! CLOSEST_INDEX: Index of the closest face to the origin
502:         ! LEN_CULL: The number of faces to cull501:         ! LEN_CULL: The number of faces to cull
503:         ! MAX_FACES: Current maximum number of faces in array502:         ! MAX_FACES: Current maximum number of faces in array
504:         ! NUM_FACES: Current number of faces in the expanding simplex503:         ! NUM_FACES: Current number of faces in the expanding simplex
505:         ! NUM_NEW: The number of new faces to add504:         ! NUM_NEW: The number of new faces to add
506:         ! CULL_FACES: Which faces need to be removed at each addition505:         ! CULL_FACES: Which faces need to be removed at each addition
 506:         ! TEST_UNIT: file unit for testing purposes
507:         INTEGER :: CLOSEST_INDEX, LEN_CULL, MAX_FACES, NUM_FACES, NUM_NEW507:         INTEGER :: CLOSEST_INDEX, LEN_CULL, MAX_FACES, NUM_FACES, NUM_NEW
508:         INTEGER, ALLOCATABLE :: CULL_FACES(:)508:         INTEGER, ALLOCATABLE :: CULL_FACES(:)
509:         INTEGER :: I, J, K509:         INTEGER :: I, J, K
 510: !        INTEGER :: TEST_UNIT
510: 511: 
511:         ! INCLUDE_EDGES: Which edges to make new faces from512:         ! INCLUDE_EDGES: Which edges to make new faces from
512:         LOGICAL, ALLOCATABLE :: INCLUDE_EDGES(:)513:         LOGICAL, ALLOCATABLE :: INCLUDE_EDGES(:)
513: 514: 
514:         ! GETUNIT: for finding an unused file unit, from utils.f515:         ! GETUNIT: for finding an unused file unit, from utils.f
515:         ! TEST_UNIT: file unit for testing purposes516: !        INTEGER :: GETUNIT
516: !        INTEGER :: GETUNIT, TEST_UNIT 
517: 517: 
518:         ! Get a file unit for test_vertices.xyz518:         ! Get a file unit for test_vertices.xyz
519: !        TEST_UNIT = GETUNIT()519: !        TEST_UNIT = GETUNIT()
520: !        OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE")520: !        OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE")
521: 521: 
522: !        WRITE (*,*) 'Starting EXPANDING_POLYTOPE'522: !        WRITE (*,*) 'Starting EXPANDING_POLYTOPE'
523: 523: 
524:         ! Start by allocating 20 faces. Will be doubled if required524:         ! Start by allocating 20 faces. Will be doubled if required
525:         MAX_FACES = 20525:         MAX_FACES = 20
526:         ALLOCATE(FACES(MAX_FACES))526:         ALLOCATE(FACES(MAX_FACES))
556: !                                           FACES(I)%B%V(3)556: !                                           FACES(I)%B%V(3)
557: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%C%V(1), FACES(I)%C%V(2), &557: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%C%V(1), FACES(I)%C%V(2), &
558: !                                           FACES(I)%C%V(3)558: !                                           FACES(I)%C%V(3)
559: !            END DO559: !            END DO
560: 560: 
561:             ! First we have to identify the closest face to the origin561:             ! First we have to identify the closest face to the origin
562:             ! Fortunately, we already have all the distances562:             ! Fortunately, we already have all the distances
563:             MIN_DISTANCE = 1.D10563:             MIN_DISTANCE = 1.D10
564:             CLOSEST_INDEX = -1564:             CLOSEST_INDEX = -1
565:             DO I = 1, NUM_FACES565:             DO I = 1, NUM_FACES
566: !                WRITE (*,*) '*** I = ', I, ' *** ORIG_DIST = ', FACES(I)%ORIG_DIST566: !                WRITE (*,*) 'I = ', I, ' ORIG_DIST = ', FACES(I)%ORIG_DIST
567: !                WRITE (*,*) 'Normal = ', FACES(I)%NORMAL(:)567: !                WRITE (*,*) 'Normal = ', FACES(I)%NORMAL(1), &
568: !                WRITE (*,*) 'A = ', FACES(I)%A%V(:)568: !                                         FACES(I)%NORMAL(2), FACES(I)%NORMAL(3)
569: !                WRITE (*,*) 'B = ', FACES(I)%B%V(:) 
570: !                WRITE (*,*) 'C = ', FACES(I)%C%V(:) 
571:                 IF (FACES(I)%ORIG_DIST .LT. MIN_DISTANCE) THEN569:                 IF (FACES(I)%ORIG_DIST .LT. MIN_DISTANCE) THEN
572:                     MIN_DISTANCE = FACES(I)%ORIG_DIST570:                     MIN_DISTANCE = FACES(I)%ORIG_DIST
573:                     CLOSEST_INDEX = I571:                     CLOSEST_INDEX = I
574:                 END IF572:                 END IF
575:             END DO ! Loop over faces573:             END DO ! Loop over faces
576: 574: 
577:             ! Check that we've actually found something575:             ! Check that we've actually found something
578:             IF (CLOSEST_INDEX .EQ. -1) THEN576:             IF (CLOSEST_INDEX .EQ. -1) THEN
579:                 WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, failed to find ',&577:                 WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, failed to find ',&
580:                                   'closest distance'578:                                   'closest distance'
581:                 CLOSE(MYUNIT)579:                 CLOSE(MYUNIT)
582:                 STOP580:                 STOP
583:             END IF581:             END IF
584: 582: 
585:             WRITE (*,*) 'Found minimum distance ', MIN_DISTANCE583: !            WRITE (*,*) 'Found minimum distance ', MIN_DISTANCE
586:             ! Generate the new point on the surface of the Minkowski difference584:             ! Generate the new point on the surface of the Minkowski difference
587:             NEW_POINT%S1 = SUPPORT_FUNC(SHAPE1,  FACES(CLOSEST_INDEX)%NORMAL(:))585:             NEW_POINT%S1 = SUPPORT_FUNC(SHAPE1,  FACES(CLOSEST_INDEX)%NORMAL(:))
588:             NEW_POINT%S2 = SUPPORT_FUNC(SHAPE2, -FACES(CLOSEST_INDEX)%NORMAL(:))586:             NEW_POINT%S2 = SUPPORT_FUNC(SHAPE2, -FACES(CLOSEST_INDEX)%NORMAL(:))
589:             NEW_POINT%V(:) = NEW_POINT%S1(:) - NEW_POINT%S2(:)587:             NEW_POINT%V(:) = NEW_POINT%S1(:) - NEW_POINT%S2(:)
590: !            WRITE (*,*) 'NEW_POINT = ', NEW_POINT%V(:)588: !            WRITE (*,*) 'NEW_POINT = ', NEW_POINT%V(1), NEW_POINT%V(2),
 589: !                                        NEW_POINT%V(3)
591: 590: 
592: !            WRITE (*,*) 'Found NEW_POINT, distance check: ', &591: !            WRITE (*,*) 'Found NEW_POINT, distance check: ', &
593: !                         DOT_PRODUCT(NEW_POINT%V(:), &592: !                         DOT_PRODUCT(NEW_POINT%V(:), &
594: !                         FACES(CLOSEST_INDEX)%NORMAL(:)) &593: !                         FACES(CLOSEST_INDEX)%NORMAL(:)) &
595: !                         - FACES(CLOSEST_INDEX)%ORIG_DIST594: !                         - FACES(CLOSEST_INDEX)%ORIG_DIST
596: 595: 
597:             ! Check the distance from the origin to the support point against596:             ! Check the distance from the origin to the support point against
598:             ! the distance from the origin to the face597:             ! the distance from the origin to the face
599:             IF (DOT_PRODUCT(NEW_POINT%V(:), FACES(CLOSEST_INDEX)%NORMAL(:)) &598:             IF (DOT_PRODUCT(NEW_POINT%V(:), FACES(CLOSEST_INDEX)%NORMAL(:)) &
600:                 - FACES(CLOSEST_INDEX)%ORIG_DIST .LT. TOL) THEN599:                 - FACES(CLOSEST_INDEX)%ORIG_DIST .LT. TOL) THEN
618:                 WITNESS2(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S2(:) + &617:                 WITNESS2(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S2(:) + &
619:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S2(:) + &618:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S2(:) + &
620:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S2(:)619:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S2(:)
621:                 EXIT620:                 EXIT
622:             END IF621:             END IF
623: 622: 
624:             ! We haven't found the answer623:             ! We haven't found the answer
625: 624: 
626:             ! Do a winding check to see which faces need to be removed625:             ! Do a winding check to see which faces need to be removed
627:             ! We remove all faces that can be 'seen' from the new point626:             ! We remove all faces that can be 'seen' from the new point
628:             ! It's possible that due to numerical rounding errors, this check 
629:             ! will be incorrect. 
630:             ALLOCATE(CULL_FACES(NUM_FACES))627:             ALLOCATE(CULL_FACES(NUM_FACES))
631:             LEN_CULL = 0628:             LEN_CULL = 0
632:             DO I = 1, NUM_FACES629:             DO I = 1, NUM_FACES
633:                 IF (DOT_PRODUCT(FACES(I)%NORMAL(:), &630:                 IF (DOT_PRODUCT(FACES(I)%NORMAL(:), &
634:                     NEW_POINT%V(:) - FACES(I)%A%V(:)) .GT. 0.D0) THEN631:                     NEW_POINT%V(:) - FACES(I)%A%V(:)) .GT. 0.D0) THEN
635:                     LEN_CULL = LEN_CULL + 1632:                     LEN_CULL = LEN_CULL + 1
636:                     CULL_FACES(LEN_CULL) = I633:                     CULL_FACES(LEN_CULL) = I
637:                 END IF634:                 END IF
638:             END DO635:             END DO
639: 636: 
689:             ! Check whether we're about to exceed the FACES array bounds686:             ! Check whether we're about to exceed the FACES array bounds
690:             IF (NUM_FACES - LEN_CULL + NUM_NEW .GT. MAX_FACES) THEN687:             IF (NUM_FACES - LEN_CULL + NUM_NEW .GT. MAX_FACES) THEN
691:                 ! Reallocate the array688:                 ! Reallocate the array
692:                 ALLOCATE(TEMP_FACES(NUM_FACES))689:                 ALLOCATE(TEMP_FACES(NUM_FACES))
693:                 TEMP_FACES(:) = FACES(1:NUM_FACES)690:                 TEMP_FACES(:) = FACES(1:NUM_FACES)
694:                 DEALLOCATE(FACES)691:                 DEALLOCATE(FACES)
695:                 MAX_FACES = MAX_FACES*2692:                 MAX_FACES = MAX_FACES*2
696:                 ALLOCATE(FACES(MAX_FACES))693:                 ALLOCATE(FACES(MAX_FACES))
697:                 FACES(1:NUM_FACES) = TEMP_FACES(:)694:                 FACES(1:NUM_FACES) = TEMP_FACES(:)
698:                 DEALLOCATE(TEMP_FACES)695:                 DEALLOCATE(TEMP_FACES)
 696:                 ! Stop here for testing
 697:                 !STOP
699:             END IF698:             END IF
700: 699: 
701:             ! Testing 
702:             IF (NUM_FACES .GT. 100) THEN 
703: !                TEST_UNIT = GETUNIT() 
704: !                OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE") 
705: !                WRITE (TEST_UNIT, *) (NUM_FACES*3) 
706: !                WRITE (TEST_UNIT, *) 'Whatever' 
707: !                DO I = 1, NUM_FACES 
708: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%A%V(:) 
709: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%B%V(:) 
710: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%C%V(:) 
711: !                END DO 
712: !                CLOSE(TEST_UNIT) 
713: !                WRITE (*,*) 'EPA: High number of faces ', NUM_FACES 
714:                 STOP 
715:             END IF 
716:  
717: !            WRITE(*,*) 'INCLUDE_EDGES = ', INCLUDE_EDGES(:) 
718: !            DO I = 1, SIZE(INCLUDE_EDGES) 
719: !                WRITE(*, *) 'Edge I A = ', CULL_EDGES(I)%A%V(:), ' B = ', CULL_EDGES(I)%B%V(:) 
720: !            END DO 
721:  
722:             ! Add all the new faces700:             ! Add all the new faces
723:             ! First delete the old ones and compress the array701:             ! Keep track of which edge we're on with J
724:             ! Use J to keep track of the new index and K to keep track of the 
725:             ! cull index 
726:             J = 1702:             J = 1
727:             K = 1703:             DO I = 1, LEN_CULL
728:             ALLOCATE(TEMP_FACES(NUM_FACES - LEN_CULL))704:                 ! Start by replacing deleted faces
729:             DO I = 1, NUM_FACES705:                 DO WHILE (.NOT. INCLUDE_EDGES(J))
730:                 IF (CULL_FACES(K) .EQ. I .AND. K .LE. LEN_CULL) THEN 
731:                     K = K + 1 
732:                 ELSE 
733:                     TEMP_FACES(J) = FACES(I) 
734:                     J = J + 1706:                     J = J + 1
735:                 END IF707:                 END DO
736:             END DO708:                 FACES(CULL_FACES(I)) = NEW_FACE(CULL_EDGES(J)%A, &
737:             NUM_FACES = NUM_FACES - LEN_CULL709:                                                 CULL_EDGES(J)%B, &
738:             FACES(1:NUM_FACES) = TEMP_FACES(:)710:                                                 NEW_POINT)
739:             DEALLOCATE(TEMP_FACES)711:                 J = J + 1
740: 712:             END DO ! Loop over deleted faces
741:             ! Now add the new faces to the end713: 
742:             DO I = 1, LEN_CULL*3714:             ! Add the rest of the new faces to the end
743:                 IF (INCLUDE_EDGES(I)) THEN715:             DO I = J, LEN_CULL*3
744:                     TEMP_FACE = NEW_FACE(CULL_EDGES(I)%A, CULL_EDGES(I)%B, &716:                 IF (.NOT. INCLUDE_EDGES(I)) CYCLE
745:                                          NEW_POINT)717:                 NUM_FACES = NUM_FACES + 1
746:                     ! It's possible numerical stability errors will have got an718:                 FACES(NUM_FACES) = NEW_FACE(CULL_EDGES(I)%A, CULL_EDGES(I)%B, &
747:                     ! incorrect face in the mix. If the normal is pointing the719:                                             NEW_POINT)
748:                     ! wrong way, we know this has happened.720:             END DO ! Loop over new edges
749:                     IF (DOT_PRODUCT(TEMP_FACE%A%V(:), TEMP_FACE%NORMAL(:)) .LT.& 
750:                         0.D0) THEN 
751:                         CYCLE 
752:                     ELSE 
753:                         NUM_FACES = NUM_FACES + 1 
754:                         FACES(NUM_FACES) = TEMP_FACE 
755:                     END IF 
756:                 END IF 
757:             END DO 
758: 721: 
759:             ! Deallocate the arrays for next time round722:             ! Deallocate the arrays for next time round
760:             DEALLOCATE(CULL_FACES)723:             DEALLOCATE(CULL_FACES)
761:             DEALLOCATE(CULL_EDGES)724:             DEALLOCATE(CULL_EDGES)
762:             DEALLOCATE(INCLUDE_EDGES)725:             DEALLOCATE(INCLUDE_EDGES)
763: 726: 
764: !            WRITE (*, *) 'End of EPA loop: NUM_FACES = ', NUM_FACES 
765:         END DO ! EPA refinement loop727:         END DO ! EPA refinement loop
766: 728: 
767:         ! Deallocate the arrays for next time round 
768:         DEALLOCATE(FACES) 
769:  
770: !        WRITE (*, *) 'Ending EPA> NUM_FACES = ', NUM_FACES 
771:  
772: !        CLOSE(TEST_UNIT)729: !        CLOSE(TEST_UNIT)
773: 730: 
774:     END SUBROUTINE EXPANDING_POLYTOPE731:     END SUBROUTINE EXPANDING_POLYTOPE
775: 732: 
776: !-------------------------------------------------------------------------------733: !-------------------------------------------------------------------------------
777: ! Utility function for dot products.734: ! Utility function for dot products.
778: ! All we care about is whether or not the dot is positive or negative735: ! All we care about is whether or not the dot is positive or negative
779: ! Print a warning if it's zero736: ! Print a warning if it's zero
780: ! VEC1, VEC2: the vectors to dot737: ! VEC1, VEC2: the vectors to dot
781: ! OUT_UNIT: File unit for warnings738: ! OUT_UNIT: File unit for warnings
818:         CROSS_121(:) = VEC_CROSS(VEC_CROSS(VEC1(:), VEC2(:)), VEC1(:))775:         CROSS_121(:) = VEC_CROSS(VEC_CROSS(VEC1(:), VEC2(:)), VEC1(:))
819:     END FUNCTION CROSS_121776:     END FUNCTION CROSS_121
820: 777: 
821: !-------------------------------------------------------------------------------778: !-------------------------------------------------------------------------------
822: ! Makes a new FACE. Always use this instead of generating by hand, to make sure779: ! Makes a new FACE. Always use this instead of generating by hand, to make sure
823: ! the normal and origin distance are always calculated.780: ! the normal and origin distance are always calculated.
824: ! A, B, C: Vertices of the face781: ! A, B, C: Vertices of the face
825: !-------------------------------------------------------------------------------782: !-------------------------------------------------------------------------------
826:     TYPE(FACE) FUNCTION NEW_FACE(A, B, C)783:     TYPE(FACE) FUNCTION NEW_FACE(A, B, C)
827: 784: 
828:         ! VEC_CROSS: Vector cross product785:         ! VEC_CROSS: vector cross product
829:         ! VEC_LEN: Length of a vector786:         ! VEC_LEN: Length of a vector
830:         ! MYUNIT: File handle for GMIN output 
831:         USE VEC3, ONLY: VEC_CROSS, VEC_LEN787:         USE VEC3, ONLY: VEC_CROSS, VEC_LEN
832:         USE COMMONS, ONLY: MYUNIT 
833: 788: 
834:         IMPLICIT NONE789:         IMPLICIT NONE
835: 790: 
836:         TYPE(SUPPORT_POINT), INTENT(IN) :: A, B, C791:         TYPE(SUPPORT_POINT), INTENT(IN) :: A, B, C
837: 792: 
838:         ! AB, AC: edge vectors793:         ! AB, AC: edge vectors
839:         DOUBLE PRECISION :: AB(DIMS), AC(DIMS)794:         DOUBLE PRECISION :: AB(DIMS), AC(DIMS)
840: 795: 
841:         ! Set the vertices796:         ! Set the vertices
842:         NEW_FACE%A = A797:         NEW_FACE%A = A
849:         ! Calculate the normal (should be pointing away from the origin)804:         ! Calculate the normal (should be pointing away from the origin)
850:         NEW_FACE%NORMAL(:) = VEC_CROSS(AB(:), AC(:))805:         NEW_FACE%NORMAL(:) = VEC_CROSS(AB(:), AC(:))
851: 806: 
852:         ! Normalise807:         ! Normalise
853:         NEW_FACE%NORMAL(:) = NEW_FACE%NORMAL(:) / VEC_LEN(NEW_FACE%NORMAL(:))808:         NEW_FACE%NORMAL(:) = NEW_FACE%NORMAL(:) / VEC_LEN(NEW_FACE%NORMAL(:))
854: 809: 
855:         ! Calculate the distance to the origin810:         ! Calculate the distance to the origin
856:         NEW_FACE%ORIG_DIST = ABS(DOT_PRODUCT(NEW_FACE%NORMAL(:), &811:         NEW_FACE%ORIG_DIST = ABS(DOT_PRODUCT(NEW_FACE%NORMAL(:), &
857:                                              NEW_FACE%A%V(:)))812:                                              NEW_FACE%A%V(:)))
858: 813: 
859:         IF (ALL(AB .EQ. 0.D0) .OR. ALL(AC .EQ. 0.D0)) THEN 
860:             WRITE (MYUNIT,*) 'NEW_FACE> NaN' 
861:             WRITE (MYUNIT,*) 'A = ', A%V(:) 
862:             WRITE (MYUNIT,*) 'B = ', B%V(:) 
863:             WRITE (MYUNIT,*) 'C = ', C%V(:) 
864:             WRITE (MYUNIT,*) 'AB = ', AB(:) 
865:             WRITE (MYUNIT,*) 'AC = ', AB(:) 
866:             WRITE (MYUNIT,*) 'NORMAL = ', NEW_FACE%NORMAL(:) 
867:             STOP 
868:         END IF 
869:  
870:     END FUNCTION NEW_FACE814:     END FUNCTION NEW_FACE
871: 815: 
872: !-------------------------------------------------------------------------------816: !-------------------------------------------------------------------------------
873: ! Checks whether two edges are the same as each other (regardless of direction)817: ! Checks whether two edges are the same as each other (regardless of direction)
874: ! EDGE1, EDGE2: The two edges to check818: ! EDGE1, EDGE2: The two edges to check
875: !-------------------------------------------------------------------------------819: !-------------------------------------------------------------------------------
876:     LOGICAL FUNCTION CHECK_EDGE(EDGE1, EDGE2)820:     LOGICAL FUNCTION CHECK_EDGE(EDGE1, EDGE2)
877: 821: 
878:         IMPLICIT NONE822:         IMPLICIT NONE
879: 823: 
880:         TYPE(EDGE), INTENT(IN) :: EDGE1, EDGE2824:         TYPE(EDGE), INTENT(IN) :: EDGE1, EDGE2
881: 825: 
882:         ! TOL: numerical tolerance for testing equality826:         ! TOL: numerical tolerance for testing equality
883:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06827:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-10
884: 828: 
885:         ! DIFFAA, DIFFBB, DIFFAB, DIFFBA: differences between the vertices829:         ! DIFFAA, DIFFBB, DIFFAB, DIFFBA: differences between the vertices
886:         DOUBLE PRECISION :: DIFFAA(DIMS), DIFFBB(DIMS)830:         DOUBLE PRECISION :: DIFFAA(DIMS), DIFFBB(DIMS)
887:         DOUBLE PRECISION :: DIFFAB(DIMS), DIFFBA(DIMS)831:         DOUBLE PRECISION :: DIFFAB(DIMS), DIFFBA(DIMS)
888: 832: 
889:         DIFFAA(:) = ABS(EDGE1%A%V(:) - EDGE2%A%V(:))833:         DIFFAA(:) = ABS(EDGE1%A%V(:) - EDGE2%A%V(:))
890:         DIFFBB(:) = ABS(EDGE1%B%V(:) - EDGE2%B%V(:))834:         DIFFBB(:) = ABS(EDGE1%B%V(:) - EDGE2%B%V(:))
891:         DIFFAB(:) = ABS(EDGE1%A%V(:) - EDGE2%B%V(:))835:         DIFFAB(:) = ABS(EDGE1%A%V(:) - EDGE2%B%V(:))
892:         DIFFBA(:) = ABS(EDGE1%B%V(:) - EDGE2%A%V(:))836:         DIFFBA(:) = ABS(EDGE1%B%V(:) - EDGE2%A%V(:))
893: 837: 


legend
Lines Added 
Lines changed
 Lines Removed

hdiff - version: 2.1.0