Directory: | ./ |
---|---|
File: | src/BVH/BVH_utility.cpp |
Date: | 2025-04-01 09:23:31 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 332 | 387 | 85.8% |
Branches: | 401 | 922 | 43.5% |
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 */ | ||
37 | |||
38 | #include "coal/BVH/BVH_utility.h" | ||
39 | #include "coal/narrowphase/narrowphase.h" | ||
40 | #include "coal/shape/geometric_shapes_utility.h" | ||
41 | #include "coal/internal/shape_shape_func.h" | ||
42 | |||
43 | namespace coal { | ||
44 | |||
45 | namespace details { | ||
46 | template <typename BV> | ||
47 | 80 | BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3s& pose, | |
48 | const AABB& _aabb) { | ||
49 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 40 times.
|
80 | assert(model.getModelType() == BVH_MODEL_TRIANGLES); |
50 | 80 | const Matrix3s& q = pose.getRotation(); | |
51 |
3/6✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40 times.
✗ Branch 9 not taken.
|
80 | AABB aabb = translate(_aabb, -pose.getTranslation()); |
52 | |||
53 |
1/2✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
|
80 | Transform3s box_pose; |
54 |
1/2✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
|
80 | Box box; |
55 |
1/2✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
|
80 | constructBox(_aabb, box, box_pose); |
56 |
2/4✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
|
80 | box_pose = pose.inverseTimes(box_pose); |
57 | |||
58 |
1/2✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
|
80 | GJKSolver gjk; |
59 | // Early-stop GJK as soon as a separating plane is found | ||
60 | 80 | gjk.gjk.setDistanceEarlyBreak(Scalar(1e-3)); | |
61 | |||
62 | // Check what triangles should be kept. | ||
63 | // TODO use the BV hierarchy | ||
64 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
80 | std::vector<bool> keep_vertex(model.num_vertices, false); |
65 |
1/2✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
|
80 | std::vector<bool> keep_tri(model.num_tris, false); |
66 | 80 | unsigned int ntri = 0; | |
67 | 80 | const std::vector<Vec3s>& model_vertices_ = *(model.vertices); | |
68 | 80 | const std::vector<Triangle>& model_tri_indices_ = *(model.tri_indices); | |
69 |
2/2✓ Branch 0 taken 480 times.
✓ Branch 1 taken 40 times.
|
1040 | for (unsigned int i = 0; i < model.num_tris; ++i) { |
70 | 960 | const Triangle& t = model_tri_indices_[i]; | |
71 | |||
72 | 960 | bool keep_this_tri = | |
73 |
6/12✓ Branch 2 taken 480 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 480 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 480 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 480 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 480 times.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✓ Branch 20 taken 480 times.
|
960 | keep_vertex[t[0]] || keep_vertex[t[1]] || keep_vertex[t[2]]; |
74 | |||
75 |
1/2✓ Branch 0 taken 480 times.
✗ Branch 1 not taken.
|
960 | if (!keep_this_tri) { |
76 |
2/2✓ Branch 0 taken 1120 times.
✓ Branch 1 taken 240 times.
|
2720 | for (unsigned int j = 0; j < 3; ++j) { |
77 |
5/8✓ Branch 3 taken 1120 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1120 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1120 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 240 times.
✓ Branch 12 taken 880 times.
|
2240 | if (aabb.contain(q * model_vertices_[t[j]])) { |
78 | 480 | keep_this_tri = true; | |
79 | 480 | break; | |
80 | } | ||
81 | } | ||
82 | |||
83 |
1/2✓ Branch 7 taken 480 times.
✗ Branch 8 not taken.
|
960 | const TriangleP tri(model_vertices_[t[0]], model_vertices_[t[1]], |
84 | model_vertices_[t[2]]); | ||
85 | 960 | const bool enable_nearest_points = false; | |
86 | 960 | const bool compute_penetration = false; // we only need to know if there | |
87 | // is a collision or not | ||
88 |
1/2✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
|
960 | const DistanceRequest distanceRequest(enable_nearest_points, |
89 | compute_penetration); | ||
90 |
1/2✓ Branch 2 taken 480 times.
✗ Branch 3 not taken.
|
960 | DistanceResult distanceResult; |
91 |
1/2✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
|
960 | const Scalar distance = ShapeShapeDistance<Box, TriangleP>( |
92 |
1/2✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
|
960 | &box, box_pose, &tri, Transform3s(), &gjk, distanceRequest, |
93 | distanceResult); | ||
94 | 960 | bool is_collision = | |
95 |
1/2✓ Branch 1 taken 480 times.
✗ Branch 2 not taken.
|
960 | (distance <= gjk.getDistancePrecision(compute_penetration)); |
96 | |||
97 |
4/4✓ Branch 0 taken 240 times.
✓ Branch 1 taken 240 times.
✓ Branch 2 taken 16 times.
✓ Branch 3 taken 224 times.
|
960 | if (!keep_this_tri && is_collision) { |
98 | 32 | keep_this_tri = true; | |
99 | } | ||
100 | 960 | } | |
101 |
2/2✓ Branch 0 taken 256 times.
✓ Branch 1 taken 224 times.
|
960 | if (keep_this_tri) { |
102 |
3/6✓ Branch 2 taken 256 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 256 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 256 times.
✗ Branch 13 not taken.
|
512 | keep_vertex[t[0]] = keep_vertex[t[1]] = keep_vertex[t[2]] = true; |
103 |
1/2✓ Branch 1 taken 256 times.
✗ Branch 2 not taken.
|
512 | keep_tri[i] = true; |
104 | 512 | ntri++; | |
105 | } | ||
106 | } | ||
107 | |||
108 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 32 times.
|
80 | if (ntri == 0) return NULL; |
109 | |||
110 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✗ Branch 5 not taken.
|
64 | BVHModel<BV>* new_model(new BVHModel<BV>()); |
111 |
1/2✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
|
64 | new_model->beginModel(ntri, std::min(ntri * 3, model.num_vertices)); |
112 |
1/2✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
|
64 | std::vector<unsigned int> idxConversion(model.num_vertices); |
113 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
|
64 | assert(new_model->num_vertices == 0); |
114 | 64 | std::vector<Vec3s>& new_model_vertices_ = *(new_model->vertices); | |
115 |
2/2✓ Branch 1 taken 1152 times.
✓ Branch 2 taken 32 times.
|
2368 | for (unsigned int i = 0; i < keep_vertex.size(); ++i) { |
116 |
3/4✓ Branch 1 taken 1152 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 768 times.
✓ Branch 5 taken 384 times.
|
2304 | if (keep_vertex[i]) { |
117 | 1536 | idxConversion[i] = new_model->num_vertices; | |
118 |
1/2✓ Branch 3 taken 768 times.
✗ Branch 4 not taken.
|
1536 | new_model_vertices_[new_model->num_vertices] = model_vertices_[i]; |
119 | 1536 | new_model->num_vertices++; | |
120 | } | ||
121 | } | ||
122 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 32 times.
|
64 | assert(new_model->num_tris == 0); |
123 | 64 | std::vector<Triangle>& new_model_tri_indices_ = *(new_model->tri_indices); | |
124 |
2/2✓ Branch 1 taken 384 times.
✓ Branch 2 taken 32 times.
|
832 | for (unsigned int i = 0; i < keep_tri.size(); ++i) { |
125 |
3/4✓ Branch 1 taken 384 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 256 times.
✓ Branch 5 taken 128 times.
|
768 | if (keep_tri[i]) { |
126 | 512 | new_model_tri_indices_[new_model->num_tris].set( | |
127 | idxConversion[model_tri_indices_[i][0]], | ||
128 | idxConversion[model_tri_indices_[i][1]], | ||
129 | idxConversion[model_tri_indices_[i][2]]); | ||
130 | 512 | new_model->num_tris++; | |
131 | } | ||
132 | } | ||
133 |
2/4✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 32 times.
|
64 | if (new_model->endModel() != BVH_OK) { |
134 | ✗ | delete new_model; | |
135 | ✗ | return NULL; | |
136 | } | ||
137 | 64 | return new_model; | |
138 | 80 | } | |
139 | } // namespace details | ||
140 | |||
141 | template <> | ||
142 | 5 | BVHModel<OBB>* BVHExtract(const BVHModel<OBB>& model, const Transform3s& pose, | |
143 | const AABB& aabb) { | ||
144 | 5 | return details::BVHExtract(model, pose, aabb); | |
145 | } | ||
146 | template <> | ||
147 | 5 | BVHModel<AABB>* BVHExtract(const BVHModel<AABB>& model, const Transform3s& pose, | |
148 | const AABB& aabb) { | ||
149 | 5 | return details::BVHExtract(model, pose, aabb); | |
150 | } | ||
151 | template <> | ||
152 | 5 | BVHModel<RSS>* BVHExtract(const BVHModel<RSS>& model, const Transform3s& pose, | |
153 | const AABB& aabb) { | ||
154 | 5 | return details::BVHExtract(model, pose, aabb); | |
155 | } | ||
156 | template <> | ||
157 | 5 | BVHModel<kIOS>* BVHExtract(const BVHModel<kIOS>& model, const Transform3s& pose, | |
158 | const AABB& aabb) { | ||
159 | 5 | return details::BVHExtract(model, pose, aabb); | |
160 | } | ||
161 | template <> | ||
162 | 5 | BVHModel<OBBRSS>* BVHExtract(const BVHModel<OBBRSS>& model, | |
163 | const Transform3s& pose, const AABB& aabb) { | ||
164 | 5 | return details::BVHExtract(model, pose, aabb); | |
165 | } | ||
166 | template <> | ||
167 | 5 | BVHModel<KDOP<16> >* BVHExtract(const BVHModel<KDOP<16> >& model, | |
168 | const Transform3s& pose, const AABB& aabb) { | ||
169 | 5 | return details::BVHExtract(model, pose, aabb); | |
170 | } | ||
171 | template <> | ||
172 | 5 | BVHModel<KDOP<18> >* BVHExtract(const BVHModel<KDOP<18> >& model, | |
173 | const Transform3s& pose, const AABB& aabb) { | ||
174 | 5 | return details::BVHExtract(model, pose, aabb); | |
175 | } | ||
176 | template <> | ||
177 | 5 | BVHModel<KDOP<24> >* BVHExtract(const BVHModel<KDOP<24> >& model, | |
178 | const Transform3s& pose, const AABB& aabb) { | ||
179 | 5 | return details::BVHExtract(model, pose, aabb); | |
180 | } | ||
181 | |||
182 | 1789350 | void getCovariance(Vec3s* ps, Vec3s* ps2, Triangle* ts, unsigned int* indices, | |
183 | unsigned int n, Matrix3s& M) { | ||
184 |
2/4✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
|
1789350 | Vec3s S1(Vec3s::Zero()); |
185 |
6/12✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1789350 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1789350 times.
✗ Branch 17 not taken.
|
1789350 | Vec3s S2[3] = {Vec3s::Zero(), Vec3s::Zero(), Vec3s::Zero()}; |
186 | |||
187 |
2/2✓ Branch 0 taken 1775801 times.
✓ Branch 1 taken 13549 times.
|
1789350 | if (ts) { |
188 |
2/2✓ Branch 0 taken 10154655 times.
✓ Branch 1 taken 1775801 times.
|
11930456 | for (unsigned int i = 0; i < n; ++i) { |
189 |
1/2✓ Branch 0 taken 10154655 times.
✗ Branch 1 not taken.
|
10154655 | const Triangle& t = (indices) ? ts[indices[i]] : ts[i]; |
190 | |||
191 | 10154655 | const Vec3s& p1 = ps[t[0]]; | |
192 | 10154655 | const Vec3s& p2 = ps[t[1]]; | |
193 | 10154655 | const Vec3s& p3 = ps[t[2]]; | |
194 | |||
195 |
4/8✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
|
10154655 | S1[0] += (p1[0] + p2[0] + p3[0]); |
196 |
4/8✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
|
10154655 | S1[1] += (p1[1] + p2[1] + p3[1]); |
197 |
4/8✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
|
10154655 | S1[2] += (p1[2] + p2[2] + p3[2]); |
198 |
7/14✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
|
10154655 | S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); |
199 |
7/14✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
|
10154655 | S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); |
200 |
7/14✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
|
10154655 | S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); |
201 |
7/14✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
|
10154655 | S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); |
202 |
7/14✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
|
10154655 | S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); |
203 |
7/14✓ Branch 1 taken 10154655 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10154655 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10154655 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10154655 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 10154655 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 10154655 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 10154655 times.
✗ Branch 20 not taken.
|
10154655 | S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); |
204 | |||
205 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 10154655 times.
|
10154655 | if (ps2) { |
206 | ✗ | const Vec3s& p1 = ps2[t[0]]; | |
207 | ✗ | const Vec3s& p2 = ps2[t[1]]; | |
208 | ✗ | const Vec3s& p3 = ps2[t[2]]; | |
209 | |||
210 | ✗ | S1[0] += (p1[0] + p2[0] + p3[0]); | |
211 | ✗ | S1[1] += (p1[1] + p2[1] + p3[1]); | |
212 | ✗ | S1[2] += (p1[2] + p2[2] + p3[2]); | |
213 | |||
214 | ✗ | S2[0][0] += (p1[0] * p1[0] + p2[0] * p2[0] + p3[0] * p3[0]); | |
215 | ✗ | S2[1][1] += (p1[1] * p1[1] + p2[1] * p2[1] + p3[1] * p3[1]); | |
216 | ✗ | S2[2][2] += (p1[2] * p1[2] + p2[2] * p2[2] + p3[2] * p3[2]); | |
217 | ✗ | S2[0][1] += (p1[0] * p1[1] + p2[0] * p2[1] + p3[0] * p3[1]); | |
218 | ✗ | S2[0][2] += (p1[0] * p1[2] + p2[0] * p2[2] + p3[0] * p3[2]); | |
219 | ✗ | S2[1][2] += (p1[1] * p1[2] + p2[1] * p2[2] + p3[1] * p3[2]); | |
220 | } | ||
221 | } | ||
222 | } else { | ||
223 |
2/2✓ Branch 0 taken 213428 times.
✓ Branch 1 taken 13549 times.
|
226977 | for (unsigned int i = 0; i < n; ++i) { |
224 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 213428 times.
|
213428 | const Vec3s& p = (indices) ? ps[indices[i]] : ps[i]; |
225 |
1/2✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
|
213428 | S1 += p; |
226 |
3/6✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
|
213428 | S2[0][0] += (p[0] * p[0]); |
227 |
3/6✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
|
213428 | S2[1][1] += (p[1] * p[1]); |
228 |
3/6✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
|
213428 | S2[2][2] += (p[2] * p[2]); |
229 |
3/6✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
|
213428 | S2[0][1] += (p[0] * p[1]); |
230 |
3/6✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
|
213428 | S2[0][2] += (p[0] * p[2]); |
231 |
3/6✓ Branch 1 taken 213428 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 213428 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 213428 times.
✗ Branch 8 not taken.
|
213428 | S2[1][2] += (p[1] * p[2]); |
232 | |||
233 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 213428 times.
|
213428 | if (ps2) // another frame |
234 | { | ||
235 | ✗ | const Vec3s& p = (indices) ? ps2[indices[i]] : ps2[i]; | |
236 | ✗ | S1 += p; | |
237 | ✗ | S2[0][0] += (p[0] * p[0]); | |
238 | ✗ | S2[1][1] += (p[1] * p[1]); | |
239 | ✗ | S2[2][2] += (p[2] * p[2]); | |
240 | ✗ | S2[0][1] += (p[0] * p[1]); | |
241 | ✗ | S2[0][2] += (p[0] * p[2]); | |
242 | ✗ | S2[1][2] += (p[1] * p[2]); | |
243 | } | ||
244 | } | ||
245 | } | ||
246 | |||
247 |
3/4✗ Branch 0 not taken.
✓ Branch 1 taken 1789350 times.
✓ Branch 2 taken 1775801 times.
✓ Branch 3 taken 13549 times.
|
1789350 | unsigned int n_points = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; |
248 | |||
249 |
4/8✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
|
1789350 | M(0, 0) = S2[0][0] - S1[0] * S1[0] / Scalar(n_points); |
250 |
4/8✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
|
1789350 | M(1, 1) = S2[1][1] - S1[1] * S1[1] / Scalar(n_points); |
251 |
4/8✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
|
1789350 | M(2, 2) = S2[2][2] - S1[2] * S1[2] / Scalar(n_points); |
252 |
4/8✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
|
1789350 | M(0, 1) = S2[0][1] - S1[0] * S1[1] / Scalar(n_points); |
253 |
4/8✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
|
1789350 | M(1, 2) = S2[1][2] - S1[1] * S1[2] / Scalar(n_points); |
254 |
4/8✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1789350 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1789350 times.
✗ Branch 11 not taken.
|
1789350 | M(0, 2) = S2[0][2] - S1[0] * S1[2] / Scalar(n_points); |
255 |
2/4✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
|
1789350 | M(1, 0) = M(0, 1); |
256 |
2/4✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
|
1789350 | M(2, 0) = M(0, 2); |
257 |
2/4✓ Branch 1 taken 1789350 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1789350 times.
✗ Branch 5 not taken.
|
1789350 | M(2, 1) = M(1, 2); |
258 | 1789350 | } | |
259 | |||
260 | /** @brief Compute the RSS bounding volume parameters: radius, rectangle size | ||
261 | * and the origin. The bounding volume axes are known. | ||
262 | */ | ||
263 | 1347836 | void getRadiusAndOriginAndRectangleSize(Vec3s* ps, Vec3s* ps2, Triangle* ts, | |
264 | unsigned int* indices, unsigned int n, | ||
265 | const Matrix3s& axes, Vec3s& origin, | ||
266 | Scalar l[2], Scalar& r) { | ||
267 | 1347836 | bool indirect_index = true; | |
268 |
2/2✓ Branch 0 taken 26381 times.
✓ Branch 1 taken 1321455 times.
|
1347836 | if (!indices) indirect_index = false; |
269 | |||
270 |
3/4✗ Branch 0 not taken.
✓ Branch 1 taken 1347836 times.
✓ Branch 2 taken 1321455 times.
✓ Branch 3 taken 26381 times.
|
1347836 | unsigned int size_P = ((ps2) ? 2 : 1) * ((ts) ? 3 : 1) * n; |
271 | |||
272 |
2/4✓ Branch 0 taken 1347836 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 1347836 times.
✗ Branch 5 not taken.
|
1347836 | Scalar(*P)[3] = new Scalar[size_P][3]; |
273 | |||
274 | 1347836 | int P_id = 0; | |
275 | |||
276 |
2/2✓ Branch 0 taken 1321455 times.
✓ Branch 1 taken 26381 times.
|
1347836 | if (ts) { |
277 |
2/2✓ Branch 0 taken 7265973 times.
✓ Branch 1 taken 1321455 times.
|
8587428 | for (unsigned int i = 0; i < n; ++i) { |
278 |
1/2✓ Branch 0 taken 7265973 times.
✗ Branch 1 not taken.
|
7265973 | unsigned int index = indirect_index ? indices[i] : i; |
279 | 7265973 | const Triangle& t = ts[index]; | |
280 | |||
281 |
2/2✓ Branch 0 taken 21797919 times.
✓ Branch 1 taken 7265973 times.
|
29063892 | for (Triangle::index_type j = 0; j < 3; ++j) { |
282 | 21797919 | Triangle::index_type point_id = t[j]; | |
283 | 21797919 | const Vec3s& p = ps[point_id]; | |
284 |
4/8✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 21797919 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 21797919 times.
✗ Branch 11 not taken.
|
21797919 | Vec3s v(p[0], p[1], p[2]); |
285 |
2/4✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
|
21797919 | P[P_id][0] = axes.col(0).dot(v); |
286 |
2/4✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
|
21797919 | P[P_id][1] = axes.col(1).dot(v); |
287 |
2/4✓ Branch 1 taken 21797919 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21797919 times.
✗ Branch 5 not taken.
|
21797919 | P[P_id][2] = axes.col(2).dot(v); |
288 | 21797919 | P_id++; | |
289 | } | ||
290 | |||
291 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7265973 times.
|
7265973 | if (ps2) { |
292 | ✗ | for (Triangle::index_type j = 0; j < 3; ++j) { | |
293 | ✗ | Triangle::index_type point_id = t[j]; | |
294 | ✗ | const Vec3s& p = ps2[point_id]; | |
295 | // FIXME Is this right ????? | ||
296 | ✗ | Vec3s v(p[0], p[1], p[2]); | |
297 | ✗ | P[P_id][0] = axes.col(0).dot(v); | |
298 | ✗ | P[P_id][1] = axes.col(0).dot(v); | |
299 | ✗ | P[P_id][2] = axes.col(1).dot(v); | |
300 | ✗ | P_id++; | |
301 | } | ||
302 | } | ||
303 | } | ||
304 | } else { | ||
305 |
2/2✓ Branch 0 taken 250378 times.
✓ Branch 1 taken 26381 times.
|
276759 | for (unsigned int i = 0; i < n; ++i) { |
306 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 250378 times.
|
250378 | unsigned int index = indirect_index ? indices[i] : i; |
307 | |||
308 | 250378 | const Vec3s& p = ps[index]; | |
309 |
4/8✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 250378 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 250378 times.
✗ Branch 11 not taken.
|
250378 | Vec3s v(p[0], p[1], p[2]); |
310 |
2/4✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
|
250378 | P[P_id][0] = axes.col(0).dot(v); |
311 |
2/4✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
|
250378 | P[P_id][1] = axes.col(1).dot(v); |
312 |
2/4✓ Branch 1 taken 250378 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 250378 times.
✗ Branch 5 not taken.
|
250378 | P[P_id][2] = axes.col(2).dot(v); |
313 | 250378 | P_id++; | |
314 | |||
315 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 250378 times.
|
250378 | if (ps2) { |
316 | ✗ | const Vec3s& v = ps2[index]; | |
317 | ✗ | P[P_id][0] = axes.col(0).dot(v); | |
318 | ✗ | P[P_id][1] = axes.col(1).dot(v); | |
319 | ✗ | P[P_id][2] = axes.col(2).dot(v); | |
320 | ✗ | P_id++; | |
321 | } | ||
322 | } | ||
323 | } | ||
324 | |||
325 | Scalar minx, maxx, miny, maxy, minz, maxz; | ||
326 | |||
327 | Scalar cz, radsqr; | ||
328 | |||
329 | 1347836 | minz = maxz = P[0][2]; | |
330 | |||
331 |
2/2✓ Branch 0 taken 20700461 times.
✓ Branch 1 taken 1347836 times.
|
22048297 | for (unsigned int i = 1; i < size_P; ++i) { |
332 | 20700461 | Scalar z_value = P[i][2]; | |
333 |
2/2✓ Branch 0 taken 1683918 times.
✓ Branch 1 taken 19016543 times.
|
20700461 | if (z_value < minz) |
334 | 1683918 | minz = z_value; | |
335 |
2/2✓ Branch 0 taken 1665672 times.
✓ Branch 1 taken 17350871 times.
|
19016543 | else if (z_value > maxz) |
336 | 1665672 | maxz = z_value; | |
337 | } | ||
338 | |||
339 | 1347836 | r = (Scalar)0.5 * (maxz - minz); | |
340 | 1347836 | radsqr = r * r; | |
341 | 1347836 | cz = (Scalar)0.5 * (maxz + minz); | |
342 | |||
343 | // compute an initial norm of rectangle along x direction | ||
344 | |||
345 | // find minx and maxx as starting points | ||
346 | |||
347 | 1347836 | unsigned int minindex = 0, maxindex = 0; | |
348 | Scalar mintmp, maxtmp; | ||
349 | 1347836 | mintmp = maxtmp = P[0][0]; | |
350 | |||
351 |
2/2✓ Branch 0 taken 20700461 times.
✓ Branch 1 taken 1347836 times.
|
22048297 | for (unsigned int i = 1; i < size_P; ++i) { |
352 | 20700461 | Scalar x_value = P[i][0]; | |
353 |
2/2✓ Branch 0 taken 1736049 times.
✓ Branch 1 taken 18964412 times.
|
20700461 | if (x_value < mintmp) { |
354 | 1736049 | minindex = i; | |
355 | 1736049 | mintmp = x_value; | |
356 |
2/2✓ Branch 0 taken 1776736 times.
✓ Branch 1 taken 17187676 times.
|
18964412 | } else if (x_value > maxtmp) { |
357 | 1776736 | maxindex = i; | |
358 | 1776736 | maxtmp = x_value; | |
359 | } | ||
360 | } | ||
361 | |||
362 | Scalar x, dz; | ||
363 | 1347836 | dz = P[minindex][2] - cz; | |
364 | 1347836 | minx = P[minindex][0] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
365 | 1347836 | dz = P[maxindex][2] - cz; | |
366 | 1347836 | maxx = P[maxindex][0] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
367 | |||
368 | // grow minx/maxx | ||
369 | |||
370 |
2/2✓ Branch 0 taken 22048297 times.
✓ Branch 1 taken 1347836 times.
|
23396133 | for (unsigned int i = 0; i < size_P; ++i) { |
371 |
2/2✓ Branch 0 taken 3160131 times.
✓ Branch 1 taken 18888166 times.
|
22048297 | if (P[i][0] < minx) { |
372 | 3160131 | dz = P[i][2] - cz; | |
373 | 3160131 | x = P[i][0] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
374 |
2/2✓ Branch 0 taken 211139 times.
✓ Branch 1 taken 2948992 times.
|
3160131 | if (x < minx) minx = x; |
375 |
2/2✓ Branch 0 taken 3126881 times.
✓ Branch 1 taken 15761285 times.
|
18888166 | } else if (P[i][0] > maxx) { |
376 | 3126881 | dz = P[i][2] - cz; | |
377 | 3126881 | x = P[i][0] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
378 |
2/2✓ Branch 0 taken 217396 times.
✓ Branch 1 taken 2909485 times.
|
3126881 | if (x > maxx) maxx = x; |
379 | } | ||
380 | } | ||
381 | |||
382 | // compute an initial norm of rectangle along y direction | ||
383 | |||
384 | // find miny and maxy as starting points | ||
385 | |||
386 | 1347836 | minindex = maxindex = 0; | |
387 | 1347836 | mintmp = maxtmp = P[0][1]; | |
388 |
2/2✓ Branch 0 taken 20700461 times.
✓ Branch 1 taken 1347836 times.
|
22048297 | for (unsigned int i = 1; i < size_P; ++i) { |
389 | 20700461 | Scalar y_value = P[i][1]; | |
390 |
2/2✓ Branch 0 taken 2386779 times.
✓ Branch 1 taken 18313682 times.
|
20700461 | if (y_value < mintmp) { |
391 | 2386779 | minindex = i; | |
392 | 2386779 | mintmp = y_value; | |
393 |
2/2✓ Branch 0 taken 1729269 times.
✓ Branch 1 taken 16584413 times.
|
18313682 | } else if (y_value > maxtmp) { |
394 | 1729269 | maxindex = i; | |
395 | 1729269 | maxtmp = y_value; | |
396 | } | ||
397 | } | ||
398 | |||
399 | Scalar y; | ||
400 | 1347836 | dz = P[minindex][2] - cz; | |
401 | 1347836 | miny = P[minindex][1] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
402 | 1347836 | dz = P[maxindex][2] - cz; | |
403 | 1347836 | maxy = P[maxindex][1] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
404 | |||
405 | // grow miny/maxy | ||
406 | |||
407 |
2/2✓ Branch 0 taken 22048297 times.
✓ Branch 1 taken 1347836 times.
|
23396133 | for (unsigned int i = 0; i < size_P; ++i) { |
408 |
2/2✓ Branch 0 taken 3064489 times.
✓ Branch 1 taken 18983808 times.
|
22048297 | if (P[i][1] < miny) { |
409 | 3064489 | dz = P[i][2] - cz; | |
410 | 3064489 | y = P[i][1] + std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
411 |
2/2✓ Branch 0 taken 117534 times.
✓ Branch 1 taken 2946955 times.
|
3064489 | if (y < miny) miny = y; |
412 |
2/2✓ Branch 0 taken 3045839 times.
✓ Branch 1 taken 15937969 times.
|
18983808 | } else if (P[i][1] > maxy) { |
413 | 3045839 | dz = P[i][2] - cz; | |
414 | 3045839 | y = P[i][1] - std::sqrt(std::max<Scalar>(radsqr - dz * dz, 0)); | |
415 |
2/2✓ Branch 0 taken 113644 times.
✓ Branch 1 taken 2932195 times.
|
3045839 | if (y > maxy) maxy = y; |
416 | } | ||
417 | } | ||
418 | |||
419 | // corners may have some points which are not covered - grow lengths if | ||
420 | // necessary quite conservative (can be improved) | ||
421 | Scalar dx, dy, u, t; | ||
422 | 1347836 | Scalar a = std::sqrt((Scalar)0.5); | |
423 |
2/2✓ Branch 0 taken 22048297 times.
✓ Branch 1 taken 1347836 times.
|
23396133 | for (unsigned int i = 0; i < size_P; ++i) { |
424 |
2/2✓ Branch 0 taken 2837914 times.
✓ Branch 1 taken 19210383 times.
|
22048297 | if (P[i][0] > maxx) { |
425 |
2/2✓ Branch 0 taken 456658 times.
✓ Branch 1 taken 2381256 times.
|
2837914 | if (P[i][1] > maxy) { |
426 | 456658 | dx = P[i][0] - maxx; | |
427 | 456658 | dy = P[i][1] - maxy; | |
428 | 456658 | u = dx * a + dy * a; | |
429 | 456658 | t = (a * u - dx) * (a * u - dx) + (a * u - dy) * (a * u - dy) + | |
430 | 456658 | (cz - P[i][2]) * (cz - P[i][2]); | |
431 | 456658 | u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0)); | |
432 |
2/2✓ Branch 0 taken 95062 times.
✓ Branch 1 taken 361596 times.
|
456658 | if (u > 0) { |
433 | 95062 | maxx += u * a; | |
434 | 95062 | maxy += u * a; | |
435 | } | ||
436 |
2/2✓ Branch 0 taken 458630 times.
✓ Branch 1 taken 1922626 times.
|
2381256 | } else if (P[i][1] < miny) { |
437 | 458630 | dx = P[i][0] - maxx; | |
438 | 458630 | dy = P[i][1] - miny; | |
439 | 458630 | u = dx * a - dy * a; | |
440 | 458630 | t = (a * u - dx) * (a * u - dx) + (-a * u - dy) * (-a * u - dy) + | |
441 | 458630 | (cz - P[i][2]) * (cz - P[i][2]); | |
442 | 458630 | u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0)); | |
443 |
2/2✓ Branch 0 taken 100484 times.
✓ Branch 1 taken 358146 times.
|
458630 | if (u > 0) { |
444 | 100484 | maxx += u * a; | |
445 | 100484 | miny -= u * a; | |
446 | } | ||
447 | } | ||
448 |
2/2✓ Branch 0 taken 2842349 times.
✓ Branch 1 taken 16368034 times.
|
19210383 | } else if (P[i][0] < minx) { |
449 |
2/2✓ Branch 0 taken 439663 times.
✓ Branch 1 taken 2402686 times.
|
2842349 | if (P[i][1] > maxy) { |
450 | 439663 | dx = P[i][0] - minx; | |
451 | 439663 | dy = P[i][1] - maxy; | |
452 | 439663 | u = dy * a - dx * a; | |
453 | 439663 | t = (-a * u - dx) * (-a * u - dx) + (a * u - dy) * (a * u - dy) + | |
454 | 439663 | (cz - P[i][2]) * (cz - P[i][2]); | |
455 | 439663 | u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0)); | |
456 |
2/2✓ Branch 0 taken 91716 times.
✓ Branch 1 taken 347947 times.
|
439663 | if (u > 0) { |
457 | 91716 | minx -= u * a; | |
458 | 91716 | maxy += u * a; | |
459 | } | ||
460 |
2/2✓ Branch 0 taken 444583 times.
✓ Branch 1 taken 1958103 times.
|
2402686 | } else if (P[i][1] < miny) { |
461 | 444583 | dx = P[i][0] - minx; | |
462 | 444583 | dy = P[i][1] - miny; | |
463 | 444583 | u = -dx * a - dy * a; | |
464 | 444583 | t = (-a * u - dx) * (-a * u - dx) + (-a * u - dy) * (-a * u - dy) + | |
465 | 444583 | (cz - P[i][2]) * (cz - P[i][2]); | |
466 | 444583 | u = u - std::sqrt(std::max<Scalar>(radsqr - t, 0)); | |
467 |
2/2✓ Branch 0 taken 110241 times.
✓ Branch 1 taken 334342 times.
|
444583 | if (u > 0) { |
468 | 110241 | minx -= u * a; | |
469 | 110241 | miny -= u * a; | |
470 | } | ||
471 | } | ||
472 | } | ||
473 | } | ||
474 | |||
475 |
4/8✓ Branch 1 taken 1347836 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1347836 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1347836 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1347836 times.
✗ Branch 11 not taken.
|
1347836 | origin.noalias() = axes * Vec3s(minx, miny, cz); |
476 | |||
477 | 1347836 | l[0] = std::max<Scalar>(maxx - minx, 0); | |
478 | 1347836 | l[1] = std::max<Scalar>(maxy - miny, 0); | |
479 | |||
480 |
1/2✓ Branch 0 taken 1347836 times.
✗ Branch 1 not taken.
|
1347836 | delete[] P; |
481 | 1347836 | } | |
482 | |||
483 | /** @brief Compute the bounding volume extent and center for a set or subset of | ||
484 | * points. The bounding volume axes are known. | ||
485 | */ | ||
486 | 19868 | static inline void getExtentAndCenter_pointcloud(Vec3s* ps, Vec3s* ps2, | |
487 | unsigned int* indices, | ||
488 | unsigned int n, Matrix3s& axes, | ||
489 | Vec3s& center, Vec3s& extent) { | ||
490 | 19868 | bool indirect_index = true; | |
491 |
1/2✓ Branch 0 taken 19868 times.
✗ Branch 1 not taken.
|
19868 | if (!indices) indirect_index = false; |
492 | |||
493 | 19868 | Scalar real_max = (std::numeric_limits<Scalar>::max)(); | |
494 | |||
495 |
1/2✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
|
19868 | Vec3s min_coord(real_max, real_max, real_max); |
496 |
1/2✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
|
19868 | Vec3s max_coord(-real_max, -real_max, -real_max); |
497 | |||
498 |
2/2✓ Branch 0 taken 61150 times.
✓ Branch 1 taken 19868 times.
|
81018 | for (unsigned int i = 0; i < n; ++i) { |
499 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 61150 times.
|
61150 | unsigned int index = indirect_index ? indices[i] : i; |
500 | |||
501 | 61150 | const Vec3s& p = ps[index]; | |
502 |
3/6✓ Branch 1 taken 61150 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 61150 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 61150 times.
✗ Branch 8 not taken.
|
61150 | Vec3s proj(axes.transpose() * p); |
503 | |||
504 |
2/2✓ Branch 0 taken 183450 times.
✓ Branch 1 taken 61150 times.
|
244600 | for (int j = 0; j < 3; ++j) { |
505 |
6/10✓ Branch 1 taken 183450 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 183450 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 89341 times.
✓ Branch 7 taken 94109 times.
✓ Branch 9 taken 89341 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 89341 times.
✗ Branch 13 not taken.
|
183450 | if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; |
506 |
6/10✓ Branch 1 taken 183450 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 183450 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 98136 times.
✓ Branch 7 taken 85314 times.
✓ Branch 9 taken 98136 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 98136 times.
✗ Branch 13 not taken.
|
183450 | if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; |
507 | } | ||
508 | |||
509 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 61150 times.
|
61150 | if (ps2) { |
510 | ✗ | const Vec3s& v = ps2[index]; | |
511 | ✗ | proj.noalias() = axes.transpose() * v; | |
512 | |||
513 | ✗ | for (int j = 0; j < 3; ++j) { | |
514 | ✗ | if (proj[j] > max_coord[j]) max_coord[j] = proj[j]; | |
515 | ✗ | if (proj[j] < min_coord[j]) min_coord[j] = proj[j]; | |
516 | } | ||
517 | } | ||
518 | } | ||
519 | |||
520 |
5/10✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19868 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19868 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19868 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 19868 times.
✗ Branch 14 not taken.
|
19868 | center.noalias() = axes * (max_coord + min_coord) / 2; |
521 | |||
522 |
4/8✓ Branch 1 taken 19868 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19868 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 19868 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 19868 times.
✗ Branch 11 not taken.
|
19868 | extent.noalias() = (max_coord - min_coord) / 2; |
523 | 19868 | } | |
524 | |||
525 | /** @brief Compute the bounding volume extent and center for a set or subset of | ||
526 | * points. The bounding volume axes are known. | ||
527 | */ | ||
528 | 1479132 | static inline void getExtentAndCenter_mesh(Vec3s* ps, Vec3s* ps2, Triangle* ts, | |
529 | unsigned int* indices, | ||
530 | unsigned int n, Matrix3s& axes, | ||
531 | Vec3s& center, Vec3s& extent) { | ||
532 | 1479132 | bool indirect_index = true; | |
533 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1479132 times.
|
1479132 | if (!indices) indirect_index = false; |
534 | |||
535 | 1479132 | Scalar real_max = (std::numeric_limits<Scalar>::max)(); | |
536 | |||
537 |
1/2✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
|
1479132 | Vec3s min_coord(real_max, real_max, real_max); |
538 |
1/2✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
|
1479132 | Vec3s max_coord(-real_max, -real_max, -real_max); |
539 | |||
540 |
2/2✓ Branch 0 taken 8273139 times.
✓ Branch 1 taken 1479132 times.
|
9752271 | for (unsigned int i = 0; i < n; ++i) { |
541 |
1/2✓ Branch 0 taken 8273139 times.
✗ Branch 1 not taken.
|
8273139 | unsigned int index = indirect_index ? indices[i] : i; |
542 | 8273139 | const Triangle& t = ts[index]; | |
543 | |||
544 |
2/2✓ Branch 0 taken 24819417 times.
✓ Branch 1 taken 8273139 times.
|
33092556 | for (Triangle::index_type j = 0; j < 3; ++j) { |
545 | 24819417 | Triangle::index_type point_id = t[j]; | |
546 | 24819417 | const Vec3s& p = ps[point_id]; | |
547 |
3/6✓ Branch 1 taken 24819417 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24819417 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 24819417 times.
✗ Branch 8 not taken.
|
24819417 | Vec3s proj(axes.transpose() * p); |
548 | |||
549 |
2/2✓ Branch 0 taken 74458251 times.
✓ Branch 1 taken 24819417 times.
|
99277668 | for (int k = 0; k < 3; ++k) { |
550 |
6/10✓ Branch 1 taken 74458251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 74458251 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10224501 times.
✓ Branch 7 taken 64233750 times.
✓ Branch 9 taken 10224501 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10224501 times.
✗ Branch 13 not taken.
|
74458251 | if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; |
551 |
6/10✓ Branch 1 taken 74458251 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 74458251 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10760103 times.
✓ Branch 7 taken 63698148 times.
✓ Branch 9 taken 10760103 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10760103 times.
✗ Branch 13 not taken.
|
74458251 | if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; |
552 | } | ||
553 | } | ||
554 | |||
555 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 8273139 times.
|
8273139 | if (ps2) { |
556 | ✗ | for (Triangle::index_type j = 0; j < 3; ++j) { | |
557 | ✗ | Triangle::index_type point_id = t[j]; | |
558 | ✗ | const Vec3s& p = ps2[point_id]; | |
559 | ✗ | Vec3s proj(axes.transpose() * p); | |
560 | |||
561 | ✗ | for (int k = 0; k < 3; ++k) { | |
562 | ✗ | if (proj[k] > max_coord[k]) max_coord[k] = proj[k]; | |
563 | ✗ | if (proj[k] < min_coord[k]) min_coord[k] = proj[k]; | |
564 | } | ||
565 | } | ||
566 | } | ||
567 | } | ||
568 | |||
569 |
3/6✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1479132 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1479132 times.
✗ Branch 8 not taken.
|
1479132 | Vec3s o((max_coord + min_coord) / 2); |
570 | |||
571 |
3/6✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1479132 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1479132 times.
✗ Branch 8 not taken.
|
1479132 | center.noalias() = axes * o; |
572 | |||
573 |
4/8✓ Branch 1 taken 1479132 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1479132 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1479132 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1479132 times.
✗ Branch 11 not taken.
|
1479132 | extent.noalias() = (max_coord - min_coord) / 2; |
574 | 1479132 | } | |
575 | |||
576 | 1499000 | void getExtentAndCenter(Vec3s* ps, Vec3s* ps2, Triangle* ts, | |
577 | unsigned int* indices, unsigned int n, Matrix3s& axes, | ||
578 | Vec3s& center, Vec3s& extent) { | ||
579 |
2/2✓ Branch 0 taken 1479132 times.
✓ Branch 1 taken 19868 times.
|
1499000 | if (ts) |
580 | 1479132 | getExtentAndCenter_mesh(ps, ps2, ts, indices, n, axes, center, extent); | |
581 | else | ||
582 | 19868 | getExtentAndCenter_pointcloud(ps, ps2, indices, n, axes, center, extent); | |
583 | 1499000 | } | |
584 | |||
585 | 6540 | void circumCircleComputation(const Vec3s& a, const Vec3s& b, const Vec3s& c, | |
586 | Vec3s& center, Scalar& radius) { | ||
587 |
2/4✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
|
6540 | Vec3s e1 = a - c; |
588 |
2/4✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
|
6540 | Vec3s e2 = b - c; |
589 |
1/2✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
|
6540 | Scalar e1_len2 = e1.squaredNorm(); |
590 |
1/2✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
|
6540 | Scalar e2_len2 = e2.squaredNorm(); |
591 |
1/2✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
|
6540 | Vec3s e3 = e1.cross(e2); |
592 |
1/2✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
|
6540 | Scalar e3_len2 = e3.squaredNorm(); |
593 |
2/4✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
|
6540 | radius = e1_len2 * e2_len2 * (e1 - e2).squaredNorm() / e3_len2; |
594 | 6540 | radius = std::sqrt(radius) * Scalar(0.5); | |
595 | |||
596 |
7/14✓ Branch 1 taken 6540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6540 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6540 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6540 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6540 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 6540 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 6540 times.
✗ Branch 20 not taken.
|
6540 | center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c; |
597 | 6540 | } | |
598 | |||
599 | 667935 | static inline Scalar maximumDistance_mesh(Vec3s* ps, Vec3s* ps2, Triangle* ts, | |
600 | unsigned int* indices, unsigned int n, | ||
601 | const Vec3s& query) { | ||
602 | 667935 | bool indirect_index = true; | |
603 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 667935 times.
|
667935 | if (!indices) indirect_index = false; |
604 | |||
605 | 667935 | Scalar maxD = 0; | |
606 |
2/2✓ Branch 0 taken 3340620 times.
✓ Branch 1 taken 667935 times.
|
4008555 | for (unsigned int i = 0; i < n; ++i) { |
607 |
1/2✓ Branch 0 taken 3340620 times.
✗ Branch 1 not taken.
|
3340620 | unsigned int index = indirect_index ? indices[i] : i; |
608 | 3340620 | const Triangle& t = ts[index]; | |
609 | |||
610 |
2/2✓ Branch 0 taken 10021860 times.
✓ Branch 1 taken 3340620 times.
|
13362480 | for (Triangle::index_type j = 0; j < 3; ++j) { |
611 | 10021860 | Triangle::index_type point_id = t[j]; | |
612 | 10021860 | const Vec3s& p = ps[point_id]; | |
613 | |||
614 |
1/2✓ Branch 2 taken 10021860 times.
✗ Branch 3 not taken.
|
10021860 | Scalar d = (p - query).squaredNorm(); |
615 |
2/2✓ Branch 0 taken 1566115 times.
✓ Branch 1 taken 8455745 times.
|
10021860 | if (d > maxD) maxD = d; |
616 | } | ||
617 | |||
618 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3340620 times.
|
3340620 | if (ps2) { |
619 | ✗ | for (Triangle::index_type j = 0; j < 3; ++j) { | |
620 | ✗ | Triangle::index_type point_id = t[j]; | |
621 | ✗ | const Vec3s& p = ps2[point_id]; | |
622 | |||
623 | ✗ | Scalar d = (p - query).squaredNorm(); | |
624 | ✗ | if (d > maxD) maxD = d; | |
625 | } | ||
626 | } | ||
627 | } | ||
628 | |||
629 | 667935 | return std::sqrt(maxD); | |
630 | } | ||
631 | |||
632 | 3 | static inline Scalar maximumDistance_pointcloud(Vec3s* ps, Vec3s* ps2, | |
633 | unsigned int* indices, | ||
634 | unsigned int n, | ||
635 | const Vec3s& query) { | ||
636 | 3 | bool indirect_index = true; | |
637 |
1/2✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
|
3 | if (!indices) indirect_index = false; |
638 | |||
639 | 3 | Scalar maxD = 0; | |
640 |
2/2✓ Branch 0 taken 36 times.
✓ Branch 1 taken 3 times.
|
39 | for (unsigned int i = 0; i < n; ++i) { |
641 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
|
36 | unsigned int index = indirect_index ? indices[i] : i; |
642 | |||
643 | 36 | const Vec3s& p = ps[index]; | |
644 |
1/2✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
|
36 | Scalar d = (p - query).squaredNorm(); |
645 |
2/2✓ Branch 0 taken 7 times.
✓ Branch 1 taken 29 times.
|
36 | if (d > maxD) maxD = d; |
646 | |||
647 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
|
36 | if (ps2) { |
648 | ✗ | const Vec3s& v = ps2[index]; | |
649 | ✗ | Scalar d = (v - query).squaredNorm(); | |
650 | ✗ | if (d > maxD) maxD = d; | |
651 | } | ||
652 | } | ||
653 | |||
654 | 3 | return std::sqrt(maxD); | |
655 | } | ||
656 | |||
657 | 667938 | Scalar maximumDistance(Vec3s* ps, Vec3s* ps2, Triangle* ts, | |
658 | unsigned int* indices, unsigned int n, | ||
659 | const Vec3s& query) { | ||
660 |
2/2✓ Branch 0 taken 667935 times.
✓ Branch 1 taken 3 times.
|
667938 | if (ts) |
661 | 667935 | return maximumDistance_mesh(ps, ps2, ts, indices, n, query); | |
662 | else | ||
663 | 3 | return maximumDistance_pointcloud(ps, ps2, indices, n, query); | |
664 | } | ||
665 | |||
666 | } // namespace coal | ||
667 |