hdiff output

r31871/convex_polyhedra.f90 2017-01-31 21:30:07.695247925 +0000 r31870/convex_polyhedra.f90 2017-01-31 21:30:10.419284944 +0000
 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:     ! NVERTS: Number of vertices in each polyhedron
 34:     ! X_SIZE: Size of the coordinates array 34:     ! X_SIZE: Size of the coordinates array
 35:     INTEGER :: NPOLY, X_SIZE 35:     INTEGER :: NPOLY, X_SIZE
 36:  36: 
 37:     ! The array of polyhedra 37:     ! The array of polyhedra
 38:     TYPE(POLYHEDRON), ALLOCATABLE :: POLYHEDRA(:) 38:     TYPE(POLYHEDRON), ALLOCATABLE :: POLYHEDRA(:)
 39:  39: 
 40:     ! Array of the reference vertex positions 
 41:     DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:) :: VERTS_0 
 42:  
 43: CONTAINS 40: CONTAINS
 44: !------------------------------------------------------------------------------- 41: !-------------------------------------------------------------------------------
 45: ! Initialise the system, including reading the vertex list from the file 42: ! Initialise the system, including reading the vertex list from the file
 46: ! "polyhedron_vertices.dat" 43: ! "polyhedron_vertices.dat"
 47: !------------------------------------------------------------------------------- 44: !-------------------------------------------------------------------------------
 48:     SUBROUTINE INITIALISE_POLYHEDRA() 45:     SUBROUTINE INITIALISE_POLYHEDRA()
 49:  46: 
 50:         ! MYUNIT: File unit of GMIN_out 47:         ! MYUNIT: File unit of GMIN_out
 51:         ! NATOMS: the number of coordinate lines in the coords file, so twice 48:         ! NATOMS: the number of coordinate lines in the coords file, so twice
 52:         !         the actual number of bodies 49:         !         the actual number of bodies
 56:         USE COMMONS, ONLY: MYUNIT, NATOMS 53:         USE COMMONS, ONLY: MYUNIT, NATOMS
 57:         USE GJK_UTILS, ONLY: DIM, NVERTS 54:         USE GJK_UTILS, ONLY: DIM, NVERTS
 58:         USE VEC3, ONLY: VEC_CROSS 55:         USE VEC3, ONLY: VEC_CROSS
 59:  56: 
 60:         IMPLICIT NONE 57:         IMPLICIT NONE
 61:  58: 
 62:         ! GETUNIT: Function for finding an unused file unit, from utils.f 59:         ! GETUNIT: Function for finding an unused file unit, from utils.f
 63:         ! VERT_UNIT: File unit for "polyhedron_vertices.dat" 60:         ! VERT_UNIT: File unit for "polyhedron_vertices.dat"
 64:         INTEGER :: GETUNIT, VERT_UNIT, I 61:         INTEGER :: GETUNIT, VERT_UNIT, I
 65:  62: 
  63:         ! TEMP_VERTS: Temporary vertex information
 66:         ! NORMAL: Normal to the first face read 64:         ! NORMAL: Normal to the first face read
 67:         ! TOLERANCE: Numerical tolerance for coplanar test 65:         ! TOLERANCE: Numerical tolerance for coplanar test
 68:         DOUBLE PRECISION :: NORMAL(DIM) 66:         DOUBLE PRECISION, ALLOCATABLE :: TEMP_VERTS(:,:)
  67:         DOUBLE PRECISION :: NORMAL(3)
 69:         DOUBLE PRECISION, PARAMETER :: TOL = 1.0D-6 68:         DOUBLE PRECISION, PARAMETER :: TOL = 1.0D-6
 70:  69: 
 71:         ! COPLANAR: whether all the vertices are coplanar 70:         ! COPLANAR: whether all the vertices are coplanar
 72:         LOGICAL :: COPLANAR 71:         LOGICAL :: COPLANAR
 73:  72: 
 74:         ! Set global module parameters 73:         ! Set global module parameters
 75:         NPOLY = NATOMS / 2 74:         NPOLY = NATOMS / 2
 76:         X_SIZE = NATOMS * 3 75:         X_SIZE = NATOMS * 3
 77:  76: 
 78:         WRITE (MYUNIT, *) 'INITIALISE_POLYHEDRA> ', NPOLY, ' polyhedra' 77:         WRITE (MYUNIT, *) 'INITIALISE_POLYHEDRA> ', NPOLY, ' polyhedra'
 93:         READ(VERT_UNIT, *) NVERTS 92:         READ(VERT_UNIT, *) NVERTS
 94:  93: 
 95:         ! Check that we've got at least four vertices 94:         ! Check that we've got at least four vertices
 96:         IF (NVERTS .LT. 4) THEN 95:         IF (NVERTS .LT. 4) THEN
 97:             WRITE (MYUNIT, *) & 96:             WRITE (MYUNIT, *) &
 98:                 'INITIALISE_POLYHEDA> ERROR: require at least 4 vertices' 97:                 'INITIALISE_POLYHEDA> ERROR: require at least 4 vertices'
 99:             STOP 98:             STOP
