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