49 #include <visp3/core/vpConfig.h>
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>
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);
104 static Point4f *point4f;
105 static Index point4f_nbr;
108 static Index *poly0, *poly1;
110 static Index *poly_tmp;
118 void open_clipping (
void)
120 static char proc_name[] =
"open_clipping";
123 malloc_huge_Bound (&clip);
126 if ((code = (Byte *) malloc (POINT_NBR *
sizeof (Byte))) == NULL
127 || (point4f = (Point4f *) malloc (POINT_NBR *
sizeof (Point4f))) == NULL
129 || (poly0 = (Index *) malloc (VERTEX_NBR *
sizeof (Index))) == NULL
130 || (poly1 = (Index *) malloc (VERTEX_NBR *
sizeof (Index))) == NULL) {
135 || (poly_tmp = (Index *) malloc (VERTEX_NBR *
sizeof (Index))) == NULL){
146 void close_clipping (
void)
148 free_huge_Bound (&clip);
149 free ((
char *) code);
150 free ((
char *) point4f);
152 free ((
char *) poly0);
153 free ((
char *) poly1);
155 free ((
char *) poly_tmp);
172 clipping (Byte mask, Index vni, Index *pi, Index *po)
182 Index vs = pi[vni-1];
184 Byte ins = code[vs] & mask;
189 inp = code[vp] & mask;
191 if (ins == IS_INSIDE) {
192 if (inp == IS_INSIDE) {
196 inter (mask, vs, vp);
197 *po++ = point4f_nbr++;
201 if (inp == IS_INSIDE) {
202 inter (mask, vs, vp);
203 *po++ = point4f_nbr++;
228 clipping_Face (Face *fi, Face *fo)
230 Index *flip = poly_tmp;
231 Index *flop = fo->vertex.ptr;
232 Index vn = fi->vertex.nbr;
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) {
243 fo->is_polygonal = fi->is_polygonal;
244 fo->is_visible = fi->is_visible;
246 fo->normal = fi->normal;
267 *clipping_Bound (Bound *bp, Matrix m)
269 Face *fi = bp->face.ptr;
270 Face *fend = fi + bp->face.nbr;
271 Face *fo = clip.face.ptr;
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);
279 if (! (clip.is_polygonal = bp->is_polygonal))
282 memmove (clip.normal.ptr, bp->normal.ptr,
283 bp->normal.nbr * sizeof (Vector));
285 for (; fi < fend; fi++) {
286 if (clipping_Face (fi, fo) != 0) {
293 fo->vertex.ptr = (fo-1)->vertex.ptr+(fo-1)->vertex.nbr;
297 if (fo == clip.face.ptr)
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;
307 if (! bp->is_polygonal)
308 clip.normal.nbr = point4f_nbr;
322 inter (Byte mask, Index v0, Index v1)
324 Point4f *p = point4f + point4f_nbr;
325 Point4f *p0 = point4f + v0;
326 Point4f *p1 = point4f + v1;
335 t = (p0->w - p0->y) - (p1->w - p1->y);
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);
344 t = (p0->w + p0->y) - (p1->w + p1->y);
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);
353 t = (p0->w - p0->x) - (p1->w - p1->x);
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);
362 t = (p0->w + p0->x) - (p1->w + p1->x);
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);
371 t = (p0->w - p0->z) - (p1->w - p1->z);
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);
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;
391 p->w += (float)M_EPSILON;
392 code[point4f_nbr] = where_is_Point4f (p);
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;
400 (n1->x - n0->x) * t + n0->x,
401 (n1->y - n0->y) * t + n0->y,
402 (n1->z - n0->z) * t + n0->z);
418 point_4D_3D (Point4f *p4,
int size, Byte *cp, Point3f *p3)
420 Point4f *pend = p4 + size;
423 for (; p4 < pend; p4++, p3++) {
424 if (*cp++ == IS_INSIDE) {
447 set_Point4f_code (Point4f *p4,
int size, Byte *cp)
449 Point4f *pend = p4 + size;
452 for (; p4 < pend; p4++, *cp++ = b) {
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;
473 where_is_Point4f (Point4f *p4)
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;