Subversion Repositories gelsvn

Rev

Details | Last modification | View Log | RSS feed

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