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
|
// Geometric Tools, LLC
// Copyright (c) 1998-2014
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
//
// File Version: 5.0.1 (2010/10/01)
#include "Wm5MathematicsPCH.h"
#include "Wm5DistPoint3Tetrahedron3.h"
#include "Wm5DistPoint3Triangle3.h"
namespace Wm5
{
//----------------------------------------------------------------------------
template <typename Real>
DistPoint3Tetrahedron3<Real>::DistPoint3Tetrahedron3 (
const Vector3<Real>& point, const Tetrahedron3<Real>& tetrahedron)
:
mPoint(&point),
mTetrahedron(&tetrahedron)
{
}
//----------------------------------------------------------------------------
template <typename Real>
const Vector3<Real>& DistPoint3Tetrahedron3<Real>::GetPoint () const
{
return *mPoint;
}
//----------------------------------------------------------------------------
template <typename Real>
const Tetrahedron3<Real>& DistPoint3Tetrahedron3<Real>::GetTetrahedron ()
const
{
return *mTetrahedron;
}
//----------------------------------------------------------------------------
template <typename Real>
Real DistPoint3Tetrahedron3<Real>::Get ()
{
return Math<Real>::Sqrt(GetSquared());
}
//----------------------------------------------------------------------------
template <typename Real>
Real DistPoint3Tetrahedron3<Real>::GetSquared ()
{
// Construct the planes for the faces of the tetrahedron. The normals
// are outer pointing, but specified not to be unit length. We only need
// to know sidedness of the query point, so we will save cycles by not
// computing unit-length normals.
Plane3<Real> planes[4];
mTetrahedron->GetPlanes(planes);
// Determine which faces are visible to the query point. Only these
// need to be processed by point-to-triangle distance queries.
Real minSqrDistance = Math<Real>::MAX_REAL;
Vector3<Real> minTetraClosest = Vector3<Real>::ZERO;
for (int i = 0; i < 4; ++i)
{
if (planes[i].WhichSide(*mPoint) >= 0)
{
int indices[3] = { 0, 0, 0 };
mTetrahedron->GetFaceIndices(i, indices);
Triangle3<Real> triangle(
mTetrahedron->V[indices[0]],
mTetrahedron->V[indices[1]],
mTetrahedron->V[indices[2]]);
DistPoint3Triangle3<Real> query(*mPoint, triangle);
Real sqrDistance = query.GetSquared();
if (sqrDistance < minSqrDistance)
{
minSqrDistance = sqrDistance;
minTetraClosest = query.GetClosestPoint1();
}
}
}
mClosestPoint0 = *mPoint;
if (minSqrDistance != Math<Real>::MAX_REAL)
{
// The query point is outside the "solid" tetrahedron.
mClosestPoint1 = minTetraClosest;
}
else
{
// The query point is inside the "solid" tetrahedron. Report a zero
// distance. The closest points are identical.
minSqrDistance = (Real)0;
mClosestPoint1 = *mPoint;
}
return minSqrDistance;
}
//----------------------------------------------------------------------------
template <typename Real>
Real DistPoint3Tetrahedron3<Real>::Get (Real t,
const Vector3<Real>& velocity0, const Vector3<Real>& velocity1)
{
Vector3<Real> movedPoint = *mPoint + t*velocity0;
Vector3<Real> movedV0 = mTetrahedron->V[0] + t*velocity1;
Vector3<Real> movedV1 = mTetrahedron->V[1] + t*velocity1;
Vector3<Real> movedV2 = mTetrahedron->V[2] + t*velocity1;
Vector3<Real> movedV3 = mTetrahedron->V[3] + t*velocity1;
Tetrahedron3<Real> movedTetra(movedV0, movedV1, movedV2, movedV3);
return DistPoint3Tetrahedron3<Real>(movedPoint, movedTetra).Get();
}
//----------------------------------------------------------------------------
template <typename Real>
Real DistPoint3Tetrahedron3<Real>::GetSquared (Real t,
const Vector3<Real>& velocity0, const Vector3<Real>& velocity1)
{
Vector3<Real> movedPoint = *mPoint + t*velocity0;
Vector3<Real> movedV0 = mTetrahedron->V[0] + t*velocity1;
Vector3<Real> movedV1 = mTetrahedron->V[1] + t*velocity1;
Vector3<Real> movedV2 = mTetrahedron->V[2] + t*velocity1;
Vector3<Real> movedV3 = mTetrahedron->V[3] + t*velocity1;
Tetrahedron3<Real> movedTetra(movedV0, movedV1, movedV2, movedV3);
return DistPoint3Tetrahedron3<Real>(movedPoint, movedTetra).GetSquared();
}
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
// Explicit instantiation.
//----------------------------------------------------------------------------
template WM5_MATHEMATICS_ITEM
class DistPoint3Tetrahedron3<float>;
template WM5_MATHEMATICS_ITEM
class DistPoint3Tetrahedron3<double>;
//----------------------------------------------------------------------------
}
|
