| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Software License Agreement (BSD License) | ||
| 3 | * | ||
| 4 | * Copyright (c) 2014-2016, CNRS-LAAS | ||
| 5 | * Copyright (c) 2023, Inria | ||
| 6 | * Author: Florent Lamiraux | ||
| 7 | * All rights reserved. | ||
| 8 | * | ||
| 9 | * Redistribution and use in source and binary forms, with or without | ||
| 10 | * modification, are permitted provided that the following conditions | ||
| 11 | * are met: | ||
| 12 | * | ||
| 13 | * * Redistributions of source code must retain the above copyright | ||
| 14 | * notice, this list of conditions and the following disclaimer. | ||
| 15 | * * Redistributions in binary form must reproduce the above | ||
| 16 | * copyright notice, this list of conditions and the following | ||
| 17 | * disclaimer in the documentation and/or other materials provided | ||
| 18 | * with the distribution. | ||
| 19 | * * Neither the name of CNRS nor the names of its | ||
| 20 | * contributors may be used to endorse or promote products derived | ||
| 21 | * from this software without specific prior written permission. | ||
| 22 | * | ||
| 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
| 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
| 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
| 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
| 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
| 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
| 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
| 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
| 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
| 34 | * POSSIBILITY OF SUCH DAMAGE. | ||
| 35 | */ | ||
| 36 | |||
| 37 | #include <iostream> | ||
| 38 | #include <fstream> | ||
| 39 | #include <sstream> | ||
| 40 | |||
| 41 | #include <chrono> | ||
| 42 | |||
| 43 | #include "coal/narrowphase/narrowphase.h" | ||
| 44 | |||
| 45 | #include "../src/BV/OBB.h" | ||
| 46 | #include "coal/internal/shape_shape_func.h" | ||
| 47 | #include "utility.h" | ||
| 48 | |||
| 49 | using namespace coal; | ||
| 50 | |||
| 51 | 10 | void randomOBBs(Vec3s& a, Vec3s& b, Scalar extentNorm) { | |
| 52 | // Extent norm is between 0 and extentNorm on each axis | ||
| 53 | // a = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); | ||
| 54 | // b = (Vec3s::Ones()+Vec3s::Random()) * extentNorm / (2*sqrt(3)); | ||
| 55 | |||
| 56 |
5/10✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
|
10 | a = extentNorm * Vec3s::Random().cwiseAbs().normalized(); |
| 57 |
5/10✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
|
10 | b = extentNorm * Vec3s::Random().cwiseAbs().normalized(); |
| 58 | 10 | } | |
| 59 | |||
| 60 | 100 | void randomTransform(Matrix3s& B, Vec3s& T, const Vec3s& a, const Vec3s& b, | |
| 61 | const Scalar extentNorm) { | ||
| 62 | // TODO Should we scale T to a and b norm ? | ||
| 63 | (void)a; | ||
| 64 | (void)b; | ||
| 65 | (void)extentNorm; | ||
| 66 | |||
| 67 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
100 | Scalar N = a.norm() + b.norm(); |
| 68 | // A translation of norm N ensures there is no collision. | ||
| 69 | // Set translation to be between 0 and 2 * N; | ||
| 70 |
5/10✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 100 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 100 times.
✗ Branch 15 not taken.
|
100 | T = (Vec3s::Random() / sqrt(3)) * 1.5 * N; |
| 71 | // T.setZero(); | ||
| 72 | |||
| 73 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | Quats q; |
| 74 |
1/2✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
|
100 | q.coeffs().setRandom(); |
| 75 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | q.normalize(); |
| 76 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | B = q; |
| 77 | 100 | } | |
| 78 | |||
| 79 | #define NB_METHODS 7 | ||
| 80 | #define MANUAL_PRODUCT 1 | ||
| 81 | |||
| 82 | #if MANUAL_PRODUCT | ||
| 83 | #define PRODUCT(M33, v3) \ | ||
| 84 | (M33.col(0) * v3[0] + M33.col(1) * v3[1] + M33.col(2) * v3[2]) | ||
| 85 | #else | ||
| 86 | #define PRODUCT(M33, v3) (M33 * v3) | ||
| 87 | #endif | ||
| 88 | |||
| 89 | typedef std::chrono::high_resolution_clock clock_type; | ||
| 90 | typedef clock_type::duration duration_type; | ||
| 91 | |||
| 92 | const char* sep = ",\t"; | ||
| 93 | const Scalar eps = Scalar(1.5e-7); | ||
| 94 | |||
| 95 | const Eigen::IOFormat py_fmt(Eigen::FullPrecision, 0, | ||
| 96 | ", ", // Coeff separator | ||
| 97 | ",\n", // Row separator | ||
| 98 | "(", // row prefix | ||
| 99 | ",)", // row suffix | ||
| 100 | "( ", // mat prefix | ||
| 101 | ", )" // mat suffix | ||
| 102 | ); | ||
| 103 | |||
| 104 | namespace obbDisjoint_impls { | ||
| 105 | /// @return true if OBB are disjoint. | ||
| 106 | 100 | bool distance(const Matrix3s& B, const Vec3s& T, const Vec3s& a, const Vec3s& b, | |
| 107 | Scalar& distance) { | ||
| 108 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | GJKSolver gjk; |
| 109 |
6/12✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
|
100 | Box ba(2 * a), bb(2 * b); |
| 110 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
100 | Transform3s tfa, tfb(B, T); |
| 111 | |||
| 112 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | Vec3s p1, p2, normal; |
| 113 | 100 | bool compute_penetration = true; | |
| 114 | 100 | distance = | |
| 115 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | gjk.shapeDistance(ba, tfa, bb, tfb, compute_penetration, p1, p2, normal); |
| 116 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
200 | return (distance > gjk.getDistancePrecision(compute_penetration)); |
| 117 | 100 | } | |
| 118 | |||
| 119 | 4100 | inline Scalar _computeDistanceForCase1(const Vec3s& T, const Vec3s& a, | |
| 120 | const Vec3s& b, const Matrix3s& Bf) { | ||
| 121 |
1/2✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
|
4100 | Vec3s AABB_corner; |
| 122 | /* This seems to be slower | ||
| 123 | AABB_corner.noalias() = T.cwiseAbs () - a; | ||
| 124 | AABB_corner.noalias() -= PRODUCT(Bf,b); | ||
| 125 | /*/ | ||
| 126 | #if MANUAL_PRODUCT | ||
| 127 |
4/8✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
|
4100 | AABB_corner.noalias() = T.cwiseAbs() - a; |
| 128 |
5/10✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4100 times.
✗ Branch 14 not taken.
|
4100 | AABB_corner.noalias() -= Bf.col(0) * b[0]; |
| 129 |
5/10✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4100 times.
✗ Branch 14 not taken.
|
4100 | AABB_corner.noalias() -= Bf.col(1) * b[1]; |
| 130 |
5/10✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 4100 times.
✗ Branch 14 not taken.
|
4100 | AABB_corner.noalias() -= Bf.col(2) * b[2]; |
| 131 | #else | ||
| 132 | AABB_corner.noalias() = T.cwiseAbs() - Bf * b - a; | ||
| 133 | #endif | ||
| 134 | // */ | ||
| 135 |
4/8✓ Branch 1 taken 4100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4100 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4100 times.
✗ Branch 11 not taken.
|
4100 | return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm(); |
| 136 | } | ||
| 137 | |||
| 138 | 1435 | inline Scalar _computeDistanceForCase2(const Matrix3s& B, const Vec3s& T, | |
| 139 | const Vec3s& a, const Vec3s& b, | ||
| 140 | const Matrix3s& Bf) { | ||
| 141 | /* | ||
| 142 | Vec3s AABB_corner(PRODUCT(B.transpose(), T).cwiseAbs() - b); | ||
| 143 | AABB_corner.noalias() -= PRODUCT(Bf.transpose(), a); | ||
| 144 | return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm (); | ||
| 145 | /*/ | ||
| 146 | #if MANUAL_PRODUCT | ||
| 147 | 1435 | Scalar s, t = 0; | |
| 148 |
5/10✓ Branch 1 taken 1435 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1435 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1435 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1435 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1435 times.
✗ Branch 15 not taken.
|
1435 | s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0]; |
| 149 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1435 times.
|
1435 | if (s > 0) t += s * s; |
| 150 |
5/10✓ Branch 1 taken 1435 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1435 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1435 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1435 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1435 times.
✗ Branch 15 not taken.
|
1435 | s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1]; |
| 151 |
2/2✓ Branch 0 taken 82 times.
✓ Branch 1 taken 1353 times.
|
1435 | if (s > 0) t += s * s; |
| 152 |
5/10✓ Branch 1 taken 1435 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1435 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1435 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1435 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1435 times.
✗ Branch 15 not taken.
|
1435 | s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2]; |
| 153 |
2/2✓ Branch 0 taken 82 times.
✓ Branch 1 taken 1353 times.
|
1435 | if (s > 0) t += s * s; |
| 154 | 1435 | return t; | |
| 155 | #else | ||
| 156 | Vec3s AABB_corner((B.transpose() * T).cwiseAbs() - Bf.transpose() * a - b); | ||
| 157 | return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm(); | ||
| 158 | #endif | ||
| 159 | // */ | ||
| 160 | } | ||
| 161 | |||
| 162 | 100 | int separatingAxisId(const Matrix3s& B, const Vec3s& T, const Vec3s& a, | |
| 163 | const Vec3s& b, const Scalar& breakDistance2, | ||
| 164 | Scalar& squaredLowerBoundDistance) { | ||
| 165 | 100 | int id = 0; | |
| 166 | |||
| 167 |
2/4✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
|
100 | Matrix3s Bf(B.cwiseAbs()); |
| 168 | |||
| 169 | // Corner of b axis aligned bounding box the closest to the origin | ||
| 170 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); |
| 171 |
2/2✓ Branch 0 taken 65 times.
✓ Branch 1 taken 35 times.
|
100 | if (squaredLowerBoundDistance > breakDistance2) return id; |
| 172 | 35 | ++id; | |
| 173 | |||
| 174 | // | B^T T| - b - Bf^T a | ||
| 175 |
1/2✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
|
35 | squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); |
| 176 |
2/2✓ Branch 0 taken 4 times.
✓ Branch 1 taken 31 times.
|
35 | if (squaredLowerBoundDistance > breakDistance2) return id; |
| 177 | 31 | ++id; | |
| 178 | |||
| 179 | 31 | int ja = 1, ka = 2, jb = 1, kb = 2; | |
| 180 |
2/2✓ Branch 0 taken 91 times.
✓ Branch 1 taken 29 times.
|
120 | for (int ia = 0; ia < 3; ++ia) { |
| 181 |
2/2✓ Branch 0 taken 273 times.
✓ Branch 1 taken 89 times.
|
362 | for (int ib = 0; ib < 3; ++ib) { |
| 182 |
4/8✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 273 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 273 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 273 times.
✗ Branch 11 not taken.
|
273 | const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); |
| 183 | |||
| 184 |
4/8✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 273 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 273 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 273 times.
✗ Branch 11 not taken.
|
273 | const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + |
| 185 |
4/8✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 273 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 273 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 273 times.
✗ Branch 11 not taken.
|
273 | b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); |
| 186 | // We need to divide by the norm || Aia x Bib || | ||
| 187 | // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 | ||
| 188 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 271 times.
|
273 | if (diff > 0) { |
| 189 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); |
| 190 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if (sinus2 > 1e-6) { |
| 191 | 2 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 192 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if (squaredLowerBoundDistance > breakDistance2) { |
| 193 | 2 | return id; | |
| 194 | } | ||
| 195 | } | ||
| 196 | /* // or | ||
| 197 | Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); | ||
| 198 | squaredLowerBoundDistance = diff * diff; | ||
| 199 | if (squaredLowerBoundDistance > breakDistance2 * sinus2) { | ||
| 200 | squaredLowerBoundDistance /= sinus2; | ||
| 201 | return true; | ||
| 202 | } | ||
| 203 | // */ | ||
| 204 | } | ||
| 205 | 271 | ++id; | |
| 206 | |||
| 207 | 271 | jb = kb; | |
| 208 | 271 | kb = ib; | |
| 209 | } | ||
| 210 | 89 | ja = ka; | |
| 211 | 89 | ka = ia; | |
| 212 | } | ||
| 213 | |||
| 214 | 29 | return id; | |
| 215 | } | ||
| 216 | |||
| 217 | // ------------------------ 0 -------------------------------------- | ||
| 218 | 1000 | bool withRuntimeLoop(const Matrix3s& B, const Vec3s& T, const Vec3s& a, | |
| 219 | const Vec3s& b, const Scalar& breakDistance2, | ||
| 220 | Scalar& squaredLowerBoundDistance) { | ||
| 221 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
1000 | Matrix3s Bf(B.cwiseAbs()); |
| 222 | |||
| 223 | // Corner of b axis aligned bounding box the closest to the origin | ||
| 224 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); |
| 225 |
2/2✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
|
1000 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 226 | |||
| 227 | // | B^T T| - b - Bf^T a | ||
| 228 |
1/2✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
|
350 | squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); |
| 229 |
2/2✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
|
350 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 230 | |||
| 231 | 310 | int ja = 1, ka = 2, jb = 1, kb = 2; | |
| 232 |
2/2✓ Branch 0 taken 910 times.
✓ Branch 1 taken 290 times.
|
1200 | for (int ia = 0; ia < 3; ++ia) { |
| 233 |
2/2✓ Branch 0 taken 2730 times.
✓ Branch 1 taken 890 times.
|
3620 | for (int ib = 0; ib < 3; ++ib) { |
| 234 |
4/8✓ Branch 1 taken 2730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2730 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2730 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2730 times.
✗ Branch 11 not taken.
|
2730 | const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); |
| 235 | |||
| 236 |
4/8✓ Branch 1 taken 2730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2730 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2730 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2730 times.
✗ Branch 11 not taken.
|
2730 | const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + |
| 237 |
4/8✓ Branch 1 taken 2730 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2730 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2730 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2730 times.
✗ Branch 11 not taken.
|
2730 | b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); |
| 238 | // We need to divide by the norm || Aia x Bib || | ||
| 239 | // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 | ||
| 240 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2710 times.
|
2730 | if (diff > 0) { |
| 241 |
2/4✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
|
20 | Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); |
| 242 |
1/2✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
|
20 | if (sinus2 > 1e-6) { |
| 243 | 20 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 244 |
1/2✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
|
20 | if (squaredLowerBoundDistance > breakDistance2) { |
| 245 | 20 | return true; | |
| 246 | } | ||
| 247 | } | ||
| 248 | /* // or | ||
| 249 | Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); | ||
| 250 | squaredLowerBoundDistance = diff * diff; | ||
| 251 | if (squaredLowerBoundDistance > breakDistance2 * sinus2) { | ||
| 252 | squaredLowerBoundDistance /= sinus2; | ||
| 253 | return true; | ||
| 254 | } | ||
| 255 | // */ | ||
| 256 | } | ||
| 257 | |||
| 258 | 2710 | jb = kb; | |
| 259 | 2710 | kb = ib; | |
| 260 | } | ||
| 261 | 890 | ja = ka; | |
| 262 | 890 | ka = ia; | |
| 263 | } | ||
| 264 | |||
| 265 | 290 | return false; | |
| 266 | } | ||
| 267 | |||
| 268 | // ------------------------ 1 -------------------------------------- | ||
| 269 | 1000 | bool withManualLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, | |
| 270 | const Vec3s& a, const Vec3s& b, | ||
| 271 | const Scalar& breakDistance2, | ||
| 272 | Scalar& squaredLowerBoundDistance) { | ||
| 273 | Scalar t, s; | ||
| 274 | Scalar diff; | ||
| 275 | |||
| 276 | // Matrix3s Bf = abs(B); | ||
| 277 | // Bf += reps; | ||
| 278 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
1000 | Matrix3s Bf(B.cwiseAbs()); |
| 279 | |||
| 280 | // Corner of b axis aligned bounding box the closest to the origin | ||
| 281 |
4/8✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 not taken.
|
1000 | Vec3s AABB_corner(T.cwiseAbs() - Bf * b); |
| 282 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
1000 | Vec3s diff3(AABB_corner - a); |
| 283 |
3/6✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
1000 | diff3 = diff3.cwiseMax(Vec3s::Zero()); |
| 284 | |||
| 285 | // for (Vec3s::Index i=0; i<3; ++i) diff3 [i] = std::max (0, diff3 [i]); | ||
| 286 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | squaredLowerBoundDistance = diff3.squaredNorm(); |
| 287 |
2/2✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
|
1000 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 288 | |||
| 289 |
7/14✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 350 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 350 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 350 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 350 times.
✗ Branch 20 not taken.
|
350 | AABB_corner = (B.transpose() * T).cwiseAbs() - Bf.transpose() * a; |
| 290 | // diff3 = | B^T T| - b - Bf^T a | ||
| 291 |
2/4✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
|
350 | diff3 = AABB_corner - b; |
| 292 |
3/6✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
|
350 | diff3 = diff3.cwiseMax(Vec3s::Zero()); |
| 293 |
1/2✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
|
350 | squaredLowerBoundDistance = diff3.squaredNorm(); |
| 294 | |||
| 295 |
2/2✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
|
350 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 296 | |||
| 297 | // A0 x B0 | ||
| 298 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 0) - T[1] * B(2, 0); |
| 299 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 300 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | assert(t == fabs(s)); |
| 301 | |||
| 302 | Scalar sinus2; | ||
| 303 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + |
| 304 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[2] * Bf(0, 1)); |
| 305 | // We need to divide by the norm || A0 x B0 || | ||
| 306 | // As ||A0|| = ||B0|| = 1, | ||
| 307 | // 2 2 | ||
| 308 | // || A0 x B0 || + (A0 | B0) = 1 | ||
| 309 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | if (diff > 0) { |
| 310 | ✗ | sinus2 = 1 - Bf(0, 0) * Bf(0, 0); | |
| 311 | ✗ | if (sinus2 > 1e-6) { | |
| 312 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 313 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 314 | ✗ | return true; | |
| 315 | } | ||
| 316 | } | ||
| 317 | } | ||
| 318 | |||
| 319 | // A0 x B1 | ||
| 320 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 1) - T[1] * B(2, 1); |
| 321 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 322 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | assert(t == fabs(s)); |
| 323 | |||
| 324 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + |
| 325 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[2] * Bf(0, 0)); |
| 326 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | if (diff > 0) { |
| 327 | ✗ | sinus2 = 1 - Bf(0, 1) * Bf(0, 1); | |
| 328 | ✗ | if (sinus2 > 1e-6) { | |
| 329 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 330 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 331 | ✗ | return true; | |
| 332 | } | ||
| 333 | } | ||
| 334 | } | ||
| 335 | |||
| 336 | // A0 x B2 | ||
| 337 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 2) - T[1] * B(2, 2); |
| 338 |
2/2✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 339 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | assert(t == fabs(s)); |
| 340 | |||
| 341 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + |
| 342 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[1] * Bf(0, 0)); |
| 343 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 300 times.
|
310 | if (diff > 0) { |
| 344 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | sinus2 = 1 - Bf(0, 2) * Bf(0, 2); |
| 345 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (sinus2 > 1e-6) { |
| 346 | 10 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 347 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (squaredLowerBoundDistance > breakDistance2) { |
| 348 | 10 | return true; | |
| 349 | } | ||
| 350 | } | ||
| 351 | } | ||
| 352 | |||
| 353 | // A1 x B0 | ||
| 354 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 0) - T[2] * B(0, 0); |
| 355 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 356 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 357 | |||
| 358 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + |
| 359 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(1, 1)); |
| 360 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 361 | ✗ | sinus2 = 1 - Bf(1, 0) * Bf(1, 0); | |
| 362 | ✗ | if (sinus2 > 1e-6) { | |
| 363 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 364 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 365 | ✗ | return true; | |
| 366 | } | ||
| 367 | } | ||
| 368 | } | ||
| 369 | |||
| 370 | // A1 x B1 | ||
| 371 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 1) - T[2] * B(0, 1); |
| 372 |
2/2✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 373 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 374 | |||
| 375 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + |
| 376 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(1, 0)); |
| 377 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 378 | ✗ | sinus2 = 1 - Bf(1, 1) * Bf(1, 1); | |
| 379 | ✗ | if (sinus2 > 1e-6) { | |
| 380 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 381 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 382 | ✗ | return true; | |
| 383 | } | ||
| 384 | } | ||
| 385 | } | ||
| 386 | |||
| 387 | // A1 x B2 | ||
| 388 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 2) - T[2] * B(0, 2); |
| 389 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 390 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 391 | |||
| 392 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + |
| 393 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[1] * Bf(1, 0)); |
| 394 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 395 | ✗ | sinus2 = 1 - Bf(1, 2) * Bf(1, 2); | |
| 396 | ✗ | if (sinus2 > 1e-6) { | |
| 397 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 398 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 399 | ✗ | return true; | |
| 400 | } | ||
| 401 | } | ||
| 402 | } | ||
| 403 | |||
| 404 | // A2 x B0 | ||
| 405 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 0) - T[0] * B(1, 0); |
| 406 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 407 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 408 | |||
| 409 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + |
| 410 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(2, 1)); |
| 411 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 412 | ✗ | sinus2 = 1 - Bf(2, 0) * Bf(2, 0); | |
| 413 | ✗ | if (sinus2 > 1e-6) { | |
| 414 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 415 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 416 | ✗ | return true; | |
| 417 | } | ||
| 418 | } | ||
| 419 | } | ||
| 420 | |||
| 421 | // A2 x B1 | ||
| 422 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 1) - T[0] * B(1, 1); |
| 423 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 424 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 425 | |||
| 426 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + |
| 427 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(2, 0)); |
| 428 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 429 | ✗ | sinus2 = 1 - Bf(2, 1) * Bf(2, 1); | |
| 430 | ✗ | if (sinus2 > 1e-6) { | |
| 431 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 432 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 433 | ✗ | return true; | |
| 434 | } | ||
| 435 | } | ||
| 436 | } | ||
| 437 | |||
| 438 | // A2 x B2 | ||
| 439 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 2) - T[0] * B(1, 2); |
| 440 |
2/2✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 441 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 442 | |||
| 443 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + |
| 444 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[1] * Bf(2, 0)); |
| 445 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 290 times.
|
300 | if (diff > 0) { |
| 446 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | sinus2 = 1 - Bf(2, 2) * Bf(2, 2); |
| 447 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (sinus2 > 1e-6) { |
| 448 | 10 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 449 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (squaredLowerBoundDistance > breakDistance2) { |
| 450 | 10 | return true; | |
| 451 | } | ||
| 452 | } | ||
| 453 | } | ||
| 454 | |||
| 455 | 290 | return false; | |
| 456 | } | ||
| 457 | |||
| 458 | // ------------------------ 2 -------------------------------------- | ||
| 459 | 1000 | bool withManualLoopUnrolling_2(const Matrix3s& B, const Vec3s& T, | |
| 460 | const Vec3s& a, const Vec3s& b, | ||
| 461 | const Scalar& breakDistance2, | ||
| 462 | Scalar& squaredLowerBoundDistance) { | ||
| 463 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
1000 | Matrix3s Bf(B.cwiseAbs()); |
| 464 | |||
| 465 | // Corner of b axis aligned bounding box the closest to the origin | ||
| 466 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); |
| 467 |
2/2✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
|
1000 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 468 | |||
| 469 |
1/2✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
|
350 | squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); |
| 470 |
2/2✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
|
350 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 471 | |||
| 472 | // A0 x B0 | ||
| 473 | Scalar t, s; | ||
| 474 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 0) - T[1] * B(2, 0); |
| 475 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 476 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | assert(t == fabs(s)); |
| 477 | |||
| 478 | Scalar sinus2; | ||
| 479 | Scalar diff; | ||
| 480 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + |
| 481 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[2] * Bf(0, 1)); |
| 482 | // We need to divide by the norm || A0 x B0 || | ||
| 483 | // As ||A0|| = ||B0|| = 1, | ||
| 484 | // 2 2 | ||
| 485 | // || A0 x B0 || + (A0 | B0) = 1 | ||
| 486 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | if (diff > 0) { |
| 487 | ✗ | sinus2 = 1 - Bf(0, 0) * Bf(0, 0); | |
| 488 | ✗ | if (sinus2 > 1e-6) { | |
| 489 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 490 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 491 | ✗ | return true; | |
| 492 | } | ||
| 493 | } | ||
| 494 | } | ||
| 495 | |||
| 496 | // A0 x B1 | ||
| 497 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 1) - T[1] * B(2, 1); |
| 498 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 499 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | assert(t == fabs(s)); |
| 500 | |||
| 501 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + |
| 502 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[2] * Bf(0, 0)); |
| 503 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | if (diff > 0) { |
| 504 | ✗ | sinus2 = 1 - Bf(0, 1) * Bf(0, 1); | |
| 505 | ✗ | if (sinus2 > 1e-6) { | |
| 506 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 507 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 508 | ✗ | return true; | |
| 509 | } | ||
| 510 | } | ||
| 511 | } | ||
| 512 | |||
| 513 | // A0 x B2 | ||
| 514 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 2) - T[1] * B(2, 2); |
| 515 |
2/2✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 516 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | assert(t == fabs(s)); |
| 517 | |||
| 518 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + |
| 519 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[1] * Bf(0, 0)); |
| 520 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 300 times.
|
310 | if (diff > 0) { |
| 521 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | sinus2 = 1 - Bf(0, 2) * Bf(0, 2); |
| 522 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (sinus2 > 1e-6) { |
| 523 | 10 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 524 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (squaredLowerBoundDistance > breakDistance2) { |
| 525 | 10 | return true; | |
| 526 | } | ||
| 527 | } | ||
| 528 | } | ||
| 529 | |||
| 530 | // A1 x B0 | ||
| 531 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 0) - T[2] * B(0, 0); |
| 532 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 533 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 534 | |||
| 535 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + |
| 536 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(1, 1)); |
| 537 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 538 | ✗ | sinus2 = 1 - Bf(1, 0) * Bf(1, 0); | |
| 539 | ✗ | if (sinus2 > 1e-6) { | |
| 540 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 541 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 542 | ✗ | return true; | |
| 543 | } | ||
| 544 | } | ||
| 545 | } | ||
| 546 | |||
| 547 | // A1 x B1 | ||
| 548 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 1) - T[2] * B(0, 1); |
| 549 |
2/2✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 550 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 551 | |||
| 552 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + |
| 553 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(1, 0)); |
| 554 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 555 | ✗ | sinus2 = 1 - Bf(1, 1) * Bf(1, 1); | |
| 556 | ✗ | if (sinus2 > 1e-6) { | |
| 557 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 558 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 559 | ✗ | return true; | |
| 560 | } | ||
| 561 | } | ||
| 562 | } | ||
| 563 | |||
| 564 | // A1 x B2 | ||
| 565 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 2) - T[2] * B(0, 2); |
| 566 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 567 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 568 | |||
| 569 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + |
| 570 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[1] * Bf(1, 0)); |
| 571 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 572 | ✗ | sinus2 = 1 - Bf(1, 2) * Bf(1, 2); | |
| 573 | ✗ | if (sinus2 > 1e-6) { | |
| 574 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 575 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 576 | ✗ | return true; | |
| 577 | } | ||
| 578 | } | ||
| 579 | } | ||
| 580 | |||
| 581 | // A2 x B0 | ||
| 582 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 0) - T[0] * B(1, 0); |
| 583 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 584 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 585 | |||
| 586 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + |
| 587 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(2, 1)); |
| 588 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 589 | ✗ | sinus2 = 1 - Bf(2, 0) * Bf(2, 0); | |
| 590 | ✗ | if (sinus2 > 1e-6) { | |
| 591 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 592 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 593 | ✗ | return true; | |
| 594 | } | ||
| 595 | } | ||
| 596 | } | ||
| 597 | |||
| 598 | // A2 x B1 | ||
| 599 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 1) - T[0] * B(1, 1); |
| 600 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 601 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 602 | |||
| 603 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + |
| 604 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(2, 0)); |
| 605 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 606 | ✗ | sinus2 = 1 - Bf(2, 1) * Bf(2, 1); | |
| 607 | ✗ | if (sinus2 > 1e-6) { | |
| 608 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 609 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 610 | ✗ | return true; | |
| 611 | } | ||
| 612 | } | ||
| 613 | } | ||
| 614 | |||
| 615 | // A2 x B2 | ||
| 616 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 2) - T[0] * B(1, 2); |
| 617 |
2/2✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 618 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | assert(t == fabs(s)); |
| 619 | |||
| 620 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + |
| 621 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[1] * Bf(2, 0)); |
| 622 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 290 times.
|
300 | if (diff > 0) { |
| 623 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | sinus2 = 1 - Bf(2, 2) * Bf(2, 2); |
| 624 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (sinus2 > 1e-6) { |
| 625 | 10 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 626 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (squaredLowerBoundDistance > breakDistance2) { |
| 627 | 10 | return true; | |
| 628 | } | ||
| 629 | } | ||
| 630 | } | ||
| 631 | |||
| 632 | 290 | return false; | |
| 633 | } | ||
| 634 | |||
| 635 | // ------------------------ 3 -------------------------------------- | ||
| 636 | template <int ia, int ib, int ja = (ia + 1) % 3, int ka = (ia + 2) % 3, | ||
| 637 | int jb = (ib + 1) % 3, int kb = (ib + 2) % 3> | ||
| 638 | struct loop_case_1 { | ||
| 639 | 5460 | static inline bool run(const Matrix3s& B, const Vec3s& T, const Vec3s& a, | |
| 640 | const Vec3s& b, const Matrix3s& Bf, | ||
| 641 | const Scalar& breakDistance2, | ||
| 642 | Scalar& squaredLowerBoundDistance) { | ||
| 643 | 5460 | const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); | |
| 644 | |||
| 645 | 5460 | const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + | |
| 646 | 5460 | b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); | |
| 647 | // We need to divide by the norm || Aia x Bib || | ||
| 648 | // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 | ||
| 649 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2710 times.
|
5460 | if (diff > 0) { |
| 650 | 40 | Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); | |
| 651 |
1/2✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
|
40 | if (sinus2 > 1e-6) { |
| 652 | 40 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 653 |
1/2✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
|
40 | if (squaredLowerBoundDistance > breakDistance2) { |
| 654 | 40 | return true; | |
| 655 | } | ||
| 656 | } | ||
| 657 | /* // or | ||
| 658 | Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); | ||
| 659 | squaredLowerBoundDistance = diff * diff; | ||
| 660 | if (squaredLowerBoundDistance > breakDistance2 * sinus2) { | ||
| 661 | squaredLowerBoundDistance /= sinus2; | ||
| 662 | return true; | ||
| 663 | } | ||
| 664 | // */ | ||
| 665 | } | ||
| 666 | 5420 | return false; | |
| 667 | } | ||
| 668 | }; | ||
| 669 | |||
| 670 | 1000 | bool withTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, | |
| 671 | const Vec3s& a, const Vec3s& b, | ||
| 672 | const Scalar& breakDistance2, | ||
| 673 | Scalar& squaredLowerBoundDistance) { | ||
| 674 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
1000 | Matrix3s Bf(B.cwiseAbs()); |
| 675 | |||
| 676 | // Corner of b axis aligned bounding box the closest to the origin | ||
| 677 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); |
| 678 |
2/2✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
|
1000 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 679 | |||
| 680 |
1/2✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
|
350 | squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); |
| 681 |
2/2✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
|
350 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 682 | |||
| 683 | // Ai x Bj | ||
| 684 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 310 times.
|
310 | if (loop_case_1<0, 0>::run(B, T, a, b, Bf, breakDistance2, |
| 685 | squaredLowerBoundDistance)) | ||
| 686 | ✗ | return true; | |
| 687 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 310 times.
|
310 | if (loop_case_1<0, 1>::run(B, T, a, b, Bf, breakDistance2, |
| 688 | squaredLowerBoundDistance)) | ||
| 689 | ✗ | return true; | |
| 690 |
3/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 300 times.
|
310 | if (loop_case_1<0, 2>::run(B, T, a, b, Bf, breakDistance2, |
| 691 | squaredLowerBoundDistance)) | ||
| 692 | 10 | return true; | |
| 693 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
|
300 | if (loop_case_1<1, 0>::run(B, T, a, b, Bf, breakDistance2, |
| 694 | squaredLowerBoundDistance)) | ||
| 695 | ✗ | return true; | |
| 696 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
|
300 | if (loop_case_1<1, 1>::run(B, T, a, b, Bf, breakDistance2, |
| 697 | squaredLowerBoundDistance)) | ||
| 698 | ✗ | return true; | |
| 699 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
|
300 | if (loop_case_1<1, 2>::run(B, T, a, b, Bf, breakDistance2, |
| 700 | squaredLowerBoundDistance)) | ||
| 701 | ✗ | return true; | |
| 702 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
|
300 | if (loop_case_1<2, 0>::run(B, T, a, b, Bf, breakDistance2, |
| 703 | squaredLowerBoundDistance)) | ||
| 704 | ✗ | return true; | |
| 705 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 300 times.
|
300 | if (loop_case_1<2, 1>::run(B, T, a, b, Bf, breakDistance2, |
| 706 | squaredLowerBoundDistance)) | ||
| 707 | ✗ | return true; | |
| 708 |
3/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✓ Branch 4 taken 290 times.
|
300 | if (loop_case_1<2, 2>::run(B, T, a, b, Bf, breakDistance2, |
| 709 | squaredLowerBoundDistance)) | ||
| 710 | 10 | return true; | |
| 711 | |||
| 712 | 290 | return false; | |
| 713 | } | ||
| 714 | |||
| 715 | // ------------------------ 4 -------------------------------------- | ||
| 716 | |||
| 717 | template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3> | ||
| 718 | struct loop_case_2 { | ||
| 719 | 5460 | static inline bool run(int ia, int ja, int ka, const Matrix3s& B, | |
| 720 | const Vec3s& T, const Vec3s& a, const Vec3s& b, | ||
| 721 | const Matrix3s& Bf, const Scalar& breakDistance2, | ||
| 722 | Scalar& squaredLowerBoundDistance) { | ||
| 723 | 5460 | const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib); | |
| 724 | |||
| 725 | 5460 | const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) + | |
| 726 | 5460 | b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb)); | |
| 727 | // We need to divide by the norm || Aia x Bib || | ||
| 728 | // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2 | ||
| 729 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 2710 times.
|
5460 | if (diff > 0) { |
| 730 | 40 | Scalar sinus2 = 1 - Bf(ia, ib) * Bf(ia, ib); | |
| 731 |
1/2✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
|
40 | if (sinus2 > 1e-6) { |
| 732 | 40 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 733 |
1/2✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
|
40 | if (squaredLowerBoundDistance > breakDistance2) { |
| 734 | 40 | return true; | |
| 735 | } | ||
| 736 | } | ||
| 737 | /* // or | ||
| 738 | Scalar sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib); | ||
| 739 | squaredLowerBoundDistance = diff * diff; | ||
| 740 | if (squaredLowerBoundDistance > breakDistance2 * sinus2) { | ||
| 741 | squaredLowerBoundDistance /= sinus2; | ||
| 742 | return true; | ||
| 743 | } | ||
| 744 | // */ | ||
| 745 | } | ||
| 746 | 5420 | return false; | |
| 747 | } | ||
| 748 | }; | ||
| 749 | |||
| 750 | 1000 | bool withPartialTemplateLoopUnrolling_1(const Matrix3s& B, const Vec3s& T, | |
| 751 | const Vec3s& a, const Vec3s& b, | ||
| 752 | const Scalar& breakDistance2, | ||
| 753 | Scalar& squaredLowerBoundDistance) { | ||
| 754 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
1000 | Matrix3s Bf(B.cwiseAbs()); |
| 755 | |||
| 756 | // Corner of b axis aligned bounding box the closest to the origin | ||
| 757 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | squaredLowerBoundDistance = _computeDistanceForCase1(T, a, b, Bf); |
| 758 |
2/2✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
|
1000 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 759 | |||
| 760 |
1/2✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
|
350 | squaredLowerBoundDistance = _computeDistanceForCase2(B, T, a, b, Bf); |
| 761 |
2/2✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
|
350 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 762 | |||
| 763 | // Ai x Bj | ||
| 764 | 310 | int ja = 1, ka = 2; | |
| 765 |
2/2✓ Branch 0 taken 910 times.
✓ Branch 1 taken 290 times.
|
1200 | for (int ia = 0; ia < 3; ++ia) { |
| 766 |
2/4✓ Branch 1 taken 910 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 910 times.
|
910 | if (loop_case_2<0>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, |
| 767 | squaredLowerBoundDistance)) | ||
| 768 | ✗ | return true; | |
| 769 |
2/4✓ Branch 1 taken 910 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 910 times.
|
910 | if (loop_case_2<1>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, |
| 770 | squaredLowerBoundDistance)) | ||
| 771 | ✗ | return true; | |
| 772 |
3/4✓ Branch 1 taken 910 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20 times.
✓ Branch 4 taken 890 times.
|
910 | if (loop_case_2<2>::run(ia, ja, ka, B, T, a, b, Bf, breakDistance2, |
| 773 | squaredLowerBoundDistance)) | ||
| 774 | 20 | return true; | |
| 775 | 890 | ja = ka; | |
| 776 | 890 | ka = ia; | |
| 777 | } | ||
| 778 | |||
| 779 | 290 | return false; | |
| 780 | } | ||
| 781 | |||
| 782 | // ------------------------ 5 -------------------------------------- | ||
| 783 | 1000 | bool originalWithLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, | |
| 784 | const Vec3s& b, const Scalar& breakDistance2, | ||
| 785 | Scalar& squaredLowerBoundDistance) { | ||
| 786 | Scalar t, s; | ||
| 787 | Scalar diff; | ||
| 788 | |||
| 789 |
2/4✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
|
1000 | Matrix3s Bf(B.cwiseAbs()); |
| 790 | 1000 | squaredLowerBoundDistance = 0; | |
| 791 | |||
| 792 | // if any of these tests are one-sided, then the polyhedra are disjoint | ||
| 793 | |||
| 794 | // A1 x A2 = A0 | ||
| 795 |
5/8✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 520 times.
✓ Branch 4 taken 480 times.
✓ Branch 6 taken 520 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 480 times.
✗ Branch 10 not taken.
|
1000 | t = ((T[0] < 0.0) ? -T[0] : T[0]); |
| 796 | |||
| 797 |
3/6✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
1000 | diff = t - (a[0] + Bf.row(0).dot(b)); |
| 798 |
2/2✓ Branch 0 taken 290 times.
✓ Branch 1 taken 710 times.
|
1000 | if (diff > 0) { |
| 799 | 290 | squaredLowerBoundDistance = diff * diff; | |
| 800 | } | ||
| 801 | |||
| 802 | // A2 x A0 = A1 | ||
| 803 |
5/8✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 480 times.
✓ Branch 4 taken 520 times.
✓ Branch 6 taken 480 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 520 times.
✗ Branch 10 not taken.
|
1000 | t = ((T[1] < 0.0) ? -T[1] : T[1]); |
| 804 | |||
| 805 |
3/6✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
1000 | diff = t - (a[1] + Bf.row(1).dot(b)); |
| 806 |
2/2✓ Branch 0 taken 350 times.
✓ Branch 1 taken 650 times.
|
1000 | if (diff > 0) { |
| 807 | 350 | squaredLowerBoundDistance += diff * diff; | |
| 808 | } | ||
| 809 | |||
| 810 | // A0 x A1 = A2 | ||
| 811 |
5/8✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 500 times.
✓ Branch 4 taken 500 times.
✓ Branch 6 taken 500 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 500 times.
✗ Branch 10 not taken.
|
1000 | t = ((T[2] < 0.0) ? -T[2] : T[2]); |
| 812 | |||
| 813 |
3/6✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
|
1000 | diff = t - (a[2] + Bf.row(2).dot(b)); |
| 814 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 820 times.
|
1000 | if (diff > 0) { |
| 815 | 180 | squaredLowerBoundDistance += diff * diff; | |
| 816 | } | ||
| 817 | |||
| 818 |
2/2✓ Branch 0 taken 650 times.
✓ Branch 1 taken 350 times.
|
1000 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 819 | |||
| 820 | 350 | squaredLowerBoundDistance = 0; | |
| 821 | |||
| 822 | // B1 x B2 = B0 | ||
| 823 |
2/4✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
|
350 | s = B.col(0).dot(T); |
| 824 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 170 times.
|
350 | t = ((s < 0.0) ? -s : s); |
| 825 | |||
| 826 |
3/6✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
|
350 | diff = t - (b[0] + Bf.col(0).dot(a)); |
| 827 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 350 times.
|
350 | if (diff > 0) { |
| 828 | ✗ | squaredLowerBoundDistance += diff * diff; | |
| 829 | } | ||
| 830 | |||
| 831 | // B2 x B0 = B1 | ||
| 832 |
2/4✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
|
350 | s = B.col(1).dot(T); |
| 833 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 170 times.
|
350 | t = ((s < 0.0) ? -s : s); |
| 834 | |||
| 835 |
3/6✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
|
350 | diff = t - (b[1] + Bf.col(1).dot(a)); |
| 836 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 330 times.
|
350 | if (diff > 0) { |
| 837 | 20 | squaredLowerBoundDistance += diff * diff; | |
| 838 | } | ||
| 839 | |||
| 840 | // B0 x B1 = B2 | ||
| 841 |
2/4✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
|
350 | s = B.col(2).dot(T); |
| 842 |
2/2✓ Branch 0 taken 220 times.
✓ Branch 1 taken 130 times.
|
350 | t = ((s < 0.0) ? -s : s); |
| 843 | |||
| 844 |
3/6✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
|
350 | diff = t - (b[2] + Bf.col(2).dot(a)); |
| 845 |
2/2✓ Branch 0 taken 20 times.
✓ Branch 1 taken 330 times.
|
350 | if (diff > 0) { |
| 846 | 20 | squaredLowerBoundDistance += diff * diff; | |
| 847 | } | ||
| 848 | |||
| 849 |
2/2✓ Branch 0 taken 40 times.
✓ Branch 1 taken 310 times.
|
350 | if (squaredLowerBoundDistance > breakDistance2) return true; |
| 850 | |||
| 851 | // A0 x B0 | ||
| 852 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 0) - T[1] * B(2, 0); |
| 853 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 854 | |||
| 855 | Scalar sinus2; | ||
| 856 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + |
| 857 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[2] * Bf(0, 1)); |
| 858 | // We need to divide by the norm || A0 x B0 || | ||
| 859 | // As ||A0|| = ||B0|| = 1, | ||
| 860 | // 2 2 | ||
| 861 | // || A0 x B0 || + (A0 | B0) = 1 | ||
| 862 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | if (diff > 0) { |
| 863 | ✗ | sinus2 = 1 - Bf(0, 0) * Bf(0, 0); | |
| 864 | ✗ | if (sinus2 > 1e-6) { | |
| 865 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 866 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 867 | ✗ | return true; | |
| 868 | } | ||
| 869 | } | ||
| 870 | } | ||
| 871 | |||
| 872 | // A0 x B1 | ||
| 873 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 1) - T[1] * B(2, 1); |
| 874 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 875 | |||
| 876 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + |
| 877 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[2] * Bf(0, 0)); |
| 878 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 310 times.
|
310 | if (diff > 0) { |
| 879 | ✗ | sinus2 = 1 - Bf(0, 1) * Bf(0, 1); | |
| 880 | ✗ | if (sinus2 > 1e-6) { | |
| 881 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 882 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 883 | ✗ | return true; | |
| 884 | } | ||
| 885 | } | ||
| 886 | } | ||
| 887 | |||
| 888 | // A0 x B2 | ||
| 889 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 2) - T[1] * B(2, 2); |
| 890 |
2/2✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 891 | |||
| 892 |
6/12✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
|
310 | diff = t - (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + |
| 893 |
2/4✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
|
310 | b[1] * Bf(0, 0)); |
| 894 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 300 times.
|
310 | if (diff > 0) { |
| 895 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | sinus2 = 1 - Bf(0, 2) * Bf(0, 2); |
| 896 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (sinus2 > 1e-6) { |
| 897 | 10 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 898 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (squaredLowerBoundDistance > breakDistance2) { |
| 899 | 10 | return true; | |
| 900 | } | ||
| 901 | } | ||
| 902 | } | ||
| 903 | |||
| 904 | // A1 x B0 | ||
| 905 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 0) - T[2] * B(0, 0); |
| 906 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 907 | |||
| 908 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + |
| 909 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(1, 1)); |
| 910 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 911 | ✗ | sinus2 = 1 - Bf(1, 0) * Bf(1, 0); | |
| 912 | ✗ | if (sinus2 > 1e-6) { | |
| 913 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 914 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 915 | ✗ | return true; | |
| 916 | } | ||
| 917 | } | ||
| 918 | } | ||
| 919 | |||
| 920 | // A1 x B1 | ||
| 921 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 1) - T[2] * B(0, 1); |
| 922 |
2/2✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 923 | |||
| 924 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + |
| 925 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(1, 0)); |
| 926 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 927 | ✗ | sinus2 = 1 - Bf(1, 1) * Bf(1, 1); | |
| 928 | ✗ | if (sinus2 > 1e-6) { | |
| 929 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 930 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 931 | ✗ | return true; | |
| 932 | } | ||
| 933 | } | ||
| 934 | } | ||
| 935 | |||
| 936 | // A1 x B2 | ||
| 937 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 2) - T[2] * B(0, 2); |
| 938 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 939 | |||
| 940 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + |
| 941 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[1] * Bf(1, 0)); |
| 942 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 943 | ✗ | sinus2 = 1 - Bf(1, 2) * Bf(1, 2); | |
| 944 | ✗ | if (sinus2 > 1e-6) { | |
| 945 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 946 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 947 | ✗ | return true; | |
| 948 | } | ||
| 949 | } | ||
| 950 | } | ||
| 951 | |||
| 952 | // A2 x B0 | ||
| 953 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 0) - T[0] * B(1, 0); |
| 954 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 955 | |||
| 956 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + |
| 957 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(2, 1)); |
| 958 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 959 | ✗ | sinus2 = 1 - Bf(2, 0) * Bf(2, 0); | |
| 960 | ✗ | if (sinus2 > 1e-6) { | |
| 961 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 962 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 963 | ✗ | return true; | |
| 964 | } | ||
| 965 | } | ||
| 966 | } | ||
| 967 | |||
| 968 | // A2 x B1 | ||
| 969 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 1) - T[0] * B(1, 1); |
| 970 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 971 | |||
| 972 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + |
| 973 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[2] * Bf(2, 0)); |
| 974 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 300 times.
|
300 | if (diff > 0) { |
| 975 | ✗ | sinus2 = 1 - Bf(2, 1) * Bf(2, 1); | |
| 976 | ✗ | if (sinus2 > 1e-6) { | |
| 977 | ✗ | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 978 | ✗ | if (squaredLowerBoundDistance > breakDistance2) { | |
| 979 | ✗ | return true; | |
| 980 | } | ||
| 981 | } | ||
| 982 | } | ||
| 983 | |||
| 984 | // A2 x B2 | ||
| 985 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 2) - T[0] * B(1, 2); |
| 986 |
2/2✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 987 | |||
| 988 |
6/12✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
|
300 | diff = t - (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + |
| 989 |
2/4✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
|
300 | b[1] * Bf(2, 0)); |
| 990 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 290 times.
|
300 | if (diff > 0) { |
| 991 |
2/4✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
|
10 | sinus2 = 1 - Bf(2, 2) * Bf(2, 2); |
| 992 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (sinus2 > 1e-6) { |
| 993 | 10 | squaredLowerBoundDistance = diff * diff / sinus2; | |
| 994 |
1/2✓ Branch 0 taken 10 times.
✗ Branch 1 not taken.
|
10 | if (squaredLowerBoundDistance > breakDistance2) { |
| 995 | 10 | return true; | |
| 996 | } | ||
| 997 | } | ||
| 998 | } | ||
| 999 | |||
| 1000 | 290 | return false; | |
| 1001 | } | ||
| 1002 | |||
| 1003 | // ------------------------ 6 -------------------------------------- | ||
| 1004 | 1000 | bool originalWithNoLowerBound(const Matrix3s& B, const Vec3s& T, const Vec3s& a, | |
| 1005 | const Vec3s& b, const Scalar&, | ||
| 1006 | Scalar& squaredLowerBoundDistance) { | ||
| 1007 | Scalar t, s; | ||
| 1008 | 1000 | const Scalar reps = Scalar(1e-6); | |
| 1009 | |||
| 1010 | 1000 | squaredLowerBoundDistance = 0; | |
| 1011 | |||
| 1012 |
4/8✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1000 times.
✗ Branch 11 not taken.
|
1000 | Matrix3s Bf(B.array().abs() + reps); |
| 1013 | // Bf += reps; | ||
| 1014 | |||
| 1015 | // if any of these tests are one-sided, then the polyhedra are disjoint | ||
| 1016 | |||
| 1017 | // A1 x A2 = A0 | ||
| 1018 |
5/8✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 520 times.
✓ Branch 4 taken 480 times.
✓ Branch 6 taken 520 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 480 times.
✗ Branch 10 not taken.
|
1000 | t = ((T[0] < 0.0) ? -T[0] : T[0]); |
| 1019 | |||
| 1020 | // if(t > (a[0] + Bf.dotX(b))) | ||
| 1021 |
5/8✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1000 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 290 times.
✓ Branch 10 taken 710 times.
|
1000 | if (t > (a[0] + Bf.row(0).dot(b))) return true; |
| 1022 | |||
| 1023 | // B1 x B2 = B0 | ||
| 1024 | // s = B.transposeDotX(T); | ||
| 1025 |
2/4✓ Branch 1 taken 710 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 710 times.
✗ Branch 5 not taken.
|
710 | s = B.col(0).dot(T); |
| 1026 |
2/2✓ Branch 0 taken 410 times.
✓ Branch 1 taken 300 times.
|
710 | t = ((s < 0.0) ? -s : s); |
| 1027 | |||
| 1028 | // if(t > (b[0] + Bf.transposeDotX(a))) | ||
| 1029 |
5/8✓ Branch 1 taken 710 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 710 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 710 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 140 times.
✓ Branch 10 taken 570 times.
|
710 | if (t > (b[0] + Bf.col(0).dot(a))) return true; |
| 1030 | |||
| 1031 | // A2 x A0 = A1 | ||
| 1032 |
5/8✓ Branch 1 taken 570 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 290 times.
✓ Branch 4 taken 280 times.
✓ Branch 6 taken 290 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 280 times.
✗ Branch 10 not taken.
|
570 | t = ((T[1] < 0.0) ? -T[1] : T[1]); |
| 1033 | |||
| 1034 | // if(t > (a[1] + Bf.dotY(b))) | ||
| 1035 |
5/8✓ Branch 1 taken 570 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 570 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 570 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 160 times.
✓ Branch 10 taken 410 times.
|
570 | if (t > (a[1] + Bf.row(1).dot(b))) return true; |
| 1036 | |||
| 1037 | // A0 x A1 = A2 | ||
| 1038 |
5/8✓ Branch 1 taken 410 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 190 times.
✓ Branch 4 taken 220 times.
✓ Branch 6 taken 190 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 220 times.
✗ Branch 10 not taken.
|
410 | t = ((T[2] < 0.0) ? -T[2] : T[2]); |
| 1039 | |||
| 1040 | // if(t > (a[2] + Bf.dotZ(b))) | ||
| 1041 |
5/8✓ Branch 1 taken 410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 410 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 410 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 60 times.
✓ Branch 10 taken 350 times.
|
410 | if (t > (a[2] + Bf.row(2).dot(b))) return true; |
| 1042 | |||
| 1043 | // B2 x B0 = B1 | ||
| 1044 | // s = B.transposeDotY(T); | ||
| 1045 |
2/4✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
|
350 | s = B.col(1).dot(T); |
| 1046 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 170 times.
|
350 | t = ((s < 0.0) ? -s : s); |
| 1047 | |||
| 1048 | // if(t > (b[1] + Bf.transposeDotY(a))) | ||
| 1049 |
5/8✓ Branch 1 taken 350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 350 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
✓ Branch 10 taken 330 times.
|
350 | if (t > (b[1] + Bf.col(1).dot(a))) return true; |
| 1050 | |||
| 1051 | // B0 x B1 = B2 | ||
| 1052 | // s = B.transposeDotZ(T); | ||
| 1053 |
2/4✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330 times.
✗ Branch 5 not taken.
|
330 | s = B.col(2).dot(T); |
| 1054 |
2/2✓ Branch 0 taken 210 times.
✓ Branch 1 taken 120 times.
|
330 | t = ((s < 0.0) ? -s : s); |
| 1055 | |||
| 1056 | // if(t > (b[2] + Bf.transposeDotZ(a))) | ||
| 1057 |
5/8✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 330 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 20 times.
✓ Branch 10 taken 310 times.
|
330 | if (t > (b[2] + Bf.col(2).dot(a))) return true; |
| 1058 | |||
| 1059 | // A0 x B0 | ||
| 1060 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 0) - T[1] * B(2, 0); |
| 1061 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 130 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 1062 | |||
| 1063 | 310 | if (t > | |
| 1064 |
9/18✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 310 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 310 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 310 times.
|
310 | (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1))) |
| 1065 | ✗ | return true; | |
| 1066 | |||
| 1067 | // A0 x B1 | ||
| 1068 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 1) - T[1] * B(2, 1); |
| 1069 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 160 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 1070 | |||
| 1071 | 310 | if (t > | |
| 1072 |
9/18✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 310 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 310 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 310 times.
|
310 | (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0))) |
| 1073 | ✗ | return true; | |
| 1074 | |||
| 1075 | // A0 x B2 | ||
| 1076 |
4/8✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
|
310 | s = T[2] * B(1, 2) - T[1] * B(2, 2); |
| 1077 |
2/2✓ Branch 0 taken 200 times.
✓ Branch 1 taken 110 times.
|
310 | t = ((s < 0.0) ? -s : s); |
| 1078 | |||
| 1079 | 310 | if (t > | |
| 1080 |
10/18✓ Branch 1 taken 310 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 310 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 310 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 310 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 310 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 310 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 310 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 310 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 10 times.
✓ Branch 25 taken 300 times.
|
310 | (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0))) |
| 1081 | 10 | return true; | |
| 1082 | |||
| 1083 | // A1 x B0 | ||
| 1084 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 0) - T[2] * B(0, 0); |
| 1085 |
2/2✓ Branch 0 taken 150 times.
✓ Branch 1 taken 150 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 1086 | |||
| 1087 | 300 | if (t > | |
| 1088 |
9/18✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
|
300 | (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1))) |
| 1089 | ✗ | return true; | |
| 1090 | |||
| 1091 | // A1 x B1 | ||
| 1092 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 1) - T[2] * B(0, 1); |
| 1093 |
2/2✓ Branch 0 taken 190 times.
✓ Branch 1 taken 110 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 1094 | |||
| 1095 | 300 | if (t > | |
| 1096 |
9/18✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
|
300 | (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0))) |
| 1097 | ✗ | return true; | |
| 1098 | |||
| 1099 | // A1 x B2 | ||
| 1100 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[0] * B(2, 2) - T[2] * B(0, 2); |
| 1101 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 1102 | |||
| 1103 | 300 | if (t > | |
| 1104 |
9/18✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
|
300 | (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0))) |
| 1105 | ✗ | return true; | |
| 1106 | |||
| 1107 | // A2 x B0 | ||
| 1108 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 0) - T[0] * B(1, 0); |
| 1109 |
2/2✓ Branch 0 taken 180 times.
✓ Branch 1 taken 120 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 1110 | |||
| 1111 | 300 | if (t > | |
| 1112 |
9/18✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
|
300 | (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1))) |
| 1113 | ✗ | return true; | |
| 1114 | |||
| 1115 | // A2 x B1 | ||
| 1116 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 1) - T[0] * B(1, 1); |
| 1117 |
2/2✓ Branch 0 taken 120 times.
✓ Branch 1 taken 180 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 1118 | |||
| 1119 | 300 | if (t > | |
| 1120 |
9/18✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✓ Branch 25 taken 300 times.
|
300 | (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0))) |
| 1121 | ✗ | return true; | |
| 1122 | |||
| 1123 | // A2 x B2 | ||
| 1124 |
4/8✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
|
300 | s = T[1] * B(0, 2) - T[0] * B(1, 2); |
| 1125 |
2/2✓ Branch 0 taken 170 times.
✓ Branch 1 taken 130 times.
|
300 | t = ((s < 0.0) ? -s : s); |
| 1126 | |||
| 1127 | 300 | if (t > | |
| 1128 |
10/18✓ Branch 1 taken 300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 300 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 300 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 300 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 300 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 300 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 300 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 300 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 10 times.
✓ Branch 25 taken 290 times.
|
300 | (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0))) |
| 1129 | 10 | return true; | |
| 1130 | |||
| 1131 | 290 | return false; | |
| 1132 | } | ||
| 1133 | } // namespace obbDisjoint_impls | ||
| 1134 | |||
| 1135 | struct BenchmarkResult { | ||
| 1136 | /// The test ID: | ||
| 1137 | /// - 0-10 identifies a separating axes. | ||
| 1138 | /// - 11 means no separatins axes could be found. (distance should be <= 0) | ||
| 1139 | int ifId; | ||
| 1140 | Scalar distance; | ||
| 1141 | Scalar squaredLowerBoundDistance; | ||
| 1142 | duration_type duration[NB_METHODS]; | ||
| 1143 | bool failure; | ||
| 1144 | |||
| 1145 | ✗ | static std::ostream& headers(std::ostream& os) { | |
| 1146 | ✗ | const std::string unit = " (us)"; | |
| 1147 | os << "separating axis" << sep << "distance lower bound" << sep | ||
| 1148 | << "distance" << sep << "failure" << sep << "Runtime Loop" << unit << sep | ||
| 1149 | << "Manual Loop Unrolling 1" << unit << sep << "Manual Loop Unrolling 2" | ||
| 1150 | << unit << sep << "Template Unrolling" << unit << sep | ||
| 1151 | << "Partial Template Unrolling" << unit << sep << "Original (LowerBound)" | ||
| 1152 | ✗ | << unit << sep << "Original (NoLowerBound)" << unit; | |
| 1153 | ✗ | return os; | |
| 1154 | ✗ | } | |
| 1155 | |||
| 1156 | ✗ | std::ostream& print(std::ostream& os) const { | |
| 1157 | ✗ | os << ifId << sep << std::sqrt(squaredLowerBoundDistance) << sep << distance | |
| 1158 | ✗ | << sep << failure; | |
| 1159 | ✗ | for (int i = 0; i < NB_METHODS; ++i) | |
| 1160 | ✗ | os << sep | |
| 1161 | ✗ | << static_cast<double>( | |
| 1162 | ✗ | std::chrono::duration_cast<std::chrono::nanoseconds>( | |
| 1163 | ✗ | duration[i]) | |
| 1164 | ✗ | .count()) * | |
| 1165 | ✗ | 1e-3; | |
| 1166 | ✗ | return os; | |
| 1167 | } | ||
| 1168 | }; | ||
| 1169 | |||
| 1170 | ✗ | std::ostream& operator<<(std::ostream& os, const BenchmarkResult& br) { | |
| 1171 | ✗ | return br.print(os); | |
| 1172 | } | ||
| 1173 | |||
| 1174 | 100 | BenchmarkResult benchmark_obb_case(const Matrix3s& B, const Vec3s& T, | |
| 1175 | const Vec3s& a, const Vec3s& b, | ||
| 1176 | const CollisionRequest& request, | ||
| 1177 | std::size_t N) { | ||
| 1178 | 100 | const Scalar breakDistance(request.break_distance + request.security_margin); | |
| 1179 | 100 | const Scalar breakDistance2 = breakDistance * breakDistance; | |
| 1180 | |||
| 1181 | BenchmarkResult result; | ||
| 1182 | // First determine which axis provide the answer | ||
| 1183 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | result.ifId = obbDisjoint_impls::separatingAxisId( |
| 1184 | B, T, a, b, breakDistance2, result.squaredLowerBoundDistance); | ||
| 1185 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | bool disjoint = obbDisjoint_impls::distance(B, T, a, b, result.distance); |
| 1186 |
2/4✓ Branch 0 taken 100 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
|
100 | assert(0 <= result.ifId && result.ifId <= 11); |
| 1187 | |||
| 1188 | // Sanity check | ||
| 1189 | 100 | result.failure = true; | |
| 1190 | 100 | bool overlap = (result.ifId == 11); | |
| 1191 | 100 | Scalar dist_thr = request.break_distance + request.security_margin; | |
| 1192 |
3/4✓ Branch 0 taken 71 times.
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 71 times.
|
100 | if (!overlap && result.distance <= 0) { |
| 1193 | ✗ | std::cerr << "Failure: negative distance for disjoint OBBs."; | |
| 1194 |
3/4✓ Branch 0 taken 71 times.
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 71 times.
|
100 | } else if (!overlap && result.squaredLowerBoundDistance < 0) { |
| 1195 | ✗ | std::cerr << "Failure: negative distance lower bound."; | |
| 1196 |
3/4✓ Branch 0 taken 71 times.
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 100 times.
|
171 | } else if (!overlap && eps < std::sqrt(result.squaredLowerBoundDistance) - |
| 1197 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 71 times.
|
71 | result.distance) { |
| 1198 | ✗ | std::cerr << "Failure: distance is inferior to its lower bound (diff = " | |
| 1199 | ✗ | << std::sqrt(result.squaredLowerBoundDistance) - result.distance | |
| 1200 | ✗ | << ")."; | |
| 1201 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
100 | } else if (overlap != !disjoint && result.distance >= dist_thr - eps) { |
| 1202 | ✗ | std::cerr << "Failure: overlapping test and distance query mismatch."; | |
| 1203 |
3/4✓ Branch 0 taken 29 times.
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29 times.
|
100 | } else if (overlap && result.distance >= dist_thr - eps) { |
| 1204 | ✗ | std::cerr << "Failure: positive distance for overlapping OBBs."; | |
| 1205 | } else { | ||
| 1206 | 100 | result.failure = false; | |
| 1207 | } | ||
| 1208 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
|
100 | if (result.failure) { |
| 1209 | ✗ | std::cerr << "\nR = " << Quats(B).coeffs().transpose().format(py_fmt) | |
| 1210 | ✗ | << "\nT = " << T.transpose().format(py_fmt) | |
| 1211 | ✗ | << "\na = " << a.transpose().format(py_fmt) | |
| 1212 | ✗ | << "\nb = " << b.transpose().format(py_fmt) | |
| 1213 | ✗ | << "\nresult = " << result << '\n' | |
| 1214 | ✗ | << std::endl; | |
| 1215 | } | ||
| 1216 | |||
| 1217 | // Compute time | ||
| 1218 | Scalar tmp; | ||
| 1219 | 100 | clock_type::time_point start, end; | |
| 1220 | |||
| 1221 | // ------------------------ 0 -------------------------------------- | ||
| 1222 | 100 | start = clock_type::now(); | |
| 1223 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
|
1100 | for (std::size_t i = 0; i < N; ++i) |
| 1224 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | obbDisjoint_impls::withRuntimeLoop(B, T, a, b, breakDistance2, tmp); |
| 1225 | 100 | end = clock_type::now(); | |
| 1226 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | result.duration[0] = (end - start) / N; |
| 1227 | |||
| 1228 | // ------------------------ 1 -------------------------------------- | ||
| 1229 | 100 | start = clock_type::now(); | |
| 1230 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
|
1100 | for (std::size_t i = 0; i < N; ++i) |
| 1231 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | obbDisjoint_impls::withManualLoopUnrolling_1(B, T, a, b, breakDistance2, |
| 1232 | tmp); | ||
| 1233 | 100 | end = clock_type::now(); | |
| 1234 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | result.duration[1] = (end - start) / N; |
| 1235 | |||
| 1236 | // ------------------------ 2 -------------------------------------- | ||
| 1237 | 100 | start = clock_type::now(); | |
| 1238 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
|
1100 | for (std::size_t i = 0; i < N; ++i) |
| 1239 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | obbDisjoint_impls::withManualLoopUnrolling_2(B, T, a, b, breakDistance2, |
| 1240 | tmp); | ||
| 1241 | 100 | end = clock_type::now(); | |
| 1242 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | result.duration[2] = (end - start) / N; |
| 1243 | |||
| 1244 | // ------------------------ 3 -------------------------------------- | ||
| 1245 | 100 | start = clock_type::now(); | |
| 1246 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
|
1100 | for (std::size_t i = 0; i < N; ++i) |
| 1247 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | obbDisjoint_impls::withTemplateLoopUnrolling_1(B, T, a, b, breakDistance2, |
| 1248 | tmp); | ||
| 1249 | 100 | end = clock_type::now(); | |
| 1250 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | result.duration[3] = (end - start) / N; |
| 1251 | |||
| 1252 | // ------------------------ 4 -------------------------------------- | ||
| 1253 | 100 | start = clock_type::now(); | |
| 1254 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
|
1100 | for (std::size_t i = 0; i < N; ++i) |
| 1255 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | obbDisjoint_impls::withPartialTemplateLoopUnrolling_1(B, T, a, b, |
| 1256 | breakDistance2, tmp); | ||
| 1257 | 100 | end = clock_type::now(); | |
| 1258 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | result.duration[4] = (end - start) / N; |
| 1259 | |||
| 1260 | // ------------------------ 5 -------------------------------------- | ||
| 1261 | 100 | start = clock_type::now(); | |
| 1262 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
|
1100 | for (std::size_t i = 0; i < N; ++i) |
| 1263 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | obbDisjoint_impls::originalWithLowerBound(B, T, a, b, breakDistance2, tmp); |
| 1264 | 100 | end = clock_type::now(); | |
| 1265 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | result.duration[5] = (end - start) / N; |
| 1266 | |||
| 1267 | // ------------------------ 6 -------------------------------------- | ||
| 1268 | 100 | start = clock_type::now(); | |
| 1269 | // The 2 last arguments are unused. | ||
| 1270 |
2/2✓ Branch 0 taken 1000 times.
✓ Branch 1 taken 100 times.
|
1100 | for (std::size_t i = 0; i < N; ++i) |
| 1271 |
1/2✓ Branch 1 taken 1000 times.
✗ Branch 2 not taken.
|
1000 | obbDisjoint_impls::originalWithNoLowerBound(B, T, a, b, breakDistance2, |
| 1272 | tmp); | ||
| 1273 | 100 | end = clock_type::now(); | |
| 1274 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
100 | result.duration[6] = (end - start) / N; |
| 1275 | |||
| 1276 | 200 | return result; | |
| 1277 | } | ||
| 1278 | |||
| 1279 | 1 | std::size_t obb_overlap_and_lower_bound_distance(std::ostream* output) { | |
| 1280 | 1 | std::size_t nbFailure = 0; | |
| 1281 | |||
| 1282 | // Create two OBBs axis | ||
| 1283 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Vec3s a, b; |
| 1284 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Matrix3s B; |
| 1285 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Vec3s T; |
| 1286 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CollisionRequest request; |
| 1287 | |||
| 1288 | #ifndef NDEBUG // if debug mode | ||
| 1289 | static const size_t nbRandomOBB = 10; | ||
| 1290 | static const size_t nbTransformPerOBB = 10; | ||
| 1291 | static const size_t nbRunForTimeMeas = 10; | ||
| 1292 | #else | ||
| 1293 | static const size_t nbRandomOBB = 100; | ||
| 1294 | static const size_t nbTransformPerOBB = 100; | ||
| 1295 | static const size_t nbRunForTimeMeas = 1000; | ||
| 1296 | #endif | ||
| 1297 | static const Scalar extentNorm = 1.; | ||
| 1298 | |||
| 1299 |
1/6✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
1 | if (output != NULL) *output << BenchmarkResult::headers << '\n'; |
| 1300 | |||
| 1301 | BenchmarkResult result; | ||
| 1302 |
2/2✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
|
11 | for (std::size_t iobb = 0; iobb < nbRandomOBB; ++iobb) { |
| 1303 |
1/2✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
|
10 | randomOBBs(a, b, extentNorm); |
| 1304 |
2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 10 times.
|
110 | for (std::size_t itf = 0; itf < nbTransformPerOBB; ++itf) { |
| 1305 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | randomTransform(B, T, a, b, extentNorm); |
| 1306 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
100 | result = benchmark_obb_case(B, T, a, b, request, nbRunForTimeMeas); |
| 1307 |
1/6✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
100 | if (output != NULL) *output << result << '\n'; |
| 1308 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 100 times.
|
100 | if (result.failure) nbFailure++; |
| 1309 | } | ||
| 1310 | } | ||
| 1311 | 1 | return nbFailure; | |
| 1312 | } | ||
| 1313 | |||
| 1314 | 1 | int main(int argc, char** argv) { | |
| 1315 | 1 | std::ostream* output = NULL; | |
| 1316 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
1 | if (argc > 1 && strcmp(argv[1], "--generate-output") == 0) { |
| 1317 | ✗ | output = &std::cout; | |
| 1318 | } | ||
| 1319 | |||
| 1320 | std::cout << "The benchmark real time measurements may be noisy and " | ||
| 1321 | "will incur extra overhead." | ||
| 1322 | "\nUse the following commands to turn ON:" | ||
| 1323 | "\n\tsudo cpufreq-set --governor performance" | ||
| 1324 | "\nor OFF:" | ||
| 1325 | "\n\tsudo cpufreq-set --governor powersave" | ||
| 1326 | 1 | "\n"; | |
| 1327 | |||
| 1328 | 1 | std::size_t nbFailure = obb_overlap_and_lower_bound_distance(output); | |
| 1329 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
|
1 | if (nbFailure > INT_MAX) return INT_MAX; |
| 1330 | 1 | return (int)nbFailure; | |
| 1331 | } | ||
| 1332 |