100:         END IF 99:         END IF
101: 100: 
102:         ! Read the reference vertices101:         ! Read the reference vertices
103:         ALLOCATE(VERTS_0(DIM, NVERTS))102:         ALLOCATE(TEMP_VERTS(DIM, NVERTS))
104:         DO I = 1, NVERTS103:         DO I = 1, NVERTS
105:             READ(VERT_UNIT, *) VERTS_0(1,I), VERTS_0(2,I), VERTS_0(3,I)104:             READ(VERT_UNIT, *) TEMP_VERTS(1,I), TEMP_VERTS(2,I), TEMP_VERTS(3,I)
106:         END DO105:         END DO
107: 106: 
108:         ! Check that the vertices are not all coplanar107:         ! Check that the vertices are not all coplanar
109:         NORMAL(:) = VEC_CROSS((VERTS_0(:, 1) - VERTS_0(:, 2)), &108:         NORMAL = VEC_CROSS((TEMP_VERTS(:, 1) - TEMP_VERTS(:, 2)), &
110:                                (VERTS_0(:, 1) - VERTS_0(:, 3)))109:                                (TEMP_VERTS(:, 1) - TEMP_VERTS(:, 3)))
111:         COPLANAR = .TRUE.110:         COPLANAR = .TRUE.
112:         DO I = 4, NVERTS ! Loop over vertices111:         DO I = 4, NVERTS ! Loop over vertices
113:             IF (ABS(DOT_PRODUCT((VERTS_0(:, I) - VERTS_0(:, 1)), NORMAL)) &112:             IF (DOT_PRODUCT((TEMP_VERTS(:, I) - TEMP_VERTS(:, 1)), NORMAL) &
114:                 .GT. TOL) THEN113:                 .GT. TOL) THEN
115:                 COPLANAR = .FALSE.114:                 COPLANAR = .FALSE.
116:                 EXIT115:                 EXIT
117:             END IF116:             END IF
118:         END DO ! Loop over vertices117:         END DO ! Loop over vertices
119: 118: 
120:         ! Error out if the vertices are all coplanar119:         ! Error out if the vertices are all coplanar
121:         IF (COPLANAR) THEN120:         IF (COPLANAR) THEN
122:             WRITE (MYUNIT, *) &121:             WRITE (MYUNIT, *) &
123:                 'INITIALISE_POLYHEDRA> ERROR: all vertices are coplanar'122:                 'INITIALISE_POLYHEDRA> ERROR: all vertices are coplanar'
124:             STOP123:             STOP
125:         END IF124:         END IF
126: 125: 
127:         ! Copy the reference vertices to the polyhedra126:         ! Copy the reference vertices to the polyhedra
128:         DO I = 1, NPOLY127:         DO I = 1, NPOLY
129:             ALLOCATE(POLYHEDRA(I)%VERTS(DIM, NVERTS))128:             ALLOCATE(POLYHEDRA(NPOLY)%VERTS(DIM, NVERTS))
130:             POLYHEDRA(I)%VERTS(:, :) = VERTS_0(:, :)129:             POLYHEDRA(NPOLY)%VERTS(:, :) = TEMP_VERTS(:, :)
131:         END DO130:         END DO
132: 131: 
133:     END SUBROUTINE INITIALISE_POLYHEDRA132:     END SUBROUTINE INITIALISE_POLYHEDRA
134: 133: 
135: !-------------------------------------------------------------------------------134: !-------------------------------------------------------------------------------
136: ! Updates a polyhedron with the rotation matrix and derivatives,135: ! Updates a polyhedron with the rotation matrix and derivatives,
137: ! and the position of the vertices.136: ! and the position of the vertices.
138: ! POLY: the polyhedron to operate on137: ! POLY: the polyhedron to operate on
139: ! R: Coordinates of the centre of the particle138: ! R: Coordinates of the centre of the particle
140: ! P: Angle-axis style orientation of the particle139: ! P: Angle-axis style orientation of the particle
141: !    INTENT(INOUT) because orientations are reranged140: !    INTENT(INOUT) because orientations are reranged
142: ! GTEST: Whether to computer derivatives141: ! GTEST: Whether to computer derivatives
143: !-------------------------------------------------------------------------------142: !-------------------------------------------------------------------------------
144:     SUBROUTINE UPDATE_POLYHEDRON(POLY, R, P, GTEST)143:     SUBROUTINE UPDATE_POLYHEDRON(POLY, R, P, GTEST)
145: 144: 
146:         ! DIM: Dimension of the space145:         ! DIM: Dimension of the space
147:         ! NVERTS: Number of vertices per particle 
148:         ! POLYHEDRON: Type for storing a polyhedron146:         ! POLYHEDRON: Type for storing a polyhedron
149:         USE GJK_UTILS, ONLY: DIM, NVERTS, POLYHEDRON147:         USE GJK_UTILS, ONLY: DIM, POLYHEDRON
150: 148: 
151:         IMPLICIT NONE149:         IMPLICIT NONE
152: 150: 
153:         TYPE(POLYHEDRON), INTENT(OUT) :: POLY151:         TYPE(POLYHEDRON), INTENT(OUT) :: POLY
154:         DOUBLE PRECISION, INTENT(IN) :: R(DIM)152:         DOUBLE PRECISION, INTENT(IN) :: R(DIM)
155:         DOUBLE PRECISION, INTENT(INOUT) :: P(DIM)153:         DOUBLE PRECISION, INTENT(INOUT) :: P(DIM)
156:         LOGICAL, INTENT(IN) :: GTEST154:         LOGICAL, INTENT(IN) :: GTEST
157: 155: 
158:         INTEGER :: I 
159:         DOUBLE PRECISION :: PI = 4.0D0 * ATAN(1.0D0)156:         DOUBLE PRECISION :: PI = 4.0D0 * ATAN(1.0D0)
160:         ! PMOD: angle of the angle axis orientation157:         ! PMOD: angle of the angle axis orientation
161:         DOUBLE PRECISION :: PMOD158:         DOUBLE PRECISION :: PMOD
162: 159: 
163:         POLY%R(:) = R(:)160:         POLY%R(:) = R(:)
164: 161: 
165:         ! Make sure that 0 < |p| < 2*pi162:         ! Make sure that 0 < |p| < 2*pi
166:         PMOD = sqrt(dot_product(p, p))163:         PMOD = sqrt(dot_product(p, p))
167:         IF(PMOD > 2 * PI) P(:) = P(:) / (PMOD * MOD(PMOD, 2 * PI))164:         IF(PMOD > 2 * PI) P(:) = P(:) / (PMOD * MOD(PMOD, 2 * PI))
168:         POLY%P(:) = P(:)165:         POLY%P(:) = P(:)
169: 166: 
170:         ! Compute the rotation matrices and derivatives167:         ! Compute the rotation matrices and derivatives
171:         CALL RMDRVT(POLY%P, POLY%RM, POLY%RMD(1,:,:), POLY%RMD(2,:,:), &168:         CALL RMDRVT(POLY%P, POLY%RM, POLY%RMD(1,:,:), POLY%RMD(2,:,:), &
172:             POLY%RMD(3,:,:), GTEST)169:             POLY%RMD(3,:,:), GTEST)
173: 170: 
174:         ! Apply rotation matrix and translational offset to all the vertices 
175:         ALLOCATE(POLY%VERTS(DIM,NVERTS)) 
176:         DO I = 1, NVERTS 
177:             POLY%VERTS(:, I) = POLY%R(:) + MATMUL(POLY%RM, VERTS_0(:, I)) 
178:         END DO 
179:  
180:     END SUBROUTINE UPDATE_POLYHEDRON171:     END SUBROUTINE UPDATE_POLYHEDRON
181: 172: 
182: !-------------------------------------------------------------------------------173: !-------------------------------------------------------------------------------
183: ! Calculates the potential, and optionally the gradients174: ! Calculates the potential, and optionally the gradients
184: ! X: array of coordinates, all translations, then all orientations175: ! X: array of coordinates, all translations, then all orientations
185: !    INTENT(INOUT) because orientation may be reranged176: !    INTENT(INOUT) because orientation may be reranged
186: ! GRAD: array of gradients, all translations, then all orientations177: ! GRAD: array of gradients, all translations, then all orientations
187: ! ENERGY: the energy of the system178: ! ENERGY: the energy of the system
188: ! GTEST: whether gradients are required179: ! GTEST: whether gradients are required
189: !-------------------------------------------------------------------------------180: !-------------------------------------------------------------------------------
194:         USE GJK_MODULE, ONLY: GJK_INTERSECTION185:         USE GJK_MODULE, ONLY: GJK_INTERSECTION
195:         USE GJK_UTILS, ONLY: DIM186:         USE GJK_UTILS, ONLY: DIM
196: 187: 
197:         IMPLICIT NONE188:         IMPLICIT NONE
198: 189: 
199:         DOUBLE PRECISION, INTENT(INOUT) :: X(X_SIZE)190:         DOUBLE PRECISION, INTENT(INOUT) :: X(X_SIZE)
200:         DOUBLE PRECISION, INTENT(OUT) :: GRAD(X_SIZE), ENERGY191:         DOUBLE PRECISION, INTENT(OUT) :: GRAD(X_SIZE), ENERGY
201:         LOGICAL, INTENT(IN) :: GTEST192:         LOGICAL, INTENT(IN) :: GTEST
202: 193: 
203:         ! MOLI_IND, MOLJ_IND: indices for looping over molecules194:         ! MOLI_IND, MOLJ_IND: indices for looping over molecules
204:         ! OFFSET: The end of the position coordinates in X195:         ! OFFSET: The start of the orientation coordinates in X
205:         INTEGER :: MOLI_IND, MOLJ_IND, OFFSET196:         INTEGER :: MOLI_IND, MOLJ_IND, OFFSET
206: 197: 
207:         ! INIT_AXIS: Initial direction guess for GJK, just use the difference of198:         ! INIT_AXIS: Initial direction guess for GJK, just use the difference of
208:         !            the centres199:         !            the centres
209:         ! RESULT_SIMPLEX: a simplex enclosing the origin if GJK finds an200:         ! RESULT_SIMPLEX: a simplex enclosing the origin if GJK finds an
210:         !                 intersection201:         !                 intersection
211:         DOUBLE PRECISION :: INIT_AXIS(DIM), RESULT_SIMPLEX(DIM, DIM+1)202:         DOUBLE PRECISION :: INIT_AXIS(DIM), RESULT_SIMPLEX(DIM, DIM+1)
212: 203: 
213:         ! RESULT: whether or not GJK finds an intersection204:         ! RESULT: whether or not GJK finds an intersection
214:         LOGICAL :: RESULT205:         LOGICAL :: RESULT
215: 206: 
216:         ! Initialise ouput variables207:         ! Initialise ouput variables
217:         ENERGY = 0.D0208:         ENERGY = 0.D0
218:         IF (GTEST) GRAD(:) = 0.D0209:         IF (GTEST) GRAD(:) = 0.D0
219: 210: 
220:         ! Calculate offset from the number of particles211:         ! Calculate offset from the number of particles
221:         OFFSET = 3*NPOLY212:         OFFSET = 3*NPOLY + 1
222: 213: 
223:         ! Update all the particle positions and rotation matrices214:         ! Update all the particle positions and rotation matrices
224:         DO MOLI_IND = 1, NPOLY215:         DO MOLI_IND = 1, NPOLY
225:             CALL UPDATE_POLYHEDRON(POLYHEDRA(MOLI_IND), &216:             CALL UPDATE_POLYHEDRON(POLYHEDRA(MOLI_IND), &
226:                                    X(MOLI_IND*3-2:MOLI_IND*3), &217:                                    X(MOLI_IND*3-2:MOLI_IND*3), &
227:                                    X(OFFSET+MOLI_IND*3-2:OFFSET+MOLI_IND*3), &218:                                    X(OFFSET+MOLI_IND*3-2:OFFSET+MOLI_IND*3), &
228:                                    GTEST)219:                                    GTEST)
229:         END DO220:         END DO
230: 221: 
231:         ! Just looking to test at the moment222:         ! Just looking to test at the moment


