GCC Code Coverage Report

Directory: ./
File: src/BV/OBB.cpp
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 215 243 88.5%
Branches: 478 920 52.0%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan, Florent Lamiraux */
37
38 #include "coal/BV/OBB.h"
39 #include "coal/BVH/BVH_utility.h"
40 #include "coal/math/transform.h"
41 #include "coal/collision_data.h"
42 #include "coal/internal/tools.h"
43
44 #include <iostream>
45 #include <limits>
46
47 namespace coal {
48
49 /// @brief Compute the 8 vertices of a OBB
50 39222 inline void computeVertices(const OBB& b, Vec3s vertices[8]) {
51
3/6
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
 39222 Matrix3s extAxes(b.axes * b.extent.asDiagonal());
52
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[0].noalias() = b.To + extAxes * Vec3s(-1, -1, -1);
53
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[1].noalias() = b.To + extAxes * Vec3s(1, -1, -1);
54
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[2].noalias() = b.To + extAxes * Vec3s(1, 1, -1);
55
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[3].noalias() = b.To + extAxes * Vec3s(-1, 1, -1);
56
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[4].noalias() = b.To + extAxes * Vec3s(-1, -1, 1);
57
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[5].noalias() = b.To + extAxes * Vec3s(1, -1, 1);
58
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[6].noalias() = b.To + extAxes * Vec3s(1, 1, 1);
59
5/10
✓ Branch 1 taken 39222 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39222 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39222 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39222 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39222 times.
✗ Branch 14 not taken.
 39222 vertices[7].noalias() = b.To + extAxes * Vec3s(-1, 1, 1);
60 39222 }
61
62 /// @brief OBB merge method when the centers of two smaller OBB are far away
63 21 inline OBB merge_largedist(const OBB& b1, const OBB& b2) {
64
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 OBB b;
65
3/4
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 336 times.
✓ Branch 4 taken 21 times.
 357 Vec3s vertex[16];
66
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 computeVertices(b1, vertex);
67
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 computeVertices(b2, vertex + 8);
68
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 Matrix3s M;
69
3/4
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 63 times.
✓ Branch 4 taken 21 times.
 84 Vec3s E[3];
70 21 Scalar s[3] = {0, 0, 0};
71
72 // obb axes
73
5/10
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 21 times.
✗ Branch 14 not taken.
 21 b.axes.col(0).noalias() = (b1.To - b2.To).normalized();
74
75
3/4
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 336 times.
✓ Branch 4 taken 21 times.
 357 Vec3s vertex_proj[16];
76
2/2
✓ Branch 0 taken 336 times.
✓ Branch 1 taken 21 times.
 357 for (int i = 0; i < 16; ++i)
77
1/2
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
 336 vertex_proj[i].noalias() =
78
6/12
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 336 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 336 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 336 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 336 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 336 times.
✗ Branch 17 not taken.
 672 vertex[i] - b.axes.col(0) * vertex[i].dot(b.axes.col(0));
79
80
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 getCovariance(vertex_proj, NULL, NULL, NULL, 16, M);
81
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 eigen(M, s, E);
82
83 int min, mid, max;
84
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 18 times.
 21 if (s[0] > s[1]) {
85 3 max = 0;
86 3 min = 1;
87 } else {
88 18 min = 0;
89 18 max = 1;
90 }
91
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 12 times.
 21 if (s[2] < s[min]) {
92 9 mid = min;
93 9 min = 2;
94
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 } else if (s[2] > s[max]) {
95 6 mid = max;
96 6 max = 2;
97 } else {
98 6 mid = 2;
99 }
100
101
7/14
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 21 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 21 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 21 times.
✗ Branch 20 not taken.
 21 b.axes.col(1) << E[0][max], E[1][max], E[2][max];
102
7/14
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 21 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 21 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 21 times.
✗ Branch 20 not taken.
 21 b.axes.col(2) << E[0][mid], E[1][mid], E[2][mid];
103
104 // set obb centers and extensions
105
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
 21 Vec3s center, extent;
106
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 getExtentAndCenter(vertex, NULL, NULL, NULL, 16, b.axes, center, extent);
107
108
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
 21 b.To.noalias() = center;
109
2/4
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21 times.
✗ Branch 5 not taken.
 21 b.extent.noalias() = extent;
110
111 42 return b;
112 }
113
114 /// @brief OBB merge method when the centers of two smaller OBB are close
115 19590 inline OBB merge_smalldist(const OBB& b1, const OBB& b2) {
116
1/2
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
 19590 OBB b;
117
3/6
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19590 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19590 times.
✗ Branch 8 not taken.
 19590 b.To = (b1.To + b2.To) * 0.5;
118
2/4
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19590 times.
✗ Branch 5 not taken.
 19590 Quats q0(b1.axes), q1(b2.axes);
119
4/6
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5466 times.
✓ Branch 4 taken 14124 times.
✓ Branch 7 taken 5466 times.
✗ Branch 8 not taken.
 19590 if (q0.dot(q1) < 0) q1.coeffs() *= -1;
120
121
3/6
✓ Branch 3 taken 19590 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 19590 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 19590 times.
✗ Branch 10 not taken.
 19590 Quats q((q0.coeffs() + q1.coeffs()).normalized());
122
1/2
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
 19590 b.axes = q.toRotationMatrix();
123
124
4/6
✓ Branch 1 taken 156720 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 156720 times.
✓ Branch 4 taken 19590 times.
✓ Branch 6 taken 19590 times.
✗ Branch 7 not taken.
 176310 Vec3s vertex[8], diff;
125 19590 Scalar real_max = (std::numeric_limits<Scalar>::max)();
126
1/2
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
 19590 Vec3s pmin(real_max, real_max, real_max);
127
1/2
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
 19590 Vec3s pmax(-real_max, -real_max, -real_max);
128
129
1/2
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
 19590 computeVertices(b1, vertex);
130
2/2
✓ Branch 0 taken 156720 times.
✓ Branch 1 taken 19590 times.
 176310 for (int i = 0; i < 8; ++i) {
131
2/4
✓ Branch 1 taken 156720 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 156720 times.
✗ Branch 5 not taken.
 156720 diff = vertex[i] - b.To;
132
2/2
✓ Branch 0 taken 470160 times.
✓ Branch 1 taken 156720 times.
 626880 for (int j = 0; j < 3; ++j) {
133
2/4
✓ Branch 1 taken 470160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 470160 times.
✗ Branch 5 not taken.
 470160 Scalar dot = diff.dot(b.axes.col(j));
134
3/4
✓ Branch 1 taken 470160 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 175926 times.
✓ Branch 4 taken 294234 times.
 470160 if (dot > pmax[j])
135
1/2
✓ Branch 1 taken 175926 times.
✗ Branch 2 not taken.
 175926 pmax[j] = dot;
136
3/4
✓ Branch 1 taken 294234 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 114381 times.
✓ Branch 4 taken 179853 times.
 294234 else if (dot < pmin[j])
137
1/2
✓ Branch 1 taken 114381 times.
✗ Branch 2 not taken.
 114381 pmin[j] = dot;
138 }
139 }
140
141
1/2
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
 19590 computeVertices(b2, vertex);
142
2/2
✓ Branch 0 taken 156720 times.
✓ Branch 1 taken 19590 times.
 176310 for (int i = 0; i < 8; ++i) {
143
2/4
✓ Branch 1 taken 156720 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 156720 times.
✗ Branch 5 not taken.
 156720 diff = vertex[i] - b.To;
144
2/2
✓ Branch 0 taken 470160 times.
✓ Branch 1 taken 156720 times.
 626880 for (int j = 0; j < 3; ++j) {
145
2/4
✓ Branch 1 taken 470160 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 470160 times.
✗ Branch 5 not taken.
 470160 Scalar dot = diff.dot(b.axes.col(j));
146
3/4
✓ Branch 1 taken 470160 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 55653 times.
✓ Branch 4 taken 414507 times.
 470160 if (dot > pmax[j])
147
1/2
✓ Branch 1 taken 55653 times.
✗ Branch 2 not taken.
 55653 pmax[j] = dot;
148
3/4
✓ Branch 1 taken 414507 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 52290 times.
✓ Branch 4 taken 362217 times.
 414507 else if (dot < pmin[j])
149
1/2
✓ Branch 1 taken 52290 times.
✗ Branch 2 not taken.
 52290 pmin[j] = dot;
150 }
151 }
152
153
2/2
✓ Branch 0 taken 58770 times.
✓ Branch 1 taken 19590 times.
 78360 for (int j = 0; j < 3; ++j) {
154
6/12
✓ Branch 1 taken 58770 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 58770 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 58770 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 58770 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 58770 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 58770 times.
✗ Branch 17 not taken.
 58770 b.To.noalias() += (b.axes.col(j) * (0.5 * (pmax[j] + pmin[j])));
155
3/6
✓ Branch 1 taken 58770 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 58770 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 58770 times.
✗ Branch 8 not taken.
 58770 b.extent[j] = Scalar(0.5) * (pmax[j] - pmin[j]);
156 }
157
158 39180 return b;
159 }
160
161 6000 bool obbDisjoint(const Matrix3s& B, const Vec3s& T, const Vec3s& a,
162 const Vec3s& b) {
163 Scalar t, s;
164 6000 const Scalar reps = Scalar(1e-6);
165
166
4/8
✓ Branch 1 taken 6000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6000 times.
✗ Branch 11 not taken.
 6000 Matrix3s Bf(B.array().abs() + reps);
167 // Bf += reps;
168
169 // if any of these tests are one-sided, then the polyhedra are disjoint
170
171 // A1 x A2 = A0
172
5/8
✓ Branch 1 taken 6000 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1392 times.
✓ Branch 4 taken 4608 times.
✓ Branch 6 taken 1392 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4608 times.
✗ Branch 10 not taken.
 6000 t = ((T[0] < 0.0) ? -T[0] : T[0]);
173
174 // if(t > (a[0] + Bf.dotX(b)))
175
5/8
✓ Branch 1 taken 6000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6000 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 469 times.
✓ Branch 10 taken 5531 times.
 6000 if (t > (a[0] + Bf.row(0).dot(b))) return true;
176
177 // B1 x B2 = B0
178 // s = B.transposeDotX(T);
179
2/4
✓ Branch 1 taken 5531 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5531 times.
✗ Branch 5 not taken.
 5531 s = B.col(0).dot(T);
180
2/2
✓ Branch 0 taken 2397 times.
✓ Branch 1 taken 3134 times.
 5531 t = ((s < 0.0) ? -s : s);
181
182 // if(t > (b[0] + Bf.transposeDotX(a)))
183
5/8
✓ Branch 1 taken 5531 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5531 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5531 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 564 times.
✓ Branch 10 taken 4967 times.
 5531 if (t > (b[0] + Bf.col(0).dot(a))) return true;
184
185 // A2 x A0 = A1
186
5/8
✓ Branch 1 taken 4967 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4456 times.
✓ Branch 4 taken 511 times.
✓ Branch 6 taken 4456 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 511 times.
✗ Branch 10 not taken.
 4967 t = ((T[1] < 0.0) ? -T[1] : T[1]);
187
188 // if(t > (a[1] + Bf.dotY(b)))
189
5/8
✓ Branch 1 taken 4967 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4967 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4967 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 1102 times.
✓ Branch 10 taken 3865 times.
 4967 if (t > (a[1] + Bf.row(1).dot(b))) return true;
190
191 // A0 x A1 = A2
192
5/8
✓ Branch 1 taken 3865 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1455 times.
✓ Branch 4 taken 2410 times.
✓ Branch 6 taken 1455 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2410 times.
✗ Branch 10 not taken.
 3865 t = ((T[2] < 0.0) ? -T[2] : T[2]);
193
194 // if(t > (a[2] + Bf.dotZ(b)))
195
5/8
✓ Branch 1 taken 3865 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3865 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3865 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 149 times.
✓ Branch 10 taken 3716 times.
 3865 if (t > (a[2] + Bf.row(2).dot(b))) return true;
196
197 // B2 x B0 = B1
198 // s = B.transposeDotY(T);
199
2/4
✓ Branch 1 taken 3716 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3716 times.
✗ Branch 5 not taken.
 3716 s = B.col(1).dot(T);
200
2/2
✓ Branch 0 taken 1667 times.
✓ Branch 1 taken 2049 times.
 3716 t = ((s < 0.0) ? -s : s);
201
202 // if(t > (b[1] + Bf.transposeDotY(a)))
203
5/8
✓ Branch 1 taken 3716 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3716 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3716 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 104 times.
✓ Branch 10 taken 3612 times.
 3716 if (t > (b[1] + Bf.col(1).dot(a))) return true;
204
205 // B0 x B1 = B2
206 // s = B.transposeDotZ(T);
207
2/4
✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
 3612 s = B.col(2).dot(T);
208
2/2
✓ Branch 0 taken 1738 times.
✓ Branch 1 taken 1874 times.
 3612 t = ((s < 0.0) ? -s : s);
209
210 // if(t > (b[2] + Bf.transposeDotZ(a)))
211
5/8
✓ Branch 1 taken 3612 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3612 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3612 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 107 times.
✓ Branch 10 taken 3505 times.
 3612 if (t > (b[2] + Bf.col(2).dot(a))) return true;
212
213 // A0 x B0
214
4/8
✓ Branch 1 taken 3505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3505 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3505 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3505 times.
✗ Branch 11 not taken.
 3505 s = T[2] * B(1, 0) - T[1] * B(2, 0);
215
2/2
✓ Branch 0 taken 1099 times.
✓ Branch 1 taken 2406 times.
 3505 t = ((s < 0.0) ? -s : s);
216
217 3505 if (t >
218
10/18
✓ Branch 1 taken 3505 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3505 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3505 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3505 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3505 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3505 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3505 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3505 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 21 times.
✓ Branch 25 taken 3484 times.
 3505 (a[1] * Bf(2, 0) + a[2] * Bf(1, 0) + b[1] * Bf(0, 2) + b[2] * Bf(0, 1)))
219 21 return true;
220
221 // A0 x B1
222
4/8
✓ Branch 1 taken 3484 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3484 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3484 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3484 times.
✗ Branch 11 not taken.
 3484 s = T[2] * B(1, 1) - T[1] * B(2, 1);
223
2/2
✓ Branch 0 taken 1913 times.
✓ Branch 1 taken 1571 times.
 3484 t = ((s < 0.0) ? -s : s);
224
225 3484 if (t >
226
10/18
✓ Branch 1 taken 3484 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3484 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3484 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3484 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3484 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3484 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3484 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3484 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 4 times.
✓ Branch 25 taken 3480 times.
 3484 (a[1] * Bf(2, 1) + a[2] * Bf(1, 1) + b[0] * Bf(0, 2) + b[2] * Bf(0, 0)))
227 4 return true;
228
229 // A0 x B2
230
4/8
✓ Branch 1 taken 3480 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3480 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3480 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3480 times.
✗ Branch 11 not taken.
 3480 s = T[2] * B(1, 2) - T[1] * B(2, 2);
231
2/2
✓ Branch 0 taken 1982 times.
✓ Branch 1 taken 1498 times.
 3480 t = ((s < 0.0) ? -s : s);
232
233 3480 if (t >
234
10/18
✓ Branch 1 taken 3480 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3480 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3480 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3480 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3480 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3480 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3480 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3480 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 18 times.
✓ Branch 25 taken 3462 times.
 3480 (a[1] * Bf(2, 2) + a[2] * Bf(1, 2) + b[0] * Bf(0, 1) + b[1] * Bf(0, 0)))
235 18 return true;
236
237 // A1 x B0
238
4/8
✓ Branch 1 taken 3462 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3462 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3462 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3462 times.
✗ Branch 11 not taken.
 3462 s = T[0] * B(2, 0) - T[2] * B(0, 0);
239
2/2
✓ Branch 0 taken 1612 times.
✓ Branch 1 taken 1850 times.
 3462 t = ((s < 0.0) ? -s : s);
240
241 3462 if (t >
242
10/18
✓ Branch 1 taken 3462 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3462 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3462 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3462 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3462 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3462 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3462 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3462 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 8 times.
✓ Branch 25 taken 3454 times.
 3462 (a[0] * Bf(2, 0) + a[2] * Bf(0, 0) + b[1] * Bf(1, 2) + b[2] * Bf(1, 1)))
243 8 return true;
244
245 // A1 x B1
246
4/8
✓ Branch 1 taken 3454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3454 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3454 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3454 times.
✗ Branch 11 not taken.
 3454 s = T[0] * B(2, 1) - T[2] * B(0, 1);
247
2/2
✓ Branch 0 taken 1446 times.
✓ Branch 1 taken 2008 times.
 3454 t = ((s < 0.0) ? -s : s);
248
249 3454 if (t >
250
10/18
✓ Branch 1 taken 3454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3454 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3454 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3454 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3454 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3454 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3454 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3454 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 2 times.
✓ Branch 25 taken 3452 times.
 3454 (a[0] * Bf(2, 1) + a[2] * Bf(0, 1) + b[0] * Bf(1, 2) + b[2] * Bf(1, 0)))
251 2 return true;
252
253 // A1 x B2
254
4/8
✓ Branch 1 taken 3452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3452 times.
✗ Branch 11 not taken.
 3452 s = T[0] * B(2, 2) - T[2] * B(0, 2);
255
2/2
✓ Branch 0 taken 1940 times.
✓ Branch 1 taken 1512 times.
 3452 t = ((s < 0.0) ? -s : s);
256
257 3452 if (t >
258
10/18
✓ Branch 1 taken 3452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3452 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3452 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3452 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 12 times.
✓ Branch 25 taken 3440 times.
 3452 (a[0] * Bf(2, 2) + a[2] * Bf(0, 2) + b[0] * Bf(1, 1) + b[1] * Bf(1, 0)))
259 12 return true;
260
261 // A2 x B0
262
4/8
✓ Branch 1 taken 3440 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3440 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3440 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3440 times.
✗ Branch 11 not taken.
 3440 s = T[1] * B(0, 0) - T[0] * B(1, 0);
263
2/2
✓ Branch 0 taken 1889 times.
✓ Branch 1 taken 1551 times.
 3440 t = ((s < 0.0) ? -s : s);
264
265 3440 if (t >
266
10/18
✓ Branch 1 taken 3440 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3440 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3440 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3440 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3440 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3440 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3440 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3440 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 30 times.
✓ Branch 25 taken 3410 times.
 3440 (a[0] * Bf(1, 0) + a[1] * Bf(0, 0) + b[1] * Bf(2, 2) + b[2] * Bf(2, 1)))
267 30 return true;
268
269 // A2 x B1
270
4/8
✓ Branch 1 taken 3410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3410 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3410 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3410 times.
✗ Branch 11 not taken.
 3410 s = T[1] * B(0, 1) - T[0] * B(1, 1);
271
2/2
✓ Branch 0 taken 1496 times.
✓ Branch 1 taken 1914 times.
 3410 t = ((s < 0.0) ? -s : s);
272
273 3410 if (t >
274
10/18
✓ Branch 1 taken 3410 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3410 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3410 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3410 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3410 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3410 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3410 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3410 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 50 times.
✓ Branch 25 taken 3360 times.
 3410 (a[0] * Bf(1, 1) + a[1] * Bf(0, 1) + b[0] * Bf(2, 2) + b[2] * Bf(2, 0)))
275 50 return true;
276
277 // A2 x B2
278
4/8
✓ Branch 1 taken 3360 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3360 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3360 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3360 times.
✗ Branch 11 not taken.
 3360 s = T[1] * B(0, 2) - T[0] * B(1, 2);
279
2/2
✓ Branch 0 taken 1546 times.
✓ Branch 1 taken 1814 times.
 3360 t = ((s < 0.0) ? -s : s);
280
281 3360 if (t >
282
10/18
✓ Branch 1 taken 3360 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3360 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3360 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3360 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3360 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 3360 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 3360 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 3360 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 8 times.
✓ Branch 25 taken 3352 times.
 3360 (a[0] * Bf(1, 2) + a[1] * Bf(0, 2) + b[0] * Bf(2, 1) + b[1] * Bf(2, 0)))
283 8 return true;
284
285 3352 return false;
286 }
287
288 namespace internal {
289 227393 inline Scalar obbDisjoint_check_A_axis(const Vec3s& T, const Vec3s& a,
290 const Vec3s& b, const Matrix3s& Bf) {
291 // |T| - |B| * b - a
292
3/6
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 227393 times.
✗ Branch 8 not taken.
 227393 Vec3s AABB_corner(T.cwiseAbs() - a);
293
3/6
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 227393 times.
✗ Branch 8 not taken.
 227393 AABB_corner.noalias() -= Bf * b;
294
4/8
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 227393 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 227393 times.
✗ Branch 11 not taken.
 227393 return AABB_corner.array().max(Scalar(0)).matrix().squaredNorm();
295 }
296
297 130812 inline Scalar obbDisjoint_check_B_axis(const Matrix3s& B, const Vec3s& T,
298 const Vec3s& a, const Vec3s& b,
299 const Matrix3s& Bf) {
300 // Bf = |B|
301 // | B^T T| - Bf^T * a - b
302 130812 Scalar s, t = 0;
303
4/8
✓ Branch 2 taken 130812 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 130812 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 130812 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 130812 times.
✗ Branch 13 not taken.
 130812 s = std::abs(B.col(0).dot(T)) - Bf.col(0).dot(a) - b[0];
304
2/2
✓ Branch 0 taken 1232 times.
✓ Branch 1 taken 129580 times.
 130812 if (s > 0) t += s * s;
305
4/8
✓ Branch 2 taken 130812 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 130812 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 130812 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 130812 times.
✗ Branch 13 not taken.
 130812 s = std::abs(B.col(1).dot(T)) - Bf.col(1).dot(a) - b[1];
306
2/2
✓ Branch 0 taken 4304 times.
✓ Branch 1 taken 126508 times.
 130812 if (s > 0) t += s * s;
307
4/8
✓ Branch 2 taken 130812 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 130812 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 130812 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 130812 times.
✗ Branch 13 not taken.
 130812 s = std::abs(B.col(2).dot(T)) - Bf.col(2).dot(a) - b[2];
308
2/2
✓ Branch 0 taken 13764 times.
✓ Branch 1 taken 117048 times.
 130812 if (s > 0) t += s * s;
309 130812 return t;
310 }
311
312 template <int ib, int jb = (ib + 1) % 3, int kb = (ib + 2) % 3>
313 1955040 struct COAL_LOCAL obbDisjoint_check_Ai_cross_Bi{static inline bool run(
314 int ia, int ja, int ka, const Matrix3s& B, const Vec3s& T, const Vec3s& a,
315 const Vec3s& b, const Matrix3s& Bf, const Scalar& breakDistance2,
316 1955040 Scalar& squaredLowerBoundDistance){Scalar sinus2 =
317 1955040 1 - Bf(ia, ib) * Bf(ia, ib);
318
2/2
✓ Branch 0 taken 1610 times.
✓ Branch 1 taken 975910 times.
 1955040 if (sinus2 < 1e-6) return false;
319
320 1951820 const Scalar s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
321
322 1951820 const Scalar diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
323 1951820 b[jb] * Bf(ia, kb) + b[kb] * Bf(ia, jb));
324 // We need to divide by the norm || Aia x Bib ||
325 // As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
326
2/2
✓ Branch 0 taken 5990 times.
✓ Branch 1 taken 969920 times.
 1951820 if (diff > 0) {
327 11980 squaredLowerBoundDistance = diff * diff / sinus2;
328
2/2
✓ Branch 0 taken 5847 times.
✓ Branch 1 taken 143 times.
 11980 if (squaredLowerBoundDistance > breakDistance2) {
329 11694 return true;
330 }
331 }
332 1940126 return false;
333 } // namespace internal
334 }; // namespace coal
335 } // namespace internal
336
337 // B, T orientation and position of 2nd OBB in frame of 1st OBB,
338 // a extent of 1st OBB,
339 // b extent of 2nd OBB.
340 //
341 // This function tests whether bounding boxes should be broken down.
342 //
343 227393 bool obbDisjointAndLowerBoundDistance(const Matrix3s& B, const Vec3s& T,
344 const Vec3s& a_, const Vec3s& b_,
345 const CollisionRequest& request,
346 Scalar& squaredLowerBoundDistance) {
347
3/6
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 227393 times.
✗ Branch 9 not taken.
 227393 assert(request.security_margin >
348 -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) -
349 10 * Eigen::NumTraits<Scalar>::epsilon() &&
350 "A negative security margin could not be lower than the OBB extent.");
351 // const Scalar breakDistance(request.break_distance +
352 // request.security_margin);
353 227393 const Scalar breakDistance2 = request.break_distance * request.break_distance;
354
355
2/4
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
 227393 Matrix3s Bf(B.cwiseAbs());
