cgv
Loading...
Searching...
No Matches
proximity.h
1#pragma once
2
3#include "fvec.h"
4#include "quaternion.h"
5#include <random>
6
7#include "lib_begin.h"
8
9namespace cgv {
10 namespace math {
20 template <typename T>
21 void closest_point_on_sphere_to_point(const fvec<T, 3>& so, T sr, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
22 {
23 n = normalize(p - so);
24 q = sr * n + so;
25 }
36 template <typename T>
37 void closest_point_on_box_to_point(const fvec<T, 3>& bo, const fvec<T, 3>& be, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
38 {
39 int j = 0;
40 for (int i = 0; i < 3; ++i) {
41 q[i] = p[i] - bo[i];
42 if (q[i] < 0) {
43 n[i] = -1;
44 if (q[i] < -0.5f * be[i]) {
45 q[i] = -0.5f * be[i];
46 ++j;
47 }
48 else
49 n[i] = 0;
50 }
51 else {
52 n[i] = 1;
53 if (q[i] > 0.5f * be[i]) {
54 q[i] = 0.5f * be[i];
55 ++j;
56 }
57 else
58 n[i] = 0;
59 }
60 q[i] += bo[i];
61 }
62 if (j > 1)
63 n *= sqrt(T(2));
64 }
76 template <typename T>
77 void closest_point_on_box_to_point(const fvec<T, 3>& bo, const fvec<T, 3>& be, const quaternion<T>& bq, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
78 {
79 fvec<T, 3> p_loc = p-bo;
80 bq.inverse_rotate(p_loc);
81 int j = 0;
82 for (int i = 0; i < 3; ++i) {
83 q[i] = p[i];
84 if (q[i] < 0) {
85 n[i] = -1;
86 if (q[i] < -0.5f * be[i]) {
87 q[i] = -0.5f * be[i];
88 ++j;
89 }
90 else
91 n[i] = 0;
92 }
93 else {
94 n[i] = 1;
95 if (q[i] > 0.5f * be[i]) {
96 q[i] = 0.5f * be[i];
97 ++j;
98 }
99 else
100 n[i] = 0;
101 }
102 }
103 if (j > 1)
104 n *= sqrt(T(2));
105 bq.rotate(n);
106 bq.rotate(q);
107 q += bo;
108 }
119 template <typename T>
120 void closest_point_on_cylinder_to_point(const fvec<T, 3>& co, const fvec<T, 3>& cd, T cr, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
121 {
122 T l = dot(cd, cd);
123 T a = dot(p - co, cd) / l;
124 l = std::sqrt(l);
125 fvec<T, 3> q0 = co + a * cd;
126 T r0 = (p - q0).length();
127 if (a < 0) {
128 if (r0 > cr - a * l)
129 n = (1 / r0) * (p - q0);
130 else
131 n = -cd / l;
132 if (r0 < cr)
133 q = co + p - q0;
134 else
135 q = co + cr / r0 * (p - q0);
136 }
137 else if (a > 1) {
138 if (r0 > cr + (a - 1) * l)
139 n = (1 / r0) * (p - q0);
140 else
141 n = cd / l;
142 if (r0 < cr)
143 q = co + cd + p - q0;
144 else
145 q = co + cd + cr / r0 * (p - q0);
146 }
147 else {
148 q = q0 + (cr / r0) * (p - q0);
149 n = (1 / r0) * (p - q0);
150 }
151 }
162 template <typename T>
163 bool closest_point_on_line_to_line(const fvec<T, 3>& lo, const fvec<T, 3>& ld, const fvec<T, 3>& lo2, const fvec<T, 3>& ld2, fvec<T, 3>& q)
164 {
165 T a = dot(ld, ld);
166 T b = dot(ld, ld2);
167 T e = dot(ld2, ld2);
168
169 T d = a * e - b * b;
170 if (std::abs(d) < 1e-10f) // lines are parallel
171 return false;
172 fvec<T, 3> r = lo - lo2;
173 T c = dot(ld, r);
174 T f = dot(ld2, r);
175 T s = (b * f - c * e) / d;
176 q = lo + s * ld;
177 //T t = (a * f - b * c) / d;
178 return true;
179 }
189 template <typename T, cgv::type::uint32_type N>
190 fvec<T, N> closest_point_on_line_to_point(const fvec<T, N>& lo, const fvec<T, N>& ld, const fvec<T, N>& p)
191 {
192 return lo + (dot(p - lo, ld) / dot(ld, ld)) * ld;
193 }
204 template <typename T>
205 bool closest_point_on_circle_to_point(const fvec<T, 3>& co, const fvec<T, 3>& cn, T cr, const fvec<T, 3>& p, fvec<T, 3>& q)
206 {
207 fvec<T, 3> d = p - co;
208 d -= (dot(d, cn) / dot(cn, cn)) * cn;
209 T l = d.length();
210 if (l < 1e-6f)
211 return false;
212 q = co + (cr / l) * d;
213 return true;
214 }
215 template <typename T>
216 T closest_point_on_line_to_circle_impl(const fvec<T, 3>& lo, const fvec<T, 3>& ld, const fvec<T, 3>& co, const fvec<T, 3>& cn, T cr, fvec<T, 3>& q, int* iter_ptr = 0)
217 {
218 std::default_random_engine e;
219 std::uniform_real_distribution<T> d(-0.01f, 0.01f);
220 q = lo;
221 fvec<T, 3> q0;
222 int iter = 0;
223 T dist = std::numeric_limits<T>::max();
224 do {
225 q0 = q;
226 int cnt = 0;
227 fvec<T, 3> q1 = q;
228 while (!closest_point_on_circle_to_point(co, cn, cr, q, q)) {
229 q += fvec<T, 3>(d(e), d(e), d(e));
230 if (++cnt > 10) {
231 std::cerr << "jittering not working!" << std::endl;
232 return std::numeric_limits<T>::max();
233 }
234 }
235 q1 = q;
236 q = closest_point_on_line_to_point(lo, ld, q);
237 dist = (q - q1).length();
238 if (++iter > 1000) {
239 std::cerr << "no convergence!" << std::endl;
240 if (iter_ptr)
241 *iter_ptr = iter;
242 return std::numeric_limits<T>::max();
243 }
244 } while ((q0 - q).length() > 1e-6f);
245 if (iter_ptr)
246 *iter_ptr = iter;
247 return dist;
248 }
261 template <typename T>
262 T closest_point_on_line_to_circle(const fvec<T, 3>& lo, const fvec<T, 3>& ld, const fvec<T, 3>& co, const fvec<T, 3>& cn, T cr, fvec<T, 3>& q, int* iter_ptr = 0)
263 {
264 T dist = closest_point_on_line_to_circle_impl(lo, ld, co, cn, cr, q);
265 if (dist == std::numeric_limits<T>::max())
266 return dist;
267
268 fvec<T, 3> lo2 = closest_point_on_line_to_point(lo, ld, co);
269 lo2 = 2.0f * lo2 - q;
270 fvec<T, 3> q2;
271 T dist2 = cgv::math::closest_point_on_line_to_circle_impl(lo2, ld, co, cn, cr, q2);
272 if (dist2 == std::numeric_limits<float>::max())
273 return dist2;
274 if (dist < dist2)
275 return dist;
276 q = q2;
277 return dist2;
278 }
279 }
280}
281
282#include <cgv/config/lib_end.h>
cgv
the cgv namespace
Definition print.h:11