r31871/gjk.f90 2017-01-31 21:30:07.923251024 +0000 r31870/gjk.f90 2017-01-31 21:30:10.643287992 +0000
170:             SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM)170:             SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM)
171: 171: 
172:         ELSE IF (LEN_SIMPLEX .EQ. 3) THEN172:         ELSE IF (LEN_SIMPLEX .EQ. 3) THEN
173:             ! Simplex is a triangle. Nearest simplex still cannot be a point.173:             ! Simplex is a triangle. Nearest simplex still cannot be a point.
174:             ! 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
175:             ! third.175:             ! third.
176:             ! Origin could lie above or below the triangle.176:             ! Origin could lie above or below the triangle.
177:             RESULT = .FALSE.177:             RESULT = .FALSE.
178: 178: 
179:             ! Generate the necessary vectors179:             ! Generate the necessary vectors
 180:             AO(:) = - TEST_SIMPLEX(:, 3)
180:             AB(:) = TEST_SIMPLEX(:, 2) - TEST_SIMPLEX(:, 3)181:             AB(:) = TEST_SIMPLEX(:, 2) - TEST_SIMPLEX(:, 3)
181:             AC(:) = TEST_SIMPLEX(:, 1) - TEST_SIMPLEX(:, 3)182:             AC(:) = TEST_SIMPLEX(:, 1) - TEST_SIMPLEX(:, 3)
182:             ABCPERP(:) = VEC_CROSS(AB(:), AC(:))183:             ABCPERP(:) = VEC_CROSS(AB(:), AC(:))
 184:             ABPERP(:) = VEC_CROSS(AB(:), ABCPERP(:))
 185:             ACPERP(:) = VEC_CROSS(ABCPERP(:), AC(:))
