hdiff output

r31962/gjk.f90 2017-02-23 15:30:35.744797576 +0000 r31961/gjk.f90 2017-02-23 15:30:36.360805744 +0000
 10: !   GMIN is distributed in the hope that it will be useful, 10: !   GMIN is distributed in the hope that it will be useful,
 11: !   but WITHOUT ANY WARRANTY; without even the implied warranty of 11: !   but WITHOUT ANY WARRANTY; without even the implied warranty of
 12: !   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the 12: !   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 13: !   GNU General Public License for more details. 13: !   GNU General Public License for more details.
 14: ! 14: !
 15: !   You should have received a copy of the GNU General Public License 15: !   You should have received a copy of the GNU General Public License
 16: !   along with this program; if not, write to the Free Software 16: !   along with this program; if not, write to the Free Software
 17: !   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 17: !   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 18: ! 18: !
 19: ! An implementation of the Gilbert–Johnson–Keerthi algorithm to test whether 19: ! An implementation of the Gilbert–Johnson–Keerthi algorithm to test whether
 20: ! convex shapes overlap, and the expanding polytope algorithm to find the 20: ! convex shapes overlap and the expanding polytope algorithm to find the minimum
 21: ! minimum distance move to get rid of overlap. See 21: ! distance move to get rid of overlap. See
 22: ! http://www.dyn4j.org/2010/04/gjk-gilbert-johnson-keerthi/#gjk-support 22: ! http://www.dyn4j.org/2010/04/gjk-gilbert-johnson-keerthi/#gjk-support
 23: ! (particularly the video tutorial) for an explanation of GJK and 23: ! (particularly the video tutorial) for an explanation of GJK and
 24: ! http://www.dyn4j.org/2010/05/epa-expanding-polytope-algorithm/ for EPA. 24: ! http://www.dyn4j.org/2010/05/epa-expanding-polytope-algorithm/ for EPA.
 25: ! Questions to jwrm2 25: ! Questions to jwrm2
 26:  26: 
 27: MODULE GJK_MODULE 27: MODULE GJK_MODULE
 28:  28: 
 29:     ! DIMS: Working in 3 Dimensions 29:     ! DIMS: Working in 3 Dimensions
 30:     INTEGER, PARAMETER :: DIMS = 3 30:     INTEGER, PARAMETER :: DIMS = 3
 31:  31: 
