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