| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Software License Agreement (BSD License) | ||
| 3 | * | ||
| 4 | * Copyright (c) 2021, INRIA | ||
| 5 | * All rights reserved. | ||
| 6 | * | ||
| 7 | * Redistribution and use in source and binary forms, with or without | ||
| 8 | * modification, are permitted provided that the following conditions | ||
| 9 | * are met: | ||
| 10 | * | ||
| 11 | * * Redistributions of source code must retain the above copyright | ||
| 12 | * notice, this list of conditions and the following disclaimer. | ||
| 13 | * * Redistributions in binary form must reproduce the above | ||
| 14 | * copyright notice, this list of conditions and the following | ||
| 15 | * disclaimer in the documentation and/or other materials provided | ||
| 16 | * with the distribution. | ||
| 17 | * * Neither the name of Open Source Robotics Foundation nor the names of its | ||
| 18 | * contributors may be used to endorse or promote products derived | ||
| 19 | * from this software without specific prior written permission. | ||
| 20 | * | ||
| 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 32 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 33 | */ | ||
| 34 | |||
| 35 | /** \author Justin Carpentier */ | ||
| 36 | |||
| 37 | #define BOOST_TEST_MODULE COAL_HEIGHT_FIELDS | ||
| 38 | #include <boost/test/included/unit_test.hpp> | ||
| 39 | #include <boost/filesystem.hpp> | ||
| 40 | |||
| 41 | #include "fcl_resources/config.h" | ||
| 42 | |||
| 43 | #include "coal/collision.h" | ||
| 44 | #include "coal/hfield.h" | ||
| 45 | #include "coal/math/transform.h" | ||
| 46 | #include "coal/shape/geometric_shapes.h" | ||
| 47 | #include "coal/mesh_loader/assimp.h" | ||
| 48 | #include "coal/mesh_loader/loader.h" | ||
| 49 | |||
| 50 | #include "coal/collision.h" | ||
| 51 | #include "coal/internal/traversal_node_hfield_shape.h" | ||
| 52 | |||
| 53 | #include "utility.h" | ||
| 54 | #include <iostream> | ||
| 55 | |||
| 56 | using namespace coal; | ||
| 57 | using RowVector = Eigen::RowVector<Scalar, Eigen::Dynamic>; | ||
| 58 | |||
| 59 | template <typename BV> | ||
| 60 | 12 | void test_constant_hfields(const Eigen::DenseIndex nx, | |
| 61 | const Eigen::DenseIndex ny, | ||
| 62 | const Scalar min_altitude, | ||
| 63 | const Scalar max_altitude) { | ||
| 64 | 12 | const Scalar x_dim = 1., y_dim = 2.; | |
| 65 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); |
| 66 | |||
| 67 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude); |
| 68 | |||
| 69 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(hfield.getXDim() == x_dim); |
| 70 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(hfield.getYDim() == y_dim); |
| 71 | |||
| 72 | 12 | const VecXs& x_grid = hfield.getXGrid(); | |
| 73 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 6 times.
|
12 | BOOST_CHECK(x_grid[0] == -x_dim / 2.); |
| 74 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 6 times.
|
12 | BOOST_CHECK(x_grid[nx - 1] == x_dim / 2.); |
| 75 | |||
| 76 | 12 | const VecXs& y_grid = hfield.getYGrid(); | |
| 77 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 6 times.
|
12 | BOOST_CHECK(y_grid[0] == y_dim / 2.); |
| 78 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 6 times.
|
12 | BOOST_CHECK(y_grid[ny - 1] == -y_dim / 2.); |
| 79 | |||
| 80 | // Test local AABB | ||
| 81 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | hfield.computeLocalAABB(); |
| 82 | |||
| 83 |
2/2✓ Branch 0 taken 244 times.
✓ Branch 1 taken 6 times.
|
500 | for (Eigen::DenseIndex i = 0; i < nx; ++i) { |
| 84 |
2/2✓ Branch 0 taken 20088 times.
✓ Branch 1 taken 244 times.
|
40664 | for (Eigen::DenseIndex j = 0; j < ny; ++j) { |
| 85 |
4/8✓ Branch 1 taken 20088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20088 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20088 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20088 times.
✗ Branch 11 not taken.
|
40176 | Vec3s point(x_grid[i], y_grid[j], heights(j, i)); |
| 86 |
7/14✓ Branch 1 taken 20088 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 20088 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 20088 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 20088 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 20088 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 20088 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 20088 times.
|
40176 | BOOST_CHECK(hfield.aabb_local.contain(point)); |
| 87 | } | ||
| 88 | } | ||
| 89 | |||
| 90 | // Test clone | ||
| 91 | { | ||
| 92 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | HeightField<BV>* hfield_clone = hfield.clone(); |
| 93 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | hfield_clone->computeLocalAABB(); |
| 94 |
7/14✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 6 times.
|
12 | BOOST_CHECK(*hfield_clone == hfield); |
| 95 | |||
| 96 |
1/2✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
|
12 | delete hfield_clone; |
| 97 | } | ||
| 98 | |||
| 99 | // Build equivalent object | ||
| 100 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); |
| 101 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | const Transform3s box_placement( |
| 102 | Matrix3s::Identity(), | ||
| 103 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Vec3s(0., 0., (max_altitude + min_altitude) / Scalar(2))); |
| 104 | |||
| 105 | // Test collision | ||
| 106 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | const Sphere sphere(1.); |
| 107 |
4/8✓ Branch 0 taken 2 times.
✓ Branch 1 taken 4 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
12 | static const Transform3s IdTransform; |
| 108 | |||
| 109 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | const Box box(Vec3s::Ones()); |
| 110 | |||
| 111 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Transform3s M_sphere, M_box; |
| 112 | |||
| 113 | // No collision case | ||
| 114 | { | ||
| 115 | 12 | const Scalar eps_no_collision = | |
| 116 | 12 | +Scalar(0.1) * (max_altitude - min_altitude); | |
| 117 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_sphere.setTranslation( |
| 118 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
| 119 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_box.setTranslation( |
| 120 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
| 121 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionRequest request; |
| 122 | |||
| 123 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result; |
| 124 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 125 | |||
| 126 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(!result.isCollision()); |
| 127 | |||
| 128 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check_sphere; |
| 129 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 130 | request, result_check_sphere); | ||
| 131 | |||
| 132 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(!result_check_sphere.isCollision()); |
| 133 | |||
| 134 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check_box; |
| 135 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 136 | result_check_box); | ||
| 137 | |||
| 138 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(!result_check_box.isCollision()); |
| 139 | 12 | } | |
| 140 | |||
| 141 | // Collision case | ||
| 142 | { | ||
| 143 | 12 | const Scalar eps_collision = -Scalar(0.1) * (max_altitude - min_altitude); | |
| 144 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_sphere.setTranslation( |
| 145 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); |
| 146 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_box.setTranslation( |
| 147 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); |
| 148 | CollisionRequest | ||
| 149 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); |
| 150 | |||
| 151 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result; |
| 152 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 153 | |||
| 154 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result.isCollision()); |
| 155 | |||
| 156 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check; |
| 157 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 158 | request, result_check); | ||
| 159 | |||
| 160 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result_check.isCollision()); |
| 161 | |||
| 162 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check_box; |
| 163 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 164 | result_check_box); | ||
| 165 | |||
| 166 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result_check_box.isCollision()); |
| 167 | 12 | } | |
| 168 | |||
| 169 | // Update height | ||
| 170 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | hfield.updateHeights( |
| 171 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | MatrixXs::Constant(ny, nx, max_altitude / 2)); // We change nothing |
| 172 | |||
| 173 | // No collision case | ||
| 174 | { | ||
| 175 | 12 | const Scalar eps_no_collision = | |
| 176 | 12 | +Scalar(0.1) * (max_altitude - min_altitude); | |
| 177 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_sphere.setTranslation( |
| 178 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
| 179 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_box.setTranslation( |
| 180 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
| 181 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionRequest request; |
| 182 | |||
| 183 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result; |
| 184 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 185 | |||
| 186 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(!result.isCollision()); |
| 187 | |||
| 188 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check_sphere; |
| 189 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 190 | request, result_check_sphere); | ||
| 191 | |||
| 192 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(!result_check_sphere.isCollision()); |
| 193 | |||
| 194 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check_box; |
| 195 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 196 | result_check_box); | ||
| 197 | |||
| 198 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(!result_check_box.isCollision()); |
| 199 | 12 | } | |
| 200 | |||
| 201 | // Collision case | ||
| 202 | { | ||
| 203 | 12 | const Scalar eps_collision = -Scalar(0.1) * (max_altitude - min_altitude); | |
| 204 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_sphere.setTranslation( |
| 205 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); |
| 206 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_box.setTranslation( |
| 207 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); |
| 208 | CollisionRequest | ||
| 209 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); |
| 210 | |||
| 211 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result; |
| 212 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 213 | |||
| 214 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(!result.isCollision()); |
| 215 | |||
| 216 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check; |
| 217 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 218 | request, result_check); | ||
| 219 | |||
| 220 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result_check.isCollision()); |
| 221 | |||
| 222 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check_box; |
| 223 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 224 | result_check_box); | ||
| 225 | |||
| 226 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result_check_box.isCollision()); |
| 227 | 12 | } | |
| 228 | |||
| 229 | // Restore height | ||
| 230 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | hfield.updateHeights( |
| 231 | MatrixXs::Constant(ny, nx, max_altitude)); // We change nothing | ||
| 232 | |||
| 233 | // Collision case | ||
| 234 | { | ||
| 235 | 12 | const Scalar eps_collision = -Scalar(0.1) * (max_altitude - min_altitude); | |
| 236 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_sphere.setTranslation( |
| 237 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | Vec3s(0., 0., max_altitude + sphere.radius + eps_collision)); |
| 238 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | M_box.setTranslation( |
| 239 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_collision)); |
| 240 | CollisionRequest | ||
| 241 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | request; //(CONTACT | DISTANCE_LOWER_BOUND, (size_t)((nx-1)*(ny-1))); |
| 242 | |||
| 243 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result; |
| 244 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 245 | |||
| 246 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result.isCollision()); |
| 247 | |||
| 248 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check; |
| 249 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 250 | request, result_check); | ||
| 251 | |||
| 252 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result_check.isCollision()); |
| 253 | |||
| 254 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | CollisionResult result_check_box; |
| 255 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 256 | result_check_box); | ||
| 257 | |||
| 258 |
6/12✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 6 times.
|
12 | BOOST_CHECK(result_check_box.isCollision()); |
| 259 | 12 | } | |
| 260 | 12 | } | |
| 261 | |||
| 262 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(building_constant_hfields) { |
| 263 | 2 | const Scalar max_altitude = 1., min_altitude = 0.; | |
| 264 | |||
| 265 | 2 | test_constant_hfields<OBBRSS>(2, 2, min_altitude, | |
| 266 | max_altitude); // Simple case | ||
| 267 | 2 | test_constant_hfields<OBBRSS>(20, 2, min_altitude, max_altitude); | |
| 268 | 2 | test_constant_hfields<OBBRSS>(100, 100, min_altitude, max_altitude); | |
| 269 | // test_constant_hfields<OBBRSS>(1000,1000,min_altitude,max_altitude); | ||
| 270 | |||
| 271 | 2 | test_constant_hfields<AABB>(2, 2, min_altitude, max_altitude); // Simple case | |
| 272 | 2 | test_constant_hfields<AABB>(20, 2, min_altitude, max_altitude); | |
| 273 | 2 | test_constant_hfields<AABB>(100, 100, min_altitude, max_altitude); | |
| 274 | 2 | } | |
| 275 | |||
| 276 | template <typename BV> | ||
| 277 | 1 | void test_negative_security_margin(const Eigen::DenseIndex nx, | |
| 278 | const Eigen::DenseIndex ny, | ||
| 279 | const Scalar min_altitude, | ||
| 280 | const Scalar max_altitude) { | ||
| 281 | 1 | const Scalar x_dim = 1., y_dim = 2.; | |
| 282 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | const MatrixXs heights = MatrixXs::Constant(ny, nx, max_altitude); |
| 283 | |||
| 284 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | HeightField<BV> hfield(x_dim, y_dim, heights, min_altitude); |
| 285 | |||
| 286 | // Build equivalent object | ||
| 287 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); |
| 288 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | const Transform3s box_placement( |
| 289 | Matrix3s::Identity(), | ||
| 290 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vec3s(0, 0, (max_altitude + min_altitude) / Scalar(2))); |
| 291 | |||
| 292 | // Test collision | ||
| 293 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | const Sphere sphere(1.); |
| 294 |
3/8✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
|
1 | static const Transform3s IdTransform; |
| 295 | |||
| 296 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | const Box box(Vec3s::Ones()); |
| 297 | |||
| 298 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Transform3s M_sphere, M_box; |
| 299 | |||
| 300 | // No collision case | ||
| 301 | { | ||
| 302 | 1 | const Scalar eps_no_collision = | |
| 303 | 1 | +Scalar(0.1) * (max_altitude - min_altitude); | |
| 304 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_sphere.setTranslation( |
| 305 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
| 306 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_box.setTranslation( |
| 307 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
| 308 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionRequest request; |
| 309 | |||
| 310 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result; |
| 311 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 312 | |||
| 313 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(!result.isCollision()); |
| 314 | |||
| 315 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_sphere; |
| 316 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 317 | request, result_check_sphere); | ||
| 318 | |||
| 319 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(!result_check_sphere.isCollision()); |
| 320 | |||
| 321 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_box; |
| 322 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 323 | result_check_box); | ||
| 324 | |||
| 325 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(!result_check_box.isCollision()); |
| 326 | 1 | } | |
| 327 | |||
| 328 | // Collision case - positive security_margin | ||
| 329 | { | ||
| 330 | 1 | const Scalar eps_no_collision = | |
| 331 | 1 | +Scalar(0.1) * (max_altitude - min_altitude); | |
| 332 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_sphere.setTranslation( |
| 333 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
| 334 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_box.setTranslation( |
| 335 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
| 336 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionRequest request; |
| 337 | 1 | request.security_margin = eps_no_collision + Scalar(1e-6); | |
| 338 | |||
| 339 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result; |
| 340 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 341 | |||
| 342 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(result.isCollision()); |
| 343 | |||
| 344 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_sphere; |
| 345 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 346 | request, result_check_sphere); | ||
| 347 | |||
| 348 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(result_check_sphere.isCollision()); |
| 349 | |||
| 350 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_box; |
| 351 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 352 | result_check_box); | ||
| 353 | |||
| 354 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(result_check_box.isCollision()); |
| 355 | 1 | } | |
| 356 | |||
| 357 | // Collision case | ||
| 358 | { | ||
| 359 | 1 | const Scalar eps_no_collision = | |
| 360 | 1 | -Scalar(0.1) * (max_altitude - min_altitude); | |
| 361 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_sphere.setTranslation( |
| 362 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
| 363 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_box.setTranslation( |
| 364 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
| 365 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionRequest request; |
| 366 | |||
| 367 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result; |
| 368 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 369 | |||
| 370 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(result.isCollision()); |
| 371 | |||
| 372 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_sphere; |
| 373 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 374 | request, result_check_sphere); | ||
| 375 | |||
| 376 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(result_check_sphere.isCollision()); |
| 377 | |||
| 378 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_box; |
| 379 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 380 | result_check_box); | ||
| 381 | |||
| 382 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(result_check_box.isCollision()); |
| 383 | 1 | } | |
| 384 | |||
| 385 | // No collision case - negative security_margin | ||
| 386 | { | ||
| 387 | 1 | const Scalar eps_no_collision = | |
| 388 | 1 | -Scalar(0.1) * (max_altitude - min_altitude); | |
| 389 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_sphere.setTranslation( |
| 390 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vec3s(0., 0., max_altitude + sphere.radius + eps_no_collision)); |
| 391 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | M_box.setTranslation( |
| 392 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Vec3s(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); |
| 393 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionRequest request; |
| 394 | 1 | request.security_margin = eps_no_collision - Scalar(1e-4); | |
| 395 | |||
| 396 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result; |
| 397 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | collide(&hfield, IdTransform, &sphere, M_sphere, request, result); |
| 398 | |||
| 399 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(!result.isCollision()); |
| 400 | |||
| 401 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_sphere; |
| 402 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, |
| 403 | request, result_check_sphere); | ||
| 404 | |||
| 405 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(!result_check_sphere.isCollision()); |
| 406 | |||
| 407 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionResult result_check_box; |
| 408 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, |
| 409 | result_check_box); | ||
| 410 | |||
| 411 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
1 | BOOST_CHECK(!result_check_box.isCollision()); |
| 412 | 1 | } | |
| 413 | 1 | } | |
| 414 | |||
| 415 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(negative_security_margin) { |
| 416 | 2 | const Scalar max_altitude = 1., min_altitude = 0.; | |
| 417 | |||
| 418 | // test_negative_security_margin<OBBRSS>(100, 100, min_altitude, | ||
| 419 | // max_altitude); | ||
| 420 | 2 | test_negative_security_margin<AABB>(100, 100, min_altitude, max_altitude); | |
| 421 | 2 | } | |
| 422 | |||
| 423 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { |
| 424 | 2 | const Eigen::DenseIndex nx = 100, ny = 100; | |
| 425 | |||
| 426 | typedef AABB BV; | ||
| 427 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const MatrixXs X = RowVector::LinSpaced(nx, -1., 1.).replicate(ny, 1); |
| 428 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const MatrixXs Y = VecXs::LinSpaced(ny, 1., -1.).replicate(1, nx); |
| 429 | |||
| 430 | 2 | const Scalar dim_square = 0.5; | |
| 431 | |||
| 432 | const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole = | ||
| 433 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
|
2 | (X.array().abs() < dim_square) && (Y.array().abs() < dim_square); |
| 434 | |||
| 435 | const MatrixXs heights = | ||
| 436 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | MatrixXs::Ones(ny, nx) - hole.cast<Scalar>().matrix(); |
| 437 | |||
| 438 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const HeightField<BV> hfield(2., 2., heights, -10.); |
| 439 | |||
| 440 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Sphere sphere(Scalar(0.48)); |
| 441 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Transform3s sphere_pos(Vec3s(0, 0, Scalar(0.5))); |
| 442 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 443 | |||
| 444 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const CollisionRequest request; |
| 445 | |||
| 446 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 447 | { | ||
| 448 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 449 | |||
| 450 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 451 | } | ||
| 452 | |||
| 453 | 2 | sphere.radius = Scalar(0.51); | |
| 454 | |||
| 455 | { | ||
| 456 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 457 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Sphere sphere2(Scalar(0.51)); |
| 458 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere2, sphere_pos, request, result); |
| 459 | |||
| 460 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(result.isCollision()); |
| 461 | 2 | } | |
| 462 | 2 | } | |
| 463 | |||
| 464 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { |
| 465 | 2 | const Eigen::DenseIndex nx = 100, ny = 100; | |
| 466 | |||
| 467 | // typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug | ||
| 468 | // mode), as the overlap of OBBRSS is not satisfactory yet. | ||
| 469 | typedef AABB BV; | ||
| 470 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const MatrixXs X = RowVector::LinSpaced(nx, -1., 1.).replicate(ny, 1); |
| 471 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | const MatrixXs Y = VecXs::LinSpaced(ny, 1., -1.).replicate(1, nx); |
| 472 | |||
| 473 | 2 | const Scalar dim_hole = 1; | |
| 474 | |||
| 475 | const Eigen::Array<bool, Eigen::Dynamic, Eigen::Dynamic> hole = | ||
| 476 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
2 | (X.array().square() + Y.array().square() <= dim_hole); |
| 477 | |||
| 478 | const MatrixXs heights = | ||
| 479 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | MatrixXs::Ones(ny, nx) - hole.cast<Scalar>().matrix(); |
| 480 | |||
| 481 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const HeightField<BV> hfield(2., 2., heights, -10.); |
| 482 | |||
| 483 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(hfield.getXGrid()[0] == -1.); |
| 484 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(hfield.getXGrid()[nx - 1] == +1.); |
| 485 | |||
| 486 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(hfield.getYGrid()[0] == +1.); |
| 487 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✗ Branch 26 not taken.
✓ Branch 27 taken 1 times.
|
2 | BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); |
| 488 | |||
| 489 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Sphere sphere(Scalar(0.975)); |
| 490 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Transform3s sphere_pos(Vec3s(0, 0, 1)); |
| 491 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 492 | |||
| 493 | 2 | const Scalar thresholds[3] = {0, Scalar(0.01), Scalar(-0.005)}; | |
| 494 | |||
| 495 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
8 | for (int i = 0; i < 3; ++i) { |
| 496 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | CollisionResult result; |
| 497 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | CollisionRequest request; |
| 498 | 6 | request.security_margin = thresholds[i]; | |
| 499 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 500 | |||
| 501 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 3 times.
|
6 | BOOST_CHECK(!result.isCollision()); |
| 502 | 6 | } | |
| 503 | |||
| 504 | // Increase the size of the sphere to force the collision | ||
| 505 | 2 | sphere.radius = Scalar(1.01); | |
| 506 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
|
8 | for (int i = 0; i < 3; ++i) { |
| 507 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | CollisionResult result; |
| 508 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | CollisionRequest request; |
| 509 | 6 | request.security_margin = thresholds[i]; | |
| 510 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 511 | |||
| 512 |
6/12✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 3 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 3 times.
|
6 | BOOST_CHECK(result.isCollision()); |
| 513 | 6 | } | |
| 514 | |||
| 515 | { | ||
| 516 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 517 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 518 | 2 | request.security_margin = Scalar(-0.02); | |
| 519 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 520 | |||
| 521 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 522 | 2 | } | |
| 523 | 2 | } | |
| 524 | |||
| 525 | 7 | bool isApprox(const Scalar v1, const Scalar v2, | |
| 526 | const Scalar tol = Scalar(1e-6)) { | ||
| 527 | 7 | return std::fabs(v1 - v2) <= tol; | |
| 528 | } | ||
| 529 | |||
| 530 | 17 | Vec3s computeFaceNormal(const Triangle32& triangle, | |
| 531 | const std::vector<Vec3s>& points) { | ||
| 532 |
1/2✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
|
17 | const Vec3s pointA = points[triangle[0]]; |
| 533 |
1/2✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
|
17 | const Vec3s pointB = points[triangle[1]]; |
| 534 |
1/2✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
|
17 | const Vec3s pointC = points[triangle[2]]; |
| 535 | |||
| 536 |
4/8✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
|
17 | return (pointB - pointA).cross(pointC - pointA).normalized(); |
| 537 | } | ||
| 538 | |||
| 539 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_hfield_bin_face_normal_orientation) { |
| 540 | 2 | const Scalar sphere_radius = 1.; | |
| 541 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Sphere sphere(sphere_radius); |
| 542 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXs altitutes(2, 2); |
| 543 | 2 | Scalar altitude_value = 1.; | |
| 544 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | altitutes.fill(altitude_value); |
| 545 | |||
| 546 | typedef AABB BV; | ||
| 547 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | HeightField<BV> hfield(1., 1., altitutes, 0.); |
| 548 | |||
| 549 | 2 | const HeightField<BV>::BVS& nodes = hfield.getNodes(); | |
| 550 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(nodes.size() == 1); |
| 551 | 2 | const HeightField<BV>::Node& node = nodes[0]; | |
| 552 | |||
| 553 | typedef HFNodeBase::FaceOrientation FaceOrientation; | ||
| 554 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::BOTTOM) == |
| 555 | int(FaceOrientation::BOTTOM)); | ||
| 556 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::TOP) == |
| 557 | int(FaceOrientation::TOP)); | ||
| 558 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::NORTH) == |
| 559 | int(FaceOrientation::NORTH)); | ||
| 560 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::SOUTH) == |
| 561 | int(FaceOrientation::SOUTH)); | ||
| 562 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::EAST) == |
| 563 | int(FaceOrientation::EAST)); | ||
| 564 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::WEST) == |
| 565 | int(FaceOrientation::WEST)); | ||
| 566 | |||
| 567 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | ConvexTpl<Triangle32> convex1, convex2; |
| 568 | int convex1_active_faces, convex2_active_faces; | ||
| 569 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | details::buildConvexTriangles(node, hfield, convex1, convex1_active_faces, |
| 570 | convex2, convex2_active_faces); | ||
| 571 | |||
| 572 | // Check face normals for convex1 | ||
| 573 | { | ||
| 574 | 2 | const std::vector<Vec3s>& points = *(convex1.points); | |
| 575 | // BOTTOM | ||
| 576 | { | ||
| 577 | 2 | const Triangle32& triangle = (*(convex1.polygons))[0]; | |
| 578 | |||
| 579 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK( |
| 580 | computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); | ||
| 581 | } | ||
| 582 | |||
| 583 | // TOP | ||
| 584 | { | ||
| 585 | 2 | const Triangle32& triangle = (*(convex1.polygons))[1]; | |
| 586 | |||
| 587 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); |
| 588 | } | ||
| 589 | |||
| 590 | // WEST sides | ||
| 591 | { | ||
| 592 | 2 | const Triangle32& triangle1 = (*(convex1.polygons))[2]; | |
| 593 | 2 | const Triangle32& triangle2 = (*(convex1.polygons))[3]; | |
| 594 | |||
| 595 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK( |
| 596 | computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitX())); | ||
| 597 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK( |
| 598 | computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitX())); | ||
| 599 | } | ||
| 600 | |||
| 601 | // SOUTH-EAST sides | ||
| 602 | { | ||
| 603 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Vec3s south_east_normal = Vec3s(1., -1., 0).normalized(); |
| 604 | |||
| 605 | 2 | const Triangle32& triangle1 = (*(convex1.polygons))[4]; | |
| 606 | 2 | const Triangle32& triangle2 = (*(convex1.polygons))[5]; | |
| 607 | |||
| 608 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK( |
| 609 | computeFaceNormal(triangle1, points).isApprox(south_east_normal)); | ||
| 610 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK( |
| 611 | computeFaceNormal(triangle2, points).isApprox(south_east_normal)); | ||
| 612 | } | ||
| 613 | |||
| 614 | // NORTH sides | ||
| 615 | { | ||
| 616 | 2 | const Triangle32& triangle1 = (*(convex1.polygons))[6]; | |
| 617 | 2 | const Triangle32& triangle2 = (*(convex1.polygons))[7]; | |
| 618 | |||
| 619 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::cout << "computeFaceNormal(triangle1,points): " |
| 620 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
4 | << computeFaceNormal(triangle1, points).transpose() |
| 621 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << std::endl; |
| 622 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK( |
| 623 | computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitY())); | ||
| 624 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK( |
| 625 | computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitY())); | ||
| 626 | } | ||
| 627 | } | ||
| 628 | |||
| 629 | // Check face normals for convex2 | ||
| 630 | { | ||
| 631 | 2 | const std::vector<Vec3s>& points = *(convex2.points); | |
| 632 | |||
| 633 | // BOTTOM | ||
| 634 | { | ||
| 635 | 2 | const Triangle32& triangle = (*(convex2.polygons))[0]; | |
| 636 | |||
| 637 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK( |
| 638 | computeFaceNormal(triangle, points).isApprox(-Vec3s::UnitZ())); | ||
| 639 | } | ||
| 640 | |||
| 641 | // TOP | ||
| 642 | { | ||
| 643 | 2 | const Triangle32& triangle = (*(convex2.polygons))[1]; | |
| 644 | |||
| 645 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(computeFaceNormal(triangle, points).isApprox(Vec3s::UnitZ())); |
| 646 | } | ||
| 647 | |||
| 648 | // SOUTH sides | ||
| 649 | { | ||
| 650 | 2 | const Triangle32& triangle1 = (*(convex2.polygons))[2]; | |
| 651 | 2 | const Triangle32& triangle2 = (*(convex2.polygons))[3]; | |
| 652 | |||
| 653 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK( |
| 654 | computeFaceNormal(triangle1, points).isApprox(-Vec3s::UnitY())); | ||
| 655 |
10/20✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 1 times.
|
2 | BOOST_CHECK( |
| 656 | computeFaceNormal(triangle2, points).isApprox(-Vec3s::UnitY())); | ||
| 657 | } | ||
| 658 | |||
| 659 | // NORTH-WEST sides | ||
| 660 | { | ||
| 661 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Vec3s north_west_normal = Vec3s(-1., 1., 0).normalized(); |
| 662 | |||
| 663 | 2 | const Triangle32& triangle1 = (*(convex2.polygons))[4]; | |
| 664 | 2 | const Triangle32& triangle2 = (*(convex2.polygons))[5]; | |
| 665 | |||
| 666 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK( |
| 667 | computeFaceNormal(triangle1, points).isApprox(north_west_normal)); | ||
| 668 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK( |
| 669 | computeFaceNormal(triangle2, points).isApprox(north_west_normal)); | ||
| 670 | } | ||
| 671 | |||
| 672 | // EAST sides | ||
| 673 | { | ||
| 674 | 2 | const Triangle32& triangle1 = (*(convex2.polygons))[6]; | |
| 675 | 2 | const Triangle32& triangle2 = (*(convex2.polygons))[7]; | |
| 676 | |||
| 677 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK( |
| 678 | computeFaceNormal(triangle1, points).isApprox(Vec3s::UnitX())); | ||
| 679 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK( |
| 680 | computeFaceNormal(triangle2, points).isApprox(Vec3s::UnitX())); | ||
| 681 | } | ||
| 682 | } | ||
| 683 | 2 | } | |
| 684 | |||
| 685 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_hfield_bin_active_faces) { |
| 686 | typedef HFNodeBase::FaceOrientation FaceOrientation; | ||
| 687 | 2 | const Scalar sphere_radius = 1.; | |
| 688 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Sphere sphere(sphere_radius); |
| 689 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXs altitutes(3, 3); |
| 690 | 2 | Scalar altitude_value = 1.; | |
| 691 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | altitutes.fill(altitude_value); |
| 692 | |||
| 693 | typedef AABB BV; | ||
| 694 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | HeightField<BV> hfield(1., 1., altitutes, 0.); |
| 695 | |||
| 696 | 2 | const HeightField<BV>::BVS& nodes = hfield.getNodes(); | |
| 697 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(nodes.size() == 7); |
| 698 | |||
| 699 |
2/2✓ Branch 5 taken 7 times.
✓ Branch 6 taken 1 times.
|
16 | for (const auto& node : nodes) { |
| 700 |
2/2✓ Branch 1 taken 4 times.
✓ Branch 2 taken 3 times.
|
14 | if (node.isLeaf()) { |
| 701 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 4 times.
|
8 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::BOTTOM) == |
| 702 | int(FaceOrientation::BOTTOM)); | ||
| 703 |
6/12✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 4 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 4 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 4 times.
|
8 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::TOP) == |
| 704 | int(FaceOrientation::TOP)); | ||
| 705 | |||
| 706 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
8 | if (node.x_id == 0) |
| 707 |
6/12✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 2 times.
|
4 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::WEST) == |
| 708 | int(FaceOrientation::WEST)); | ||
| 709 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
8 | if (node.y_id == 0) |
| 710 |
6/12✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 2 times.
|
4 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::NORTH) == |
| 711 | int(FaceOrientation::NORTH)); | ||
| 712 | |||
| 713 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
8 | if (node.x_id == 1) |
| 714 |
6/12✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 2 times.
|
4 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::EAST) == |
| 715 | int(FaceOrientation::EAST)); | ||
| 716 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
|
8 | if (node.y_id == 1) |
| 717 |
6/12✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 2 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 2 times.
|
4 | BOOST_CHECK((node.contact_active_faces & FaceOrientation::SOUTH) == |
| 718 | int(FaceOrientation::SOUTH)); | ||
| 719 | } | ||
| 720 | } | ||
| 721 | 2 | } | |
| 722 | |||
| 723 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_hfield_single_bin) { |
| 724 | 2 | const Scalar sphere_radius = 1.; | |
| 725 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Sphere sphere(sphere_radius); |
| 726 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | MatrixXs altitutes(2, 2); |
| 727 | 2 | Scalar altitude_value = 1.; | |
| 728 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | altitutes.fill(altitude_value); |
| 729 | |||
| 730 | typedef AABB BV; | ||
| 731 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | HeightField<BV> hfield(1., 1., altitutes, 0.); |
| 732 | |||
| 733 | 2 | const HeightField<BV>::BVS& nodes = hfield.getNodes(); | |
| 734 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(nodes.size() == 1); |
| 735 | |||
| 736 | // Collision from the TOP | ||
| 737 | { | ||
| 738 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Transform3s sphere_pos(Vec3s(0., 0., 2.)); |
| 739 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 740 | |||
| 741 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 742 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 743 | 2 | request.security_margin = -Scalar(0.005); | |
| 744 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 745 | |||
| 746 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 747 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK( |
| 748 | isApprox(result.distance_lower_bound, -request.security_margin)); | ||
| 749 | 2 | } | |
| 750 | |||
| 751 | // Same, but with a positive margin. | ||
| 752 | { | ||
| 753 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Transform3s sphere_pos(Vec3s(0., 0., 2.)); |
| 754 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 755 | |||
| 756 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 757 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 758 | 2 | request.security_margin = +Scalar(0.005); | |
| 759 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 760 | |||
| 761 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(result.isCollision()); |
| 762 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | if (result.isCollision()) { |
| 763 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Contact& contact = result.getContact(0); |
| 764 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitZ())); |
| 765 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | std::cout << "contact.penetration_depth: " << contact.penetration_depth |
| 766 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << std::endl; |
| 767 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); |
| 768 | } | ||
| 769 | 2 | } | |
| 770 | |||
| 771 | // Collision from the BOTTOM | ||
| 772 | { | ||
| 773 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Transform3s sphere_pos(Vec3s(0., 0., -1.)); |
| 774 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 775 | |||
| 776 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 777 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 778 | 2 | request.security_margin = -Scalar(0.005); | |
| 779 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 780 | |||
| 781 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 782 | 2 | } | |
| 783 | |||
| 784 | { | ||
| 785 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | const Transform3s sphere_pos(Vec3s(0., 0., -1.)); |
| 786 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 787 | |||
| 788 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 789 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 790 | 2 | request.security_margin = +Scalar(0.005); | |
| 791 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 792 | |||
| 793 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(result.isCollision()); |
| 794 | { | ||
| 795 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Contact& contact = result.getContact(0); |
| 796 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitZ())); |
| 797 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | std::cout << "contact.penetration_depth: " << contact.penetration_depth |
| 798 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | << std::endl; |
| 799 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); |
| 800 | } | ||
| 801 | 2 | } | |
| 802 | |||
| 803 | // Collision from the WEST | ||
| 804 | { | ||
| 805 | const Transform3s sphere_pos( | ||
| 806 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); |
| 807 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 808 | |||
| 809 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 810 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 811 | 2 | request.security_margin = -Scalar(0.005); | |
| 812 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 813 | |||
| 814 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 815 | 2 | } | |
| 816 | |||
| 817 | { | ||
| 818 | const Transform3s sphere_pos( | ||
| 819 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(hfield.getXGrid()[0] - sphere_radius, 0., 0.5)); |
| 820 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 821 | |||
| 822 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 823 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 824 | 2 | request.security_margin = +Scalar(0.005); | |
| 825 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 826 | |||
| 827 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(result.isCollision()); |
| 828 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | if (result.isCollision()) { |
| 829 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Contact& contact = result.getContact(0); |
| 830 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitX())); |
| 831 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); |
| 832 | } | ||
| 833 | 2 | } | |
| 834 | |||
| 835 | // Collision from the EAST | ||
| 836 | { | ||
| 837 | const Transform3s sphere_pos( | ||
| 838 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); |
| 839 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 840 | |||
| 841 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 842 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 843 | 2 | request.security_margin = -Scalar(0.005); | |
| 844 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 845 | |||
| 846 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 847 | 2 | } | |
| 848 | |||
| 849 | { | ||
| 850 | const Transform3s sphere_pos( | ||
| 851 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(hfield.getXGrid()[1] + sphere_radius, 0., 0.5)); |
| 852 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 853 | |||
| 854 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 855 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 856 | 2 | request.security_margin = +Scalar(0.005); | |
| 857 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 858 | |||
| 859 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(result.isCollision()); |
| 860 | |||
| 861 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | if (result.isCollision()) { |
| 862 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Contact& contact = result.getContact(0); |
| 863 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitX())); |
| 864 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); |
| 865 | } | ||
| 866 | 2 | } | |
| 867 | |||
| 868 | // Collision from the NORTH | ||
| 869 | { | ||
| 870 | const Transform3s sphere_pos( | ||
| 871 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(0., hfield.getYGrid()[0] + sphere_radius, 0.5)); |
| 872 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 873 | |||
| 874 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 875 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 876 | 2 | request.security_margin = -Scalar(0.005); | |
| 877 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 878 | |||
| 879 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 880 | 2 | } | |
| 881 | |||
| 882 | { | ||
| 883 | const Transform3s sphere_pos( | ||
| 884 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(0, hfield.getYGrid()[0] + sphere_radius, Scalar(0.5))); |
| 885 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 886 | |||
| 887 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 888 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 889 | 2 | request.security_margin = +Scalar(0.005); | |
| 890 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 891 | |||
| 892 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(result.isCollision()); |
| 893 | |||
| 894 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | if (result.isCollision()) { |
| 895 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Contact& contact = result.getContact(0); |
| 896 |
8/16✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✗ Branch 29 not taken.
✓ Branch 30 taken 1 times.
|
2 | BOOST_CHECK(contact.normal.isApprox(Vec3s::UnitY())); |
| 897 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); |
| 898 | } | ||
| 899 | 2 | } | |
| 900 | |||
| 901 | // Collision from the SOUTH | ||
| 902 | { | ||
| 903 | const Transform3s sphere_pos( | ||
| 904 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(0., hfield.getYGrid()[1] - sphere_radius, 0.5)); |
| 905 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 906 | |||
| 907 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 908 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 909 | 2 | request.security_margin = -Scalar(0.005); | |
| 910 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 911 | |||
| 912 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(!result.isCollision()); |
| 913 | 2 | } | |
| 914 | |||
| 915 | { | ||
| 916 | const Transform3s sphere_pos( | ||
| 917 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
2 | Vec3s(0., hfield.getYGrid()[1] - sphere_radius, Scalar(0.5))); |
| 918 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Transform3s hfield_pos; |
| 919 | |||
| 920 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionResult result; |
| 921 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | CollisionRequest request; |
| 922 | 2 | request.security_margin = +Scalar(0.005); | |
| 923 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); |
| 924 | |||
| 925 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(result.isCollision()); |
| 926 | |||
| 927 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | if (result.isCollision()) { |
| 928 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | const Contact& contact = result.getContact(0); |
| 929 |
9/18✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 1 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 1 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✓ Branch 33 taken 1 times.
|
2 | BOOST_CHECK(contact.normal.isApprox(-Vec3s::UnitY())); |
| 930 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
✗ Branch 23 not taken.
✓ Branch 24 taken 1 times.
|
2 | BOOST_CHECK(isApprox(contact.penetration_depth, 0.)); |
| 931 | } | ||
| 932 | 2 | } | |
| 933 | 2 | } | |
| 934 |