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
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
|
/////////////////////////////////////////////////////////////
// //
// Copyright (c) 2007-2017 by The University of Queensland //
// Centre for Geoscience Computing //
// http://earth.uq.edu.au/centre-geoscience-computing //
// //
// Primary Business: Brisbane, Queensland, Australia //
// Licensed under the Open Software License version 3.0 //
// http://www.apache.org/licenses/LICENSE-2.0 //
// //
/////////////////////////////////////////////////////////////
#include "Triangle3D.h"
#include <iostream>
#include <utility>
using std::pair;
using std::make_pair;
/*
modified 3x3 equation system solver - only 2 components needed
*/
pair<double,double> rsolve(Vector3& v1,Vector3& v2,Vector3& v3,Vector3& rhs)
{
double det1,detx,dety;
det1=v1.X()*(v2.Y()*v3.Z()-v3.Y()*v2.Z())+v2.X()*(v3.Y()*v1.Z()-v1.Y()*v3.Z())+v3.X()*(v1.Y()*v2.Z()-v2.Y()*v1.Z());
detx=rhs.X()*(v2.Y()*v3.Z()-v3.Y()*v2.Z())+v2.X()*(v3.Y()*rhs.Z()-rhs.Y()*v3.Z())+v3.X()*(rhs.Y()*v2.Z()-v2.Y()*rhs.Z());
dety=v1.X()*(rhs.Y()*v3.Z()-v3.Y()*rhs.Z())+rhs.X()*(v3.Y()*v1.Z()-v1.Y()*v3.Z())+v3.X()*(v1.Y()*rhs.Z()-rhs.Y()*v1.Z());
double x=detx/det1;
double y=dety/det1;
return make_pair(x,y);
}
Triangle3D::Triangle3D(const Vector3& p1,const Vector3& p2,const Vector3& p3, int tag)
:m_p1(p1),m_p2(p2),m_p3(p3),m_tag(tag)
{}
/*!
check if line between 2 points intersects the triangle
\param p1 point 1
\param p2 point 2
*/
bool Triangle3D::crosses(const Vector3& p1, const Vector3& p2) const
{
bool res=false;
//-- check if points on same side of plane
Vector3 plane_normal=cross((m_p2-m_p1),(m_p3-m_p1));
//std::cout << "normal: " << plane_normal << std::endl;
double d1=plane_normal*(p1-m_p1);
double d2=plane_normal*(p2-m_p1);
//std::cout << "d1,d2 " << d1 << " / " << d2 << std::endl;
if(d1*d2<0.0){ // sign(d1)!=sign(d2) -> different sides
Vector3 v1=m_p2-m_p1;
Vector3 v2=m_p3-m_p1;
Vector3 v3=p2-p1;
Vector3 rhs=p1-m_p1;
pair<double,double> p=rsolve(v1,v2,v3,rhs);
res=((p.first>=0.0) && (p.first<=1.0) &&
(p.second>=0.0) && (p.second<=1.0) &&
(p.first+p.second<=1.0));
}
return res;
}
/*!
get minimum corner of the bounding box
*/
Vector3 Triangle3D::getMinPoint() const
{
double xmin=(m_p1.x() < m_p2.x()) ? m_p1.x() : m_p2.x();
xmin = (xmin < m_p3.x()) ? xmin : m_p3.x();
double ymin=(m_p1.y() < m_p2.y()) ? m_p1.y() : m_p2.y();
ymin = (ymin < m_p3.y()) ? ymin : m_p3.y();
double zmin=(m_p1.z() < m_p2.z()) ? m_p1.z() : m_p2.z();
zmin = (zmin < m_p3.z()) ? zmin : m_p3.z();
return Vector3(xmin,ymin,zmin);
}
/*!
get maximum corner of the bounding box
*/
Vector3 Triangle3D::getMaxPoint() const
{
double xmax=(m_p1.x() > m_p2.x()) ? m_p1.x() : m_p2.x();
xmax = (xmax > m_p3.x()) ? xmax : m_p3.x();
double ymax=(m_p1.y() > m_p2.y()) ? m_p1.y() : m_p2.y();
ymax = (ymax > m_p3.y()) ? ymax : m_p3.y();
double zmax=(m_p1.z() > m_p2.z()) ? m_p1.z() : m_p2.z();
zmax = (zmax > m_p3.z()) ? zmax : m_p3.z();
return Vector3(xmax,ymax,zmax);
}
/*
local helper function to calculate dist between line segment and point
*/
double EdgeSep(const Vector3& p0,const Vector3& p1,const Vector3& p)
{
double sep;
Vector3 v=p1-p0;
Vector3 vu=v.unit();
double d=((p-p0)*vu);
if((d>0.0)&(d<v.norm())){
sep=((p-p0)-d*vu).norm();
}else{
sep=-1;
}
return sep;
}
/*!
Get the distance of a point from the Triangle
\param p the point
*/
double Triangle3D::getDist(const Vector3& p) const
{
double dist=-1.0;
// check if closest point is in triangle
// calc. the closest point in triangle local coords.
Vector3 v1=m_p2-m_p1; // 1-2 edge of the triangle
Vector3 v2=m_p3-m_p1; // 1-3 edge of the triangle
Vector3 normal=cross(v1,v2).unit(); // plane normal
Vector3 rhs=p-m_p1; // vector from point 1 of the triangle to input point
pair<double,double> cp=rsolve(v2,v1,normal,rhs);
if((cp.first>=0.0) && (cp.first<=1.0) &&
(cp.second>=0.0) && (cp.second<=1.0) &&
(cp.first+cp.second<=1.0)){ // point inside
dist=fabs((p-m_p1)*normal);
} else { // need to check distance to edges/corners
double d1=EdgeSep(m_p1,m_p2,p);
double d2=EdgeSep(m_p1,m_p3,p);
double d3=EdgeSep(m_p2,m_p3,p);
// find the minimum separation != -1 (messy)
if(d1>0.0){
if(d2>0.0){
dist=(d1<d2) ? d1 : d2;
if(d3>0.0){
dist=(d3<dist) ? d3 : dist;
}
} else if(d3>0){
dist=(d1<d3) ? d1 : d3;
} else {
dist=d1;
}
} else if (d2>0){
if (d3>0){
dist=(d2<d3) ? d2 : d3;
} else {
dist=d2;
}
} else {
dist=d3;
}
if(dist==-1.0){ // no edge-> get corner dist
d1=(p-m_p1).norm();
d2=(p-m_p2).norm();
d3=(p-m_p3).norm();
dist=(d1<d2) ? d1 : d2;
dist=(dist<d3) ? dist : d3;
}
}
//std::cout << "Triangle dist: [" << m_p1 << " " << m_p2 << " " << m_p3 << " ] " << p << " - " << dist << std::endl;
return dist;
}
|
