hdiff output

r31886/convex_polyhedra.f90 2017-02-03 23:30:16.636161684 +0000 r31885/convex_polyhedra.f90 2017-02-03 23:30:17.732176277 +0000
 18: ! 18: !
 19: ! Potential for convex polyhedra. The polyhedra only interact when they are. In 19: ! Potential for convex polyhedra. The polyhedra only interact when they are. In
 20: ! this case, there is a harmonic repulsion based on the smallest distance 20: ! this case, there is a harmonic repulsion based on the smallest distance
 21: ! required to make them not overlap. Additionally, there is a harmonic 21: ! required to make them not overlap. Additionally, there is a harmonic
 22: ! compression towards the origin on all particles, to promote dense packing. 22: ! compression towards the origin on all particles, to promote dense packing.
 23: ! Questions to jwrm2. 23: ! Questions to jwrm2.
 24:  24: 
 25: MODULE CONVEX_POLYHEDRA_MODULE 25: MODULE CONVEX_POLYHEDRA_MODULE
 26:  26: 
 27: ! POLYHEDRON: type to store a polyhedron 27: ! POLYHEDRON: type to store a polyhedron
 28:     USE GJK_MODULE, ONLY: POLYHEDRON, DIMS 28:     USE GJK_UTILS, ONLY: POLYHEDRON
 29:  29: 
 30:     IMPLICIT none 30:     IMPLICIT none
 31:  31: 
 32:     ! NPOLY: Number of polyhedra 32:     ! NPOLY: Number of polyhedra
  33:     ! NVERTS: Number of vertices in each polyhedron
 33:     ! X_SIZE: Size of the coordinates array 34:     ! X_SIZE: Size of the coordinates array
 34:     INTEGER :: NPOLY, X_SIZE 35:     INTEGER :: NPOLY, X_SIZE
 35:  36: 
 36:     ! The array of polyhedra 37:     ! The array of polyhedra
 37:     TYPE(POLYHEDRON), ALLOCATABLE :: POLYHEDRA(:) 38:     TYPE(POLYHEDRON), ALLOCATABLE :: POLYHEDRA(:)
 38:  39: 
 39:     ! Array of the reference vertex positions 40:     ! Array of the reference vertex positions
 40:     DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:) :: VERTS_0 41:     DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:) :: VERTS_0
 41:  42: 
 42:     PRIVATE :: VERTS_0, NPOLY, X_SIZE, POLYHEDRA, UPDATE_POLYHEDRON 
 43:  
 44: CONTAINS 43: CONTAINS
 45: !------------------------------------------------------------------------------- 44: !-------------------------------------------------------------------------------
 46: ! Initialise the system, including reading the vertex list from the file 45: ! Initialise the system, including reading the vertex list from the file
 47: ! "polyhedron_vertices.dat" 46: ! "polyhedron_vertices.dat"
 48: !------------------------------------------------------------------------------- 47: !-------------------------------------------------------------------------------
 49:     SUBROUTINE INITIALISE_POLYHEDRA() 48:     SUBROUTINE INITIALISE_POLYHEDRA()
 50:  49: 
 51:         ! MYUNIT: File unit of GMIN_out 50:         ! MYUNIT: File unit of GMIN_out
 52:         ! NATOMS: the number of coordinate lines in the coords file, so twice 51:         ! NATOMS: the number of coordinate lines in the coords file, so twice
 53:         !         the actual number of bodies 52:         !         the actual number of bodies
 54:         ! DIMS: Number of dimensions of the system 53:         ! DIM: Number of dimensions of the system
 55:         ! NVERTS: the number of vertices per polyhedron 54:         ! NVERTS: the number of vertices per polyhedron
 56:         ! VEC_CROSS: finds the cross product of two vectors 55:         ! VEC_CROSS: finds the cross product of two vectors
 57:         USE COMMONS, ONLY: MYUNIT, NATOMS 56:         USE COMMONS, ONLY: MYUNIT, NATOMS
 58:         USE GJK_MODULE, ONLY: DIMS, NVERTS 57:         USE GJK_UTILS, ONLY: DIM, NVERTS
 59:         USE VEC3, ONLY: VEC_CROSS 58:         USE VEC3, ONLY: VEC_CROSS
 60:  59: 
 61:         IMPLICIT NONE 60:         IMPLICIT NONE
 62:  61: 
 63:         ! GETUNIT: Function for finding an unused file unit, from utils.f 62:         ! GETUNIT: Function for finding an unused file unit, from utils.f
 64:         ! VERT_UNIT: File unit for "polyhedron_vertices.dat" 63:         ! VERT_UNIT: File unit for "polyhedron_vertices.dat"
 65:         INTEGER :: GETUNIT, VERT_UNIT, I 64:         INTEGER :: GETUNIT, VERT_UNIT, I
 66:  65: 
 67:         ! NORMAL: Normal to the first face read 66:         ! NORMAL: Normal to the first face read
 68:         ! TOLERANCE: Numerical tolerance for coplanar test 67:         ! TOLERANCE: Numerical tolerance for coplanar test
 69:         DOUBLE PRECISION :: NORMAL(DIMS) 68:         DOUBLE PRECISION :: NORMAL(DIM)
 70:         DOUBLE PRECISION, PARAMETER :: TOL = 1.0D-6 69:         DOUBLE PRECISION, PARAMETER :: TOL = 1.0D-6
 71:  70: 
 72:         ! COPLANAR: whether all the vertices are coplanar 71:         ! COPLANAR: whether all the vertices are coplanar
 73:         LOGICAL :: COPLANAR 72:         LOGICAL :: COPLANAR
 74:  73: 
 75:         ! Set global module parameters 74:         ! Set global module parameters
 76:         NPOLY = NATOMS / 2 75:         NPOLY = NATOMS / 2
 77:         X_SIZE = NATOMS * 3 76:         X_SIZE = NATOMS * 3
 78:  77: 
 79:         WRITE (MYUNIT, *) 'INITIALISE_POLYHEDRA> ', NPOLY, ' polyhedra' 78:         WRITE (MYUNIT, *) 'INITIALISE_POLYHEDRA> ', NPOLY, ' polyhedra'
 94:         READ(VERT_UNIT, *) NVERTS 93:         READ(VERT_UNIT, *) NVERTS
 95:  94: 
 96:         ! Check that we've got at least four vertices 95:         ! Check that we've got at least four vertices
 97:         IF (NVERTS .LT. 4) THEN 96:         IF (NVERTS .LT. 4) THEN
 98:             WRITE (MYUNIT, *) & 97:             WRITE (MYUNIT, *) &
 99:                 'INITIALISE_POLYHEDA> ERROR: require at least 4 vertices' 98:                 'INITIALISE_POLYHEDA> ERROR: require at least 4 vertices'
