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