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