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