Subversion Repositories gelsvn

Rev

Rev 291 | Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
290 jrf 1 
#include "verification.h"
2 
 
3 
using namespace CGLA;
4 
using namespace std;
5 
 
6 
float SqrDistance (const Vec3f& rkPoint,const Triangle& rkTri)
7 
{
8 
    Vec3f kDiff = rkTri.get_v0() - rkPoint;
9 
    double fA00 = sqr_length(rkTri.get_edge(0));
10 
    double fA01 = dot(rkTri.get_edge(0),-rkTri.get_edge(2));
11 
    double fA11 = sqr_length(rkTri.get_edge(2));
12 
    double fB0 = dot(kDiff,rkTri.get_edge(0));
13 
    double fB1 = dot(kDiff,-rkTri.get_edge(2));
14 
    double fC = sqr_length(kDiff);
15 
    double fDet = fabs(fA00*fA11-fA01*fA01);
16 
    double fS = fA01*fB1-fA11*fB0;
17 
    double fT = fA01*fB0-fA00*fB1;
18 
    double fSqrDist;
19 
 
20 
    if ( fS + fT <= fDet )
21 
    {
22 
        if ( fS < (double)0.0 )
23 
        {
24 
            if ( fT < (double)0.0 )  // region 4
25 
            {
26 
                if ( fB0 < (double)0.0 )
27 
                {
28 
                    fT = (double)0.0;
29 
                    if ( -fB0 >= fA00 )
30 
                    {
31 
                        fS = (double)1.0;
32 
                        fSqrDist = fA00+((double)2.0)*fB0+fC;
33 
                    }
34 
                    else
35 
                    {
36 
                        fS = -fB0/fA00;
37 
                        fSqrDist = fB0*fS+fC;
38 
                    }
39 
                }
40 
                else
41 
                {
42 
                    fS = (double)0.0;
43 
                    if ( fB1 >= (double)0.0 )
44 
                    {
45 
                        fT = (double)0.0;
46 
                        fSqrDist = fC;
47 
                    }
48 
                    else if ( -fB1 >= fA11 )
49 
                    {
50 
                        fT = (double)1.0;
51 
                        fSqrDist = fA11+((double)2.0)*fB1+fC;
52 
                    }
53 
                    else
54 
                    {
55 
                        fT = -fB1/fA11;
56 
                        fSqrDist = fB1*fT+fC;
57 
                    }
58 
                }
59 
            }
60 
            else  // region 3
61 
            {
62 
                fS = (double)0.0;
63 
                if ( fB1 >= (double)0.0 )
64 
                {
65 
                    fT = (double)0.0;
66 
                    fSqrDist = fC;
67 
                }
68 
                else if ( -fB1 >= fA11 )
69 
                {
70 
                    fT = (double)1.0;
71 
                    fSqrDist = fA11+((double)2.0)*fB1+fC;
72 
                }
73 
                else
74 
                {
75 
                    fT = -fB1/fA11;
76 
                    fSqrDist = fB1*fT+fC;
77 
                }
78 
            }
79 
        }
80 
        else if ( fT < (double)0.0 )  // region 5
81 
        {
82 
            fT = (double)0.0;
83 
            if ( fB0 >= (double)0.0 )
84 
            {
85 
                fS = (double)0.0;
86 
                fSqrDist = fC;
87 
            }
88 
            else if ( -fB0 >= fA00 )
89 
            {
90 
                fS = (double)1.0;
91 
                fSqrDist = fA00+((double)2.0)*fB0+fC;
92 
            }
93 
            else
94 
            {
95 
                fS = -fB0/fA00;
96 
                fSqrDist = fB0*fS+fC;
97 
            }
98 
        }
99 
        else  // region 0
100 
        {
101 
            // minimum at interior point
102 
            double fInvDet = ((double)1.0)/fDet;
103 
            fS *= fInvDet;
104 
            fT *= fInvDet;
105 
            fSqrDist = fS*(fA00*fS+fA01*fT+((double)2.0)*fB0) +
106 
                fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
107 
        }
108 
    }
109 
    else
110 
    {
111 
        double fTmp0, fTmp1, fNumer, fDenom;
112 
 
113 
        if ( fS < (double)0.0 )  // region 2
114 
        {
115 
            fTmp0 = fA01 + fB0;
116 
            fTmp1 = fA11 + fB1;
117 
            if ( fTmp1 > fTmp0 )
118 
            {
119 
                fNumer = fTmp1 - fTmp0;
120 
                fDenom = fA00-2.0f*fA01+fA11;
121 
                if ( fNumer >= fDenom )
122 
                {
123 
                    fS = (double)1.0;
124 
                    fT = (double)0.0;
125 
                    fSqrDist = fA00+((double)2.0)*fB0+fC;
126 
                }
127 
                else
128 
                {
129 
                    fS = fNumer/fDenom;
130 
                    fT = (double)1.0 - fS;
131 
                    fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
132 
                        fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
133 
                }
134 
            }
135 
            else
136 
            {
137 
                fS = (double)0.0;
138 
                if ( fTmp1 <= (double)0.0 )
139 
                {
140 
                    fT = (double)1.0;
141 
                    fSqrDist = fA11+((double)2.0)*fB1+fC;
142 
                }
143 
                else if ( fB1 >= (double)0.0 )
144 
                {
145 
                    fT = (double)0.0;
146 
                    fSqrDist = fC;
147 
                }
148 
                else
149 
                {
150 
                    fT = -fB1/fA11;
151 
                    fSqrDist = fB1*fT+fC;
152 
                }
153 
            }
154 
        }
155 
        else if ( fT < (double)0.0 )  // region 6
156 
        {
157 
            fTmp0 = fA01 + fB1;
158 
            fTmp1 = fA00 + fB0;
159 
            if ( fTmp1 > fTmp0 )
160 
            {
161 
                fNumer = fTmp1 - fTmp0;
162 
                fDenom = fA00-((double)2.0)*fA01+fA11;
163 
                if ( fNumer >= fDenom )
164 
                {
165 
                    fT = (double)1.0;
166 
                    fS = (double)0.0;
167 
                    fSqrDist = fA11+((double)2.0)*fB1+fC;
168 
                }
169 
                else
170 
                {
171 
                    fT = fNumer/fDenom;
172 
                    fS = (double)1.0 - fT;
173 
                    fSqrDist = fS*(fA00*fS+fA01*fT+((double)2.0)*fB0) +
174 
                        fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
175 
                }
176 
            }
177 
            else
178 
            {
179 
                fT = (double)0.0;
180 
                if ( fTmp1 <= (double)0.0 )
181 
                {
182 
                    fS = (double)1.0;
183 
                    fSqrDist = fA00+((double)2.0)*fB0+fC;
184 
                }
185 
                else if ( fB0 >= (double)0.0 )
186 
                {
187 
                    fS = (double)0.0;
188 
                    fSqrDist = fC;
189 
                }
190 
                else
191 
                {
192 
                    fS = -fB0/fA00;
193 
                    fSqrDist = fB0*fS+fC;
194 
                }
195 
            }
196 
        }
197 
        else  // region 1
198 
        {
199 
            fNumer = fA11 + fB1 - fA01 - fB0;
200 
            if ( fNumer <= (double)0.0 )
201 
            {
202 
                fS = (double)0.0;
203 
                fT = (double)1.0;
204 
                fSqrDist = fA11+((double)2.0)*fB1+fC;
205 
            }
206 
            else
207 
            {
208 
                fDenom = fA00-2.0f*fA01+fA11;
209 
                if ( fNumer >= fDenom )
210 
                {
211 
                    fS = (double)1.0;
212 
                    fT = (double)0.0;
213 
                    fSqrDist = fA00+((double)2.0)*fB0+fC;
214 
                }
215 
                else
216 
                {
217 
                    fS = fNumer/fDenom;
218 
                    fT = (double)1.0 - fS;
219 
                    fSqrDist = fS*(fA00*fS+fA01*fT+((double)2.0)*fB0) +
220 
                        fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
221 
                }
222 
            }
223 
        }
224 
    }
225 
    return fabs(fSqrDist);
226 
}