100:             STOP 99:             STOP
101:         END IF100:         END IF
102: 101: 
103:         ! Read the reference vertices102:         ! Read the reference vertices
104:         ALLOCATE(VERTS_0(DIMS, NVERTS))103:         ALLOCATE(VERTS_0(DIM, NVERTS))
105:         DO I = 1, NVERTS104:         DO I = 1, NVERTS
106:             READ(VERT_UNIT, *) VERTS_0(1,I), VERTS_0(2,I), VERTS_0(3,I)105:             READ(VERT_UNIT, *) VERTS_0(1,I), VERTS_0(2,I), VERTS_0(3,I)
107:         END DO106:         END DO
108: 107: 
109:         ! Check that the vertices are not all coplanar108:         ! Check that the vertices are not all coplanar
110:         NORMAL(:) = VEC_CROSS((VERTS_0(:, 1) - VERTS_0(:, 2)), &109:         NORMAL(:) = VEC_CROSS((VERTS_0(:, 1) - VERTS_0(:, 2)), &
111:                                (VERTS_0(:, 1) - VERTS_0(:, 3)))110:                                (VERTS_0(:, 1) - VERTS_0(:, 3)))
112:         COPLANAR = .TRUE.111:         COPLANAR = .TRUE.
113:         DO I = 4, NVERTS ! Loop over vertices112:         DO I = 4, NVERTS ! Loop over vertices
114:             IF (ABS(DOT_PRODUCT((VERTS_0(:, I) - VERTS_0(:, 1)), NORMAL)) &113:             IF (ABS(DOT_PRODUCT((VERTS_0(:, I) - VERTS_0(:, 1)), NORMAL)) &
120: 119: 
121:         ! Error out if the vertices are all coplanar120:         ! Error out if the vertices are all coplanar
122:         IF (COPLANAR) THEN121:         IF (COPLANAR) THEN
123:             WRITE (MYUNIT, *) &122:             WRITE (MYUNIT, *) &
124:                 'INITIALISE_POLYHEDRA> ERROR: all vertices are coplanar'123:                 'INITIALISE_POLYHEDRA> ERROR: all vertices are coplanar'
125:             STOP124:             STOP
126:         END IF125:         END IF
127: 126: 
128:         ! Copy the reference vertices to the polyhedra127:         ! Copy the reference vertices to the polyhedra
129:         DO I = 1, NPOLY128:         DO I = 1, NPOLY
130:             ALLOCATE(POLYHEDRA(I)%VERTS(DIMS, NVERTS))129:             ALLOCATE(POLYHEDRA(I)%VERTS(DIM, NVERTS))
131:             POLYHEDRA(I)%VERTS(:, :) = VERTS_0(:, :)130:             POLYHEDRA(I)%VERTS(:, :) = VERTS_0(:, :)
132:         END DO131:         END DO
133: 132: 
134:     END SUBROUTINE INITIALISE_POLYHEDRA133:     END SUBROUTINE INITIALISE_POLYHEDRA
135: 134: 
136: !-------------------------------------------------------------------------------135: !-------------------------------------------------------------------------------
137: ! Updates a polyhedron with the rotation matrix and derivatives,136: ! Updates a polyhedron with the rotation matrix and derivatives,
138: ! and the position of the vertices.137: ! and the position of the vertices.
139: ! POLY: the polyhedron to operate on138: ! POLY: the polyhedron to operate on
140: ! R: Coordinates of the centre of the particle139: ! R: Coordinates of the centre of the particle
141: ! P: Angle-axis style orientation of the particle140: ! P: Angle-axis style orientation of the particle
142: !    INTENT(INOUT) because orientations are reranged141: !    INTENT(INOUT) because orientations are reranged
143: ! GTEST: Whether to computer derivatives142: ! GTEST: Whether to computer derivatives
144: !-------------------------------------------------------------------------------143: !-------------------------------------------------------------------------------
145:     SUBROUTINE UPDATE_POLYHEDRON(POLY, R, P, GTEST)144:     SUBROUTINE UPDATE_POLYHEDRON(POLY, R, P, GTEST)
146: 145: 
147:         ! DIMS: Dimension of the space146:         ! DIM: Dimension of the space
148:         ! NVERTS: Number of vertices per polyhedron147:         ! NVERTS: Number of vertices per particle
149:         ! POLYHEDRON: Type for storing a polyhedron148:         ! POLYHEDRON: Type for storing a polyhedron
150:         USE GJK_MODULE, ONLY: DIMS, NVERTS, POLYHEDRON149:         USE GJK_UTILS, ONLY: DIM, NVERTS, POLYHEDRON
151: 150: 
152:         IMPLICIT NONE151:         IMPLICIT NONE
153: 152: 
154:         TYPE(POLYHEDRON), INTENT(OUT) :: POLY153:         TYPE(POLYHEDRON), INTENT(OUT) :: POLY
155:         DOUBLE PRECISION, INTENT(IN) :: R(DIMS)154:         DOUBLE PRECISION, INTENT(IN) :: R(DIM)
156:         DOUBLE PRECISION, INTENT(INOUT) :: P(DIMS)155:         DOUBLE PRECISION, INTENT(INOUT) :: P(DIM)
157:         LOGICAL, INTENT(IN) :: GTEST156:         LOGICAL, INTENT(IN) :: GTEST
158: 157: 
159:         INTEGER :: I158:         INTEGER :: I
160:         DOUBLE PRECISION :: PI = 4.0D0 * ATAN(1.0D0)159:         DOUBLE PRECISION :: PI = 4.0D0 * ATAN(1.0D0)
161:         ! PMOD: angle of the angle axis orientation160:         ! PMOD: angle of the angle axis orientation
162:         DOUBLE PRECISION :: PMOD161:         DOUBLE PRECISION :: PMOD
163: 162: 
164:         POLY%R(:) = R(:)163:         POLY%R(:) = R(:)
165: 164: 
166:         ! Make sure that 0 < |p| < 2*pi165:         ! Make sure that 0 < |p| < 2*pi
167:         PMOD = sqrt(dot_product(p, p))166:         PMOD = sqrt(dot_product(p, p))
168:         IF(PMOD > 2 * PI) P(:) = P(:) / (PMOD * MOD(PMOD, 2 * PI))167:         IF(PMOD > 2 * PI) P(:) = P(:) / (PMOD * MOD(PMOD, 2 * PI))
169:         POLY%P(:) = P(:)168:         POLY%P(:) = P(:)
170: 169: 
171:         ! Compute the rotation matrices and derivatives170:         ! Compute the rotation matrices and derivatives
172:         CALL RMDRVT(POLY%P, POLY%RM, POLY%RMD(1,:,:), POLY%RMD(2,:,:), &171:         CALL RMDRVT(POLY%P, POLY%RM, POLY%RMD(1,:,:), POLY%RMD(2,:,:), &
173:             POLY%RMD(3,:,:), GTEST)172:             POLY%RMD(3,:,:), GTEST)
174: 173: 
175:         ! Apply rotation matrix and translational offset to all the vertices174:         ! Apply rotation matrix and translational offset to all the vertices
176:         ALLOCATE(POLY%VERTS(DIMS,NVERTS))175:         ALLOCATE(POLY%VERTS(DIM,NVERTS))
177:         DO I = 1, NVERTS176:         DO I = 1, NVERTS
178:             POLY%VERTS(:, I) = POLY%R(:) + MATMUL(POLY%RM, VERTS_0(:, I))177:             POLY%VERTS(:, I) = POLY%R(:) + MATMUL(POLY%RM, VERTS_0(:, I))
179:         END DO178:         END DO
180: 179: 
181:     END SUBROUTINE UPDATE_POLYHEDRON180:     END SUBROUTINE UPDATE_POLYHEDRON
182: 181: 
183: !-------------------------------------------------------------------------------182: !-------------------------------------------------------------------------------
184: ! Calculates the potential, and optionally the gradients183: ! Calculates the potential, and optionally the gradients
185: ! X: array of coordinates, all translations, then all orientations184: ! X: array of coordinates, all translations, then all orientations
186: !    INTENT(INOUT) because orientation may be reranged185: !    INTENT(INOUT) because orientation may be reranged
187: ! GRAD: array of gradients, all translations, then all orientations186: ! GRAD: array of gradients, all translations, then all orientations
188: ! ENERGY: the energy of the system187: ! ENERGY: the energy of the system
189: ! GTEST: whether gradients are required188: ! GTEST: whether gradients are required
190: !-------------------------------------------------------------------------------189: !-------------------------------------------------------------------------------
191:     SUBROUTINE CONVEX_POLYHEDRA(X, GRAD, ENERGY, GTEST)190:     SUBROUTINE CONVEX_POLYHEDRA(X, GRAD, ENERGY, GTEST)
192: 191: 
193:         ! GJK_INTERSECTION: Tests whether two shapes overlap192:         ! GJK_INTERSECTION: Tests whether two shapes overlap
194:         ! EXPANDING_POLYTOP: Finds the intersection information of two shapes193:         ! DIM: Dimension of the space
195:         ! DIMS: Dimension of the space194:         USE GJK_MODULE, ONLY: GJK_INTERSECTION
196:         ! SUPPORT_POINT: Type for storing points in Minkowski difference space195:         USE GJK_UTILS, ONLY: DIM
197:         USE GJK_MODULE, ONLY: GJK_INTERSECTION, EXPANDING_POLYTOPE, DIMS, & 
198:                               SUPPORT_POINT 
199: 196: 
200:         IMPLICIT NONE197:         IMPLICIT NONE
201: 198: 
202:         DOUBLE PRECISION, INTENT(INOUT) :: X(X_SIZE)199:         DOUBLE PRECISION, INTENT(INOUT) :: X(X_SIZE)
203:         DOUBLE PRECISION, INTENT(OUT) :: GRAD(X_SIZE), ENERGY200:         DOUBLE PRECISION, INTENT(OUT) :: GRAD(X_SIZE), ENERGY
204:         LOGICAL, INTENT(IN) :: GTEST201:         LOGICAL, INTENT(IN) :: GTEST
205: 202: 
206:         ! MOLI_IND, MOLJ_IND: indices for looping over molecules203:         ! MOLI_IND, MOLJ_IND: indices for looping over molecules
207:         ! OFFSET: The end of the position coordinates in X204:         ! OFFSET: The end of the position coordinates in X
208:         INTEGER :: MOLI_IND, MOLJ_IND, OFFSET205:         INTEGER :: MOLI_IND, MOLJ_IND, OFFSET
209: 206: 
210:         ! RESULT_SIMPLEX: A simplex enclosing the origin if GJK finds an 
211:         !                 intersection 
212:         TYPE(SUPPORT_POINT) :: RESULT_SIMPLEX(DIMS+1) 
213:  
214:         ! INIT_AXIS: Initial direction guess for GJK, just use the difference of207:         ! INIT_AXIS: Initial direction guess for GJK, just use the difference of
215:         !            the centres208:         !            the centres
216:         ! OVERLAP_VECT: Normalised vector in the overlap direction, from EPA209:         ! RESULT_SIMPLEX: a simplex enclosing the origin if GJK finds an
217:         ! OVERLAP_DIST: Penetration distance, from EPA210:         !                 intersection
218:         ! WITNESS1, WITNESS2: The witness points, from EPA211:         DOUBLE PRECISION :: INIT_AXIS(DIM), RESULT_SIMPLEX(DIM, DIM+1)
219:         DOUBLE PRECISION :: INIT_AXIS(DIMS) 
220:         DOUBLE PRECISION :: OVERLAP_VECT(DIMS), OVERLAP_DIST 
221:         DOUBLE PRECISION :: WITNESS1(DIMS), WITNESS2(DIMS) 
222: 212: 
223:         ! RESULT: whether or not GJK finds an intersection213:         ! RESULT: whether or not GJK finds an intersection
224:         LOGICAL :: RESULT214:         LOGICAL :: RESULT
225: 215: 
226:         ! Initialise ouput variables216:         ! Initialise ouput variables
227:         ENERGY = 0.D0217:         ENERGY = 0.D0
228:         IF (GTEST) GRAD(:) = 0.D0218:         IF (GTEST) GRAD(:) = 0.D0
229: 219: 
230:         ! Calculate offset from the number of particles220:         ! Calculate offset from the number of particles
231:         OFFSET = 3*NPOLY221:         OFFSET = 3*NPOLY
234:         DO MOLI_IND = 1, NPOLY224:         DO MOLI_IND = 1, NPOLY
235:             CALL UPDATE_POLYHEDRON(POLYHEDRA(MOLI_IND), &225:             CALL UPDATE_POLYHEDRON(POLYHEDRA(MOLI_IND), &
236:                                    X(MOLI_IND*3-2:MOLI_IND*3), &226:                                    X(MOLI_IND*3-2:MOLI_IND*3), &
237:                                    X(OFFSET+MOLI_IND*3-2:OFFSET+MOLI_IND*3), &227:                                    X(OFFSET+MOLI_IND*3-2:OFFSET+MOLI_IND*3), &
238:                                    GTEST)228:                                    GTEST)
239:         END DO229:         END DO
240: 230: 
241:         ! Just looking to test at the moment231:         ! Just looking to test at the moment
242:         ! Testing with two particles, just print whether they overlap or not232:         ! Testing with two particles, just print whether they overlap or not
243:         INIT_AXIS(:) = POLYHEDRA(1)%R(:) - POLYHEDRA(2)%R(:)233:         INIT_AXIS(:) = POLYHEDRA(1)%R(:) - POLYHEDRA(2)%R(:)
244:         CALL GJK_INTERSECTION(POLYHEDRA(1), POLYHEDRA(2), INIT_AXIS(:), &234:         CALL GJK_INTERSECTION(POLYHEDRA(1), POLYHEDRA(2), INIT_AXIS(:), DIM, &
245:                               RESULT, RESULT_SIMPLEX(:))235:                               RESULT, RESULT_SIMPLEX(:,:))
246: 236: 
247:         WRITE(*,*) 'GJK returned ', RESULT237:         WRITE(*,*) 'GJK returned ', RESULT
248:         IF (RESULT) THEN238:         IF (RESULT) THEN
249:             WRITE(*,*) 'Result simplex is:'239:             WRITE(*,*) 'Result simplex is:'
250:             DO MOLJ_IND = 1, 4240:             DO MOLJ_IND = 1, 4
251:                 WRITE(*,*) RESULT_SIMPLEX(MOLJ_IND)%V(1), &241:                 WRITE(*,*) RESULT_SIMPLEX(1,MOLJ_IND), &
252:                            RESULT_SIMPLEX(MOLJ_IND)%V(2), &242:                            RESULT_SIMPLEX(2,MOLJ_IND), &
253:                            RESULT_SIMPLEX(MOLJ_IND)%V(3)243:                            RESULT_SIMPLEX(3,MOLJ_IND)
254:             END DO244:             END DO
255:         END IF245:         END IF
256:  
257:         IF (RESULT) THEN 
258: !            WRITE(*,*) 'Calling EXPANDING_POLYTOPE' 
259:             CALL EXPANDING_POLYTOPE(POLYHEDRA(1), POLYHEDRA(2), & 
260:                                     RESULT_SIMPLEX(:), OVERLAP_DIST, & 
261:                                     OVERLAP_VECT(:), WITNESS1(:), WITNESS2(:)) 
262:             WRITE (*,*) 'Overlap distance is ', OVERLAP_DIST 
263:             WRITE (*,*) 'Overlap vector is ', OVERLAP_VECT(:) 
264:             WRITE (*,*) 'Witness point on shape 1 is ', WITNESS1(:) 
265:             WRITE (*,*) 'Witness point on shape 2 is ', WITNESS2(:) 
266:         END IF 
267:  
268:         WRITE (*,*) 'Exiting now'246:         WRITE (*,*) 'Exiting now'
269:         STOP247:         STOP
270: 248: 
271:     END SUBROUTINE CONVEX_POLYHEDRA249:     END SUBROUTINE CONVEX_POLYHEDRA
272: 250: 
273: !------------------------------------------------------------------------------251: !------------------------------------------------------------------------------
274: ! Prints out the vertex information to the file poly.xyz252: ! Prints out the vertex information to the file poly.xyz
275: !------------------------------------------------------------------------------253: !------------------------------------------------------------------------------
276:     SUBROUTINE VIEW_POLYHEDRA()254:     SUBROUTINE VIEW_POLYHEDRA()
277: 255: 


r31886/gjk.f90 2017-02-03 23:30:17.180168934 +0000 r31885/gjk.f90 2017-02-03 23:30:17.980179581 +0000
 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 minimum 20: ! convex shapes overlap and the expanding polytope algorithm to find the minimum
 21: ! 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:  
 29:     ! DIMS: Working in 3 Dimensions 
 30:     INTEGER, PARAMETER :: DIMS = 3 
 31:  
 32:     ! NVERTS: number of vertices in each polyhedron 
 33:     INTEGER :: NVERTS 
 34:  
 35:     ! Stores a vertex in Minkowski difference space 
 36:     TYPE :: SUPPORT_POINT 
 37:         ! V: the point in difference space 
 38:         ! S1, S2: the points of the individual support functions 
 39:         DOUBLE PRECISION :: V(DIMS), S1(DIMS), S2(DIMS) 
 40:     END TYPE SUPPORT_POINT 
 41:  
 42:     ! Stores a single polyhedron 
 43:     TYPE :: POLYHEDRON 
 44:         ! R: Coordinates of the centre of the particle 
 45:         ! P: Angle-axis style orientation of the particle 
 46:         ! RM: Rotation matrix, derived from P 
 47:         ! RMD: Derivatives of the rotation matrix 
 48:         DOUBLE PRECISION :: R(DIMS), P(DIMS), RM(DIMS, DIMS) 
 49:         DOUBLE PRECISION :: RMD(DIMS, DIMS, DIMS) 
 50:         ! VERTS: Vertex positions 
 51:         DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:) :: VERTS 
 52:     END TYPE POLYHEDRON 
 53:  
 54:     ! Stores a triangular face for the EPA algorithm 
 55:     TYPE :: FACE 
 56:         ! A, B, C: vertex positions of the faces 
 57:         ! NORMAL: normal to the face (AB x AC) 
 58:         ! ORIG_DIST: distance to the origin 
 59:         TYPE(SUPPORT_POINT) :: A, B, C 
 60:         DOUBLE PRECISION :: NORMAL(DIMS), ORIG_DIST 
 61:     END TYPE FACE 
 62:  
 63:     ! Stores an edge for the EPA algorithm 
 64:     TYPE :: EDGE 
 65:         ! A, B: end points of the edge 
 66:         TYPE(SUPPORT_POINT) :: A, B 
 67:     END TYPE EDGE 
 68:  
 69:     PRIVATE :: NEAREST_SIMPLEX, CHECK_FACE, SAME_DIRECTION, CROSS_121 
 70:     PRIVATE :: FACE, NEW_FACE, CHECK_EDGE, BARYCENTRIC 
 71:  
 72: CONTAINS 28: CONTAINS
 73: !------------------------------------------------------------------------------- 29: !-------------------------------------------------------------------------------
 74: ! Performs the GJK intersection test 30: ! Performs the GJK intersection test
 75: ! SHAPE1: The first shape of the test 31: ! SHAPE1: The first shape of the test
 76: ! SHAPE2: The second shape of the test 32: ! SHAPE2: The second shape of the test
 77: ! INIT_AXIS: The initial guess, just needs to be a point on the surface of the 33: ! INIT_AXIS: The initial guess, just needs to be a point on the surface of the
 78: !            Minkowski difference of the shapes 34: !            Minkowski difference of the shapes
  35: ! DIM: dimension of the space (probably 3)
 79: ! RESULT: whether or not the shapes intersect 36: ! RESULT: whether or not the shapes intersect
 80: ! RESULT_SIMPLEX: if the shapes intersect, the simplex containing the origin 37: ! RESULT_SIMPLEX: if the shapes intersect, the simplex containing the origin
 81: !------------------------------------------------------------------------------- 38: !-------------------------------------------------------------------------------
 82:     SUBROUTINE GJK_INTERSECTION(SHAPE1, SHAPE2, INIT_AXIS, RESULT, & 39:     SUBROUTINE GJK_INTERSECTION(SHAPE1, SHAPE2, INIT_AXIS, DIM, RESULT, &
 83:                                 RESULT_SIMPLEX) 40:                                 RESULT_SIMPLEX)
 84:  41: 
 85:         ! MYUNIT: File unit of GMIN_out 42:         ! MYUNIT: File unit of GMIN_out
 86:         ! SUPPORT_FUNC: Support function for convex polyhedra 43:         ! SUPPORT_FUNC: Support function for convex polyhedra
 87:         ! POLYHEDRON: Type to store a polyhedron 44:         ! POLYHEDRON: Type to store a polyhedron
 88:         USE COMMONS, ONLY: MYUNIT 45:         USE COMMONS, ONLY: MYUNIT
  46:         USE GJK_UTILS, ONLY: POLYHEDRON, SUPPORT_FUNC
 89:  47: 
 90:         IMPLICIT NONE 48:         IMPLICIT NONE
 91:  49: 
 92:         TYPE(POLYHEDRON), INTENT(IN) :: SHAPE1, SHAPE2 50:         TYPE(POLYHEDRON), INTENT(IN) :: SHAPE1, SHAPE2
 93:         DOUBLE PRECISION, INTENT(IN) :: INIT_AXIS(DIMS) 51:         INTEGER, INTENT(IN) :: DIM
 94:         TYPE(SUPPORT_POINT), INTENT(OUT) :: RESULT_SIMPLEX(DIMS+1) 52:         DOUBLE PRECISION, INTENT(IN) :: INIT_AXIS(DIM)
  53:         DOUBLE PRECISION, INTENT(OUT) :: RESULT_SIMPLEX(DIM, DIM+1)
 95:         LOGICAL, INTENT(OUT) :: RESULT 54:         LOGICAL, INTENT(OUT) :: RESULT
 96:  55: 
 97:         ! WORK_SIMPLEX: the working simplex 56:         ! WORK_SIMPLEX: the working simplex, unused parts set to 1.D100
 98:         ! NEW_POINT: the next point to be considered for addition to the simplex 
 99:         ! LEN_SIMPLEX: how many vertices the simplex currently contains 
100:         TYPE(SUPPORT_POINT) :: WORK_SIMPLEX(DIMS+1), NEW_POINT 
101:  
102:         ! SEARCH_DIRECTION: the current search direction 57:         ! SEARCH_DIRECTION: the current search direction
103:         DOUBLE PRECISION :: SEARCH_DIRECTION(DIMS) 58:         ! NEW_POINT: the next point to be considered for addition to the simplex
104:  
105:         ! LEN_SIMPLEX: how many vertices the simplex currently contains 59:         ! LEN_SIMPLEX: how many vertices the simplex currently contains
  60:         DOUBLE PRECISION :: WORK_SIMPLEX(DIM, DIM+1), SEARCH_DIRECTION(DIM)
  61:         DOUBLE PRECISION :: NEW_POINT(DIM)
106:         INTEGER :: LEN_SIMPLEX 62:         INTEGER :: LEN_SIMPLEX
107:  63: 
108:         ! Initialise the output variables 64:         ! Initialise the output variables
109:         RESULT = .FALSE. 65:         RESULT = .FALSE.
  66:         RESULT_SIMPLEX(:,:) = 1.D100
110:  67: 
111:         ! Initialise the working simplex 68:         ! Initialise the working simplex
112:         WORK_SIMPLEX(1)%S1(:) = SUPPORT_FUNC(SHAPE1,  INIT_AXIS(:)) 69:         WORK_SIMPLEX(:,:) = 1.D100
113:         WORK_SIMPLEX(1)%S2(:) = SUPPORT_FUNC(SHAPE2, -INIT_AXIS(:)) 70:         WORK_SIMPLEX(:, 1) = SUPPORT_FUNC(SHAPE1,  INIT_AXIS(:)) &
114:         WORK_SIMPLEX(1)%V(:) = WORK_SIMPLEX(1)%S1(:) - WORK_SIMPLEX(1)%S2(:) 71:                            - SUPPORT_FUNC(SHAPE2, -INIT_AXIS(:))
115:         SEARCH_DIRECTION(:) = - WORK_SIMPLEX(1)%V(:) 72:         SEARCH_DIRECTION(:) = - WORK_SIMPLEX(:, 1)
116:         LEN_SIMPLEX = 1 73:         LEN_SIMPLEX = 1
117:  74: 
118:         ! Start the GJK refinement loop 75:         ! Start the GJK refinement loop
119:         DO 76:         DO
120:             ! Go as far as we can within the Minkowski difference in the search 77:             ! Go as far as we can within the Minkowski difference in the search
121:             ! direction. 78:             ! direction.
122:             NEW_POINT%S1(:) = SUPPORT_FUNC(SHAPE1,  SEARCH_DIRECTION(:)) 79:             NEW_POINT(:) = SUPPORT_FUNC(SHAPE1,  SEARCH_DIRECTION(:)) &
123:             NEW_POINT%S2(:) = SUPPORT_FUNC(SHAPE2, -SEARCH_DIRECTION(:)) 80:                          - SUPPORT_FUNC(SHAPE2, -SEARCH_DIRECTION(:))
124:             NEW_POINT%V(:) = NEW_POINT%S1(:) - NEW_POINT%S2(:) 
125:  81: 
126:             ! If we haven't gone as far as the origin, the shapes definitely 82:             ! If we haven't gone as far as the origin, the shapes definitely
127:             ! don't intersect 83:             ! don't intersect
128:             IF (DOT_PRODUCT(SEARCH_DIRECTION(:), NEW_POINT%V(:)) < 0.D0) THEN 84:             IF (DOT_PRODUCT(SEARCH_DIRECTION(:), NEW_POINT(:)) < 0.D0) THEN
129:                 RESULT = .FALSE. 85:                 RESULT = .FALSE.
130:                 EXIT 86:                 EXIT
131:             END IF 87:             END IF
132:  88: 
133:             ! Add the new point to the simplex 89:             ! Add the new point to the simplex
134:             LEN_SIMPLEX = LEN_SIMPLEX + 1 90:             LEN_SIMPLEX = LEN_SIMPLEX + 1
135:             WORK_SIMPLEX(LEN_SIMPLEX) = NEW_POINT 91:             WORK_SIMPLEX(:, LEN_SIMPLEX) = NEW_POINT(:)
136:  92: 
137:             ! Check if the new simplex contains the origin 93:             ! Check if the new simplex contains the origin
138:             ! Update the simplex and the search direction 94:             ! Update the simplex and the search direction
139:             CALL NEAREST_SIMPLEX(WORK_SIMPLEX, LEN_SIMPLEX, & 95:             CALL NEAREST_SIMPLEX(WORK_SIMPLEX, LEN_SIMPLEX, DIM, &
140:                                  SEARCH_DIRECTION, RESULT) 96:                                  SEARCH_DIRECTION, RESULT)
141:  97: 
142:             ! If the simplex contains the origin, we're done 98:             ! If the simplex contains the origin, we're done
143:             IF (RESULT) THEN 99:             IF (RESULT) THEN
144:                 ! The simplex length should be DIMS+1 (ie a tetrahedron in 3D),100:                 ! The simplex length should be DIM+1 (ie a tetrahedron in 3D),
145:                 ! or something horrible has happened.101:                 ! or something horrible has happened.
146:                 IF (LEN_SIMPLEX .NE. DIMS + 1) THEN102:                 IF (LEN_SIMPLEX .NE. DIM + 1) THEN
147:                     WRITE(MYUNIT, *) 'GJK_INTERSECTION> ERROR, length of ', &103:                     WRITE(MYUNIT, *) 'GJK_INTERSECTION> ERROR, length of ', &
148:                     ' simpled is ', LEN_SIMPLEX, ' but DIMSension is ', DIMS104:                     ' simpled is ', LEN_SIMPLEX, ' but dimension is ', DIM
149:                     STOP105:                     STOP
150:                 END IF106:                 END IF
151: 107: 
152:                 RESULT_SIMPLEX(:) = WORK_SIMPLEX(:)108:                 RESULT_SIMPLEX(:,:) = WORK_SIMPLEX(:,:)
153:                 EXIT109:                 EXIT
154:             END IF110:             END IF
155: 111: 
156:         END DO ! GJK refinement loop112:         END DO ! GJK refinement loop
157: 113: 
158:     END SUBROUTINE GJK_INTERSECTION114:     END SUBROUTINE GJK_INTERSECTION
159: 115: 
160: !-------------------------------------------------------------------------------116: !-------------------------------------------------------------------------------
161: ! Support function for the GJK algorithm. Returns the vertex furthest along 
162: ! the direction DIR 
163: ! POLY: the polyhedron of interest 
164: ! DIR: the search direction 
165: !------------------------------------------------------------------------------- 
166:         FUNCTION SUPPORT_FUNC(POLY, DIR) 
167:  
168:             IMPLICIT NONE 
169:  
170:             DOUBLE PRECISION :: SUPPORT_FUNC(DIMS) 
171:             DOUBLE PRECISION :: DIR(:) 
172:             TYPE(POLYHEDRON), INTENT(IN) :: POLY 
173:  
174:             ! BEST_INDEX: index of the best vertex 
175:             INTEGER :: I, BEST_INDEX 
176:             ! DOT: temporary dot product 
177:             ! MAX_DOT: largest dot product between DIR and a vertex 
178:             DOUBLE PRECISION :: DOT, MAX_DOT 
179:  
180:             BEST_INDEX = 1 
181:             MAX_DOT = -1.0D10 
182:  
183:             ! Loop over the vertices 
184:             DO I = 1, NVERTS 
185:                 DOT = DOT_PRODUCT(POLY%VERTS(:,I), DIR(:)) 
186:                 IF (DOT .GT. MAX_DOT) THEN 
187:                     MAX_DOT = DOT 
188:                     BEST_INDEX = I 
189:                 END IF 
190:             END DO ! Loop over vertices 
191:  
192:             SUPPORT_FUNC(:) = POLY%VERTS(:,BEST_INDEX) 
193: !            WRITE (*,*) 'SEARCH_DIRECTION = ', DIR(:) 
194: !            WRITE (*,*) 'SUPPORT_FUNC = ', SUPPORT_FUNC(:) 
195:  
196:         END FUNCTION SUPPORT_FUNC 
197:  
198: !------------------------------------------------------------------------------- 
199: ! Finds the nearest simplex to the origin of rank lower than the supplied117: ! Finds the nearest simplex to the origin of rank lower than the supplied
200: ! simplex. If the simplex is at the maximum size, check if the origin lies118: ! simplex. If the simplex is at the maximum size, check if the origin lies
201: ! within the simplex.119: ! within the simplex.
202: ! Provides the new search direction based on the closest simplex.120: ! Provides the new search direction based on the closest simplex.
203: ! TEST_SIMPLEX: the simplex to check121: ! TEST_SIMPLEX: the simplex to check
204: ! LEN_SIMPLEX: current length of the simplex122: ! LEN_SIMPLEX: current length of the simplex
 123: ! DIM: dimension of the space (probably 3)
205: ! SEARCH_DIRECTION: the new search direction124: ! SEARCH_DIRECTION: the new search direction
206: ! RESULT: whether the origin lies within the simplex125: ! RESULT: whether the origin lies within the simplex
207: !-------------------------------------------------------------------------------126: !-------------------------------------------------------------------------------
208:     SUBROUTINE NEAREST_SIMPLEX(TEST_SIMPLEX, LEN_SIMPLEX, &127:     SUBROUTINE NEAREST_SIMPLEX(TEST_SIMPLEX, LEN_SIMPLEX, DIM, &
209:                                SEARCH_DIRECTION, RESULT)128:                                SEARCH_DIRECTION, RESULT)
210: 129: 
211:         ! MYUNIT: File unit of GMIN_out130:         ! MYUNIT: File unit of GMIN_out
212:         ! VEC_CROSS: vector cross product131:         ! VEC_CROSS: vector cross product
213:         ! VEC_RANDOM: a vector with random direction 
214:         USE COMMONS, ONLY: MYUNIT132:         USE COMMONS, ONLY: MYUNIT
215:         USE VEC3, ONLY: VEC_CROSS, VEC_RANDOM133:         USE VEC3, ONLY: VEC_CROSS
216: 134: 
217:         IMPLICIT NONE135:         IMPLICIT NONE
218: 136: 
219:         TYPE(SUPPORT_POINT), INTENT(INOUT) :: TEST_SIMPLEX(DIMS+1)137:         INTEGER, INTENT(IN) :: DIM
220:         DOUBLE PRECISION, INTENT(OUT) :: SEARCH_DIRECTION(DIMS)138:         DOUBLE PRECISION, INTENT(INOUT) :: TEST_SIMPLEX(DIM, DIM+1)
 139:         DOUBLE PRECISION, INTENT(OUT) :: SEARCH_DIRECTION(DIM)
221:         INTEGER, INTENT(INOUT) :: LEN_SIMPLEX140:         INTEGER, INTENT(INOUT) :: LEN_SIMPLEX
222:         LOGICAL, INTENT(OUT) :: RESULT141:         LOGICAL, INTENT(OUT) :: RESULT
223: 142: 
224:         ! O: the origin143:         ! O: the origin
225:         ! AB, AC, AD, AO, ABPERP, ACPERP, ADPERP, ABCPERP, ACDPERP, ADBPERP:144:         ! AB, AC, AD, AO, ABPERP, ACPERP, ADPERP, ABCPERP, ACDPERP, ADBPERP:
226:         !     storage for plane tests and search direction calculation145:         !     storage for plane tests and search direction calculation
227:         ! TOL: For checking whether things are zero146:         DOUBLE PRECISION :: AB(DIM), AC(DIM), AD(DIM), AO(DIM)
228:         DOUBLE PRECISION :: AB(DIMS), AC(DIMS), AD(DIMS), AO(DIMS)147:         DOUBLE PRECISION :: ABPERP(DIM), ACPERP(DIM), ADPERP(DIM)
229:         DOUBLE PRECISION :: ABPERP(DIMS), ACPERP(DIMS), ADPERP(DIMS)148:         DOUBLE PRECISION :: ABCPERP(DIM), ACDPERP(DIM), ADBPERP(DIM)
230:         DOUBLE PRECISION :: ABCPERP(DIMS), ACDPERP(DIMS), ADBPERP(DIMS) 
231:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06 
232:         ! OVERABC, OVERACD, OVERADB: Results of plane tests149:         ! OVERABC, OVERACD, OVERADB: Results of plane tests
233:         LOGICAL :: OVERABC, OVERACD, OVERADB150:         LOGICAL :: OVERABC, OVERACD, OVERADB
234: 151: 
235:         ! Initialise output152:         ! Initialise output
236:         SEARCH_DIRECTION(:) = 1.D100153:         SEARCH_DIRECTION(:) = 1.D100
237:         RESULT = .FALSE.154:         RESULT = .FALSE.
238: 155: 
239:         ! The point referred to as A is always the mose recent added to the156:         ! The point referred to as A is always the mose recent added to the
240:         ! simplex, B the second most recent, etc.157:         ! simplex, B the second most recent, etc.
241:         ! No point can be the closest to the origin. In the GJK loop we've158:         ! No point can be the closest to the origin. In the GJK loop we've
242:         ! checked to make sure the origin has been passed. Since it has, there's159:         ! checked to make sure the origin has been passed. Since it has, there's
243:         ! no way the points can be closer than the lines connecting them to160:         ! no way the points can be closer than the lines connecting them to
244:         ! other points.161:         ! other points.
245:         ! Likewise, only lines and faces involving A are candidates.162:         ! Likewise, only lines and faces involving A are candidates.
246: 163: 
247:         ! Now assuming that DIMS is 3. May need to rewrite.164:         ! Now assuming that DIM is 3. May need to rewrite.
248:         IF (LEN_SIMPLEX .EQ. 2) THEN165:         IF (LEN_SIMPLEX .EQ. 2) THEN
249:             ! Simplest case of a line. The nearest simplex can only be the line.166:             ! Simplest case of a line. The nearest simplex can only be the line.
250:             RESULT = .FALSE.167:             RESULT = .FALSE.
251:             AO(:) = - TEST_SIMPLEX(2)%V(:)168:             AO(:) = - TEST_SIMPLEX(:, 2)
252:             AB(:) = TEST_SIMPLEX(1)%V(:) - TEST_SIMPLEX(2)%V(:)169:             AB(:) = TEST_SIMPLEX(:, 1) - TEST_SIMPLEX(:, 2)
253:             SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:))170:             SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM)
254:  
255:             ! Check if the origin lies on the line, indicated by a zero vector 
256:             IF (ABS(SEARCH_DIRECTION(1)) .LT. TOL .AND. & 
257:                 ABS(SEARCH_DIRECTION(2)) .LT. TOL .AND. & 
258:                 ABS(SEARCH_DIRECTION(3)) .LT. TOL) THEN 
259:                 ! It's on the line. Search direction doesn't really matter. 
260:                 SEARCH_DIRECTION(:) = VEC_RANDOM() 
261:             END IF 
262: 171: 
263:         ELSE IF (LEN_SIMPLEX .EQ. 3) THEN172:         ELSE IF (LEN_SIMPLEX .EQ. 3) THEN
264:             ! Simplex is a triangle. Nearest simplex still cannot be a point.173:             ! Simplex is a triangle. Nearest simplex still cannot be a point.
265:             ! It could be either of the lines with the latest point, but not the174:             ! It could be either of the lines with the latest point, but not the
266:             ! third.175:             ! third.
267:             ! Origin could lie above or below the triangle.176:             ! Origin could lie above or below the triangle.
268:             RESULT = .FALSE.177:             RESULT = .FALSE.
269: 178: 
270:             ! Generate the necessary vectors179:             ! Generate the necessary vectors
271:             AB(:) = TEST_SIMPLEX(2)%V(:) - TEST_SIMPLEX(3)%V(:)180:             AB(:) = TEST_SIMPLEX(:, 2) - TEST_SIMPLEX(:, 3)
272:             AC(:) = TEST_SIMPLEX(1)%V(:) - TEST_SIMPLEX(3)%V(:)181:             AC(:) = TEST_SIMPLEX(:, 1) - TEST_SIMPLEX(:, 3)
273:             ABCPERP(:) = VEC_CROSS(AB(:), AC(:))182:             ABCPERP(:) = VEC_CROSS(AB(:), AC(:))
274: 183: 
275:             ! Check whether the origin is outside the triangle on the AB side184:             ! Check whether the origin is outside the triangle on the AB side
276:             CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, AB, &185:             CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, AB, &
277:                             AC, ABCPERP)186:                             AC, ABCPERP, DIM)
278:  
279:             ! Check if the origin lies on the line or face, indicated by a zero 
280:             ! vector 
281:             IF (ABS(SEARCH_DIRECTION(1)) .LT. TOL .AND. & 
282:                 ABS(SEARCH_DIRECTION(2)) .LT. TOL .AND. & 
283:                 ABS(SEARCH_DIRECTION(3)) .LT. TOL) THEN 
284:                 ! It's on the line or face. Search direction doesn't really 
285:                 ! matter. 
286:                 SEARCH_DIRECTION(:) = VEC_RANDOM() 
287:                 ! Except it should be 'above' the triangle 
288:                 IF (DOT_PRODUCT(SEARCH_DIRECTION(:), ABCPERP(:)) .LT. 0.D0) & 
289:                 THEN 
290:                     SEARCH_DIRECTION(:) = -SEARCH_DIRECTION(:) 
291:                 END IF 
292:             END IF 
293: 187: 
294:         ELSE IF (LEN_SIMPLEX .EQ. 4) THEN188:         ELSE IF (LEN_SIMPLEX .EQ. 4) THEN
295:             ! Simplex is a tetrahedron. It might enclose the origin.189:             ! Simplex is a tetrahedron. It might enclose the origin.
296:             ! Only lines and faces involving A need to be considered.190:             ! Only lines and faces involving A need to be considered.
297:             ! See http://vec3.ca/gjk/implementation/ for an explanation191:             ! See http://vec3.ca/gjk/implementation/ for an explanation
298: 192: 
299:             ! Generate the necessary vectors193:             ! Generate the necessary vectors
300:             AO(:) = - TEST_SIMPLEX(4)%V(:)194:             AO(:) = - TEST_SIMPLEX(:, 4)
301:             ! Edge vectors195:             ! Edge vectors
302:             AB(:) = TEST_SIMPLEX(3)%V(:) - TEST_SIMPLEX(4)%V(:)196:             AB(:) = TEST_SIMPLEX(:, 3) - TEST_SIMPLEX(:, 4)
303:             AC(:) = TEST_SIMPLEX(2)%V(:) - TEST_SIMPLEX(4)%V(:)197:             AC(:) = TEST_SIMPLEX(:, 2) - TEST_SIMPLEX(:, 4)
304:             AD(:) = TEST_SIMPLEX(1)%V(:) - TEST_SIMPLEX(4)%V(:)198:             AD(:) = TEST_SIMPLEX(:, 1) - TEST_SIMPLEX(:, 4)
305:             ! Perpendiculars to faces (pointing outwards)199:             ! Perpendiculars to faces (pointing outwards)
306:             ABCPERP(:) = VEC_CROSS(AB(:), AC(:))200:             ABCPERP(:) = VEC_CROSS(AB(:), AC(:))
307:             ACDPERP(:) = VEC_CROSS(AC(:), AD(:))201:             ACDPERP(:) = VEC_CROSS(AC(:), AD(:))
308:             ADBPERP(:) = VEC_CROSS(AD(:), AB(:))202:             ADBPERP(:) = VEC_CROSS(AD(:), AB(:))
309: 203: 
310:             ! Store the results of plane tests, so we only have to do them once204:             ! Store the results of plane tests, so we only have to do them once
311:             OVERABC = SAME_DIRECTION(ABCPERP(:), AO(:), MYUNIT)205:             OVERABC = SAME_DIRECTION(ABCPERP(:), AO(:), DIM, MYUNIT)
312:             OVERACD = SAME_DIRECTION(ACDPERP(:), AO(:), MYUNIT)206:             OVERACD = SAME_DIRECTION(ACDPERP(:), AO(:), DIM, MYUNIT)
313:             OVERADB = SAME_DIRECTION(ADBPERP(:), AO(:), MYUNIT)207:             OVERADB = SAME_DIRECTION(ADBPERP(:), AO(:), DIM, MYUNIT)
314: 208: 
315:             IF (.NOT. (OVERABC .OR. OVERACD .OR. OVERADB)) THEN209:             IF (.NOT. (OVERABC .OR. OVERACD .OR. OVERADB)) THEN
316:                 ! Origin is inside the simplex210:                 ! Origin is inside the simplex
317:                 RESULT = .TRUE.211:                 RESULT = .TRUE.
318: 212: 
319:             ! Deal with the two faces case213:             ! Deal with the two faces case
320:             ELSE IF (OVERABC .AND. OVERACD .AND. .NOT. OVERADB) THEN214:             ELSE IF (OVERABC .AND. OVERACD .AND. .NOT. OVERADB) THEN
321:                 ! Check the common edge, relative to the first face215:                 ! Check the common edge, relative to the first face
322:                 ACPERP(:) = VEC_CROSS(ABCPERP(:), AC(:))216:                 ACPERP(:) = VEC_CROSS(ABCPERP(:), AC(:))
323:                 IF (SAME_DIRECTION(ACPERP(:), AO(:), MYUNIT)) THEN217:                 IF (SAME_DIRECTION(ACPERP(:), AO(:), DIM, MYUNIT)) THEN
324:                     ! Now need to do the full check for the second face.218:                     ! Now need to do the full check for the second face.
325:                     ! Fiddle the logicals and let it fall through.219:                     ! Fiddle the logicals and let it fall through.
326:                     OVERABC = .FALSE.220:                     OVERABC = .FALSE.
327:                 ELSE221:                 ELSE
328:                     ! Just need to decide between the first face and the other222:                     ! Just need to decide between the first face and the other
329:                     ! edge. However, it's simpler (if slightly less efficient)223:                     ! edge. However, it's simpler (if slightly less efficient)
330:                     ! to do the full check for the first face.224:                     ! to do the full check for the first face.
331:                     OVERACD = .FALSE.225:                     OVERACD = .FALSE.
332:                 ENDIF226:                 ENDIF
333:             ELSE IF (OVERACD .AND. OVERADB .AND. .NOT. OVERABC) THEN227:             ELSE IF (OVERACD .AND. OVERADB .AND. .NOT. OVERABC) THEN
334:                 ! Check the common edge, relative to the first face228:                 ! Check the common edge, relative to the first face
335:                 ADPERP(:) = VEC_CROSS(ACDPERP(:), AD(:))229:                 ADPERP(:) = VEC_CROSS(ACDPERP(:), AD(:))
336:                 IF (SAME_DIRECTION(ADPERP(:), AO(:), MYUNIT)) THEN230:                 IF (SAME_DIRECTION(ADPERP(:), AO(:), DIM, MYUNIT)) THEN
337:                     ! Now need to do the full check for the second face.231:                     ! Now need to do the full check for the second face.
338:                     ! Fiddle the logicals and let it fall through.232:                     ! Fiddle the logicals and let it fall through.
339:                     OVERACD = .FALSE.233:                     OVERACD = .FALSE.
340:                 ELSE234:                 ELSE
341:                     ! Just need to decide between the first face and the other235:                     ! Just need to decide between the first face and the other
342:                     ! edge. However, it's simpler (if slightly less efficient)236:                     ! edge. However, it's simpler (if slightly less efficient)
343:                     ! to do the full check for the first face.237:                     ! to do the full check for the first face.
344:                     OVERADB = .FALSE.238:                     OVERADB = .FALSE.
345:                 ENDIF239:                 ENDIF
346:             ELSE IF (OVERADB .AND. OVERABC .AND. .NOT. OVERACD) THEN240:             ELSE IF (OVERADB .AND. OVERABC .AND. .NOT. OVERACD) THEN
347:                 ! Check the common edge, relative to the first face241:                 ! Check the common edge, relative to the first face
348:                 ABPERP(:) = VEC_CROSS(ADBPERP(:), AB(:))242:                 ABPERP(:) = VEC_CROSS(ADBPERP(:), AB(:))
349:                 IF (SAME_DIRECTION(ABPERP(:), AO(:), MYUNIT)) THEN243:                 IF (SAME_DIRECTION(ABPERP(:), AO(:), DIM, MYUNIT)) THEN
350:                     ! Now need to do the full check for the second face.244:                     ! Now need to do the full check for the second face.
351:                     ! Fiddle the logicals and let it fall through.245:                     ! Fiddle the logicals and let it fall through.
352:                     OVERADB = .FALSE.246:                     OVERADB = .FALSE.
353:                 ELSE247:                 ELSE
354:                     ! Just need to decide between the first face and the other248:                     ! Just need to decide between the first face and the other
355:                     ! edge. However, it's simpler (if slightly less efficient)249:                     ! edge. However, it's simpler (if slightly less efficient)
356:                     ! to do the full check for the first face.250:                     ! to do the full check for the first face.
357:                     OVERABC = .FALSE.251:                     OVERABC = .FALSE.
358:                 ENDIF252:                 ENDIF
359:             ENDIF ! Two faces case253:             ENDIF ! Two faces case
360: 254: 
361:             ! Deal with the only one face cases.255:             ! Deal with the only one face cases.
362:             ! Need to check the bordering lines.256:             ! Need to check the bordering lines.
363:             IF (OVERABC .AND. .NOT. OVERACD .AND. .NOT. OVERADB) THEN257:             IF (OVERABC .AND. .NOT. OVERACD .AND. .NOT. OVERADB) THEN
364:                 ! Adjust TEXT_SIMPLEX for the ABC face258:                 ! Adjust TEXT_SIMPLEX for the ABC face
365:                 TEST_SIMPLEX(1) = TEST_SIMPLEX(2)259:                 TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
366:                 TEST_SIMPLEX(2) = TEST_SIMPLEX(3)260:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3)
367:                 TEST_SIMPLEX(3) = TEST_SIMPLEX(4)261:                 TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)
368:                 ! Check the face and it's edges262:                 ! Check the face and it's edges
369:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &263:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &
370:                                 AB, AC, ABCPERP)264:                                 AB, AC, ABCPERP, DIM)
371:             ELSE IF (OVERACD .AND. .NOT. OVERADB .AND. .NOT. OVERABC) THEN265:             ELSE IF (OVERACD .AND. .NOT. OVERADB .AND. .NOT. OVERABC) THEN
372:                 ! Adjust TEXT_SIMPLEX for the ACD face266:                 ! Adjust TEXT_SIMPLEX for the ACD face
373:                 TEST_SIMPLEX(3) = TEST_SIMPLEX(4)267:                 TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)
374:                 ! Check the face and it's edges268:                 ! Check the face and it's edges
375:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &269:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &
376:                                 AC, AD, ACDPERP)270:                                 AC, AD, ACDPERP, DIM)
377:             ELSE IF (OVERADB .AND. .NOT. OVERABC .AND. .NOT. OVERACD) THEN271:             ELSE IF (OVERADB .AND. .NOT. OVERABC .AND. .NOT. OVERACD) THEN
378:                 ! Adjust TEXT_SIMPLEX for the ADB face272:                 ! Adjust TEXT_SIMPLEX for the ADB face
379:                 TEST_SIMPLEX(2) = TEST_SIMPLEX(1)273:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 1)
380:                 TEST_SIMPLEX(1) = TEST_SIMPLEX(3)274:                 TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 3)
381:                 TEST_SIMPLEX(3) = TEST_SIMPLEX(4)275:                 TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)
382:                 ! Check the face and it's edges276:                 ! Check the face and it's edges
383:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &277:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &
384:                                 AD, AB, ADBPERP)278:                                 AD, AB, ADBPERP, DIM)
385:             END IF ! One face case279:             END IF ! One face case
386:         ELSE280:         ELSE
387:             ! Simplex is not the right size, something horrible has happened281:             ! Simplex is not the right size, something horrible has happened
388:             WRITE (MYUNIT, *) 'NEAREST_SIMPLEX> ERROR, simplex size is ', &282:             WRITE (MYUNIT, *) 'NEAREST_SIMPLEX> ERROR, simplex size is ', &
389:                 LEN_SIMPLEX, ' This should not happen.'283:                 LEN_SIMPLEX, ' This should not happen.'
390:             STOP284:             STOP
391:         END IF ! Simplex size285:         END IF ! Simplex size
392: 286: 
393:     END SUBROUTINE NEAREST_SIMPLEX287:     END SUBROUTINE NEAREST_SIMPLEX
394: 288: 
395: !-------------------------------------------------------------------------------289: !-------------------------------------------------------------------------------
396: ! Assuming we know a given face or it's edges are the nearest simplex, find290: ! Assuming we know a given face or it's edges are the nearest simplex, find
397: ! which of the two edges or face it is, and generate the result simplex and291: ! which of the two edges or face it is, and generate the result simplex and
398: ! search direction.292: ! search direction.
399: ! TEST_SIMPLEX: the simplex we're checking. The third point is 'A'293: ! TEST_SIMPLEX: the simplex we're checking. The third point is 'A'
400: ! LEN_SIMPLEX: length of the simplex, 3 at the start294: ! LEN_SIMPLEX: length of the simplex, 3 at the start
401: ! SEARCH_DIRECTION: new direction to search next295: ! SEARCH_DIRECTION: new direction to search next
402: ! AB, AC: edge vectors296: ! AB, AC: edge vectors
403: ! ABCPERP: perpendicular to the face297: ! ABCPERP: perpendicular to the face
 298: ! DIM: dimension of vectors
