commit 8b2755579987c610bc794eeff6eb74ba03048deb
parent aa367ce9d1e2cf578a66d1afd899b8b58ccd7702
Author: Christophe Coustet <christophe.coustet@meso-star.com>
Date: Mon, 20 Feb 2023 16:55:50 +0100
Add intersection detection on polygon components
At this stage only report segment intersections and colinear segments
Diffstat:
5 files changed, 1124 insertions(+), 0 deletions(-)
diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt
@@ -53,6 +53,7 @@ set(VERSION ${VERSION_MAJOR}.${VERSION_MINOR}.${VERSION_PATCH})
set(SCPR_FILES_SRC
scpr_device.c
+ scpr_intersector.c
scpr_mesh.c
scpr_polygon.c)
set(SCPR_FILES_INC_API scpr.h)
@@ -97,6 +98,7 @@ if(NOT NO_TEST)
endfunction()
new_test(test_scpr_clip)
new_test(test_scpr_device)
+ new_test(test_scpr_intersector)
new_test(test_scpr_offset)
new_test(test_scpr_mesh)
new_test(test_scpr_polygon)
diff --git a/src/scpr.h b/src/scpr.h
@@ -60,6 +60,7 @@ struct mem_allocator;
struct scpr_device;
struct scpr_polygon;
struct scpr_mesh;
+struct scpr_intersector;
/* Input arguments of the scpr_device_create function */
struct scpr_device_create_args {
@@ -74,6 +75,20 @@ struct scpr_device_create_args {
static const struct scpr_device_create_args SCPR_DEVICE_CREATE_ARGS_DEFAULT =
SCPR_DEVICE_CREATE_ARGS_DEFAULT__;
+/* A struct holding callbacks that are called when carying an intersection check
+ * on a bunch of segments. */
+struct scpr_intersector_check_callbacks {
+ void(*simple_intersection)
+ (struct scpr_polygon* polygon1, size_t component1, size_t segment1,
+ struct scpr_polygon* polygon2, size_t component2, size_t segment2,
+ void* data);
+ void(*colinear_segments)
+ (struct scpr_polygon* polygon1, size_t component1, size_t segment1,
+ struct scpr_polygon* polygon2, size_t component2, size_t segment2,
+ void* data);
+};
+#define SCPR_INTERSECTOR_CHECK_CALLBACKS_NULL__ { NULL, NULL }
+
BEGIN_DECLS
/*******************************************************************************
@@ -267,6 +282,36 @@ scpr_mesh_get_position
const size_t ivert,
double position[2]);
+/*******************************************************************************
+ * An intersector provide a way to check for polygon intersections, either
+ * self intersections or intersections between polygons.
+ ******************************************************************************/
+SCPR_API res_T
+scpr_intersector_create
+ (struct scpr_device* dev,
+ struct scpr_intersector** intersector);
+
+SCPR_API res_T
+scpr_intersector_ref_get
+ (struct scpr_intersector* intersector);
+
+SCPR_API res_T
+scpr_intersector_ref_put
+ (struct scpr_intersector* intersector);
+
+/* Register a polygon's component for further analysis */
+SCPR_API res_T
+scpr_intersector_register_component
+ (struct scpr_intersector* intersector,
+ struct scpr_polygon* polygon,
+ const size_t icomponent);
+
+SCPR_API res_T
+scpr_intersector_check
+ (struct scpr_intersector* intersector,
+ struct scpr_intersector_check_callbacks* callbacks,
+ void* data);
+
END_DECLS
#endif /* SCPR_H */
diff --git a/src/scpr_c.h b/src/scpr_c.h
@@ -18,6 +18,7 @@
#include "scpr.h"
+#include <cstddef>
#include <rsys/rsys.h>
#include <rsys/ref_count.h>
#include <rsys/double2.h>
@@ -90,4 +91,179 @@ struct scpr_mesh {
struct scpr_device* device;
};
+struct intersector_segment {
+ scpr_polygon* polygon;
+ size_t component, rank;
+};
+#define DARRAY_NAME segments
+#define DARRAY_DATA struct intersector_segment
+#include <rsys/dynamic_array.h>
+
+/* A segment htable links a count (1 or 2) to a segment index in the segments
+ * darray.
+ * The count is the number of ends of the segment at some range_point. */
+#define HTABLE_NAME segment_idx
+#define HTABLE_KEY size_t
+#define HTABLE_DATA char
+#include <rsys/hash_table.h>
+
+/* Each range_point stores the segments that have 1 or 2 ends there */
+struct intersector_range_point {
+ struct htable_segment_idx segments;
+ int64_t coord;
+};
+static FINLINE void
+range_point_init
+ (struct mem_allocator* alloc, struct intersector_range_point* data)
+{
+ data->coord = INT64_MIN;
+ htable_segment_idx_init(alloc, &data->segments);
+}
+static FINLINE void
+range_point_release(struct intersector_range_point* data)
+{
+ htable_segment_idx_release(&data->segments);
+}
+static FINLINE res_T
+range_point_copy
+ (struct intersector_range_point* dst, struct intersector_range_point const* src)
+{
+ dst->coord = src->coord;
+ return htable_segment_idx_copy(&dst->segments, &src->segments);
+}
+static FINLINE res_T
+range_point_copy_and_release
+ (struct intersector_range_point* dst, struct intersector_range_point* src)
+{
+ dst->coord = src->coord;
+ return htable_segment_idx_copy_and_release(&dst->segments, &src->segments);
+}
+/* A darray of range_point. */
+#define DARRAY_NAME range_points
+#define DARRAY_DATA struct intersector_range_point
+#define DARRAY_FUNCTOR_INIT range_point_init
+#define DARRAY_FUNCTOR_COPY range_point_copy
+#define DARRAY_FUNCTOR_RELEASE range_point_release
+#define DARRAY_FUNCTOR_COPY_AND_RELEASE range_point_copy_and_release
+#include <rsys/dynamic_array.h>
+
+/* An htable to link a pointer to a range_point to an 1D coordinate */
+#define HTABLE_NAME range_point_idx
+#define HTABLE_KEY int64_t
+#define HTABLE_DATA size_t
+#include <rsys/hash_table.h>
+
+enum segment_interaction {
+ NO_INTERACTION = 0,
+ OVERLAP_X = BIT(0),
+ CONTACT_X = BIT(1),
+ OVERLAP_Y = BIT(2),
+ CONTACT_Y = BIT(3),
+ SOMETHING_X = OVERLAP_X | CONTACT_X,
+ SOMETHING_Y = OVERLAP_Y | CONTACT_Y,
+ SOME_OVERLAP = OVERLAP_X | OVERLAP_Y
+};
+/* A dimension is a structure containing all the points where a segment starts
+ * or ends, each one storing a link to all the segments involved there. */
+struct intersector_dimension {
+ /* Unsorted 1D points registered against this dimension */
+ struct htable_range_point_idx unsorted;
+ /* Same points, but sorted by increasing coordinates along this dimension */
+ struct darray_range_points sorted;
+ /* Range on this dimension */
+ int64_t range[2];
+ /* Flags about segments interaction for this dimension */
+ enum segment_interaction overlap, contact;
+};
+
+/* An htable to store overlapping information */
+struct intersector_segment_pair {
+ size_t rank1, rank2;
+};
+static INLINE char
+pair_eq
+ (struct intersector_segment_pair const* a,
+ struct intersector_segment_pair const* b)
+{
+ ASSERT(a && b);
+ return a->rank1 == b->rank1 && a->rank2 == b->rank2;
+}
+#define HTABLE_NAME interacting_segments
+#define HTABLE_KEY struct intersector_segment_pair
+#define HTABLE_DATA unsigned char
+#define HTABLE_KEY_FUNCTOR_EQ pair_eq
+#include <rsys/hash_table.h>
+
+/* An htable to keep track of registered components */
+struct intersector_component {
+ scpr_polygon* polygon;
+ size_t component;
+};
+static FINLINE void
+component_init(struct mem_allocator* alloc, struct intersector_component* key)
+{
+ ASSERT(alloc && key);
+ (void)alloc;
+ key->polygon = NULL;
+}
+static INLINE char
+component_eq
+ (struct intersector_component const* a,
+ struct intersector_component const* b)
+{
+ ASSERT(a && b);
+ return a->polygon == b->polygon && a->component == b->component;
+}
+static FINLINE void
+component_release(struct intersector_component* key)
+{
+ ASSERT(key);
+ if(key->polygon) SCPR(polygon_ref_put(key->polygon));
+}
+static FINLINE res_T
+component_copy
+ (struct intersector_component* dst, struct intersector_component const* src)
+{
+ ASSERT(dst && src);
+ dst->polygon = src->polygon;
+ dst->component = src->component;
+ if(dst->polygon) SCPR(polygon_ref_get(dst->polygon));
+ return RES_OK;
+}
+static FINLINE res_T
+component_copy_and_release
+ (struct intersector_component* dst, struct intersector_component* src)
+{
+ ASSERT(dst && src);
+ dst->polygon = src->polygon;
+ dst->component = src->component;
+ return RES_OK;
+}
+#define HTABLE_NAME registered_components
+#define HTABLE_KEY struct intersector_component
+#define HTABLE_DATA char
+#define HTABLE_KEY_FUNCTOR_INIT component_init
+#define HTABLE_KEY_FUNCTOR_EQ component_eq
+#define HTABLE_KEY_FUNCTOR_COPY component_copy
+#define HTABLE_KEY_FUNCTOR_RELEASE component_release
+#define HTABLE_KEY_FUNCTOR_COPY_AND_RELEASE component_copy_and_release
+#include <rsys/hash_table.h>
+
+/* A set of segments from different polygons spatially sorted to help find
+ * (self-)intersections. */
+struct scpr_intersector {
+ struct scpr_device* device;
+ /* All the registered segments */
+ struct darray_segments segments;
+ /* Spatial partition of the segments along the 2 dimensions */
+ struct intersector_dimension dimensions[2];
+ /* All the pairs of segment that are in interaction in some way or another.
+ * They are all the candidates for actual intersection plus some others that
+ * are just close, but are without interaction. */
+ struct htable_interacting_segments interacting_segments;
+ /* All the registererd components */
+ struct htable_registered_components registererd_components;
+ ref_T ref;
+};
+
#endif
diff --git a/src/scpr_intersector.c b/src/scpr_intersector.c
@@ -0,0 +1,708 @@
+/* Copyright (C) 2016-2018, 2021 |Meso|Star> (contact@meso-star.com)
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>. */
+
+#include "scpr.h"
+#include "scpr_c.h"
+
+#include <polygon.h>
+#include <rsys/logger.h>
+#include <rsys/ref_count.h>
+#include <rsys/mem_allocator.h>
+#include <rsys/rsys.h>
+#include <rsys/double2.h>
+#include <rsys/double3.h>
+
+#undef PI
+#include <clipper2/clipper.h>
+
+#include <new>
+#include <stdlib.h>
+#include <math.h>
+
+/*******************************************************************************
+ * Helper functions
+ ******************************************************************************/
+static void
+init_dimension
+ (struct mem_allocator* allocator,
+ struct intersector_dimension* dim,
+ enum segment_interaction overlap,
+ enum segment_interaction share)
+{
+ ASSERT(dim && allocator);
+ htable_range_point_idx_init(allocator, &dim->unsorted);
+ darray_range_points_init(allocator, &dim->sorted);
+ dim->range[0] = INT64_MAX;
+ dim->range[1] = INT64_MIN;
+ dim->overlap= overlap;
+ dim->contact = share;
+}
+
+static void
+release_dimension
+ (struct intersector_dimension* dim)
+{
+ ASSERT(dim);
+ htable_range_point_idx_release(&dim->unsorted);
+ darray_range_points_release(&dim->sorted);
+}
+
+static void
+intersector_release(ref_T* ref)
+{
+ struct scpr_intersector* intersector;
+ struct mem_allocator* allocator;
+ ASSERT(ref);
+ intersector = CONTAINER_OF(ref, struct scpr_intersector, ref);
+ allocator = intersector->device->allocator;
+ darray_segments_release(&intersector->segments);
+ release_dimension(intersector->dimensions);
+ release_dimension(intersector->dimensions + 1);
+ SCPR(device_ref_put(intersector->device));
+ htable_interacting_segments_release(&intersector->interacting_segments);
+ htable_registered_components_release(&intersector->registererd_components);
+ MEM_RM(allocator, intersector);
+}
+
+/* Register a point (a segment end) against a dimension */
+static res_T
+register_point
+ (struct scpr_intersector* intersector,
+ const int dim_idx,
+ const int64_t coord,
+ const size_t seg_idx)
+{
+ size_t* pidx;
+ size_t rp_idx;
+ struct intersector_dimension* dim;
+ struct intersector_range_point* sorted;
+ res_T res = RES_OK;
+ ASSERT(intersector);
+
+ dim = intersector->dimensions + dim_idx;
+ pidx = htable_range_point_idx_find(&dim->unsorted, &coord);
+ if(!pidx) {
+ /* First time this point: create it in sorted and store its index in unsorted */
+ char one = 1;
+ size_t count = darray_range_points_size_get(&dim->sorted);
+ ERR(darray_range_points_resize(&dim->sorted, 1+count));
+ sorted = darray_range_points_data_get(&dim->sorted);
+ rp_idx = count;
+ sorted[rp_idx].coord = coord;
+ /* Register this segment at this range point */
+ ERR(htable_segment_idx_set(&sorted[rp_idx].segments, &seg_idx, &one));
+ /* Update range */
+ if(dim->range[0] > coord) dim->range[0] = coord;
+ if(dim->range[1] < coord) dim->range[1] = coord;
+ } else {
+ /* This point is already known: look for seg_idx already there */
+ char *found, one_or_two;
+ rp_idx = *pidx;
+ sorted = darray_range_points_data_get(&dim->sorted);
+ found = htable_segment_idx_find(&sorted[rp_idx].segments, &seg_idx);
+ one_or_two = found ? 2 : 1;
+ ERR(htable_segment_idx_set(&sorted[rp_idx].segments, &seg_idx, &one_or_two));
+ }
+ ERR(htable_range_point_idx_set(&dim->unsorted, &coord, &rp_idx));
+
+exit:
+ return res;
+error:
+ goto exit;
+}
+
+/* Compare 2 range_points */
+static int
+compare_range_points(const void* a, const void* b)
+{
+ struct intersector_range_point *pa = (struct intersector_range_point*)a;
+ struct intersector_range_point *pb = (struct intersector_range_point*)b;
+ ASSERT(a && b && pa->coord != pb->coord);
+ return (int)(pa->coord - pb->coord);
+}
+
+/* Sort the range points by increasing coordinates */
+static void
+sort_points
+ (struct intersector_dimension* dim)
+{
+ size_t count;
+ struct intersector_range_point* sorted;
+ ASSERT(dim);
+
+ /* All the range points are already in sorted; just need to sort them */
+ count = darray_range_points_size_get(&dim->sorted);
+ ASSERT(count == htable_range_point_idx_size_get(&dim->unsorted));
+ sorted = darray_range_points_data_get(&dim->sorted);
+ qsort(sorted, count, sizeof(*sorted), compare_range_points);
+}
+
+/* Create a segment pair suitable for use as an htable key
+ * To ensure unicity, pairs are ordered */
+static void
+init_segment_pair
+ (struct intersector_segment_pair* pair,
+ const size_t seg_idx1,
+ const size_t seg_idx2)
+{
+ ASSERT(pair);
+ pair->rank1 = seg_idx1;
+ pair->rank2 = seg_idx2;
+ if(pair->rank1 > pair->rank2) {
+ SWAP(ptrdiff_t, pair->rank1, pair->rank2);
+ }
+}
+
+static int
+segments_bbox_interacts_1_core
+ (int64_t lower1, int64_t upper1, int64_t lower2, int64_t upper2)
+{
+ if(lower1 > upper2) return 0;
+ if(lower2 > upper1) return 0;
+ return 1;
+}
+
+static int
+segments_bbox_interacts_1
+ (size_t seg_idx1,
+ size_t seg_idx2,
+ enum segment_interaction inter,
+ struct scpr_intersector* intersector)
+{
+ const struct intersector_segment *segments, *seg1, *seg2;
+ Clipper2Lib::Point64 *v11, * v12, *v21, *v22;
+ int64_t lower1, upper1, lower2, upper2;
+ size_t count, r11, r12, r21, r22;
+
+ segments = darray_segments_cdata_get(&intersector->segments);
+ seg1 = segments + seg_idx1;
+ seg2 = segments + seg_idx2;
+
+ /* Get vertices */
+ r11 = seg1->rank;
+ SCPR(polygon_get_vertices_count(seg1->polygon, seg1->component, &count));
+ r12 = (r11 + 1) % count;
+ r21 = seg2->rank;
+ SCPR(polygon_get_vertices_count(seg2->polygon, seg2->component, &count));
+ r22 = (r21 + 1) % count;
+ v11 = &seg1->polygon->paths[seg1->component][r11];
+ v12 = &seg1->polygon->paths[seg1->component][r12];
+ v21 = &seg2->polygon->paths[seg2->component][r21];
+ v22 = &seg2->polygon->paths[seg2->component][r22];
+ if(inter & SOMETHING_X) {
+ lower1 = MMIN(v11->x, v12->x);
+ upper1 = MMAX(v11->x, v12->x);
+ lower2 = MMIN(v21->x, v22->x);
+ upper2 = MMAX(v21->x, v22->x);
+ } else {
+ lower1 = MMIN(v11->y, v12->y);
+ upper1 = MMAX(v11->y, v12->y);
+ lower2 = MMIN(v21->y, v22->y);
+ upper2 = MMAX(v21->y, v22->y);
+ }
+ return segments_bbox_interacts_1_core(lower1, upper1, lower2, upper2);
+}
+
+/* Register interaction of 2 segments along 1 dimension */
+static res_T
+register_segment_interaction
+ (const size_t seg_idx1,
+ const size_t seg_idx2,
+ const enum segment_interaction inter,
+ struct scpr_intersector* intersector)
+{
+ res_T res = RES_OK;
+ struct intersector_segment_pair pair;
+ unsigned char interact, *pinteract;
+
+ ASSERT(intersector);
+ if(seg_idx1 != seg_idx2) {
+ ASSERT(segments_bbox_interacts_1(seg_idx1, seg_idx2, inter, intersector));
+
+ /* Fill a record for this pair of segments */
+ init_segment_pair(&pair, seg_idx1, seg_idx2);
+ pinteract =
+ htable_interacting_segments_find(&intersector->interacting_segments, &pair);
+ if(!pinteract) { /* First occurence of this pair: create empty record */
+ pinteract = &interact;
+ interact = NO_INTERACTION;
+ }
+ /* Register this interaction */
+ ASSERT((*pinteract | inter) < UCHAR_MAX);
+ *pinteract = (unsigned char)(*pinteract | inter);
+ ERR(htable_interacting_segments_set(&intersector->interacting_segments,
+ &pair, pinteract));
+ }
+
+exit:
+ return res;
+error:
+ goto exit;
+}
+
+/* Register interaction of segment of seg_idx with all the segments in
+ * open_segments along 1 dimension */
+static res_T
+register_segment_interactions
+ (const size_t seg_idx,
+ struct htable_segment_idx* open_segments,
+ const enum segment_interaction inter,
+ struct scpr_intersector* intersector)
+{
+ res_T res = RES_OK;
+ struct htable_segment_idx_iterator it, end;
+
+ ASSERT(open_segments && intersector);
+
+ htable_segment_idx_begin(open_segments, &it);
+ htable_segment_idx_end(open_segments, &end);
+ /* Loop on segments using this point */
+ while(!htable_segment_idx_iterator_eq(&it, &end)) {
+ size_t seg2_idx = *htable_segment_idx_iterator_key_get(&it);
+ ERR(register_segment_interaction(seg_idx, seg2_idx, inter, intersector));
+ htable_segment_idx_iterator_next(&it);
+ }
+
+exit:
+ return res;
+error:
+ goto exit;
+}
+
+/* Register segments interaction along 1 dimension */
+static res_T
+register_interaction
+ (const int dim_idx,
+ struct scpr_intersector* intersector)
+{
+ res_T res = RES_OK;
+ size_t count, i;
+ struct intersector_range_point* points;
+ struct htable_segment_idx open_segments;
+ struct mem_allocator* allocator;
+ struct intersector_dimension* dimension;
+ char *begins = NULL, *ends = NULL;
+ size_t* seg_index = NULL;
+ ASSERT(intersector);
+
+ allocator = intersector->device->allocator;
+ dimension = intersector->dimensions + dim_idx;
+ htable_segment_idx_init(allocator, &open_segments);
+ count = darray_range_points_size_get(&dimension->sorted);
+ points = darray_range_points_data_get(&dimension->sorted);
+ for(i = 0; i < count; i++) {
+ struct intersector_range_point* point = points + i;
+ struct htable_segment_idx_iterator it, end;
+ size_t scount, s;
+
+ /* 0) Init begins, ends and seg_index
+ * Cannot recompute them on the fly as found will change */
+ scount = htable_segment_idx_size_get(&point->segments);
+ begins = (char*)MEM_REALLOC(allocator, begins, scount * sizeof(*begins));
+ ends = (char*)MEM_REALLOC(allocator, ends, scount * sizeof(*ends));
+ seg_index = (size_t*)MEM_REALLOC(allocator, seg_index,
+ scount * sizeof(*seg_index));
+ if(!begins || !ends || !seg_index) {
+ res = RES_MEM_ERR;
+ goto error;
+ }
+ s = 0;
+ htable_segment_idx_begin(&point->segments, &it);
+ htable_segment_idx_end(&point->segments, &end);
+ while(!htable_segment_idx_iterator_eq(&it, &end)) {
+ char c = *htable_segment_idx_iterator_data_get(&it);
+ size_t seg_idx = *htable_segment_idx_iterator_key_get(&it);
+ if(c == 2) { /* Both ends of the segment at this point */
+ ends[s] = begins[s] = 1;
+ } else {
+ char* found = htable_segment_idx_find(&open_segments, &seg_idx);
+ begins[s] = (found == NULL);
+ ends[s] = (found != NULL);
+ }
+ seg_index[s] = seg_idx;
+ s++;
+ htable_segment_idx_iterator_next(&it);
+ }
+ ASSERT(s == scount);
+
+ /* 1) Segments beginning and ending at this point overlap other segments that
+ * begin and end at the same point and contact segments that only begin or
+ * only end at this point */
+ for(s = 0; s < scount; s++) {
+ if(begins[s] && ends[s]) {
+ size_t j;
+ for(j = 0; j < scount; j++) {
+ if(j > s && begins[j] && ends[j]) {
+ ERR(register_segment_interaction(seg_index[s], seg_index[j],
+ dimension->overlap, intersector));
+ }
+ else if(begins[j] ^ ends[j]) {
+ ERR(register_segment_interaction(seg_index[s], seg_index[j],
+ dimension->contact, intersector));
+ }
+ }
+ }
+ }
+
+ /* 2) Segments only ending at this point overlap open segments */
+ for(s = 0; s < scount; s++) {
+ if(!begins[s] && ends[s]) {
+ ERR(register_segment_interactions(seg_index[s], &open_segments,
+ dimension->overlap, intersector));
+ }
+ }
+
+ /* 3) Segments ending at this point are now closed */
+ for(s = 0; s < scount; s++) {
+ if(ends[s]) {
+ size_t n = htable_segment_idx_erase(&open_segments, seg_index + s);
+ CHK(n == (begins[s] ? 0 : 1));
+ }
+ }
+
+ /* 4) Segments beginning and ending at this point overlap open segments */
+ for(s = 0; s < scount; s++) {
+ if(begins[s] && ends[s]) {
+ ERR(register_segment_interactions(seg_index[s], &open_segments,
+ dimension->overlap, intersector));
+ }
+ }
+
+ /* 5) Segments only beginning at this point are now open */
+ for(s = 0; s < scount; s++) {
+ if(begins[s] && !ends[s]) {
+ char one = 1;
+ ERR(htable_segment_idx_set(&open_segments, seg_index + s, &one));
+ }
+ }
+
+ /* 6) Segments only beginning at this point overlap open segments */
+ for(s = 0; s < scount; s++) {
+ if(begins[s] && !ends[s]) {
+ ERR(register_segment_interactions(seg_index[s], &open_segments,
+ dimension->overlap, intersector));
+ }
+ }
+ }
+
+exit:
+ MEM_RM(allocator, begins);
+ MEM_RM(allocator, ends);
+ MEM_RM(allocator, seg_index);
+ htable_segment_idx_release(&open_segments);
+ return res;
+error:
+ goto exit;
+}
+
+/* Check if 2 polygon Bbox interact */
+static int
+segments_bbox_interacts
+ (Clipper2Lib::Point64 *v11,
+ Clipper2Lib::Point64 *v12,
+ Clipper2Lib::Point64 *v21,
+ Clipper2Lib::Point64 *v22)
+{
+ int64_t lower1[2], upper1[2], lower2[2], upper2[2];
+ ASSERT(v11 && v12 && v21 && v22);
+ lower1[0] = MMIN(v11->x, v12->x);
+ lower1[1] = MMIN(v11->y, v12->y);
+ lower2[0] = MMIN(v21->x, v22->x);
+ lower2[1] = MMIN(v21->y, v22->y);
+ upper1[0] = MMAX(v11->x, v12->x);
+ upper1[1] = MMAX(v11->y, v12->y);
+ upper2[0] = MMAX(v21->x, v22->x);
+ upper2[1] = MMAX(v21->y, v22->y);
+ if(!segments_bbox_interacts_1_core(lower1[0], upper1[0], lower2[0], upper2[0]))
+ return 0;
+ return segments_bbox_interacts_1_core(lower1[1], upper1[1], lower2[1], upper2[1]);
+}
+
+static int64_t
+safe_mul
+ (int64_t a,
+ int64_t b,
+ int* ovflw)
+{
+ if(*ovflw) return 0;
+ if(b != 0 && abs(a) > 1 + (INT64_MAX / abs(b))) goto error;
+ return a * b;
+error:
+ *ovflw = 1;
+ return 0;
+}
+
+/* Check if a triangle is CCW (>0), CW (<0), or flat (=0) */
+static int
+is_ccw
+ (Clipper2Lib::Point64* v1,
+ Clipper2Lib::Point64* v2,
+ Clipper2Lib::Point64* v3,
+ int* ovflw)
+{
+ int64_t tmp1, tmp2;
+ ASSERT(v1 && v2 && v3);
+ /* Limited coordinate range garanties that sub computations stay in int64_t
+ * range */
+ tmp1 = safe_mul(v2->x - v1->x, v3->y - v1->y, ovflw);
+ tmp2 = safe_mul(v3->x - v1->x, v2->y - v1->y, ovflw);
+ if(tmp1 < tmp2) return -1;
+ if(tmp1 > tmp2) return +1;
+ return 0;
+}
+
+/* Check if 2 segments intersect */
+static res_T
+check_two_segments_intersection
+ (const struct intersector_segment* seg1,
+ const struct intersector_segment* seg2,
+ struct scpr_intersector_check_callbacks* callbacks,
+ void* data)
+{
+ res_T res = RES_OK;
+ size_t count, r11, r12, r21, r22;
+ Clipper2Lib::Point64 *v11, * v12, *v21, *v22;
+ int ccw1, ccw2, ccw3, ccw4, ovflw = 0;
+
+ ASSERT(seg1 && seg2 && callbacks);
+
+ /* Get vertices */
+ r11 = seg1->rank;
+ ERR(scpr_polygon_get_vertices_count(seg1->polygon, seg1->component, &count));
+ r12 = (r11 + 1) % count;
+ r21 = seg2->rank;
+ ERR(scpr_polygon_get_vertices_count(seg2->polygon, seg2->component, &count));
+ r22 = (r21 + 1) % count;
+ v11 = &seg1->polygon->paths[seg1->component][r11];
+ v12 = &seg1->polygon->paths[seg1->component][r12];
+ v21 = &seg2->polygon->paths[seg2->component][r21];
+ v22 = &seg2->polygon->paths[seg2->component][r22];
+ ASSERT(segments_bbox_interacts(v11, v12, v21, v22));
+
+ /* Test triangles orientation */
+ ccw1 = is_ccw(v11, v12, v21, &ovflw);
+ ccw2 = is_ccw(v11, v12, v22, &ovflw);
+ ccw3 = is_ccw(v21, v22, v11, &ovflw);
+ ccw4 = is_ccw(v21, v22, v12, &ovflw);
+ if(ovflw) {
+ res = RES_BAD_ARG;
+ goto error;
+ }
+ /* Analyse ccw results */
+ if(ccw1 * ccw2 > 0 && ccw3 * ccw4 > 0) {
+ /* No intersection at all */
+ }
+ else if(ccw1 == 0 && ccw2 == 0 && ccw3 == 0 && ccw4 == 0) {
+ /* Colinear segments */
+ callbacks->colinear_segments(seg1->polygon, seg1->component, seg1->rank,
+ seg2->polygon, seg2->component, seg2->rank, data);
+ }
+ else if(ccw1 * ccw2 < 0 && ccw3 * ccw4 < 0) {
+ /* Basic segment intersection */
+ callbacks->simple_intersection(seg1->polygon, seg1->component, seg1->rank,
+ seg2->polygon, seg2->component, seg2->rank, data);
+ }
+
+exit:
+ return res;
+error:
+ goto exit;
+}
+
+/* Test intersection of all the registered pairs of interacting segments */
+static res_T
+check_interacting_pairs
+ (struct scpr_intersector* intersector,
+ struct scpr_intersector_check_callbacks* callbacks,
+ void* data)
+{
+ res_T res = RES_OK;
+ struct htable_interacting_segments_iterator it, end;
+ const struct intersector_segment* segments;
+
+ ASSERT(intersector && callbacks);
+
+ segments = darray_segments_cdata_get(&intersector->segments);
+ htable_interacting_segments_begin(&intersector->interacting_segments, &it);
+ htable_interacting_segments_end(&intersector->interacting_segments, &end);
+ /* Loop on interacting segments */
+ while(!htable_interacting_segments_iterator_eq(&it, &end)) {
+ unsigned char overlapping
+ = *htable_interacting_segments_iterator_data_get(&it);
+ if(overlapping & SOME_OVERLAP
+ && overlapping & SOMETHING_X && overlapping & SOMETHING_Y)
+ {
+ struct intersector_segment_pair* pair
+ = htable_interacting_segments_iterator_key_get(&it);
+ const struct intersector_segment* seg1 = segments + pair->rank1;
+ const struct intersector_segment* seg2 = segments + pair->rank2;
+ ERR(check_two_segments_intersection(seg1, seg2, callbacks, data));
+ }
+ htable_interacting_segments_iterator_next(&it);
+ }
+
+exit:
+ return res;
+error:
+ goto exit;
+}
+
+/*******************************************************************************
+ * Exported functions
+ ******************************************************************************/
+res_T
+scpr_intersector_create
+ (struct scpr_device* device,
+ struct scpr_intersector** out_intersector)
+{
+ res_T res = RES_OK;
+ struct scpr_intersector* intersector = NULL;
+ struct mem_allocator* allocator;
+
+ if(!device || ! out_intersector) {
+ res = RES_BAD_ARG;
+ goto error;
+ }
+
+ allocator = device->allocator;
+ intersector = (struct scpr_intersector*)MEM_ALLOC(allocator, sizeof(*intersector));
+ if(!intersector) {
+ res = RES_MEM_ERR;
+ goto error;
+ }
+ ref_init(&intersector->ref);
+ intersector->device = device;
+ ERR(scpr_device_ref_get(device));
+ darray_segments_init(allocator, &intersector->segments);
+ init_dimension(allocator, intersector->dimensions, OVERLAP_X, CONTACT_X);
+ init_dimension(allocator, intersector->dimensions + 1, OVERLAP_Y, CONTACT_Y);
+ htable_interacting_segments_init(allocator, &intersector->interacting_segments);
+ htable_registered_components_init(allocator, &intersector->registererd_components);
+
+exit:
+ if(out_intersector) *out_intersector = intersector;
+ return res;
+error:
+ if(intersector) {
+ SCPR(intersector_ref_put(intersector));
+ intersector = NULL;
+ }
+ goto exit;
+}
+
+res_T
+scpr_intersector_ref_get
+ (struct scpr_intersector* intersector)
+{
+ if(!intersector) return RES_BAD_ARG;
+ ref_get(&intersector->ref);
+ return RES_OK;
+}
+
+res_T
+scpr_intersector_ref_put
+ (struct scpr_intersector* intersector)
+{
+ if(!intersector) return RES_BAD_ARG;
+ ref_put(&intersector->ref, intersector_release);
+ return RES_OK;
+}
+
+res_T
+scpr_intersector_register_component
+ (struct scpr_intersector* intersector,
+ struct scpr_polygon* polygon,
+ const size_t icomponent)
+{
+ res_T res = RES_OK;
+ size_t prev_count, new_count, i;
+ struct intersector_component comp;
+ char *found, one = 1;
+
+ if(!intersector || !polygon || icomponent >= polygon->paths.size()) {
+ res = RES_BAD_ARG;
+ goto error;
+ }
+
+ /* Check single registration */
+ comp.polygon = polygon;
+ comp.component = icomponent;
+ found = htable_registered_components_find(&intersector->registererd_components,
+ &comp);
+ if(found) {
+ logger_print(intersector->device->logger, LOG_ERROR,
+ "Component already registered.\n");
+ res = RES_BAD_ARG;
+ goto error;
+ }
+ htable_registered_components_set(&intersector->registererd_components, &comp,
+ &one);
+
+ prev_count = darray_segments_size_get(&intersector->segments);
+ new_count = polygon->paths[icomponent].size();
+ ERR(darray_segments_resize(&intersector->segments, prev_count + new_count));
+
+ for(i = 0; i < new_count; i++) {
+ Clipper2Lib::Point64 *p0, *p1;
+ size_t seg_idx = prev_count + i;
+ struct intersector_segment* segment
+ = darray_segments_data_get(&intersector->segments) + seg_idx;
+ segment->polygon = polygon;
+ segment->component = icomponent;
+ segment->rank = i;
+ p0 = &polygon->paths[icomponent][i];
+ p1 = &polygon->paths[icomponent][(i+1)%new_count];
+ ERR(register_point(intersector, 0, p0->x, seg_idx));
+ ERR(register_point(intersector, 0, p1->x, seg_idx));
+ ERR(register_point(intersector, 1, p0->y, seg_idx));
+ ERR(register_point(intersector, 1, p1->y, seg_idx));
+ }
+
+exit:
+ return res;
+error:
+ goto exit;
+}
+
+res_T
+scpr_intersector_check
+ (struct scpr_intersector* intersector,
+ struct scpr_intersector_check_callbacks* callbacks,
+ void* data)
+{
+ int i;
+ res_T res = RES_OK;
+
+ if(!intersector || !callbacks
+ || !callbacks->simple_intersection
+ || !callbacks->colinear_segments)
+ {
+ res = RES_BAD_ARG;
+ goto error;
+ }
+
+ /* Construct candidates for intersection */
+ for(i = 0; i < 2; i++) {
+ sort_points(&intersector->dimensions[i]);
+ ERR(register_interaction(i, intersector));
+ }
+ /* Check intersections */
+ ERR(check_interacting_pairs(intersector, callbacks, data));
+
+exit:
+ return res;
+error:
+ goto exit;
+}
diff --git a/src/test_scpr_intersector.c b/src/test_scpr_intersector.c
@@ -0,0 +1,193 @@
+/* Copyright (C) 2016-2018, 2021 |Meso|Star> (contact@meso-star.com)
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>. */
+
+#define _POSIX_C_SOURCE 200112L
+
+#include "scpr.h"
+#include "test_scpr_utils.h"
+
+#include <rsys/rsys.h>
+
+#include <memory.h>
+
+struct cpt {
+ size_t basic, colinear;
+};
+
+static void
+basic_intersection
+ (struct scpr_polygon* polygon1, size_t component1, size_t segment1,
+ struct scpr_polygon* polygon2, size_t component2, size_t segment2,
+ void* data)
+{
+ struct cpt* cpt = (struct cpt*)data;
+ (void)polygon1; (void)component1; (void)segment1;
+ (void)polygon2; (void)component2; (void)segment2;
+ ASSERT(cpt);
+ cpt->basic++;
+}
+
+static void
+colinear_segments
+ (struct scpr_polygon* polygon1, size_t component1, size_t segment1,
+ struct scpr_polygon* polygon2, size_t component2, size_t segment2,
+ void* data)
+{
+ struct cpt* cpt = (struct cpt*)data;
+ (void)polygon1; (void)component1; (void)segment1;
+ (void)polygon2; (void)component2; (void)segment2;
+ ASSERT(cpt);
+ cpt->colinear++;
+}
+
+int
+main(int argc, char** argv)
+{
+ struct mem_allocator allocator;
+ struct scpr_device* dev;
+ struct scpr_device_create_args args = SCPR_DEVICE_CREATE_ARGS_DEFAULT;
+ struct scpr_intersector* inter;
+ struct scpr_intersector_check_callbacks cb
+ = SCPR_INTERSECTOR_CHECK_CALLBACKS_NULL__ ;
+ struct polygon_context ctx;
+ struct scpr_polygon *p01, *p23, *p4;
+ size_t ncomps = 2;
+ double** coords;
+ /* 1,1
+ * +-----------+----+
+ * | / | /
+ * | / | /
+ * +-----------+-C2--+
+ * | | | |
+ * | | | |
+ * | +-C0--+-+---+----+----+
+ * | 0,0 |/ | |
+ * | +-C3-------+-C4-+
+ * +-C1--------+
+ * -0.5,-0.5
+ */
+ double coords0[] = {
+ 0.0, 0.0,
+ 1.0, 0.0,
+ 1.0, 1.0,
+ 0.0, 1.0
+ };
+ double coords1[] = {
+ -0.5, -0.5,
+ 0.5, -0.5,
+ 0.5, 0.5,
+ -0.5, 0.5
+ };
+ double coords2[] = {
+ 0.5, 0.5,
+ 1.0, 0.5,
+ 1.5, 1.0,
+ 1.0, 1.0
+ };
+ double coords3[] = {
+ 0.5, -0.3,
+ 1.5, -0.3,
+ 1.5, 0.0,
+ 0.7, 0.0
+ };
+ double coords4[] = {
+ 1.5, -0.3,
+ 2.0, -0.3,
+ 2.0, 0.0,
+ 1.5, 0
+ };
+ size_t nverts[] = { 4, 4 };
+ struct cpt cpt = { 0, 0 };
+ (void)argc, (void)argv;
+
+ mem_init_proxy_allocator(&allocator, &mem_default_allocator);
+ args.allocator = &allocator;
+ args.verbosity_level = 3;
+ args.precision = 6;
+ OK(scpr_device_create(&args, &dev));
+
+ BAD(scpr_intersector_create(NULL, NULL));
+ BAD(scpr_intersector_create(dev, NULL));
+ BAD(scpr_intersector_create(NULL, &inter));
+ OK(scpr_intersector_create(dev, &inter));
+
+ BAD(scpr_intersector_ref_get(NULL));
+ OK(scpr_intersector_ref_get(inter));
+
+ BAD(scpr_intersector_ref_put(NULL));
+ OK(scpr_intersector_ref_put(inter));
+
+ OK(scpr_polygon_create(dev, &p01));
+ OK(scpr_polygon_create(dev, &p23));
+ OK(scpr_polygon_create(dev, &p4));
+ coords = (double**)MEM_CALLOC(&allocator, ncomps, sizeof(*coords));
+ coords[0] = (double*)MEM_CALLOC(&allocator, nverts[0], 2*sizeof(**coords));
+ coords[1] = (double*)MEM_CALLOC(&allocator, nverts[1], 2*sizeof(**coords));
+
+ ctx.coords = coords;
+ ctx.nverts = nverts;
+ ctx.ncomps = ncomps;
+
+ #define SETUP scpr_polygon_setup_indexed_vertices
+ memcpy(coords[0], coords0, 2*nverts[0]*sizeof(**coords));
+ memcpy(coords[1], coords1, 2*nverts[1]*sizeof(**coords));
+ OK(SETUP(p01, ncomps, pget_nverts, pget_pos, &ctx));
+
+ memcpy(coords[0], coords2, 2*nverts[0]*sizeof(**coords));
+ memcpy(coords[1], coords3, 2*nverts[1]*sizeof(**coords));
+ OK(SETUP(p23, ncomps, pget_nverts, pget_pos, &ctx));
+
+ memcpy(coords[0], coords4, 2*nverts[0]*sizeof(**coords));
+ OK(SETUP(p4, 1, pget_nverts, pget_pos, &ctx));
+
+ BAD(scpr_intersector_register_component(NULL, NULL, ncomps));
+ BAD(scpr_intersector_register_component(NULL, NULL, 0));
+ BAD(scpr_intersector_register_component(NULL, p01, ncomps));
+ BAD(scpr_intersector_register_component(inter, NULL, ncomps));
+ BAD(scpr_intersector_register_component(NULL, p01, 0));
+ BAD(scpr_intersector_register_component(inter, NULL, 0));
+ BAD(scpr_intersector_register_component(inter, p01, ncomps));
+ OK(scpr_intersector_register_component(inter, p01, 0));
+ OK(scpr_intersector_register_component(inter, p01, 1));
+ OK(scpr_intersector_register_component(inter, p23, 0));
+ OK(scpr_intersector_register_component(inter, p23, 1));
+ OK(scpr_intersector_register_component(inter, p4, 0));
+
+ BAD(scpr_intersector_check(NULL, NULL, NULL));
+ BAD(scpr_intersector_check(NULL, &cb, NULL));
+ BAD(scpr_intersector_check(inter, NULL, NULL));
+ BAD(scpr_intersector_check(inter, &cb, NULL));
+ cb.simple_intersection = basic_intersection;
+ cb.colinear_segments = colinear_segments;
+ OK(scpr_intersector_check(inter, &cb, &cpt));
+ CHK(cpt.basic == 2);
+ CHK(cpt.colinear == 2);
+
+ OK(scpr_polygon_ref_put(p01));
+ OK(scpr_polygon_ref_put(p23));
+ OK(scpr_polygon_ref_put(p4));
+ OK(scpr_intersector_ref_put(inter));
+ OK(scpr_device_ref_put(dev));
+
+ MEM_RM(&allocator, coords[0]);
+ MEM_RM(&allocator, coords[1]);
+ MEM_RM(&allocator, coords);
+
+ check_memory_allocator(&allocator);
+ mem_shutdown_proxy_allocator(&allocator);
+ CHK(mem_allocated_size() == 0);
+ return 0;
+}
+
