← Back to branch summary

~ubuntu-branches/ubuntu/maverick/freecad/maverick

« back to all changes in this revision

Viewing changes to src/Mod/Mesh/App/WildMagic4/Wm4DistVector3Triangle3.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Teemu Ikonen
  • Date: 2009-07-16 18:37:41 UTC
  • Revision ID: james.westby@ubuntu.com-20090716183741-oww9kcxqrk991i1n
Tags: upstream-0.8.2237
Import upstream version 0.8.2237

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Mod/Mesh/App/WildMagic4/Wm4DistVector3Triangle3.cpp
 
1
// Wild Magic Source Code
 
2
// David Eberly
 
3
// http://www.geometrictools.com
 
4
// Copyright (c) 1998-2007
 
5
//
 
6
// This library is free software; you can redistribute it and/or modify it
 
7
// under the terms of the GNU Lesser General Public License as published by
 
8
// the Free Software Foundation; either version 2.1 of the License, or (at
 
9
// your option) any later version.  The license is available for reading at
 
10
// either of the locations:
 
11
//     http://www.gnu.org/copyleft/lgpl.html
 
12
//     http://www.geometrictools.com/License/WildMagicLicense.pdf
 
13
// The license applies to versions 0 through 4 of Wild Magic.
 
14
//
 
15
// Version: 4.0.0 (2006/06/28)
 
16
 
 
17
#include "Wm4FoundationPCH.h"
 
18
#include "Wm4DistVector3Triangle3.h"
 
19
 
 
20
namespace Wm4
 
