1 | /*
|
---|
2 | * The projection code in this file was originally due to Hugues Hoppe
|
---|
3 | * and was distributed under the following terms:
|
---|
4 | *
|
---|
5 | * Copyright (c) 1992, 1993, 1994, Hugues Hoppe, University of Washington.
|
---|
6 | * Copying, use, and development for non-commercial purposes permitted.
|
---|
7 | * All rights for commercial use reserved.
|
---|
8 | *
|
---|
9 | * Anoop Bhattacharjya <anoop@epal.smos.com> made some small changes.
|
---|
10 | *
|
---|
11 | * I made some small modifications, primarily to fix some C++ const-related
|
---|
12 | * issues. And I added the function __gfx_hoppe_dist() to tie it in with
|
---|
13 | * my GFX library code.
|
---|
14 | *
|
---|
15 | * - Michael Garland
|
---|
16 | *
|
---|
17 | */
|
---|
18 |
|
---|
19 | #include <stdio.h>
|
---|
20 | #include <gfx/std.h>
|
---|
21 | #include <gfx/geom/3D.h>
|
---|
22 |
|
---|
23 | #ifdef DOTP
|
---|
24 | #undef DOTP
|
---|
25 | #endif
|
---|
26 |
|
---|
27 | #define DOTP(a, b) (a[0] * b[0] + a[1] * b[1] + a[2] * b[2])
|
---|
28 |
|
---|
29 | using namespace simplif;
|
---|
30 |
|
---|
31 | static real Distance2(real x[3], real *y)
|
---|
32 | {
|
---|
33 | real a, b, c;
|
---|
34 |
|
---|
35 | a = x[0] - y[0]; b = x[1] - y[1]; c = x[2] - y[2];
|
---|
36 | return (a * a + b * b + c * c);
|
---|
37 | }
|
---|
38 |
|
---|
39 | static void interp(real *proj, const real *p1, const real *p2,
|
---|
40 | const real *p3, const real *bary)
|
---|
41 | {
|
---|
42 | proj[0] = p1[0] * bary[0] + p2[0] * bary[1] + p3[0] * bary[2];
|
---|
43 | proj[1] = p1[1] * bary[0] + p2[1] * bary[1] + p3[1] * bary[2];
|
---|
44 | proj[2] = p1[2] * bary[0] + p2[2] * bary[1] + p3[2] * bary[2];
|
---|
45 | }
|
---|
46 |
|
---|
47 | static void Projecth(const real *v1, const real *v2, const real *v3,real *bary)
|
---|
48 | {
|
---|
49 | int i;
|
---|
50 | real vvi[3],vppi[3];
|
---|
51 | real d12sq, don12,d2, mind2, a;
|
---|
52 | real proj[3];
|
---|
53 | real pf[3][3];
|
---|
54 | real ba[3];
|
---|
55 | real cba[3];
|
---|
56 |
|
---|
57 | mind2 = 1e30;
|
---|
58 | interp(proj,v1,v2,v3,bary);
|
---|
59 | pf[0][0] = v1[0]; pf[0][1] = v1[1]; pf[0][2] = v1[2];
|
---|
60 | pf[1][0] = v2[0]; pf[1][1] = v2[1]; pf[1][2] = v2[2];
|
---|
61 | pf[2][0] = v3[0]; pf[2][1] = v3[1]; pf[2][2] = v3[2];
|
---|
62 |
|
---|
63 | ba[0] = bary[0]; ba[1] = bary[1]; ba[2] = bary[2];
|
---|
64 |
|
---|
65 | for (i = 0; i < 3; i++){
|
---|
66 | if (ba[(i+2)%3] >= 0) continue;
|
---|
67 | /* project proj onto segment pf[(i+0)%3]--pf[(i+1)%3] */
|
---|
68 |
|
---|
69 | vvi[0] = pf[(i+1) % 3][0] - pf[i][0];
|
---|
70 | vvi[1] = pf[(i+1) % 3][1] - pf[i][1];
|
---|
71 | vvi[2] = pf[(i+1) % 3][2] - pf[i][2];
|
---|
72 |
|
---|
73 | vppi[0] = proj[0] - pf[i][0];
|
---|
74 | vppi[1] = proj[1] - pf[i][1];
|
---|
75 | vppi[2] = proj[2] - pf[i][2];
|
---|
76 |
|
---|
77 | d12sq = DOTP(vvi, vvi);
|
---|
78 | don12 = DOTP(vvi, vppi);
|
---|
79 |
|
---|
80 | if (don12<=0) {
|
---|
81 | d2 = Distance2(pf[i], proj);
|
---|
82 | if (d2 >= mind2) continue;
|
---|
83 | mind2=d2; cba[i]=1; cba[(i+1)%3]=0; cba[(i+2)%3]=0;
|
---|
84 | }
|
---|
85 | else {
|
---|
86 | if (don12 >= d12sq) {
|
---|
87 | d2 = Distance2(pf[(i+1)%3], proj);
|
---|
88 | if (d2>=mind2) continue;
|
---|
89 | mind2=d2; cba[i]=0; cba[(i+1)%3]=1; cba[(i+2)%3]=0;
|
---|
90 | }
|
---|
91 | else {
|
---|
92 | a = don12/d12sq;
|
---|
93 | cba[i]=1-a; cba[(i+1)%3]=a; cba[(i+2)%3]=0;
|
---|
94 | break;
|
---|
95 | }
|
---|
96 | }
|
---|
97 | }
|
---|
98 |
|
---|
99 | bary[0] = cba[0]; bary[1] = cba[1]; bary[2] = cba[2];
|
---|
100 | }
|
---|
101 |
|
---|
102 | static void ProjectPtri(const real *point, const real *v1,
|
---|
103 | const real *v2, const real *v3, real *bary)
|
---|
104 | {
|
---|
105 | int i;
|
---|
106 | real localv2[3], localv3[3], vpp1[3];
|
---|
107 | real v22,v33,v23,v2pp1,v3pp1;
|
---|
108 | real a1,a2,a3,denom;
|
---|
109 |
|
---|
110 | for (i = 0; i < 3; i++){
|
---|
111 | localv2[i] = v2[i] - v1[i];
|
---|
112 | localv3[i] = v3[i] - v1[i];
|
---|
113 | vpp1[i] = point[i] - v1[i];
|
---|
114 | }
|
---|
115 |
|
---|
116 | v22 = DOTP(localv2, localv2);
|
---|
117 | v33 = DOTP(localv3, localv3);
|
---|
118 | v23 = DOTP(localv2, localv3);
|
---|
119 | v2pp1 = DOTP(localv2, vpp1);
|
---|
120 | v3pp1 = DOTP(localv3, vpp1);
|
---|
121 |
|
---|
122 | if (!v22) v22=1; /* recover if v2==0 */
|
---|
123 | if (!v33) v33=1; /* recover if v3==0 */
|
---|
124 |
|
---|
125 | denom = ( v33 - v23 * v23 / v22);
|
---|
126 | if (!denom) {
|
---|
127 | a2 = a3 = 1.0/3.0; /* recover if v23*v23==v22*v33 */
|
---|
128 | }
|
---|
129 | else {
|
---|
130 | a3=(v3pp1-v23/v22*v2pp1)/denom;
|
---|
131 | a2=(v2pp1-a3*v23)/v22;
|
---|
132 | }
|
---|
133 | a1 = 1 - a2 - a3;
|
---|
134 |
|
---|
135 | bary[0] = a1; bary[1] = a2; bary[2] = a3;
|
---|
136 |
|
---|
137 | if ((a1 < 0) || (a2 < 0) || (a3 < 0)){
|
---|
138 | Projecth(v1,v2,v3,bary);
|
---|
139 | return;
|
---|
140 | }
|
---|
141 | }
|
---|
142 |
|
---|
143 | real __gfx_hoppe_dist(const Face3& f, const Vec3& v)
|
---|
144 | {
|
---|
145 | Vec3 bary;
|
---|
146 |
|
---|
147 | const Vec3& v0 = f.vertexPos(0);
|
---|
148 | const Vec3& v1 = f.vertexPos(1);
|
---|
149 | const Vec3& v2 = f.vertexPos(2);
|
---|
150 |
|
---|
151 | ProjectPtri(v.raw(), v0.raw(), v1.raw(), v2.raw(), bary.raw());
|
---|
152 |
|
---|
153 | Vec3 p = bary[X]*v0 + bary[Y]*v1 + bary[Z]*v2;
|
---|
154 |
|
---|
155 | Vec3 diff = v - p;
|
---|
156 |
|
---|
157 | return diff*diff;
|
---|
158 | }
|
---|