183: 186: 
184:             ! Check whether the origin is outside the triangle on the AB side187:             ! Check whether the origin is outside the triangle on the AB side
185:             CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, AB, &188:             IF (SAME_DIRECTION(ABPERP(:), AO(:), DIM, MYUNIT)) THEN
186:                             AC, ABCPERP, DIM)189:                 ! AB is the closest simplex
 190:                 ! Delete C from the simplex
 191:                 TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
 192:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3)
 193:                 LEN_SIMPLEX = 2
 194:                 SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM)
 195: 
 196:             ! Check whether the origin is outside the triangle on the AC side
 197:             ELSE IF (SAME_DIRECTION(ACPERP(:), AO(:), DIM, MYUNIT)) THEN
 198:                 ! AC is the closest simplex
 199:                 ! Delete B from the simplex
 200:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3)
 201:                 LEN_SIMPLEX = 2
 202:                 SEARCH_DIRECTION(:) = CROSS_121(AC(:), AO(:), DIM)
 203:             ELSE
 204:                 ! ABC is the closest simplex
 205:                 ! Need to pick the correct direction (above or below face)
 206:                 IF (SAME_DIRECTION(ABCPERP(:), AO(:), DIM, MYUNIT)) THEN
 207:                     SEARCH_DIRECTION(:) = ABCPERP(:)
 208:                 ELSE
 209:                     SEARCH_DIRECTION(:) = -ABCPERP(:)
 210:                     ! Swap the simplex ordering, so the point is above. Makes it
 211:                     ! easier for tetrahedra if this is guaranteed.
 212:                     TEST_SIMPLEX(:, 4) = TEST_SIMPLEX(:, 1)
 213:                     TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
 214:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
 215:                 END IF
 216:             END IF