158:         END DO ! GJK refinement loop158:         END DO ! GJK refinement loop
159: 159: 
160:     END SUBROUTINE GJK_INTERSECTION160:     END SUBROUTINE GJK_INTERSECTION
161: 161: 
162: !-------------------------------------------------------------------------------162: !-------------------------------------------------------------------------------
163: ! Support function for the GJK algorithm. Returns the vertex furthest along163: ! Support function for the GJK algorithm. Returns the vertex furthest along
164: ! the direction DIR164: ! the direction DIR
165: ! POLY: the polyhedron of interest165: ! POLY: the polyhedron of interest
166: ! DIR: the search direction166: ! DIR: the search direction
167: !-------------------------------------------------------------------------------167: !-------------------------------------------------------------------------------
168:         PURE FUNCTION SUPPORT_FUNC(POLY, DIR)168:         FUNCTION SUPPORT_FUNC(POLY, DIR)
169: 169: 
170:             IMPLICIT NONE170:             IMPLICIT NONE
171: 171: 
172:             DOUBLE PRECISION :: SUPPORT_FUNC(DIMS)172:             DOUBLE PRECISION :: SUPPORT_FUNC(DIMS)
173:             DOUBLE PRECISION, INTENT(IN) :: DIR(:)173:             DOUBLE PRECISION :: DIR(:)
174:             TYPE(POLYHEDRON), INTENT(IN) :: POLY174:             TYPE(POLYHEDRON), INTENT(IN) :: POLY
175: 175: 
176:             ! BEST_INDEX: index of the best vertex176:             ! BEST_INDEX: index of the best vertex
177:             INTEGER :: I, BEST_INDEX177:             INTEGER :: I, BEST_INDEX
178:             ! DOT: temporary dot product178:             ! DOT: temporary dot product
179:             ! MAX_DOT: largest dot product between DIR and a vertex179:             ! MAX_DOT: largest dot product between DIR and a vertex
180:             DOUBLE PRECISION :: DOT, MAX_DOT180:             DOUBLE PRECISION :: DOT, MAX_DOT
181: 181: 
182:             BEST_INDEX = 1182:             BEST_INDEX = 1
183:             MAX_DOT = -1.0D10183:             MAX_DOT = -1.0D10
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:         ! TEMP_FACE: Used for checking the normal
486:         TYPE(FACE), ALLOCATABLE :: FACES(:), TEMP_FACES(:)486:         TYPE(FACE), ALLOCATABLE :: FACES(:), TEMP_FACES(:), TEMP_FACE
487:         TYPE(FACE) :: TEMP_FACE 
488: 487: 
489:         ! CULL_EDGES: Stores edges while checking for culling488:         ! CULL_EDGES: Stores edges while checking for culling
490:         TYPE(EDGE), ALLOCATABLE :: CULL_EDGES(:)489:         TYPE(EDGE), ALLOCATABLE :: CULL_EDGES(:)
491: 490: 
492:         ! NEW_POINT: the new point on the surface of the Minkowski difference491:         ! NEW_POINT: the new point on the surface of the Minkowski difference
493:         TYPE(SUPPORT_POINT) :: NEW_POINT492:         TYPE(SUPPORT_POINT) :: NEW_POINT
494: 493: 
495:         ! CLOSEST_BARY: Barycentric coordinates of the origin projected onto the494:         ! CLOSEST_BARY: Barycentric coordinates of the origin projected onto the
496:         !               closest triangle495:         !               closest triangle
497:         ! MIN_DISTANCE: Keep track of the closest distance to the origin496:         ! MIN_DISTANCE: Keep track of the closest distance to the origin
503:         ! LEN_CULL: The number of faces to cull502:         ! LEN_CULL: The number of faces to cull
504:         ! MAX_FACES: Current maximum number of faces in array503:         ! MAX_FACES: Current maximum number of faces in array
505:         ! NUM_FACES: Current number of faces in the expanding simplex504:         ! NUM_FACES: Current number of faces in the expanding simplex
506:         ! NUM_NEW: The number of new faces to add505:         ! NUM_NEW: The number of new faces to add
507:         ! CULL_FACES: Which faces need to be removed at each addition506:         ! CULL_FACES: Which faces need to be removed at each addition
508:         INTEGER :: CLOSEST_INDEX, LEN_CULL, MAX_FACES, NUM_FACES, NUM_NEW507:         INTEGER :: CLOSEST_INDEX, LEN_CULL, MAX_FACES, NUM_FACES, NUM_NEW
509:         INTEGER, ALLOCATABLE :: CULL_FACES(:)508:         INTEGER, ALLOCATABLE :: CULL_FACES(:)
510:         INTEGER :: I, J, K509:         INTEGER :: I, J, K
511: 510: 
512:         ! INCLUDE_EDGES: Which edges to make new faces from511:         ! INCLUDE_EDGES: Which edges to make new faces from
513:         ! DELETE_FACE: Used for a sanity check 
514:         LOGICAL, ALLOCATABLE :: INCLUDE_EDGES(:)512:         LOGICAL, ALLOCATABLE :: INCLUDE_EDGES(:)
515:         LOGICAL :: DELETE_FACE 
516: 513: 
517:         ! GETUNIT: for finding an unused file unit, from utils.f514:         ! GETUNIT: for finding an unused file unit, from utils.f
518:         ! TEST_UNIT, TEST_UNIT_2: file unit for testing purposes515:         ! TEST_UNIT: file unit for testing purposes
519: !        INTEGER :: GETUNIT, TEST_UNIT, TEST_UNIT_2516: !        INTEGER :: GETUNIT, TEST_UNIT
520: 517: 
521:         ! Get a file unit for test_vertices.xyz518:         ! Get a file unit for test_vertices.xyz
522: !        TEST_UNIT = GETUNIT()519: !        TEST_UNIT = GETUNIT()
523: !        OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE")520: !        OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE")
524: !        TEST_UNIT_2 = GETUNIT() 
525: !        OPEN(UNIT=TEST_UNIT_2, FILE="test_output.txt", STATUS="REPLACE") 
526: 521: 
527: !        WRITE (TEST_UNIT_2,*) 'Starting EXPANDING_POLYTOPE'522: !        WRITE (*,*) 'Starting EXPANDING_POLYTOPE'
528: 523: 
529:         ! Start by allocating 20 faces. Will be doubled if required524:         ! Start by allocating 20 faces. Will be doubled if required
530:         MAX_FACES = 20525:         MAX_FACES = 20
531:         ALLOCATE(FACES(MAX_FACES))526:         ALLOCATE(FACES(MAX_FACES))
532: 527: 
533:         ! Add the initial simplex to the array528:         ! Add the initial simplex to the array
534:         NUM_FACES = SIZE(START_SIMPLEX)529:         NUM_FACES = SIZE(START_SIMPLEX)
535:         IF (NUM_FACES .NE. 4) THEN530:         IF (NUM_FACES .NE. 4) THEN
536:             WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, START_SIMPLEX size ',&531:             WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, START_SIMPLEX size ',&
537:                               'is ', NUM_FACES, ' but should be 4'532:                               'is ', NUM_FACES, ' but should be 4'
541: 536: 
542:         ! ABC537:         ! ABC
543:         FACES(1) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(3),START_SIMPLEX(2))538:         FACES(1) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(3),START_SIMPLEX(2))
544:         ! ACD539:         ! ACD
545:         FACES(2) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(2),START_SIMPLEX(1))540:         FACES(2) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(2),START_SIMPLEX(1))
546:         ! ADB541:         ! ADB
547:         FACES(3) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(1),START_SIMPLEX(3))542:         FACES(3) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(1),START_SIMPLEX(3))
548:         ! CBD543:         ! CBD
549:         FACES(4) = NEW_FACE(START_SIMPLEX(2),START_SIMPLEX(3),START_SIMPLEX(1))544:         FACES(4) = NEW_FACE(START_SIMPLEX(2),START_SIMPLEX(3),START_SIMPLEX(1))
550: 545: 
551: !        WRITE (TEST_UNIT_2,*) 'Entering EPA loop'546: !        WRITE (*,*) 'Entering EPA loop'
552:         ! Now start the refinement loop547:         ! Now start the refinement loop
553:         DO548:         DO
554:             ! Write all the vertices to a file, for testing549:             ! Write all the vertices to a file, for testing
555: !            WRITE (TEST_UNIT, *) (NUM_FACES*3)550: !            WRITE (TEST_UNIT, *) (NUM_FACES*3)
556: !            WRITE (TEST_UNIT, *) 'Whatever'551: !            WRITE (TEST_UNIT, *) 'Whatever'
557: !            DO I = 1, NUM_FACES552: !            DO I = 1, NUM_FACES
558: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%A%V(1), FACES(I)%A%V(2), &553: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%A%V(1), FACES(I)%A%V(2), &
559: !                                           FACES(I)%A%V(3)554: !                                           FACES(I)%A%V(3)
560: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%B%V(1), FACES(I)%B%V(2), &555: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%B%V(1), FACES(I)%B%V(2), &
561: !                                           FACES(I)%B%V(3)556: !                                           FACES(I)%B%V(3)
562: !                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), &
563: !                                           FACES(I)%C%V(3)558: !                                           FACES(I)%C%V(3)
564: !            END DO559: !            END DO
565: 560: 
566:             ! First we have to identify the closest face to the origin561:             ! First we have to identify the closest face to the origin
567:             ! Fortunately, we already have all the distances562:             ! Fortunately, we already have all the distances
568:             MIN_DISTANCE = 1.D10563:             MIN_DISTANCE = 1.D10
569:             CLOSEST_INDEX = -1564:             CLOSEST_INDEX = -1
570:             DO I = 1, NUM_FACES565:             DO I = 1, NUM_FACES
571: !                WRITE (TEST_UNIT_2,*) '*** I = ', I, ' *** ORIG_DIST = ', FACES(I)%ORIG_DIST566: !                WRITE (*,*) '*** I = ', I, ' *** ORIG_DIST = ', FACES(I)%ORIG_DIST
572: !                WRITE (TEST_UNIT_2,*) 'Normal = ', FACES(I)%NORMAL(:)567: !                WRITE (*,*) 'Normal = ', FACES(I)%NORMAL(:)
573: !                WRITE (TEST_UNIT_2,*) 'A = ', FACES(I)%A%V(:)568: !                WRITE (*,*) 'A = ', FACES(I)%A%V(:)
574: !                WRITE (TEST_UNIT_2,*) 'B = ', FACES(I)%B%V(:)569: !                WRITE (*,*) 'B = ', FACES(I)%B%V(:)
575: !                WRITE (TEST_UNIT_2,*) 'C = ', FACES(I)%C%V(:)570: !                WRITE (*,*) 'C = ', FACES(I)%C%V(:)
576:                 IF (FACES(I)%ORIG_DIST .LT. MIN_DISTANCE) THEN571:                 IF (FACES(I)%ORIG_DIST .LT. MIN_DISTANCE) THEN
577:                     MIN_DISTANCE = FACES(I)%ORIG_DIST572:                     MIN_DISTANCE = FACES(I)%ORIG_DIST
578:                     CLOSEST_INDEX = I573:                     CLOSEST_INDEX = I
579:                 END IF574:                 END IF
580:             END DO ! Loop over faces575:             END DO ! Loop over faces
581: 576: 
582:             ! Check that we've actually found something577:             ! Check that we've actually found something
583:             IF (CLOSEST_INDEX .EQ. -1) THEN578:             IF (CLOSEST_INDEX .EQ. -1) THEN
584:                 WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, failed to find ',&579:                 WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, failed to find ',&
585:                                   'closest distance'580:                                   'closest distance'
586:                 CLOSE(MYUNIT)581:                 CLOSE(MYUNIT)
587:                 STOP582:                 STOP
588:             END IF583:             END IF
589: 584: 
590: !            WRITE (TEST_UNIT_2,*) 'Found minimum distance ', MIN_DISTANCE, ' Closest face index is ', CLOSEST_INDEX585:             WRITE (*,*) 'Found minimum distance ', MIN_DISTANCE
591:             ! Generate the new point on the surface of the Minkowski difference586:             ! Generate the new point on the surface of the Minkowski difference
592:             NEW_POINT%S1 = SUPPORT_FUNC(SHAPE1,  FACES(CLOSEST_INDEX)%NORMAL(:))587:             NEW_POINT%S1 = SUPPORT_FUNC(SHAPE1,  FACES(CLOSEST_INDEX)%NORMAL(:))
593:             NEW_POINT%S2 = SUPPORT_FUNC(SHAPE2, -FACES(CLOSEST_INDEX)%NORMAL(:))588:             NEW_POINT%S2 = SUPPORT_FUNC(SHAPE2, -FACES(CLOSEST_INDEX)%NORMAL(:))
594:             NEW_POINT%V(:) = NEW_POINT%S1(:) - NEW_POINT%S2(:)589:             NEW_POINT%V(:) = NEW_POINT%S1(:) - NEW_POINT%S2(:)
595: !            WRITE (TEST_UNIT_2,*) 'NEW_POINT = ', NEW_POINT%V(:)590: !            WRITE (*,*) 'NEW_POINT = ', NEW_POINT%V(:)
596: 591: 
597: !            WRITE (TEST_UNIT_2,*) 'Found NEW_POINT, distance check: ', &592: !            WRITE (*,*) 'Found NEW_POINT, distance check: ', &
598: !                         DOT_PRODUCT(NEW_POINT%V(:), &593: !                         DOT_PRODUCT(NEW_POINT%V(:), &
599: !                         FACES(CLOSEST_INDEX)%NORMAL(:)) &594: !                         FACES(CLOSEST_INDEX)%NORMAL(:)) &
600: !                         - FACES(CLOSEST_INDEX)%ORIG_DIST595: !                         - FACES(CLOSEST_INDEX)%ORIG_DIST
601: 596: 
602:             ! Check the distance from the origin to the support point against597:             ! Check the distance from the origin to the support point against
603:             ! the distance from the origin to the face598:             ! the distance from the origin to the face
604:             IF (DOT_PRODUCT(NEW_POINT%V(:), FACES(CLOSEST_INDEX)%NORMAL(:)) &599:             IF (DOT_PRODUCT(NEW_POINT%V(:), FACES(CLOSEST_INDEX)%NORMAL(:)) &
605:                 - FACES(CLOSEST_INDEX)%ORIG_DIST .LT. TOL) THEN600:                 - FACES(CLOSEST_INDEX)%ORIG_DIST .LT. TOL) THEN
606:                 ! We've found the closest point601:                 ! We've found the closest point
607:                 OVERLAP_DIST = FACES(CLOSEST_INDEX)%ORIG_DIST602:                 OVERLAP_DIST = FACES(CLOSEST_INDEX)%ORIG_DIST
616: !                WRITE(*,*) 'A%S1 = ', FACES(CLOSEST_INDEX)%C%S1(:)611: !                WRITE(*,*) 'A%S1 = ', FACES(CLOSEST_INDEX)%C%S1(:)
617: 612: 
618:                 ! Calculate the witness points613:                 ! Calculate the witness points
619:                 WITNESS1(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S1(:) + &614:                 WITNESS1(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S1(:) + &
620:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S1(:) + &615:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S1(:) + &
621:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S1(:)616:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S1(:)
622: 617: 
623:                 WITNESS2(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S2(:) + &618:                 WITNESS2(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S2(:) + &
624:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S2(:) + &619:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S2(:) + &
625:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S2(:)620:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S2(:)
626:                 EXIT ! Break out of the main EPA loop621:                 EXIT
627:             END IF622:             END IF
628: 623: 
629:             ! We haven't found the answer624:             ! We haven't found the answer
630: 625: 
631:             ! Something horrible can happen. It's possible to generate a support 
632:             ! point already in the list of vertices. If everything was working 
633:             ! well, then that means termination. However, due to numerical 
634:             ! inaccuracies, and the possibility of faces being coplanar, 
635:             ! spurious faces can be added to the list. If we have a repeated 
636:             ! vertex as the support point, we want to remove the face that 
637:             ! generated it and try again. 
638:             DELETE_FACE = .FALSE. 
639:             DO I = 1, NUM_FACES 
640:                 IF (ALL(ABS(NEW_POINT%V(:) - FACES(I)%A%V(:)) .LT. TOL) .OR. & 
641:                     ALL(ABS(NEW_POINT%V(:) - FACES(I)%B%V(:)) .LT. TOL) .OR. & 
642:                     ALL(ABS(NEW_POINT%V(:) - FACES(I)%C%V(:)) .LT. TOL)) THEN 
643:                     DELETE_FACE = .TRUE. 
644:                     EXIT 
645:                 END IF 
646:             END DO 
647:  
648:             IF (DELETE_FACE) THEN 
649:                 ! We have to delete the face 
650:                 ALLOCATE(TEMP_FACES(NUM_FACES - 1)) 
651:                 ! Use J to keep track of the new index 
652:                 J = 1 
653:                 DO I = 1, NUM_FACES 
654:                     IF (I .NE. CLOSEST_INDEX) THEN 
655:                         TEMP_FACES(J) = FACES(I) 
656:                         J = J + 1 
657:                     END IF 
658:                 END DO 
659:                 NUM_FACES = NUM_FACES - 1 
660:                 FACES(1:NUM_FACES) = TEMP_FACES(:) 
661:                 DEALLOCATE(TEMP_FACES) 
662:                 CYCLE ! Cycle main EPA loop 
663:             END IF ! End deleting a face 
664:  
665:             ! Do a winding check to see which faces need to be removed626:             ! Do a winding check to see which faces need to be removed
666:             ! We remove all faces that can be 'seen' from the new point627:             ! We remove all faces that can be 'seen' from the new point
667:             ! It's possible that due to numerical rounding errors, this check628:             ! It's possible that due to numerical rounding errors, this check
668:             ! will be incorrect.629:             ! will be incorrect.
669:             ALLOCATE(CULL_FACES(NUM_FACES))630:             ALLOCATE(CULL_FACES(NUM_FACES))
670:             LEN_CULL = 0631:             LEN_CULL = 0
671:             DO I = 1, NUM_FACES632:             DO I = 1, NUM_FACES
672: !                WRITE (TEST_UNIT_2,*) "I = ", I, " new point distance = ", & 
673: !                                      DOT_PRODUCT(FACES(I)%NORMAL(:), & 
674: !                                      NEW_POINT%V(:) - FACES(I)%A%V(:)) 
675:                 IF (DOT_PRODUCT(FACES(I)%NORMAL(:), &633:                 IF (DOT_PRODUCT(FACES(I)%NORMAL(:), &
676:                     NEW_POINT%V(:) - FACES(I)%A%V(:)) .GT. 0.D0) THEN634:                     NEW_POINT%V(:) - FACES(I)%A%V(:)) .GT. 0.D0) THEN
677:                     LEN_CULL = LEN_CULL + 1635:                     LEN_CULL = LEN_CULL + 1
678:                     CULL_FACES(LEN_CULL) = I636:                     CULL_FACES(LEN_CULL) = I
679:                 END IF637:                 END IF
680:             END DO638:             END DO
681: 639: 
682:             ! Testing only640:             ! Testing only
683: !            WRITE(TEST_UNIT_2,*) 'LEN_CULL = ', LEN_CULL641: !            WRITE(*,*) 'LEN_CULL = ', LEN_CULL
684: !            DO I = 1, LEN_CULL642: !            DO I = 1, LEN_CULL
685: !                WRITE (TEST_UNIT_2,*) 'Cull face ', CULL_FACES(I)643: !                WRITE (*,*) 'Cull face ', CULL_FACES(I)
686: !            END DO644: !            END DO
687: 645: 
688:             ! Now we loop through the culled edges. We only want to include646:             ! Now we loop through the culled edges. We only want to include
689:             ! unique edges, which gives us the boundary of the 'hole' left by647:             ! unique edges, which gives us the boundary of the 'hole' left by
690:             ! the culled faces.648:             ! the culled faces.
691:             ALLOCATE(CULL_EDGES(LEN_CULL*3))649:             ALLOCATE(CULL_EDGES(LEN_CULL*3))
692:             ALLOCATE(INCLUDE_EDGES(LEN_CULL*3))650:             ALLOCATE(INCLUDE_EDGES(LEN_CULL*3))
693:             INCLUDE_EDGES(:) = .TRUE.651:             INCLUDE_EDGES(:) = .TRUE.
694:             DO I = 1, LEN_CULL ! Loop over the culled faces652:             DO I = 1, LEN_CULL ! Loop over the culled faces
695:                 ! Add the new edges to the list653:                 ! Add the new edges to the list
745: !                TEST_UNIT = GETUNIT()703: !                TEST_UNIT = GETUNIT()
746: !                OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE")704: !                OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE")
747: !                WRITE (TEST_UNIT, *) (NUM_FACES*3)705: !                WRITE (TEST_UNIT, *) (NUM_FACES*3)
748: !                WRITE (TEST_UNIT, *) 'Whatever'706: !                WRITE (TEST_UNIT, *) 'Whatever'
749: !                DO I = 1, NUM_FACES707: !                DO I = 1, NUM_FACES
750: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%A%V(:)708: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%A%V(:)
751: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%B%V(:)709: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%B%V(:)
752: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%C%V(:)710: !                    WRITE(TEST_UNIT, *) 'O ', FACES(I)%C%V(:)
753: !                END DO711: !                END DO
754: !                CLOSE(TEST_UNIT)712: !                CLOSE(TEST_UNIT)
755:                 WRITE (*,*) 'EPA: High number of faces ', NUM_FACES713: !                WRITE (*,*) 'EPA: High number of faces ', NUM_FACES
756:                 STOP714:                 STOP
757:             END IF715:             END IF
758: 716: 
759: !            WRITE(*,*) 'INCLUDE_EDGES = ', INCLUDE_EDGES(:)717: !            WRITE(*,*) 'INCLUDE_EDGES = ', INCLUDE_EDGES(:)
760: !            DO I = 1, SIZE(INCLUDE_EDGES)718: !            DO I = 1, SIZE(INCLUDE_EDGES)
761: !                WRITE(*, *) 'Edge I A = ', CULL_EDGES(I)%A%V(:), ' B = ', CULL_EDGES(I)%B%V(:)719: !                WRITE(*, *) 'Edge I A = ', CULL_EDGES(I)%A%V(:), ' B = ', CULL_EDGES(I)%B%V(:)
762: !            END DO720: !            END DO
763: 721: 
764:             ! Add all the new faces722:             ! Add all the new faces
765:             ! First delete the old ones and compress the array723:             ! First delete the old ones and compress the array
796:                         FACES(NUM_FACES) = TEMP_FACE754:                         FACES(NUM_FACES) = TEMP_FACE
797:                     END IF755:                     END IF
798:                 END IF756:                 END IF
799:             END DO757:             END DO
800: 758: 
801:             ! Deallocate the arrays for next time round759:             ! Deallocate the arrays for next time round
802:             DEALLOCATE(CULL_FACES)760:             DEALLOCATE(CULL_FACES)
803:             DEALLOCATE(CULL_EDGES)761:             DEALLOCATE(CULL_EDGES)
804:             DEALLOCATE(INCLUDE_EDGES)762:             DEALLOCATE(INCLUDE_EDGES)
805: 763: 
806: !            WRITE (TEST_UNIT_2, *) 'End of EPA loop: NUM_FACES = ', NUM_FACES764: !            WRITE (*, *) 'End of EPA loop: NUM_FACES = ', NUM_FACES
807:         END DO ! EPA refinement loop765:         END DO ! EPA refinement loop
808: 766: 
809:         ! Deallocate the arrays for next time round767:         ! Deallocate the arrays for next time round
810:         DEALLOCATE(FACES)768:         DEALLOCATE(FACES)
811: 769: 
812: !        WRITE (TEST_UNIT_2, *) 'Ending EPA> NUM_FACES = ', NUM_FACES770: !        WRITE (*, *) 'Ending EPA> NUM_FACES = ', NUM_FACES
813: 771: 
814: !        CLOSE(TEST_UNIT)772: !        CLOSE(TEST_UNIT)
815: !        CLOSE(TEST_UNIT_2) 
816: 773: 
817:     END SUBROUTINE EXPANDING_POLYTOPE774:     END SUBROUTINE EXPANDING_POLYTOPE
818: 775: 
819: !-------------------------------------------------------------------------------776: !-------------------------------------------------------------------------------
820: ! Utility function for dot products.777: ! Utility function for dot products.
821: ! All we care about is whether or not the dot is positive or negative778: ! All we care about is whether or not the dot is positive or negative
822: ! Print a warning if it's zero779: ! Print a warning if it's zero
823: ! VEC1, VEC2: the vectors to dot780: ! VEC1, VEC2: the vectors to dot
824: ! OUT_UNIT: File unit for warnings781: ! OUT_UNIT: File unit for warnings
825: !-------------------------------------------------------------------------------782: !-------------------------------------------------------------------------------
841:         ELSE798:         ELSE
842:             SAME_DIRECTION = (DOT .GT. 0.D0)799:             SAME_DIRECTION = (DOT .GT. 0.D0)
843:         END IF800:         END IF
844:     END FUNCTION SAME_DIRECTION801:     END FUNCTION SAME_DIRECTION
845: 802: 
846: !-------------------------------------------------------------------------------803: !-------------------------------------------------------------------------------
847: ! Utility function for cross products.804: ! Utility function for cross products.
848: ! Carries out VEC1 x VEC2 x VEC1805: ! Carries out VEC1 x VEC2 x VEC1
849: ! VEC1, VEC2: the vectors to cross806: ! VEC1, VEC2: the vectors to cross
850: !-------------------------------------------------------------------------------807: !-------------------------------------------------------------------------------
851:     PURE FUNCTION CROSS_121(VEC1, VEC2)808:     FUNCTION CROSS_121(VEC1, VEC2)
852: 809: 
853:         ! VEC_CROSS: cross producr810:         ! VEC_CROSS: cross producr
854:         USE VEC3, ONLY: VEC_CROSS811:         USE VEC3, ONLY: VEC_CROSS
855: 812: 
856:         IMPLICIT NONE813:         IMPLICIT NONE
857: 814: 
858:         DOUBLE PRECISION, INTENT(IN) :: VEC1(DIMS), VEC2(DIMS)815:         DOUBLE PRECISION, INTENT(IN) :: VEC1(DIMS), VEC2(DIMS)
859:         DOUBLE PRECISION :: CROSS_121(DIMS)816:         DOUBLE PRECISION :: CROSS_121(DIMS)
860: 817: 
861:         CROSS_121(:) = VEC_CROSS(VEC_CROSS(VEC1(:), VEC2(:)), VEC1(:))818:         CROSS_121(:) = VEC_CROSS(VEC_CROSS(VEC1(:), VEC2(:)), VEC1(:))
909:             WRITE (MYUNIT,*) 'NORMAL = ', NEW_FACE%NORMAL(:)866:             WRITE (MYUNIT,*) 'NORMAL = ', NEW_FACE%NORMAL(:)
910:             STOP867:             STOP
911:         END IF868:         END IF
912: 869: 
913:     END FUNCTION NEW_FACE870:     END FUNCTION NEW_FACE
914: 871: 
915: !-------------------------------------------------------------------------------872: !-------------------------------------------------------------------------------
916: ! Checks whether two edges are the same as each other (regardless of direction)873: ! Checks whether two edges are the same as each other (regardless of direction)
917: ! EDGE1, EDGE2: The two edges to check874: ! EDGE1, EDGE2: The two edges to check
918: !-------------------------------------------------------------------------------875: !-------------------------------------------------------------------------------
919:     PURE LOGICAL FUNCTION CHECK_EDGE(EDGE1, EDGE2)876:     LOGICAL FUNCTION CHECK_EDGE(EDGE1, EDGE2)
920: 877: 
921:         IMPLICIT NONE878:         IMPLICIT NONE
922: 879: 
923:         TYPE(EDGE), INTENT(IN) :: EDGE1, EDGE2880:         TYPE(EDGE), INTENT(IN) :: EDGE1, EDGE2
924: 881: 
925:         ! TOL: numerical tolerance for testing equality882:         ! TOL: numerical tolerance for testing equality
926:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06883:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06
927: 884: 
928:         ! DIFFAA, DIFFBB, DIFFAB, DIFFBA: differences between the vertices885:         ! DIFFAA, DIFFBB, DIFFAB, DIFFBA: differences between the vertices
929:         DOUBLE PRECISION :: DIFFAA(DIMS), DIFFBB(DIMS)886:         DOUBLE PRECISION :: DIFFAA(DIMS), DIFFBB(DIMS)
946: !        WRITE(*, *) 'Result is: ', CHECK_EDGE903: !        WRITE(*, *) 'Result is: ', CHECK_EDGE
947: 904: 
948:     END FUNCTION CHECK_EDGE905:     END FUNCTION CHECK_EDGE
949: 906: 
950: !-------------------------------------------------------------------------------907: !-------------------------------------------------------------------------------
951: ! Calculates the projection of the origin onto a triangle in barycentric908: ! Calculates the projection of the origin onto a triangle in barycentric
952: ! coordinates909: ! coordinates
953: ! See the bottom of http://hacktank.net/blog/?p=119910: ! See the bottom of http://hacktank.net/blog/?p=119
954: ! TRIANG: the face we are calculating for911: ! TRIANG: the face we are calculating for
955: !-------------------------------------------------------------------------------912: !-------------------------------------------------------------------------------
956:     PURE FUNCTION BARYCENTRIC(TRIANG)913:     FUNCTION BARYCENTRIC(TRIANG)
957: 914: 
958:         IMPLICIT NONE915:         IMPLICIT NONE
959: 916: 
960:         TYPE(FACE), INTENT(IN) :: TRIANG917:         TYPE(FACE), INTENT(IN) :: TRIANG
961:         DOUBLE PRECISION :: BARYCENTRIC(DIMS)918:         DOUBLE PRECISION :: BARYCENTRIC(DIMS)
962: 919: 
963:         ! V0, V1, V2, D00, D01, D11, D20, D21, DENOM: temporary quantities920:         ! V0, V1, V2, D00, D01, D11, D20, D21, DENOM: temporary quantities
964:         DOUBLE PRECISION :: V0(DIMS), V1(DIMS), V2(DIMS)921:         DOUBLE PRECISION :: V0(DIMS), V1(DIMS), V2(DIMS)
965:         DOUBLE PRECISION :: D00, D01, D11, D20, D21, DENOM922:         DOUBLE PRECISION :: D00, D01, D11, D20, D21, DENOM
966: 923: 


r31962/vec3.f90 2017-02-23 15:30:36.032801395 +0000 r31961/vec3.f90 2017-02-23 15:30:36.652809616 +0000
  1: ! vr274> this module contains various helper function for handling  1: ! vr274> this module contains various helper function for handling 
  2: !        3 dimensional vectors + matrices. This can be useful for  2: !        3 dimensional vectors + matrices. This can be useful for
  3: !        coordinate handling.  3: !        coordinate handling.
  4: module vec3  4: module vec3
  5: contains  5: contains
  6:   6: 
  7:     ! length of vector  7:     ! length of vector
  8:     pure function vec_len(v)  8:     function vec_len(v)
  9:         implicit none  9:         implicit none
 10:         double precision, intent(in) :: v(3) 10:         double precision, intent(in) :: v(3)
 11:         double precision vec_len 11:         double precision vec_len
 12:         vec_len = sqrt(dot_product(v,v)) 12:         vec_len = sqrt(dot_product(v,v))
 13:     end function 13:     end function
 14:  14: 
 15:     ! angle between 2 vectors in radians 15:     ! angle between 2 vectors in radians
 16:     pure function vec_angle(v1, v2) 16:     function vec_angle(v1, v2)
 17:         implicit none 17:         implicit none
 18:         double precision, intent(in) :: v1(3), v2(3) 18:         double precision, intent(in) :: v1(3), v2(3)
 19:         double precision vec_angle 19:         double precision vec_angle
 20:         vec_angle = acos(dot_product(v1, v2) / (vec_len(v1) * vec_len(v2))) 20:         vec_angle = acos(dot_product(v1, v2) / (vec_len(v1) * vec_len(v2)))
 21:     end function 21:     end function
 22:  22: 
 23:     ! cross product of 2 vectors 23:     ! cross product of 2 vectors
 24:     pure function vec_cross(v1, v2) result(v3) 24:     function vec_cross(v1, v2) result(v3)
 25:         implicit none 25:         implicit none
 26:         double precision, intent(in) :: v1(3), v2(3) 26:         double precision, intent(in) :: v1(3), v2(3)
 27:         double precision v3(3) 27:         double precision v3(3)
 28:         v3(1) = v1(2)*v2(3) - v1(3)*v2(2) 28:         v3(1) = v1(2)*v2(3) - v1(3)*v2(2)
 29:         v3(2) = v1(3)*v2(1) - v1(1)*v2(3) 29:         v3(2) = v1(3)*v2(1) - v1(1)*v2(3)
 30:         v3(3) = v1(1)*v2(2) - v1(2)*v2(1) 30:         v3(3) = v1(1)*v2(2) - v1(2)*v2(1)
 31:     end function 31:     end function
 32:  32: 
 33:     ! dyadic product of 2 vectors 33:     ! dyadic product of 2 vectors
 34:     pure function vec_dyad(v1, v2) result(m) 34:     function vec_dyad(v1, v2) result(m)
 35:         implicit none 35:         implicit none
 36:         double precision, intent(in) :: v1(3), v2(3) 36:         double precision, intent(in) :: v1(3), v2(3)
 37:         double precision m(3,3) 37:         double precision m(3,3)
 38:         integer i,j 38:         integer i,j
 39:         m=0d0 39:         m=0d0
 40:         do i=1,3 40:         do i=1,3
 41:           do j=i,3 41:           do j=i,3
 42:              m(i,j) = v1(i)*v2(j) 42:              m(i,j) = v1(i)*v2(j)
 43:           end do 43:           end do
 44:         end do 44:         end do
 45:     end function 45:     end function
 46:  46: 
 47:     ! uniform random unit vector 47:     ! uniform random unit vector 
 48:     function vec_random() result(p) 48:     function vec_random() result(p)
 49:         double precision :: p(3) 49:         double precision :: p(3)
 50:         double precision :: PI 50:         double precision :: PI
 51:         double precision :: dprand 51:         double precision :: dprand
 52:         parameter (pi=3.141592654d0) 52:         parameter (pi=3.141592654d0)
 53:         double precision z 53:         double precision z
 54:         double precision u(2) 54:         double precision u(2)
 55:         u = (/dprand(),dprand()/) 55:         u = (/dprand(),dprand()/)
 56:         z = 2*u(1) - 1d0 56:         z = 2*u(1) - 1d0
 57:         p(1) = sqrt(1-z*z) * cos(2d0*PI*u(2)) 57:         p(1) = sqrt(1-z*z) * cos(2d0*PI*u(2))
 58:         p(2) = sqrt(1-z*z) * sin(2d0*PI*u(2)) 58:         p(2) = sqrt(1-z*z) * sin(2d0*PI*u(2))
 59:         p(3) = z 59:         p(3) = z
 60:     end function 60:     end function
 61:  61: 
 62:     ! invert a 3x3 matrix 62:     ! invert a 3x3 matrix
 63:     pure subroutine invert3x3 (A, Ainv) 63:     subroutine invert3x3 (A, Ainv)
 64:         double precision, intent(in) :: A(3,3) 64:         double precision, intent(in) :: A(3,3)
 65:         double precision, intent(out) ::Ainv(3,3) 65:         double precision, intent(out) ::Ainv(3,3)
 66:         double precision Adet 66:         double precision Adet
 67:  67: 
 68:         Ainv = 0d+0; 68:         Ainv = 0d+0;
 69:         Ainv(1,1) = A(2,2)*A(3,3) - A(3,2)*A(2,3) 69:         Ainv(1,1) = A(2,2)*A(3,3) - A(3,2)*A(2,3)
 70:         Ainv(1,2) = A(3,2)*A(1,3) - A(1,2)*A(3,3) 70:         Ainv(1,2) = A(3,2)*A(1,3) - A(1,2)*A(3,3)
 71:         Ainv(1,3) = A(1,2)*A(2,3) - A(1,3)*A(2,2) 71:         Ainv(1,3) = A(1,2)*A(2,3) - A(1,3)*A(2,2)
 72:  72: 
 73:         Adet = Ainv(1,1)*A(1,1) + Ainv(1,2)*A(2,1) + Ainv(1,3)*A(3,1) 73:         Adet = Ainv(1,1)*A(1,1) + Ainv(1,2)*A(2,1) + Ainv(1,3)*A(3,1)
 79:         Ainv(2,1) = (A(2,3)*A(3,1) - A(2,1)*A(3,3))/Adet 79:         Ainv(2,1) = (A(2,3)*A(3,1) - A(2,1)*A(3,3))/Adet
 80:         Ainv(2,2) = (A(1,1)*A(3,3) - A(3,1)*A(1,3))/Adet 80:         Ainv(2,2) = (A(1,1)*A(3,3) - A(3,1)*A(1,3))/Adet
 81:         Ainv(2,3) = (A(2,1)*A(1,3) - A(1,1)*A(2,3))/Adet 81:         Ainv(2,3) = (A(2,1)*A(1,3) - A(1,1)*A(2,3))/Adet
 82:  82: 
 83:         Ainv(3,1) = (A(2,1)*A(3,2) - A(2,2)*A(3,1))/Adet 83:         Ainv(3,1) = (A(2,1)*A(3,2) - A(2,2)*A(3,1))/Adet
 84:         Ainv(3,2) = (A(3,1)*A(1,2) - A(1,1)*A(3,2))/Adet 84:         Ainv(3,2) = (A(3,1)*A(1,2) - A(1,1)*A(3,2))/Adet
 85:         Ainv(3,3) = (A(1,1)*A(2,2) - A(1,2)*A(2,1))/Adet 85:         Ainv(3,3) = (A(1,1)*A(2,2) - A(1,2)*A(2,1))/Adet
 86:         return 86:         return
 87:     end subroutine invert3x3 87:     end subroutine invert3x3
 88:  88: 
 89:     pure function det3x3(A) result(Adet) 89:     function det3x3(A) result(Adet)
 90:         implicit none 90:         implicit none
 91:         double precision, intent(in) :: A(3,3) 91:         double precision, intent(in) :: A(3,3)
 92:         double precision Adet, tmp(3) 92:         double precision Adet, tmp(3)
 93:  93: 
 94:         tmp(1) = A(2,2)*A(3,3) - A(3,2)*A(2,3) 94:         tmp(1) = A(2,2)*A(3,3) - A(3,2)*A(2,3)
 95:         tmp(2) = A(3,2)*A(1,3) - A(1,2)*A(3,3) 95:         tmp(2) = A(3,2)*A(1,3) - A(1,2)*A(3,3)
 96:         tmp(3) = A(1,2)*A(2,3) - A(1,3)*A(2,2) 96:         tmp(3) = A(1,2)*A(2,3) - A(1,3)*A(2,2)
 97:  97: 
 98:         Adet = tmp(1)*A(1,1) + tmp(2)*A(2,1) + tmp(3)*A(3,1) 98:         Adet = tmp(1)*A(1,1) + tmp(2)*A(2,1) + tmp(3)*A(3,1)
 99:     end function 99:     end function
100: 100: 
101: 101: 
102:     pure subroutine identity3x3(A)102:     subroutine identity3x3(A)
103:         double precision, intent(out) :: A(3,3)103:         double precision, intent(out) :: A(3,3)
104:         A=0d0104:         A=0d0
105:         A(1,1)=1d0105:         A(1,1)=1d0
106:         A(2,2)=1d0106:         A(2,2)=1d0
107:         A(3,3)=1d0107:         A(3,3)=1d0
108:     end subroutine108:     end subroutine
109: end module vec3109: end module vec3


legend
Lines Added 
Lines changed
 Lines Removed

hdiff - version: 2.1.0