356
2/4
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
 227393 const Vec3s a((a_ + Vec3s::Constant(request.security_margin / 2))
357
1/2
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
 227393 .array()
358
2/4
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
 227393 .max(Scalar(0)));
359
2/4
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
 227393 const Vec3s b((b_ + Vec3s::Constant(request.security_margin / 2))
360
1/2
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
 227393 .array()
361
2/4
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 227393 times.
✗ Branch 5 not taken.
 227393 .max(Scalar(0)));
362
363 // Corner of b axis aligned bounding box the closest to the origin
364
1/2
✓ Branch 1 taken 227393 times.
✗ Branch 2 not taken.
 227393 squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf);
365
2/2
✓ Branch 0 taken 96581 times.
✓ Branch 1 taken 130812 times.
 227393 if (squaredLowerBoundDistance > breakDistance2) return true;
366
367 130812 squaredLowerBoundDistance =
368
1/2
✓ Branch 1 taken 130812 times.
✗ Branch 2 not taken.
 130812 internal::obbDisjoint_check_B_axis(B, T, a, b, Bf);
369
2/2
✓ Branch 0 taken 17927 times.
✓ Branch 1 taken 112885 times.
 130812 if (squaredLowerBoundDistance > breakDistance2) return true;
370
371 // Ai x Bj
372 112885 int ja = 1, ka = 2;
373
2/2
✓ Branch 0 taken 329212 times.
✓ Branch 1 taken 107038 times.
 436250 for (int ia = 0; ia < 3; ++ia) {
374
3/4
✓ Branch 1 taken 329212 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4505 times.
✓ Branch 4 taken 324707 times.
 329212 if (internal::obbDisjoint_check_Ai_cross_Bi<0>::run(
375 ia, ja, ka, B, T, a, b, Bf, breakDistance2,
376 squaredLowerBoundDistance))
377 4505 return true;
378
3/4
✓ Branch 1 taken 324707 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1106 times.
✓ Branch 4 taken 323601 times.
 324707 if (internal::obbDisjoint_check_Ai_cross_Bi<1>::run(
379 ia, ja, ka, B, T, a, b, Bf, breakDistance2,
380 squaredLowerBoundDistance))
381 1106 return true;
382
3/4
✓ Branch 1 taken 323601 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 236 times.
✓ Branch 4 taken 323365 times.
 323601 if (internal::obbDisjoint_check_Ai_cross_Bi<2>::run(
383 ia, ja, ka, B, T, a, b, Bf, breakDistance2,
384 squaredLowerBoundDistance))
385 236 return true;
386 323365 ja = ka;
387 323365 ka = ia;
388 }
389
390 107038 return false;
391 }
392
393 6000 bool OBB::overlap(const OBB& other) const {
394 /// compute what transform [R,T] that takes us from cs1 to cs2.
395 /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
396 /// First compute the rotation part, then translation part
397
4/8
✓ Branch 1 taken 6000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6000 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6000 times.
✗ Branch 11 not taken.
 6000 Vec3s T(axes.transpose() * (other.To - To));
398
3/6
✓ Branch 1 taken 6000 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6000 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6000 times.
✗ Branch 8 not taken.
 6000 Matrix3s R(axes.transpose() * other.axes);
399
400
1/2
✓ Branch 1 taken 6000 times.
✗ Branch 2 not taken.
 6000 return !obbDisjoint(R, T, extent, other.extent);
401 }
402
403 154162 bool OBB::overlap(const OBB& other, const CollisionRequest& request,
404 Scalar& sqrDistLowerBound) const {
405 /// compute what transform [R,T] that takes us from cs1 to cs2.
406 /// [R,T] = [R1,T1]'[R2,T2] = [R1',-R1'T][R2,T2] = [R1'R2, R1'(T2-T1)]
407 /// First compute the rotation part, then translation part
408 // Vec3s t = other.To - To; // T2 - T1
409 // Vec3s T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
410 // Matrix3s R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
411 // axis[0].dot(other.axis[2]),
412 // axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
413 // axis[1].dot(other.axis[2]),
414 // axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
415 // axis[2].dot(other.axis[2]));
416
4/8
✓ Branch 1 taken 154162 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 154162 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 154162 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 154162 times.
✗ Branch 11 not taken.
 154162 Vec3s T(axes.transpose() * (other.To - To));
417
3/6
✓ Branch 1 taken 154162 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 154162 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 154162 times.
✗ Branch 8 not taken.
 154162 Matrix3s R(axes.transpose() * other.axes);
418
419
1/2
✓ Branch 1 taken 154162 times.
✗ Branch 2 not taken.
 154162 return !obbDisjointAndLowerBoundDistance(R, T, extent, other.extent, request,
420 154162 sqrDistLowerBound);
421 }
422
423 ✗ bool OBB::contain(const Vec3s& p) const {
424 ✗ Vec3s local_p(p - To);
425 ✗ Scalar proj = local_p.dot(axes.col(0));
426 ✗ if ((proj > extent[0]) || (proj < -extent[0])) return false;
427
428 ✗ proj = local_p.dot(axes.col(1));
429 ✗ if ((proj > extent[1]) || (proj < -extent[1])) return false;
430
431 ✗ proj = local_p.dot(axes.col(2));
432 ✗ if ((proj > extent[2]) || (proj < -extent[2])) return false;
433
434 ✗ return true;
435 }
436
437 ✗ OBB& OBB::operator+=(const Vec3s& p) {
438 ✗ OBB bvp;
439 ✗ bvp.To = p;
440 ✗ bvp.axes.noalias() = axes;
441 ✗ bvp.extent.setZero();
442
443 ✗ *this += bvp;
444 ✗ return *this;
445 }
446
447 19611 OBB OBB::operator+(const OBB& other) const {
448
2/4
✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19611 times.
✗ Branch 5 not taken.
 19611 Vec3s center_diff = To - other.To;
449
3/6
✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19611 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19611 times.
✗ Branch 8 not taken.
 19611 Scalar max_extent = std::max(std::max(extent[0], extent[1]), extent[2]);
450 Scalar max_extent2 =
451
3/6
✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19611 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19611 times.
✗ Branch 8 not taken.
 19611 std::max(std::max(other.extent[0], other.extent[1]), other.extent[2]);
452
3/4
✓ Branch 1 taken 19611 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 21 times.
✓ Branch 4 taken 19590 times.
 19611 if (center_diff.norm() > 2 * (max_extent + max_extent2)) {
453
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 21 return merge_largedist(*this, other);
454 } else {
455
1/2
✓ Branch 1 taken 19590 times.
✗ Branch 2 not taken.
 19590 return merge_smalldist(*this, other);
456 }
457 }
458
459 ✗ Scalar OBB::distance(const OBB& /*other*/, Vec3s* /*P*/, Vec3s* /*Q*/) const {
460 ✗ std::cerr << "OBB distance not implemented!" << std::endl;
461 ✗ return 0.0;
462 }
463
464 ✗ bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1,
465 const OBB& b2) {
466 ✗ Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
467 ✗ Vec3s T(b1.axes.transpose() * Ttemp);
468 ✗ Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
469
470 ✗ return !obbDisjoint(R, T, b1.extent, b2.extent);
471 }
472
473 73231 bool overlap(const Matrix3s& R0, const Vec3s& T0, const OBB& b1, const OBB& b2,
474 const CollisionRequest& request, Scalar& sqrDistLowerBound) {
475
5/10
✓ Branch 1 taken 73231 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 73231 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 73231 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 73231 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 73231 times.
✗ Branch 14 not taken.
 73231 Vec3s Ttemp(R0.transpose() * (b2.To - T0) - b1.To);
476
3/6
✓ Branch 1 taken 73231 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 73231 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 73231 times.
✗ Branch 8 not taken.
 73231 Vec3s T(b1.axes.transpose() * Ttemp);
477
5/10
✓ Branch 1 taken 73231 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 73231 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 73231 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 73231 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 73231 times.
✗ Branch 14 not taken.
 73231 Matrix3s R(b1.axes.transpose() * R0.transpose() * b2.axes);
478
479
1/2
✓ Branch 1 taken 73231 times.
✗ Branch 2 not taken.
 73231 return !obbDisjointAndLowerBoundDistance(R, T, b1.extent, b2.extent, request,
480 73231 sqrDistLowerBound);
481 }
482
483 ✗ OBB translate(const OBB& bv, const Vec3s& t) {
484 ✗ OBB res(bv);
485 ✗ res.To += t;
486 ✗ return res;
487 }
488
489 } // namespace coal
490