187: 217: 
188:         ELSE IF (LEN_SIMPLEX .EQ. 4) THEN218:         ELSE IF (LEN_SIMPLEX .EQ. 4) THEN
189:             ! Simplex is a tetrahedron. It might enclose the origin.219:             ! Simplex is a tetrahedron. It might enclose the origin.
190:             ! Only lines and faces involving A need to be considered.220:             ! Only lines and faces involving A need to be considered.
191:             ! See http://vec3.ca/gjk/implementation/ for an explanation221:             ! See http://vec3.ca/gjk/implementation/ for an explanation
192: 222: 
193:             ! Generate the necessary vectors223:             ! Generate the necessary vectors
194:             AO(:) = - TEST_SIMPLEX(:, 4)224:             AO(:) = - TEST_SIMPLEX(:, 4)
195:             ! Edge vectors225:             ! Edge vectors
196:             AB(:) = TEST_SIMPLEX(:, 3) - TEST_SIMPLEX(:, 4)226:             AB(:) = TEST_SIMPLEX(:, 3) - TEST_SIMPLEX(:, 4)
248:                     ! Just need to decide between the first face and the other278:                     ! Just need to decide between the first face and the other
249:                     ! edge. However, it's simpler (if slightly less efficient)279:                     ! edge. However, it's simpler (if slightly less efficient)
250:                     ! to do the full check for the first face.280:                     ! to do the full check for the first face.
251:                     OVERABC = .FALSE.281:                     OVERABC = .FALSE.
252:                 ENDIF282:                 ENDIF
253:             ENDIF ! Two faces case283:             ENDIF ! Two faces case
254: 284: 
255:             ! Deal with the only one face cases.285:             ! Deal with the only one face cases.
256:             ! Need to check the bordering lines.286:             ! Need to check the bordering lines.
257:             IF (OVERABC .AND. .NOT. OVERACD .AND. .NOT. OVERADB) THEN287:             IF (OVERABC .AND. .NOT. OVERACD .AND. .NOT. OVERADB) THEN
258:                 ! Adjust TEXT_SIMPLEX for the ABC face288:                 ! Perpendiculars to the edges, in plane of this face
259:                 TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)289:                 ABPERP(:) = VEC_CROSS(AB(:), ABCPERP(:))
260:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3)290:                 ACPERP(:) = VEC_CROSS(ABCPERP(:), AC(:))
261:                 TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)291:                 IF (SAME_DIRECTION(ABPERP(:), AO(:), DIM, MYUNIT)) THEN
262:                 ! Check the face and it's edges292:                     ! In the AB line region
263:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &293:                     TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 3)
264:                                 AB, AC, ABCPERP, DIM)294:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
 295:                     LEN_SIMPLEX = 2
 296:                     SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM)
 297:                 ELSE IF (SAME_DIRECTION(ACPERP(:), AO(:), DIM, MYUNIT)) THEN
 298:                     ! In the AC line region
 299:                     TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
 300:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
 301:                     LEN_SIMPLEX = 2
 302:                     SEARCH_DIRECTION(:) = CROSS_121(AC(:), AO(:), DIM)
 303:                 ELSE
 304:                     ! In the ABC region
 305:                     TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
 306:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3)
 307:                     TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)
 308:                     LEN_SIMPLEX = 3
 309:                     SEARCH_DIRECTION(:) = ABCPERP(:)
 310:                 END IF
