Line data Source code
1 : /*
2 : * vector3.cpp
3 : * Copyright (C) Andrew Tridgell 2012
4 : *
5 : * This file is free software: you can redistribute it and/or modify it
6 : * under the terms of the GNU General Public License as published by the
7 : * Free Software Foundation, either version 3 of the License, or
8 : * (at your option) any later version.
9 : *
10 : * This file is distributed in the hope that it will be useful, but
11 : * WITHOUT ANY WARRANTY; without even the implied warranty of
12 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 : * See the GNU General Public License for more details.
14 : *
15 : * You should have received a copy of the GNU General Public License along
16 : * with this program. If not, see <http://www.gnu.org/licenses/>.
17 : */
18 :
19 : #pragma GCC optimize("O2")
20 :
21 : #include "AP_Math.h"
22 : #include <AP_InternalError/AP_InternalError.h>
23 : #include <AP_CustomRotations/AP_CustomRotations.h>
24 : #include <AP_Vehicle/AP_Vehicle_Type.h>
25 :
26 : // rotate a vector by a standard rotation, attempting
27 : // to use the minimum number of floating point operations
28 : template <typename T>
29 4390985440 : void Vector3<T>::rotate(enum Rotation rotation)
30 : {
31 : T tmp;
32 4390985440 : switch (rotation) {
33 : case ROTATION_NONE:
34 : return;
35 28187 : case ROTATION_YAW_45: {
36 28187 : tmp = HALF_SQRT_2*(ftype)(x - y);
37 28187 : y = HALF_SQRT_2*(ftype)(x + y);
38 28187 : x = tmp;
39 28187 : return;
40 : }
41 2376 : case ROTATION_YAW_90: {
42 2376 : tmp = x; x = -y; y = tmp;
43 2376 : return;
44 : }
45 6704 : case ROTATION_YAW_135: {
46 6704 : tmp = -HALF_SQRT_2*(ftype)(x + y);
47 6704 : y = HALF_SQRT_2*(ftype)(x - y);
48 6704 : x = tmp;
49 6704 : return;
50 : }
51 5641460 : case ROTATION_YAW_180:
52 5641460 : x = -x; y = -y;
53 5641460 : return;
54 6625 : case ROTATION_YAW_225: {
55 6625 : tmp = HALF_SQRT_2*(ftype)(y - x);
56 6625 : y = -HALF_SQRT_2*(ftype)(x + y);
57 6625 : x = tmp;
58 6625 : return;
59 : }
60 20 : case ROTATION_YAW_270: {
61 20 : tmp = x; x = y; y = -tmp;
62 20 : return;
63 : }
64 20 : case ROTATION_YAW_315: {
65 20 : tmp = HALF_SQRT_2*(ftype)(x + y);
66 20 : y = HALF_SQRT_2*(ftype)(y - x);
67 20 : x = tmp;
68 20 : return;
69 : }
70 20 : case ROTATION_ROLL_180: {
71 20 : y = -y; z = -z;
72 20 : return;
73 : }
74 20 : case ROTATION_ROLL_180_YAW_45: {
75 20 : tmp = HALF_SQRT_2*(ftype)(x + y);
76 20 : y = HALF_SQRT_2*(ftype)(x - y);
77 20 : x = tmp; z = -z;
78 20 : return;
79 : }
80 46 : case ROTATION_ROLL_180_YAW_90:
81 : case ROTATION_PITCH_180_YAW_270: {
82 46 : tmp = x; x = y; y = tmp; z = -z;
83 46 : return;
84 : }
85 20 : case ROTATION_ROLL_180_YAW_135: {
86 20 : tmp = HALF_SQRT_2*(ftype)(y - x);
87 20 : y = HALF_SQRT_2*(ftype)(y + x);
88 20 : x = tmp; z = -z;
89 20 : return;
90 : }
91 9028 : case ROTATION_PITCH_180: {
92 9028 : x = -x; z = -z;
93 9028 : return;
94 : }
95 20 : case ROTATION_ROLL_180_YAW_225: {
96 20 : tmp = -HALF_SQRT_2*(ftype)(x + y);
97 20 : y = HALF_SQRT_2*(ftype)(y - x);
98 20 : x = tmp; z = -z;
99 20 : return;
100 : }
101 536279866 : case ROTATION_ROLL_180_YAW_270:
102 : case ROTATION_PITCH_180_YAW_90: {
103 536279866 : tmp = x; x = -y; y = -tmp; z = -z;
104 536279866 : return;
105 : }
106 20 : case ROTATION_ROLL_180_YAW_315: {
107 20 : tmp = HALF_SQRT_2*(ftype)(x - y);
108 20 : y = -HALF_SQRT_2*(ftype)(x + y);
109 20 : x = tmp; z = -z;
110 20 : return;
111 : }
112 12286 : case ROTATION_ROLL_90: {
113 12286 : tmp = z; z = y; y = -tmp;
114 12286 : return;
115 : }
116 20 : case ROTATION_ROLL_90_YAW_45: {
117 20 : tmp = z; z = y; y = -tmp;
118 20 : tmp = HALF_SQRT_2*(ftype)(x - y);
119 20 : y = HALF_SQRT_2*(ftype)(x + y);
120 20 : x = tmp;
121 20 : return;
122 : }
123 20 : case ROTATION_ROLL_90_YAW_90: {
124 20 : tmp = z; z = y; y = -tmp;
125 20 : tmp = x; x = -y; y = tmp;
126 20 : return;
127 : }
128 20 : case ROTATION_ROLL_90_YAW_135: {
129 20 : tmp = z; z = y; y = -tmp;
130 20 : tmp = -HALF_SQRT_2*(ftype)(x + y);
131 20 : y = HALF_SQRT_2*(ftype)(x - y);
132 20 : x = tmp;
133 20 : return;
134 : }
135 20 : case ROTATION_ROLL_270: {
136 20 : tmp = z; z = -y; y = tmp;
137 20 : return;
138 : }
139 20 : case ROTATION_ROLL_270_YAW_45: {
140 20 : tmp = z; z = -y; y = tmp;
141 20 : tmp = HALF_SQRT_2*(ftype)(x - y);
142 20 : y = HALF_SQRT_2*(ftype)(x + y);
143 20 : x = tmp;
144 20 : return;
145 : }
146 20 : case ROTATION_ROLL_270_YAW_90: {
147 20 : tmp = z; z = -y; y = tmp;
148 20 : tmp = x; x = -y; y = tmp;
149 20 : return;
150 : }
151 20 : case ROTATION_ROLL_270_YAW_135: {
152 20 : tmp = z; z = -y; y = tmp;
153 20 : tmp = -HALF_SQRT_2*(ftype)(x + y);
154 20 : y = HALF_SQRT_2*(ftype)(x - y);
155 20 : x = tmp;
156 20 : return;
157 : }
158 874127 : case ROTATION_PITCH_90: {
159 874127 : tmp = z; z = -x; x = tmp;
160 874127 : return;
161 : }
162 1746226759 : case ROTATION_PITCH_270: {
163 1746226759 : tmp = z; z = x; x = -tmp;
164 1746226759 : return;
165 : }
166 20 : case ROTATION_ROLL_90_PITCH_90: {
167 20 : tmp = z; z = y; y = -tmp;
168 20 : tmp = z; z = -x; x = tmp;
169 20 : return;
170 : }
171 20 : case ROTATION_ROLL_180_PITCH_90: {
172 20 : y = -y; z = -z;
173 20 : tmp = z; z = -x; x = tmp;
174 20 : return;
175 : }
176 20 : case ROTATION_ROLL_270_PITCH_90: {
177 20 : tmp = z; z = -y; y = tmp;
178 20 : tmp = z; z = -x; x = tmp;
179 20 : return;
180 : }
181 20 : case ROTATION_ROLL_90_PITCH_180: {
182 20 : tmp = z; z = y; y = -tmp;
183 20 : x = -x; z = -z;
184 20 : return;
185 : }
186 20 : case ROTATION_ROLL_270_PITCH_180: {
187 20 : tmp = z; z = -y; y = tmp;
188 20 : x = -x; z = -z;
189 20 : return;
190 : }
191 20 : case ROTATION_ROLL_90_PITCH_270: {
192 20 : tmp = z; z = y; y = -tmp;
193 20 : tmp = z; z = x; x = -tmp;
194 20 : return;
195 : }
196 20 : case ROTATION_ROLL_180_PITCH_270: {
197 20 : y = -y; z = -z;
198 20 : tmp = z; z = x; x = -tmp;
199 20 : return;
200 : }
201 20 : case ROTATION_ROLL_270_PITCH_270: {
202 20 : tmp = z; z = -y; y = tmp;
203 20 : tmp = z; z = x; x = -tmp;
204 20 : return;
205 : }
206 20 : case ROTATION_ROLL_90_PITCH_180_YAW_90: {
207 20 : tmp = z; z = y; y = -tmp;
208 20 : x = -x; z = -z;
209 20 : tmp = x; x = -y; y = tmp;
210 20 : return;
211 : }
212 20 : case ROTATION_ROLL_90_YAW_270: {
213 20 : tmp = z; z = y; y = -tmp;
214 20 : tmp = x; x = y; y = -tmp;
215 20 : return;
216 : }
217 20 : case ROTATION_ROLL_90_PITCH_68_YAW_293: {
218 20 : T tmpx = x;
219 20 : T tmpy = y;
220 20 : T tmpz = z;
221 20 : x = 0.14303897231223747232853327204793 * tmpx + 0.36877648650320382639478111741482 * tmpy + -0.91844638134308709265241077446262 * tmpz;
222 20 : y = -0.33213277779664740485543461545603 * tmpx + -0.85628942146641884303193137384369 * tmpy + -0.39554550256296522325882847326284 * tmpz;
223 20 : z = -0.93232380121551217122544130688766 * tmpx + 0.36162457008209242248497616856184 * tmpy + 0.00000000000000002214311861220361 * tmpz;
224 20 : return;
225 : }
226 18 : case ROTATION_PITCH_315: {
227 18 : tmp = HALF_SQRT_2*(ftype)(x - z);
228 18 : z = HALF_SQRT_2*(ftype)(x + z);
229 18 : x = tmp;
230 18 : return;
231 : }
232 18 : case ROTATION_ROLL_90_PITCH_315: {
233 18 : tmp = z; z = y; y = -tmp;
234 18 : tmp = HALF_SQRT_2*(ftype)(x - z);
235 18 : z = HALF_SQRT_2*(ftype)(x + z);
236 18 : x = tmp;
237 18 : return;
238 : }
239 20 : case ROTATION_PITCH_7: {
240 : const T sin_pitch = 0.1218693434051474899781908334262; // sinF(pitch);
241 : const T cos_pitch = 0.99254615164132198312785249072476; // cosF(pitch);
242 20 : T tmpx = x;
243 20 : T tmpz = z;
244 20 : x = cos_pitch * tmpx + sin_pitch * tmpz;
245 20 : z = -sin_pitch * tmpx + cos_pitch * tmpz;
246 20 : return;
247 : }
248 20 : case ROTATION_ROLL_45: {
249 20 : tmp = HALF_SQRT_2*(ftype)(y - z);
250 20 : z = HALF_SQRT_2*(ftype)(y + z);
251 20 : y = tmp;
252 20 : return;
253 : }
254 20 : case ROTATION_ROLL_315: {
255 20 : tmp = HALF_SQRT_2*(ftype)(y + z);
256 20 : z = HALF_SQRT_2*(ftype)(z - y);
257 20 : y = tmp;
258 20 : return;
259 : }
260 84 : case ROTATION_CUSTOM_1:
261 : case ROTATION_CUSTOM_2:
262 : #if AP_CUSTOMROTATIONS_ENABLED
263 : // custom rotations not supported on eg. Periph by default
264 84 : AP::custom_rotations().rotate(rotation, *this);
265 84 : return;
266 : #endif
267 : case ROTATION_MAX:
268 : case ROTATION_CUSTOM_OLD:
269 : case ROTATION_CUSTOM_END:
270 : break;
271 : }
272 : // rotation invalid
273 2 : INTERNAL_ERROR(AP_InternalError::error_t::bad_rotation);
274 : }
275 :
276 : template <typename T>
277 225441595 : void Vector3<T>::rotate_inverse(enum Rotation rotation)
278 : {
279 225441595 : Vector3<T> x_vec(1.0f,0.0f,0.0f);
280 225441595 : Vector3<T> y_vec(0.0f,1.0f,0.0f);
281 225441595 : Vector3<T> z_vec(0.0f,0.0f,1.0f);
282 :
283 225441595 : x_vec.rotate(rotation);
284 225441595 : y_vec.rotate(rotation);
285 225441595 : z_vec.rotate(rotation);
286 :
287 225441595 : Matrix3<T> M(
288 : x_vec.x, y_vec.x, z_vec.x,
289 : x_vec.y, y_vec.y, z_vec.y,
290 : x_vec.z, y_vec.z, z_vec.z
291 : );
292 :
293 225441595 : (*this) = M.mul_transpose(*this);
294 225441595 : }
295 :
296 : // rotate vector by angle in radians in xy plane leaving z untouched
297 : template <typename T>
298 103 : void Vector3<T>::rotate_xy(T angle_rad)
299 : {
300 103 : const T cs = cosF(angle_rad);
301 103 : const T sn = sinF(angle_rad);
302 103 : T rx = x * cs - y * sn;
303 103 : T ry = x * sn + y * cs;
304 103 : x = rx;
305 103 : y = ry;
306 103 : }
307 :
308 : // vector cross product
309 : template <typename T>
310 1833271837 : Vector3<T> Vector3<T>::operator %(const Vector3<T> &v) const
311 : {
312 1833271837 : Vector3<T> temp(y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x);
313 1833271812 : return temp;
314 : }
315 :
316 : // dot product
317 : template <typename T>
318 1691599291 : T Vector3<T>::operator *(const Vector3<T> &v) const
319 : {
320 1691599291 : return x*v.x + y*v.y + z*v.z;
321 : }
322 :
323 : template <typename T>
324 3116509581 : T Vector3<T>::length(void) const
325 : {
326 3116509581 : return norm(x, y, z);
327 : }
328 :
329 : // limit xy component vector to a given length. returns true if vector was limited
330 : template <typename T>
331 8 : bool Vector3<T>::limit_length_xy(T max_length)
332 : {
333 8 : const T length_xy = norm(x, y);
334 8 : if ((length_xy > max_length) && is_positive(length_xy)) {
335 4 : x *= (max_length / length_xy);
336 4 : y *= (max_length / length_xy);
337 4 : return true;
338 : }
339 : return false;
340 : }
341 :
342 : template <typename T>
343 977217924 : Vector3<T> &Vector3<T>::operator *=(const T num)
344 : {
345 977217924 : x*=num; y*=num; z*=num;
346 977217924 : return *this;
347 : }
348 :
349 : template <typename T>
350 1123890826 : Vector3<T> &Vector3<T>::operator /=(const T num)
351 : {
352 1123890826 : x /= num; y /= num; z /= num;
353 1123890826 : return *this;
354 : }
355 :
356 : template <typename T>
357 1667221134 : Vector3<T> &Vector3<T>::operator -=(const Vector3<T> &v)
358 : {
359 1667221134 : x -= v.x; y -= v.y; z -= v.z;
360 1667221134 : return *this;
361 : }
362 :
363 : template <typename T>
364 985752500 : bool Vector3<T>::is_nan(void) const
365 : {
366 985752500 : return isnan(x) || isnan(y) || isnan(z);
367 : }
368 :
369 : template <typename T>
370 594963797 : bool Vector3<T>::is_inf(void) const
371 : {
372 594963797 : return isinf(x) || isinf(y) || isinf(z);
373 : }
374 :
375 : template <typename T>
376 12680909757 : Vector3<T> &Vector3<T>::operator +=(const Vector3<T> &v)
377 : {
378 12680909757 : x+=v.x; y+=v.y; z+=v.z;
379 12680909757 : return *this;
380 : }
381 :
382 : template <typename T>
383 2126984717 : Vector3<T> Vector3<T>::operator /(const T num) const
384 : {
385 2126984717 : return Vector3<T>(x/num, y/num, z/num);
386 : }
387 :
388 : template <typename T>
389 22789358052 : Vector3<T> Vector3<T>::operator *(const T num) const
390 : {
391 22789358052 : return Vector3<T>(x*num, y*num, z*num);
392 : }
393 :
394 : template <typename T>
395 4328114327 : Vector3<T> Vector3<T>::operator -(const Vector3<T> &v) const
396 : {
397 4328114327 : return Vector3<T>(x-v.x, y-v.y, z-v.z);
398 : }
399 :
400 : template <typename T>
401 9677350541 : Vector3<T> Vector3<T>::operator +(const Vector3<T> &v) const
402 : {
403 9677350541 : return Vector3<T>(x+v.x, y+v.y, z+v.z);
404 : }
405 :
406 : template <typename T>
407 873101537 : Vector3<T> Vector3<T>::operator -(void) const
408 : {
409 873101537 : return Vector3<T>(-x,-y,-z);
410 : }
411 :
412 : template <typename T>
413 139205986 : bool Vector3<T>::operator ==(const Vector3<T> &v) const
414 : {
415 139205986 : return (is_equal(x,v.x) && is_equal(y,v.y) && is_equal(z,v.z));
416 : }
417 :
418 : template <typename T>
419 19243156 : bool Vector3<T>::operator !=(const Vector3<T> &v) const
420 : {
421 19243156 : return (!is_equal(x,v.x) || !is_equal(y,v.y) || !is_equal(z,v.z));
422 : }
423 :
424 : template <typename T>
425 121862961 : T Vector3<T>::angle(const Vector3<T> &v2) const
426 : {
427 121862961 : const T len = this->length() * v2.length();
428 121862961 : if (len <= 0) {
429 : return 0.0f;
430 : }
431 121862961 : const T cosv = ((*this)*v2) / len;
432 121862961 : if (cosv >= 1 || cosv <= -1) {
433 : return 0.0f;
434 : }
435 41286549 : return acosF(cosv);
436 : }
437 :
438 : // multiplication of transpose by a vector
439 : template <typename T>
440 402 : Vector3<T> Vector3<T>::row_times_mat(const Matrix3<T> &m) const
441 : {
442 804 : return Vector3<T>(*this * m.colx(),
443 402 : *this * m.coly(),
444 402 : *this * m.colz());
445 : }
446 :
447 : // multiply a column vector by a row vector, returning a 3x3 matrix
448 : template <typename T>
449 402 : Matrix3<T> Vector3<T>::mul_rowcol(const Vector3<T> &v2) const
450 : {
451 402 : const Vector3<T> v1 = *this;
452 : return Matrix3<T>(v1.x * v2.x, v1.x * v2.y, v1.x * v2.z,
453 : v1.y * v2.x, v1.y * v2.y, v1.y * v2.z,
454 402 : v1.z * v2.x, v1.z * v2.y, v1.z * v2.z);
455 : }
456 :
457 : // extrapolate position given bearing and pitch (in degrees) and distance
458 : template <typename T>
459 56496 : void Vector3<T>::offset_bearing(T bearing, T pitch, T distance)
460 : {
461 56496 : y += cosF(radians(pitch)) * sinF(radians(bearing)) * distance;
462 56496 : x += cosF(radians(pitch)) * cosF(radians(bearing)) * distance;
463 56496 : z += sinF(radians(pitch)) * distance;
464 56496 : }
465 :
466 : // distance from the tip of this vector to a line segment specified by two vectors
467 : template <typename T>
468 57753 : T Vector3<T>::distance_to_segment(const Vector3<T> &seg_start, const Vector3<T> &seg_end) const
469 : {
470 : // triangle side lengths
471 57753 : const T a = (*this-seg_start).length();
472 57753 : const T b = (seg_start-seg_end).length();
473 57753 : const T c = (seg_end-*this).length();
474 :
475 : // protect against divide by zero later
476 57753 : if (::is_zero(b)) {
477 : return 0.0f;
478 : }
479 :
480 : // semiperimeter of triangle
481 57753 : const T s = (a+b+c) * 0.5f;
482 :
483 57753 : T area_squared = s*(s-a)*(s-b)*(s-c);
484 : // area must be constrained above 0 because a triangle could have 3 points could be on a line and float rounding could push this under 0
485 57753 : if (area_squared < 0.0f) {
486 : area_squared = 0.0f;
487 : }
488 57753 : const T area = safe_sqrt(area_squared);
489 57753 : return 2.0f*area/b;
490 : }
491 :
492 : // Shortest distance between point(p) to a point contained in the line segment defined by w1,w2
493 : template <typename T>
494 2 : T Vector3<T>::closest_distance_between_line_and_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p)
495 : {
496 2 : const Vector3<T> nearest = point_on_line_closest_to_other_point(w1, w2, p);
497 2 : const T dist = (nearest - p).length();
498 2 : return dist;
499 : }
500 :
501 : // Point in the line segment defined by w1,w2 which is closest to point(p)
502 : // this is based on the explanation given here: www.fundza.com/vectors/point2line/index.html
503 : template <typename T>
504 326051 : Vector3<T> Vector3<T>::point_on_line_closest_to_other_point(const Vector3<T> &w1, const Vector3<T> &w2, const Vector3<T> &p)
505 : {
506 326051 : const Vector3<T> line_vec = w2-w1;
507 326051 : const Vector3<T> p_vec = p - w1;
508 :
509 326051 : const T line_vec_len = line_vec.length();
510 : // protection against divide by zero
511 326051 : if(::is_zero(line_vec_len)) {
512 2 : return w1;
513 : }
514 :
515 326049 : const T scale = 1/line_vec_len;
516 326049 : const Vector3<T> unit_vec = line_vec * scale;
517 326049 : const Vector3<T> scaled_p_vec = p_vec * scale;
518 :
519 326049 : T dot_product = unit_vec * scaled_p_vec;
520 326049 : dot_product = constrain_ftype(dot_product,0.0f,1.0f);
521 :
522 326049 : const Vector3<T> closest_point = line_vec * dot_product;
523 326049 : return (closest_point + w1);
524 : }
525 :
526 : // Closest point between two line segments
527 : // This implementation is borrowed from: http://geomalgorithms.com/a07-_distance.html
528 : // INPUT: 4 points corresponding to start and end of two line segments
529 : // OUTPUT: closest point on segment 2, from segment 1, gets passed on reference as "closest_point"
530 : template <typename T>
531 1180 : void Vector3<T>::segment_to_segment_closest_point(const Vector3<T>& seg1_start, const Vector3<T>& seg1_end, const Vector3<T>& seg2_start, const Vector3<T>& seg2_end, Vector3<T>& closest_point)
532 : {
533 : // direction vectors
534 1180 : const Vector3<T> line1 = seg1_end - seg1_start;
535 1180 : const Vector3<T> line2 = seg2_end - seg2_start;
536 :
537 1180 : const Vector3<T> diff = seg1_start - seg2_start;
538 :
539 1180 : const T a = line1*line1;
540 1180 : const T b = line1*line2;
541 1180 : const T c = line2*line2;
542 1180 : const T d = line1*diff;
543 1180 : const T e = line2*diff;
544 :
545 1180 : const T discriminant = (a*c) - (b*b);
546 : T sN, sD = discriminant; // default sD = D >= 0
547 : T tc, tN, tD = discriminant; // tc = tN / tD, default tD = D >= 0
548 :
549 1180 : if (discriminant < FLT_EPSILON) {
550 : sN = 0.0; // force using point seg1_start on line 1
551 : sD = 1.0; // to prevent possible division by 0.0 later
552 : tN = e;
553 : tD = c;
554 : } else {
555 : // get the closest points on the infinite lines
556 987 : sN = (b*e - c*d);
557 987 : tN = (a*e - b*d);
558 987 : if (sN < 0.0) {
559 : // sc < 0 => the s=0 edge is visible
560 : sN = 0.0;
561 : tN = e;
562 : tD = c;
563 516 : } else if (sN > sD) {
564 : // sc > 1 => the s=1 edge is visible
565 : sN = sD;
566 468 : tN = e + b;
567 : tD = c;
568 : }
569 : }
570 :
571 1180 : if (tN < 0.0) {
572 : // tc < 0 => the t=0 edge is visible
573 : tN = 0.0;
574 : // recompute sc for this edge
575 : if (-d < 0.0) {
576 : sN = 0.0;
577 : } else if (-d > a) {
578 : sN = sD;
579 : } else {
580 : sN = -d;
581 : sD = a;
582 : }
583 1031 : } else if (tN > tD) {
584 : // tc > 1 => the t=1 edge is visible
585 : tN = tD;
586 : // recompute sc for this edge
587 : if ((-d + b) < 0.0) {
588 : sN = 0;
589 : } else if ((-d + b) > a) {
590 : sN = sD;
591 : } else {
592 : sN = (-d + b);
593 : sD = a;
594 : }
595 : }
596 : // finally do the division to get tc
597 1180 : tc = (::is_zero(tN) ? 0.0 : tN / tD);
598 :
599 : // closest point on seg2
600 1180 : closest_point = seg2_start + line2*tc;
601 1180 : }
602 :
603 : // Returns true if the passed 3D segment passes through a plane defined by plane normal, and a point on the plane
604 : template <typename T>
605 1176 : bool Vector3<T>::segment_plane_intersect(const Vector3<T>& seg_start, const Vector3<T>& seg_end, const Vector3<T>& plane_normal, const Vector3<T>& plane_point)
606 : {
607 1176 : Vector3<T> u = seg_end - seg_start;
608 1176 : Vector3<T> w = seg_start - plane_point;
609 :
610 1176 : T D = plane_normal * u;
611 1176 : T N = -(plane_normal * w);
612 :
613 1176 : if (::is_zero(D)) {
614 33 : if (::is_zero(N)) {
615 : // segment lies in this plane
616 : return true;
617 : } else {
618 : // does not intersect
619 33 : return false;
620 : }
621 : }
622 1143 : const T sI = N / D;
623 1143 : if (sI < 0 || sI > 1) {
624 : // does not intersect
625 1143 : return false;
626 : }
627 : // intersects at unique point
628 : return true;
629 : }
630 :
631 : // define for float and double
632 : template class Vector3<float>;
633 : template class Vector3<double>;
634 :
635 : // define needed ops for Vector3l, Vector3i as needed
636 : template Vector3<int32_t> &Vector3<int32_t>::operator +=(const Vector3<int32_t> &v);
637 : template bool Vector3<int16_t>::operator ==(const Vector3<int16_t> &v) const;
|
