ViSP
vpClipping.cpp
1 /****************************************************************************
2  *
3  * $Id: vpClipping.cpp 5285 2015-02-09 14:32:54Z fspindle $
4  *
5 * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  * Description:
34  * Le module "clipping.c" contient les procedures de decoupage
35  * d'une scene 3D par l'algorithme de Sutherland et Hodgman.
36  * Pour plus de reseignements, voir :
37  * I. Sutherland, E. Hodgman, W. Gary.
38  * "Reentrant Polygon Clipping".
39  * Communications of the ACM,
40  * Junary 1974, Volume 17, Number 1, pp 32-44.
41  *
42  * Authors:
43  * Jean-Luc CORRE
44  *
45  *****************************************************************************/
46 
47 
48 
49 
50 
51 
52 #include <visp/vpConfig.h>
53 
54 #ifndef DOXYGEN_SHOULD_SKIP_THIS
55 #include <visp/vpMy.h>
56 #include <visp/vpArit.h>
57 #include <visp/vpBound.h>
58 #include <visp/vpView.h>
59 #include <stdio.h>
60 #include <stdlib.h>
61 #include <limits>
62 #include <cmath>
63 
64 void open_clipping (void);
65 void close_clipping (void);
66 static Index clipping (Byte mask, Index vni, Index *pi, Index *po);
67 static Index clipping_Face (Face *fi, Face *fo);
68 static Index clipping_Face (Face *fi, Face *fo);
69 static void inter (Byte mask, Index v0, Index v1);
70 static void point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3);
71 void set_Point4f_code (Point4f *p4, int size, Byte *cp);
72 Byte where_is_Point4f (Point4f *p4);
73 
74 /*
75  * Variables utilisees par le decoupage :
76  *
77  * CLIP :
78  * Surface resultat apres le decoupage.
79  * La surface est adaptee pour la reception de tous les types de surfaces.
80  * Sa taille est definie par les macros "..._NBR" de "bound.h".
81  *
82  * FACE_NBR : son nombre de faces
83  * POINT_NBR : son nombre de points
84  * VECTOR_NBR : son monbre de vecteurs
85  * VERTEX_NBR : son nombre de sommets par face.
86  *
87  * La surface recoit une a une les faces decoupees.
88  * La surface recoit en une fois tous les points 3D de la surface decoupee
89  * par rapport aux 6 plans de la pyramide tronquee de vision.
90  *
91  * CODE :
92  * Tableau de booleens durant le decoupage.
93  * Le tableau est initialise par les booleens indiquant le positionnement des
94  * points 4D par rapport aux 6 plans de decoupage.
95  * Le tableau recoit ensuite un a un les booleans des points 4D d'intersection
96  * de la surface avec les 6 plans de decoupage.
97  *
98  * POINT4F :
99  * Tableau de points durant le decoupage.
100  * Le tableau est initialise par les points de la surface en entree apres
101  * transforation en coordonnees homogenes 4D.
102  * Le tableau recoit ensuite un a un les points 4D d'intersection de la surface
103  * avec les 6 plans de la pyramide tronquee de vision.
104  */
105 static Bound clip; /* surface a decouper */
106 static Byte *code; /* tableau de bits */
107 static Point4f *point4f; /* tableau de points 4D */
108 static Index point4f_nbr; /* nombre de points 4D */
109 
110 #if clip_opt
111 static Index *poly0, *poly1; /* polygones temporaires*/
112 #else
113 static Index *poly_tmp; /* polygone temporaire */
114 #endif /* clip_opt */
115 
116 
117 /*
118  * La procedure "open_clipping" alloue et initialise les variables utilisees
119  * par le mode "clipping".
120  */
121 void open_clipping (void)
122 {
123  static char proc_name[] = "open_clipping";
124 
125  /* alloue la surface de travail */
126  malloc_huge_Bound (&clip);
127 
128  /* alloue les tableaux */
129  if ((code = (Byte *) malloc (POINT_NBR * sizeof (Byte))) == NULL
130  || (point4f = (Point4f *) malloc (POINT_NBR * sizeof (Point4f))) == NULL
131 #ifdef clip_opt
132  || (poly0 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL
133  || (poly1 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL) {
134  perror (proc_name);
135  exit (1);
136  }
137 #else
138  || (poly_tmp = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL){
139  perror (proc_name);
140  exit (1);
141  }
142 #endif /* clip_opt */
143 }
144 
145 /*
146  * La procedure "close_clipping" libere les variables utilisees par
147  * le mode "clipping".
148  */
149 void close_clipping (void)
150 {
151  free_huge_Bound (&clip);
152  free ((char *) code);
153  free ((char *) point4f);
154 #ifdef clip_opt
155  free ((char *) poly0);
156  free ((char *) poly1);
157 #else
158  free ((char *) poly_tmp);
159 #endif /* clip_opt */
160 }
161 
162 
163 /*
164  * La procedure "clipping" decoupe un polygone par rapport a un plan
165  * suivant l'algorithme de Sutherland et Hodgman.
166  * Entree :
167  * mask Masque du plan de decoupage pour le code.
168  * vni Nombre de sommets du polygone en entree.
169  * pi Polygone en entree.
170  * po Polygone resultat du decoupage.
171  * Sortie :
172  * Nombre de sommets du polygone resultat "po".
173  */
174 static Index
175 clipping (Byte mask, Index vni, Index *pi, Index *po)
176 {
177  /*
178  * vno Nombre de sommets du polygone "po".
179  * vs Premier sommet de l'arete a decouper.
180  * vp Second sommet de l'arete a decouper.
181  * ins TRUE si le sommet "vs" est interieur, FALSE sinon.
182  * inp TRUE si le sommet "vp" est interieur, FALSE sinon.
183  */
184  Index vno = vni; /* nombre de sommets */
185  Index vs = pi[vni-1]; /* premier sommet */
186  Index vp; /* second sommet */
187  Byte ins = code[vs] & mask; /* code de "vs" */
188  Byte inp; /* code de "vp" */
189 
190  while (vni--) { /* pour tous les sommets */
191  vp = *pi++; /* second sommet */
192  inp = code[vp] & mask; /* code du plan courant */
193 
194  if (ins == IS_INSIDE) {
195  if (inp == IS_INSIDE) { /* arete interieure */
196  *po++ = vp;
197  }
198  else { /* intersection */
199  inter (mask, vs, vp);
200  *po++ = point4f_nbr++;
201  }
202  }
203  else {
204  if (inp == IS_INSIDE) { /* intersection */
205  inter (mask, vs, vp);
206  *po++ = point4f_nbr++;
207  *po++ = vp;
208  vno++;
209  }
210  else { /* arete exterieure */
211  vno--;
212  }
213  }
214  vs = vp;
215  ins = inp;
216  }
217  return (vno);
218 }
219 
220 
221 /*
222  * La procedure "clipping_Face" decoupe une face par rapport aux 6 plans
223  * de decoupage de la pyramide tronquee de vision.
224  * Entree :
225  * fi Face a decouper.
226  * fo Face resultat du decoupage.
227  * Sortie :
228  * Le nombre de sommets de la face resultat.
229  */
230 static Index
231 clipping_Face (Face *fi, Face *fo)
232 {
233  Index *flip = poly_tmp; /* polygone temporaire */
234  Index *flop = fo->vertex.ptr; /* polygone resultat */
235  Index vn = fi->vertex.nbr; /* nombre de sommets */
236 
237  if ((vn = clipping (IS_ABOVE, vn, fi->vertex.ptr, flip)) != 0)
238  if ((vn = clipping (IS_BELOW, vn, flip, flop)) != 0)
239  if ((vn = clipping (IS_RIGHT, vn, flop, flip)) != 0)
240  if ((vn = clipping (IS_LEFT, vn, flip, flop)) != 0)
241  if ((vn = clipping (IS_BACK, vn, flop, flip)) != 0)
242  if ((vn = clipping (IS_FRONT, vn, flip, flop)) != 0) {
243  /* recopie de "fi" dans "fo" */
244  /* fo->vertex.ptr == flop */
245  fo->vertex.nbr = vn;
246  fo->is_polygonal = fi->is_polygonal;
247  fo->is_visible = fi->is_visible;
248 #ifdef face_normal
249  fo->normal = fi->normal;
250 #endif /* face_normal */
251  return (vn);
252  }
253  return (0);
254 }
255 
256 /*
257  * La procedure "clipping_Bound" decoupe une surface par rapport aux 6 plans
258  * de decoupage de la pyramide tronquee de vision.
259  * Les calculs geometriques sont effectues en coordonnees homogenes.
260  * Note : Les points invisibles de la surface "clip" ont une profondeur negative
261  * c'est a dire une coordonnee Z negative.
262  * Entree :
263  * bp Surface a decouper.
264  * m Matrice de projection dans le volume canonique.
265  * Sortie :
266  * Pointeur de la surface resultat "clip" si elle est visible,
267  * NULL sinon.
268  */
269 Bound
270 *clipping_Bound (Bound *bp, Matrix m)
271 {
272  Face *fi = bp->face.ptr; /* 1ere face */
273  Face *fend = fi + bp->face.nbr; /* borne de "fi"*/
274  Face *fo = clip.face.ptr; /* face clippee */
275 
276  /* recopie de "bp" dans les tableaux intermediaires */
277 
278  point4f_nbr = bp->point.nbr;
279  point_3D_4D (bp->point.ptr, (int) point4f_nbr, m, point4f);
280  set_Point4f_code (point4f, (int) point4f_nbr, code);
281 #ifdef face_normal
282  if (! (clip.is_polygonal = bp->is_polygonal))
283  //bcopy (bp->normal.ptr, clip.normal.ptr,
284  // bp->normal.nbr * sizeof (Vector));
285  memmove (clip.normal.ptr, bp->normal.ptr,
286  bp->normal.nbr * sizeof (Vector));
287 #endif /* face_normal */
288  for (; fi < fend; fi++) { /* pour toutes les faces*/
289  if (clipping_Face (fi, fo) != 0) {
290  fo++; /* ajoute la face a "clip" */
291  /*
292  * Construction a la volee du future polygone.
293  * dont l'espace memoire est deja alloue (voir
294  * la procedure "malloc_huge_Bound").
295  */
296  fo->vertex.ptr = (fo-1)->vertex.ptr+(fo-1)->vertex.nbr;
297  }
298  }
299 
300  if (fo == clip.face.ptr)
301  return (NULL); /* Rien a voir, circulez... */
302 
303  /* recopie des tableaux intermediaires dans "clip" */
304 
305  point_4D_3D (point4f, (int) point4f_nbr, code, clip.point.ptr);
306  clip.type = bp->type;
307  clip.face.nbr = (Index)( fo - clip.face.ptr );
308  clip.point.nbr = point4f_nbr;
309 #ifdef face_normal
310  if (! bp->is_polygonal)
311  clip.normal.nbr = point4f_nbr;
312 #endif /* face_normal */
313  return (&clip);
314 }
315 
316 /*
317  * La procedure "inter" calcule le point d'intersection "point4f[point4f_nbr]"
318  * de l'arete (v0,v1) avec le plan "mask".
319  * Entree :
320  * mask Mask du plan de decoupage.
321  * v0 Permier sommet de l'arete.
322  * v1 Second sommet de l'arete.
323  */
324 static void
325 inter (Byte mask, Index v0, Index v1)
326 {
327  Point4f *p = point4f + point4f_nbr;
328  Point4f *p0 = point4f + v0;
329  Point4f *p1 = point4f + v1;
330  float t; /* parametre entre 0 et 1 */
331 
332  /* calcule le point d'intersection */
333 
334  switch (mask) {
335 
336  case IS_ABOVE :
337  /* t = (p0->w - p0->y) / ((p0->w - p0->y) - (p1->w - p1->y)); */
338  t = (p0->w - p0->y) - (p1->w - p1->y);
339  //t = (t == 0) ? (float)1.0 : (p0->w - p0->y) / t;
340  t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->y) / t;
341  PAR_COORD3(*p,t,*p0,*p1);
342  p->w = p->y; /* propriete du point d'intersection */
343  break;
344 
345  case IS_BELOW :
346  /* t = (p0->w + p0->y) / ((p0->w + p0->y) - (p1->w + p1->y)); */
347  t = (p0->w + p0->y) - (p1->w + p1->y);
348  //t = (t == 0) ? (float)1.0 : (p0->w + p0->y) / t;
349  t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->y) / t;
350  PAR_COORD3(*p,t,*p0,*p1);
351  p->w = - p->y; /* propriete du point d'intersection */
352  break;
353 
354  case IS_RIGHT :
355  /* t = (p0->w - p0->x) / ((p0->w - p0->x) - (p1->w - p1->x)); */
356  t = (p0->w - p0->x) - (p1->w - p1->x);
357  //t = (t == 0) ? (float)1.0 : (p0->w - p0->x) / t;
358  t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->x) / t;
359  PAR_COORD3(*p,t,*p0,*p1);
360  p->w = p->x; /* propriete du point d'intersection */
361  break;
362 
363  case IS_LEFT :
364  /* t = (p0->w + p0->x) / ((p0->w + p0->x) - (p1->w + p1->x)); */
365  t = (p0->w + p0->x) - (p1->w + p1->x);
366  //t = (t == 0) ? (float)1.0 : (p0->w + p0->x) / t;
367  t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->x) / t;
368  PAR_COORD3(*p,t,*p0,*p1);
369  p->w = - p->x; /* propriete du point d'intersection */
370  break;
371 
372  case IS_BACK :
373  /* t = (p0->w - p0->z) / ((p0->w - p0->z) - (p1->w - p1->z)); */
374  t = (p0->w - p0->z) - (p1->w - p1->z);
375  //t = (t == 0) ? (float)1.0 : (p0->w - p0->z) / t;
376  t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->z) / t;
377  PAR_COORD3(*p,t,*p0,*p1);
378  p->w = p->z; /* propriete du point d'intersection */
379  break;
380 
381  case IS_FRONT :
382  /* t = p0->z / (p0->z - p1->z); */
383  t = (p0->z - p1->z);
384  //t = (t == 0) ? (float)1.0 : p0->z / t;
385  t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : p0->z / t;
386  p->x = (p1->x - p0->x) * t + p0->x;
387  p->y = (p1->y - p0->y) * t + p0->y;
388  p->w = (p1->w - p0->w) * t + p0->w;
389  p->z = (float)M_EPSILON; /* propriete du point d'intersection */
390  break;
391  }
392  /* resout les problemes d'arrondis pour "where_is_Point4f" */
393  /* p->w += (p->w < 0) ? (- M_EPSILON) : M_EPSILON; */
394  p->w += (float)M_EPSILON;
395  code[point4f_nbr] = where_is_Point4f (p); /* localise "p" */
396 #ifdef face_normal
397  if (! clip.is_polygonal) {
398  Vector *n0 = clip.normal.ptr + v0;
399  Vector *n1 = clip.normal.ptr + v1;
400  Vector *n = clip.normal.ptr + point4f_nbr;
401 
402  SET_COORD3(*n,
403  (n1->x - n0->x) * t + n0->x,
404  (n1->y - n0->y) * t + n0->y,
405  (n1->z - n0->z) * t + n0->z);
406  }
407 #endif /* face_normal */
408 }
409 
410 /*
411  * La procedure "point_4D_3D" transforme les points homogenes 4D visibles
412  * en points 3D par projection.
413  * Note : On marque un point 3D invisible par une profondeur negative.
414  * Entree :
415  * p4 Tableau de points 4D a transformer.
416  * size Taille du tableau "p4".
417  * cp Tableau de code indiquant la visibilite des points 4D.
418  * p3 Tableau de points 3D issus de la transformation.
419  */
420 static void
421 point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3)
422 {
423  Point4f *pend = p4 + size; /* borne de p4 */
424  float w;
425 
426  for (; p4 < pend; p4++, p3++) {
427  if (*cp++ == IS_INSIDE) { /* point visible */
428  w = p4->w;
429 
430  p3->x = p4->x / w; /* projection 4D en 3D */
431  p3->y = p4->y / w;
432  p3->z = p4->z / w;
433  }
434  else { /* marque invisible */
435  p3->z = -1.0;
436  }
437  }
438 }
439 
440 /*
441  * La procedure "set_Point4f_code" initialise la position des points 4D
442  * par rapport a 6 plans de la pyramide tronquee de vision.
443  * A chaque point est associe un code indiquant la position respective du point.
444  * Entree :
445  * p4 Tableau de points 4D a localiser.
446  * size Taille du tableau "p4".
447  * cp Tableau de codes de localisation resultat.
448  */
449 void
450 set_Point4f_code (Point4f *p4, int size, Byte *cp)
451 {
452  Point4f *pend = p4 + size; /* borne de p4 */
453  Byte b; /* code de p4 */
454 
455  for (; p4 < pend; p4++, *cp++ = b) {
456  b = IS_INSIDE;
457  if ( p4->w < p4->y) b |= IS_ABOVE;
458  else if (- p4->w > p4->y) b |= IS_BELOW;
459  if ( p4->w < p4->x) b |= IS_RIGHT;
460  else if (- p4->w > p4->x) b |= IS_LEFT;
461  if ( p4->w < p4->z) b |= IS_BACK;
462  else if ( -0.9 > p4->z) b |= IS_FRONT;
463  }
464 }
465 
466 
467 /*
468  * La procedure "where_is_Point4f" localise un point 4D par rapport aux 6 plans
469  * de decoupage de la pyramide tronquee de vision.
470  * Entree :
471  * p4 Point homogene 4D a localiser.
472  * Sortie :
473  * Code indiquant le position de "p4" par rapport aux 6 plans.
474  */
475 Byte
476 where_is_Point4f (Point4f *p4)
477 {
478  Byte b = IS_INSIDE; /* code de "p4" */
479 
480  if ( p4->w < p4->y) b |= IS_ABOVE;
481  else if (- p4->w > p4->y) b |= IS_BELOW;
482  if ( p4->w < p4->x) b |= IS_RIGHT;
483  else if (- p4->w > p4->x) b |= IS_LEFT;
484  if ( p4->w < p4->z) b |= IS_BACK;
485  else if ( -0.9 > p4->z) b |= IS_FRONT;
486  return (b);
487 }
488 
489 #endif