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