Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
godotengine
GitHub Repository: godotengine/godot
 Path: blob/master/thirdparty/manifold/src/tri_dist.h
10278 views
1
// Copyright 2024 The Manifold Authors.

2
//

3
// Licensed under the Apache License, Version 2.0 (the "License");

4
// you may not use this file except in compliance with the License.

5
// You may obtain a copy of the License at

6
//

7
//      http://www.apache.org/licenses/LICENSE-2.0

8
//

9
// Unless required by applicable law or agreed to in writing, software

10
// distributed under the License is distributed on an "AS IS" BASIS,

11
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

12
// See the License for the specific language governing permissions and

13
// limitations under the License.

14


15
#pragma once

16


17
#include <array>

18
#include <cstdint>

19


20
#include "manifold/common.h"

21


22
namespace manifold {

23


24
// From NVIDIA-Omniverse PhysX - BSD 3-Clause "New" or "Revised" License

25
// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/LICENSE.md

26
// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/physx/source/geomutils/src/sweep/GuSweepCapsuleCapsule.cpp

27
// With minor modifications

28


29
/**

30
 * Returns the distance between two line segments.

31
 *

32
 * @param[out] x Closest point on line segment pa.

33
 * @param[out] y Closest point on line segment qb.

34
 * @param[in]  p  One endpoint of the first line segment.

35
 * @param[in]  a  Other endpoint of the first line segment.

36
 * @param[in]  p  One endpoint of the second line segment.

37
 * @param[in]  b  Other endpoint of the second line segment.

38
 */

39
inline void EdgeEdgeDist(vec3& x, vec3& y,  // closest points

40
                         const vec3& p,

41
                         const vec3& a,  // seg 1 origin, vector

42
                         const vec3& q,

43
                         const vec3& b)  // seg 2 origin, vector

44
{

45
  const vec3 T = q - p;

46
  const auto ADotA = la::dot(a, a);

47
  const auto BDotB = la::dot(b, b);

48
  const auto ADotB = la::dot(a, b);

49
  const auto ADotT = la::dot(a, T);

50
  const auto BDotT = la::dot(b, T);

51


52
  // t parameterizes ray (p, a)

53
  // u parameterizes ray (q, b)

54


55
  // Compute t for the closest point on ray (p, a) to ray (q, b)

56
  const auto Denom = ADotA * BDotB - ADotB * ADotB;

57


58
  double t;  // We will clamp result so t is on the segment (p, a)

59
  t = Denom != 0.0

60
          ? la::clamp((ADotT * BDotB - BDotT * ADotB) / Denom, 0.0, 1.0)

61
          : 0.0;

62


63
  // find u for point on ray (q, b) closest to point at t

64
  double u;

65
  if (BDotB != 0.0) {

66
    u = (t * ADotB - BDotT) / BDotB;

67


68
    // if u is on segment (q, b), t and u correspond to closest points,

69
    // otherwise, clamp u, recompute and clamp t

70
    if (u < 0.0) {

71
      u = 0.0;

72
      t = ADotA != 0.0 ? la::clamp(ADotT / ADotA, 0.0, 1.0) : 0.0;

73
    } else if (u > 1.0) {

74
      u = 1.0;

75
      t = ADotA != 0.0 ? la::clamp((ADotB + ADotT) / ADotA, 0.0, 1.0) : 0.0;

76
    }

77
  } else {

78
    u = 0.0;

79
    t = ADotA != 0.0 ? la::clamp(ADotT / ADotA, 0.0, 1.0) : 0.0;

80
  }

81
  x = p + a * t;

82
  y = q + b * u;

83
}

84


85
// From NVIDIA-Omniverse PhysX - BSD 3-Clause "New" or "Revised" License

86
// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/LICENSE.md

87
// https://github.com/NVIDIA-Omniverse/PhysX/blob/main/physx/source/geomutils/src/distance/GuDistanceTriangleTriangle.cpp

88
// With minor modifications

89


90
/**

91
 * Returns the minimum squared distance between two triangles.

92
 *

93
 * @param  p  First  triangle.

94
 * @param  q  Second triangle.

95
 */

96
inline auto DistanceTriangleTriangleSquared(const std::array<vec3, 3>& p,

97
                                            const std::array<vec3, 3>& q) {

98
  std::array<vec3, 3> Sv;

99
  Sv[0] = p[1] - p[0];

100
  Sv[1] = p[2] - p[1];

101
  Sv[2] = p[0] - p[2];

102


103
  std::array<vec3, 3> Tv;

104
  Tv[0] = q[1] - q[0];

105
  Tv[1] = q[2] - q[1];

106
  Tv[2] = q[0] - q[2];

107


108
  bool shown_disjoint = false;

109


110
  auto mindd = std::numeric_limits<double>::max();

111


112
  for (uint32_t i = 0; i < 3; i++) {

113
    for (uint32_t j = 0; j < 3; j++) {

114
      vec3 cp;

115
      vec3 cq;

116
      EdgeEdgeDist(cp, cq, p[i], Sv[i], q[j], Tv[j]);

117
      const vec3 V = cq - cp;

118
      const auto dd = la::dot(V, V);

119


120
      if (dd <= mindd) {

121
        mindd = dd;

122


123
        uint32_t id = i + 2;

124
        if (id >= 3) id -= 3;

125
        vec3 Z = p[id] - cp;

126
        auto a = la::dot(Z, V);

127
        id = j + 2;

128
        if (id >= 3) id -= 3;

129
        Z = q[id] - cq;

130
        auto b = la::dot(Z, V);

131


132
        if ((a <= 0.0) && (b >= 0.0)) {

133
          return la::dot(V, V);

134
        };

135


136
        if (a <= 0.0)

137
          a = 0.0;

138
        else if (b > 0.0)

139
          b = 0.0;

140


141
        if ((mindd - a + b) > 0.0) shown_disjoint = true;

142
      }

143
    }

144
  }

145


146
  vec3 Sn = la::cross(Sv[0], Sv[1]);

147
  auto Snl = la::dot(Sn, Sn);

148


149
  if (Snl > 1e-15) {

150
    const vec3 Tp(la::dot(p[0] - q[0], Sn), la::dot(p[0] - q[1], Sn),

151
                  la::dot(p[0] - q[2], Sn));

152


153
    int index = -1;

154
    if ((Tp[0] > 0.0) && (Tp[1] > 0.0) && (Tp[2] > 0.0)) {

155
      index = Tp[0] < Tp[1] ? 0 : 1;

156
      if (Tp[2] < Tp[index]) index = 2;

157
    } else if ((Tp[0] < 0.0) && (Tp[1] < 0.0) && (Tp[2] < 0.0)) {

158
      index = Tp[0] > Tp[1] ? 0 : 1;

159
      if (Tp[2] > Tp[index]) index = 2;

160
    }

161


162
    if (index >= 0) {

163
      shown_disjoint = true;

164


165
      const vec3& qIndex = q[index];

166


167
      vec3 V = qIndex - p[0];

168
      vec3 Z = la::cross(Sn, Sv[0]);

169
      if (la::dot(V, Z) > 0.0) {

170
        V = qIndex - p[1];

171
        Z = la::cross(Sn, Sv[1]);

172
        if (la::dot(V, Z) > 0.0) {

173
          V = qIndex - p[2];

174
          Z = la::cross(Sn, Sv[2]);

175
          if (la::dot(V, Z) > 0.0) {

176
            vec3 cp = qIndex + Sn * Tp[index] / Snl;

177
            vec3 cq = qIndex;

178
            return la::dot(cp - cq, cp - cq);

179
          }

180
        }

181
      }

182
    }

183
  }

184


185
  vec3 Tn = la::cross(Tv[0], Tv[1]);

186
  auto Tnl = la::dot(Tn, Tn);

187


188
  if (Tnl > 1e-15) {

189
    const vec3 Sp(la::dot(q[0] - p[0], Tn), la::dot(q[0] - p[1], Tn),

190
                  la::dot(q[0] - p[2], Tn));

191


192
    int index = -1;

193
    if ((Sp[0] > 0.0) && (Sp[1] > 0.0) && (Sp[2] > 0.0)) {

194
      index = Sp[0] < Sp[1] ? 0 : 1;

195
      if (Sp[2] < Sp[index]) index = 2;

196
    } else if ((Sp[0] < 0.0) && (Sp[1] < 0.0) && (Sp[2] < 0.0)) {

197
      index = Sp[0] > Sp[1] ? 0 : 1;

198
      if (Sp[2] > Sp[index]) index = 2;

199
    }

200


201
    if (index >= 0) {

202
      shown_disjoint = true;

203


204
      const vec3& pIndex = p[index];

205


206
      vec3 V = pIndex - q[0];

207
      vec3 Z = la::cross(Tn, Tv[0]);

208
      if (la::dot(V, Z) > 0.0) {

209
        V = pIndex - q[1];

210
        Z = la::cross(Tn, Tv[1]);

211
        if (la::dot(V, Z) > 0.0) {

212
          V = pIndex - q[2];

213
          Z = la::cross(Tn, Tv[2]);

214
          if (la::dot(V, Z) > 0.0) {

215
            vec3 cp = pIndex;

216
            vec3 cq = pIndex + Tn * Sp[index] / Tnl;

217
            return la::dot(cp - cq, cp - cq);

218
          }

219
        }

220
      }

221
    }

222
  }

223


224
  return shown_disjoint ? mindd : 0.0;

225
};

226
}  // namespace manifold

227


228