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