124 using Eigen::Vector3d;
125 Vector3d planeNormal = Vector3d::Constant(1.0);
126 Vector3d pointOnPlane = Vector3d::Constant(0.0);
127 Vector3d firstSegmentPoint = Vector3d::Constant(1.0);
128 Vector3d secondSegmentPoint = Vector3d::Constant(-1.0);
129 Vector3d intersectionPoint = Vector3d::Constant(1.0);
130 Vector3d expected = Vector3d::Constant(0.0);
134 pointOnPlane, planeNormal, firstSegmentPoint,
135 secondSegmentPoint, intersectionPoint);
137 BOOST_CHECK(
equals(intersectionPoint, expected));
140 secondSegmentPoint = Vector3d::Constant(0.0);
142 pointOnPlane, planeNormal, firstSegmentPoint,
143 secondSegmentPoint, intersectionPoint);
145 BOOST_CHECK(
equals(intersectionPoint, expected));
148 firstSegmentPoint = Vector3d::Constant(0.0);
149 secondSegmentPoint = Vector3d::Constant(-1.0);
151 pointOnPlane, planeNormal, firstSegmentPoint,
152 secondSegmentPoint, intersectionPoint);
154 BOOST_CHECK(
equals(intersectionPoint, expected));
157 firstSegmentPoint << 0.0, 0.0, -3.0;
158 intersectionPoint << 1.0, 2.0, 3.0;
159 expected = intersectionPoint;
161 pointOnPlane, planeNormal, firstSegmentPoint,
162 secondSegmentPoint, intersectionPoint);
164 BOOST_CHECK(
equals(intersectionPoint, expected));
167 firstSegmentPoint << 0.0, 0.0, 0.0;
168 secondSegmentPoint << 1.0, 1.0, -2.0;
170 pointOnPlane, planeNormal, firstSegmentPoint,
171 secondSegmentPoint, intersectionPoint);
173 BOOST_CHECK(
equals(intersectionPoint, expected));
176 firstSegmentPoint << -2.0, -2.0, -2.0;
177 secondSegmentPoint << -1.0, -1.0, -1.0;
179 pointOnPlane, planeNormal, firstSegmentPoint,
180 secondSegmentPoint, intersectionPoint);
182 BOOST_CHECK(
equals(intersectionPoint, expected));
185 firstSegmentPoint << -1.0, -1.0, -1.0;
186 secondSegmentPoint << -2.0, -2.0, -2.0;
188 pointOnPlane, planeNormal, firstSegmentPoint,
189 secondSegmentPoint, intersectionPoint);
191 BOOST_CHECK(
equals(intersectionPoint, expected));
217 Eigen::Vector2d center2D(0, 0);
218 Eigen::Vector2d sidelengths2D(1, 1);
221 Eigen::Vector2d testPoint2D(2, 2);
223 sidelengths2D, center2D, testPoint2D);
226 testPoint2D << -2.0, -2.0;
228 sidelengths2D, center2D, testPoint2D);
231 testPoint2D << 2.0, -2.0;
233 sidelengths2D, center2D, testPoint2D);
236 testPoint2D << -2.0, 2.0;
238 sidelengths2D, center2D, testPoint2D);
241 testPoint2D << 2.0, 0.0;
243 sidelengths2D, center2D, testPoint2D);
246 testPoint2D << -2.0, 0.0;
248 sidelengths2D, center2D, testPoint2D);
251 testPoint2D << 0.0, 2.0;
253 sidelengths2D, center2D, testPoint2D);
256 testPoint2D << 0.0, -2.0;
258 sidelengths2D, center2D, testPoint2D);
262 testPoint2D << 0.0, 0.0;
264 sidelengths2D, center2D, testPoint2D);
267 testPoint2D << 0.25, 0.25;
269 sidelengths2D, center2D, testPoint2D);
272 testPoint2D << -0.25, 0.25;
274 sidelengths2D, center2D, testPoint2D);
277 testPoint2D << 0.25, -0.25;
279 sidelengths2D, center2D, testPoint2D);
282 testPoint2D << -0.25, -0.25;
284 sidelengths2D, center2D, testPoint2D);
287 testPoint2D << 0.49999999999, 0.49999999999;
289 sidelengths2D, center2D, testPoint2D);
292 testPoint2D << -0.49999999999, -0.49999999999;
294 sidelengths2D, center2D, testPoint2D);
298 testPoint2D << 0.5, 0.5;
300 sidelengths2D, center2D, testPoint2D);
303 testPoint2D << -0.5, 0.5;
305 sidelengths2D, center2D, testPoint2D);
308 testPoint2D << 0.5, -0.5;
310 sidelengths2D, center2D, testPoint2D);
313 testPoint2D << -0.5, -0.5;
315 sidelengths2D, center2D, testPoint2D);
318 testPoint2D << 0.4999999999999999, 0.4999999999999999;
320 sidelengths2D, center2D, testPoint2D);
323 testPoint2D << 0.500000000000001, 0.500000000000001;
325 sidelengths2D, center2D, testPoint2D);
328 testPoint2D << -0.500000000000001, -0.500000000000001;
330 sidelengths2D, center2D, testPoint2D);
333 testPoint2D << 0.5, 0.0;
335 sidelengths2D, center2D, testPoint2D);
338 testPoint2D << -0.5, 0.0;
340 sidelengths2D, center2D, testPoint2D);
343 testPoint2D << 0.0, 0.5;
345 sidelengths2D, center2D, testPoint2D);
348 testPoint2D << 0.0, -0.5;
350 sidelengths2D, center2D, testPoint2D);
353 testPoint2D << 0.5, 0.0;
355 sidelengths2D, center2D, testPoint2D);
358 testPoint2D << -0.5, 0.0;
360 sidelengths2D, center2D, testPoint2D);
363 testPoint2D << 0.0, 0.499999999999999;
365 sidelengths2D, center2D, testPoint2D);
368 testPoint2D << 0.0, -0.499999999999999;
370 sidelengths2D, center2D, testPoint2D);
373 testPoint2D << 0.0, 0.500000000000001;
375 sidelengths2D, center2D, testPoint2D);
378 testPoint2D << 0.0, -0.500000000000001;
380 sidelengths2D, center2D, testPoint2D);
383 testPoint2D << 0.499999999999999, 0.0;
385 sidelengths2D, center2D, testPoint2D);
388 testPoint2D << -0.499999999999999, 0.0;
390 sidelengths2D, center2D, testPoint2D);
393 testPoint2D << 0.500000000000001, 0.0;
395 sidelengths2D, center2D, testPoint2D);
398 testPoint2D << -0.500000000000001, 0.0;
400 sidelengths2D, center2D, testPoint2D);
404 Eigen::Vector3d center3D = Eigen::Vector3d::Zero();
405 Eigen::Vector3d sidelengths3D = Eigen::Vector3d::Constant(1.0);
408 Eigen::Vector3d testPoint3D(2, 2, 2);
410 sidelengths3D, center3D, testPoint3D);
413 testPoint3D << -2.0, -2.0, -2.0;
415 sidelengths3D, center3D, testPoint3D);
418 testPoint3D << 2.0, -2.0, 2.0;
420 sidelengths3D, center3D, testPoint3D);
423 testPoint3D << -2.0, 2.0, 2.0;
425 sidelengths3D, center3D, testPoint3D);
428 testPoint3D << 2.0, 0.0, 0.0;
430 sidelengths3D, center3D, testPoint3D);
433 testPoint3D << -2.0, 0.0, 0.0;
435 sidelengths3D, center3D, testPoint3D);
438 testPoint3D << 0.0, 2.0, 0.0;
440 sidelengths3D, center3D, testPoint3D);
443 testPoint3D << 0.0, 0.0, 2.0;
445 sidelengths3D, center3D, testPoint3D);
448 testPoint3D << 0.0, 0.0, -2.0;
450 sidelengths3D, center3D, testPoint3D);
454 testPoint3D << 0.0, 0.0, 0.0;
456 sidelengths3D, center3D, testPoint3D);
459 testPoint3D << 0.25, 0.25, 0.25;
461 sidelengths3D, center3D, testPoint3D);
464 testPoint3D << -0.25, 0.25, 0.25;
466 sidelengths3D, center3D, testPoint3D);
469 testPoint3D << 0.25, -0.25, 0.25;
471 sidelengths3D, center3D, testPoint3D);
474 testPoint3D << -0.25, -0.25, 0.25;
476 sidelengths3D, center3D, testPoint3D);
479 testPoint3D << 0.25, 0.25, -0.25;
481 sidelengths3D, center3D, testPoint3D);
484 testPoint3D << 0.49999999999, 0.49999999999, 0.49999999999;
486 sidelengths3D, center3D, testPoint3D);
489 testPoint3D << -0.49999999999, -0.49999999999, -0.49999999999;
491 sidelengths3D, center3D, testPoint3D);
495 testPoint3D << 0.5, 0.5, 0.5;
497 sidelengths3D, center3D, testPoint3D);
500 testPoint3D << -0.5, 0.5, 0.5;
502 sidelengths3D, center3D, testPoint3D);
505 testPoint3D << 0.5, -0.5, 0.5;
507 sidelengths3D, center3D, testPoint3D);
510 testPoint3D << -0.5, -0.5, -0.5;
512 sidelengths3D, center3D, testPoint3D);
515 testPoint3D << 0.4999999999999999, 0.4999999999999999, 0.4999999999999999;
517 sidelengths3D, center3D, testPoint3D);
520 testPoint3D << 0.500000000000001, 0.500000000000001, 0.500000000000001;
522 sidelengths3D, center3D, testPoint3D);
525 testPoint3D << -0.500000000000001, -0.500000000000001, -0.500000000000001;
527 sidelengths3D, center3D, testPoint3D);
530 testPoint3D << 0.5, 0.0, 0.0;
532 sidelengths3D, center3D, testPoint3D);
535 testPoint3D << -0.5, 0.0, 0.0;
537 sidelengths3D, center3D, testPoint3D);
540 testPoint3D << 0.0, 0.5, 0.0;
542 sidelengths3D, center3D, testPoint3D);
545 testPoint3D << 0.0, -0.5, 0.0;
547 sidelengths3D, center3D, testPoint3D);
550 testPoint3D << 0.5, 0.0, -0.5;
552 sidelengths3D, center3D, testPoint3D);
555 testPoint3D << -0.5, 0.0, 0.5;
557 sidelengths3D, center3D, testPoint3D);
560 testPoint3D << 0.0, 0.499999999999999, 0.0;
562 sidelengths3D, center3D, testPoint3D);
565 testPoint3D << 0.0, -0.499999999999999, 0.0;
567 sidelengths3D, center3D, testPoint3D);
570 testPoint3D << 0.0, 0.500000000000001, 0.0;
572 sidelengths3D, center3D, testPoint3D);
575 testPoint3D << 0.0, -0.500000000000001, 0.0;
577 sidelengths3D, center3D, testPoint3D);
580 testPoint3D << 0.499999999999999, 0.0, 0.0;
582 sidelengths3D, center3D, testPoint3D);
585 testPoint3D << -0.499999999999999, 0.0, 0.0;
587 sidelengths3D, center3D, testPoint3D);
590 testPoint3D << 0.500000000000001, 0.0, 0.0;
592 sidelengths3D, center3D, testPoint3D);
595 testPoint3D << -0.500000000000001, 0.0, 0.0;
597 sidelengths3D, center3D, testPoint3D);
600 testPoint3D << 0.0, 0.0, 0.499999999999999;
602 sidelengths3D, center3D, testPoint3D);
605 testPoint3D << 0.0, 0.0, -0.499999999999999;
607 sidelengths3D, center3D, testPoint3D);
610 testPoint3D << 0.0, 0.0, 0.500000000000001;
612 sidelengths3D, center3D, testPoint3D);
615 testPoint3D << 0.0, 0.0, -0.500000000000001;
617 sidelengths3D, center3D, testPoint3D);