404: !-------------------------------------------------------------------------------299: !-------------------------------------------------------------------------------
405:     SUBROUTINE CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, AB, AC, &300:     SUBROUTINE CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, AB, AC, &
406:                           ABCPERP)301:                           ABCPERP, DIM)
407:         USE VEC3, ONLY: VEC_CROSS302:         USE VEC3, ONLY: VEC_CROSS
408:         USE COMMONS, ONLY: MYUNIT303:         USE COMMONS, ONLY: MYUNIT
409: 304: 
410:         IMPLICIT NONE305:         IMPLICIT NONE
411: 306: 
412:         TYPE(SUPPORT_POINT), INTENT(INOUT) :: TEST_SIMPLEX(DIMS+1)307:         INTEGER, INTENT(IN) :: DIM
 308:         DOUBLE PRECISION, INTENT(INOUT) :: TEST_SIMPLEX(DIM, DIM+1)
413:         INTEGER, INTENT(INOUT) :: LEN_SIMPLEX309:         INTEGER, INTENT(INOUT) :: LEN_SIMPLEX
414:         DOUBLE PRECISION, INTENT(IN) :: AB(DIMS), AC(DIMS), ABCPERP(DIMS)310:         DOUBLE PRECISION, INTENT(IN) :: AB(DIM), AC(DIM), ABCPERP(DIM)
415:         DOUBLE PRECISION, INTENT(OUT) :: SEARCH_DIRECTION(DIMS)311:         DOUBLE PRECISION, INTENT(OUT) :: SEARCH_DIRECTION(DIM)
416: 312: 
417:         ! ABPERP, ACPERP: vectors perpendicular to the lines in the plane of the313:         ! ABPERP, ACPERP: vectors perpendicular to the lines in the plane of the
418:         !                 triangle314:         !                 triangle
419:         ! AO: edge from A to the origin315:         ! AO: edge from A to the origin
420:         DOUBLE PRECISION :: ABPERP(DIMS), ACPERP(DIMS), AO(DIMS)316:         DOUBLE PRECISION :: ABPERP(DIM), ACPERP(DIM), AO(DIM)
421: 317: 
422:         AO(:) = - TEST_SIMPLEX(3)%V(:)318:         AO(:) = - TEST_SIMPLEX(:, 3)
423:         ! Perpendiculars to the edges, in plane of this face319:         ! Perpendiculars to the edges, in plane of this face
424:         ABPERP(:) = VEC_CROSS(AB(:), ABCPERP(:))320:         ABPERP(:) = VEC_CROSS(AB(:), ABCPERP(:))
425:         ACPERP(:) = VEC_CROSS(ABCPERP(:), AC(:))321:         ACPERP(:) = VEC_CROSS(ABCPERP(:), AC(:))
426: 322: 
427:         IF (SAME_DIRECTION(ABPERP(:), AO(:), MYUNIT)) THEN323:         IF (SAME_DIRECTION(ABPERP(:), AO(:), DIM, MYUNIT)) THEN
428:             ! In the AB line region324:             ! In the AB line region
429:             TEST_SIMPLEX(1) = TEST_SIMPLEX(2)325:             TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
430:             TEST_SIMPLEX(2) = TEST_SIMPLEX(3)326:             TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3)
431:             LEN_SIMPLEX = 2327:             LEN_SIMPLEX = 2
432:             SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:))328:             SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM)
433:         ELSE IF (SAME_DIRECTION(ACPERP(:), AO(:), MYUNIT)) THEN329:         ELSE IF (SAME_DIRECTION(ACPERP(:), AO(:), DIM, MYUNIT)) THEN
434:             ! In the AC line region330:             ! In the AC line region
435:             TEST_SIMPLEX(2) = TEST_SIMPLEX(3)331:             TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3)
436:             LEN_SIMPLEX = 2332:             LEN_SIMPLEX = 2
437:             SEARCH_DIRECTION(:) = CROSS_121(AC(:), AO(:))333:             SEARCH_DIRECTION(:) = CROSS_121(AC(:), AO(:), DIM)
438:         ELSE334:         ELSE
439:             ! In the ABC region335:             ! In the ABC region
440:             ! Make sure we get the right side336:             ! Make sure we get the right side
441:             LEN_SIMPLEX = 3337:             LEN_SIMPLEX = 3
442: 338: 
443:             IF (SAME_DIRECTION(ABCPERP(:), AO(:), MYUNIT)) THEN339:             IF (SAME_DIRECTION(ABCPERP(:), AO(:), DIM, MYUNIT)) THEN
444:                 SEARCH_DIRECTION(:) = ABCPERP(:)340:                 SEARCH_DIRECTION(:) = ABCPERP(:)
445:             ELSE341:             ELSE
446:                 SEARCH_DIRECTION(:) = -ABCPERP(:)342:                 SEARCH_DIRECTION(:) = -ABCPERP(:)
447:                 ! Swap the simplex ordering, so the normal is 'outside'. Makes343:                 ! Swap the simplex ordering, so the point is above. Makes it
448:                 ! it easier for tetrahedra if this is guaranteed.344:                 ! easier for tetrahedra if this is guaranteed.
449:                 TEST_SIMPLEX(4) = TEST_SIMPLEX(1)345:                 TEST_SIMPLEX(:, 4) = TEST_SIMPLEX(:, 1)
450:                 TEST_SIMPLEX(1) = TEST_SIMPLEX(2)346:                 TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
451:                 TEST_SIMPLEX(2) = TEST_SIMPLEX(4)347:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
452:             END IF348:             END IF
453:         END IF349:         END IF
454:     END SUBROUTINE CHECK_FACE350:     END SUBROUTINE CHECK_FACE
455: 351: 
456: !-------------------------------------------------------------------------------352: !-------------------------------------------------------------------------------
457: ! Performs the Expanding Polytope Algorithm to generate overlap information 
458: ! SHAPE1, SHAPE2: the two polyhedra to test 
459: ! START_SIMPLEX: a simplex containing the origin in the Minkowski difference 
460: !                space. Get from the output of GJK_INTERSECTION 
461: !                We'll assume it's 3D, so simplex is of length 4 
462: ! OVERLAP_DIST: the overlap, or penetration distance. Defined as the smallest 
463: !               distance a polyhedron must move to remove overlap 
464: ! OVERLAP_VECT: overlap vector, required for derivatives 
465: ! WITNESS1, WITNESS2: the points on the shapes at the ends of OVERLAP_VECT 
466: !------------------------------------------------------------------------------- 
467:     SUBROUTINE EXPANDING_POLYTOPE(SHAPE1, SHAPE2, START_SIMPLEX, OVERLAP_DIST, & 
468:                                   OVERLAP_VECT, WITNESS1, WITNESS2) 
469:  
470:         ! MYUNIT: File unit for GMIN_out 
471:         USE COMMONS, ONLY: MYUNIT 
472:  
473:         IMPLICIT NONE 
474:  
475:         TYPE(POLYHEDRON), INTENT(IN) :: SHAPE1, SHAPE2 
476:         TYPE(SUPPORT_POINT), INTENT(IN) :: START_SIMPLEX(DIMS+1) 
477:         DOUBLE PRECISION, INTENT(OUT) :: OVERLAP_DIST, OVERLAP_VECT(DIMS) 
478:         DOUBLE PRECISION, INTENT(OUT) :: WITNESS1(DIMS), WITNESS2(DIMS) 
479:  
480:         ! FACES: Stores all the current faces 
481:         ! TEMP_FACES: Used for reallocating the array if necessary 
482:         TYPE(FACE), ALLOCATABLE :: FACES(:), TEMP_FACES(:) 
483:  
484:         ! CULL_EDGES: Stores edges while checking for culling 
485:         TYPE(EDGE), ALLOCATABLE :: CULL_EDGES(:) 
486:  
487:         ! NEW_POINT: the new point on the surface of the Minkowski difference 
488:         TYPE(SUPPORT_POINT) :: NEW_POINT 
489:  
490:         ! CLOSEST_BARY: Barycentric coordinates of the origin projected onto the 
491:         !               closest triangle 
492:         ! MIN_DISTANCE: Keep track of the closest distance to the origin 
493:         ! TOL: Tolerance used for deciding if we've moved closer to the origin 
494:         DOUBLE PRECISION :: CLOSEST_BARY(DIMS), MIN_DISTANCE 
495:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06 
496:  
497:         ! CLOSEST_INDEX: Index of the closest face to the origin 
498:         ! LEN_CULL: The number of faces to cull 
499:         ! MAX_FACES: Current maximum number of faces in array 
500:         ! NUM_FACES: Current number of faces in the expanding simplex 
501:         ! NUM_NEW: The number of new faces to add 
502:         ! CULL_FACES: Which faces need to be removed at each addition 
503:         ! TEST_UNIT: file unit for testing purposes 
504:         INTEGER :: CLOSEST_INDEX, LEN_CULL, MAX_FACES, NUM_FACES, NUM_NEW 
505:         INTEGER, ALLOCATABLE :: CULL_FACES(:) 
506:         INTEGER :: I, J, K 
507: !        INTEGER :: TEST_UNIT 
508:  
509:         ! INCLUDE_EDGES: Which edges to make new faces from 
510:         LOGICAL, ALLOCATABLE :: INCLUDE_EDGES(:) 
511:  
512:         ! GETUNIT: for finding an unused file unit, from utils.f 
513: !        INTEGER :: GETUNIT 
514:  
515:         ! Get a file unit for test_vertices.xyz 
516: !        TEST_UNIT = GETUNIT() 
517: !        OPEN(UNIT=TEST_UNIT, FILE="test_vertices.xyz", STATUS="REPLACE") 
518:  
519: !        WRITE (*,*) 'Starting EXPANDING_POLYTOPE' 
520:  
521:         ! Start by allocating 20 faces. Will be doubled if required 
522:         MAX_FACES = 20 
523:         ALLOCATE(FACES(MAX_FACES)) 
524:  
525:         ! Add the initial simplex to the array 
526:         NUM_FACES = SIZE(START_SIMPLEX) 
527:         IF (NUM_FACES .NE. 4) THEN 
528:             WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, START_SIMPLEX size ',& 
529:                               'is ', NUM_FACES, ' but should be 4' 
530:             STOP 
531:         END IF 
532:  
533:         ! ABC 
534:         FACES(1) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(3),START_SIMPLEX(2)) 
535:         ! ACD 
536:         FACES(2) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(2),START_SIMPLEX(1)) 
537:         ! ADB 
538:         FACES(3) = NEW_FACE(START_SIMPLEX(4),START_SIMPLEX(1),START_SIMPLEX(3)) 
539:         ! CBD 
540:         FACES(4) = NEW_FACE(START_SIMPLEX(2),START_SIMPLEX(3),START_SIMPLEX(1)) 
541:  
542: !        WRITE (*,*) 'Entering EPA loop' 
543:         ! Now start the refinement loop 
544:         DO 
545:             ! Write all the vertices to a file, for testing 
546: !            WRITE (TEST_UNIT, *) (NUM_FACES*3) 
547: !            WRITE (TEST_UNIT, *) 'Whatever' 
548: !            DO I = 1, NUM_FACES 
549: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%A%V(1), FACES(I)%A%V(2), & 
550: !                                           FACES(I)%A%V(3) 
551: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%B%V(1), FACES(I)%B%V(2), & 
552: !                                           FACES(I)%B%V(3) 
553: !                WRITE (TEST_UNIT, *) 'O ', FACES(I)%C%V(1), FACES(I)%C%V(2), & 
554: !                                           FACES(I)%C%V(3) 
555: !            END DO 
556:  
557:             ! First we have to identify the closest face to the origin 
558:             ! Fortunately, we already have all the distances 
559:             MIN_DISTANCE = 1.D10 
560:             CLOSEST_INDEX = -1 
561:             DO I = 1, NUM_FACES 
562: !                WRITE (*,*) 'I = ', I, ' ORIG_DIST = ', FACES(I)%ORIG_DIST 
563: !                WRITE (*,*) 'Normal = ', FACES(I)%NORMAL(1), & 
564: !                                         FACES(I)%NORMAL(2), FACES(I)%NORMAL(3) 
565:                 IF (FACES(I)%ORIG_DIST .LT. MIN_DISTANCE) THEN 
566:                     MIN_DISTANCE = FACES(I)%ORIG_DIST 
567:                     CLOSEST_INDEX = I 
568:                 END IF 
569:             END DO ! Loop over faces 
570:  
571:             ! Check that we've actually found something 
572:             IF (CLOSEST_INDEX .EQ. -1) THEN 
573:                 WRITE (MYUNIT, *) 'EXPANDING_POLYTOPE> ERROR, failed to find ',& 
574:                                   'closest distance' 
575:                 STOP 
576:             END IF 
577:  
578: !            WRITE (*,*) 'Found minimum distance ', MIN_DISTANCE 
579:             ! Generate the new point on the surface of the Minkowski difference 
580:             NEW_POINT%S1 = SUPPORT_FUNC(SHAPE1,  FACES(CLOSEST_INDEX)%NORMAL(:)) 
581:             NEW_POINT%S2 = SUPPORT_FUNC(SHAPE2, -FACES(CLOSEST_INDEX)%NORMAL(:)) 
582:             NEW_POINT%V(:) = NEW_POINT%S1(:) - NEW_POINT%S2(:) 
583: !            WRITE (*,*) 'NEW_POINT = ', NEW_POINT%V(1), NEW_POINT%V(2), 
584: !                                        NEW_POINT%V(3) 
585:  
586: !            WRITE (*,*) 'Found NEW_POINT, distance check: ', & 
587: !                         DOT_PRODUCT(NEW_POINT%V(:), & 
588: !                         FACES(CLOSEST_INDEX)%NORMAL(:)) & 
589: !                         - FACES(CLOSEST_INDEX)%ORIG_DIST 
590:  
591:             ! Check the distance from the origin to the support point against 
592:             ! the distance from the origin to the face 
593:             IF (DOT_PRODUCT(NEW_POINT%V(:), FACES(CLOSEST_INDEX)%NORMAL(:)) & 
594:                 - FACES(CLOSEST_INDEX)%ORIG_DIST .LT. TOL) THEN 
595:                 ! We've found the closest point 
596:                 OVERLAP_DIST = FACES(CLOSEST_INDEX)%ORIG_DIST 
597:                 OVERLAP_VECT = FACES(CLOSEST_INDEX)%NORMAL(:) 
598:  
599:                 ! Find the barycentric coordinates of the origin projected onto 
600:                 ! the triangle 
601:                 CLOSEST_BARY(:) = BARYCENTRIC(FACES(CLOSEST_INDEX)) 
602:                 WRITE(*,*) 'CLOSEST_BARY = ', CLOSEST_BARY(:) 
603:                 WRITE(*,*) 'A%S1 = ', FACES(CLOSEST_INDEX)%A%S1(:) 
604:                 WRITE(*,*) 'A%S1 = ', FACES(CLOSEST_INDEX)%B%S1(:) 
605:                 WRITE(*,*) 'A%S1 = ', FACES(CLOSEST_INDEX)%C%S1(:) 
606:  
607:                 ! Calculate the witness points 
608:                 WITNESS1(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S1(:) + & 
609:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S1(:) + & 
610:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S1(:) 
611:  
612:                 WITNESS2(:) = CLOSEST_BARY(1)*FACES(CLOSEST_INDEX)%A%S2(:) + & 
613:                               CLOSEST_BARY(2)*FACES(CLOSEST_INDEX)%B%S2(:) + & 
614:                               CLOSEST_BARY(3)*FACES(CLOSEST_INDEX)%C%S2(:) 
615:                 EXIT 
616:             END IF 
617:  
618:             ! We haven't found the answer 
619:  
620:             ! Do a winding check to see which faces need to be removed 
621:             ! We remove all faces that can be 'seen' from the new point 
622:             ALLOCATE(CULL_FACES(NUM_FACES)) 
623:             LEN_CULL = 0 
624:             DO I = 1, NUM_FACES 
625:                 IF (DOT_PRODUCT(FACES(I)%NORMAL(:), & 
626:                     NEW_POINT%V(:) - FACES(I)%A%V(:)) .GT. 0.D0) THEN 
627:                     LEN_CULL = LEN_CULL + 1 
628:                     CULL_FACES(LEN_CULL) = I 
629:                 END IF 
630:             END DO 
631:  
632:             ! Testing only 
633: !            WRITE(*,*) 'LEN_CULL = ', LEN_CULL 
634: !            DO I = 1, LEN_CULL 
635: !                WRITE (*,*) 'Cull face ', CULL_FACES(I) 
636: !            END DO 
637:  
638:             ! Now we loop through the culled edges. We only want to include 
639:             ! unique edges, which gives us the boundary of the 'hole' left by 
640:             ! the culled faces. 
641:             ALLOCATE(CULL_EDGES(LEN_CULL*3)) 
642:             ALLOCATE(INCLUDE_EDGES(LEN_CULL*3)) 
643:             INCLUDE_EDGES(:) = .TRUE. 
644:             DO I = 1, LEN_CULL ! Loop over the culled faces 
645:                 ! Add the new edges to the list 
646:                 CULL_EDGES(3*I-2)%A = FACES(CULL_FACES(I))%A 
647:                 CULL_EDGES(3*I-2)%B = FACES(CULL_FACES(I))%B 
648:                 CULL_EDGES(3*I-1)%A = FACES(CULL_FACES(I))%B 
649:                 CULL_EDGES(3*I-1)%B = FACES(CULL_FACES(I))%C 
650:                 CULL_EDGES(3*I  )%A = FACES(CULL_FACES(I))%C 
651:                 CULL_EDGES(3*I  )%B = FACES(CULL_FACES(I))%A 
652:  
653:                 DO J = 1, 3 ! Loop over the new edges 
654:                     DO K = 1, 3*(I-1) ! Loop over the old edges 
655:                         IF (INCLUDE_EDGES(K)) THEN 
656:                             IF (CHECK_EDGE(CULL_EDGES(K), & 
657:                                            CULL_EDGES(3*(I-1) + J))) THEN 
658:                                 INCLUDE_EDGES(K) = .FALSE. 
659:                                 INCLUDE_EDGES(3*(I-1) + J) = .FALSE. 
660:                                 EXIT ! Don't need to check any more old edges 
661:                             END IF 
662:                         END IF 
663:                     END DO ! Loop over the old edges 
664:                 END DO ! Loop over the new edges 
665:             END DO ! Loop over the culled faces 
666:  
667: !            DO I = 1, LEN_CULL*3 
668: !                WRITE (*,*) 'Added edge: ', INCLUDE_EDGES(I), & 
669: !                CULL_EDGES(I)%A%V(1), CULL_EDGES(I)%A%V(2), & 
670: !                CULL_EDGES(I)%A%V(3), ' to ', & 
671: !                CULL_EDGES(I)%B%V(1), CULL_EDGES(I)%B%V(2), & 
672: !                CULL_EDGES(I)%B%V(3) 
673: !            END DO 
674:  
675:             ! Get the number of new faces 
676:             NUM_NEW = 0 
677:             DO I = 1, LEN_CULL*3 ! Loop over new edges 
678:                 IF (INCLUDE_EDGES(I)) NUM_NEW = NUM_NEW + 1 
679:             END DO 
680:  
681:             ! Check whether we're about to exceed the FACES array bounds 
682:             IF (NUM_FACES - LEN_CULL + NUM_NEW .GT. MAX_FACES) THEN 
683:                 ! Reallocate the array 
684:                 ALLOCATE(TEMP_FACES(NUM_FACES)) 
685:                 TEMP_FACES(:) = FACES(1:NUM_FACES) 
686:                 DEALLOCATE(FACES) 
687:                 MAX_FACES = MAX_FACES*2 
688:                 ALLOCATE(FACES(MAX_FACES)) 
689:                 FACES(1:NUM_FACES) = TEMP_FACES(:) 
690:                 DEALLOCATE(TEMP_FACES) 
691:                 ! Stop here for testing 
692:                 STOP 
693:             END IF 
694:  
695:             ! Add all the new faces 
696:             ! Keep track of which edge we're on with J 
697:             J = 1 
698:             DO I = 1, LEN_CULL 
699:                 ! Start by replacing deleted faces 
700:                 DO WHILE (.NOT. INCLUDE_EDGES(J)) 
701:                     J = J + 1 
702:                 END DO 
703:                 FACES(CULL_FACES(I)) = NEW_FACE(CULL_EDGES(J)%A, & 
704:                                                 CULL_EDGES(J)%B, & 
705:                                                 NEW_POINT) 
706:                 J = J + 1 
707:             END DO ! Loop over deleted faces 
708:  
709:             ! Add the rest of the new faces to the end 
710:             DO I = J, LEN_CULL*3 
711:                 IF (.NOT. INCLUDE_EDGES(I)) CYCLE 
712:                 NUM_FACES = NUM_FACES + 1 
713:                 FACES(NUM_FACES) = NEW_FACE(CULL_EDGES(I)%A, CULL_EDGES(I)%B, & 
714:                                             NEW_POINT) 
715:             END DO ! Loop over new edges 
716:  
717:             ! Deallocate the arrays for next time round 
718:             DEALLOCATE(CULL_FACES) 
719:             DEALLOCATE(CULL_EDGES) 
720:             DEALLOCATE(INCLUDE_EDGES) 
721:  
722:         END DO ! EPA refinement loop 
723:  
724: !        CLOSE(TEST_UNIT) 
725:  
726:     END SUBROUTINE EXPANDING_POLYTOPE 
727:  
728: !------------------------------------------------------------------------------- 
729: ! Utility function for dot products.353: ! Utility function for dot products.
730: ! All we care about is whether or not the dot is positive or negative354: ! All we care about is whether or not the dot is positive or negative
731: ! Print a warning if it's zero355: ! Print a warning if it's zero
732: ! VEC1, VEC2: the vectors to dot356: ! VEC1, VEC2: the vectors to dot
 357: ! DIM: dimension of vectors
733: ! OUT_UNIT: File unit for warnings358: ! OUT_UNIT: File unit for warnings
734: !-------------------------------------------------------------------------------359: !-------------------------------------------------------------------------------
735:     LOGICAL FUNCTION SAME_DIRECTION(VEC1, VEC2, OUT_UNIT)360:     LOGICAL FUNCTION SAME_DIRECTION(VEC1, VEC2, DIM, OUT_UNIT)
736: 361: 
737:         IMPLICIT NONE362:         IMPLICIT NONE
738: 363: 
739:         INTEGER, INTENT(IN) :: OUT_UNIT364:         INTEGER, INTENT(IN) :: DIM, OUT_UNIT
740:         DOUBLE PRECISION, INTENT(IN) :: VEC1(DIMS), VEC2(DIMS)365:         DOUBLE PRECISION, INTENT(IN) :: VEC1(DIM), VEC2(DIM)
741: 366: 
742:         ! DOT: store the dot product367:         ! DOT: store the dot product
743:         DOUBLE PRECISION :: DOT368:         DOUBLE PRECISION :: DOT
744: 369: 
745:         DOT = DOT_PRODUCT(VEC1(:), VEC2(:))370:         DOT = DOT_PRODUCT(VEC1(:), VEC2(:))
746: 371: 
747:         IF (DOT .EQ. 0.D0) THEN372:         IF (DOT .EQ. 0.D0) THEN
748:             WRITE(OUT_UNIT, *) 'NEAREST_SIMPLEX> WARNING: exact overlap'373:             WRITE(OUT_UNIT, *) 'NEAREST_SIMPLEX> WARNING: exact overlap'
749:             SAME_DIRECTION = .FALSE.374:             SAME_DIRECTION = .FALSE.
750:         ELSE375:         ELSE
751:             SAME_DIRECTION = (DOT .GT. 0.D0)376:             SAME_DIRECTION = (DOT .GT. 0.D0)
752:         END IF377:         END IF
753:     END FUNCTION SAME_DIRECTION378:     END FUNCTION SAME_DIRECTION
754: 379: 
755: !-------------------------------------------------------------------------------380: !-------------------------------------------------------------------------------
756: ! Utility function for cross products.381: ! Utility function for cross products.
757: ! Carries out VEC1 x VEC2 x VEC1382: ! Carries out VEC1 x VEC2 x VEC1
758: ! VEC1, VEC2: the vectors to cross383: ! VEC1, VEC2: the vectors to cross
 384: ! DIM: Dimension of the space (which I suppose must be 3 for cross products)
759: !-------------------------------------------------------------------------------385: !-------------------------------------------------------------------------------
760:     FUNCTION CROSS_121(VEC1, VEC2)386:     FUNCTION CROSS_121(VEC1, VEC2, DIM)
761: 387: 
762:         ! VEC_CROSS: cross producr388:         ! VEC_CROSS: cross producr
763:         USE VEC3, ONLY: VEC_CROSS389:         USE VEC3, ONLY: VEC_CROSS
764: 390: 
765:         IMPLICIT NONE391:         IMPLICIT NONE
766: 392: 
767:         DOUBLE PRECISION, INTENT(IN) :: VEC1(DIMS), VEC2(DIMS)393:         INTEGER, INTENT(IN) :: DIM
768:         DOUBLE PRECISION :: CROSS_121(DIMS)394:         DOUBLE PRECISION, INTENT(IN) :: VEC1(DIM), VEC2(DIM)
 395:         DOUBLE PRECISION :: CROSS_121(DIM)
769: 396: 
770:         CROSS_121(:) = VEC_CROSS(VEC_CROSS(VEC1(:), VEC2(:)), VEC1(:))397:         CROSS_121(:) = VEC_CROSS(VEC_CROSS(VEC1(:), VEC2(:)), VEC1(:))
771:     END FUNCTION CROSS_121398:     END FUNCTION CROSS_121
772: 399: 
773: !------------------------------------------------------------------------------- 
774: ! Makes a new FACE. Always use this instead of generating by hand, to make sure 
775: ! the normal and origin distance are always calculated. 
776: ! A, B, C: Vertices of the face 
777: !------------------------------------------------------------------------------- 
778:     TYPE(FACE) FUNCTION NEW_FACE(A, B, C) 
779:  
780:         ! VEC_CROSS: vector cross product 
781:         ! VEC_LEN: Length of a vector 
782:         USE VEC3, ONLY: VEC_CROSS, VEC_LEN 
783:  
784:         IMPLICIT NONE 
785:  
786:         TYPE(SUPPORT_POINT), INTENT(IN) :: A, B, C 
787:  
788:         ! AB, AC: edge vectors 
789:         DOUBLE PRECISION :: AB(DIMS), AC(DIMS) 
790:  
791:         ! Set the vertices 
792:         NEW_FACE%A = A 
793:         NEW_FACE%B = B 
794:         NEW_FACE%C = C 
795:  
796:         AB(:) = B%V(:) - A%V(:) 
797:         AC(:) = C%V(:) - A%V(:) 
798:  
799:         ! Calculate the normal (should be pointing away from the origin) 
800:         NEW_FACE%NORMAL(:) = VEC_CROSS(AB(:), AC(:)) 
801:  
802:         ! Normalise 
803:         NEW_FACE%NORMAL(:) = NEW_FACE%NORMAL(:) / VEC_LEN(NEW_FACE%NORMAL(:)) 
804:  
805:         ! Calculate the distance to the origin 
806:         NEW_FACE%ORIG_DIST = ABS(DOT_PRODUCT(NEW_FACE%NORMAL(:), & 
807:                                              NEW_FACE%A%V(:))) 
808:  
809:     END FUNCTION NEW_FACE 
810:  
811: !------------------------------------------------------------------------------- 
812: ! Checks whether two edges are the same as each other (regardless of direction) 
813: ! EDGE1, EDGE2: The two edges to check 
814: !------------------------------------------------------------------------------- 
815:     LOGICAL FUNCTION CHECK_EDGE(EDGE1, EDGE2) 
816:  
817:         IMPLICIT NONE 
818:  
819:         TYPE(EDGE), INTENT(IN) :: EDGE1, EDGE2 
820:  
821:         ! TOL: numerical tolerance for testing equality 
822:         DOUBLE PRECISION, PARAMETER :: TOL = 1.D-06 
823:  
824:         ! DIFFAA, DIFFBB, DIFFAB, DIFFBA: differences between the vertices 
825:         DOUBLE PRECISION :: DIFFAA(DIMS), DIFFBB(DIMS) 
826:         DOUBLE PRECISION :: DIFFAB(DIMS), DIFFBA(DIMS) 
827:  
828:         DIFFAA(:) = ABS(EDGE1%A%V(:) - EDGE2%A%V(:)) 
829:         DIFFBB(:) = ABS(EDGE1%B%V(:) - EDGE2%B%V(:)) 
830:         DIFFAB(:) = ABS(EDGE1%A%V(:) - EDGE2%B%V(:)) 
831:         DIFFBA(:) = ABS(EDGE1%B%V(:) - EDGE2%A%V(:)) 
832:  
833: !        WRITE(*, *) 'Checking edge: ',EDGE1%A%V(1),EDGE1%A%V(2),EDGE1%A%V(3), & 
834: !                               ' to ',EDGE1%B%V(1),EDGE1%B%V(2),EDGE1%B%V(3) 
835: !        WRITE(*, *) 'Against: ', EDGE2%A%V(1), EDGE2%A%V(2), EDGE2%A%V(3), & 
836: !                         ' to ', EDGE2%B%V(1), EDGE2%B%V(2), EDGE2%B%V(3) 
837:  
838:  
839:         CHECK_EDGE = ((ALL(DIFFAA .LT. TOL) .AND. ALL(DIFFBB .LT. TOL)) .OR. & 
840:                       (ALL(DIFFAB .LT. TOL) .AND. ALL(DIFFBA .LT. TOL))) 
841:  
842: !        WRITE(*, *) 'Result is: ', CHECK_EDGE 
843:  
844:     END FUNCTION CHECK_EDGE 
845:  
846: !------------------------------------------------------------------------------- 
847: ! Calculates the projection of the origin onto a triangle in barycentric 
848: ! coordinates 
849: ! See the bottom of http://hacktank.net/blog/?p=119 
850: ! TRIANG: the face we are calculating for 
851: !------------------------------------------------------------------------------- 
852:     FUNCTION BARYCENTRIC(TRIANG) 
853:  
854:         IMPLICIT NONE 
855:  
856:         TYPE(FACE), INTENT(IN) :: TRIANG 
857:         DOUBLE PRECISION :: BARYCENTRIC(DIMS) 
858:  
859:         ! V0, V1, V2, D00, D01, D11, D20, D21, DENOM: temporary quantities 
860:         DOUBLE PRECISION :: V0(DIMS), V1(DIMS), V2(DIMS) 
861:         DOUBLE PRECISION :: D00, D01, D11, D20, D21, DENOM 
862:  
863:         V0(:) = TRIANG%B%V(:) - TRIANG%A%V(:) 
864:         V1(:) = TRIANG%C%V(:) - TRIANG%A%V(:) 
865:         V2(:) = TRIANG%ORIG_DIST*TRIANG%NORMAL(:) - TRIANG%A%V(:) 
866:  
867:         D00 = DOT_PRODUCT(V0(:), V0(:)) 
868:         D01 = DOT_PRODUCT(V0(:), V1(:)) 
869:         D11 = DOT_PRODUCT(V1(:), V1(:)) 
870:         D20 = DOT_PRODUCT(V2(:), V0(:)) 
871:         D21 = DOT_PRODUCT(V2(:), V1(:)) 
872:  
873:         DENOM = D00*D11 - D01*D01 
874:         BARYCENTRIC(2) = (D11*D20 - D01*D21) / DENOM 
875:         BARYCENTRIC(3) = (D00*D21 - D01*D20) / DENOM 
876:         BARYCENTRIC(1) = 1.D0 - BARYCENTRIC(2) - BARYCENTRIC(3) 
877:  
878:     END FUNCTION BARYCENTRIC 
879:  
880: END MODULE GJK_MODULE400: END MODULE GJK_MODULE


legend
Lines Added 
Lines changed
 Lines Removed

hdiff - version: 2.1.0