21
{
 
22
//----------------------------------------------------------------------------
 
23
template <class Real>
 
24
DistVector3Triangle3<Real>::DistVector3Triangle3 (
 
25
    const Vector3<Real>& rkVector, const Triangle3<Real>& rkTriangle)
 
26
    :
 
27
    m_rkVector(rkVector),
 
28
    m_rkTriangle(rkTriangle)
 
29
{
 
30
}
 
31
//----------------------------------------------------------------------------
 
32
template <class Real>
 
33
const Vector3<Real>& DistVector3Triangle3<Real>::GetVector () const
 
34
{
 
35
    return m_rkVector;
 
36
}
 
37
//----------------------------------------------------------------------------
 
38
template <class Real>
 
39
const Triangle3<Real>& DistVector3Triangle3<Real>::GetTriangle () const
 
40
{
 
41
    return m_rkTriangle;
 
42
}
 
43
//----------------------------------------------------------------------------
 
44
template <class Real>
 
45
Real DistVector3Triangle3<Real>::Get ()
 
46
{
 
47
    Real fSqrDist = GetSquared();
 
48
    return Math<Real>::Sqrt(fSqrDist);
 
49
}
 
50
//----------------------------------------------------------------------------
 
51
template <class Real>
 
52
Real DistVector3Triangle3<Real>::GetSquared ()
 
53
{
 
54
    Vector3<Real> kDiff = m_rkTriangle.V[0] - m_rkVector;
 
55
    Vector3<Real> kEdge0 = m_rkTriangle.V[1] - m_rkTriangle.V[0];
 
56
    Vector3<Real> kEdge1 = m_rkTriangle.V[2] - m_rkTriangle.V[0];
 
57
    Real fA00 = kEdge0.SquaredLength();
 
58
    Real fA01 = kEdge0.Dot(kEdge1);
 
59
    Real fA11 = kEdge1.SquaredLength();
 
60
    Real fB0 = kDiff.Dot(kEdge0);
 
61
    Real fB1 = kDiff.Dot(kEdge1);
 
62
    Real fC = kDiff.SquaredLength();
 
63
    Real fDet = Math<Real>::FAbs(fA00*fA11-fA01*fA01);
 
64
    Real fS = fA01*fB1-fA11*fB0;
 
65
    Real fT = fA01*fB0-fA00*fB1;
 
66
    Real fSqrDistance;
 
67
 
 
68
    if (fS + fT <= fDet)
 
69
    {
 
70
        if (fS < (Real)0.0)
 
71
        {
 
72
            if (fT < (Real)0.0)  // region 4
 
73
            {
 
74
                if (fB0 < (Real)0.0)
 
75
                {
 
76
                    fT = (Real)0.0;
 
77
                    if (-fB0 >= fA00)
 
78
                    {
 
79
                        fS = (Real)1.0;
 
80
                        fSqrDistance = fA00+((Real)2.0)*fB0+fC;
 
81
                    }
 
82
                    else
 
83
                    {
 
84
                        fS = -fB0/fA00;
 
85
                        fSqrDistance = fB0*fS+fC;
 
86
                    }
 
87
                }
 
88
                else
 
89
                {
 
90
                    fS = (Real)0.0;
 
91
                    if (fB1 >= (Real)0.0)
 
92
                    {
 
93
                        fT = (Real)0.0;
 
94
                        fSqrDistance = fC;
 
95
                    }
 
96
                    else if (-fB1 >= fA11)
 
97
                    {
 
98
                        fT = (Real)1.0;
 
99
                        fSqrDistance = fA11+((Real)2.0)*fB1+fC;
 
100
                    }
 
101
                    else
 
102
                    {
 
103
                        fT = -fB1/fA11;
 
104
                        fSqrDistance = fB1*fT+fC;
 
105
                    }
 
106
                }
 
107
            }
 
108
            else  // region 3
 
109
            {
 
110
                fS = (Real)0.0;
 
111
                if (fB1 >= (Real)0.0)
 
112
                {
 
113
                    fT = (Real)0.0;
 
114
                    fSqrDistance = fC;
 
115
                }
 
116
                else if (-fB1 >= fA11)
 
117
                {
 
118
                    fT = (Real)1.0;
 
119
                    fSqrDistance = fA11+((Real)2.0)*fB1+fC;
 
120
                }
 
121
                else
 
122
                {
 
123
                    fT = -fB1/fA11;
 
124
                    fSqrDistance = fB1*fT+fC;
 
125
                }
 
126
            }
 
127
        }
 
128
        else if (fT < (Real)0.0)  // region 5
 
129
        {
 
130
            fT = (Real)0.0;
 
131
            if (fB0 >= (Real)0.0)
 
132
            {
 
133
                fS = (Real)0.0;
 
134
                fSqrDistance = fC;
 
135
            }
 
136
            else if (-fB0 >= fA00)
 
137
            {
 
138
                fS = (Real)1.0;
 
139
                fSqrDistance = fA00+((Real)2.0)*fB0+fC;
 
140
            }
 
141
            else
 
142
            {
 
143
                fS = -fB0/fA00;
 
144
                fSqrDistance = fB0*fS+fC;
 
145
            }
 
146
        }
 
147
        else  // region 0
 
148
        {
 
149
            // minimum at interior point
 
150
            Real fInvDet = ((Real)1.0)/fDet;
 
151
            fS *= fInvDet;
 
152
            fT *= fInvDet;
 
153
            fSqrDistance = fS*(fA00*fS+fA01*fT+((Real)2.0)*fB0) +
 
154
                fT*(fA01*fS+fA11*fT+((Real)2.0)*fB1)+fC;
 
155
        }
 
156
    }
 
157
    else
 
158
    {
 
159
        Real fTmp0, fTmp1, fNumer, fDenom;
 
160
 
 
161
        if (fS < (Real)0.0)  // region 2
 
162
        {
 
163
            fTmp0 = fA01 + fB0;
 
164
            fTmp1 = fA11 + fB1;
 
165
            if (fTmp1 > fTmp0)
 
166
            {
 
167
                fNumer = fTmp1 - fTmp0;
 
168
                fDenom = fA00-2.0f*fA01+fA11;
 
169
                if (fNumer >= fDenom)
 
170
                {
 
171
                    fS = (Real)1.0;
 
172
                    fT = (Real)0.0;
 
173
                    fSqrDistance = fA00+((Real)2.0)*fB0+fC;
 
174
                }
 
175
                else
 
176
                {
 
177
                    fS = fNumer/fDenom;
 
178
                    fT = (Real)1.0 - fS;
 
179
                    fSqrDistance = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
 
180
                        fT*(fA01*fS+fA11*fT+((Real)2.0)*fB1)+fC;
 
181
                }
 
182
            }
 
183
            else
 
184
            {
 
185
                fS = (Real)0.0;
 
186
                if (fTmp1 <= (Real)0.0)
 
187
                {
 
188
                    fT = (Real)1.0;
 
189
                    fSqrDistance = fA11+((Real)2.0)*fB1+fC;
 
190
                }
 
191
                else if (fB1 >= (Real)0.0)
 
192
                {
 
193
                    fT = (Real)0.0;
 
194
                    fSqrDistance = fC;
 
195
                }
 
196
                else
 
197
                {
 
198
                    fT = -fB1/fA11;
 
199
                    fSqrDistance = fB1*fT+fC;
 
200
                }
 
201
            }
 
202
        }
 
203
        else if (fT < (Real)0.0)  // region 6
 
204
        {
 
205
            fTmp0 = fA01 + fB1;
 
206
            fTmp1 = fA00 + fB0;
 
207
            if (fTmp1 > fTmp0)
 
208
            {
 
209
                fNumer = fTmp1 - fTmp0;
 
210
                fDenom = fA00-((Real)2.0)*fA01+fA11;
 
211
                if (fNumer >= fDenom)
 
212
                {
 
213
                    fT = (Real)1.0;
 
214
                    fS = (Real)0.0;
 
215
                    fSqrDistance = fA11+((Real)2.0)*fB1+fC;
 
216
                }
 
217
                else
 
218
                {
 
219
                    fT = fNumer/fDenom;
 
220
                    fS = (Real)1.0 - fT;
 
221
                    fSqrDistance = fS*(fA00*fS+fA01*fT+((Real)2.0)*fB0) +
 
222
                        fT*(fA01*fS+fA11*fT+((Real)2.0)*fB1)+fC;
 
223
                }
 
224
            }
 
225
            else
 
226
            {
 
227
                fT = (Real)0.0;
 
228
                if (fTmp1 <= (Real)0.0)
 
229
                {
 
230
                    fS = (Real)1.0;
 
231
                    fSqrDistance = fA00+((Real)2.0)*fB0+fC;
 
232
                }
 
233
                else if (fB0 >= (Real)0.0)
 
234
                {
 
235
                    fS = (Real)0.0;
 
236
                    fSqrDistance = fC;
 
237
                }
 
238
                else
 
239
                {
 
240
                    fS = -fB0/fA00;
 
241
                    fSqrDistance = fB0*fS+fC;
 
242
                }
 
243
            }
 
244
        }
 
245
        else  // region 1
 
246
        {
 
247
            fNumer = fA11 + fB1 - fA01 - fB0;
 
248
            if (fNumer <= (Real)0.0)
 
249
            {
 
250
                fS = (Real)0.0;
 
251
                fT = (Real)1.0;
 
252
                fSqrDistance = fA11+((Real)2.0)*fB1+fC;
 
253
            }
 
254
            else
 
255
            {
 
256
                fDenom = fA00-2.0f*fA01+fA11;
 
257
                if (fNumer >= fDenom)
 
258
                {
 
259
                    fS = (Real)1.0;
 
260
                    fT = (Real)0.0;
 
261
                    fSqrDistance = fA00+((Real)2.0)*fB0+fC;
 
262
                }
 
263
                else
 
264
                {
 
265
                    fS = fNumer/fDenom;
 
266
                    fT = (Real)1.0 - fS;
 
267
                    fSqrDistance = fS*(fA00*fS+fA01*fT+((Real)2.0)*fB0) +
 
268
                        fT*(fA01*fS+fA11*fT+((Real)2.0)*fB1)+fC;
 
269
                }
 
270
            }
 
271
        }
 
272
    }
 
273
 
 
274
    // account for numerical round-off error
 
275
    if (fSqrDistance < (Real)0.0)
 
276
    {
 
277
        fSqrDistance = (Real)0.0;
 
278
    }
 
279
 
 
280
    m_kClosestPoint0 = m_rkVector;
 
281
    m_kClosestPoint1 = m_rkTriangle.V[0] + fS*kEdge0 + fT*kEdge1;
 
282
    m_afTriangleBary[1] = fS;
 
283
    m_afTriangleBary[2] = fT;
 
284
    m_afTriangleBary[0] = (Real)1.0 - fS - fT;
 
285
    return fSqrDistance;
 
286
}
 
287
//----------------------------------------------------------------------------
 
288
template <class Real>
 
289
Real DistVector3Triangle3<Real>::Get (Real fT,
 
290
    const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
 
291
{
 
292
    Vector3<Real> kMVector = m_rkVector + fT*rkVelocity0;
 
293
    Vector3<Real> kMV0 = m_rkTriangle.V[0] + fT*rkVelocity1;
 
294
    Vector3<Real> kMV1 = m_rkTriangle.V[1] + fT*rkVelocity1;
 
295
    Vector3<Real> kMV2 = m_rkTriangle.V[2] + fT*rkVelocity1;
 
296
    Triangle3<Real> kMTriangle(kMV0,kMV1,kMV2);
 
297
    return DistVector3Triangle3<Real>(kMVector,kMTriangle).Get();
 
298
}
 
299
//----------------------------------------------------------------------------
 
300
template <class Real>
 
301
Real DistVector3Triangle3<Real>::GetSquared (Real fT,
 
302
    const Vector3<Real>& rkVelocity0, const Vector3<Real>& rkVelocity1)
 
303
{
 
304
    Vector3<Real> kMVector = m_rkVector + fT*rkVelocity0;
 
305
    Vector3<Real> kMV0 = m_rkTriangle.V[0] + fT*rkVelocity1;
 
306
    Vector3<Real> kMV1 = m_rkTriangle.V[1] + fT*rkVelocity1;
 
307
    Vector3<Real> kMV2 = m_rkTriangle.V[2] + fT*rkVelocity1;
 
308
    Triangle3<Real> kMTriangle(kMV0,kMV1,kMV2);
 
309
    return DistVector3Triangle3<Real>(kMVector,kMTriangle).GetSquared();
 
310
}
 
311
//----------------------------------------------------------------------------
 
312
template <class Real>
 
313
Real DistVector3Triangle3<Real>::GetTriangleBary (int i) const
 
314
{
 
315
    assert(0 <= i && i < 3);
 
316
    return m_afTriangleBary[i];
 
317
}
 
318
//----------------------------------------------------------------------------
 
319
 
 
320
//----------------------------------------------------------------------------
 
321
// explicit instantiation
 
322
//----------------------------------------------------------------------------
 
323
template WM4_FOUNDATION_ITEM
 
324
class DistVector3Triangle3<float>;
 
325
 
 
326
template WM4_FOUNDATION_ITEM
 
327
class DistVector3Triangle3<double>;
 
328
//----------------------------------------------------------------------------
 
329
}