-
Notifications
You must be signed in to change notification settings - Fork 16
/
Copy pathintersect_RayTriangle.py
159 lines (135 loc) · 5.41 KB
/
intersect_RayTriangle.py
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
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
"""
Copyright (C) <2010> Autin L. TSRI
This file git_upy/intersect_RayTriangle.py is part of upy.
upy is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
upy is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with upy. If not, see <http://www.gnu.org/licenses/gpl-3.0.html>.
"""
# -*- coding: utf-8 -*-
"""
Created on Tue Nov 1 15:27:38 2011
@author: Michel Sanner - TSRI
"""
SMALL_NUM = 0.00000001 #anything that avoids division overflow
from math import fabs
## intersect_RayTriangle(): intersect a ray with a 3D triangle
## Input: a ray R, and a triangle T
## Returns -1, None = triangle is degenerate (a segment or point)
## 0, None = disjoint (no intersect)
## 1, I = intersect in unique point I
## 2, None = are in the same plane
def intersect_RayTrianglePy( ray, Triangle):
point1 = ray[0]
point2 = ray[1]
# get triangle edge vectors and plane normal
t0 = Triangle[0]
t1 = Triangle[1]
t2 = Triangle[2]
u = [ t1[0]-t0[0], t1[1]-t0[1], t1[2]-t0[2] ]
v = [ t2[0]-t0[0], t2[1]-t0[1], t2[2]-t0[2] ]
# cross product
n = ( u[1]*v[2]-u[2]*v[1], u[2]*v[0]-u[0]*v[2], u[0]*v[1]-u[1]*v[0])
if n[0]*n[0]+n[1]*n[1]+n[2]*n[2]<SMALL_NUM: # triangle is degenerate
return -1,None # do not deal with this case
# ray direction vector
dir = ( point2[0]-point1[0], point2[1]-point1[1], point2[2]-point1[2])
w0 = ( point1[0]-t0[0], point1[1]-t0[1], point1[2]-t0[2] )
a = -n[0]*w0[0] - n[1]*w0[1] - n[2]*w0[2]
b = n[0]*dir[0] + n[1]*dir[1] + n[2]*dir[2]
if fabs(b) < SMALL_NUM: # ray is parallel to triangle plane
if a == 0: # ray lies in triangle plane
return 2,None
else:
return 0 ,None # ray disjoint from plane
# get intersect point of ray with triangle plane
r = a / b
if r < 0.0: # ray goes away from triangle => no intersect
return 0,None
#if r > 1.0: # segment too short => no intersect
# return 0,None
# intersect point of ray and plane
I = (point1[0] + r*dir[0], point1[1] + r*dir[1], point1[2] + r*dir[2] )
# is I inside Triangle?
uu = u[0]*u[0]+u[1]*u[1]+u[2]*u[2]
uv = u[0]*v[0]+u[1]*v[1]+u[2]*v[2]
vv = v[0]*v[0]+v[1]*v[1]+v[2]*v[2]
w = ( I[0] - t0[0], I[1] - t0[1], I[2] - t0[2] )
wu = w[0]*u[0]+w[1]*u[1]+w[2]*u[2]
wv = w[0]*v[0]+w[1]*v[1]+w[2]*v[2]
D = uv * uv - uu * vv
# get and test parametric coords
s = (uv * wv - vv * wu) / D
if s < 0.0 or s > 1.0: # I is outside Triangle
return 0,None
t = (uv * wu - uu * wv) / D
if t < 0.0 or (s + t) > 1.0: # I is outside Triangle
return 0,None
return 1, I # I is in Triangle
try:
from geomutils.geomalgorithms import intersect_RayTriangle
except ImportError:
print ("shapefit.intersect_RayTriangle.py: defaulting to python implementation")
intersect_RayTriangle = intersect_RayTrianglePy
def intersectRayPolyhedron( pol, pt1, pt2):
# compute intersection points between a polyhedron defined by a list
# of triangles (p0, p1, p2) and a ray starting at pt1 and going through pt2
inter = []
interi = []
ray = [list(pt1), list(pt2)]
for ti, t in enumerate(pol):
interPt = intersect_RayTriangle(ray, t)
#if status==1:
inter.append(interPt)
interi.append(ti)
if len(inter)>1: # find closest to pt1
mini=9999999999.0
miniInd = None
for i, p in enumerate(inter):
v = ( p[0]-pt1[0], p[1]-pt1[1], p[2]-pt1[2] )
d = v[0]*v[0]+v[1]*v[1]+v[2]*v[2]
if d < mini:
mini = d
interPt = inter[i]
tind = interi[i]
return tind, interPt
def intersectRayPolyhedronPy( pol, pt1, pt2):
# compute intersection points between a polyhedron defined by a list
# of triangles (p0, p1, p2) and a ray starting at pt1 and going through pt2
inter = []
interi = []
ray = [list(pt1), list(pt2)]
for ti, t in enumerate(pol):
status,interPt = intersect_RayTrianglePy(ray, t)
if status==1:
inter.append(interPt)
interi.append(ti)
if len(inter)>1: # find closest to pt1
mini=9999999999.0
miniInd = None
for i, p in enumerate(inter):
v = ( p[0]-pt1[0], p[1]-pt1[1], p[2]-pt1[2] )
d = v[0]*v[0]+v[1]*v[1]+v[2]*v[2]
if d < mini:
mini = d
interPt = inter[i]
tind = interi[i]
return tind, interPt
def IndexedPolgonsToTriPoints(geom,transform=True):
verts = geom.getVertices()
tri = geom.getFaces()
assert tri.shape[1]==3
triv = []
for t in tri:
triv.append( [verts[i].tolist() for i in t] )
return triv
#from upy.intersect_RayTriangle import intersectRayPolyhedron, IndexedPolgonsToTriPoints
#triP = IndexedPolgonsToTriPoints(geom)
#res = intersectRayPolyhedron( triP, facesCenter, peomCenter)
#