s2d_scene_view_closest_point.c (6890B)
1 /* Copyright (C) 2016-2021, 2023 |Méso|Star> (contact@meso-star.com) 2 * 3 * This program is free software: you can redistribute it and/or modify 4 * it under the terms of the GNU General Public License as published by 5 * the Free Software Foundation, either version 3 of the License, or 6 * (at your option) any later version. 7 * 8 * This program is distributed in the hope that it will be useful, 9 * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 * GNU General Public License for more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * along with this program. If not, see <http://www.gnu.org/licenses/>. */ 15 16 #include "s2d.h" 17 #include "s2d_device_c.h" 18 #include "s2d_geometry.h" 19 #include "s2d_line_segments.h" 20 #include "s2d_scene_view_c.h" 21 22 #include <rsys/float2.h> 23 #include <rsys/double2.h> 24 25 struct point_query_context { 26 struct RTCPointQueryContext rtc; 27 struct s2d_scene_view* scnview; 28 float radius; /* Submitted radius */ 29 void* data; /* Per point query defined data */ 30 }; 31 32 /******************************************************************************* 33 * Helper functions 34 ******************************************************************************/ 35 static INLINE float* 36 closest_point_segment 37 (const float p_f[2], /* Position */ 38 const float v0_f[2], /* 1st segment vertex */ 39 const float v1_f[2], /* 2nd segment vertex */ 40 float closest_pt_f[2], /* Closest position */ 41 float* s_f) /* Parametric coordinate of the closest point onto [v0, v1] */ 42 { 43 double v[2]; /* Vector from v0 to p */ 44 double E[2]; /* v0 -> v1 vector */ 45 double p[2], v0[2], v1[2], closest_pt[2]; 46 double segment_len2; /* Square length of the [v0, v1] */ 47 double dst_x_seglen; /* |p' v0| x |v0 v1| 48 * p' is the orthogonal projection of p onto [v0, v1] */ 49 double s; 50 ASSERT(p_f && v0_f && v1_f); 51 52 d2_set_f2(v0, v0_f); 53 d2_set_f2(v1, v1_f); 54 d2_set_f2(p, p_f); 55 d2_sub(E, v1, v0); 56 57 /* Orthogonally project the point onto the segment */ 58 d2_sub(v, p, v0); 59 dst_x_seglen = d2_dot(v, E); 60 61 /* Check if the closest point is the segment vertex 'v0' */ 62 if(dst_x_seglen <= 0) { 63 *s_f = 0; 64 return f2_set(closest_pt_f, v0_f); 65 } 66 /* Check if the closest point is the segment vertex 'v1' */ 67 segment_len2 = d2_dot(E, E); 68 if(dst_x_seglen >= segment_len2) { 69 *s_f = 1; 70 return f2_set(closest_pt_f, v1_f); 71 } 72 73 /* The closest point is on the segment */ 74 s = dst_x_seglen / segment_len2; 75 d2_add(closest_pt, d2_muld(closest_pt, E, s), v0); 76 *s_f = (float)s; 77 ASSERT(*s_f == CLAMP(*s_f, 0, 1)); 78 return f2_set_d2(closest_pt_f, closest_pt); 79 } 80 81 static bool 82 closest_point_line_segments 83 (struct RTCPointQueryFunctionArguments* args, 84 struct geometry* geom, 85 const float radius, 86 void* query_data) 87 { 88 struct s2d_hit hit = S2D_HIT_NULL; 89 struct s2d_hit* out_hit = NULL; 90 struct hit_filter* filter = NULL; 91 const uint32_t* ids = NULL; 92 float v0[2], v1[2]; /* Segment vertices */ 93 float N[2]; /* Segment normal */ 94 float query_pos[2]; /* Submitted position */ 95 float range[2]; 96 float closest_point[2]; /* Computed closest point */ 97 float vec[2]; /* Vector from query pos to the closest point */ 98 float dst; /* Distance to the closest point */ 99 float s; /* Parametric coordinate of the closest point */ 100 ASSERT(args && geom); 101 ASSERT(args->primID < line_segments_get_nsegments(geom->lines)); 102 ASSERT(radius >=0); 103 104 /* Fetch the line segments indices */ 105 ids = line_segments_get_ids(geom->lines) + args->primID*2/*#indices per segment*/; 106 107 /* Fetch segments vertices */ 108 ASSERT(geom->lines->attribs_type[S2D_POSITION] == S2D_FLOAT2); 109 f2_set(v0, line_segments_get_pos(geom->lines)+ids[0]*2/*#coords per vertex */); 110 f2_set(v1, line_segments_get_pos(geom->lines)+ids[1]*2/*#coords per vertex */); 111 112 query_pos[0] = args->query->x; 113 query_pos[1] = args->query->y; 114 115 /* Compute the closest point on the segment from the submitted query_pos */ 116 closest_point_segment(query_pos, v0, v1, closest_point, &s); 117 118 f2_sub(vec, closest_point, query_pos); 119 dst = f2_len(vec); 120 if(dst >= args->query->radius) return 0; 121 122 /* Compute the segment normal (left hand convention) */ 123 N[0] = v1[1] - v0[1]; 124 N[1] = v0[0] - v1[0]; 125 126 /* Flip the geometry normal wrt the flip line_segments flag */ 127 if(geom->flip_contour) f2_minus(N, N); 128 129 /* Setup the hit */ 130 hit.prim.mesh__ = geom; 131 hit.prim.prim_id = args->primID; 132 hit.prim.geom_id = geom->name; 133 hit.prim.scene_prim_id = hit.prim.prim_id + geom->scene_prim_id_offset; 134 hit.normal[0] = N[0]; 135 hit.normal[1] = N[1]; 136 hit.u = s; 137 hit.distance = dst; 138 139 range[0] = 0; 140 range[1] = radius; 141 142 /* `vec' is the direction along which the closest point was found. Submit it 143 * to the filter function as the direction of the computed hit. */ 144 filter = &geom->lines->filter; 145 if(filter->func 146 && filter->func(&hit, query_pos, vec, range, query_data, filter->data)) { 147 return 0; /* This point is filtered. Discard it! */ 148 } 149 150 /* Update output data */ 151 out_hit = args->userPtr; 152 *out_hit = hit; 153 args->query->radius = dst; /* Shrink the query radius */ 154 155 return 1; /* Notify that the query radius was updated */ 156 } 157 158 static bool 159 closest_point(struct RTCPointQueryFunctionArguments* args) 160 { 161 struct point_query_context* ctx = NULL; 162 struct geometry* geom = NULL; 163 ASSERT(args); 164 165 ctx = CONTAINER_OF(args->context, struct point_query_context, rtc); 166 167 /* Instance are not supported by Star-2D */ 168 ASSERT(args->context->instStackSize == 0); 169 170 geom = scene_view_geometry_from_embree_id(ctx->scnview, args->geomID); 171 return closest_point_line_segments(args, geom, ctx->radius, ctx->data); 172 } 173 174 /******************************************************************************* 175 * Exported functions 176 ******************************************************************************/ 177 res_T 178 s2d_scene_view_closest_point 179 (struct s2d_scene_view* scnview, 180 const float pos[2], 181 const float radius, 182 void* query_data, 183 struct s2d_hit* hit) 184 { 185 struct RTCPointQuery query; 186 struct point_query_context query_ctx; 187 188 if(!scnview || !pos || radius <= 0 || !hit) 189 return RES_BAD_ARG; 190 191 if((scnview->mask & S2D_TRACE) == 0) { 192 log_error(scnview->scn->dev, 193 "%s: the S2D_TRACE flag is not active onto the submitted scene view.\n", 194 FUNC_NAME); 195 return RES_BAD_OP; 196 } 197 198 *hit = S2D_HIT_NULL; 199 200 /* Initialise the point query */ 201 query.x = pos[0]; 202 query.y = pos[1]; 203 query.z = 0; 204 query.radius = radius; 205 query.time = FLT_MAX; /* Invalid fields */ 206 207 /* Initialise the point query context */ 208 rtcInitPointQueryContext(&query_ctx.rtc); 209 query_ctx.scnview = scnview; 210 query_ctx.radius = radius; 211 query_ctx.data = query_data; 212 213 /* Here we go! */ 214 rtcPointQuery(scnview->rtc_scn, &query, &query_ctx.rtc, closest_point, hit); 215 return RES_OK; 216 }