| Directory: | ./ |
|---|---|
| File: | src/BV/kIOS.cpp |
| Date: | 2025-04-13 14:25:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 45 | 89 | 50.6% |
| Branches: | 41 | 142 | 28.9% |
| 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/BV/kIOS.h" | ||
| 39 | #include "coal/BVH/BVH_utility.h" | ||
| 40 | #include "coal/math/transform.h" | ||
| 41 | |||
| 42 | #include <iostream> | ||
| 43 | #include <limits> | ||
| 44 | |||
| 45 | namespace coal { | ||
| 46 | |||
| 47 | ✗ | bool kIOS::overlap(const kIOS& other) const { | |
| 48 | ✗ | for (unsigned int i = 0; i < num_spheres; ++i) { | |
| 49 | ✗ | for (unsigned int j = 0; j < other.num_spheres; ++j) { | |
| 50 | ✗ | Scalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); | |
| 51 | ✗ | Scalar sum_r = spheres[i].r + other.spheres[j].r; | |
| 52 | ✗ | if (o_dist > sum_r * sum_r) return false; | |
| 53 | } | ||
| 54 | } | ||
| 55 | |||
| 56 | ✗ | return obb.overlap(other.obb); | |
| 57 | } | ||
| 58 | |||
| 59 | 2974 | bool kIOS::overlap(const kIOS& other, const CollisionRequest& request, | |
| 60 | Scalar& sqrDistLowerBound) const { | ||
| 61 |
2/2✓ Branch 0 taken 10093 times.
✓ Branch 1 taken 2763 times.
|
12856 | for (unsigned int i = 0; i < num_spheres; ++i) { |
| 62 |
2/2✓ Branch 0 taken 42592 times.
✓ Branch 1 taken 9882 times.
|
52474 | for (unsigned int j = 0; j < other.num_spheres; ++j) { |
| 63 |
1/2✓ Branch 2 taken 42592 times.
✗ Branch 3 not taken.
|
42592 | Scalar o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm(); |
| 64 | 42592 | Scalar sum_r = spheres[i].r + other.spheres[j].r; | |
| 65 |
2/2✓ Branch 0 taken 211 times.
✓ Branch 1 taken 42381 times.
|
42592 | if (o_dist > sum_r * sum_r) { |
| 66 | 211 | o_dist = sqrt(o_dist) - sum_r; | |
| 67 | 211 | sqrDistLowerBound = o_dist * o_dist; | |
| 68 | 211 | return false; | |
| 69 | } | ||
| 70 | } | ||
| 71 | } | ||
| 72 | |||
| 73 | 2763 | return obb.overlap(other.obb, request, sqrDistLowerBound); | |
| 74 | } | ||
| 75 | |||
| 76 | ✗ | bool kIOS::contain(const Vec3s& p) const { | |
| 77 | ✗ | for (unsigned int i = 0; i < num_spheres; ++i) { | |
| 78 | ✗ | Scalar r = spheres[i].r; | |
| 79 | ✗ | if ((spheres[i].o - p).squaredNorm() > r * r) return false; | |
| 80 | } | ||
| 81 | |||
| 82 | ✗ | return true; | |
| 83 | } | ||
| 84 | |||
| 85 | ✗ | kIOS& kIOS::operator+=(const Vec3s& p) { | |
| 86 | ✗ | for (unsigned int i = 0; i < num_spheres; ++i) { | |
| 87 | ✗ | Scalar r = spheres[i].r; | |
| 88 | ✗ | Scalar new_r_sqr = (p - spheres[i].o).squaredNorm(); | |
| 89 | ✗ | if (new_r_sqr > r * r) { | |
| 90 | ✗ | spheres[i].r = sqrt(new_r_sqr); | |
| 91 | } | ||
| 92 | } | ||
| 93 | |||
| 94 | ✗ | obb += p; | |
| 95 | ✗ | return *this; | |
| 96 | } | ||
| 97 | |||
| 98 | 6537 | kIOS kIOS::operator+(const kIOS& other) const { | |
| 99 | 6537 | kIOS result; | |
| 100 | 6537 | unsigned int new_num_spheres = std::min(num_spheres, other.num_spheres); | |
| 101 |
2/2✓ Branch 0 taken 19611 times.
✓ Branch 1 taken 6537 times.
|
26148 | for (unsigned int i = 0; i < new_num_spheres; ++i) { |
| 102 | 19611 | result.spheres[i] = encloseSphere(spheres[i], other.spheres[i]); | |
| 103 | } | ||
| 104 | |||
| 105 | 6537 | result.num_spheres = new_num_spheres; | |
| 106 | |||
| 107 | 6537 | result.obb = obb + other.obb; | |
| 108 | |||
| 109 | 6537 | return result; | |
| 110 | } | ||
| 111 | |||
| 112 | ✗ | Scalar kIOS::width() const { return obb.width(); } | |
| 113 | |||
| 114 | ✗ | Scalar kIOS::height() const { return obb.height(); } | |
| 115 | |||
| 116 | ✗ | Scalar kIOS::depth() const { return obb.depth(); } | |
| 117 | |||
| 118 | 7592 | Scalar kIOS::volume() const { return obb.volume(); } | |
| 119 | |||
| 120 | 7592 | Scalar kIOS::size() const { return volume(); } | |
| 121 | |||
| 122 | 3106 | Scalar kIOS::distance(const kIOS& other, Vec3s* P, Vec3s* Q) const { | |
| 123 | 3106 | Scalar d_max = 0; | |
| 124 | 3106 | long id_a = -1, id_b = -1; | |
| 125 |
2/2✓ Branch 0 taken 14932 times.
✓ Branch 1 taken 3106 times.
|
18038 | for (unsigned int i = 0; i < num_spheres; ++i) { |
| 126 |
2/2✓ Branch 0 taken 64232 times.
✓ Branch 1 taken 14932 times.
|
79164 | for (unsigned int j = 0; j < other.num_spheres; ++j) { |
| 127 |
1/2✓ Branch 2 taken 64232 times.
✗ Branch 3 not taken.
|
64232 | Scalar d = (spheres[i].o - other.spheres[j].o).norm() - |
| 128 | 64232 | (spheres[i].r + other.spheres[j].r); | |
| 129 |
2/2✓ Branch 0 taken 2271 times.
✓ Branch 1 taken 61961 times.
|
64232 | if (d_max < d) { |
| 130 | 2271 | d_max = d; | |
| 131 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 2271 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
2271 | if (P && Q) { |
| 132 | ✗ | id_a = (long)i; | |
| 133 | ✗ | id_b = (long)j; | |
| 134 | } | ||
| 135 | } | ||
| 136 | } | ||
| 137 | } | ||
| 138 | |||
| 139 |
1/4✗ Branch 0 not taken.
✓ Branch 1 taken 3106 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
|
3106 | if (P && Q) { |
| 140 | ✗ | if (id_a != -1 && id_b != -1) { | |
| 141 | ✗ | const Vec3s v = spheres[id_a].o - spheres[id_b].o; | |
| 142 | ✗ | Scalar len_v = v.norm(); | |
| 143 | ✗ | *P = spheres[id_a].o - v * (spheres[id_a].r / len_v); | |
| 144 | ✗ | *Q = spheres[id_b].o + v * (spheres[id_b].r / len_v); | |
| 145 | } | ||
| 146 | } | ||
| 147 | |||
| 148 | 3106 | return d_max; | |
| 149 | } | ||
| 150 | |||
| 151 | ✗ | bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, | |
| 152 | const kIOS& b2) { | ||
| 153 | ✗ | kIOS b2_temp = b2; | |
| 154 | ✗ | for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { | |
| 155 | ✗ | b2_temp.spheres[i].o.noalias() = | |
| 156 | ✗ | R0.transpose() * (b2_temp.spheres[i].o - T0); | |
| 157 | } | ||
| 158 | |||
| 159 | ✗ | b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0); | |
| 160 | ✗ | b2_temp.obb.axes.applyOnTheLeft(R0.transpose()); | |
| 161 | |||
| 162 | ✗ | return b1.overlap(b2_temp); | |
| 163 | } | ||
| 164 | |||
| 165 | 1648 | bool overlap(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, | |
| 166 | const kIOS& b2, const CollisionRequest& request, | ||
| 167 | Scalar& sqrDistLowerBound) { | ||
| 168 |
1/2✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
|
1648 | kIOS b2_temp = b2; |
| 169 |
2/2✓ Branch 0 taken 7720 times.
✓ Branch 1 taken 1648 times.
|
9368 | for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { |
| 170 |
1/2✓ Branch 1 taken 7720 times.
✗ Branch 2 not taken.
|
7720 | b2_temp.spheres[i].o.noalias() = |
| 171 |
4/8✓ Branch 1 taken 7720 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7720 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7720 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7720 times.
✗ Branch 11 not taken.
|
15440 | R0.transpose() * (b2_temp.spheres[i].o - T0); |
| 172 | } | ||
| 173 | |||
| 174 |
5/10✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1648 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1648 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1648 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1648 times.
✗ Branch 14 not taken.
|
1648 | b2_temp.obb.To.noalias() = R0.transpose() * (b2_temp.obb.To - T0); |
| 175 |
2/4✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1648 times.
✗ Branch 5 not taken.
|
1648 | b2_temp.obb.axes.applyOnTheLeft(R0.transpose()); |
| 176 | |||
| 177 |
1/2✓ Branch 1 taken 1648 times.
✗ Branch 2 not taken.
|
3296 | return b1.overlap(b2_temp, request, sqrDistLowerBound); |
| 178 | } | ||
| 179 | |||
| 180 | 1292 | Scalar distance(const Matrix3s& R0, const Vec3s& T0, const kIOS& b1, | |
| 181 | const kIOS& b2, Vec3s* P, Vec3s* Q) { | ||
| 182 |
1/2✓ Branch 1 taken 1292 times.
✗ Branch 2 not taken.
|
1292 | kIOS b2_temp = b2; |
| 183 |
2/2✓ Branch 0 taken 5106 times.
✓ Branch 1 taken 1292 times.
|
6398 | for (unsigned int i = 0; i < b2_temp.num_spheres; ++i) { |
| 184 |
3/6✓ Branch 1 taken 5106 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5106 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5106 times.
✗ Branch 8 not taken.
|
5106 | b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0; |
| 185 | } | ||
| 186 | |||
| 187 |
1/2✓ Branch 1 taken 1292 times.
✗ Branch 2 not taken.
|
2584 | return b1.distance(b2_temp, P, Q); |
| 188 | } | ||
| 189 | |||
| 190 | ✗ | kIOS translate(const kIOS& bv, const Vec3s& t) { | |
| 191 | ✗ | kIOS res(bv); | |
| 192 | ✗ | for (size_t i = 0; i < res.num_spheres; ++i) { | |
| 193 | ✗ | res.spheres[i].o += t; | |
| 194 | } | ||
| 195 | |||
| 196 | ✗ | translate(res.obb, t); | |
| 197 | ✗ | return res; | |
| 198 | } | ||
| 199 | |||
| 200 | } // namespace coal | ||
| 201 |