265:             ELSE IF (OVERACD .AND. .NOT. OVERADB .AND. .NOT. OVERABC) THEN311:             ELSE IF (OVERACD .AND. .NOT. OVERADB .AND. .NOT. OVERABC) THEN
266:                 ! Adjust TEXT_SIMPLEX for the ACD face312:                 ! Perpendiculars to the edges, in plane of this face
267:                 TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)313:                 ACPERP(:) = VEC_CROSS(AC(:), ACDPERP(:))
268:                 ! Check the face and it's edges314:                 ADPERP(:) = VEC_CROSS(ACDPERP(:), AD(:))
269:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &315:                 IF (SAME_DIRECTION(ACPERP(:), AO(:), DIM, MYUNIT)) THEN
270:                                 AC, AD, ACDPERP, DIM)316:                     ! In the AC line region
 317:                     TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
 318:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
 319:                     LEN_SIMPLEX = 2
 320:                     SEARCH_DIRECTION(:) = CROSS_121(AC(:), AO(:), DIM)
 321:                 ELSE IF (SAME_DIRECTION(ADPERP(:), AO(:), DIM, MYUNIT)) THEN
 322:                     ! In the AD line region
 323:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
 324:                     LEN_SIMPLEX = 2
 325:                     SEARCH_DIRECTION(:) = CROSS_121(AD(:), AO(:), DIM)
 326:                 ELSE
 327:                     ! In the ACD region
 328:                     TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)
 329:                     LEN_SIMPLEX = 3
 330:                     SEARCH_DIRECTION(:) = ACDPERP(:)
 331:                 END IF
271:             ELSE IF (OVERADB .AND. .NOT. OVERABC .AND. .NOT. OVERACD) THEN332:             ELSE IF (OVERADB .AND. .NOT. OVERABC .AND. .NOT. OVERACD) THEN
272:                 ! Adjust TEXT_SIMPLEX for the ADB face333:                 ! Perpendiculars to the edges, in plane of this face
273:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 1)334:                 ADPERP(:) = VEC_CROSS(AD(:), ADBPERP(:))
274:                 TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 3)335:                 ABPERP(:) = VEC_CROSS(ADBPERP(:), AB(:))
275:                 TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)336:                 IF (SAME_DIRECTION(ADPERP(:), AO(:), DIM, MYUNIT)) THEN
276:                 ! Check the face and it's edges337:                     ! In the AD line region
277:                 CALL CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, &338:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
278:                                 AD, AB, ADBPERP, DIM)339:                     LEN_SIMPLEX = 2
 340:                     SEARCH_DIRECTION(:) = CROSS_121(AD(:), AO(:), DIM)
 341:                 ELSE IF (SAME_DIRECTION(ABPERP(:), AO(:), DIM, MYUNIT)) THEN
 342:                     ! In the AB line region
 343:                     TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 3)
 344:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
 345:                     LEN_SIMPLEX = 2
 346:                     SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM)
 347:                 ELSE
 348:                     ! In the ADC region
 349:                     TEST_SIMPLEX(:, 3) = TEST_SIMPLEX(:, 4)
 350:                     TEST_SIMPLEX(:, 4) = TEST_SIMPLEX(:, 1)
 351:                     TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2)
 352:                     TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4)
 353:                     LEN_SIMPLEX = 3
 354:                     SEARCH_DIRECTION(:) = ABCPERP(:)
 355:                 END IF
