WebSVN – gelsvn – Diff – /branches/SPD/src/Geometry/verification.cpp


#include "verification.h"
 
using namespace CGLA;
using namespace std;
 
namespace Geometry
{
 
float SqrDistance (const Vec3f& rkPoint,const Triangle& rkTri)
{
    Vec3f kDiff = rkTri.get_v0() - rkPoint;
    double fA00 = sqr_length(rkTri.get_edge(0));
    double fA01 = dot(rkTri.get_edge(0),-rkTri.get_edge(2));
    double fA11 = sqr_length(rkTri.get_edge(2));
    double fB0 = dot(kDiff,rkTri.get_edge(0));
    double fB1 = dot(kDiff,-rkTri.get_edge(2));
    double fC = sqr_length(kDiff);
    double fDet = fabs(fA00*fA11-fA01*fA01);
    double fS = fA01*fB1-fA11*fB0;
    double fT = fA01*fB0-fA00*fB1;
    double fSqrDist;
 
    if ( fS + fT <= fDet )
    {
        if ( fS < (double)0.0 )
        {
            if ( fT < (double)0.0 )  // region 4
            {
                if ( fB0 < (double)0.0 )
                {
                    fT = (double)0.0;
                    if ( -fB0 >= fA00 )
                    {
                        fS = (double)1.0;
                        fSqrDist = fA00+((double)2.0)*fB0+fC;
                    }
                    else
                    {
                        fS = -fB0/fA00;
                        fSqrDist = fB0*fS+fC;
                    }
                }
                else
                {
                    fS = (double)0.0;
                    if ( fB1 >= (double)0.0 )
                    {
                        fT = (double)0.0;
                        fSqrDist = fC;
                    }
                    else if ( -fB1 >= fA11 )
                    {
                        fT = (double)1.0;
                        fSqrDist = fA11+((double)2.0)*fB1+fC;
                    }
                    else
                    {
                        fT = -fB1/fA11;
                        fSqrDist = fB1*fT+fC;
                    }
                }
            }
            else  // region 3
            {
                fS = (double)0.0;
                if ( fB1 >= (double)0.0 )
                {
                    fT = (double)0.0;
                    fSqrDist = fC;
                }
                else if ( -fB1 >= fA11 )
                {
                    fT = (double)1.0;
                    fSqrDist = fA11+((double)2.0)*fB1+fC;
                }
                else
                {
                    fT = -fB1/fA11;
                    fSqrDist = fB1*fT+fC;
                }
            }
        }
        else if ( fT < (double)0.0 )  // region 5
        {
            fT = (double)0.0;
            if ( fB0 >= (double)0.0 )
            {
                fS = (double)0.0;
                fSqrDist = fC;
            }
            else if ( -fB0 >= fA00 )
            {
                fS = (double)1.0;
                fSqrDist = fA00+((double)2.0)*fB0+fC;
            }
            else
            {
                fS = -fB0/fA00;
                fSqrDist = fB0*fS+fC;
            }
        }
        else  // region 0
        {
            // minimum at interior point
            double fInvDet = ((double)1.0)/fDet;
            fS *= fInvDet;
            fT *= fInvDet;
            fSqrDist = fS*(fA00*fS+fA01*fT+((double)2.0)*fB0) +
                fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
        }
    }
    else
    {
        double fTmp0, fTmp1, fNumer, fDenom;
 
        if ( fS < (double)0.0 )  // region 2
        {
            fTmp0 = fA01 + fB0;
            fTmp1 = fA11 + fB1;
            if ( fTmp1 > fTmp0 )
            {
                fNumer = fTmp1 - fTmp0;
                fDenom = fA00-2.0f*fA01+fA11;
                if ( fNumer >= fDenom )
                {
                    fS = (double)1.0;
                    fT = (double)0.0;
                    fSqrDist = fA00+((double)2.0)*fB0+fC;
                }
                else
                {
                    fS = fNumer/fDenom;
                    fT = (double)1.0 - fS;
                    fSqrDist = fS*(fA00*fS+fA01*fT+2.0f*fB0) +
                        fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
                }
            }
            else
            {
                fS = (double)0.0;
                if ( fTmp1 <= (double)0.0 )
                {
                    fT = (double)1.0;
                    fSqrDist = fA11+((double)2.0)*fB1+fC;
                }
                else if ( fB1 >= (double)0.0 )
                {
                    fT = (double)0.0;
                    fSqrDist = fC;
                }
                else
                {
                    fT = -fB1/fA11;
                    fSqrDist = fB1*fT+fC;
                }
            }
        }
        else if ( fT < (double)0.0 )  // region 6
        {
            fTmp0 = fA01 + fB1;
            fTmp1 = fA00 + fB0;
            if ( fTmp1 > fTmp0 )
            {
                fNumer = fTmp1 - fTmp0;
                fDenom = fA00-((double)2.0)*fA01+fA11;
                if ( fNumer >= fDenom )
                {
                    fT = (double)1.0;
                    fS = (double)0.0;
                    fSqrDist = fA11+((double)2.0)*fB1+fC;
                }
                else
                {
                    fT = fNumer/fDenom;
                    fS = (double)1.0 - fT;
                    fSqrDist = fS*(fA00*fS+fA01*fT+((double)2.0)*fB0) +
                        fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
                }
            }
            else
            {
                fT = (double)0.0;
                if ( fTmp1 <= (double)0.0 )
                {
                    fS = (double)1.0;
                    fSqrDist = fA00+((double)2.0)*fB0+fC;
                }
                else if ( fB0 >= (double)0.0 )
                {
                    fS = (double)0.0;
                    fSqrDist = fC;
                }
                else
                {
                    fS = -fB0/fA00;
                    fSqrDist = fB0*fS+fC;
                }
            }
        }
        else  // region 1
        {
            fNumer = fA11 + fB1 - fA01 - fB0;
            if ( fNumer <= (double)0.0 )
            {
                fS = (double)0.0;
                fT = (double)1.0;
                fSqrDist = fA11+((double)2.0)*fB1+fC;
            }
            else
            {
                fDenom = fA00-2.0f*fA01+fA11;
                if ( fNumer >= fDenom )
                {
                    fS = (double)1.0;
                    fT = (double)0.0;
                    fSqrDist = fA00+((double)2.0)*fB0+fC;
                }
                else
                {
                    fS = fNumer/fDenom;
                    fT = (double)1.0 - fS;
                    fSqrDist = fS*(fA00*fS+fA01*fT+((double)2.0)*fB0) +
                        fT*(fA01*fS+fA11*fT+((double)2.0)*fB1)+fC;
                }
            }
        }
    }
    return fabs(fSqrDist);
}
 
}
 
Generated by GNU Enscript 1.6.6.
 
 
 

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

Subversion Repositories gelsvn

(root)/branches/SPD/src/Geometry/verification.cpp – Rev 290 → 291