nlib
SimdGeometry.h
Go to the documentation of this file.
1 
2 #pragma once
3 #ifndef INCLUDE_NN_NLIB_SIMD_SIMDGEOMETRY_H_
4 #define INCLUDE_NN_NLIB_SIMD_SIMDGEOMETRY_H_
5 
11 
12 NLIB_NAMESPACE_BEGIN
13 namespace simd {
14 
15 class AxisAlignedBox;
16 class OrientedBox;
17 class Frustum;
18 
19 // satisfies plane[0] * x + plane[1] * y + plane[2] * z + plane[3] = 0,
20 // note that most of the APIs require the plane to be normalized.
22  public:
23  static f128 __vectorcall Dot(SimdPlaneArg plane, SimdVectorArg vec) NLIB_NOEXCEPT;
24  static f128 __vectorcall DotCoord(SimdPlaneArg plane, SimdVectorArg vec) NLIB_NOEXCEPT;
25  static f128 __vectorcall DotNormal(SimdPlaneArg plane, SimdVectorArg vec) NLIB_NOEXCEPT;
26  static SimdPlane __vectorcall FromPointAndNormal(SimdVectorArg point,
28  static SimdPlane __vectorcall FromPoint(SimdVectorArg point0,
29  SimdVectorArg point1,
31  static SimdPlane __vectorcall Normalize(SimdPlaneArg plane) NLIB_NOEXCEPT;
32  static SimdPlane __vectorcall NormalizeEst(SimdPlaneArg plane) NLIB_NOEXCEPT;
33  static SimdPlane __vectorcall Transform(SimdPlaneArg plane, SimdMatrixArg m) NLIB_NOEXCEPT;
34 
35  private:
36  Plane(); // forbidden
37 };
38 
39 // the lane 012 is the center of the sphere, and the lane 3 is radius.
41  public:
42  // ctors
43  static SimdSphere __vectorcall FromPoints(const Float3* points, size_t count) NLIB_NOEXCEPT;
44  static SimdSphere __vectorcall Merge(SimdSphereArg sphere0,
46 
47  // get/set
48  static float __vectorcall GetRadius(SimdSphereArg sphere) NLIB_NOEXCEPT;
49  static SimdSphere __vectorcall SetRadius(SimdSphereArg sphere, float radius) NLIB_NOEXCEPT;
50  static SimdVector __vectorcall GetCenter(SimdSphereArg sphere) NLIB_NOEXCEPT;
51  static SimdSphere __vectorcall
52  SetCenter(SimdSphereArg sphere, SimdVectorArg center) NLIB_NOEXCEPT;
53 
54  static SimdSphere __vectorcall Transform(SimdSphereArg sphere, SimdMatrixArg m) NLIB_NOEXCEPT;
55 
56  private:
57  Sphere(); // forbidden
58 };
59 
60 // point_min[012] must be less than point_max[012]. the lane 3 is ignored.
62  public:
63  AxisAlignedBox() NLIB_NOEXCEPT {} // no initialize
64  AxisAlignedBox(const Float3& pmin, const Float3& pmax);
65  void GetCorners(Float3* corners) const NLIB_NOEXCEPT;
66  void __vectorcall Transform(AxisAlignedBox* box, SimdMatrixArg m) NLIB_NOEXCEPT;
67  static void Merge(AxisAlignedBox* box, const AxisAlignedBox& box0,
68  const AxisAlignedBox& box1) NLIB_NOEXCEPT;
69  static void __vectorcall FromSphere(AxisAlignedBox* box, SimdSphereArg sphere) NLIB_NOEXCEPT;
70  static void __vectorcall FromPoints(AxisAlignedBox* box, SimdVectorArg point0,
72  static void FromPoints(AxisAlignedBox* box, const Float3* points, size_t count) NLIB_NOEXCEPT;
73 
74  public:
77 };
78 
79 // extent[012] must be positive
81  public:
82  OrientedBox() NLIB_NOEXCEPT {} // no initialize
83  void __vectorcall Transform(OrientedBox* box, SimdMatrixArg m) NLIB_NOEXCEPT;
84  void GetCorners(Float3* corners) const NLIB_NOEXCEPT;
85  static void FromAxisAlignedBox(OrientedBox* box, const AxisAlignedBox& aabb) NLIB_NOEXCEPT;
86  // static void FromPoints(OrientedBox* box, const Float3* points, size_t count) NLIB_NOEXCEPT;
87 
88  public:
92 };
93 
94 class NLIB_VIS_HIDDEN Capsule {
95  public:
96  Capsule() NLIB_NOEXCEPT {} // no initialize
97  Capsule(const Float3& pt0, const Float3& pt1, float radius) NLIB_NOEXCEPT;
98  void __vectorcall Transform(Capsule* capsule, SimdMatrixArg m) NLIB_NOEXCEPT;
99 
100  public:
101  SimdSphere sphere;
102  SimdVector point;
103 };
104 
106  public:
107  Frustum() NLIB_NOEXCEPT {} // no initialize
108  Frustum(const Frustum& rhs) NLIB_NOEXCEPT;
109  Frustum& operator=(const Frustum& rhs) NLIB_NOEXCEPT;
110  Frustum(SimdVectorArg center, SimdQuaternionArg rotation, float top, float bottom, float left,
111  float right, float n, float f) NLIB_NOEXCEPT;
112  void __vectorcall Set(SimdVectorArg center, SimdQuaternionArg rotation, float top, float bottom,
113  float left, float right, float n, float f) NLIB_NOEXCEPT;
114  void __vectorcall Transform(Frustum* frustum, float scale, SimdQuaternionArg rotation,
115  SimdVectorArg translation) const NLIB_NOEXCEPT;
116  void __vectorcall Transform(Frustum* frustum, SimdMatrixArg m) const NLIB_NOEXCEPT;
117  void GetCorners(Float3* corners) const NLIB_NOEXCEPT;
118 
119  private:
120  // Right-handed
121  SimdVector center_;
122  SimdQuaternion rotation_;
123  SimdPlane near_plane_; // plane = (0, 0, -1, -near)
124  SimdPlane far_plane_; // plane = (0, 0, 1, -far)
125  SimdPlane top_plane_; // plane = (0, 1, top/near, 0)
126  SimdPlane bottom_plane_; // plane = (0, -1, -bottom/near, 0)
127  SimdPlane left_plane_; // plane = (-1, 0, -left/near, 0)
128  SimdPlane right_plane_; // plane = (1, 0, right/near, 0)
129  friend class Intersection;
130  friend class Containment;
131 };
132 
133 // has only static member functions
135  public:
136  static f128 __vectorcall PointLine(SimdVectorArg point, SimdVectorArg line_point,
137  SimdVectorArg line_dir_normalized) NLIB_NOEXCEPT;
138  static f128 __vectorcall PointRay(SimdVectorArg point, SimdVectorArg ray_point,
139  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
140  static f128 __vectorcall PointSegment(SimdVectorArg point, SimdVectorArg segment_point0,
141  SimdVectorArg segment_point1) NLIB_NOEXCEPT;
142  static f128 __vectorcall PointPlane(SimdVector* point_on_plane, SimdVectorArg point,
143  SimdPlaneArg plane_normalized) NLIB_NOEXCEPT;
144  static f128 __vectorcall
145  SpherePlane(SimdSphereArg sphere, SimdPlaneArg plane_normalized) NLIB_NOEXCEPT;
146  static f128 __vectorcall PointAxisAlignedBox(SimdVector* point_on_box, SimdVectorArg point,
147  const AxisAlignedBox& aabb) NLIB_NOEXCEPT;
148  static f128 __vectorcall LineLine(SimdVector* point_on_line0, SimdVector* point_on_line1,
149  SimdVectorArg line0_point, SimdVectorArg line0_dir_normalized,
150  SimdVectorArg line1_point,
151  SimdVectorArg line1_dir_normalized) NLIB_NOEXCEPT;
152  static f128 __vectorcall
153  SegmentSegment(SimdVector* point_on_segment0, SimdVector* point_on_segment1,
154  SimdVectorArg segment0_point0, SimdVectorArg segment0_point1,
155  SimdVectorArg segment1_point0, SimdVectorArg segment1_point1) NLIB_NOEXCEPT;
156  static f128 __vectorcall LineRay(SimdVector* point_on_line, SimdVector* point_on_ray,
157  SimdVectorArg line_point, SimdVectorArg line_dir_normalized,
158  SimdVectorArg ray_point,
159  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
160  static f128 __vectorcall
161  LineSegment(SimdVector* point_on_line, SimdVector* point_on_segment,
162  SimdVectorArg line_point, SimdVectorArg line_dir_normalized,
163  SimdVectorArg segment_point0, SimdVectorArg segment_point1) NLIB_NOEXCEPT;
164  static f128 __vectorcall RayRay(SimdVector* point_on_ray0, SimdVector* point_on_ray1,
165  SimdVectorArg ray0_point, SimdVectorArg ray0_dir_normalized,
166  SimdVectorArg ray1_point,
167  SimdVectorArg ray1_dir_normalized) NLIB_NOEXCEPT;
168  static f128 __vectorcall RaySegment(SimdVector* point_on_ray, SimdVector* point_on_segment,
169  SimdVectorArg ray_point0, SimdVectorArg ray_dir_normalized,
170  SimdVectorArg segment_point0,
171  SimdVectorArg segment_point1) NLIB_NOEXCEPT;
172 
173  private:
174  DistanceSq(); // forbidden
175 };
176 
177 // has only static member functions
179  public:
180  // OrientedBox, AxisAlignedBox, Sphere, Triangle, Plane, Line, Ray, Segment
181  enum PlaneResult {
182  PLANE_FRONT = 0,
183  PLANE_INTERSECT = 1,
184  PLANE_BACK = 2
185  };
186  static SimdVector __vectorcall PlaneLine(SimdPlaneArg plane, SimdVectorArg line_point,
187  SimdVectorArg line_dir_normalized) NLIB_NOEXCEPT;
188  static SimdVector __vectorcall PlaneRay(SimdPlaneArg plane, SimdVectorArg ray_point,
189  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
190  static SimdVector __vectorcall PlaneSegment(SimdPlaneArg plane, SimdVectorArg segment_point0,
191  SimdVectorArg segment_point1) NLIB_NOEXCEPT;
192 
193  // static bool __vectorcall SphereLine(SimdSphereArg sphere, SimdVectorArg line_point,
194  // SimdVectorArg line_dir_normalized) NLIB_NOEXCEPT;
195  static bool __vectorcall SphereRay(float* distance, SimdSphereArg sphere,
196  SimdVectorArg ray_point,
197  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
198  // static bool __vectorcall SphereSegment(SimdVector* point0, SimdVector* point1,
199  // SimdSphereArg sphere, SimdVectorArg segment_point0,
200  // SimdVectorArg segment_point1) NLIB_NOEXCEPT;
201 
202  static bool __vectorcall TriangleRay(float* distance, SimdVectorArg triangle_point0,
203  SimdVectorArg triangle_point1,
204  SimdVectorArg triangle_point2, SimdVectorArg ray_point,
205  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
206  static PlaneResult __vectorcall
207  TrianglePlane(SimdVectorArg triangle_point0, SimdVectorArg triangle_point1,
208  SimdVectorArg triangle_point2, SimdPlaneArg plane_normalized) NLIB_NOEXCEPT;
209  static PlaneResult __vectorcall
210  SpherePlane(SimdSphereArg sphere, SimdPlaneArg plane_normalized) NLIB_NOEXCEPT;
211  static bool __vectorcall
212  SphereSphere(SimdSphereArg sphere0, SimdSphereArg sphere1) NLIB_NOEXCEPT;
213  static f128x2 __vectorcall PlanePlane(SimdPlaneArg plane0, SimdPlaneArg plane1) NLIB_NOEXCEPT;
214  static bool __vectorcall SphereTriangle(SimdSphereArg sphere, SimdVectorArg triangle_point0,
215  SimdVectorArg triangle_point1,
216  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
217 
218  static PlaneResult __vectorcall
219  AxisAlignedBoxPlane(const AxisAlignedBox& aabb,
220  SimdPlaneArg plane_normalized) NLIB_NOEXCEPT;
221  static bool __vectorcall AxisAlignedBoxRay(const AxisAlignedBox& aabb, SimdVectorArg ray_point,
222  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
223  static bool __vectorcall
224  AxisAlignedBoxSphere(const AxisAlignedBox& aabb, SimdSphereArg sphere) NLIB_NOEXCEPT;
225  static bool __vectorcall
226  AxisAlignedBoxAxisAlignedBox(const AxisAlignedBox& aabb0,
227  const AxisAlignedBox& aabb1) NLIB_NOEXCEPT;
228  static bool __vectorcall AxisAlignedBoxTriangle(const AxisAlignedBox& aabb,
229  SimdVectorArg triangle_point0,
230  SimdVectorArg triangle_point1,
231  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
232 
233  static bool __vectorcall
234  OrientedBoxSphere(const OrientedBox& box, SimdSphereArg sphere) NLIB_NOEXCEPT;
235  static bool __vectorcall
236  OrientedBoxAxisAlignedBox(const OrientedBox& box, const AxisAlignedBox& aabb) NLIB_NOEXCEPT;
237  static bool __vectorcall
238  OrientedBoxOrientedBox(const OrientedBox& box0, const OrientedBox& box1) NLIB_NOEXCEPT;
239  static bool __vectorcall OrientedBoxTriangle(const OrientedBox& box,
240  SimdVectorArg triangle_point0,
241  SimdVectorArg triangle_point1,
242  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
243  static PlaneResult __vectorcall
244  OrientedBoxPlane(const OrientedBox& box, SimdPlaneArg plane_normalized) NLIB_NOEXCEPT;
245  static bool __vectorcall OrientedBoxRay(const OrientedBox& box, SimdVectorArg ray_point,
246  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
247 
248  static bool __vectorcall
249  FrustumSphere(const Frustum& frustum, SimdSphereArg sphere) NLIB_NOEXCEPT;
250  static bool __vectorcall
251  FrustumAxisAlignedBox(const Frustum& frustum, const AxisAlignedBox& aabb) NLIB_NOEXCEPT;
252  static bool __vectorcall
253  FrustumOrientedBox(const Frustum& frustum, const OrientedBox& obb) NLIB_NOEXCEPT;
254  static bool __vectorcall FrustumTriangle(const Frustum& frustum, SimdVectorArg triangle_point0,
255  SimdVectorArg triangle_point1,
256  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
257  static PlaneResult __vectorcall
258  FrustumPlane(const Frustum& frustum, SimdPlaneArg plane) NLIB_NOEXCEPT;
259  static bool __vectorcall FrustumRay(const Frustum& frustum, SimdVectorArg ray_point,
260  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT;
261 
262  // TODO(nishida_kenji): TriangleTriangle
263  private:
264  Intersection(); // forbidden
265 };
266 
267 // has only static member functions
269  public:
270  // OrientedBox, AxisAlignedBox, Sphere
271  // Point, Triangle
272 
273  // Sphere contains X
274  static bool __vectorcall SpherePoint(SimdSphereArg sphere, SimdVectorArg point) NLIB_NOEXCEPT;
275  static bool __vectorcall SphereTriangle(SimdSphereArg sphere, SimdVectorArg triangle_point0,
276  SimdVectorArg triangle_point1,
277  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
278  static bool __vectorcall
279  SphereOrientedBox(SimdSphereArg sphere, const OrientedBox& obb) NLIB_NOEXCEPT;
280  static bool __vectorcall
281  SphereAxisAlignedBox(SimdSphereArg sphere, const AxisAlignedBox& aabb) NLIB_NOEXCEPT;
282  static bool __vectorcall
283  SphereSphere(SimdSphereArg sphere, SimdSphereArg contained_sphere) NLIB_NOEXCEPT;
284  static bool __vectorcall
285  SphereFrustum(SimdSphereArg sphere, const Frustum& frustum) NLIB_NOEXCEPT;
286 
287  // AABB contains X
288  static bool __vectorcall
289  AxisAlignedBoxPoint(const AxisAlignedBox& aabb, SimdVectorArg point) NLIB_NOEXCEPT;
290  static bool __vectorcall AxisAlignedBoxTriangle(const AxisAlignedBox& aabb,
291  SimdVectorArg triangle_point0,
292  SimdVectorArg triangle_point1,
293  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
294  static bool __vectorcall
295  AxisAlignedBoxOrientedBox(const AxisAlignedBox& aabb, const OrientedBox& obb) NLIB_NOEXCEPT;
296  static bool __vectorcall
297  AxisAlignedBoxAxisAlignedBox(const AxisAlignedBox& aabb,
298  const AxisAlignedBox& contained_aabb) NLIB_NOEXCEPT;
299  static bool __vectorcall
300  AxisAlignedBoxSphere(const AxisAlignedBox& aabb, SimdSphereArg sphere) NLIB_NOEXCEPT;
301  static bool __vectorcall
302  AxisAlignedBoxFrustum(const AxisAlignedBox& aabb, const Frustum& frustum) NLIB_NOEXCEPT;
303 
304  // OBB contains X
305  static bool __vectorcall
306  OrientedBoxPoint(const OrientedBox& box, SimdVectorArg point) NLIB_NOEXCEPT;
307  static bool __vectorcall OrientedBoxTriangle(const OrientedBox& box,
308  SimdVectorArg triangle_point0,
309  SimdVectorArg triangle_point1,
310  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
311  static bool __vectorcall OrientedBoxOrientedBox(const OrientedBox& box,
312  const OrientedBox& box_contained) NLIB_NOEXCEPT;
313  static bool __vectorcall
314  OrientedBoxAxisAlignedBox(const OrientedBox& box, const AxisAlignedBox& aabb) NLIB_NOEXCEPT;
315  static bool __vectorcall
316  OrientedBoxSphere(const OrientedBox& box, SimdSphereArg sphere) NLIB_NOEXCEPT;
317  static bool __vectorcall
318  OrientedBoxFrustum(const OrientedBox& box, const Frustum& frustum) NLIB_NOEXCEPT;
319 
320  // Frustum contains X
321  static bool __vectorcall
322  FrustumPoint(const Frustum& frustum, SimdVectorArg point) NLIB_NOEXCEPT;
323  static bool __vectorcall FrustumTriangle(const Frustum& frustum, SimdVectorArg triangle_point0,
324  SimdVectorArg triangle_point1,
325  SimdVectorArg triangle_point2) NLIB_NOEXCEPT;
326  static bool __vectorcall
327  FrustumSphere(const Frustum& frustum, SimdSphereArg sphere) NLIB_NOEXCEPT;
328  static bool __vectorcall
329  FrustumAxisAlignedBox(const Frustum& frustum, const AxisAlignedBox& aabb) NLIB_NOEXCEPT;
330  static bool __vectorcall
331  FrustumOrientedBox(const Frustum& frustum, const OrientedBox& box) NLIB_NOEXCEPT;
332  static bool __vectorcall
333  FrustumFrustum(const Frustum& frustum, const Frustum& contained) NLIB_NOEXCEPT;
334 
335  private:
336  Containment(); // forbidden
337 };
338 
339 #ifndef NLIB_DOXYGEN
340 
341 #define NLIB_M(tp) inline tp __vectorcall
342 #define NLIB_B inline bool __vectorcall
343 
344 //
345 // Plane
346 //
347 #ifdef _MSC_VER
348 # pragma region Plane
349 #endif
350 // XMPlaneDot
351 NLIB_M(f128) Plane::Dot(SimdPlaneArg plane, SimdVectorArg vec) NLIB_NOEXCEPT {
352  return Vector4::Dot(plane, vec);
353 }
354 
355 // XMPlaneDotCoord
356 NLIB_M(f128) Plane::DotCoord(SimdPlaneArg plane, SimdVectorArg vec) NLIB_NOEXCEPT {
357  return Vector4::Dot(plane, F128::SetFloatToLane<3>(vec, 1.f));
358 }
359 
360 // XMPlaneDotNormal
361 NLIB_M(f128) Plane::DotNormal(SimdPlaneArg plane, SimdVectorArg vec) NLIB_NOEXCEPT {
362  return Vector3::Dot(plane, vec);
363 }
364 
365 // XMPlaneFromPointNormal
366 NLIB_M(SimdPlane) Plane::FromPointAndNormal(SimdVectorArg point,
367  SimdVectorArg normal) NLIB_NOEXCEPT {
368  f128 dot = Vector3::Dot(point, normal);
369  dot = F128::Negate(dot);
370  return F128::Splat<false, false, false, true>(normal, dot);
371 }
372 
373 // XMPlaneFromPoints
374 NLIB_M(SimdPlane) Plane::FromPoint(SimdVectorArg point0, SimdVectorArg point1,
375  SimdVectorArg point2) NLIB_NOEXCEPT {
376  SimdVector v10 = F128::Sub(point0, point1);
377  SimdVector v20 = F128::Sub(point0, point2);
378  SimdVector normal = Vector3::Normalize(Vector3::Cross(v10, v20));
379  return FromPointAndNormal(point0, normal);
380 }
381 
382 // XMPlaneNormalize
383 NLIB_M(SimdVector) Plane::Normalize(SimdPlaneArg plane) NLIB_NOEXCEPT {
384  // fail safe needed?
385  return F128::Mult(Vector3::RecpLength(plane), plane);
386 }
387 
388 // XMPlaneNormalizeEst
389 NLIB_M(SimdVector) Plane::NormalizeEst(SimdPlaneArg plane) NLIB_NOEXCEPT {
390  return F128::Mult(Vector3::RecpLengthEst(plane), plane);
391 }
392 
393 // XMPlaneTransform
394 NLIB_M(SimdPlane) Plane::Transform(SimdPlaneArg plane, SimdMatrixArg m) NLIB_NOEXCEPT {
395  return Vector4::Transform(plane, m);
396 }
397 #ifdef _MSC_VER
398 # pragma endregion Plane function implementation
399 #endif
400 
401 //
402 // Sphere
403 //
404 #ifdef _MSC_VER
405 # pragma region Sphere
406 #endif
407 // BoundingSphere::Transform
408 NLIB_M(SimdSphere) Sphere::Transform(SimdSphereArg sphere, SimdMatrixArg m) NLIB_NOEXCEPT {
409  f128 radius = F128::SetValue<3>(sphere, each_select32);
410  SimdVector center = Vector3::Transform(sphere, m);
411  f128 dot_x = Vector3::Dot(m.r[0], m.r[0]);
412  f128 dot_y = Vector3::Dot(m.r[1], m.r[1]);
413  f128 dot_z = Vector3::Dot(m.r[2], m.r[2]);
414  f128 scale = F128::Max(dot_x, dot_y);
415  scale = F128::Max(scale, dot_z);
416  radius = F128::Mult(radius, scale);
417 
418  return F128::Splat<false, false, false, true>(center, radius);
419 }
420 
421 // BoundingSphere::CreateFromPoints
422 NLIB_M(SimdSphere) Sphere::FromPoints(const Float3* points, size_t count) NLIB_NOEXCEPT {
423  // approximation algorithm from Graphics Gems
424  f128 min_x, min_y, min_z;
425  f128 max_x, max_y, max_z;
426  min_x = min_y = min_z = max_x = max_y = max_z = Vector3::LoadFloat3(points);
427 
428  size_t i;
429  for (i = 1; i < count; ++i) {
430  f128 cmp0, cmp1;
431  f128 pt = Vector3::LoadFloat3(points + i);
432 
433  f128 pt_x = F128::SetValue<0>(pt, each_select32);
434  cmp0 = F128::CmpLt(pt_x, F128::SetValue<0>(min_x, each_select32));
435  min_x = F128::Select(cmp0, pt, min_x);
436  cmp1 = F128::CmpGt(pt_x, F128::SetValue<0>(max_x, each_select32));
437  max_x = F128::Select(cmp1, pt, max_x);
438 
439  f128 pt_y = F128::SetValue<1>(pt, each_select32);
440  cmp0 = F128::CmpLt(pt_y, F128::SetValue<0>(min_y, each_select32));
441  min_y = F128::Select(cmp0, pt, min_y);
442  cmp1 = F128::CmpGt(pt_y, F128::SetValue<0>(max_y, each_select32));
443  max_y = F128::Select(cmp1, pt, max_y);
444 
445  f128 pt_z = F128::SetValue<2>(pt, each_select32);
446  cmp0 = F128::CmpLt(pt_z, F128::SetValue<0>(min_z, each_select32));
447  min_z = F128::Select(cmp0, pt, min_z);
448  cmp1 = F128::CmpGt(pt_z, F128::SetValue<0>(max_z, each_select32));
449  max_z = F128::Select(cmp1, pt, max_z);
450  }
451 
452  f128 center;
453  f128 radius;
454  f128 c1_2 = F128::SetValue(0.5f, each_float);
455  {
456  f128 distsq_x = Vector3::LengthSq(F128::Sub(max_x, min_x));
457  f128 distsq_y = Vector3::LengthSq(F128::Sub(max_y, min_y));
458  f128 distsq_z = Vector3::LengthSq(F128::Sub(max_z, min_z));
459  if (Vector4::CmpGt(distsq_x, distsq_y)) {
460  if (Vector4::CmpGt(distsq_x, distsq_z)) {
461  center = F128::Lerp(min_x, max_x, c1_2);
462  radius = F128::Mult(F128::Sqrt(distsq_x), c1_2);
463  } else {
464  center = F128::Lerp(min_z, max_z, c1_2);
465  radius = F128::Mult(F128::Sqrt(distsq_z), c1_2);
466  }
467  } else {
468  if (Vector4::CmpGt(distsq_y, distsq_z)) {
469  center = F128::Lerp(min_y, max_y, c1_2);
470  radius = F128::Mult(F128::Sqrt(distsq_y), c1_2);
471  } else {
472  center = F128::Lerp(min_z, max_z, c1_2);
473  radius = F128::Mult(F128::Sqrt(distsq_z), c1_2);
474  }
475  }
476  }
477 
478  f128 radsq = F128::Mult(radius, radius);
479  for (i = 0; i < count; ++i) {
480  f128 pt = Vector3::LoadFloat3(points + i);
481  f128 diff = F128::Sub(pt, center);
482  f128 distsq = Vector3::LengthSq(diff);
483  if (Vector3::CmpGt(distsq, radsq)) {
484  f128 dist = F128::Sqrt(distsq);
485  f128 t = F128::Div(radius, dist);
486  t = F128::MultSub(c1_2, t, c1_2);
487  radius = F128::Mult(c1_2, F128::Add(radius, dist));
488  radsq = F128::Mult(radius, radius);
489  center = F128::MultAdd(t, diff, center);
490  }
491  }
492 
493  return F128::Splat<false, false, false, true>(center, radius);
494 }
495 
496 // BoundingSphere::CreateMerged()
497 NLIB_M(SimdSphere) Sphere::Merge(SimdSphereArg sphere0, SimdSphereArg sphere1) NLIB_NOEXCEPT {
498  SimdVector diff = F128::Sub(sphere1, sphere0);
499  f128 dist_sq = Vector3::LengthSq(diff);
500  f128 radius_diff_sq = F128::SetValue<3>(F128::Mult(diff, diff), each_select32);
501  f128 is_contained = F128::CmpLe(dist_sq, radius_diff_sq);
502 
503  f128 dist = F128::Sqrt(dist_sq);
504  f128 r0 = F128::SetValue<3>(sphere0, each_select32);
505  f128 r1 = F128::SetValue<3>(sphere1, each_select32);
506  f128 new_radius = F128::Add(r0, r1);
507  new_radius = F128::Add(new_radius, dist);
508  new_radius = F128::Mult(0.5f, new_radius);
509 
510  f128 t = F128::Div(diff, dist);
511  f128 use_new_center = F128::CmpGt(dist, F128::SetEpsilon());
512 
513  SimdSphere ret;
514  ret = F128::MultAdd(t, F128::Sub(new_radius, r0), sphere0);
515  ret = F128::Select(use_new_center, ret, sphere0);
516  ret = F128::Splat<false, false, false, true>(ret, new_radius);
517 
518  f128 s0_contained = F128::And(is_contained, F128::CmpLt(r0, r1));
519  f128 s1_contained = F128::And(is_contained, F128::CmpGe(r0, r1));
520  ret = F128::Select(s0_contained, sphere1, ret);
521  ret = F128::Select(s1_contained, sphere0, ret);
522  return ret;
523 }
524 
525 NLIB_M(float) Sphere::GetRadius(SimdSphereArg sphere) NLIB_NOEXCEPT { // NOLINT
526  return F128::GetFloatFromLane<3>(sphere);
527 }
528 
529 NLIB_M(SimdSphere) Sphere::SetRadius(SimdSphereArg sphere, float radius) NLIB_NOEXCEPT {
530  return F128::SetFloatToLane<3>(sphere, radius);
531 }
532 
533 NLIB_M(SimdSphere) Sphere::GetCenter(SimdSphereArg sphere) NLIB_NOEXCEPT {
534  return sphere;
535 }
536 
537 NLIB_M(SimdSphere) Sphere::SetCenter(SimdSphereArg sphere, SimdVectorArg center) NLIB_NOEXCEPT {
538  return F128::Permute<4, 5, 6, 3>(sphere, center);
539 }
540 #ifdef _MSC_VER
541 # pragma endregion Sphere function implementation
542 #endif
543 
544 //
545 // AxisAlignedBox
546 //
547 #ifdef _MSC_VER
548 # pragma region AxisAlignedBox
549 #endif
550 inline AxisAlignedBox::AxisAlignedBox(const Float3& pmin, const Float3& pmax) {
551  Vector3::LoadFloat3(&pmin);
552  Vector3::LoadFloat3(&pmax);
553 }
554 
555 // BoundingBox::GetCorners
556 inline void AxisAlignedBox::GetCorners(Float3* corners) const NLIB_NOEXCEPT {
557  f128 p0 = point_min;
558  f128 p1 = F128::Splat<true, false, false, false>(point_min, point_max);
559  f128 p2 = F128::Splat<true, true, false, false>(point_min, point_max);
560  f128 p3 = F128::Splat<false, true, false, false>(point_min, point_max);
561  f128 p4 = F128::Splat<false, true, true, true>(point_min, point_max);
562  f128 p5 = F128::Splat<false, false, true, true>(point_min, point_max);
563  f128 p6 = F128::Splat<true, false, true, true>(point_min, point_max);
564  f128 p7 = point_max;
565 
566  Vector3::StoreFloat3(corners + 0, p0);
567  Vector3::StoreFloat3(corners + 1, p1);
568  Vector3::StoreFloat3(corners + 2, p2);
569  Vector3::StoreFloat3(corners + 3, p3);
570  Vector3::StoreFloat3(corners + 4, p4);
571  Vector3::StoreFloat3(corners + 5, p5);
572  Vector3::StoreFloat3(corners + 6, p6);
573  Vector3::StoreFloat3(corners + 7, p7);
574 }
575 
576 // BoundingBox::Transform
577 inline void __vectorcall
578 AxisAlignedBox::Transform(AxisAlignedBox* box, SimdMatrixArg m) NLIB_NOEXCEPT {
579  f128 p0 = point_min;
580  f128 p1 = F128::Splat<true, false, false, false>(point_min, point_max);
581  f128 p2 = F128::Splat<true, true, false, false>(point_min, point_max);
582  f128 p3 = F128::Splat<false, true, false, false>(point_min, point_max);
583  f128 p4 = F128::Splat<false, true, true, true>(point_min, point_max);
584  f128 p5 = F128::Splat<false, false, true, true>(point_min, point_max);
585  f128 p6 = F128::Splat<true, false, true, true>(point_min, point_max);
586  f128 p7 = point_max;
587 
588  p0 = Vector3::Transform(p0, m);
589  p1 = Vector3::Transform(p1, m);
590  p2 = Vector3::Transform(p2, m);
591  p3 = Vector3::Transform(p3, m);
592  p4 = Vector3::Transform(p4, m);
593  p5 = Vector3::Transform(p5, m);
594  p6 = Vector3::Transform(p6, m);
595  p7 = Vector3::Transform(p7, m);
596 
597  f128 pmin, pmax;
598  pmin = F128::Min(p0, p1);
599  pmax = F128::Max(p0, p1);
600  pmin = F128::Min(pmin, p2);
601  pmax = F128::Max(pmax, p2);
602  pmin = F128::Min(pmin, p3);
603  pmax = F128::Max(pmax, p3);
604  pmin = F128::Min(pmin, p4);
605  pmax = F128::Max(pmax, p4);
606  pmin = F128::Min(pmin, p5);
607  pmax = F128::Max(pmax, p5);
608  pmin = F128::Min(pmin, p6);
609  pmax = F128::Max(pmax, p6);
610  pmin = F128::Min(pmin, p7);
611  pmax = F128::Max(pmax, p7);
612  box->point_min = pmin;
613  box->point_max = pmax;
614 }
615 
616 // BoundingBox::CreateMerged
617 inline void AxisAlignedBox::Merge(AxisAlignedBox* box, const AxisAlignedBox& box0,
618  const AxisAlignedBox& box1) NLIB_NOEXCEPT {
619  box->point_min = F128::Min(box0.point_min, box1.point_min);
620  box->point_max = F128::Max(box0.point_max, box1.point_max);
621 }
622 
623 // BoundingBox::CreateFromSphere
624 inline void __vectorcall
625 AxisAlignedBox::FromSphere(AxisAlignedBox* box, SimdSphereArg sphere) NLIB_NOEXCEPT {
626  f128 r = F128::SetValue<3>(sphere, each_select32);
627  box->point_min = F128::Sub(sphere, r);
628  box->point_max = F128::Add(sphere, r);
629 }
630 
631 // BoundingBox::CreateFromPoints
632 inline void __vectorcall AxisAlignedBox::FromPoints(AxisAlignedBox* box, SimdVectorArg point0,
633  SimdVectorArg point1) NLIB_NOEXCEPT {
634  box->point_min = F128::Min(point0, point1);
635  box->point_max = F128::Max(point0, point1);
636 }
637 
638 // BoundingBox::CreateFromPoints
639 inline void AxisAlignedBox::FromPoints(AxisAlignedBox* box, const Float3* points,
640  size_t count) NLIB_NOEXCEPT {
641  f128 pmin, pmax;
642  pmin = pmax = Vector3::LoadFloat3(points);
643  for (size_t i = 1; i < count; ++i) {
644  f128 pt = Vector3::LoadFloat3(points + i);
645  pmin = F128::Min(pmin, pt);
646  pmax = F128::Max(pmax, pt);
647  }
648  box->point_min = pmin;
649  box->point_max = pmax;
650 }
651 #ifdef _MSC_VER
652 # pragma endregion AxisAlignedBox function implementation
653 #endif
654 
655 //
656 // OrientedBox
657 //
658 #ifdef _MSC_VER
659 # pragma region OrientedBox
660 #endif
661 
662 // BoundingOrientedBox::Transform
663 inline void __vectorcall OrientedBox::Transform(OrientedBox* box, SimdMatrixArg m) NLIB_NOEXCEPT {
664  box->center = Vector3::Transform(this->center, m);
665  f128 inv_dx = Vector3::RecpLength(m.r[0]);
666  f128 inv_dy = Vector3::RecpLength(m.r[1]);
667  f128 inv_dz = Vector3::RecpLength(m.r[2]);
668 
669  {
670  SimdMatrix rot_matrix;
671  rot_matrix.r[0] = F128::Mult(m.r[0], inv_dx);
672  rot_matrix.r[1] = F128::Mult(m.r[1], inv_dy);
673  rot_matrix.r[2] = F128::Mult(m.r[2], inv_dz);
674  rot_matrix.r[3] = F128::Set0001();
675  SimdQuaternion rot = Quaternion::FromRotationMatrix(rot_matrix);
676  box->rotation = Quaternion::Mult(this->rotation, rot);
677  }
678 
679  f128 scale;
680  scale = F128::Permute<0, 4, -1, -1>(inv_dx, inv_dy);
681  scale = F128::Permute<0, 1, 4, -1>(scale, inv_dz);
682  box->extent = F128::Div(this->extent, scale);
683 }
684 
685 // BoundingOrientedBox::GetCorners
686 inline void OrientedBox::GetCorners(Float3* corners) const NLIB_NOEXCEPT {
687  f128 point_max = F128::Add(center, extent);
688  f128 point_min = F128::Sub(center, extent);
689  point_max = F128::SetZeroToLane<3>(point_max);
690  point_min = F128::SetZeroToLane<3>(point_min);
691  f128 pt;
692 
693  pt = point_min;
694  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
695  Vector3::StoreFloat3(corners + 0, pt);
696 
697  pt = F128::Splat<true, false, false, false>(point_min, point_max);
698  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
699  Vector3::StoreFloat3(corners + 1, pt);
700 
701  pt = F128::Splat<true, true, false, false>(point_min, point_max);
702  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
703  Vector3::StoreFloat3(corners + 2, pt);
704 
705  pt = F128::Splat<false, true, false, false>(point_min, point_max);
706  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
707  Vector3::StoreFloat3(corners + 3, pt);
708 
709  pt = F128::Splat<false, true, true, true>(point_min, point_max);
710  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
711  Vector3::StoreFloat3(corners + 4, pt);
712 
713  pt = F128::Splat<false, false, true, true>(point_min, point_max);
714  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
715  Vector3::StoreFloat3(corners + 5, pt);
716 
717  pt = F128::Splat<true, false, true, true>(point_min, point_max);
718  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
719  Vector3::StoreFloat3(corners + 6, pt);
720 
721  pt = point_max;
722  pt = F128::Add(center, Vector3::Rotate(pt, rotation));
723  Vector3::StoreFloat3(corners + 7, pt);
724 }
725 
726 // BoundingOrientedBox::CreateFromBoundingBox
727 inline void OrientedBox::FromAxisAlignedBox(OrientedBox* box,
728  const AxisAlignedBox& aabb) NLIB_NOEXCEPT {
729  f128 c1_2 = F128::SetValue(0.5f, each_float);
730  f128 half = F128::Mult(c1_2, aabb.point_max);
731  box->rotation = F128::Set0001();
732  SimdVector box_min = aabb.point_min;
733  box->center = F128::MultAdd(c1_2, box_min, half);
734  box->extent = F128::MultSub(c1_2, box_min, half);
735 }
736 
737 #ifdef _MSC_VER
738 # pragma endregion OrientedBox function implementation
739 #endif
740 
741 //
742 // Capsule
743 //
744 #ifdef _MSC_VER
745 # pragma region Capsule
746 #endif
747 inline Capsule::Capsule(const Float3& pt0, const Float3& pt1, float radius) NLIB_NOEXCEPT {
748  f128 sphere_ = Vector3::LoadFloat3(&pt0);
749  point = Vector3::LoadFloat3(&pt1);
750  sphere = F128::SetFloatToLane<3>(sphere_, radius);
751 }
752 
753 inline void __vectorcall Capsule::Transform(Capsule* capsule, SimdMatrixArg m) NLIB_NOEXCEPT {
754  capsule->sphere = Sphere::Transform(sphere, m);
755  capsule->point = Vector3::Transform(point, m);
756 }
757 #ifdef _MSC_VER
758 # pragma endregion Capsule function implementation
759 #endif
760 
761 //
762 // Frustum
763 //
764 #ifdef _MSC_VER
765 # pragma region Frustum
766 #endif
767 
768 inline Frustum::Frustum(const Frustum& rhs) NLIB_NOEXCEPT : center_(rhs.center_),
769  rotation_(rhs.rotation_),
770  near_plane_(rhs.near_plane_),
771  far_plane_(rhs.far_plane_),
772  top_plane_(rhs.top_plane_),
773  bottom_plane_(rhs.bottom_plane_),
774  left_plane_(rhs.left_plane_),
775  right_plane_(rhs.right_plane_) {
776 }
777 
778 inline Frustum& Frustum::operator=(const Frustum& rhs) NLIB_NOEXCEPT {
779  center_ = rhs.center_;
780  rotation_ = rhs.rotation_;
781  near_plane_ = rhs.near_plane_;
782  far_plane_ = rhs.far_plane_;
783  top_plane_ = rhs.top_plane_;
784  bottom_plane_ = rhs.bottom_plane_;
785  left_plane_ = rhs.left_plane_;
786  right_plane_ = rhs.right_plane_;
787  return *this;
788 }
789 
790 inline Frustum::Frustum(SimdVectorArg center, SimdQuaternionArg rotation, float top, float bottom,
791  float left, float right, float n, float f) NLIB_NOEXCEPT {
792  // Right-handed
793  NLIB_ASSERT(n > 0.f);
794  center_ = center;
795  rotation_ = rotation;
796  near_plane_ = Plane::Normalize(F128::SetValue(0.f, 0.f, 1.f, n));
797  far_plane_ = Plane::Normalize(F128::SetValue(0.f, 0.f, -1.f, -f));
798  top_plane_ = Plane::Normalize(F128::SetValue(0.f, 1.f, top / n, 0.f));
799  bottom_plane_ = Plane::Normalize(F128::SetValue(0.f, -1.f, -bottom / n, 0.f));
800  left_plane_ = Plane::Normalize(F128::SetValue(-1.f, 0.f, -left / n, 0.f));
801  right_plane_ = Plane::Normalize(F128::SetValue(1.f, 0.f, right / n, 0.f));
802 }
803 
804 inline void __vectorcall Frustum::Set(SimdVectorArg center, SimdQuaternionArg rotation, float top,
805  float bottom, float left, float right, float n,
806  float f) NLIB_NOEXCEPT {
807  // Right-handed
808  NLIB_ASSERT(n > 0.f);
809  center_ = center;
810  rotation_ = rotation;
811  near_plane_ = Plane::Normalize(F128::SetValue(0.f, 0.f, 1.f, n));
812  far_plane_ = Plane::Normalize(F128::SetValue(0.f, 0.f, -1.f, -f));
813  top_plane_ = Plane::Normalize(F128::SetValue(0.f, 1.f, top / n, 0.f));
814  bottom_plane_ = Plane::Normalize(F128::SetValue(0.f, -1.f, -bottom / n, 0.f));
815  left_plane_ = Plane::Normalize(F128::SetValue(-1.f, 0.f, -left / n, 0.f));
816  right_plane_ = Plane::Normalize(F128::SetValue(1.f, 0.f, right / n, 0.f));
817 }
818 
819 inline void __vectorcall Frustum::Transform(Frustum* frustum, float scale,
820  SimdQuaternionArg rotation,
821  SimdVectorArg translation) const NLIB_NOEXCEPT {
822  f128 s = F128::SetValue(1.f, 1.f, 1.f, scale);
823  frustum->rotation_ = Quaternion::Mult(rotation_, rotation);
824  SimdVector center = Vector3::Rotate(F128::Mult(scale, center_), rotation);
825  frustum->center_ = F128::Add(center, translation);
826  frustum->top_plane_ = top_plane_;
827  frustum->bottom_plane_ = bottom_plane_;
828  frustum->left_plane_ = left_plane_;
829  frustum->right_plane_ = right_plane_;
830  frustum->near_plane_ = F128::Mult(s, near_plane_);
831  frustum->far_plane_ = F128::Mult(s, far_plane_);
832 }
833 
834 inline void __vectorcall Frustum::Transform(Frustum* frustum, SimdMatrixArg m) const NLIB_NOEXCEPT {
835  f128 r0 = m.r[0];
836  f128 r1 = m.r[1];
837  f128 r2 = m.r[2];
838  {
839  SimdQuaternion rotation = rotation_;
840  SimdVector center = center_;
841  SimdMatrix M;
842  M.r[0] = Vector3::Normalize(r0);
843  M.r[1] = Vector3::Normalize(r1);
844  M.r[2] = Vector3::Normalize(r2);
845  M.r[3] = F128::Set0001();
846  SimdQuaternion rot = Quaternion::FromRotationMatrix(M);
847  frustum->rotation_ = Quaternion::Mult(rotation, rot);
848  frustum->center_ = Vector3::Transform(center, m);
849  }
850 
851  float scale;
852  {
853  f128 x_sq = Vector3::LengthSq(r0);
854  f128 y_sq = Vector3::LengthSq(r1);
855  f128 z_sq = Vector3::LengthSq(r2);
856  f128 sq = F128::Max(z_sq, F128::Max(x_sq, y_sq));
857  scale = F128::GetFloatFromLane<0>(F128::Sqrt(sq));
858  }
859 
860  f128 s = F128::SetValue(1.f, 1.f, 1.f, scale);
861  frustum->top_plane_ = top_plane_;
862  frustum->bottom_plane_ = bottom_plane_;
863  frustum->left_plane_ = left_plane_;
864  frustum->right_plane_ = right_plane_;
865  frustum->near_plane_ = F128::Mult(s, near_plane_);
866  frustum->far_plane_ = F128::Mult(s, far_plane_);
867 }
868 
869 inline void Frustum::GetCorners(Float3* corners) const NLIB_NOEXCEPT {
870  float n = F128::GetFloatFromLane<3>(near_plane_) / F128::GetFloatFromLane<2>(near_plane_);
871  float f = F128::GetFloatFromLane<3>(far_plane_) / F128::GetFloatFromLane<2>(far_plane_);
872  float top = F128::GetFloatFromLane<2>(top_plane_) / F128::GetFloatFromLane<1>(top_plane_);
873  top = top * n;
874  float bottom =
875  F128::GetFloatFromLane<2>(bottom_plane_) / F128::GetFloatFromLane<1>(bottom_plane_);
876  bottom = bottom * n;
877  float left = F128::GetFloatFromLane<2>(left_plane_) / F128::GetFloatFromLane<0>(left_plane_);
878  left = left * n;
879  float right =
880  F128::GetFloatFromLane<2>(right_plane_) / F128::GetFloatFromLane<0>(right_plane_);
881  right = right * n;
882 
883  SimdVector pt;
884  SimdQuaternion rot = rotation_;
885  SimdVector center = center_;
886 
887  // left top
888  pt = F128::SetValue(left, top, -n, 0.f);
889  pt = F128::Add(Vector3::Rotate(pt, rot), center);
890  Vector3::StoreFloat3(corners + 0, pt);
891 
892  // right top
893  pt = F128::SetValue(right, top, -n, 0.f);
894  pt = F128::Add(Vector3::Rotate(pt, rot), center);
895  Vector3::StoreFloat3(corners + 1, pt);
896 
897  // right bottom
898  pt = F128::SetValue(right, bottom, -n, 0.f);
899  pt = F128::Add(Vector3::Rotate(pt, rot), center);
900  Vector3::StoreFloat3(corners + 2, pt);
901 
902  // left bottom
903  pt = F128::SetValue(left, bottom, -n, 0.f);
904  pt = F128::Add(Vector3::Rotate(pt, rot), center);
905  Vector3::StoreFloat3(corners + 3, pt);
906 
907  float f_n = f / n;
908 
909  // left top far
910  pt = F128::SetValue(left * f_n, top * f_n, -f, 0.f);
911  pt = F128::Add(Vector3::Rotate(pt, rot), center);
912  Vector3::StoreFloat3(corners + 4, pt);
913 
914  // right top far
915  pt = F128::SetValue(right * f_n, top * f_n, -f, 0.f);
916  pt = F128::Add(Vector3::Rotate(pt, rot), center);
917  Vector3::StoreFloat3(corners + 5, pt);
918 
919  // right bottom far
920  pt = F128::SetValue(right * f_n, bottom * f_n, -f, 0.f);
921  pt = F128::Add(Vector3::Rotate(pt, rot), center);
922  Vector3::StoreFloat3(corners + 6, pt);
923 
924  // left bottom far
925  pt = F128::SetValue(left * f_n, bottom * f_n, -f, 0.f);
926  pt = F128::Add(Vector3::Rotate(pt, rot), center);
927  Vector3::StoreFloat3(corners + 7, pt);
928 }
929 
930 #ifdef _MSC_VER
931 # pragma endregion Frustum function implementation
932 #endif
933 
934 //
935 // DistanceSq
936 //
937 #ifdef _MSC_VER
938 # pragma region DistanceSq
939 #endif
940 NLIB_M(f128) DistanceSq::PointLine(SimdVectorArg point, SimdVectorArg line_point,
941  SimdVectorArg line_dir_normalized) NLIB_NOEXCEPT {
942  SimdVector l0_pt = F128::Sub(point, line_point);
943  f128 dot = Vector3::Dot(l0_pt, line_dir_normalized);
944  SimdVector dist_vec = F128::Mult(line_dir_normalized, dot);
945  dist_vec = F128::Sub(l0_pt, dist_vec);
946  return Vector3::LengthSq(dist_vec);
947 }
948 
949 NLIB_M(f128) DistanceSq::PointRay(SimdVectorArg point, SimdVectorArg ray_point,
950  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
951  SimdVector l0_pt = F128::Sub(point, ray_point);
952  f128 dot = Vector3::Dot(l0_pt, ray_dir_normalized);
953  SimdVector dist_vec = F128::Mult(ray_dir_normalized, dot);
954  dist_vec = F128::Sub(l0_pt, dist_vec);
955 
956  f128 distsq0 = Vector3::LengthSq(dist_vec);
957  f128 distsq1 = Vector3::LengthSq(l0_pt);
958  f128 cmp = F128::CmpLt(dot, F128::SetZero());
959  return F128::Select(cmp, distsq1, distsq0);
960 }
961 
962 NLIB_M(f128) DistanceSq::PointSegment(SimdVectorArg point, SimdVectorArg segment_point0,
963  SimdVectorArg segment_point1) NLIB_NOEXCEPT {
964  SimdVector l0_pt = F128::Sub(point, segment_point0);
965  SimdVector l1_pt = F128::Sub(point, segment_point1);
966  SimdVector seg_dir_normalized = F128::Sub(segment_point1, segment_point0);
967  f128 seg_recplen = Vector3::RecpLength(seg_dir_normalized);
968  seg_dir_normalized = F128::Mult(seg_dir_normalized, seg_recplen);
969 
970  f128 dot = Vector3::Dot(l0_pt, seg_dir_normalized);
971  SimdVector dist_vec = F128::Mult(seg_dir_normalized, dot);
972  dist_vec = F128::Sub(l0_pt, dist_vec);
973 
974  f128 distsq0 = Vector3::LengthSq(dist_vec);
975  f128 distsq1 = Vector3::LengthSq(l0_pt);
976  f128 distsq2 = Vector3::LengthSq(l1_pt);
977  f128 cmp = F128::CmpLt(dot, F128::SetZero());
978  f128 ret = F128::Select(cmp, distsq1, distsq0);
979  cmp = F128::CmpLt(F128::Mult(dot, seg_recplen), F128::SetOne());
980  ret = F128::Select(cmp, ret, distsq2);
981  return ret;
982 }
983 
984 // pointer arguments can be NULL
985 NLIB_M(f128) DistanceSq::PointPlane(SimdVector* point_on_plane, SimdVectorArg point,
986  SimdPlaneArg plane_normalized) NLIB_NOEXCEPT {
987  SimdVector pt = F128::SetZeroToLane<3>(point);
988  f128 dot = Vector4::Dot(pt, plane_normalized);
989  f128 ret = F128::Mult(dot, dot);
990  if (point_on_plane) {
991  *point_on_plane = F128::Sub(point, F128::Mult(dot, plane_normalized));
992  }
993  return ret;
994 }
995 
996 NLIB_M(f128) DistanceSq::SpherePlane(SimdSphereArg sphere,
997  SimdPlaneArg plane_normalized) NLIB_NOEXCEPT {
998  SimdVector c = F128::SetZeroToLane<3>(sphere);
999  f128 r = F128::SetValue<3>(sphere, each_select32);
1000  f128 dist = F128::Abs(Vector4::Dot(c, plane_normalized));
1001  f128 cmp = F128::CmpGt(dist, r);
1002  f128 ret = F128::Sub(dist, r);
1003  ret = F128::Mult(ret, ret);
1004  return F128::And(cmp, ret);
1005 }
1006 
1007 // pointer arguments can be NULL
1008 NLIB_M(f128) DistanceSq::PointAxisAlignedBox(SimdVector* point_on_box, SimdVectorArg point,
1009  const AxisAlignedBox& aabb) NLIB_NOEXCEPT {
1010  f128 vv = F128::Clamp(point, aabb.point_min, aabb.point_max);
1011  f128 distsq = F128::Sub(vv, point);
1012  distsq = Vector3::LengthSq(distsq);
1013  if (point_on_box) *point_on_box = vv;
1014  return distsq;
1015 }
1016 
1017 // pointer arguments can be NULL
1018 NLIB_M(f128) DistanceSq::LineLine(SimdVector* point_on_line0, SimdVector* point_on_line1,
1019  SimdVectorArg line0_point, SimdVectorArg line0_dir_normalized,
1020  SimdVectorArg line1_point,
1021  SimdVectorArg line1_dir_normalized) NLIB_NOEXCEPT {
1022  SimdVector u;
1023  u = F128::Sub(line0_point, line1_point);
1024  f128 b = Vector3::Dot(line0_dir_normalized, line1_dir_normalized);
1025  f128 d = Vector3::Dot(line0_dir_normalized, u);
1026  f128 e = Vector3::Dot(line1_dir_normalized, u);
1027  f128 det = F128::MultSub(b, b, F128::SetOne());
1028 
1029  f128 recp_det = F128::Recp(det);
1030  f128 s = F128::MultAdd(b, e, F128::Negate(d));
1031  f128 t = F128::MultSub(b, d, e);
1032  s = F128::Mult(s, recp_det);
1033  t = F128::Mult(t, recp_det);
1034 
1035  SimdVector tmp = F128::Mult(s, line0_dir_normalized);
1036  tmp = F128::MultSub(t, line1_dir_normalized, tmp);
1037  SimdVector ret = F128::Add(u, tmp);
1038  ret = Vector3::LengthSq(ret);
1039 
1040  f128 parallel = F128::CmpLt(det, F128::SetEpsilon());
1041  tmp = Vector3::LengthSq(F128::MultSub(e, line1_dir_normalized, u));
1042  ret = F128::Select(parallel, tmp, ret);
1043 
1044  if (point_on_line0) {
1045  s = F128::AndNot(parallel, s);
1046  *point_on_line0 = F128::MultAdd(s, line0_dir_normalized, line0_point);
1047  }
1048  if (point_on_line1) {
1049  t = F128::Select(parallel, e, t);
1050  *point_on_line1 = F128::MultAdd(t, line1_dir_normalized, line1_point);
1051  }
1052 
1053  return ret;
1054 }
1055 
1056 // pointer arguments can be NULL
1057 NLIB_M(f128) DistanceSq::SegmentSegment(SimdVector* point_on_segment0,
1058  SimdVector* point_on_segment1,
1059  SimdVectorArg segment0_point0,
1060  SimdVectorArg segment0_point1,
1061  SimdVectorArg segment1_point0,
1062  SimdVectorArg segment1_point1) NLIB_NOEXCEPT {
1063  SimdVector u = F128::Sub(segment0_point1, segment0_point0);
1064  SimdVector v = F128::Sub(segment1_point1, segment1_point0);
1065  SimdVector w = F128::Sub(segment0_point0, segment1_point0);
1066  f128 a = Vector3::LengthSq(u);
1067  f128 b = Vector3::Dot(u, v);
1068  f128 c = Vector3::LengthSq(v);
1069  f128 d = Vector3::Dot(u, w);
1070  f128 e = Vector3::Dot(v, w);
1071  f128 D = F128::MultSub(b, b, F128::Mult(a, c)); // D >= 0, D == 0 if parallel
1072 
1073  f128 epsilon = F128::SetEpsilon();
1074  f128 sD;
1075  f128 tD;
1076  f128 sN = F128::MultSub(c, d, F128::Mult(b, e)); // b * e - c * d
1077  f128 tN = F128::MultSub(b, d, F128::Mult(a, e)); // a * e - b * d
1078 
1079  {
1080  f128 parallel = F128::CmpLt(D, epsilon);
1081  f128 sN_under = F128::CmpLtZero(sN); // sc < 0
1082  f128 sN_out = F128::Or(sN_under, F128::CmpGt(sN, D));
1083  f128 cmp = F128::Or(parallel, sN_out);
1084  f128 under_or_parallel = F128::Or(parallel, sN_under);
1085 
1086  // sN:
1087  // 0.f if parallel
1088  // 0.f if sN < 0.f
1089  // D if sN > D
1090  // sN otherwise
1091  sN = F128::Select(cmp, F128::AndNot(under_or_parallel, D), sN);
1092 
1093  // tN:
1094  // e if parallel
1095  // e if sN < 0.f
1096  // e + b if sN > D
1097  // tN otherwise
1098  tN = F128::Select(cmp, F128::Select(under_or_parallel, e, F128::Add(e, b)), tN);
1099 
1100  // tD:
1101  // c if parallel
1102  // c if sN < 0.f
1103  // c if sN > D
1104  // D otherwise
1105  tD = F128::Select(cmp, c, D);
1106 
1107  // 1.f if parallel
1108  // D otherwise
1109  sD = F128::Select(parallel, F128::SetOne(), D);
1110  }
1111 
1112  {
1113  f128 tN_under = F128::CmpLtZero(tN);
1114  f128 tN_out = F128::Or(tN_under, F128::CmpGt(tN, tD));
1115 
1116  // tN:
1117  // 0.f if tN < 0.f
1118  // tD if tN > tD
1119  // tN otherwise
1120  tN = F128::Clamp(tN, F128::SetZero(), tD);
1121 
1122  f128 val = F128::Select(tN_under, F128::Negate(d), F128::Sub(b, d));
1123 
1124  // sD:
1125  // a if tN < 0.f && !(0.f <= -d <= a)
1126  // a if tN > tD && !(0.f <= b - d <= a)
1127  // sD otherwise
1128  f128 val_in = F128::And(F128::CmpGeZero(val), F128::CmpLe(val, a));
1129  sD = F128::Select(F128::And(tN_out, val_in), a, sD);
1130 
1131  // sN:
1132  // 0.f if tN < 0.f && -d < 0.f
1133  // 0.f if tN > tD && b - d < 0.f
1134  // sD if tN < 0.f && -d > a
1135  // sD if tN > tD && b - d > a
1136  // sN otherwise
1137  f128 sntmp = F128::AndNot(F128::CmpLtZero(val), val);
1138  sntmp = F128::Select(F128::CmpGt(val, a), sD, sntmp);
1139  sN = F128::Select(tN_out, sntmp, sN);
1140  }
1141 
1142  f128 sc = F128::AndNot(F128::InBound(sN, epsilon), F128::Div(sN, sD));
1143  f128 tc = F128::AndNot(F128::InBound(tN, epsilon), F128::Div(tN, tD));
1144 
1145  // w + sc * u - tc * v
1146  f128 pt_s0 = F128::MultAdd(sc, u, segment0_point0);
1147  f128 pt_s1 = F128::MultAdd(tc, v, segment1_point0);
1148  f128 ret = F128::Sub(pt_s0, pt_s1);
1149  ret = Vector3::LengthSq(ret);
1150 
1151  if (point_on_segment0) *point_on_segment0 = pt_s0;
1152  if (point_on_segment1) *point_on_segment1 = pt_s1;
1153  return ret;
1154 }
1155 
1156 // pointer arguments can be NULL
1157 NLIB_M(f128) DistanceSq::LineRay(SimdVector* point_on_line, SimdVector* point_on_ray,
1158  SimdVectorArg line_point, SimdVectorArg line_dir_normalized,
1159  SimdVectorArg ray_point,
1160  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
1161  SimdVector u = F128::Sub(line_point, ray_point);
1162  f128 b = Vector3::Dot(line_dir_normalized, ray_dir_normalized);
1163  f128 d = Vector3::Dot(line_dir_normalized, u);
1164  f128 e = Vector3::Dot(ray_dir_normalized, u);
1165  f128 one = F128::SetOne();
1166  f128 det = F128::MultSub(b, b, one);
1167  f128 parallel = F128::CmpLt(det, F128::SetEpsilon()); // true if parallel
1168 
1169  // tNum:
1170  // e if parallel
1171  // e - b * d otherwise
1172  f128 tNum = F128::MultSub(b, d, e);
1173  tNum = F128::Select(parallel, e, tNum);
1174  f128 tNum_negative = F128::CmpLtZero(tNum);
1175 
1176  // sNum:
1177  // 0.f if parallel
1178  // b * e - d otherwise
1179  f128 minus_d = F128::Negate(d);
1180  f128 sNum = F128::MultAdd(b, e, minus_d);
1181  sNum = F128::AndNot(parallel, sNum);
1182 
1183  // tDenom:
1184  // 1.f if parallel
1185  // det otherwise
1186  f128 tDenom = F128::Select(parallel, one, det);
1187 
1188  // sDenom:
1189  // 1.f if tNum < 0.f or parallel
1190  // det otherwise
1191  f128 sDenom = F128::Select(F128::Or(tNum_negative, parallel), one, det);
1192 
1193  // sNum:
1194  // -d if tNum < 0.f
1195  // sNum otherwise
1196  sNum = F128::Select(tNum_negative, minus_d, sNum);
1197 
1198  // tNum:
1199  // 0.f if tNum < 0.f
1200  // tNum otherwise
1201  tNum = F128::AndNot(tNum_negative, tNum);
1202 
1203  f128 s = F128::Div(sNum, sDenom);
1204  f128 t = F128::Div(tNum, tDenom);
1205 
1206  f128 pt_l = F128::MultAdd(s, line_dir_normalized, line_point);
1207  f128 pt_r = F128::MultAdd(t, ray_dir_normalized, ray_point);
1208  SimdVector ret = F128::Sub(pt_l, pt_r);
1209  ret = Vector3::LengthSq(ret);
1210 
1211  if (point_on_line) *point_on_line = pt_l;
1212  if (point_on_ray) *point_on_ray = pt_r;
1213  return ret;
1214 }
1215 
1216 // pointer arguments can be NULL
1217 NLIB_M(f128) DistanceSq::LineSegment(SimdVector* point_on_line, SimdVector* point_on_segment,
1218  SimdVectorArg line_point, SimdVectorArg line_dir_normalized,
1219  SimdVectorArg segment_point0,
1220  SimdVectorArg segment_point1) NLIB_NOEXCEPT {
1221  SimdVector seg_dir = F128::Sub(segment_point1, segment_point0);
1222  SimdVector u = F128::Sub(line_point, segment_point0);
1223  f128 b = Vector3::Dot(line_dir_normalized, seg_dir);
1224  f128 c = Vector3::LengthSq(seg_dir);
1225  f128 d = Vector3::Dot(line_dir_normalized, u);
1226  f128 e = Vector3::Dot(seg_dir, u);
1227  f128 det = F128::MultSub(b, b, c); // 0 if parallel
1228  f128 parallel = F128::CmpLt(det, F128::SetEpsilon());
1229 
1230  // tNum:
1231  // e if parallel
1232  // e - b * d otherwise
1233  f128 tNum = F128::Select(parallel, e, F128::MultSub(b, d, e));
1234 
1235  // tDenom:
1236  // c if parallel
1237  // det otherwise
1238  f128 tDenom = F128::Select(parallel, c, det);
1239 
1240  // sNum:
1241  // 0.f if parallel
1242  // b * e - c * d otherwise
1243  f128 tmp = F128::Mult(b, e);
1244  tmp = F128::MultSub(c, d, tmp);
1245  f128 sNum = F128::AndNot(parallel, tmp);
1246 
1247  // sNum:
1248  // -d if tNum < 0.f
1249  // b - d if tNum > tDenom
1250  // sNum otherwise
1251  f128 tNum_under = F128::CmpLtZero(tNum);
1252  f128 tNum_out = F128::Or(tNum_under, F128::CmpGt(tNum, tDenom));
1253  tmp = F128::Select(tNum_under, F128::Negate(d), F128::Sub(b, d));
1254  sNum = F128::Select(tNum_out, tmp, sNum);
1255 
1256  // sDenom:
1257  // 1.f if tNum < 0.f or tNum > tDenom or parallel
1258  // det otherwise
1259  f128 sDenom = F128::Select(F128::Or(tNum_out, parallel), F128::SetOne(), det);
1260 
1261  // tNum:
1262  // 0.f if tNum < 0.f
1263  // tDenom if tNum > tDenom
1264  tNum = F128::Clamp(tNum, F128::SetZero(), tDenom);
1265 
1266  f128 s = F128::Div(sNum, sDenom);
1267  f128 t = F128::Div(tNum, tDenom);
1268 
1269  f128 pt_l = F128::MultAdd(s, line_dir_normalized, line_point);
1270  f128 pt_s = F128::MultAdd(t, seg_dir, segment_point0);
1271  SimdVector ret = F128::Sub(pt_l, pt_s);
1272  ret = Vector3::LengthSq(ret);
1273 
1274  if (point_on_line) *point_on_line = pt_l;
1275  if (point_on_segment) *point_on_segment = pt_s;
1276  return ret;
1277 }
1278 
1279 // pointer arguments can be NULL
1280 NLIB_M(f128) DistanceSq::RayRay(SimdVector* point_on_ray0, SimdVector* point_on_ray1,
1281  SimdVectorArg ray0_point, SimdVectorArg ray0_dir_normalized,
1282  SimdVectorArg ray1_point,
1283  SimdVectorArg ray1_dir_normalized) NLIB_NOEXCEPT {
1284  SimdVector u = F128::Sub(ray0_point, ray1_point);
1285  f128 b = Vector3::Dot(ray0_dir_normalized, ray1_dir_normalized);
1286  f128 d = Vector3::Dot(ray0_dir_normalized, u);
1287  f128 e = Vector3::Dot(ray1_dir_normalized, u);
1288  f128 one = F128::SetOne();
1289  f128 det = F128::MultSub(b, b, one);
1290  f128 parallel = F128::CmpLt(det, F128::SetEpsilon());
1291 
1292  // sNum:
1293  // 0.f if parallel
1294  // b * e - d otherwise
1295  f128 minus_d = F128::Negate(d);
1296  f128 sNum = F128::AndNot(parallel, F128::MultAdd(b, e, minus_d));
1297  f128 sNum_negative = F128::CmpLtZero(sNum);
1298  f128 parallel_or_sNum_negative = F128::Or(parallel, sNum_negative);
1299 
1300  // tNum:
1301  // e if parallel or sNum < 0.f
1302  // e - b * d otherwise
1303  f128 tNum = F128::Select(parallel_or_sNum_negative, e, F128::MultSub(b, d, e));
1304 
1305  // tDenom:
1306  // 1.f if parallel or sNum < 0.f
1307  // det otherwise
1308  f128 tDenom = F128::Select(parallel_or_sNum_negative, one, det);
1309 
1310  // sNum:
1311  // 0.f if sNum < 0.f
1312  // sNum otherwise
1313  sNum = F128::AndNot(sNum_negative, sNum);
1314 
1315  // sNum:
1316  // 0.f if tNum < 0.f and d > 0
1317  // -d if tNum < 0.f and d <= 0
1318  // sNum otherwise
1319  f128 tNum_negative = F128::CmpLtZero(tNum);
1320  f128 d_negative = F128::CmpLeZero(d);
1321  f128 tmp = F128::And(d_negative, minus_d);
1322  sNum = F128::Select(tNum_negative, tmp, sNum);
1323 
1324  // sDenom:
1325  // 1.f if (tNum < 0.f && d <= 0) || parallel
1326  // det otherwise
1327  tmp = F128::Or(parallel, F128::And(tNum_negative, d_negative));
1328  f128 sDenom = F128::Select(tmp, one, det);
1329 
1330  // tNum:
1331  // 0.f if tNum < 0.f
1332  // tNum otherwise
1333  tNum = F128::AndNot(tNum_negative, tNum);
1334 
1335  f128 s = F128::Div(sNum, sDenom);
1336  f128 t = F128::Div(tNum, tDenom);
1337 
1338  f128 pt_r0 = F128::MultAdd(s, ray0_dir_normalized, ray0_point);
1339  f128 pt_r1 = F128::MultAdd(t, ray1_dir_normalized, ray1_point);
1340  SimdVector ret = F128::Sub(pt_r0, pt_r1);
1341  ret = Vector3::LengthSq(ret);
1342 
1343  if (point_on_ray0) *point_on_ray0 = pt_r0;
1344  if (point_on_ray1) *point_on_ray1 = pt_r1;
1345  return ret;
1346 }
1347 
1348 // pointer arguments can be NULL
1349 NLIB_M(f128) DistanceSq::RaySegment(SimdVector* point_on_ray, SimdVector* point_on_segment,
1350  SimdVectorArg ray_point, SimdVectorArg ray_dir_normalized,
1351  SimdVectorArg segment_point0,
1352  SimdVectorArg segment_point1) NLIB_NOEXCEPT {
1353  SimdVector seg_dir = F128::Sub(segment_point1, segment_point0);
1354  SimdVector u = F128::Sub(ray_point, segment_point0);
1355  f128 b = Vector3::Dot(ray_dir_normalized, seg_dir);
1356  f128 c = Vector3::LengthSq(seg_dir);
1357  f128 d = Vector3::Dot(ray_dir_normalized, u);
1358  f128 e = Vector3::Dot(seg_dir, u);
1359  f128 det = F128::MultSub(b, b, c);
1360  f128 one = F128::SetOne();
1361  f128 parallel = F128::CmpLt(det, F128::SetEpsilon());
1362  f128 tmp0, tmp1;
1363 
1364  // sNum:
1365  // 0.f if parallel
1366  // b * e - c * d otherwise
1367  tmp0 = F128::Mult(b, e);
1368  tmp0 = F128::MultSub(c, d, tmp0);
1369  f128 sNum = F128::AndNot(parallel, tmp0);
1370 
1371  // tNum:
1372  // e if parallel or sNum < 0.f
1373  // e - b * d otherwise
1374  f128 sNum_negative = F128::CmpLtZero(sNum);
1375  f128 parallel_or_sNum_negative = F128::Or(parallel, sNum_negative);
1376  tmp1 = F128::MultSub(b, d, e);
1377  f128 tNum = F128::Select(parallel_or_sNum_negative, e, tmp1);
1378 
1379  // tDenom:
1380  // c if parallel or sNum < 0.f
1381  // det otherwise
1382  f128 tDenom = F128::Select(parallel_or_sNum_negative, c, det);
1383 
1384  // sNum:
1385  // 0.f if sNum < 0.f
1386  // sNum otherwise
1387  sNum = F128::AndNot(sNum_negative, sNum);
1388 
1389  // sNum:
1390  // 0.f if tNum < 0.f and -d < 0.f
1391  // -d if tNum < 0.f and !(-d < 0.f)
1392  // 0.f if tNum > tDenom and b - d < 0.f
1393  // b - d if tNum > tDenom and !(b - d < 0.f)
1394  // sNum otherwise
1395  f128 minus_d = F128::Negate(d);
1396  f128 b_minus_d = F128::Sub(b, d);
1397  f128 tNum_negative = F128::CmpLtZero(tNum);
1398  f128 tNum_out = F128::Or(tNum_negative, F128::CmpGt(tNum, tDenom));
1399  tmp0 = F128::Select(tNum_negative, minus_d, b_minus_d);
1400  f128 cmp = F128::CmpGeZero(tmp0);
1401  tmp1 = F128::And(cmp, tmp0);
1402  sNum = F128::Select(tNum_out, tmp1, sNum);
1403 
1404  // sDenom:
1405  // 1.f parallel
1406  // 1.f tNum < 0.f and !(-d < 0)
1407  // 1.f tNum > tDenom and !(b - d < 0.f)
1408  // det otherwise
1409  tmp0 = F128::Or(parallel, F128::And(tNum_out, cmp));
1410  f128 sDenom = F128::Select(tmp0, one, det);
1411 
1412  // tNum:
1413  // 0.f if tNum < 0.f
1414  // tDenom if tNum > tDenom
1415  // tNum otherwise
1416  tNum = F128::Clamp(tNum, F128::SetZero(), tDenom);
1417 
1418  f128 s = F128::Div(sNum, sDenom);
1419  f128 t = F128::Div(tNum, tDenom);
1420 
1421  f128 pt_r = F128::MultAdd(s, ray_dir_normalized, ray_point);
1422  f128 pt_s = F128::MultAdd(t, seg_dir, segment_point0);
1423  SimdVector ret = F128::Sub(pt_r, pt_s);
1424  ret = Vector3::LengthSq(ret);
1425 
1426  if (point_on_ray) *point_on_ray = pt_r;
1427  if (point_on_segment) *point_on_segment = pt_s;
1428  return ret;
1429 }
1430 
1431 #ifdef _MSC_VER
1432 # pragma endregion DistanceSq function implementation
1433 #endif
1434 
1435 //
1436 // Intersection
1437 //
1438 #ifdef _MSC_VER
1439 # pragma region Intersection
1440 #endif
1441 
1442 // nan if parallel
1443 // XMPlaneIntersectLine
1444 NLIB_M(SimdVector) Intersection::PlaneLine(SimdPlaneArg plane, SimdVectorArg line_point,
1445  SimdVectorArg line_dir_normalized) NLIB_NOEXCEPT {
1446  f128 dot = Plane::DotNormal(plane, line_dir_normalized);
1447  SimdVector t = Plane::DotCoord(plane, line_point);
1448  t = F128::Div(t, dot);
1449  SimdVector ret = F128::MultSub(t, line_dir_normalized, line_point);
1450 
1451  f128 eps = F128::SetEpsilon();
1452  f128 dotzero = F128::CmpNearEqZero(dot, eps);
1453  f128 nan = F128::SetNaN();
1454 
1455  return F128::Select(dotzero, nan, ret);
1456 }
1457 
1458 // nan if parallel or no intersection
1459 NLIB_M(SimdVector) Intersection::PlaneRay(SimdPlaneArg plane, SimdVectorArg ray_point,
1460  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
1461  f128 dot = Plane::DotNormal(plane, ray_dir_normalized);
1462  SimdVector t = Plane::DotCoord(plane, ray_point);
1463  t = F128::Div(t, dot);
1464  SimdVector ret = F128::MultSub(t, ray_dir_normalized, ray_point);
1465 
1466  f128 eps = F128::SetEpsilon();
1467  f128 dotzero = F128::CmpNearEqZero(dot, eps);
1468  f128 t_gt_0 = F128::CmpGtZero(t);
1469  f128 nan = F128::SetNaN();
1470 
1471  return F128::Select(F128::Or(t_gt_0, dotzero), nan, ret);
1472 }
1473 
1474 // nan if no intersection
1475 NLIB_M(SimdVector) Intersection::PlaneSegment(SimdPlaneArg plane, SimdVectorArg segment_point0,
1476  SimdVectorArg segment_point1) NLIB_NOEXCEPT {
1477  SimdVector dir = F128::Sub(segment_point1, segment_point0);
1478  f128 recp_len = Vector3::RecpLength(dir);
1479  dir = F128::Mult(dir, recp_len); // normalization
1480 
1481  f128 dot = Plane::DotNormal(plane, dir);
1482  SimdVector t = Plane::DotCoord(plane, segment_point1);
1483  t = F128::Div(t, dot);
1484  SimdVector ret = F128::MultSub(t, dir, segment_point1);
1485 
1486  f128 t_gt_len = F128::CmpGt(F128::Mult(t, recp_len), F128::SetOne());
1487  f128 t_lt_0 = F128::CmpLtZero(t);
1488  f128 nan = F128::SetNaN();
1489 
1490  return F128::Select(F128::Or(t_gt_len, t_lt_0), nan, ret);
1491 }
1492 
1493 // BoundingSphere::Intersects()
1494 NLIB_B Intersection::SphereRay(float* distance, SimdSphereArg sphere, SimdVectorArg ray_point,
1495  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
1496  SimdVector pt_c = F128::Sub(sphere, ray_point);
1497 
1498  f128 len_on_ray = Vector3::Dot(pt_c, ray_dir_normalized);
1499  f128 lensq_pt_c = Vector3::LengthSq(pt_c);
1500  f128 r_sq = F128::SetValue<3>(sphere, each_select32);
1501  r_sq = F128::Mult(r_sq, r_sq);
1502  f128 distsq = F128::MultSub(len_on_ray, len_on_ray, lensq_pt_c);
1503 
1504  f128 origin_inside = F128::CmpLe(lensq_pt_c, r_sq);
1505  f128 intersection_none;
1506  intersection_none = F128::CmpLt(len_on_ray, F128::SetZero());
1507  intersection_none = F128::AndNot(origin_inside, intersection_none);
1508  intersection_none = F128::Or(intersection_none, F128::CmpGt(distsq, r_sq));
1509 
1510  if (F128::IsAllMaskFalse(intersection_none)) {
1511  if (distance) {
1512  // compute the distance from ray_point to the surface of the sphere
1513  // r_sq = distsq + q^2
1514  f128 q = F128::Sqrt(F128::Sub(r_sq, distsq));
1515  f128 t0 = F128::Sub(len_on_ray, q); // from outside to inside
1516  f128 t1 = F128::Add(len_on_ray, q); // from inside to outside
1517  f128 t = F128::Select(origin_inside, t1, t0);
1518  *distance = F128::GetFloatFromLane<0>(t);
1519  }
1520  return true;
1521  } else {
1522  return false;
1523  }
1524 }
1525 
1526 // TriangleTests::Intersects()
1527 NLIB_B Intersection::TriangleRay(float* distance, SimdVectorArg triangle_point0,
1528  SimdVectorArg triangle_point1, SimdVectorArg triangle_point2,
1529  SimdVectorArg ray_point,
1530  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
1531  // See http://www.graphics.cornell.edu/pubs/1997/MT97.html
1532 
1533  SimdVector E1 = F128::Sub(triangle_point1, triangle_point0);
1534  SimdVector E2 = F128::Sub(triangle_point2, triangle_point0);
1535  SimdVector P = Vector3::Cross(ray_dir_normalized, E2); // D x E2
1536 
1537  f128 det = Vector3::Dot(E1, P);
1538  f128 inv_det = F128::Recp(det);
1539 
1540  // if det is near zero, ray lies in plane of triangle
1541  f128 intersection_none = F128::InBound(det, F128::SetEpsilon());
1542 
1543  // calculate distance from triangle_point0 to ray_point
1544  SimdVector T = F128::Sub(ray_point, triangle_point0); // O - V0
1545  SimdVector Q = Vector3::Cross(T, E1); // Q = T x E1
1546 
1547  // calculate u parameter
1548  f128 u = Vector3::Dot(T, P);
1549  // calculate v parameter
1550  f128 v = Vector3::Dot(ray_dir_normalized, Q);
1551  f128 uv = F128::Mult(F128::Permute<0, 1, 4, 5>(u, v), inv_det);
1552  // calculate t
1553  f128 t = F128::Mult(Vector3::Dot(E2, Q), inv_det);
1554 
1555  // intersects if 0 <= u,v <= 1 and t >= 0
1556  intersection_none = F128::Or(intersection_none, F128::CmpLt(uv, F128::SetZero()));
1557  intersection_none = F128::Or(intersection_none, F128::CmpGt(uv, F128::SetOne()));
1558  intersection_none = F128::Or(intersection_none, F128::CmpLt(t, F128::SetZero()));
1559 
1560  if (F128::IsAllMaskFalse(intersection_none)) {
1561  if (distance) {
1562  *distance = F128::GetFloatFromLane<0>(t);
1563  }
1564  return true;
1565  } else {
1566  return false;
1567  }
1568 }
1569 
1570 // TriangleTests::Intersects()
1572 Intersection::TrianglePlane(SimdVectorArg triangle_point0, SimdVectorArg triangle_point1,
1573  SimdVectorArg triangle_point2,
1574  SimdPlaneArg plane_normalized) NLIB_NOEXCEPT {
1575  SimdVector tp0 = F128::SetFloatToLane<3>(triangle_point0, 1.f);
1576  SimdVector tp1 = F128::SetFloatToLane<3>(triangle_point1, 1.f);
1577  SimdVector tp2 = F128::SetFloatToLane<3>(triangle_point2, 1.f);
1578 
1579 #ifdef NLIB_SSE41
1580  f128 dist0 = Vector4::Dot(plane_normalized, tp0);
1581  f128 dist1 = Vector4::Dot(plane_normalized, tp1);
1582  f128 dist2 = Vector4::Dot(plane_normalized, tp2);
1583 #else
1584  f128 dist012 = Vector4::Dot3(plane_normalized, tp0, tp1, tp2);
1585  f128 dist0 = F128::SetValue<0>(dist012, each_select32);
1586  f128 dist1 = F128::SetValue<1>(dist012, each_select32);
1587  f128 dist2 = F128::SetValue<2>(dist012, each_select32);
1588 #endif
1589  f128 min_dist = F128::Min(dist0, dist1);
1590  f128 max_dist = F128::Max(dist0, dist1);
1591  min_dist = F128::Min(min_dist, dist2);
1592  max_dist = F128::Max(max_dist, dist2);
1593 
1594  f128 outside = F128::CmpGtZero(min_dist);
1595  f128 inside = F128::CmpLtZero(max_dist);
1596 
1597  if (F128::IsAllMaskTrue(outside)) return PLANE_FRONT;
1598  if (F128::IsAllMaskTrue(inside)) return PLANE_BACK;
1599  return PLANE_INTERSECT;
1600 }
1601 
1602 // BoundingSphere::Intersects()
1604 Intersection::SpherePlane(SimdSphereArg sphere, SimdPlaneArg plane_normalized) NLIB_NOEXCEPT {
1605  f128 c = F128::SetFloatToLane<3>(sphere, 1.f);
1606  f128 r = F128::SetValue<3>(sphere, each_select32);
1607  f128 dist = Vector4::Dot(plane_normalized, c);
1608  f128 outside = F128::CmpGt(dist, r);
1609  f128 inside = F128::CmpLt(dist, F128::Negate(r));
1610 
1611  if (F128::IsAllMaskTrue(outside)) return PLANE_FRONT;
1612  if (F128::IsAllMaskTrue(inside)) return PLANE_BACK;
1613  return PLANE_INTERSECT;
1614 }
1615 
1616 // BoundingSphere::Intersects()
1617 NLIB_B Intersection::SphereSphere(SimdSphereArg sphere0, SimdSphereArg sphere1) NLIB_NOEXCEPT {
1618  f128 distsq = Vector3::LengthSq(F128::Sub(sphere0, sphere1));
1619  f128 radsq = F128::SetValue<3>(F128::Add(sphere0, sphere1), each_select32);
1620  radsq = F128::Mult(radsq, radsq);
1621  return F128::IsAllMaskFalse(F128::CmpGt(distsq, radsq));
1622 }
1623 
1624 // return nan if parallel
1625 // XMPlaneIntersectPlane
1626 NLIB_M(f128x2) Intersection::PlanePlane(SimdPlaneArg plane0, SimdPlaneArg plane1) NLIB_NOEXCEPT {
1627  // Len(A x B) = Len(A) * Len(B) * sin(AB)
1628  SimdVector lineVec = Vector3::Cross(plane1, plane0);
1629  f128 lineVecLenSq = Vector3::LengthSq(lineVec);
1630 
1631  // A x (B x C) = Dot(A, C)B - Dot(A, B)C
1632  // n1 x (n1 x n0) = Dot(n0,n1)n1 - Dot(n1,n1)n0
1633  SimdVector p1_line = Vector3::Cross(plane1, lineVec);
1634  SimdVector point = F128::Mult<3>(plane0, p1_line, each_select32);
1635 
1636  // (A x B) x C = Dot(A, C)B - Dot(B, C)A
1637  // (n1 x n0) x n0 = Dot(n0,n1)n0 - Dot(n0,n0)n1
1638  SimdVector line_p0 = Vector3::Cross(lineVec, plane0);
1639  point = F128::MultAdd<3>(plane1, line_p0, point, each_select32);
1640 
1641  SimdVector point0 = F128::Div(point, lineVecLenSq);
1642  SimdVector point1 = F128::Add(point0, lineVec); // lineVec is along the plane0 and plane1
1643  f128 eps = F128::SetEpsilon();
1644  f128 nan = F128::SetNaN();
1645  f128 parallel = F128::CmpNearEq(F128::SetZero(), lineVecLenSq, eps);
1646  f128x2 ret;
1647  ret.val[0] = F128::Select(parallel, nan, point0);
1648  ret.val[1] = F128::Select(parallel, nan, point1);
1649  return ret;
1650 }
1651 
1652 // BoundingSphere::Intersects()
1653 NLIB_B Intersection::SphereTriangle(SimdSphereArg sphere, SimdVectorArg triangle_point0,
1654  SimdVectorArg triangle_point1,
1655  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
1656  SimdPlane plane = Plane::FromPoint(triangle_point0, triangle_point1, triangle_point2);
1657 
1658  SimdVector q;
1659  // no intersection if the plane is farther from the center than the radius.
1660  f128 distsq = DistanceSq::PointPlane(&q, sphere, plane);
1661  f128 r_sq = F128::SetValue<3>(sphere, each_select32);
1662  r_sq = F128::Mult(r_sq, r_sq);
1663  f128 cmp = F128::CmpLe(distsq, r_sq);
1664  if (F128::IsAllMaskFalse(cmp)) return false;
1665 
1666  // if q is inside the triangle, it must intersect.
1667  // (e.g. in case of a bigger triangle, each edge is far from the sphere)
1668  SimdVector c0 =
1669  Vector3::Cross(F128::Sub(q, triangle_point0), F128::Sub(triangle_point1, triangle_point0));
1670  SimdVector c1 =
1671  Vector3::Cross(F128::Sub(q, triangle_point1), F128::Sub(triangle_point2, triangle_point1));
1672  SimdVector c2 =
1673  Vector3::Cross(F128::Sub(q, triangle_point2), F128::Sub(triangle_point0, triangle_point2));
1674  plane = F128::SetZeroToLane<3>(plane); // normal vector now
1675  f128 three_dots = Vector4::Dot3(plane, c0, c1, c2);
1676  three_dots = F128::Swizzle<0, 1, 2, 2>(three_dots);
1677  cmp = F128::CmpGeZero(three_dots);
1678  if (F128::IsAllMaskFalse(cmp) || F128::IsAllMaskTrue(cmp)) return true;
1679 
1680  // if the distance between an edge of the triangle and the center is
1681  // less(or equal) than the radius, it must intersect.
1682  f128 seg_dist01 = DistanceSq::PointSegment(sphere, triangle_point0, triangle_point1);
1683  f128 seg_dist02 = DistanceSq::PointSegment(sphere, triangle_point0, triangle_point2);
1684  f128 seg_dist = F128::Min(seg_dist01, seg_dist02);
1685  f128 seg_dist12 = DistanceSq::PointSegment(sphere, triangle_point1, triangle_point2);
1686  seg_dist = F128::Min(seg_dist, seg_dist12);
1687  cmp = F128::CmpLe(seg_dist, r_sq);
1688  if (F128::IsAllMaskFalse(cmp)) return false;
1689  return true;
1690 }
1691 
1692 // BoundingBox::Intersects()
1694 Intersection::AxisAlignedBoxPlane(const AxisAlignedBox& aabb,
1695  SimdPlaneArg plane_normalized) NLIB_NOEXCEPT {
1696  SimdVector aabb_max = aabb.point_max;
1697  SimdVector aabb_min = aabb.point_min;
1698  SimdVector center = F128::Add(aabb_max, aabb_min);
1699  center = F128::Mult(0.5f, center);
1700  SimdVector extents = F128::Sub(aabb_max, center);
1701  center = F128::SetFloatToLane<3>(center, 1.f);
1702 
1703  f128 dist = Vector4::Dot(center, plane_normalized);
1704  f128 radius = Vector3::Dot(extents, F128::Abs(plane_normalized));
1705  if (F128::IsAllMaskFalse(F128::CmpLe(dist, radius))) {
1706  return PLANE_FRONT;
1707  }
1708  if (F128::IsAllMaskFalse(F128::CmpGe(dist, F128::Negate(radius)))) {
1709  return PLANE_BACK;
1710  }
1711  return PLANE_INTERSECT;
1712 }
1713 
1714 // BoundingBox::Intersects()
1715 NLIB_B Intersection::AxisAlignedBoxRay(const AxisAlignedBox& aabb, SimdVectorArg ray_point,
1716  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
1717  // https://www.siggraph.org/education/materials/HyperGraph/raytrace/rtinter3.htm
1718  // https://truesculpt.googlecode.com/hg-history/Release%25200.8/Doc/ray_box_intersect.pdf
1719  f128 inv_dir = F128::Recp(ray_dir_normalized);
1720  f128 selector = F128::CmpGeZero(ray_dir_normalized);
1721  SimdVector aabb_min = aabb.point_min;
1722  SimdVector aabb_max = aabb.point_max;
1723  f128 tmin = F128::Select(selector, aabb_min, aabb_max);
1724  f128 tmax = F128::Select(selector, aabb_max, aabb_min);
1725  tmin = F128::Sub(tmin, ray_point);
1726  tmax = F128::Sub(tmax, ray_point);
1727  tmin = F128::Mult(tmin, inv_dir);
1728  tmax = F128::Mult(tmax, inv_dir);
1729 
1730  // false if
1731  // tmin.x > tmax.y || tmin.y > tmax.x ||
1732  // tmin.x > tmax.z || tmin.z > tmax.x
1733  tmin = F128::Swizzle<0, 1, 2, 0>(tmin);
1734  tmax = F128::Swizzle<2, 0, 0, 1>(tmax);
1735  f128 result = F128::CmpGeZero(tmax);
1736  if (!F128::IsAllMaskFalse(F128::CmpGt(tmin, tmax))) return false;
1737 
1738  // true if min(tmax.xyz) >= 0
1739  return F128::IsAllMaskTrue(result);
1740 }
1741 
1742 // BoundingBox::Intersects()
1743 NLIB_B Intersection::AxisAlignedBoxSphere(const AxisAlignedBox& aabb,
1744  SimdSphereArg sphere) NLIB_NOEXCEPT {
1745  SimdVector aabb_min = aabb.point_min;
1746  SimdVector aabb_max = aabb.point_max;
1747  f128 max_center = F128::Sub(sphere, aabb_max);
1748  f128 min_center = F128::Sub(sphere, aabb_min);
1749  f128 max_mask = F128::CmpGt(sphere, aabb_max);
1750  f128 min_mask = F128::CmpLt(sphere, aabb_min);
1751  f128 dist_sq = F128::SetZero();
1752  dist_sq = F128::Select(max_mask, max_center, dist_sq);
1753  dist_sq = F128::Select(min_mask, min_center, dist_sq);
1754  dist_sq = Vector3::LengthSq(dist_sq);
1755  f128 r_sq = F128::SetValue<3>(sphere, each_select32);
1756  r_sq = F128::Mult(r_sq, r_sq);
1757  return F128::IsAllMaskTrue(F128::CmpLe(dist_sq, r_sq));
1758 }
1759 
1760 // BoundingBox::Intersects()
1761 NLIB_B Intersection::AxisAlignedBoxAxisAlignedBox(const AxisAlignedBox& aabb0,
1762  const AxisAlignedBox& aabb1) NLIB_NOEXCEPT {
1763  f128 cond0 = F128::CmpGt(aabb0.point_min, aabb1.point_max); // true if disjoint
1764  f128 cond1 = F128::CmpGt(aabb1.point_min, aabb0.point_max); // true if disjoint
1765  return F128::IsAllMaskFalse(F128::Or(cond0, cond1));
1766 }
1767 
1768 // BoundingBox::Intersects()
1769 NLIB_B Intersection::AxisAlignedBoxTriangle(const AxisAlignedBox& aabb,
1770  SimdVectorArg triangle_point0,
1771  SimdVectorArg triangle_point1,
1772  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
1773  // See "Fast 3D Triangle-Box Overlap Testing"
1774 
1775  SimdVector box_min = aabb.point_min;
1776  SimdVector box_max = aabb.point_max;
1777  SimdVector tri0 = triangle_point0;
1778  SimdVector tri1 = triangle_point1;
1779  SimdVector tri2 = triangle_point2;
1780  {
1781  // minimal AABB around the triangle
1782  SimdVector tri_min =
1783  F128::Min(F128::Min(tri0, tri1), tri2);
1784  SimdVector tri_max =
1785  F128::Max(F128::Max(tri0, tri1), tri2);
1786  f128 cond0 = F128::CmpGt(tri_min, box_max);
1787  f128 cond1 = F128::CmpGt(box_min, tri_max);
1788  if (!F128::IsAllMaskFalse(F128::Or(cond0, cond1))) {
1789  // no intersection if the two AABBs are disjoint
1790  return false;
1791  }
1792 
1793  // true if a vertex in AABB
1794  f128 point_in;
1795  point_in = F128::And(F128::CmpGe(tri0, box_min),
1796  F128::CmpLe(tri0, box_max));
1797  if (F128::IsAllMaskTrue(point_in)) return true;
1798  point_in = F128::And(F128::CmpGe(tri1, box_min),
1799  F128::CmpLe(tri1, box_max));
1800  if (F128::IsAllMaskTrue(point_in)) return true;
1801  point_in = F128::And(F128::CmpGe(tri2, box_min),
1802  F128::CmpLe(tri2, box_max));
1803  if (F128::IsAllMaskTrue(point_in)) return true;
1804  }
1805 
1806  SimdVector f0 = F128::Sub(tri1, tri0);
1807  SimdVector f1 = F128::Sub(tri2, tri1);
1808 
1809  // fast plane/AABB overlap test
1810  {
1811  // Nv - D = 0
1812  SimdVector N = Vector3::Cross(f0, f1);
1813  N = F128::SetZeroToLane<3>(N);
1814  f128 D = Vector3::Dot(N, tri0);
1815  f128 normal_gt_0 = F128::CmpGt(N, F128::SetZero());
1816  SimdVector v_near = F128::Select(normal_gt_0, box_min, box_max);
1817  SimdVector v_far = F128::Select(normal_gt_0, box_max, box_min);
1818  f128 min_near = Vector4::Dot(N, v_near);
1819  f128 max_far = Vector4::Dot(N, v_far);
1820  min_near = F128::CmpGt(min_near, D);
1821  max_far = F128::CmpLt(max_far, D);
1822  if (!F128::IsAllMaskFalse(F128::Or(min_near, max_far))) {
1823  return false;
1824  }
1825  }
1826 
1827  SimdVector box_center = F128::Add(box_max, box_min);
1828  box_center = F128::Mult(0.5f, box_center);
1829  SimdVector extents = F128::Sub(box_center, box_min);
1830  SimdVector tp0 = F128::Sub(tri0, box_center);
1831  SimdVector tp1 = F128::Sub(tri1, box_center);
1832  SimdVector tp2 = F128::Sub(tri2, box_center);
1833  tp0 = F128::SetZeroToLane<3>(tp0);
1834  tp1 = F128::SetZeroToLane<3>(tp1);
1835  tp2 = F128::SetZeroToLane<3>(tp2);
1836 
1837  SimdVector f2 = F128::Sub(tri0, tri2);
1838  f0 = F128::SetZeroToLane<3>(f0);
1839  f1 = F128::SetZeroToLane<3>(f1);
1840  f2 = F128::SetZeroToLane<3>(f2);
1841 
1842  f128 pmax, pmin;
1843  f128 radius;
1844  f128 tp0dot_A00A01A02A10;
1845  f128 tp0dot_A11A12A20A21;
1846  f128 tp1dot_A01A02;
1847  f128 tp1dot_A11A12A21A22;
1848  f128 tp2dot_A00A10A20A22;
1849  f128 neg_f0 = F128::Negate(f0);
1850  f128 neg_f1 = F128::Negate(f1);
1851  f128 neg_f2 = F128::Negate(f2);
1852 
1853  // e = (1, 0, 0), cross(e, f0) = (0, -f0.z, f0.y)
1854  SimdVector A00 = F128::Permute<7, 6, 1, 3>(f0, neg_f0);
1855  // e = (1, 0, 0), cross(e, f1) = (0, -f1.z, f1.y)
1856  SimdVector A01 = F128::Permute<7, 6, 1, 3>(f1, neg_f1);
1857  // e = (1, 0, 0), cross(e, f1) = (0, -f2.z, f2.y)
1858  SimdVector A02 = F128::Permute<7, 6, 1, 3>(f2, neg_f2);
1859 
1860  // e = (0, 1, 0), cross(e, f0) = (f0.z, 0, -f0.x)
1861  SimdVector A10 = F128::Permute<2, 3, 4, 3>(f0, neg_f0);
1862  // e = (0, 1, 0), cross(e, f1) = (f1.z, 0, -f1.x)
1863  SimdVector A11 = F128::Permute<2, 3, 4, 3>(f1, neg_f1);
1864  // e = (0, 1, 0), cross(e, f1) = (f2.z, 0, -f2.x)
1865  SimdVector A12 = F128::Permute<2, 3, 4, 3>(f2, neg_f2);
1866 
1867  // e = (0, 0, 1), cross(e, f0) = (-f0.y, f0.x, 0)
1868  SimdVector A20 = F128::Permute<5, 0, 3, 3>(f0, neg_f0);
1869  // e = (0, 0, 1), cross(e, f1) = (f1.z, 0, -f1.x)
1870  SimdVector A21 = F128::Permute<5, 0, 3, 3>(f1, neg_f1);
1871  // e = (0, 0, 1), cross(e, f1) = (f2.z, 0, -f2.x)
1872  SimdVector A22 = F128::Permute<5, 0, 3, 3>(f2, neg_f2);
1873 
1874  tp0dot_A00A01A02A10 = Vector4::Dot4(tp0, A00, A01, A02, A10);
1875  tp0dot_A11A12A20A21 = Vector4::Dot4(tp0, A11, A12, A10, A11);
1876  tp1dot_A01A02 = Vector4::Dot2(tp1, A01, A02);
1877  tp1dot_A11A12A21A22 = Vector4::Dot4(tp1, A11, A12, A21, A22);
1878  tp2dot_A00A10A20A22 = Vector4::Dot4(tp2, A00, A10, A20, A22);
1879 
1880  A00 = F128::Abs(A00);
1881  A01 = F128::Abs(A01);
1882  A02 = F128::Abs(A02);
1883  A10 = F128::Abs(A10);
1884  A11 = F128::Abs(A11);
1885  A12 = F128::Abs(A12);
1886  A20 = F128::Abs(A20);
1887  A21 = F128::Abs(A21);
1888  A22 = F128::Abs(A22);
1889  f128 tp12dot;
1890 
1891  // A00, A01, A02, A10
1892  radius = Vector4::Dot4(extents, A00, A01, A02, A10);
1893  tp12dot = F128::Permute<4, 0, 1, 5>(tp1dot_A01A02, tp2dot_A00A10A20A22);
1894  pmax = F128::Max(tp0dot_A00A01A02A10, tp12dot);
1895  pmin = F128::Min(tp0dot_A00A01A02A10, tp12dot);
1896  f128 intersection_none = F128::CmpGt(pmin, radius);
1897  intersection_none = F128::Or(intersection_none, F128::CmpLt(pmax, F128::Negate(radius)));
1898 
1899  // A11, A12, A20, A21
1900  radius = Vector4::Dot4(extents, A11, A12, A20, A21);
1901  tp12dot = F128::Permute<0, 1, 6, 2>(tp1dot_A11A12A21A22, tp2dot_A00A10A20A22);
1902  pmax = F128::Max(tp0dot_A11A12A20A21, tp12dot);
1903  pmin = F128::Min(tp0dot_A11A12A20A21, tp12dot);
1904  intersection_none = F128::Or(intersection_none, F128::CmpGt(pmin, radius));
1905  intersection_none = F128::Or(intersection_none, F128::CmpLt(pmax, F128::Negate(radius)));
1906 
1907  // A22
1908  radius = Vector4::Dot(extents, A22);
1909  pmax = F128::Max(tp1dot_A11A12A21A22, tp2dot_A00A10A20A22);
1910  pmin = F128::Min(tp1dot_A11A12A21A22, tp2dot_A00A10A20A22);
1911  pmax = F128::SetValue<3>(pmax, each_select32);
1912  pmin = F128::SetValue<3>(pmin, each_select32);
1913  intersection_none = F128::Or(intersection_none, F128::CmpGt(pmin, radius));
1914  intersection_none = F128::Or(intersection_none, F128::CmpLt(pmax, F128::Negate(radius)));
1915 
1916  return F128::IsAllMaskFalse(intersection_none);
1917 }
1918 
1919 // BoundingOrientedBox::Intersects()
1920 NLIB_B
1921 Intersection::OrientedBoxAxisAlignedBox(const OrientedBox& box,
1922  const AxisAlignedBox& aabb) NLIB_NOEXCEPT {
1923  SimdVector aabb_center, aabb_extent;
1924  {
1925  SimdVector aabb_max = aabb.point_max;
1926  SimdVector aabb_min = aabb.point_min;
1927  aabb_center = F128::Mult(0.5f, F128::Add(aabb_max, aabb_min));
1928  aabb_extent = F128::Sub(aabb_max, aabb_center);
1929  aabb_extent = F128::SetZeroToLane<3>(aabb_extent);
1930  }
1931  SimdVector box_center = F128::Sub(box.center, aabb_center);
1932  box_center = F128::SetZeroToLane<3>(box_center);
1933  SimdVector box_extent = F128::SetZeroToLane<3>(box.extent);
1934  SimdVector Mx0, Mx1, Mx2;
1935  SimdVector neg_Mx0, neg_Mx1, neg_Mx2;
1936  SimdVector abs_M0x, abs_M1x, abs_M2x;
1937 
1938  {
1939  SimdMatrix rot = Matrix::FromRotationQuaternion(box.rotation);
1940  abs_M0x = F128::Abs(rot.r[0]);
1941  abs_M1x = F128::Abs(rot.r[1]);
1942  abs_M2x = F128::Abs(rot.r[2]);
1943  NLIB_F128_TRANSPOSE(rot.r[0], rot.r[1], rot.r[2], rot.r[3]);
1944  Mx0 = rot.r[0];
1945  Mx1 = rot.r[1];
1946  Mx2 = rot.r[2];
1947  neg_Mx0 = F128::Negate(Mx0);
1948  neg_Mx1 = F128::Negate(Mx1);
1949  neg_Mx2 = F128::Negate(Mx2);
1950  }
1951 
1952  // use the separating axis theorem.
1953  // (two convex sets are disjoint if there is a plane(axis) separating them)
1954  //
1955  // there are 15 separating axes(planes).
1956  // Axx are the normals of the planes.
1957  //
1958  // aabb(0-2) = (1, 0, 0), (0, 1, 0), (0, 0, 1)
1959  // box(0-2) = B0, B1, B2 = Mx0, Mx1, Mx2
1960  // aabb_extent = extents of aabb
1961  // box_extent = extents of box
1962  //
1963  // A0(0-2) (1, 0, 0), (0, 1, 0), (0, 0, 1)
1964  // A1(0-2) B0, B1, B2
1965  // A2(0-2) (1, 0, 0) x B0, B1, and B2
1966  // A3(0-2) (0, 1, 0) x B0, B1, and B2
1967  // A4(0-2) (0, 0, 1) x B0, B1, and B2
1968  //
1969  // aabb_extent and box_extent are mapped onto Aij,
1970  // d_aabb and d_box are the ranges(radius) of them.
1971  // if the two ranges do not overlap, box and aabb are disjoint(the separating axis theorem).
1972  //
1973  // for (Aij in separating axes)
1974  // d_aabb = dot(aabb_extent, abs{dot((1, 0, 0), Aij), ...})
1975  // d_box = dot(box_extent, abs{dot(B0, Aij), dot(B1, Aij), dot(B2, Aij)})
1976  // d = abs(dot(box_center, Aij))
1977  // if (d > d_aabb + d_box) return false;
1978  f128 d, d_aabb, d_box;
1979  SimdVector t0, t1, t2;
1980  f128 intersection_none = F128::SetZero();
1981  f128 d_aabb_box;
1982  f128 cmp;
1983 
1984  // A00, A01, A02
1985  d = F128::Abs(box_center);
1986  d_aabb = aabb_extent;
1987  d_box = Vector4::Dot3(box_extent, F128::Abs(Mx0), F128::Abs(Mx1), F128::Abs(Mx2));
1988  d_aabb_box = F128::Add(d_aabb, d_box);
1989  intersection_none = F128::CmpGt(d, d_aabb_box);
1990 
1991  // A10, A11, A12
1992  d = F128::Abs(Vector4::Dot3(box_center, Mx0, Mx1, Mx2));
1993  d_aabb = Vector4::Dot3(aabb_extent, F128::Abs(Mx0), F128::Abs(Mx1), F128::Abs(Mx2));
1994  d_box = box_extent;
1995  d_aabb_box = F128::Add(d_aabb, d_box);
1996  cmp = F128::CmpGt(d, d_aabb_box);
1997  intersection_none = F128::Or(intersection_none, cmp);
1998 
1999  // note that dot(P, Q x R) == dot(Q, R x P)
2000 
2001  // A20 = (1, 0, 0) x Mx0 = (0, -M20, M10)
2002  // A21 = (1, 0, 0) x Mx1 = (0, -M21, M11)
2003  // A22 = (1, 0, 0) x Mx2 = (0, -M22, M12)
2004  t0 = F128::Permute<3, 6, 1, 3>(Mx0, neg_Mx0);
2005  t1 = F128::Permute<3, 6, 1, 3>(Mx1, neg_Mx1);
2006  t2 = F128::Permute<3, 6, 1, 3>(Mx2, neg_Mx2);
2007  d = F128::Abs(Vector4::Dot3(box_center, t0, t1, t2));
2008  d_aabb = Vector4::Dot3(aabb_extent, F128::Abs(t0), F128::Abs(t1), F128::Abs(t2));
2009  // abs(dot(Mx0-2, (1, 0, 0) x Mx0)) = abs(dot((1, 0, 0), Mx0 x Mx0-2)) =
2010  t0 = F128::Swizzle<3, 2, 1, 3>(abs_M0x); // abs(0, M02, M01)
2011  t1 = F128::Swizzle<2, 3, 0, 3>(abs_M0x); // abs(M02, 0, M00)
2012  t2 = F128::Swizzle<1, 0, 3, 3>(abs_M0x); // abs(M01, M00, 0)
2013  d_box = Vector4::Dot3(box_extent, t0, t1, t2);
2014  d_aabb_box = F128::Add(d_aabb, d_box);
2015  cmp = F128::CmpGt(d, d_aabb_box);
2016  intersection_none = F128::Or(intersection_none, cmp);
2017 
2018  // A30 = (0, 1, 0) x Mx0 = (M20, 0, -M00)
2019  // A31 = (0, 1, 0) x Mx1 = (M21, 0, -M01)
2020  // A32 = (0, 1, 0) x Mx2 = (M22, 0, -M02)
2021  t0 = F128::Permute<2, 3, 4, 3>(Mx0, neg_Mx0);
2022  t1 = F128::Permute<2, 3, 4, 3>(Mx1, neg_Mx1);
2023  t2 = F128::Permute<2, 3, 4, 3>(Mx2, neg_Mx2);
2024  d = F128::Abs(Vector4::Dot3(box_center, t0, t1, t2));
2025  d_aabb = Vector4::Dot3(aabb_extent, F128::Abs(t0), F128::Abs(t1), F128::Abs(t2));
2026  // abs(dot(Mx0-2, (0, 1, 0) x Mx0)) = abs(dot((0, 1, 0), Mx0 x Mx0-2)) =
2027  t0 = F128::Swizzle<3, 2, 1, 3>(abs_M1x); // abs(0, M12, M11)
2028  t1 = F128::Swizzle<2, 3, 0, 3>(abs_M1x); // abs(M12, 0, M10)
2029  t2 = F128::Swizzle<1, 0, 3, 3>(abs_M1x); // abs(M11, M10, 0)
2030  d_box = Vector4::Dot3(box_extent, t0, t1, t2);
2031  d_aabb_box = F128::Add(d_aabb, d_box);
2032  cmp = F128::CmpGt(d, d_aabb_box);
2033  intersection_none = F128::Or(intersection_none, cmp);
2034 
2035  // A40 = (0, 0, 1) x Mx0 = (-M10, M00, 0)
2036  // A41 = (0, 0, 1) x Mx1 = (-M11, M01, 0)
2037  // A42 = (0, 0, 1) x Mx2 = (-M12, M02, 0)
2038  t0 = F128::Permute<5, 0, 3, 3>(Mx0, neg_Mx0);
2039  t1 = F128::Permute<5, 0, 3, 3>(Mx1, neg_Mx1);
2040  t2 = F128::Permute<5, 0, 3, 3>(Mx2, neg_Mx2);
2041  d = F128::Abs(Vector4::Dot3(box_center, t0, t1, t2));
2042  d_aabb = Vector4::Dot3(aabb_extent, F128::Abs(t0), F128::Abs(t1), F128::Abs(t2));
2043  // abs(dot(Mx0-2, (0, 0, 1) x Mx0)) = abs(dot((0, 0, 1), Mx0 x Mx0-2)) =
2044  t0 = F128::Swizzle<3, 2, 1, 3>(abs_M2x); // abs(0, M22, M21)
2045  t1 = F128::Swizzle<2, 3, 0, 3>(abs_M2x); // abs(M22, 0, M20)
2046  t2 = F128::Swizzle<1, 0, 3, 3>(abs_M2x); // abs(M21, M20, 0)
2047  d_box = Vector4::Dot3(box_extent, t0, t1, t2);
2048  d_aabb_box = F128::Add(d_aabb, d_box);
2049  cmp = F128::CmpGt(d, d_aabb_box);
2050  intersection_none = F128::Or(intersection_none, cmp);
2051 
2052  intersection_none = F128::SetZeroToLane<3>(intersection_none);
2053  return F128::IsAllMaskFalse(intersection_none);
2054 }
2055 
2056 // BoundingOrientedBox::Intersects()
2057 NLIB_B Intersection::OrientedBoxOrientedBox(const OrientedBox& box0,
2058  const OrientedBox& box1) NLIB_NOEXCEPT {
2059  OrientedBox obox;
2060  SimdQuaternion box0_rot = box0.rotation;
2061  SimdQuaternion box1_rot = box1.rotation;
2062  obox.rotation = Quaternion::Mult(box0_rot, Quaternion::Conjugate(box1_rot));
2063  obox.center = Vector3::InvRotate(F128::Sub(box1.center, box0.center), box0_rot);
2064  obox.extent = box1.extent;
2065 
2066  AxisAlignedBox aabb;
2067  aabb.point_max = box0.extent;
2068  aabb.point_min = F128::Negate(aabb.point_max);
2069 
2070  return Intersection::OrientedBoxAxisAlignedBox(obox, aabb);
2071 }
2072 
2073 // BoundingOrientedBox::Intersects()
2074 NLIB_B Intersection::OrientedBoxSphere(const OrientedBox& box, SimdSphereArg sphere) NLIB_NOEXCEPT {
2075  SimdVector box_center = box.center;
2076  SimdVector box_extent = box.extent;
2077  SimdQuaternion box_rotation = box.rotation;
2078 
2079  // transform the sphere
2080  SimdVector sphere_center = F128::Sub(sphere, box_center);
2081  sphere_center = Vector3::InvRotate(sphere_center, box_rotation);
2082 
2083  // intersection between AABB and sphere (local to the AABB)
2084  f128 lt_min = F128::CmpLt(sphere_center, F128::Negate(box_extent));
2085  f128 gt_max = F128::CmpGt(sphere_center, box_extent);
2086  SimdVector diff_min = F128::Add(sphere_center, box_extent);
2087  SimdVector diff_max = F128::Sub(sphere_center, box_extent);
2088 
2089  f128 dist_sq = F128::SetZero();
2090  dist_sq = F128::Select(lt_min, diff_min, dist_sq);
2091  dist_sq = F128::Select(gt_max, diff_max, dist_sq);
2092  dist_sq = Vector3::Dot(dist_sq, dist_sq);
2093 
2094  f128 rad_sq = F128::Mult(sphere, sphere);
2095  rad_sq = F128::SetValue<3>(rad_sq, each_select32);
2096 
2097  return Vector4::CmpLe(dist_sq, rad_sq);
2098 }
2099 
2100 // BoundingOrientedBox::Intersects()
2101 NLIB_B Intersection::OrientedBoxTriangle(const OrientedBox& box, SimdVectorArg triangle_point0,
2102  SimdVectorArg triangle_point1,
2103  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
2104  // Transform the triangle points
2105  SimdVector box_center = box.center;
2106  SimdQuaternion box_rotation = box.rotation;
2107  SimdVector tp0 = F128::Sub(triangle_point0, box_center);
2108  SimdVector tp1 = F128::Sub(triangle_point1, box_center);
2109  SimdVector tp2 = F128::Sub(triangle_point2, box_center);
2110  tp0 = Vector3::InvRotate(tp0, box_rotation);
2111  tp1 = Vector3::InvRotate(tp1, box_rotation);
2112  tp2 = Vector3::InvRotate(tp2, box_rotation);
2113 
2114  AxisAlignedBox aabb;
2115  aabb.point_max = box.extent;
2116  aabb.point_min = F128::Negate(aabb.point_max);
2117 
2118  return Intersection::AxisAlignedBoxTriangle(aabb, tp0, tp1, tp2);
2119 }
2120 
2121 // BoundingOrientedBox::Intersects()
2122 inline Intersection::PlaneResult __vectorcall
2123 Intersection::OrientedBoxPlane(const OrientedBox& box,
2124  SimdPlaneArg plane_normalized) NLIB_NOEXCEPT {
2125  // Transform the plane
2126  SimdVector new_plane = Vector3::InvRotate(plane_normalized, box.rotation);
2127  f128 new_d = F128::Sub(plane_normalized, Vector3::Dot(plane_normalized, box.center));
2128  new_plane = F128::Permute<0, 1, 2, 7>(new_plane, new_d);
2129 
2130  AxisAlignedBox aabb;
2131  aabb.point_max = box.extent;
2132  aabb.point_min = F128::Negate(aabb.point_max);
2133 
2134  return Intersection::AxisAlignedBoxPlane(aabb, new_plane);
2135 }
2136 
2137 // BoundingOrientedBox::Intersects()
2138 NLIB_B Intersection::OrientedBoxRay(const OrientedBox& box, SimdVectorArg ray_point,
2139  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
2140  SimdQuaternion box_rotation = box.rotation;
2141  SimdVector P = F128::Sub(ray_point, box.center);
2142  P = Vector3::InvRotate(P, box_rotation);
2143  SimdVector D = Vector3::InvRotate(ray_dir_normalized, box_rotation);
2144 
2145  AxisAlignedBox aabb;
2146  aabb.point_max = box.extent;
2147  aabb.point_min = F128::Negate(aabb.point_max);
2148 
2149  return Intersection::AxisAlignedBoxRay(aabb, P, D);
2150 }
2151 
2152 NLIB_B Intersection::FrustumSphere(const Frustum& frustum, SimdSphereArg sphere) NLIB_NOEXCEPT {
2153  SimdPlane near_plane = frustum.near_plane_;
2154  SimdPlane far_plane = frustum.far_plane_;
2155  SimdPlane left_plane = frustum.left_plane_;
2156  SimdPlane right_plane = frustum.right_plane_;
2157  SimdPlane top_plane = frustum.top_plane_;
2158  SimdPlane bottom_plane = frustum.bottom_plane_;
2159  SimdVector frustum_center = frustum.center_;
2160  SimdQuaternion frustum_rot = frustum.rotation_;
2161  SimdVector center = sphere;
2162  SimdVector radius = F128::SetValue<3>(center, each_select32);
2163 
2164  // The center of the sphere to the local space of frustum
2165  center = Vector3::InvRotate(F128::Sub(center, frustum_center), frustum_rot);
2166  center = F128::SetFloatToLane<3>(center, 1.f);
2167 
2168  // distance from the planes
2169  f128 dist0, dist1;
2170  dist0 = Vector4::Dot4(center, near_plane, far_plane, left_plane, right_plane);
2171  dist1 = Vector4::Dot2(center, top_plane, bottom_plane);
2172  dist1 = F128::Swizzle<0, 1, 0, 1>(dist1);
2173 
2174  // outside if Px exists such that Px(center) > radius
2175  f128 outside = F128::CmpGt(dist0, radius);
2176  outside = F128::Or(F128::CmpGt(dist1, radius), outside);
2177  if (!F128::IsAllMaskFalse(outside)) return false;
2178 
2179  // inside if the center is inside all the planes
2180  f128 center_inside = F128::CmpLeZero(dist0);
2181  center_inside = F128::And(F128::CmpLeZero(dist0), center_inside);
2182  if (F128::IsAllMaskTrue(center_inside)) return true;
2183 
2184  // dist to a plane <= radius already satisfied(outside is all false)
2185  // 1) map center onto the plane, which the center is outside.
2186  // 2) return true if the point(pt_plane) is inside all the adjacent planes
2187  SimdVector pt_plane;
2188  f128 inside;
2189  f128 dist_plane;
2190 
2191  dist_plane = Vector4::Dot4(center, near_plane, far_plane, left_plane, right_plane);
2192 
2193  // onto the near plane
2194  pt_plane = F128::MultSub(near_plane, F128::SetValue<0>(dist0, each_select32), center);
2195  pt_plane = F128::SetFloatToLane<3>(pt_plane, 1.f);
2196  inside = Vector4::Dot4(pt_plane, left_plane, right_plane, top_plane, bottom_plane);
2197  inside = F128::CmpLeZero(inside);
2198  inside = F128::And(inside, F128::CmpGtZero(F128::SetValue<0>(dist_plane, each_select32)));
2199  if (F128::IsAllMaskTrue(inside)) return true;
2200 
2201  // onto the far plane
2202  pt_plane = F128::MultSub(far_plane, F128::SetValue<1>(dist0, each_select32), center);
2203  pt_plane = F128::SetFloatToLane<3>(pt_plane, 1.f);
2204  inside = Vector4::Dot4(pt_plane, left_plane, right_plane, top_plane, bottom_plane);
2205  inside = F128::CmpLeZero(inside);
2206  inside = F128::And(inside, F128::CmpGtZero(F128::SetValue<1>(dist_plane, each_select32)));
2207  if (F128::IsAllMaskTrue(inside)) return true;
2208 
2209  // onto the left plane
2210  pt_plane = F128::MultSub(left_plane, F128::SetValue<2>(dist0, each_select32), center);
2211  pt_plane = F128::SetFloatToLane<3>(pt_plane, 1.f);
2212  inside = Vector4::Dot4(pt_plane, near_plane, far_plane, top_plane, bottom_plane);
2213  inside = F128::CmpLeZero(inside);
2214  inside = F128::And(inside, F128::CmpGtZero(F128::SetValue<2>(dist_plane, each_select32)));
2215  if (F128::IsAllMaskTrue(inside)) return true;
2216 
2217  // onto the right plane
2218  pt_plane = F128::MultSub(right_plane, F128::SetValue<3>(dist0, each_select32), center);
2219  pt_plane = F128::SetFloatToLane<3>(pt_plane, 1.f);
2220  inside = Vector4::Dot4(pt_plane, near_plane, far_plane, top_plane, bottom_plane);
2221  inside = F128::CmpLeZero(inside);
2222  inside = F128::And(inside, F128::CmpGtZero(F128::SetValue<3>(dist_plane, each_select32)));
2223  if (F128::IsAllMaskTrue(inside)) return true;
2224 
2225  dist_plane = Vector4::Dot2(center, top_plane, bottom_plane);
2226 
2227  // onto the top plane
2228  pt_plane = F128::MultSub(top_plane, F128::SetValue<0>(dist1, each_select32), center);
2229  pt_plane = F128::SetFloatToLane<3>(pt_plane, 1.f);
2230  inside = Vector4::Dot4(pt_plane, near_plane, far_plane, left_plane, right_plane);
2231  inside = F128::CmpLeZero(inside);
2232  inside = F128::And(inside, F128::CmpGtZero(F128::SetValue<0>(dist_plane, each_select32)));
2233  if (F128::IsAllMaskTrue(inside)) return true;
2234 
2235  // onto the bottom plane
2236  pt_plane = F128::MultSub(bottom_plane, F128::SetValue<1>(dist1, each_select32), center);
2237  pt_plane = F128::SetFloatToLane<3>(pt_plane, 1.f);
2238  inside = Vector4::Dot4(pt_plane, near_plane, far_plane, left_plane, right_plane);
2239  inside = F128::CmpLeZero(inside);
2240  inside = F128::And(inside, F128::CmpGtZero(F128::SetValue<1>(dist_plane, each_select32)));
2241  if (F128::IsAllMaskTrue(inside)) return true;
2242 
2243  // sphere may be near the edges of the frustum
2244  center = sphere; // back to world space
2245  f128 radius_sq = F128::Mult(radius, radius);
2246  f128 dist_sq;
2247 
2248  // NOTE:
2249  // depends on the implementation of Frustum::GetCorners()
2250  Float3 corners[8];
2251  frustum.GetCorners(&corners[0]);
2252  SimdVector left_top = Vector3::LoadFloat3(&corners[0]);
2253  SimdVector right_top = Vector3::LoadFloat3(&corners[1]);
2254  SimdVector right_bottom = Vector3::LoadFloat3(&corners[2]);
2255  SimdVector left_bottom = Vector3::LoadFloat3(&corners[3]);
2256  SimdVector left_top_far = Vector3::LoadFloat3(&corners[4]);
2257  SimdVector right_top_far = Vector3::LoadFloat3(&corners[5]);
2258  SimdVector right_bottom_far = Vector3::LoadFloat3(&corners[6]);
2259  SimdVector left_bottom_far = Vector3::LoadFloat3(&corners[7]);
2260 
2261  // check the distance from the segments to the corner
2262  dist_sq = DistanceSq::PointSegment(center, left_top, right_top);
2263  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, right_top, right_bottom));
2264  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, right_bottom, left_bottom));
2265  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, left_bottom, left_top));
2266 
2267  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, left_top_far, right_top_far));
2268  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, right_top_far, right_bottom_far));
2269  dist_sq =
2270  F128::Min(dist_sq, DistanceSq::PointSegment(center, right_bottom_far, left_bottom_far));
2271  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, left_bottom_far, left_top_far));
2272 
2273  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, left_top_far, left_top));
2274  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, right_top_far, right_top));
2275  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, right_bottom_far, right_bottom));
2276  dist_sq = F128::Min(dist_sq, DistanceSq::PointSegment(center, left_bottom_far, left_bottom));
2277 
2278  return F128::IsAllMaskTrue(F128::CmpLe(dist_sq, radius_sq));
2279 }
2280 
2281 NLIB_B Containment::FrustumPoint(const Frustum& frustum, SimdVectorArg point) NLIB_NOEXCEPT {
2282  SimdVector pt = F128::Sub(point, frustum.center_);
2283  pt = Vector3::InvRotate(pt, frustum.rotation_);
2284  pt = F128::SetFloatToLane<3>(pt, 1.f);
2285 
2286  f128 dot1 = Vector4::Dot4(pt, frustum.near_plane_, frustum.far_plane_, frustum.top_plane_,
2287  frustum.bottom_plane_);
2288  f128 dot2 = Vector4::Dot2(pt, frustum.left_plane_, frustum.right_plane_);
2289  dot2 = F128::Swizzle<0, 1, 0, 1>(dot2);
2290  dot1 = F128::Max(dot1, dot2);
2291  f128 outside = F128::CmpGt(dot1, F128::SetZero());
2292  return F128::IsAllMaskFalse(outside);
2293 }
2294 
2295 NLIB_B Intersection::FrustumAxisAlignedBox(const Frustum& frustum,
2296  const AxisAlignedBox& aabb) NLIB_NOEXCEPT {
2297  // Check the center point for short cut.
2298  // In general, aabb is much smaller than frustum,
2299  // and the center point is near the vertices in compared with the frustum.
2300  SimdVector C = F128::Mult(0.5f, F128::Add(aabb.point_max, aabb.point_min));
2301  if (Containment::FrustumPoint(frustum, C)) return true;
2302  // if (Containment::FrustumPoint(frustum, aabb.point_max)) return true;
2303  // if (Containment::FrustumPoint(frustum, aabb.point_min)) return true;
2304 
2305  // Transform AABB's center to the origin
2306  SimdVector extent = F128::Sub(aabb.point_max, C);
2307  Frustum fstm;
2308  frustum.Transform(&fstm, 1.f, F128::Set0001(), F128::Negate(C));
2309 
2310  Float3 corners_[8];
2311  fstm.GetCorners(&corners_[0]);
2312 
2313  SimdVector corners[8];
2314  for (size_t i = 0; i < 8; ++i) {
2315  corners[i] = Vector3::LoadFloat3(&corners_[i]);
2316  corners[i] = F128::SetFloatToLane<3>(corners[i], 1.f);
2317  }
2318 
2319  // test 3 axes of the planes of the AABB
2320  SimdVector pt_min, pt_max;
2321  pt_min = pt_max = corners[0];
2322  for (size_t i = 1; i < 8; ++i) {
2323  SimdVector vec = corners[i];
2324  pt_min = F128::Min(vec, pt_min);
2325  pt_max = F128::Max(vec, pt_max);
2326  }
2327  f128 disjoint = F128::Or(F128::CmpGt(pt_min, extent),
2328  F128::CmpLt(pt_max, F128::Negate(extent)));
2329  disjoint = F128::SetZeroToLane<3>(disjoint);
2330  if (!F128::IsAllMaskFalse(disjoint)) return false;
2331 
2332  // test 5 axes of the planes of the frustum
2333  SimdPlane plane;
2334  SimdVector N;
2335  f128 D;
2336  SimdVector frustum_center = fstm.center_;
2337  SimdQuaternion frustum_rot = fstm.rotation_;
2338  f128 dist0_frustum, dist1_frustum;
2339  f128 dist_aabb_max, dist_aabb_min;
2340  f128 intersect;
2341 
2342  // near plane / far plane (they are parallel)
2343  plane = fstm.near_plane_;
2344  N = Vector3::Rotate(plane, frustum_rot);
2345  D = F128::Sub(plane, Vector3::Dot(N, frustum_center));
2346  plane = F128::Permute<0, 1, 2, 7>(N, D);
2347  D = F128::SetValue<3>(D, each_select32);
2348  dist0_frustum = Vector4::Dot4(plane, corners[0], corners[1], corners[2], corners[3]);
2349  dist1_frustum = Vector4::Dot4(plane, corners[4], corners[5], corners[6], corners[7]);
2350  N = Vector3::Dot(F128::Abs(N), extent);
2351  dist_aabb_max = F128::Add(D, N);
2352  dist_aabb_min = F128::Sub(D, N);
2353  intersect = F128::Or(F128::CmpGe(dist0_frustum, dist_aabb_min),
2354  F128::CmpGe(dist1_frustum, dist_aabb_min));
2355  if (F128::IsAllMaskFalse(intersect)) return false;
2356  intersect = F128::Or(F128::CmpLe(dist0_frustum, dist_aabb_max),
2357  F128::CmpLe(dist1_frustum, dist_aabb_max));
2358  if (F128::IsAllMaskFalse(intersect)) return false;
2359 
2360  // left plane
2361  plane = fstm.left_plane_;
2362  N = Vector3::Rotate(plane, frustum_rot);
2363  D = F128::Sub(plane, Vector3::Dot(N, frustum_center));
2364  plane = F128::Permute<0, 1, 2, 7>(N, D);
2365  D = F128::SetValue<3>(D, each_select32);
2366  dist0_frustum = Vector4::Dot4(plane, corners[0], corners[1], corners[2], corners[3]);
2367  dist1_frustum = Vector4::Dot4(plane, corners[4], corners[5], corners[6], corners[7]);
2368  N = Vector3::Dot(F128::Abs(N), extent);
2369  dist_aabb_max = F128::Add(D, N);
2370  dist_aabb_min = F128::Sub(D, N);
2371  intersect = F128::Or(F128::CmpGe(dist0_frustum, dist_aabb_min),
2372  F128::CmpGe(dist1_frustum, dist_aabb_min));
2373  if (F128::IsAllMaskFalse(intersect)) return false;
2374  intersect = F128::Or(F128::CmpLe(dist0_frustum, dist_aabb_max),
2375  F128::CmpLe(dist1_frustum, dist_aabb_max));
2376  if (F128::IsAllMaskFalse(intersect)) return false;
2377 
2378  // right plane
2379  plane = fstm.right_plane_;
2380  N = Vector3::Rotate(plane, frustum_rot);
2381  D = F128::Sub(plane, Vector3::Dot(N, frustum_center));
2382  plane = F128::Permute<0, 1, 2, 7>(N, D);
2383  D = F128::SetValue<3>(D, each_select32);
2384  dist0_frustum = Vector4::Dot4(plane, corners[0], corners[1], corners[2], corners[3]);
2385  dist1_frustum = Vector4::Dot4(plane, corners[4], corners[5], corners[6], corners[7]);
2386  N = Vector3::Dot(F128::Abs(N), extent);
2387  dist_aabb_max = F128::Add(D, N);
2388  dist_aabb_min = F128::Sub(D, N);
2389  intersect = F128::Or(F128::CmpGe(dist0_frustum, dist_aabb_min),
2390  F128::CmpGe(dist1_frustum, dist_aabb_min));
2391  if (F128::IsAllMaskFalse(intersect)) return false;
2392  intersect = F128::Or(F128::CmpLe(dist0_frustum, dist_aabb_max),
2393  F128::CmpLe(dist1_frustum, dist_aabb_max));
2394  if (F128::IsAllMaskFalse(intersect)) return false;
2395 
2396  // top plane
2397  plane = fstm.top_plane_;
2398  N = Vector3::Rotate(plane, frustum_rot);
2399  D = F128::Sub(plane, Vector3::Dot(N, frustum_center));
2400  plane = F128::Permute<0, 1, 2, 7>(N, D);
2401  D = F128::SetValue<3>(D, each_select32);
2402  dist0_frustum = Vector4::Dot4(plane, corners[0], corners[1], corners[2], corners[3]);
2403  dist1_frustum = Vector4::Dot4(plane, corners[4], corners[5], corners[6], corners[7]);
2404  N = Vector3::Dot(F128::Abs(N), extent);
2405  dist_aabb_max = F128::Add(D, N);
2406  dist_aabb_min = F128::Sub(D, N);
2407  intersect = F128::Or(F128::CmpGe(dist0_frustum, dist_aabb_min),
2408  F128::CmpGe(dist1_frustum, dist_aabb_min));
2409  if (F128::IsAllMaskFalse(intersect)) return false;
2410  intersect = F128::Or(F128::CmpLe(dist0_frustum, dist_aabb_max),
2411  F128::CmpLe(dist1_frustum, dist_aabb_max));
2412  if (F128::IsAllMaskFalse(intersect)) return false;
2413 
2414  // bottom plane
2415  plane = fstm.bottom_plane_;
2416  N = Vector3::Rotate(plane, frustum_rot);
2417  D = F128::Sub(plane, Vector3::Dot(N, frustum_center));
2418  plane = F128::Permute<0, 1, 2, 7>(N, D);
2419  D = F128::SetValue<3>(D, each_select32);
2420  dist0_frustum = Vector4::Dot4(plane, corners[0], corners[1], corners[2], corners[3]);
2421  dist1_frustum = Vector4::Dot4(plane, corners[4], corners[5], corners[6], corners[7]);
2422  N = Vector3::Dot(F128::Abs(N), extent);
2423  dist_aabb_max = F128::Add(D, N);
2424  dist_aabb_min = F128::Sub(D, N);
2425  intersect = F128::Or(F128::CmpGe(dist0_frustum, dist_aabb_min),
2426  F128::CmpGe(dist1_frustum, dist_aabb_min));
2427  if (F128::IsAllMaskFalse(intersect)) return false;
2428  intersect = F128::Or(F128::CmpLe(dist0_frustum, dist_aabb_max),
2429  F128::CmpLe(dist1_frustum, dist_aabb_max));
2430  if (F128::IsAllMaskFalse(intersect)) return false;
2431 
2432  // Axis = Cross(aabb edge, frustum edge)
2433  SimdVector E0[3];
2434  E0[0] = F128::Set1000();
2435  E0[1] = F128::Set0100();
2436  E0[2] = F128::Set0010();
2437  SimdVector E1[6];
2438  E1[0] = F128::Sub(corners[0], corners[4]);
2439  E1[1] = F128::Sub(corners[1], corners[5]);
2440  E1[2] = F128::Sub(corners[2], corners[6]);
2441  E1[3] = F128::Sub(corners[3], corners[7]);
2442  E1[4] = F128::Sub(corners[1], corners[0]);
2443  E1[5] = F128::Sub(corners[1], corners[2]);
2444 
2445  f128 dot0, dot1;
2446  for (size_t i = 0; i < 3; ++i) {
2447  for (size_t j = 0; j < 6; ++j) {
2448  SimdVector axis = Vector3::Cross(E0[i], E1[j]);
2449  axis = F128::SetZeroToLane<3>(axis);
2450  N = Vector3::Dot(F128::Abs(axis), extent);
2451  D = F128::SetValue<3>(axis, each_select32);
2452  f128 aabb_min = F128::Sub(D, N);
2453  f128 aabb_max = F128::Add(D, N);
2454  dot0 = Vector4::Dot4(axis, corners[0], corners[1], corners[2], corners[3]);
2455  dot1 = Vector4::Dot4(axis, corners[4], corners[5], corners[6], corners[7]);
2456  // false if no overlap
2457  intersect = F128::CmpGe(F128::Max(dot0, dot1), aabb_min);
2458  if (F128::IsAllMaskFalse(intersect)) return false;
2459  intersect = F128::CmpLe(F128::Min(dot0, dot1), aabb_max);
2460  if (F128::IsAllMaskFalse(intersect)) return false;
2461  }
2462  }
2463  return true;
2464 }
2465 
2466 NLIB_B Intersection::FrustumOrientedBox(const Frustum& frustum,
2467  const OrientedBox& obb) NLIB_NOEXCEPT {
2468  Frustum frstm;
2469  frustum.Transform(&frstm, 1.f, F128::Set0001(), F128::Negate(obb.center));
2470  frstm.Transform(&frstm, 1.f, Quaternion::Conjugate(obb.rotation), F128::SetZero());
2471 
2472  AxisAlignedBox aabb;
2473  aabb.point_max = obb.extent;
2474  aabb.point_min = F128::Negate(aabb.point_max);
2475  return Intersection::FrustumAxisAlignedBox(frstm, aabb);
2476 }
2477 
2478 NLIB_B Intersection::FrustumTriangle(const Frustum& frustum, SimdVectorArg triangle_point0,
2479  SimdVectorArg triangle_point1,
2480  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
2481  // the triangle to the local space of the frustum
2482  SimdQuaternion frustum_rot = frustum.rotation_;
2483  SimdVector frustum_center = frustum.center_;
2484  SimdVector pt0 = Vector3::InvRotate(F128::Sub(triangle_point0, frustum_center), frustum_rot);
2485  SimdVector pt1 = Vector3::InvRotate(F128::Sub(triangle_point1, frustum_center), frustum_rot);
2486  SimdVector pt2 = Vector3::InvRotate(F128::Sub(triangle_point2, frustum_center), frustum_rot);
2487  pt0 = F128::SetFloatToLane<3>(pt0, 1.f);
2488  pt1 = F128::SetFloatToLane<3>(pt1, 1.f);
2489  pt2 = F128::SetFloatToLane<3>(pt2, 1.f);
2490 
2491  SimdPlane near_plane = frustum.near_plane_;
2492  SimdPlane far_plane = frustum.far_plane_;
2493  SimdPlane left_plane = frustum.left_plane_;
2494  SimdPlane right_plane = frustum.right_plane_;
2495  SimdPlane top_plane = frustum.top_plane_;
2496  SimdPlane bottom_plane = frustum.bottom_plane_;
2497 
2498  f128 dist0, dist1, dist_max;
2499  f128 dist0_min, dist1_min;
2500  f128 outside;
2501 
2502  // point0
2503  dist0 = Vector4::Dot4(pt0, near_plane, far_plane, left_plane, right_plane);
2504  dist0_min = dist0;
2505  dist1 = Vector4::Dot2(pt0, top_plane, bottom_plane);
2506  dist1 = F128::Swizzle<0, 1, 0, 1>(dist1);
2507  dist1_min = dist1;
2508  dist_max = F128::Max(dist0, dist1);
2509  outside = F128::CmpGtZero(dist_max);
2510  if (F128::IsAllMaskFalse(outside)) return true; // point0 is inside the frustum
2511 
2512  // point1
2513  dist0 = Vector4::Dot4(pt1, near_plane, far_plane, left_plane, right_plane);
2514  dist0_min = F128::Min(dist0, dist0_min);
2515  dist1 = Vector4::Dot2(pt1, top_plane, bottom_plane);
2516  dist1 = F128::Swizzle<0, 1, 0, 1>(dist1);
2517  dist1_min = F128::Min(dist1, dist1_min);
2518  dist_max = F128::Max(dist0, dist1);
2519  outside = F128::CmpGtZero(dist_max);
2520  if (F128::IsAllMaskFalse(outside)) return true; // point1 is inside the frustum
2521 
2522  // point2
2523  dist0 = Vector4::Dot4(pt2, near_plane, far_plane, left_plane, right_plane);
2524  dist0_min = F128::Min(dist0, dist0_min);
2525  dist1 = Vector4::Dot2(pt2, top_plane, bottom_plane);
2526  dist1 = F128::Swizzle<0, 1, 0, 1>(dist1);
2527  dist1_min = F128::Min(dist1, dist1_min);
2528  dist_max = F128::Max(dist0, dist1);
2529  outside = F128::CmpGtZero(dist_max);
2530  if (F128::IsAllMaskFalse(outside)) return true; // point2 is inside the frustum
2531 
2532  // check if there is a plane such that all points are outside
2533  if (!F128::IsAllMaskFalse(F128::Or(
2534  F128::CmpGtZero(dist0_min), F128::CmpGtZero(dist1_min)))) return false;
2535 
2536  // All the points are outside the frustum, but the triangle may intersect
2537 
2538  // return to the world space
2539  pt0 = triangle_point0;
2540  pt1 = triangle_point1;
2541  pt2 = triangle_point2;
2542 
2543  // Axis = the normal of the triangle
2544  SimdPlane triangle = Plane::FromPoint(pt0, pt1, pt2);
2545  Float3 corners_[8];
2546  frustum.GetCorners(&corners_[0]);
2547  SimdVector corners[8];
2548  for (size_t i = 0; i < 8; ++i) {
2549  corners[i] = Vector3::LoadFloat3(&corners_[i]);
2550  corners[i] = F128::SetFloatToLane<3>(corners[i], 1.f);
2551  }
2552  f128 dot0 = Vector4::Dot4(triangle, corners[0], corners[1], corners[2], corners[3]);
2553  f128 dot1 = Vector4::Dot4(triangle, corners[4], corners[5], corners[6], corners[7]);
2554  // all the points of the frustum are inside of the triangle plane
2555  if (F128::IsAllMaskFalse(F128::CmpGeZero(F128::Max(dot0, dot1)))) return false;
2556  // all the points of the frustum are outside of the triangle plane
2557  if (F128::IsAllMaskFalse(F128::CmpLeZero(F128::Min(dot0, dot1)))) return false;
2558 
2559  // Axis = Cross(triangle edge, frustum edge)
2560  SimdVector E0[3];
2561  E0[0] = F128::Sub(pt1, pt0);
2562  E0[1] = F128::Sub(pt2, pt1);
2563  E0[2] = F128::Sub(pt0, pt2);
2564  SimdVector E1[6];
2565  E1[0] = F128::Sub(corners[0], corners[4]);
2566  E1[1] = F128::Sub(corners[1], corners[5]);
2567  E1[2] = F128::Sub(corners[2], corners[6]);
2568  E1[3] = F128::Sub(corners[3], corners[7]);
2569  E1[4] = F128::Sub(corners[1], corners[0]);
2570  E1[5] = F128::Sub(corners[1], corners[2]);
2571 
2572  for (size_t i = 0; i < 3; ++i) {
2573  for (size_t j = 0; j < 6; ++j) {
2574  SimdVector axis = Vector3::Cross(E0[i], E1[j]);
2575  axis = F128::SetZeroToLane<3>(axis);
2576  f128 tri_min, tri_max;
2577  {
2578  f128 tmp0 = Vector3::Dot(axis, pt0);
2579  f128 tmp1 = Vector3::Dot(axis, pt1);
2580  f128 tmp2 = Vector3::Dot(axis, pt2);
2581  tri_min = F128::Min(tmp0, tmp1);
2582  tri_max = F128::Max(tmp0, tmp1);
2583  tri_min = F128::Min(tri_min, tmp2);
2584  tri_max = F128::Max(tri_max, tmp2);
2585  }
2586  dot0 = Vector4::Dot4(axis, corners[0], corners[1], corners[2], corners[3]);
2587  dot1 = Vector4::Dot4(axis, corners[4], corners[5], corners[6], corners[7]);
2588  // all the points of the frustum are inside of the triangle plane
2589  if (F128::IsAllMaskFalse(F128::CmpGe(F128::Max(dot0, dot1), tri_min))) return false;
2590  // all the points of the frustum are outside of the triangle plane
2591  if (F128::IsAllMaskFalse(F128::CmpLe(F128::Min(dot0, dot1), tri_max))) return false;
2592  }
2593  }
2594  return true;
2595 }
2596 
2597 inline Intersection::PlaneResult __vectorcall
2598 Intersection::FrustumPlane(const Frustum& frustum, SimdPlaneArg plane) NLIB_NOEXCEPT {
2599  Float3 corners_[8];
2600  frustum.GetCorners(&corners_[0]);
2601  SimdVector corners[8];
2602  for (size_t i = 0; i < 8; ++i) {
2603  corners[i] = Vector3::LoadFloat3(&corners_[i]);
2604  corners[i] = F128::SetFloatToLane<3>(corners[i], 1.f);
2605  }
2606  f128 dot0 = Vector4::Dot4(plane, corners[0], corners[1], corners[2], corners[3]);
2607  f128 dot1 = Vector4::Dot4(plane, corners[4], corners[5], corners[6], corners[7]);
2608 
2609  // check if all vertices are front
2610  f128 outside = F128::CmpLeZero(F128::Min(dot0, dot1));
2611  if (F128::IsAllMaskFalse(outside)) return Intersection::PLANE_FRONT;
2612 
2613  // check if all vertices are back
2614  f128 inside = F128::CmpGeZero(F128::Max(dot0, dot1));
2615  if (F128::IsAllMaskFalse(inside)) return Intersection::PLANE_BACK;
2616 
2617  return Intersection::PLANE_INTERSECT;
2618 }
2619 
2620 NLIB_B Intersection::FrustumRay(const Frustum& frustum, SimdVectorArg ray_point,
2621  SimdVectorArg ray_dir_normalized) NLIB_NOEXCEPT {
2622  // intersects if the ray origin is inside the frustum
2623  if (Containment::FrustumPoint(frustum, ray_point)) return true;
2624 
2625  // transform ray to the local coord of the frusutum
2626  SimdVector rp = F128::Sub(ray_point, frustum.center_);
2627  rp = Vector3::InvRotate(rp, frustum.rotation_);
2628  rp = F128::SetFloatToLane<3>(rp, 1.f);
2629  SimdVector rd = Vector3::InvRotate(ray_dir_normalized, frustum.rotation_);
2630  rd = F128::SetZeroToLane<3>(rd);
2631 
2632  f128 dot_rp0, dot_rp1;
2633  dot_rp0 = Vector4::Dot4(rp, frustum.near_plane_, frustum.far_plane_,
2634  frustum.left_plane_, frustum.right_plane_);
2635  dot_rp1 = Vector4::Dot2(rp, frustum.top_plane_, frustum.bottom_plane_);
2636  dot_rp1 = F128::Swizzle<0, 1, 0, 1>(dot_rp1);
2637 
2638  f128 dot_rd0, dot_rd1;
2639  dot_rd0 = Vector4::Dot4(rd, frustum.near_plane_, frustum.far_plane_,
2640  frustum.left_plane_, frustum.right_plane_);
2641  dot_rd1 = Vector4::Dot2(rd, frustum.top_plane_, frustum.bottom_plane_);
2642  dot_rd1 = F128::Swizzle<0, 1, 0, 1>(dot_rd1);
2643 
2644  // return false if ray is parallel to plane, and origin is outside plane
2645  f128 eps = F128::SetEpsilon();
2646  f128 parallel0 = F128::CmpLe(F128::Abs(dot_rd0), eps);
2647  f128 parallel1 = F128::CmpLe(F128::Abs(dot_rd1), eps);
2648  f128 mask0 = F128::And(parallel0, F128::CmpGtZero(dot_rp0));
2649  f128 mask1 = F128::And(parallel1, F128::CmpGtZero(dot_rp1));
2650  if (!F128::IsAllMaskFalse(F128::Or(mask0, mask1))) return false;
2651 
2652  f128 t0 = F128::Negate(F128::Div(dot_rp0, dot_rd0)); // t0 > 0
2653  f128 t1 = F128::Negate(F128::Div(dot_rp1, dot_rd1)); // t1 > 0
2654  f128 inf = F128::SetInfinity();
2655 
2656  // select the nearest back to front intersection point
2657  mask0 = F128::AndNot(parallel0, F128::CmpGtZero(dot_rd0)); // true if back -> front
2658  mask1 = F128::AndNot(parallel1, F128::CmpGtZero(dot_rd1)); // true if back -> front
2659  f128 to = F128::Min(F128::Select(mask0, t0, inf), F128::Select(mask1, t1, inf));
2660  to = F128::PairwiseMin(to, to);
2661  to = F128::PairwiseMin(to, to);
2662 
2663  // select the farthest front to back intersection point
2664  inf = F128::Negate(inf);
2665  mask0 = F128::AndNot(parallel0, F128::CmpLtZero(dot_rd0)); // true if front -> back
2666  mask1 = F128::AndNot(parallel1, F128::CmpLtZero(dot_rd1)); // true if front -> back
2667  f128 from = F128::Max(F128::Select(mask0, t0, inf), F128::Select(mask1, t1, inf));
2668  from = F128::PairwiseMax(from, from);
2669  from = F128::PairwiseMax(from, from);
2670 
2671  float n = F128::GetFloatFromLane<0>(from);
2672  float f = F128::GetFloatFromLane<0>(to);
2673  return n <= f && (n >= 0.f || f >= 0.f);
2674 }
2675 
2676 #ifdef _MSC_VER
2677 # pragma endregion Intersection function implementation
2678 #endif
2679 
2680 //
2681 // Containment
2682 //
2683 #ifdef _MSC_VER
2684 # pragma region Containment
2685 #endif
2686 
2687 // true if point in sphere
2688 // BoundingSphere::Contains()
2689 NLIB_B Containment::SpherePoint(SimdSphereArg sphere, SimdVectorArg point) NLIB_NOEXCEPT {
2690  f128 dist_sq = Vector3::LengthSq(F128::Sub(sphere, point));
2691  f128 rsq = F128::SetValue<3>(F128::Mult(sphere, sphere), each_select32);
2692  return F128::IsAllMaskFalse(F128::CmpGt(dist_sq, rsq));
2693 }
2694 
2695 // true if triangle in sphere
2696 // BoundingSphere::Contains()
2697 NLIB_B Containment::SphereTriangle(SimdSphereArg sphere, SimdVectorArg triangle_point0,
2698  SimdVectorArg triangle_point1,
2699  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
2700  f128 r_sq = F128::SetValue<3>(sphere, each_select32);
2701  SimdVector tp0_c = F128::Sub(triangle_point0, sphere);
2702  SimdVector tp1_c = F128::Sub(triangle_point1, sphere);
2703  SimdVector tp2_c = F128::Sub(triangle_point2, sphere);
2704  r_sq = F128::Mult(r_sq, r_sq);
2705  tp0_c = Vector3::LengthSq(tp0_c);
2706  tp1_c = Vector3::LengthSq(tp1_c);
2707  tp2_c = Vector3::LengthSq(tp2_c);
2708  f128 dist_sq = F128::Max(F128::Max(tp0_c, tp1_c), tp2_c);
2709  return F128::IsAllMaskFalse(F128::CmpGt(dist_sq, r_sq));
2710 }
2711 
2712 // true if aabb in sphere
2713 // BoundingSphere::Contains()
2714 NLIB_B Containment::SphereAxisAlignedBox(SimdSphereArg sphere,
2715  const AxisAlignedBox& aabb) NLIB_NOEXCEPT {
2716  f128 r_sq = F128::SetValue<3>(sphere, each_select32);
2717  SimdVector aabb_max = F128::Sub(aabb.point_max, sphere);
2718  SimdVector aabb_min = F128::Sub(aabb.point_min, sphere);
2719  r_sq = F128::Mult(r_sq, r_sq);
2720  aabb_max = F128::Abs(aabb_max);
2721  aabb_min = F128::Abs(aabb_min);
2722  f128 dist_sq = Vector3::LengthSq(F128::Max(aabb_max, aabb_min));
2723  return F128::IsAllMaskFalse(F128::CmpGt(dist_sq, r_sq));
2724 }
2725 
2726 // true if obb in sphere
2727 // BoundingSphere::Contains()
2728 NLIB_B Containment::SphereOrientedBox(SimdSphereArg sphere, const OrientedBox& obb) NLIB_NOEXCEPT {
2729  SimdVector new_sphere = F128::Sub(sphere, obb.center);
2730  new_sphere = Vector3::InvRotate(new_sphere, obb.rotation);
2731  new_sphere = F128::Permute<0, 1, 2, 7>(new_sphere, sphere);
2732  SimdVector box_extent = obb.extent;
2733  AxisAlignedBox aabb;
2734  aabb.point_max = box_extent;
2735  aabb.point_min = F128::Negate(box_extent);
2736  return Containment::SphereAxisAlignedBox(new_sphere, aabb);
2737 }
2738 
2739 // true if contained in sphere
2740 // BoundingSphere::Contains()
2741 NLIB_B Containment::SphereSphere(SimdSphereArg sphere, SimdSphereArg contained) NLIB_NOEXCEPT {
2742  float dist = F128::GetFloatFromLane<0>(Vector3::Length(F128::Sub(sphere, contained)));
2743  float r0 = F128::GetFloatFromLane<3>(sphere);
2744  float r1 = F128::GetFloatFromLane<3>(contained);
2745  return (r0 - r1) >= dist;
2746 }
2747 
2748 // true if contained in sphere
2749 // BoundingSphere::Contains()
2750 NLIB_B Containment::SphereFrustum(SimdSphereArg sphere, const Frustum& frustum) NLIB_NOEXCEPT {
2751  Float3 corners[8];
2752  frustum.GetCorners(&corners[0]);
2753  SimdSphere sp = sphere;
2754  f128 rad_sq = F128::SetValue<3>(F128::Mult(sp, sp), each_select32);
2755  f128 d_sq;
2756  f128 inside;
2757  SimdVector pt;
2758 
2759  pt = Vector3::LoadFloat3(&corners[0]);
2760  d_sq = Vector3::LengthSq(F128::Sub(pt, sp));
2761  inside = F128::CmpLe(d_sq, rad_sq);
2762  for (size_t i = 1; i < 8; ++i) {
2763  pt = Vector3::LoadFloat3(&corners[i]);
2764  d_sq = Vector3::LengthSq(F128::Sub(pt, sp));
2765  inside = F128::And(inside, F128::CmpLe(d_sq, rad_sq));
2766  }
2767  return F128::IsAllMaskTrue(inside);
2768 }
2769 
2770 // true if point in aabb
2771 // BoundingBox::Contains()
2772 NLIB_B Containment::AxisAlignedBoxPoint(const AxisAlignedBox& aabb,
2773  SimdVectorArg point) NLIB_NOEXCEPT {
2774  f128 out = F128::Or(F128::CmpGt(point, aabb.point_max),
2775  F128::CmpLt(point, aabb.point_min));
2776  out = F128::SetZeroToLane<3>(out);
2777  return F128::IsAllMaskFalse(out);
2778 }
2779 
2780 // true if triangle in aabb
2781 // BoundingBox::Contains()
2782 NLIB_B Containment::AxisAlignedBoxTriangle(const AxisAlignedBox& aabb,
2783  SimdVectorArg triangle_point0,
2784  SimdVectorArg triangle_point1,
2785  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
2786  SimdVector pt_max, pt_min;
2787  pt_max = F128::Max(triangle_point0, triangle_point1);
2788  pt_min = F128::Min(triangle_point0, triangle_point1);
2789  pt_max = F128::Max(pt_max, triangle_point2);
2790  pt_min = F128::Min(pt_min, triangle_point2);
2791  f128 out = F128::Or(F128::CmpGt(pt_max, aabb.point_max),
2792  F128::CmpLt(pt_min, aabb.point_min));
2793  out = F128::SetZeroToLane<3>(out);
2794  return F128::IsAllMaskFalse(out);
2795 }
2796 
2797 // true if box in aabb
2798 // BoundingBox::Contains()
2799 NLIB_B Containment::AxisAlignedBoxOrientedBox(const AxisAlignedBox& aabb,
2800  const OrientedBox& obb) NLIB_NOEXCEPT {
2801  f128 zero = F128::SetZero();
2802  SimdVector box_extent = obb.extent;
2803  SimdVector P = F128::Splat<false, true, true, true>(box_extent, zero);
2804  SimdVector Q = F128::Splat<true, false, true, true>(box_extent, zero);
2805  SimdVector R = F128::Splat<true, true, false, true>(box_extent, zero);
2806  SimdQuaternion box_rotation = obb.rotation;
2807  P = Vector3::Rotate(P, box_rotation);
2808  Q = Vector3::Rotate(Q, box_rotation);
2809  R = Vector3::Rotate(R, box_rotation);
2810 
2811  SimdVector pt_min, pt_max;
2812  SimdVector v, v_neg;
2813 
2814  // (x, y, z) and (-x, -y, -z)
2815  v = F128::Add(F128::Add(P, Q), R);
2816  v_neg = F128::Negate(v);
2817  pt_min = F128::Min(v, v_neg);
2818  pt_max = F128::Max(v, v_neg);
2819 
2820  // (x, y, -z) and (-x, -y, z)
2821  v = F128::Sub(F128::Add(P, Q), R);
2822  v_neg = F128::Negate(v);
2823  pt_min = F128::Min(pt_min, v);
2824  pt_max = F128::Max(pt_max, v);
2825  pt_min = F128::Min(pt_min, v_neg);
2826  pt_max = F128::Max(pt_max, v_neg);
2827 
2828  // (x, y, -z) and (-x, -y, z)
2829  v = F128::Sub(F128::Add(P, R), Q);
2830  v_neg = F128::Negate(v);
2831  pt_min = F128::Min(pt_min, v);
2832  pt_max = F128::Max(pt_max, v);
2833  pt_min = F128::Min(pt_min, v_neg);
2834  pt_max = F128::Max(pt_max, v_neg);
2835 
2836  SimdVector box_center = obb.center;
2837  SimdVector aabb_min = F128::Sub(aabb.point_min, box_center);
2838  SimdVector aabb_max = F128::Sub(aabb.point_max, box_center);
2839  f128 out = F128::Or(F128::CmpGt(pt_max, aabb_max),
2840  F128::CmpLt(pt_min, aabb_min));
2841  out = F128::SetZeroToLane<3>(out);
2842  return F128::IsAllMaskFalse(out);
2843 }
2844 
2845 // true if contained_aabb in aabb
2846 // BoundingBox::Contains()
2847 NLIB_B Containment::AxisAlignedBoxAxisAlignedBox(
2848  const AxisAlignedBox& aabb, const AxisAlignedBox& contained) NLIB_NOEXCEPT {
2849  f128 out = F128::Or(F128::CmpGt(contained.point_max, aabb.point_max),
2850  F128::CmpLt(contained.point_min, aabb.point_min));
2851  out = F128::SetZeroToLane<3>(out);
2852  return F128::IsAllMaskFalse(out);
2853 }
2854 
2855 // true if sphere in aabb
2856 // BoundingBox::Contains()
2857 NLIB_B Containment::AxisAlignedBoxSphere(const AxisAlignedBox& aabb,
2858  SimdSphereArg sphere) NLIB_NOEXCEPT {
2859  f128 radius = F128::SetValue<3>(sphere, each_select32);
2860  SimdVector pt_min = F128::Sub(sphere, radius);
2861  SimdVector pt_max = F128::Add(sphere, radius);
2862  f128 out = F128::Or(F128::CmpGt(pt_max, aabb.point_max),
2863  F128::CmpLt(pt_min, aabb.point_min));
2864  out = F128::SetZeroToLane<3>(out);
2865  return F128::IsAllMaskFalse(out);
2866 }
2867 
2868 // true if frustum in aabb
2869 // BoundingBox::Contains()
2870 NLIB_B Containment::AxisAlignedBoxFrustum(const AxisAlignedBox& aabb,
2871  const Frustum& frustum) NLIB_NOEXCEPT {
2872  Float3 corners[8];
2873  frustum.GetCorners(&corners[0]);
2874  SimdVector frustum_max = Vector3::LoadFloat3(&corners[0]);
2875  SimdVector frustum_min = frustum_max;
2876  for (size_t i = 1; i < 8; ++i) {
2877  SimdVector tmp = Vector3::LoadFloat3(&corners[i]);
2878  frustum_max = F128::Max(tmp, frustum_max);
2879  frustum_min = F128::Min(tmp, frustum_min);
2880  }
2881  AxisAlignedBox contained;
2882  contained.point_max = frustum_max;
2883  contained.point_min = frustum_min;
2884  return Containment::AxisAlignedBoxAxisAlignedBox(aabb, contained);
2885 }
2886 
2887 // true if point in box
2888 // BoundingOrientedBox::Contains()
2889 NLIB_B Containment::OrientedBoxPoint(const OrientedBox& box, SimdVectorArg point) NLIB_NOEXCEPT {
2890  SimdVector pt = F128::Sub(point, box.center);
2891  pt = Vector3::InvRotate(pt, box.rotation);
2892  return Vector3::InBound(pt, box.extent);
2893 }
2894 
2895 // true if triangle in box
2896 // BoundingOrientedBox::Contains()
2897 NLIB_B Containment::OrientedBoxTriangle(const OrientedBox& box, SimdVectorArg triangle_point0,
2898  SimdVectorArg triangle_point1,
2899  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
2900  SimdVector center = box.center;
2901  SimdVector pt0 = F128::Sub(triangle_point0, center);
2902  SimdVector pt1 = F128::Sub(triangle_point1, center);
2903  SimdVector pt2 = F128::Sub(triangle_point2, center);
2904  SimdQuaternion rot = box.rotation;
2905  pt0 = Vector3::InvRotate(pt0, rot);
2906  pt1 = Vector3::InvRotate(pt1, rot);
2907  pt2 = Vector3::InvRotate(pt2, rot);
2908  pt0 = F128::Abs(pt0);
2909  pt1 = F128::Abs(pt1);
2910  pt2 = F128::Abs(pt2);
2911  SimdVector pt_absmax = F128::Max(pt0, pt1);
2912  pt_absmax = F128::Max(pt_absmax, pt2);
2913  return Vector3::CmpLe(pt_absmax, box.extent);
2914 }
2915 
2916 // true if aabb in box
2917 // BoundingOrientedBox::Contains()
2918 NLIB_B Containment::OrientedBoxAxisAlignedBox(const OrientedBox& box,
2919  const AxisAlignedBox& aabb) NLIB_NOEXCEPT {
2920  OrientedBox new_aabb;
2921  SimdVector aabb_max = aabb.point_max;
2922  SimdVector aabb_min = aabb.point_min;
2923  SimdVector aabb_center = F128::Add(aabb_min, aabb_max);
2924  aabb_center = F128::Mult(0.5f, aabb_center);
2925  new_aabb.extent = F128::Sub(aabb_max, aabb_center);
2926  aabb_center = F128::Sub(aabb_center, box.center);
2927  new_aabb.center = aabb_center;
2928  new_aabb.rotation = Quaternion::Inverse(box.rotation);
2929 
2930  AxisAlignedBox new_box;
2931  new_box.point_max = box.extent;
2932  new_box.point_min = F128::Negate(new_box.point_max);
2933 
2934  return Containment::AxisAlignedBoxOrientedBox(new_box, new_aabb);
2935 }
2936 
2937 // true if box_contained in box
2938 // BoundingOrientedBox::Contains()
2939 NLIB_B Containment::OrientedBoxOrientedBox(const OrientedBox& box,
2940  const OrientedBox& contained) NLIB_NOEXCEPT {
2941  // box -> aabb
2942  OrientedBox contained_obb;
2943  SimdQuaternion inv_rot = Quaternion::Inverse(box.rotation);
2944  contained_obb.extent = contained.extent;
2945  contained_obb.center = Vector3::Rotate(F128::Sub(contained.center, box.center), inv_rot);
2946  contained_obb.rotation = Quaternion::Mult(contained.rotation, inv_rot);
2947 
2948  AxisAlignedBox aabb;
2949  aabb.point_max = box.extent;
2950  aabb.point_min = F128::Negate(aabb.point_max);
2951 
2952  return Containment::AxisAlignedBoxOrientedBox(aabb, contained_obb);
2953 }
2954 
2955 // true if sphere in box
2956 // BoundingOrientedBox::Contains()
2957 NLIB_B Containment::OrientedBoxSphere(const OrientedBox& box, SimdSphereArg sphere) NLIB_NOEXCEPT {
2958  AxisAlignedBox aabb;
2959  SimdVector box_extent = box.extent;
2960  aabb.point_max = box_extent;
2961  aabb.point_min = F128::Negate(box_extent);
2962 
2963  SimdSphere new_sphere = Vector3::InvRotate(F128::Sub(sphere, box.center), box.rotation);
2964  new_sphere = F128::Permute<0, 1, 2, 7>(new_sphere, sphere);
2965 
2966  return Containment::AxisAlignedBoxSphere(aabb, new_sphere);
2967 }
2968 
2969 NLIB_B Containment::OrientedBoxFrustum(const OrientedBox& box,
2970  const Frustum& frustum) NLIB_NOEXCEPT {
2971  Frustum frstm;
2972  frustum.Transform(&frstm, 1.f, F128::Set0001(), F128::Negate(box.center));
2973  frstm.Transform(&frstm, 1.f, Quaternion::Conjugate(box.rotation), F128::SetZero());
2974  AxisAlignedBox aabb;
2975  aabb.point_max = box.extent;
2976  aabb.point_min = F128::Negate(box.extent);
2977  return Containment::AxisAlignedBoxFrustum(aabb, frstm);
2978 }
2979 
2980 NLIB_B Containment::FrustumTriangle(const Frustum& frustum, SimdVectorArg triangle_point0,
2981  SimdVectorArg triangle_point1,
2982  SimdVectorArg triangle_point2) NLIB_NOEXCEPT {
2983  SimdVector pt;
2984  f128 dot, dot1, dot2;
2985  SimdPlane near_plane = frustum.near_plane_;
2986  SimdPlane far_plane = frustum.far_plane_;
2987  SimdPlane top_plane = frustum.top_plane_;
2988  SimdPlane bottom_plane = frustum.bottom_plane_;
2989  SimdPlane left_plane = frustum.left_plane_;
2990  SimdPlane right_plane = frustum.right_plane_;
2991  SimdVector center = frustum.center_;
2992  SimdQuaternion rot = frustum.rotation_;
2993 
2994  pt = F128::Sub(triangle_point0, center);
2995  pt = Vector3::InvRotate(pt, rot);
2996  pt = F128::SetFloatToLane<3>(pt, 1.f);
2997  dot1 = Vector4::Dot4(pt, near_plane, far_plane, top_plane, bottom_plane);
2998  dot2 = Vector4::Dot2(pt, left_plane, right_plane);
2999  dot2 = F128::Swizzle<0, 1, 0, 1>(dot2);
3000  dot = F128::Max(dot1, dot2);
3001 
3002  pt = F128::Sub(triangle_point1, center);
3003  pt = Vector3::InvRotate(pt, rot);
3004  pt = F128::SetFloatToLane<3>(pt, 1.f);
3005  dot1 = Vector4::Dot4(pt, near_plane, far_plane, top_plane, bottom_plane);
3006  dot2 = Vector4::Dot2(pt, left_plane, right_plane);
3007  dot2 = F128::Swizzle<0, 1, 0, 1>(dot2);
3008  dot = F128::Max(dot, F128::Max(dot1, dot2));
3009 
3010  pt = F128::Sub(triangle_point2, center);
3011  pt = Vector3::InvRotate(pt, rot);
3012  pt = F128::SetFloatToLane<3>(pt, 1.f);
3013  dot1 = Vector4::Dot4(pt, near_plane, far_plane, top_plane, bottom_plane);
3014  dot2 = Vector4::Dot2(pt, left_plane, right_plane);
3015  dot2 = F128::Swizzle<0, 1, 0, 1>(dot2);
3016  dot = F128::Max(dot, F128::Max(dot1, dot2));
3017 
3018  f128 outside = F128::CmpGt(dot, F128::SetZero());
3019  return F128::IsAllMaskFalse(outside);
3020 }
3021 
3022 NLIB_B Containment::FrustumSphere(const Frustum& frustum, SimdSphereArg sphere) NLIB_NOEXCEPT {
3023  f128 dot = F128::SetValue<3>(sphere, each_select32);
3024  dot = F128::Negate(dot);
3025 
3026  SimdVector pt = F128::Sub(sphere, frustum.center_);
3027  pt = Vector3::InvRotate(pt, frustum.rotation_);
3028  pt = F128::SetFloatToLane<3>(pt, 1.f);
3029 
3030  f128 dot1 = Vector4::Dot4(pt, frustum.near_plane_, frustum.far_plane_, frustum.top_plane_,
3031  frustum.bottom_plane_);
3032  f128 dot2 = Vector4::Dot2(pt, frustum.left_plane_, frustum.right_plane_);
3033  dot2 = F128::Swizzle<0, 1, 0, 1>(dot2);
3034  dot1 = F128::Max(dot1, dot2);
3035  f128 outside = F128::CmpGt(dot1, dot);
3036  return F128::IsAllMaskFalse(outside);
3037 }
3038 
3039 NLIB_B Containment::FrustumAxisAlignedBox(const Frustum& frustum,
3040  const AxisAlignedBox& aabb) NLIB_NOEXCEPT {
3041  SimdVector pt_min = aabb.point_min;
3042  SimdVector pt_max = aabb.point_max;
3043  SimdVector center = F128::Mult(0.5f, F128::Add(pt_min, pt_max));
3044  center = F128::SetFloatToLane<3>(center, 1.f);
3045  SimdVector extent_neg = F128::Sub(center, pt_max);
3046  extent_neg = F128::SetZeroToLane<3>(extent_neg);
3047 
3048  // transform planes
3049  SimdQuaternion rot = frustum.rotation_;
3050  SimdVector frustum_center = frustum.center_;
3051  SimdVector N;
3052  f128 D;
3053  SimdPlane tmp;
3054 
3055  // construct the 6 planes of the frustum in the world space
3056  tmp = frustum.near_plane_;
3057  N = Vector3::Rotate(tmp, rot);
3058  D = F128::Sub(tmp, Vector3::Dot(N, frustum_center));
3059  SimdPlane near_plane = F128::Permute<0, 1, 2, 7>(N, D);
3060 
3061  tmp = frustum.far_plane_;
3062  N = Vector3::Rotate(tmp, rot);
3063  D = F128::Sub(tmp, Vector3::Dot(N, frustum_center));
3064  SimdPlane far_plane = F128::Permute<0, 1, 2, 7>(N, D);
3065 
3066  tmp = frustum.right_plane_;
3067  N = Vector3::Rotate(tmp, rot);
3068  D = F128::Sub(tmp, Vector3::Dot(N, frustum_center));
3069  SimdPlane right_plane = F128::Permute<0, 1, 2, 7>(N, D);
3070 
3071  tmp = frustum.left_plane_;
3072  N = Vector3::Rotate(tmp, rot);
3073  D = F128::Sub(tmp, Vector3::Dot(N, frustum_center));
3074  SimdPlane left_plane = F128::Permute<0, 1, 2, 7>(N, D);
3075 
3076  tmp = frustum.top_plane_;
3077  N = Vector3::Rotate(tmp, rot);
3078  D = F128::Sub(tmp, Vector3::Dot(N, frustum_center));
3079  SimdPlane top_plane = F128::Permute<0, 1, 2, 7>(N, D);
3080 
3081  tmp = frustum.bottom_plane_;
3082  N = Vector3::Rotate(tmp, rot);
3083  D = F128::Sub(tmp, Vector3::Dot(N, frustum_center));
3084  SimdPlane bottom_plane = F128::Permute<0, 1, 2, 7>(N, D);
3085 
3086  f128 dist;
3087  f128 radius_neg;
3088  f128 all_points_inside;
3089 
3090  // compute the distances from AABB's center to the planes
3091  dist = Vector4::Dot4(center, near_plane, far_plane, right_plane, left_plane);
3092  // dot4(((center, 1) + (extent0-7, 0)), Plane) <= 0
3093  radius_neg = Vector4::Dot4(extent_neg, F128::Abs(near_plane), F128::Abs(far_plane),
3094  F128::Abs(right_plane), F128::Abs(left_plane));
3095  // inside if dist + radius <= 0
3096  all_points_inside = F128::CmpLe(dist, radius_neg);
3097 
3098  dist = Vector4::Dot2(center, top_plane, bottom_plane);
3099  radius_neg = Vector4::Dot2(extent_neg, F128::Abs(top_plane), F128::Abs(bottom_plane));
3100  all_points_inside =
3101  F128::And(all_points_inside, F128::Swizzle<0, 1, 0, 1>(F128::CmpLe(dist, radius_neg)));
3102 
3103  return F128::IsAllMaskTrue(all_points_inside);
3104 }
3105 
3106 NLIB_B Containment::FrustumOrientedBox(const Frustum& frustum,
3107  const OrientedBox& box) NLIB_NOEXCEPT {
3108  Frustum frstm;
3109  // move the center of the box to the origin
3110  frustum.Transform(&frstm, 1.f, F128::Set0001(), F128::Negate(box.center));
3111  // make the obb to the aabb
3112  frstm.Transform(&frstm, 1.f, Quaternion::Conjugate(box.rotation), F128::SetZero());
3113 
3114  AxisAlignedBox aabb;
3115  aabb.point_max = box.extent;
3116  aabb.point_min = F128::Negate(box.extent);
3117  return Containment::FrustumAxisAlignedBox(frstm, aabb);
3118 }
3119 
3120 NLIB_B Containment::FrustumFrustum(const Frustum& frustum, const Frustum& contained) NLIB_NOEXCEPT {
3121  Float3 corners[8];
3122  contained.GetCorners(&corners[0]);
3123  for (size_t i = 0; i < 8; ++i) {
3124  SimdVector pt = Vector3::LoadFloat3(&corners[i]);
3125  if (!Containment::FrustumPoint(frustum, pt)) return false;
3126  }
3127  return true;
3128 }
3129 
3130 #ifdef _MSC_VER
3131 # pragma endregion Containment function implementation
3132 #endif
3133 
3134 #undef NLIB_B
3135 #undef NLIB_M
3136 
3137 #endif // NLIB_DOXYGEN
3138 
3139 } // namespace simd
3140 NLIB_NAMESPACE_END
3141 
3142 #endif // INCLUDE_NN_NLIB_SIMD_SIMDGEOMETRY_H_
SimdQuaternion rotation
A quaternion representing the orientation of the OBB.
Definition: SimdGeometry.h:91
SimdVector point_min
The minimum coordinates of the AABB. Lane 3 is ignored.
Definition: SimdGeometry.h:75
Defines a quaternion.
Defines a 4x4 matrix.
Class representing the view frustum.
Definition: SimdGeometry.h:105
Frustum() noexcept
Instantiates the object with default parameters (default constructor). Does not configure the default...
Definition: SimdGeometry.h:107
f128arg SimdVectorArg
f128arg is defined using typedef.
Definition: SimdFloat.h:4137
The type for two SIMD registers for 128-bit, single-precision, floating-point numbers.
Definition: SimdFloat.h:35
SimdVector extent
The size of the x-coordinate, y-coordinate, and z-coordinate of the OBB. All of the elements must hav...
Definition: SimdGeometry.h:90
AxisAlignedBox() noexcept
Instantiates the object with default parameters (default constructor). Does not configure the default...
Definition: SimdGeometry.h:63
#define NLIB_VIS_HIDDEN
Symbols for functions and classes are not made available outside of the library.
Definition: Platform_unix.h:60
f128arg SimdSphereArg
f128arg is defined using typedef.
Definition: SimdFloat.h:4143
Class for representing oriented bounding boxes (OBB). This class has data members to hold the center ...
Definition: SimdGeometry.h:80
SimdVector point_max
The maximum coordinates of the AABB. Lane 3 is ignored.
Definition: SimdGeometry.h:76
The class with the collection of functions that determine containment relations.
Definition: SimdGeometry.h:268
The class with the collection of static member functions that handle spheres in three-dimensional spa...
Definition: SimdGeometry.h:40
#define NLIB_F128_TRANSPOSE(row0, row1, row2, row3)
A macro for in-place matrix transposition.
Definition: SimdFloat.h:4211
constexpr const each_float_tag each_float
The tag for representing a single-precision floating-point number with an each_float_tag-type constan...
Definition: SimdFloat.h:55
f128arg SimdQuaternionArg
f128arg is defined using typedef.
Definition: SimdFloat.h:4139
The class with the collection of functions that handle planes in three-dimensional space...
Definition: SimdGeometry.h:21
f128arg SimdPlaneArg
f128arg is defined using typedef.
Definition: SimdFloat.h:4141
The class with the collection of functions that perform square-of-distance calculations.
Definition: SimdGeometry.h:134
f128 r[4]
Keeps each row of a 4x4 matrix.
Definition: SimdFloat.h:4164
The structure for keeping a 4x4 matrix.
Definition: SimdFloat.h:4148
PlaneResult
The return value types when determining intersection with a plane. The front of the plane is on the s...
Definition: SimdGeometry.h:181
f128 SimdSphere
f128 is defined using typedef. Used when handling spheres.
Definition: SimdFloat.h:4142
SimdVector center
The center coordinates of the OBB. A 3D vector.
Definition: SimdGeometry.h:89
void GetCorners(Float3 *corners) const noexcept
Gets the vertices of the frustum in world coordinates.
#define NLIB_NOEXCEPT
Defines noexcept geared to the environment, or the equivalent.
Definition: Config.h:86
OrientedBox() noexcept
Instantiates the object with default parameters (default constructor). Does not configure the default...
Definition: SimdGeometry.h:82
Defines the class and functions for SIMD computations on single-precision floating-point numbers...
The type for reading and writing three-dimensional vectors in memory. Keeps float-type x...
Definition: SimdFloat.h:4278
constexpr const each_select32_tag each_select32
The tag for representing the selection of a 32-bit lane with an each_select32_tag-type constant objec...
Definition: SimdInt.h:50
Defines a three-dimensional vector.
nlib_f128_t f128
nlib_f128_t is defined using typedef.
Definition: SimdFloat.h:58
Class for representing axis-aligned bounding boxes (AABB). The class has data members to hold the min...
Definition: SimdGeometry.h:61
Defines a four-dimensional vector.
The class with the collection of functions that determine intersections.
Definition: SimdGeometry.h:178
f128 SimdQuaternion
f128 is defined using typedef. Used when handling quaternions.
Definition: SimdFloat.h:4138
void Transform(Frustum *frustum, float scale, SimdQuaternionArg rotation, SimdVectorArg translation) const noexcept
Performs a 3D transform on an existing frustum and stores the transformed frustum in frustum...
f128 SimdPlane
f128 is defined using typedef. Used when handling planes.
Definition: SimdFloat.h:4140
f128 SimdVector
f128 is defined using typedef. Used when handling three-dimensional or four-dimensional vectors...
Definition: SimdFloat.h:4136