Main Page Class List Class Members

NvBlastExtAuthoringVSA.h
Go to the documentation of this file.
1 // This code contains NVIDIA Confidential Information and is disclosed to you
2 // under a form of NVIDIA software license agreement provided separately to you.
3 //
4 // Notice
5 // NVIDIA Corporation and its licensors retain all intellectual property and
6 // proprietary rights in and to this software and related documentation and
7 // any modifications thereto. Any use, reproduction, disclosure, or
8 // distribution of this software and related documentation without an express
9 // license agreement from NVIDIA Corporation is strictly prohibited.
10 //
11 // ALL NVIDIA DESIGN SPECIFICATIONS, CODE ARE PROVIDED "AS IS.". NVIDIA MAKES
12 // NO WARRANTIES, EXPRESSED, IMPLIED, STATUTORY, OR OTHERWISE WITH RESPECT TO
13 // THE MATERIALS, AND EXPRESSLY DISCLAIMS ALL IMPLIED WARRANTIES OF NONINFRINGEMENT,
14 // MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE.
15 //
16 // Information and code furnished is believed to be accurate and reliable.
17 // However, NVIDIA Corporation assumes no responsibility for the consequences of use of such
18 // information or for any infringement of patents or other rights of third parties that may
19 // result from its use. No license is granted by implication or otherwise under any patent
20 // or patent rights of NVIDIA Corporation. Details are subject to change without notice.
21 // This code supersedes and replaces all information previously supplied.
22 // NVIDIA Corporation products are not authorized for use as critical
23 // components in life support devices or systems without express written approval of
24 // NVIDIA Corporation.
25 //
26 // Copyright (c) 2016-2020 NVIDIA Corporation. All rights reserved.
27
28
29 #ifndef NVBLASTEXTAUTHORINGVSA_H
30 #define NVBLASTEXTAUTHORINGVSA_H
31
32 namespace Nv
33 {
34 namespace Blast
35 {
36
37 /*
38 This code copied from APEX GSA
39 */
40
41 namespace VSA
42 {
43 typedef float real;
44
45 struct VS3D_Halfspace_Set
46 {
47 virtual real farthest_halfspace(real plane[4], const real point[4]) = 0;
48 };
49
50
51 // Simple types and operations for internal calculations
52 struct Vec3 { real x, y, z; }; // 3-vector
53 inline Vec3 vec3(real x, real y, real z) { Vec3 r; r.x = x; r.y = y; r.z = z; return r; } // vector builder
54 inline Vec3 operator + (const Vec3& a, const Vec3& b) { return vec3(a.x + b.x, a.y + b.y, a.z + b.z); } // vector addition
55 inline Vec3 operator * (real s, const Vec3& v) { return vec3(s*v.x, s*v.y, s*v.z); } // scalar multiplication
56 inline real operator | (const Vec3& a, const Vec3& b) { return a.x*b.x + a.y*b.y + a.z*b.z; } // dot product
57 inline Vec3 operator ^ (const Vec3& a, const Vec3& b) { return vec3(a.y*b.z - b.y*a.z, a.z*b.x - b.z*a.x, a.x*b.y - b.x*a.y); } // cross product
58
59 struct Vec4 { Vec3 v; real w; }; // 4-vector split into 3-vector and scalar parts
60 inline Vec4 vec4(const Vec3& v, real w) { Vec4 r; r.v = v; r.w = w; return r; } // vector builder
61 inline real operator | (const Vec4& a, const Vec4& b) { return (a.v | b.v) + a.w*b.w; } // dot product
62
63 // More accurate perpendicular
64 inline Vec3 perp(const Vec3& a, const Vec3& b)
65 {
66 Vec3 c = a^b; // Cross-product gives perpendicular
67 #if VS3D_HIGH_ACCURACY || REAL_DOUBLE
68 const real c2 = c | c;
69 if (c2 != 0) c = c + (1 / c2)*((a | c)*(c^b) + (b | c)*(a^c)); // Improvement to (a b)^T(c) = (0)
70 #endif
71 return c;
72 }
73
74 // Square
75 inline real sq(real x) { return x*x; }
76
77 // Returns index of the extremal element in a three-element set {e0, e1, e2} based upon comparisons c_ij. The extremal index m is such that c_mn is true, or e_m == e_n, for all n.
78 inline int ext_index(int c_10, int c_21, int c_20) { return c_10 << c_21 | (c_21&c_20) << 1; }
79
80 // Returns index (0, 1, or 2) of minimum argument
81 inline int index_of_min(real x0, real x1, real x2) { return ext_index((int)(x1 < x0), (int)(x2 < x1), (int)(x2 < x0)); }
82
83 // Compare fractions with positive deominators. Returns a_num*sqrt(a_rden2) > b_num*sqrt(b_rden2)
84 inline bool frac_gt(real a_num, real a_rden2, real b_num, real b_rden2)
85 {
86 const bool a_num_neg = a_num < 0;
87 const bool b_num_neg = b_num < 0;
88 return a_num_neg != b_num_neg ? b_num_neg : ((a_num*a_num*a_rden2 > b_num*b_num*b_rden2) != a_num_neg);
89 }
90
91 // Returns index (0, 1, or 2) of maximum fraction with positive deominators
92 inline int index_of_max_frac(real x0_num, real x0_rden2, real x1_num, real x1_rden2, real x2_num, real x2_rden2)
93 {
94 return ext_index((int)frac_gt(x1_num, x1_rden2, x0_num, x0_rden2), (int)frac_gt(x2_num, x2_rden2, x1_num, x1_rden2), (int)frac_gt(x2_num, x2_rden2, x0_num, x0_rden2));
95 }
96
97 // Compare values given their signs and squares. Returns a > b. a2 and b2 may have any constant offset applied to them.
98 inline bool sgn_sq_gt(real sgn_a, real a2, real sgn_b, real b2) { return sgn_a*sgn_b < 0 ? (sgn_b < 0) : ((a2 > b2) != (sgn_a < 0)); }
99
100 // Returns index (0, 1, or 2) of maximum value given their signs and squares. sq_x0, sq_x1, and sq_x2 may have any constant offset applied to them.
101 inline int index_of_max_sgn_sq(real sgn_x0, real sq_x0, real sgn_x1, real sq_x1, real sgn_x2, real sq_x2)
102 {
103 return ext_index((int)sgn_sq_gt(sgn_x1, sq_x1, sgn_x0, sq_x0), (int)sgn_sq_gt(sgn_x2, sq_x2, sgn_x1, sq_x1), (int)sgn_sq_gt(sgn_x2, sq_x2, sgn_x0, sq_x0));
104 }
105
106 // Project 2D (homogeneous) vector onto 2D half-space boundary
107 inline void project2D(Vec3& r, const Vec3& plane, real delta, real recip_n2, real eps2)
108 {
109 r = r + (-delta*recip_n2)*vec3(plane.x, plane.y, 0);
110 r = r + (-(r | plane)*recip_n2)*vec3(plane.x, plane.y, 0); // Second projection for increased accuracy
111 if ((r | r) > eps2) return;
112 r = (-plane.z*recip_n2)*vec3(plane.x, plane.y, 0);
113 r.z = 1;
114 }
115
116
117 // Update function for vs3d_test
118 static bool vs3d_update(Vec4& p, Vec4 S[4], int& plane_count, const Vec4& q, real eps2)
119 {
120 // h plane is the last plane
121 const Vec4& h = S[plane_count - 1];
122
123 // Handle plane_count == 1 specially (optimization; this could be commented out)
124 if (plane_count == 1)
125 {
126 // Solution is objective projected onto h plane
127 p = q;
128 p.v = p.v + -(p | h)*h.v;
129 if ((p | p) <= eps2) p = vec4(-h.w*h.v, 1); // If p == 0 then q is a direction vector, any point in h is a support point
130 return true;
131 }
132
133 // Create basis in the h plane
134 const int min_i = index_of_min(h.v.x*h.v.x, h.v.y*h.v.y, h.v.z*h.v.z);
135 const Vec3 y = h.v^vec3((real)(min_i == 0), (real)(min_i == 1), (real)(min_i == 2));
136 const Vec3 x = y^h.v;
137
138 // Use reduced vector r instead of p
139 Vec3 r = { x | q.v, y | q.v, q.w*(y | y) }; // (x|x) = (y|y) = square of plane basis scale
140
141 // If r == 0 (within epsilon), then it is a direction vector, and we have a bounded solution
142 if ((r | r) <= eps2) r.z = 1;
143
144 // Create plane equations in the h plane. These will not be normalized in general.
145 int N = 0; // Plane count in h subspace
146 Vec3 R[3]; // Planes in h subspace
147 real recip_n2[3]; // Plane normal vector reciprocal lengths squared
148 real delta[3]; // Signed distance of objective to the planes
149 int index[3]; // Keep track of original plane indices
150 for (int i = 0; i < plane_count - 1; ++i)
151 {
152 const Vec3& vi = S[i].v;
153 const real cos_theta = h.v | vi;
154 R[N] = vec3(x | vi, y | vi, S[i].w - h.w*cos_theta);
155 index[N] = i;
156 const real n2 = R[N].x*R[N].x + R[N].y*R[N].y;
157 if (n2 >= eps2)
158 {
159 const real lin_norm = (real)1.5 - (real)0.5*n2; // 1st-order approximation to 1/sqrt(n2) expanded about n2 = 1
160 R[N] = lin_norm*R[N]; // We don't need normalized plane equations, but rescaling (even with an approximate normalization) gives better numerical behavior
161 recip_n2[N] = 1 / (R[N].x*R[N].x + R[N].y*R[N].y);
162 delta[N] = r | R[N];
163 ++N; // Keep this plane
164 }
165 else if (cos_theta < 0) return false; // Parallel cases are redundant and rejected, anti-parallel cases are 1D voids
166 }
167
168 // Now work with the N-sized R array of half-spaces in the h plane
169 switch (N)
170 {
171 case 1: one_plane :
172 if (delta[0] < 0) N = 0; // S[0] is redundant, eliminate it
173 else project2D(r, R[0], delta[0], recip_n2[0], eps2);
174 break;
175 case 2: two_planes :
176 if (delta[0] < 0 && delta[1] < 0) N = 0; // S[0] and S[1] are redundant, eliminate them
177 else
178 {
179 const int max_d_index = (int)frac_gt(delta[1], recip_n2[1], delta[0], recip_n2[0]);
180 project2D(r, R[max_d_index], delta[max_d_index], recip_n2[max_d_index], eps2);
181 const int min_d_index = max_d_index ^ 1;
182 const real new_delta_min = r | R[min_d_index];
183 if (new_delta_min < 0)
184 {
185 index[0] = index[max_d_index];
186 N = 1; // S[min_d_index] is redundant, eliminate it
187 }
188 else
189 {
190 // Set r to the intersection of R[0] and R[1] and keep both
191 r = perp(R[0], R[1]);
192 if (r.z*r.z*recip_n2[0] * recip_n2[1] < eps2)
193 {
194 if (R[0].x*R[1].x + R[0].y*R[1].y < 0) return false; // 2D void found
195 goto one_plane;
196 }
197 r = (1 / r.z)*r; // We could just as well multiply r by sgn(r.z); we just need to ensure r.z > 0
198 }
199 }
200 break;
201 case 3:
202 if (delta[0] < 0 && delta[1] < 0 && delta[2] < 0) N = 0; // S[0], S[1], and S[2] are redundant, eliminate them
203 else
204 {
205 const Vec3 row_x = { R[0].x, R[1].x, R[2].x };
206 const Vec3 row_y = { R[0].y, R[1].y, R[2].y };
207 const Vec3 row_w = { R[0].z, R[1].z, R[2].z };
208 const Vec3 cof_w = perp(row_x, row_y);
209 const bool detR_pos = (row_w | cof_w) > 0;
210 const int nrw_sgn0 = cof_w.x*cof_w.x*recip_n2[1] * recip_n2[2] < eps2 ? 0 : (((int)((cof_w.x > 0) == detR_pos) << 1) - 1);
211 const int nrw_sgn1 = cof_w.y*cof_w.y*recip_n2[2] * recip_n2[0] < eps2 ? 0 : (((int)((cof_w.y > 0) == detR_pos) << 1) - 1);
212 const int nrw_sgn2 = cof_w.z*cof_w.z*recip_n2[0] * recip_n2[1] < eps2 ? 0 : (((int)((cof_w.z > 0) == detR_pos) << 1) - 1);
213
214 if ((nrw_sgn0 | nrw_sgn1 | nrw_sgn2) >= 0) return false; // 3D void found
215
216 const int positive_width_count = ((nrw_sgn0 >> 1) & 1) + ((nrw_sgn1 >> 1) & 1) + ((nrw_sgn2 >> 1) & 1);
217 if (positive_width_count == 1)
218 {
219 // A single positive width results from a redundant plane. Eliminate it and peform N = 2 calculation.
220 const int pos_width_index = ((nrw_sgn1 >> 1) & 1) | (nrw_sgn2 & 2); // Calculates which index corresponds to the positive-width side
221 R[pos_width_index] = R[2];
222 recip_n2[pos_width_index] = recip_n2[2];
223 delta[pos_width_index] = delta[2];
224 index[pos_width_index] = index[2];
225 N = 2;
226 goto two_planes;
227 }
228
229 // Find the max dot product of r and R[i]/|R_normal[i]|. For numerical accuracy when the angle between r and the i^{th} plane normal is small, we take some care below:
230 const int max_d_index = r.z != 0
231 ? index_of_max_frac(delta[0], recip_n2[0], delta[1], recip_n2[1], delta[2], recip_n2[2]) // displacement term resolves small-angle ambiguity, just use dot product
232 : index_of_max_sgn_sq(delta[0], -sq(r.x*R[0].y - r.y*R[0].x)*recip_n2[0], delta[1], -sq(r.x*R[1].y - r.y*R[1].x)*recip_n2[1], delta[2], -sq(r.x*R[2].y - r.y*R[2].x)*recip_n2[2]); // No displacement term. Use wedge product to find the sine of the angle.
233
234 // Project r onto max-d plane
235 project2D(r, R[max_d_index], delta[max_d_index], recip_n2[max_d_index], eps2);
236 N = 1; // Unless we use a vertex in the loop below
237 const int index_max = index[max_d_index];
238
239 // The number of finite widths should be >= 2. If not, it should be 0, but in any case it implies three parallel lines in the plane, which we should not have here.
240 // If we do have three parallel lines (# of finite widths < 2), we've picked the line corresponding to the half-plane farthest from r, which is correct.
241 const int finite_width_count = (nrw_sgn0 & 1) + (nrw_sgn1 & 1) + (nrw_sgn2 & 1);
242 if (finite_width_count >= 2)
243 {
244 const int i_remaining[2] = { (1 << max_d_index) & 3, (3 >> max_d_index) ^ 1 }; // = {(max_d_index+1)%3, (max_d_index+2)%3}
245 const int i_select = (int)frac_gt(delta[i_remaining[1]], recip_n2[i_remaining[1]], delta[i_remaining[0]], recip_n2[i_remaining[0]]); // Select the greater of the remaining dot products
246 for (int i = 0; i < 2; ++i)
247 {
248 const int j = i_remaining[i_select^i]; // i = 0 => the next-greatest, i = 1 => the least
249 if ((r | R[j]) >= 0)
250 {
251 r = perp(R[max_d_index], R[j]);
252 r = (1 / r.z)*r; // We could just as well multiply r by sgn(r.z); we just need to ensure r.z > 0
253 index[1] = index[j];
254 N = 2;
255 break;
256 }
257 }
258 }
259
260 index[0] = index_max;
261 }
262 break;
263 }
264
265 // Transform r back to 3D space
266 p = vec4(r.x*x + r.y*y + (-r.z*h.w)*h.v, r.z);
267
268 // Pack S array with kept planes
269 if (N < 2 || index[1] != 0) { for (int i = 0; i < N; ++i) S[i] = S[index[i]]; } // Safe to copy columns in order
270 else { const Vec4 temp = S[0]; S[0] = S[index[0]]; S[1] = temp; } // Otherwise use temp storage to avoid overwrite
271 S[N] = h;
272 plane_count = N + 1;
273
274 return true;
275 }
276
277
278 // Performs the VS algorithm for D = 3
279 inline int vs3d_test(VS3D_Halfspace_Set& halfspace_set, real* q = nullptr)
280 {
281 // Objective = q if it is not NULL, otherwise it is the origin represented in homogeneous coordinates
282 const Vec4 objective = q ? (q[3] != 0 ? vec4((1 / q[3])*vec3(q[0], q[1], q[2]), 1) : *(Vec4*)q) : vec4(vec3(0, 0, 0), 1);
283
284 // Tolerance for 3D void simplex algorithm
285 const real eps_f = (real)1 / (sizeof(real) == 4 ? (1L << 23) : (1LL << 52)); // Floating-point epsilon
286 #if VS3D_HIGH_ACCURACY || REAL_DOUBLE
287 const real eps = 8 * eps_f;
288 #else
289 const real eps = 80 * eps_f;
290 #endif
291 const real eps2 = eps*eps; // Using epsilon squared
292
293 // Maximum allowed iterations of main loop. If exceeded, error code is returned
294 const int max_iteration_count = 50;
295
296 // State
297 Vec4 S[4]; // Up to 4 planes
298 int plane_count = 0; // Number of valid planes
299 Vec4 p = objective; // Test point, initialized to objective
300
301 // Default result, changed to valid result if found in loop below
302 int result = -1;
303
304 // Iterate until a stopping condition is met or the maximum number of iterations is reached
305 for (int i = 0; result < 0 && i < max_iteration_count; ++i)
306 {
307 Vec4& plane = S[plane_count++];
308 real delta = halfspace_set.farthest_halfspace(&plane.v.x, &p.v.x);
309 #if VS3D_UNNORMALIZED_PLANE_HANDLING != 0
310 const real recip_norm = vs3d_recip_sqrt(plane.v | plane.v);
311 plane = vec4(recip_norm*plane.v, recip_norm*plane.w);
312 delta *= recip_norm;
313 #endif
314 if (delta <= 0 || delta*delta <= eps2*(p | p)) result = 1; // Intersection found
315 else if (!vs3d_update(p, S, plane_count, objective, eps2)) result = 0; // Void simplex found
316 }
317
318 // If q is given, fill it with the solution (normalize p.w if it is not zero)
319 if (q) *(Vec4*)q = (p.w != 0) ? vec4((1 / p.w)*p.v, 1) : p;
320
321 return result;
322 }
323
324 } // namespace VSA
325
326 } // namespace Blast
327 } // namespace Nv
328
329
330 #endif // ifndef NVBLASTEXTAUTHORINGVSA_H
Nv::Blast::VSA::VS3D_Halfspace_Set
Definition: NvBlastExtAuthoringVSA.h:45
Nv::Blast::VSA::vec4
Vec4 vec4(const Vec3 &v, real w)
Definition: NvBlastExtAuthoringVSA.h:60
Nv::Blast::VSA::Vec3::z
real z
Definition: NvBlastExtAuthoringVSA.h:52
Nv::Blast::VSA::frac_gt
bool frac_gt(real a_num, real a_rden2, real b_num, real b_rden2)
Definition: NvBlastExtAuthoringVSA.h:84
Nv::Blast::VSA::project2D
void project2D(Vec3 &r, const Vec3 &plane, real delta, real recip_n2, real eps2)
Definition: NvBlastExtAuthoringVSA.h:107
x
SIMD_FORCE_INLINE const btScalar & x() const
Return the x value.
Definition: btVector3.h:275
Nv::Blast::VSA::operator*
Vec3 operator*(real s, const Vec3 &v)
Definition: NvBlastExtAuthoringVSA.h:55
Nv::Blast::VSA::vec3
Vec3 vec3(real x, real y, real z)
Definition: NvBlastExtAuthoringVSA.h:53
Nv::Blast::VSA::Vec3::y
real y
Definition: NvBlastExtAuthoringVSA.h:52
Nv::Blast::VSA::VS3D_Halfspace_Set::farthest_halfspace
virtual real farthest_halfspace(real plane[4], const real point[4])=0
Nv::Blast::VSA::Vec4::w
real w
Definition: NvBlastExtAuthoringVSA.h:59
Nv::Blast::VSA::Vec4::v
Vec3 v
Definition: NvBlastExtAuthoringVSA.h:59
y
SIMD_FORCE_INLINE const btScalar & y() const
Return the y value.
Definition: btVector3.h:277
Nv::Blast::VSA::index_of_max_frac
int index_of_max_frac(real x0_num, real x0_rden2, real x1_num, real x1_rden2, real x2_num, real x2_rden2)
Definition: NvBlastExtAuthoringVSA.h:92
Nv::Blast::VSA::Vec3
Definition: NvBlastExtAuthoringVSA.h:52
Nv::Blast::VSA::sq
real sq(real x)
Definition: NvBlastExtAuthoringVSA.h:75
Nv::Blast::VSA::ext_index
int ext_index(int c_10, int c_21, int c_20)
Definition: NvBlastExtAuthoringVSA.h:78
Nv::Blast::VSA::index_of_min
int index_of_min(real x0, real x1, real x2)
Definition: NvBlastExtAuthoringVSA.h:81
Nv::Blast::VSA::index_of_max_sgn_sq
int index_of_max_sgn_sq(real sgn_x0, real sq_x0, real sgn_x1, real sq_x1, real sgn_x2, real sq_x2)
Definition: NvBlastExtAuthoringVSA.h:101
Nv::Blast::VSA::Vec3::x
real x
Definition: NvBlastExtAuthoringVSA.h:52
Nv::Blast::VSA::vs3d_test
int vs3d_test(VS3D_Halfspace_Set &halfspace_set, real *q=nullptr)
Definition: NvBlastExtAuthoringVSA.h:279
Nv::Blast::VSA::Vec4
Definition: NvBlastExtAuthoringVSA.h:59
z
SIMD_FORCE_INLINE const btScalar & z() const
Return the z value.
Definition: btVector3.h:279
Nv::Blast::VSA::perp
Vec3 perp(const Vec3 &a, const Vec3 &b)
Definition: NvBlastExtAuthoringVSA.h:64
Nv::Blast::VSA::real
float real
Definition: NvBlastExtAuthoringVSA.h:43
w
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btVector3.h:281
Nv::Blast::VSA::operator|
real operator|(const Vec3 &a, const Vec3 &b)
Definition: NvBlastExtAuthoringVSA.h:56
Nv::Blast::VSA::operator+
Vec3 operator+(const Vec3 &a, const Vec3 &b)
Definition: NvBlastExtAuthoringVSA.h:54
Nv
Definition: NvBlastArray.h:37
Nv::Blast::VSA::sgn_sq_gt
bool sgn_sq_gt(real sgn_a, real a2, real sgn_b, real b2)
Definition: NvBlastExtAuthoringVSA.h:98
Nv::Blast::VSA::operator^
Vec3 operator^(const Vec3 &a, const Vec3 &b)
Definition: NvBlastExtAuthoringVSA.h:57