Fix for NaN's in the Z component of projected points by SilhouetteGeomEngine::Project...
[blender.git] / source / blender / freestyle / intern / view_map / SilhouetteGeomEngine.cpp
1
2 //
3 //  Copyright (C) : Please refer to the COPYRIGHT file distributed 
4 //   with this source distribution. 
5 //
6 //  This program is free software; you can redistribute it and/or
7 //  modify it under the terms of the GNU General Public License
8 //  as published by the Free Software Foundation; either version 2
9 //  of the License, or (at your option) any later version.
10 //
11 //  This program is distributed in the hope that it will be useful,
12 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
13 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 //  GNU General Public License for more details.
15 //
16 //  You should have received a copy of the GNU General Public License
17 //  along with this program; if not, write to the Free Software
18 //  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
19 //
20 ///////////////////////////////////////////////////////////////////////////////
21
22 #include "Silhouette.h"
23 #include "SilhouetteGeomEngine.h"
24 #include "../geometry/GeomUtils.h"
25
26 using namespace std;
27
28 Vec3r SilhouetteGeomEngine::_Viewpoint = Vec3r(0,0,0);
29 real SilhouetteGeomEngine::_translation[3] = {0,0,0};
30 real SilhouetteGeomEngine::_modelViewMatrix[4][4] = {{1,0,0,0},
31                                                      {0,1,0,0},
32                                                      {0,0,1,0},
33                                                      {0,0,0,1}};
34 real SilhouetteGeomEngine::_projectionMatrix[4][4] = {{1,0,0,0},
35                                                      {0,1,0,0},
36                                                      {0,0,1,0},
37                                                      {0,0,0,1}};
38 real SilhouetteGeomEngine::_transform[4][4] = {{1,0,0,0},
39                                                {0,1,0,0},
40                                                {0,0,1,0},
41                                                {0,0,0,1}};
42 int SilhouetteGeomEngine::_viewport[4] = {1,1,1,1};              // the viewport
43 real SilhouetteGeomEngine::_Focal = 0.0;
44   
45 real SilhouetteGeomEngine::_glProjectionMatrix[4][4] = {{1,0,0,0},
46                                                         {0,1,0,0},
47                                                         {0,0,1,0},
48                                                         {0,0,0,1}};
49 real SilhouetteGeomEngine::_glModelViewMatrix[4][4] = {{1,0,0,0},
50                                                        {0,1,0,0},
51                                                        {0,0,1,0},
52                                                        {0,0,0,1}};
53 real SilhouetteGeomEngine::_znear = 0.0;
54 real SilhouetteGeomEngine::_zfar = 100.0;
55 bool SilhouetteGeomEngine::_isOrthographicProjection = false;  
56
57 SilhouetteGeomEngine * SilhouetteGeomEngine::_pInstance = 0;
58
59 void SilhouetteGeomEngine::setTransform(const real iModelViewMatrix[4][4], const real iProjectionMatrix[4][4], const int iViewport[4], real iFocal) 
60 {
61   unsigned int i,j;
62   _translation[0] = iModelViewMatrix[3][0];
63   _translation[1] = iModelViewMatrix[3][1];
64   _translation[2] = iModelViewMatrix[3][2];
65   
66   for(i=0; i<4; i++){
67     for(j=0; j<4; j++)
68     {
69       _modelViewMatrix[i][j] = iModelViewMatrix[j][i];
70       _glModelViewMatrix[i][j] = iModelViewMatrix[i][j];
71     }
72   }
73
74   for(i=0; i<4; i++){
75     for(j=0; j<4; j++)
76     {
77       _projectionMatrix[i][j] = iProjectionMatrix[j][i];
78       _glProjectionMatrix[i][j] = iProjectionMatrix[i][j];
79     }
80   }
81       
82   for(i=0; i<4; i++){
83     for(j=0; j<4; j++)
84     {
85       _transform[i][j] = 0;
86       for(unsigned int k=0; k<4; k++)
87         _transform[i][j] += _projectionMatrix[i][k] * _modelViewMatrix[k][j];
88     }
89   }
90       
91   for(i=0; i<4; i++){
92     _viewport[i] = iViewport[i];
93   }
94   _Focal = iFocal;
95
96         _isOrthographicProjection = (iProjectionMatrix[3][3] != 0.0);
97 }
98
99 void SilhouetteGeomEngine::setFrustum(real iZNear, real iZFar) 
100 {
101   _znear = iZNear;
102   _zfar = iZFar;
103 }
104
105 void SilhouetteGeomEngine::retrieveViewport(int viewport[4]){
106   memcpy(viewport, _viewport, 4*sizeof(int));
107 }
108 //#define HUGE 1e9
109
110 void SilhouetteGeomEngine::ProjectSilhouette(vector<SVertex*>& ioVertices)
111 {
112   Vec3r newPoint;
113   //  real min=HUGE;
114   //  real max=-HUGE;
115   vector<SVertex*>::iterator sv, svend;
116   const real depth = _zfar - _znear;
117   const real fac = (depth < 1e-6) ? 1.0 : 1.0 / depth;
118   
119   for(sv=ioVertices.begin(), svend=ioVertices.end();
120       sv!=svend;
121       sv++)
122     {
123       GeomUtils::fromWorldToImage((*sv)->point3D(), newPoint, _modelViewMatrix, _projectionMatrix, _viewport);
124       newPoint[2] = (-newPoint[2]-_znear) * fac; // normalize Z between 0 and 1
125       (*sv)->setPoint2D(newPoint);  
126       //cerr << (*sv)->point2d().z() << "  ";
127       //      real d=(*sv)->point2d()[2];
128       //      if (d>max) max =d;
129       //      if (d<min) min =d;
130     }
131       //  for(sv=ioVertices.begin(), svend=ioVertices.end();
132       //      sv!=svend;
133       //      sv++)
134       //    {
135       //      Vec3r P((*sv)->point2d());
136       //      (*sv)->setPoint2D(Vec3r(P[0], P[1], 1.0-(P[2]-min)/(max-min)));
137       //      //cerr<<(*sv)->point2d()[2]<<"  ";
138       //    }
139 }
140
141 void SilhouetteGeomEngine::ProjectSilhouette(SVertex* ioVertex)
142 {
143   Vec3r newPoint;
144   //  real min=HUGE;
145   //  real max=-HUGE;
146   vector<SVertex*>::iterator sv, svend;
147   const real depth = _zfar - _znear;
148   const real fac = (depth < 1e-6) ? 1.0 : 1.0 / depth;
149   GeomUtils::fromWorldToImage(ioVertex->point3D(), newPoint, _modelViewMatrix, _projectionMatrix, _viewport);
150   newPoint[2] = (-newPoint[2]-_znear) * fac; // normalize Z between 0 and 1
151   ioVertex->setPoint2D(newPoint);  
152 }
153
154 real SilhouetteGeomEngine::ImageToWorldParameter(FEdge *fe, real t)
155 {
156         if( _isOrthographicProjection )
157                 return t;
158
159         // we need to compute for each parameter t the corresponding 
160         // parameter T which gives the intersection in 3D.
161         real T;
162
163         // suffix w for world, c for camera, r for retina, i for image
164         Vec3r Aw = (fe)->vertexA()->point3D();
165         Vec3r Bw = (fe)->vertexB()->point3D();
166         Vec3r Ac, Bc;
167         GeomUtils::fromWorldToCamera(Aw, Ac, _modelViewMatrix);
168         GeomUtils::fromWorldToCamera(Bw, Bc, _modelViewMatrix);
169         Vec3r ABc = Bc - Ac;
170 #if 0
171         cout << "Ac " << Ac << endl;
172         cout << "Bc " << Bc << endl;
173         cout << "ABc " << ABc << endl;
174 #endif
175         Vec3r Ai = (fe)->vertexA()->point2D();
176         Vec3r Bi = (fe)->vertexB()->point2D();
177         Vec3r Ii = Ai + t * (Bi - Ai); // the intersection point in the 2D image space
178         Vec3r Ir, Ic;
179         GeomUtils::fromImageToRetina(Ii, Ir, _viewport);
180
181         real alpha, beta, denom;
182         real m11 = _projectionMatrix[0][0];
183         real m13 = _projectionMatrix[0][2];
184         real m22 = _projectionMatrix[1][1];
185         real m23 = _projectionMatrix[1][2];
186
187         if (fabs(ABc[0]) > 1e-6) {
188
189                 alpha = ABc[2] / ABc[0];
190                 beta = Ac[2] - alpha * Ac[0];
191                 denom = alpha * (Ir[0] + m13) + m11;
192                 if (fabs(denom) < 1e-6)
193                         goto iter;
194                 Ic[0] = -beta * (Ir[0] + m13) / denom;
195 //              Ic[1] = -(Ir[1] + m23) * (alpha * Ic[0] + beta) / m22;
196 //              Ic[2] = alpha * (Ic[0] - Ac[0]) + Ac[2];
197                 T = (Ic[0] - Ac[0]) / ABc[0];
198
199         } else if (fabs(ABc[1]) > 1e-6) {
200
201                 alpha = ABc[2] / ABc[1];
202                 beta = Ac[2] - alpha * Ac[1];
203                 denom = alpha * (Ir[1] + m23) + m22;
204                 if (fabs(denom) < 1e-6)
205                         goto iter;
206                 Ic[1] = -beta * (Ir[1] + m23) / denom;
207 //              Ic[0] = -(Ir[0] + m13) * (alpha * Ic[1] + beta) / m11;
208 //              Ic[2] = alpha * (Ic[1] - Ac[1]) + Ac[2];
209                 T = (Ic[1] - Ac[1]) / ABc[1];
210
211         } else {
212
213 iter:   bool x_coords, less_than;
214                 if (fabs(Bi[0] - Ai[0]) > 1e-6) {
215                         x_coords = true;
216                         less_than = Ai[0] < Bi[0];
217                 } else {
218                         x_coords = false;
219                         less_than = Ai[1] < Bi[1];
220                 }
221                 Vec3r Pc, Pr, Pi;
222                 real T_sta = 0.0;
223                 real T_end = 1.0;
224                 real delta_x, delta_y, dist, dist_threshold = 1e-6;
225                 int i, max_iters = 100;
226                 for (i = 0; i < max_iters; i++) {
227                         T = T_sta + 0.5 * (T_end - T_sta);
228                         Pc = Ac + T * ABc;
229                         GeomUtils::fromCameraToRetina(Pc, Pr, _projectionMatrix);
230                         GeomUtils::fromRetinaToImage(Pr, Pi, _viewport);
231                         delta_x = Ii[0] - Pi[0];
232                         delta_y = Ii[1] - Pi[1];
233                         dist = sqrt(delta_x * delta_x + delta_y * delta_y);
234                         if (dist < dist_threshold)
235                                 break;
236                         if (x_coords) {
237                                 if (less_than) {
238                                         if (Pi[0] < Ii[0]) { T_sta = T; } else { T_end = T; }
239                                 } else {
240                                         if (Pi[0] > Ii[0]) { T_sta = T; } else { T_end = T; }
241                                 }
242                         } else {
243                                 if (less_than) {
244                                         if (Pi[1] < Ii[1]) { T_sta = T; } else { T_end = T; }
245                                 } else {
246                                         if (Pi[1] > Ii[1]) { T_sta = T; } else { T_end = T; }
247                                 }
248                         }
249                 }
250 #if 0
251                 printf("SilhouetteGeomEngine::ImageToWorldParameter(): #iters = %d, dist = %e\n", i, dist);
252 #endif
253                 if (i == max_iters)
254                         printf("SilhouetteGeomEngine::ImageToWorldParameter(): reached to max_iters (dist = %e)\n", dist);
255         }
256
257         return T;
258 }
259
260 Vec3r SilhouetteGeomEngine::WorldToImage(const Vec3r& M)
261
262 {
263
264   const real depth = _zfar - _znear;
265   const real fac = (depth < 1e-6) ? 1.0 : 1.0 / depth;
266   Vec3r newPoint;
267   GeomUtils::fromWorldToImage(M, newPoint, _transform, _viewport);
268   newPoint[2] = (-newPoint[2]-_znear) * fac; // normalize Z between 0 and 1
269   return newPoint;
270
271 }
272