Visual Servoing Platform version 3.5.0
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
57static void inter(Byte mask, Index v0, Index v1);
58static 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 */
91static Bound clip; /* surface a decouper */
92static Byte *code; /* tableau de bits */
93static Point4f *point4f; /* tableau de points 4D */
94static Index point4f_nbr; /* nombre de points 4D */
95
96#if clip_opt
97static Index *poly0, *poly1; /* polygones temporaires*/
98#else
99static 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 */
106void 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 */
134void 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 */
158static 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 */
207static 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 */
244Bound *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 */
297static 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 */
389static 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 */
415void 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 */
443Byte 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