129 using Eigen::Vector3d;
130 Vector3d planeNormal = Vector3d::Constant(1.0);
131 Vector3d pointOnPlane = Vector3d::Constant(0.0);
132 Vector3d firstSegmentPoint = Vector3d::Constant(1.0);
133 Vector3d secondSegmentPoint = Vector3d::Constant(-1.0);
134 Vector3d intersectionPoint = Vector3d::Constant(1.0);
135 Vector3d expected = Vector3d::Constant(0.0);
139 pointOnPlane, planeNormal, firstSegmentPoint,
140 secondSegmentPoint, intersectionPoint);
142 BOOST_CHECK(
equals(intersectionPoint, expected));
145 secondSegmentPoint = Vector3d::Constant(0.0);
147 pointOnPlane, planeNormal, firstSegmentPoint,
148 secondSegmentPoint, intersectionPoint);
150 BOOST_CHECK(
equals(intersectionPoint, expected));
153 firstSegmentPoint = Vector3d::Constant(0.0);
154 secondSegmentPoint = Vector3d::Constant(-1.0);
156 pointOnPlane, planeNormal, firstSegmentPoint,
157 secondSegmentPoint, intersectionPoint);
159 BOOST_CHECK(
equals(intersectionPoint, expected));
162 firstSegmentPoint << 0.0, 0.0, -3.0;
163 intersectionPoint << 1.0, 2.0, 3.0;
164 expected = intersectionPoint;
166 pointOnPlane, planeNormal, firstSegmentPoint,
167 secondSegmentPoint, intersectionPoint);
169 BOOST_CHECK(
equals(intersectionPoint, expected));
172 firstSegmentPoint << 0.0, 0.0, 0.0;
173 secondSegmentPoint << 1.0, 1.0, -2.0;
175 pointOnPlane, planeNormal, firstSegmentPoint,
176 secondSegmentPoint, intersectionPoint);
178 BOOST_CHECK(
equals(intersectionPoint, expected));
181 firstSegmentPoint << -2.0, -2.0, -2.0;
182 secondSegmentPoint << -1.0, -1.0, -1.0;
184 pointOnPlane, planeNormal, firstSegmentPoint,
185 secondSegmentPoint, intersectionPoint);
187 BOOST_CHECK(
equals(intersectionPoint, expected));
190 firstSegmentPoint << -1.0, -1.0, -1.0;
191 secondSegmentPoint << -2.0, -2.0, -2.0;
193 pointOnPlane, planeNormal, firstSegmentPoint,
194 secondSegmentPoint, intersectionPoint);
196 BOOST_CHECK(
equals(intersectionPoint, expected));
224 Eigen::Vector2d center2D(0, 0);
225 Eigen::Vector2d sidelengths2D(1, 1);
228 Eigen::Vector2d testPoint2D(2, 2);
230 sidelengths2D, center2D, testPoint2D);
233 testPoint2D << -2.0, -2.0;
235 sidelengths2D, center2D, testPoint2D);
238 testPoint2D << 2.0, -2.0;
240 sidelengths2D, center2D, testPoint2D);
243 testPoint2D << -2.0, 2.0;
245 sidelengths2D, center2D, testPoint2D);
248 testPoint2D << 2.0, 0.0;
250 sidelengths2D, center2D, testPoint2D);
253 testPoint2D << -2.0, 0.0;
255 sidelengths2D, center2D, testPoint2D);
258 testPoint2D << 0.0, 2.0;
260 sidelengths2D, center2D, testPoint2D);
263 testPoint2D << 0.0, -2.0;
265 sidelengths2D, center2D, testPoint2D);
269 testPoint2D << 0.0, 0.0;
271 sidelengths2D, center2D, testPoint2D);
274 testPoint2D << 0.25, 0.25;
276 sidelengths2D, center2D, testPoint2D);
279 testPoint2D << -0.25, 0.25;
281 sidelengths2D, center2D, testPoint2D);
284 testPoint2D << 0.25, -0.25;
286 sidelengths2D, center2D, testPoint2D);
289 testPoint2D << -0.25, -0.25;
291 sidelengths2D, center2D, testPoint2D);
294 testPoint2D << 0.49999999999, 0.49999999999;
296 sidelengths2D, center2D, testPoint2D);
299 testPoint2D << -0.49999999999, -0.49999999999;
301 sidelengths2D, center2D, testPoint2D);
305 testPoint2D << 0.5, 0.5;
307 sidelengths2D, center2D, testPoint2D);
310 testPoint2D << -0.5, 0.5;
312 sidelengths2D, center2D, testPoint2D);
315 testPoint2D << 0.5, -0.5;
317 sidelengths2D, center2D, testPoint2D);
320 testPoint2D << -0.5, -0.5;
322 sidelengths2D, center2D, testPoint2D);
325 testPoint2D << 0.4999999999999999, 0.4999999999999999;
327 sidelengths2D, center2D, testPoint2D);
330 testPoint2D << 0.500000000000001, 0.500000000000001;
332 sidelengths2D, center2D, testPoint2D);
335 testPoint2D << -0.500000000000001, -0.500000000000001;
337 sidelengths2D, center2D, testPoint2D);
340 testPoint2D << 0.5, 0.0;
342 sidelengths2D, center2D, testPoint2D);
345 testPoint2D << -0.5, 0.0;
347 sidelengths2D, center2D, testPoint2D);
350 testPoint2D << 0.0, 0.5;
352 sidelengths2D, center2D, testPoint2D);
355 testPoint2D << 0.0, -0.5;
357 sidelengths2D, center2D, testPoint2D);
360 testPoint2D << 0.5, 0.0;
362 sidelengths2D, center2D, testPoint2D);
365 testPoint2D << -0.5, 0.0;
367 sidelengths2D, center2D, testPoint2D);
370 testPoint2D << 0.0, 0.499999999999999;
372 sidelengths2D, center2D, testPoint2D);
375 testPoint2D << 0.0, -0.499999999999999;
377 sidelengths2D, center2D, testPoint2D);
380 testPoint2D << 0.0, 0.500000000000001;
382 sidelengths2D, center2D, testPoint2D);
385 testPoint2D << 0.0, -0.500000000000001;
387 sidelengths2D, center2D, testPoint2D);
390 testPoint2D << 0.499999999999999, 0.0;
392 sidelengths2D, center2D, testPoint2D);
395 testPoint2D << -0.499999999999999, 0.0;
397 sidelengths2D, center2D, testPoint2D);
400 testPoint2D << 0.500000000000001, 0.0;
402 sidelengths2D, center2D, testPoint2D);
405 testPoint2D << -0.500000000000001, 0.0;
407 sidelengths2D, center2D, testPoint2D);
411 Eigen::Vector3d center3D = Eigen::Vector3d::Zero();
412 Eigen::Vector3d sidelengths3D = Eigen::Vector3d::Constant(1.0);
415 Eigen::Vector3d testPoint3D(2, 2, 2);
417 sidelengths3D, center3D, testPoint3D);
420 testPoint3D << -2.0, -2.0, -2.0;
422 sidelengths3D, center3D, testPoint3D);
425 testPoint3D << 2.0, -2.0, 2.0;
427 sidelengths3D, center3D, testPoint3D);
430 testPoint3D << -2.0, 2.0, 2.0;
432 sidelengths3D, center3D, testPoint3D);
435 testPoint3D << 2.0, 0.0, 0.0;
437 sidelengths3D, center3D, testPoint3D);
440 testPoint3D << -2.0, 0.0, 0.0;
442 sidelengths3D, center3D, testPoint3D);
445 testPoint3D << 0.0, 2.0, 0.0;
447 sidelengths3D, center3D, testPoint3D);
450 testPoint3D << 0.0, 0.0, 2.0;
452 sidelengths3D, center3D, testPoint3D);
455 testPoint3D << 0.0, 0.0, -2.0;
457 sidelengths3D, center3D, testPoint3D);
461 testPoint3D << 0.0, 0.0, 0.0;
463 sidelengths3D, center3D, testPoint3D);
466 testPoint3D << 0.25, 0.25, 0.25;
468 sidelengths3D, center3D, testPoint3D);
471 testPoint3D << -0.25, 0.25, 0.25;
473 sidelengths3D, center3D, testPoint3D);
476 testPoint3D << 0.25, -0.25, 0.25;
478 sidelengths3D, center3D, testPoint3D);
481 testPoint3D << -0.25, -0.25, 0.25;
483 sidelengths3D, center3D, testPoint3D);
486 testPoint3D << 0.25, 0.25, -0.25;
488 sidelengths3D, center3D, testPoint3D);
491 testPoint3D << 0.49999999999, 0.49999999999, 0.49999999999;
493 sidelengths3D, center3D, testPoint3D);
496 testPoint3D << -0.49999999999, -0.49999999999, -0.49999999999;
498 sidelengths3D, center3D, testPoint3D);
502 testPoint3D << 0.5, 0.5, 0.5;
504 sidelengths3D, center3D, testPoint3D);
507 testPoint3D << -0.5, 0.5, 0.5;
509 sidelengths3D, center3D, testPoint3D);
512 testPoint3D << 0.5, -0.5, 0.5;
514 sidelengths3D, center3D, testPoint3D);
517 testPoint3D << -0.5, -0.5, -0.5;
519 sidelengths3D, center3D, testPoint3D);
522 testPoint3D << 0.4999999999999999, 0.4999999999999999, 0.4999999999999999;
524 sidelengths3D, center3D, testPoint3D);
527 testPoint3D << 0.500000000000001, 0.500000000000001, 0.500000000000001;
529 sidelengths3D, center3D, testPoint3D);
532 testPoint3D << -0.500000000000001, -0.500000000000001, -0.500000000000001;
534 sidelengths3D, center3D, testPoint3D);
537 testPoint3D << 0.5, 0.0, 0.0;
539 sidelengths3D, center3D, testPoint3D);
542 testPoint3D << -0.5, 0.0, 0.0;
544 sidelengths3D, center3D, testPoint3D);
547 testPoint3D << 0.0, 0.5, 0.0;
549 sidelengths3D, center3D, testPoint3D);
552 testPoint3D << 0.0, -0.5, 0.0;
554 sidelengths3D, center3D, testPoint3D);
557 testPoint3D << 0.5, 0.0, -0.5;
559 sidelengths3D, center3D, testPoint3D);
562 testPoint3D << -0.5, 0.0, 0.5;
564 sidelengths3D, center3D, testPoint3D);
567 testPoint3D << 0.0, 0.499999999999999, 0.0;
569 sidelengths3D, center3D, testPoint3D);
572 testPoint3D << 0.0, -0.499999999999999, 0.0;
574 sidelengths3D, center3D, testPoint3D);
577 testPoint3D << 0.0, 0.500000000000001, 0.0;
579 sidelengths3D, center3D, testPoint3D);
582 testPoint3D << 0.0, -0.500000000000001, 0.0;
584 sidelengths3D, center3D, testPoint3D);
587 testPoint3D << 0.499999999999999, 0.0, 0.0;
589 sidelengths3D, center3D, testPoint3D);
592 testPoint3D << -0.499999999999999, 0.0, 0.0;
594 sidelengths3D, center3D, testPoint3D);
597 testPoint3D << 0.500000000000001, 0.0, 0.0;
599 sidelengths3D, center3D, testPoint3D);
602 testPoint3D << -0.500000000000001, 0.0, 0.0;
604 sidelengths3D, center3D, testPoint3D);
607 testPoint3D << 0.0, 0.0, 0.499999999999999;
609 sidelengths3D, center3D, testPoint3D);
612 testPoint3D << 0.0, 0.0, -0.499999999999999;
614 sidelengths3D, center3D, testPoint3D);
617 testPoint3D << 0.0, 0.0, 0.500000000000001;
619 sidelengths3D, center3D, testPoint3D);
622 testPoint3D << 0.0, 0.0, -0.500000000000001;
624 sidelengths3D, center3D, testPoint3D);