279:             END IF ! One face case356:             END IF ! One face case
280:         ELSE357:         ELSE
281:             ! Simplex is not the right size, something horrible has happened358:             ! Simplex is not the right size, something horrible has happened
282:             WRITE (MYUNIT, *) 'NEAREST_SIMPLEX> ERROR, simplex size is ', &359:             WRITE (MYUNIT, *) 'NEAREST_SIMPLEX> ERROR, simplex size is ', &
283:                 LEN_SIMPLEX, ' This should not happen.'360:                 LEN_SIMPLEX, ' This should not happen.'
284:             STOP361:             STOP
285:         END IF ! Simplex size362:         END IF ! Simplex size
286: 363: 
287:     END SUBROUTINE NEAREST_SIMPLEX364:     END SUBROUTINE NEAREST_SIMPLEX
288: 365: 
289: !-------------------------------------------------------------------------------366: !-------------------------------------------------------------------------------
290: ! Assuming we know a given face or it's edges are the nearest simplex, find 
291: ! which of the two edges or face it is, and generate the result simplex and 
292: ! search direction. 
293: ! TEST_SIMPLEX: the simplex we're checking. The third point is 'A' 
294: ! LEN_SIMPLEX: length of the simplex, 3 at the start 
295: ! SEARCH_DIRECTION: new direction to search next 
296: ! AB, AC: edge vectors 
297: ! ABCPERP: perpendicular to the face 
298: ! DIM: dimension of vectors 
299: !------------------------------------------------------------------------------- 
300:     SUBROUTINE CHECK_FACE(TEST_SIMPLEX, LEN_SIMPLEX, SEARCH_DIRECTION, AB, AC, & 
301:                           ABCPERP, DIM) 
302:         USE VEC3, ONLY: VEC_CROSS 
303:         USE COMMONS, ONLY: MYUNIT 
304:  
305:         IMPLICIT NONE 
306:  
307:         INTEGER, INTENT(IN) :: DIM 
308:         DOUBLE PRECISION, INTENT(INOUT) :: TEST_SIMPLEX(DIM, DIM+1) 
309:         INTEGER, INTENT(INOUT) :: LEN_SIMPLEX 
310:         DOUBLE PRECISION, INTENT(IN) :: AB(DIM), AC(DIM), ABCPERP(DIM) 
311:         DOUBLE PRECISION, INTENT(OUT) :: SEARCH_DIRECTION(DIM) 
312:  
313:         ! ABPERP, ACPERP: vectors perpendicular to the lines in the plane of the 
314:         !                 triangle 
315:         ! AO: edge from A to the origin 
316:         DOUBLE PRECISION :: ABPERP(DIM), ACPERP(DIM), AO(DIM) 
317:  
318:         AO(:) = - TEST_SIMPLEX(:, 3) 
319:         ! Perpendiculars to the edges, in plane of this face 
320:         ABPERP(:) = VEC_CROSS(AB(:), ABCPERP(:)) 
321:         ACPERP(:) = VEC_CROSS(ABCPERP(:), AC(:)) 
322:  
323:         IF (SAME_DIRECTION(ABPERP(:), AO(:), DIM, MYUNIT)) THEN 
324:             ! In the AB line region 
325:             TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2) 
326:             TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3) 
327:             LEN_SIMPLEX = 2 
328:             SEARCH_DIRECTION(:) = CROSS_121(AB(:), AO(:), DIM) 
329:         ELSE IF (SAME_DIRECTION(ACPERP(:), AO(:), DIM, MYUNIT)) THEN 
330:             ! In the AC line region 
331:             TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 3) 
332:             LEN_SIMPLEX = 2 
333:             SEARCH_DIRECTION(:) = CROSS_121(AC(:), AO(:), DIM) 
334:         ELSE 
335:             ! In the ABC region 
336:             ! Make sure we get the right side 
337:             LEN_SIMPLEX = 3 
338:  
339:             IF (SAME_DIRECTION(ABCPERP(:), AO(:), DIM, MYUNIT)) THEN 
340:                 SEARCH_DIRECTION(:) = ABCPERP(:) 
341:             ELSE 
342:                 SEARCH_DIRECTION(:) = -ABCPERP(:) 
343:                 ! Swap the simplex ordering, so the point is above. Makes it 
344:                 ! easier for tetrahedra if this is guaranteed. 
345:                 TEST_SIMPLEX(:, 4) = TEST_SIMPLEX(:, 1) 
346:                 TEST_SIMPLEX(:, 1) = TEST_SIMPLEX(:, 2) 
347:                 TEST_SIMPLEX(:, 2) = TEST_SIMPLEX(:, 4) 
348:             END IF 
349:         END IF 
350:     END SUBROUTINE CHECK_FACE 
351:  
352: !------------------------------------------------------------------------------- 
353: ! Utility function for dot products.367: ! Utility function for dot products.
354: ! All we care about is whether or not the dot is positive or negative368: ! All we care about is whether or not the dot is positive or negative
355: ! Print a warning if it's zero369: ! Print a warning if it's zero
356: ! VEC1, VEC2: the vectors to dot370: ! VEC1, VEC2: the vectors to dot
357: ! DIM: dimension of vectors371: ! DIM: dimension of vectors
358: ! OUT_UNIT: File unit for warnings372: ! OUT_UNIT: File unit for warnings
359: !-------------------------------------------------------------------------------373: !-------------------------------------------------------------------------------
360:     LOGICAL FUNCTION SAME_DIRECTION(VEC1, VEC2, DIM, OUT_UNIT)374:     LOGICAL FUNCTION SAME_DIRECTION(VEC1, VEC2, DIM, OUT_UNIT)
361: 375: 
362:         IMPLICIT NONE376:         IMPLICIT NONE


