计算二维/三维空间中两条线段的交点

地球屋里老师 2021-08-13 12:08:55

本例可用于求取二维或三维空间中两线段的交点，其算法原理简介如下：

1、先用参数方程表示相应的直线 x(t)=x0+(x1-x0)*t；

2、建立关于未知量t的线性方程组；

3、求系数矩阵及增广矩阵的秩，以此判断解的存在情况；

4、如果t的解存在且在范围[0,1]之内，求出交点坐标。

program test
implicit none
integer, parameter:: DIM = 3
integer stat
real p1(DIM), p2(DIM), p3(DIM), p4(DIM), p(DIM)
p1=[0,0,0]
p2=[1,1,1]
p3=[1,1,0]
p4=[0.0,0.0,1.5]
call intersectionPoint(p1, p2, p3, p4, p, stat)
if(stat==0) print*,p
pause
end program
!****************************************************************************
! 求两条线段的交点，二维：DIM=2，三维：DIM=3
! 输入:
! p1, p2            ... 线段1的两个端点
! p3, p4            ... 线段2的两个端点
! 输出
! p                 ... 交点坐标
! stat              ... 0: 有交点；1: 无交点
! author: LI xingwang, Chang'an university, China. 
!****************************************************************************
subroutine intersectionPoint(p1, p2, p3, p4, p, stat)
implicit none
integer, parameter:: DIM = 3
real, parameter:: EPS = 1.0e-7, ZERO = tiny(0.0)*100.0
real, intent(in):: p1(DIM), p2(DIM), p3(DIM), p4(DIM)
real,intent(out):: p(DIM)
integer,intent(out):: stat
real aMat(DIM,3) !augmented matrix，增广矩阵
real row(3), col(DIM), mVal
real, target:: x(2)
real, pointer:: t1, t2
integer ind(2), iRow, iCol, r1, r2, i

p = -huge(0.0); x = -huge(1.0)
r1 = -999; r2 = -999 !矩阵、增广矩阵的秩
t1 => x(1); t2 => x(2)
aMat(:,1) = p2 - p1
aMat(:,2) = p3 - p4
aMat(:,3) = p3 - p1
ind = maxloc(abs(aMat(:,1:2))) !前两列最大值所在位置
iRow = ind(1); iCol = ind(2)
mVal = aMat(iRow,iCol)

if(abs(mVal)<ZERO) then !为两点
  r1 = 0
  if(maxval(abs(aMat(:,3)))<ZERO) then
    r2 = 0
  else
    r2 = 1
  end if
  goto 100
end if

aMat = aMat / mVal !归一化
!行变换
select case(iRow)
case(2)
  row(:) = aMat(2,:)
  aMat(2,:) = aMat(1,:)
  aMat(1,:) = row(:)
case(3)
  row(:) = aMat(DIM,:)
  aMat(DIM,:) = aMat(1,:)
  aMat(1,:) = row(:)
end select
!消元
do i = 2, DIM
  aMat(i,:) = aMat(i,:) - aMat(i,iCol)*aMat(1,:)
end do
!列变换
if(iCol==2) then
  t1 => x(2); t2 => x(1)
  col = aMat(:,2)
  aMat(:,2) = aMat(:,1)
  aMat(:,1) = col
end if

iRow = 1 + maxloc(abs(aMat(2:,2)),1) !最大值所在位置
mVal = aMat(iRow,2)
!行变换
if(iRow==3) then
  row(:) = aMat(DIM,:)
  aMat(DIM,:) = aMat(2,:)
  aMat(2,:) = row(:)
end if
if(abs(mVal)>ZERO) then
  aMat(2:,:) = aMat(2:,:) / mVal !归一化
  do i = 3, DIM !消元
    aMat(i,:) = aMat(i,:) - aMat(i,2)*aMat(2,:)
  end do
end if

!第三行
if(DIM==3.and.abs(aMat(DIM,3))>ZERO) then
  r1 = 1; r2 = 3
  goto 100
end if
!第二行
if(abs(aMat(2,2))<ZERO) then
  r1 = 1
  if(abs(aMat(2,3))<ZERO) then
    r2 = 1
  else
    r2 = 2
  end if
else
  r1 = 2; r2 = 2
end if

100 stat = 1
!根据秩判断解
if(r2>r1) return !无解
select case(r1)
case(2)
  t2 = aMat(2,3) / aMat(2,2)
  t1 = aMat(1,3) - t2*aMat(1,2)
  if(-EPS<t1.and.t1<1.0+EPS.and.-EPS<t2.and.t2<1.0+EPS) stat = 0
case(1)
  t1 = aMat(1,3)
  if(-EPS<t1.and.t1<1.0+EPS) then
    stat = 0
  else
    t1 = aMat(1,3) - aMat(1,2)
    if(-EPS<t1.and.t1<1.0+EPS) stat = 0
  end if
case(0)
  t1 = 0.0
  stat = 0
end select

if(stat/=0) return
!求交点
if(associated(t1,x(1))) then
  p = p1 + (p2-p1)*t1
else
  p = p3 + (p4-p3)*t1
end if
end subroutine

...全文