Geometry/shape_ray_int_2d.f90 at master · lguez/Geometry · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
subroutine shape_ray_int_2d ( pc, p1, side_num, pa, pb, pint )

!*****************************************************************************80
!
!! SHAPE_RAY_INT_2D: intersection ( regular shape, ray ) in 2D.
!
!  Discussion:
!
!    The "regular shape" is assumed to be an equilateral and equiangular
!    polygon, such as the standard square, pentagon, hexagon, and so on.
!
!    The origin of the ray is assumed to be inside the shape.  This
!    guarantees that the ray will intersect the shape in exactly one point.
!
!  Licensing:
!
!    This code is distributed under the GNU LGPL license.
!
!  Modified:
!
!    29 January 2005
!
!  Author:
!
!    John Burkardt
!
!  Parameters:
!
!    Input, double precision PC(2), the center of the shape.
!
!    Input, double precision P1(2), the first vertex of the shape.
!
!    Input, integer SIDE_NUM, the number of sides.
!
!    Input, double precision PA(2), the origin of the ray.
!
!    Input, double precision PB(2), a second point on the ray.
!
!    Output, double precision PINT(2), the point on the shape intersected
!    by the ray.
!
  implicit none

  integer, parameter :: dim_num = 2

  double precision angle2
  double precision degrees_to_radians
  logical ( kind = 4 ) inside
  integer ival
  double precision p1(dim_num)
  double precision pa(dim_num)
  double precision pb(dim_num)
  double precision pc(dim_num)
  double precision pint(dim_num)
  double precision radius
  double precision sector_angle
  integer sector_index
  integer side_num
  double precision v1(dim_num)
  double precision v2(dim_num)
!
!  Warning!
!  No check is made to ensure that the ray origin is inside the shape.
!  These calculations are not valid if that is not true!
!
!  Determine the angle subtended by a single side.
!
  sector_angle = 360.0D+00 / dble(side_num)
!
!  How long is the half-diagonal?
!
  radius = sqrt ( sum ( ( p1(1:dim_num) - pc(1:dim_num) )**2 ) )
!
!  If the radius is zero, refuse to continue.
!
  if ( radius == 0.0D+00 ) then
    write ( *, '(a)' ) ' '
    write ( *, '(a)' ) 'SHAPE_RAY_INT_2D - Fatal error!'
    write ( *, '(a)' ) '  The shape has radius zero.'
    stop 1
  end if
!
!  Determine which sector side intersects the ray.
!
  v2(1:dim_num) = (/ 0.0D+00, 0.0D+00 /)

  do sector_index = 1, side_num
!
!  Determine the two vertices that define this sector.
!
    if ( sector_index == 1 ) then

      angle2 = dble(sector_index - 1) * sector_angle
      angle2 = degrees_to_radians ( angle2 )

      call vector_rotate_base_2d ( p1, pc, angle2, v1 )

    else

      v1(1:dim_num) = v2(1:dim_num)

    end if

    angle2 = dble(sector_index) * sector_angle
    angle2 = degrees_to_radians ( angle2 )

    call vector_rotate_base_2d ( p1, pc, angle2, v2 )
!
!  Draw the angle from one vertex to the ray origin to the next vertex,
!  and see if that angle contains the ray.  If so, then the ray
!  must intersect the shape side of that sector.
!
    call angle_contains_point_2d ( v1, pa, v2, pb, inside )
!
!  Determine the intersection of the lines defined by the ray and the
!  sector side.  (We're already convinced that the ray and sector line
!  segment intersect, so we can use the simpler code that treats them
!  as full lines).
!
    if ( inside ) then

      call lines_exp_int_2d ( pa, pb, v1, v2, ival, pint )

      return

    end if

  end do
!
!  If the calculation fell through the loop, then something's wrong.
!
  write ( *, '(a)' ) ' '
  write ( *, '(a)' ) 'SHAPE_RAY_INT_2D - Fatal error!'
  write ( *, '(a)' ) '  Cannot find intersection of ray and shape.'
  stop 1
end