r31871/gjk_utils.f90 2017-01-31 21:30:08.151254124 +0000 r31870/gjk_utils.f90 2017-01-31 21:30:10.867291037 +0000
 21:  21: 
 22: MODULE GJK_UTILS 22: MODULE GJK_UTILS
 23:  23: 
 24:     ! DIM: Working in 3 dimensions 24:     ! DIM: Working in 3 dimensions
 25:     INTEGER, PARAMETER :: DIM = 3 25:     INTEGER, PARAMETER :: DIM = 3
 26:  26: 
 27:     ! NVERTS: number of vertices 27:     ! NVERTS: number of vertices
 28:     INTEGER :: NVERTS 28:     INTEGER :: NVERTS
 29:  29: 
 30:     ! Stores a single polyhedron 30:     ! Stores a single polyhedron
 31:     TYPE :: POLYHEDRON 31:     TYPE POLYHEDRON
 32:         ! R: Coordinates of the centre of the particle 32:         ! R: Coordinates of the centre of the particle
 33:         ! P: Angle-axis style orientation of the particle 33:         ! P: Angle-axis style orientation of the particle
 34:         ! RM: Rotation matrix, derived from P 34:         ! RM: Rotation matrix, derived from P
 35:         ! RMD: Derivatives of the rotation matrix 35:         ! RMD: Derivatives of the rotation matrix
 36:         DOUBLE PRECISION :: R(DIM), P(DIM), RM(DIM, DIM), RMD(DIM, DIM, DIM) 36:         DOUBLE PRECISION :: R(DIM), P(DIM), RM(DIM, DIM), RMD(DIM, DIM, DIM)
 37:         ! VERTS: Vertex positions 37:         ! VERT: Vertex positions
 38:         DOUBLE PRECISION, ALLOCATABLE, DIMENSION(:,:) :: VERTS 38:         DOUBLE PRECISION, ALLOCATABLE :: VERTS(:,:)
 39:     END TYPE POLYHEDRON 39:     END TYPE POLYHEDRON
 40:  40: 
 41: CONTAINS 41: CONTAINS
 42: !------------------------------------------------------------------------------- 42: !-------------------------------------------------------------------------------
 43: ! Support function for the GJK algorithm. Returns the vertex furthest along 43: ! Support function for the GJK algorithm. Returns the vertex furthest along
 44: ! the direction DIR 44: ! the direction DIR
 45: ! POLY: the polyhedron of interest 45: ! POLY: the polyhedron of interest
 46: ! DIR: the search direction 46: ! DIR: the search direction
 47: !------------------------------------------------------------------------------- 47: !-------------------------------------------------------------------------------
 48:     FUNCTION SUPPORT_FUNC(POLY, DIR) 48:     FUNCTION SUPPORT_FUNC(POLY, DIR)
 51:  51: 
 52:         DOUBLE PRECISION :: SUPPORT_FUNC(DIM) 52:         DOUBLE PRECISION :: SUPPORT_FUNC(DIM)
 53:         DOUBLE PRECISION :: DIR(:) 53:         DOUBLE PRECISION :: DIR(:)
 54:         TYPE(POLYHEDRON), INTENT(IN) :: POLY 54:         TYPE(POLYHEDRON), INTENT(IN) :: POLY
 55:  55: 
 56:         ! BEST_INDEX: index of the best vertex 56:         ! BEST_INDEX: index of the best vertex
 57:         INTEGER :: I, BEST_INDEX 57:         INTEGER :: I, BEST_INDEX
 58:         ! DOT: temporary dot product 58:         ! DOT: temporary dot product
 59:         ! MAX_DOT: largest dot product between DIR and a vertex 59:         ! MAX_DOT: largest dot product between DIR and a vertex
 60:         DOUBLE PRECISION :: DOT, MAX_DOT 60:         DOUBLE PRECISION :: DOT, MAX_DOT
 61:          61: 
 62:         BEST_INDEX = 1 62:         BEST_INDEX = 1
 63:         MAX_DOT = -1.0D-100 63:         MAX_DOT = -1.0D-100
 64:  64: 
 65:         ! Loop over the vertices 65:         ! Loop over the vertices
 66:         DO I = 1, NVERTS 66:         DO I = 1, NVERTS
 67:             DOT = DOT_PRODUCT(POLY%VERTS(:,I), DIR(:)) 67:             DOT = DOT_PRODUCT(POLY%VERTS(:,I), DIR(:))
 68:             IF (DOT .GT. MAX_DOT) THEN 68:             IF (DOT .GT. MAX_DOT) THEN
 69:                 MAX_DOT = DOT 69:                 MAX_DOT = DOT
 70:                 BEST_INDEX = I 70:                 BEST_INDEX = I
 71:             END IF 71:             END IF
 72:         END DO ! Loop over vertices 72:         END DO ! Loop over vertices
 73:  73: 
 74:         SUPPORT_FUNC(:) = POLY%VERTS(:,BEST_INDEX) 74:         SUPPORT_FUNC(:) = POLY%VERTS(:,BEST_INDEX)
 75:     END FUNCTION SUPPORT_FUNC 75:     END FUNCTION SUPPORT_FUNC
  76: 
 76: END MODULE GJK_UTILS 77: END MODULE GJK_UTILS


legend
Lines Added 
Lines changed
 Lines Removed

hdiff - version: 2.1.0