123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131 |
- #pragma once
- #include <Mathematics/DCPQuery.h>
- #include <Mathematics/LCPSolver.h>
- #include <Mathematics/AlignedBox.h>
- #include <Mathematics/Triangle.h>
- #include <Mathematics/Vector3.h>
- namespace WwiseGTE
- {
- template <typename Real>
- class DCPQuery<Real, Triangle3<Real>, AlignedBox3<Real>>
- {
- public:
- struct Result
- {
- bool queryIsSuccessful;
-
-
- Real distance, sqrDistance;
- std::array<Real, 3> triangleParameter, boxParameter;
- Vector3<Real> closestPoint[2];
-
-
- int numLCPIterations;
- };
-
-
-
- void SetMaxLCPIterations(int maxLCPIterations)
- {
- mLCP.SetMaxIterations(maxLCPIterations);
- }
- Result operator()(Triangle3<Real> const& triangle, AlignedBox3<Real> const& box)
- {
- Result result;
-
-
- Vector3<Real> K = box.max - box.min;
- Vector3<Real> V = triangle.v[0] - box.min;
- Vector3<Real> E0 = triangle.v[1] - triangle.v[0];
- Vector3<Real> E1 = triangle.v[2] - triangle.v[0];
-
- Real dotVE0 = Dot(V, E0);
- Real dotVE1 = Dot(V, E1);
- Real dotE0E0 = Dot(E0, E0);
- Real dotE0E1 = Dot(E0, E1);
- Real dotE1E1 = Dot(E1, E1);
-
-
- std::array<Real, 9> q =
- {
- -V[0], -V[1], -V[2], dotVE0, dotVE1, K[0], K[1], K[2], (Real)1
- };
- std::array<std::array<Real, 9>, 9> M;
- M[0] = { (Real)1, (Real)0, (Real)0, -E0[0], -E1[0], (Real)1, (Real)0, (Real)0, (Real)0 };
- M[1] = { (Real)0, (Real)1, (Real)0, -E0[1], -E1[1], (Real)0, (Real)1, (Real)0, (Real)0 };
- M[2] = { (Real)0, (Real)0, (Real)1, -E0[2], -E1[2], (Real)0, (Real)0, (Real)1, (Real)0 };
- M[3] = { -E0[0], -E0[1], -E0[2], dotE0E0, dotE0E1, (Real)0, (Real)0, (Real)0, (Real)1 };
- M[4] = { -E1[0], -E1[1], -E1[2], dotE0E1, dotE1E1, (Real)0, (Real)0, (Real)0, (Real)1 };
- M[5] = { (Real)-1, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0 };
- M[6] = { (Real)0, (Real)-1, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0 };
- M[7] = { (Real)0, (Real)0, (Real)-1, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0, (Real)0 };
- M[8] = { (Real)0, (Real)0, (Real)0, (Real)-1, (Real)-1, (Real)0, (Real)0, (Real)0, (Real)0 };
- std::array<Real, 9> w, z;
- if (mLCP.Solve(q, M, w, z))
- {
- result.queryIsSuccessful = true;
- result.triangleParameter[0] = (Real)1 - z[3] - z[4];
- result.triangleParameter[1] = z[3];
- result.triangleParameter[2] = z[4];
- result.closestPoint[0] = triangle.v[0] + z[3] * E0 + z[4] * E1;
- for (int i = 0; i < 3; ++i)
- {
- result.boxParameter[i] = z[i] + box.min[i];
- result.closestPoint[1][i] = result.boxParameter[i];
- }
- Vector3<Real> diff = result.closestPoint[1] - result.closestPoint[0];
- result.sqrDistance = Dot(diff, diff);
- result.distance = std::sqrt(result.sqrDistance);
- }
- else
- {
-
-
-
-
- result.queryIsSuccessful = false;
- for (int i = 0; i < 3; ++i)
- {
- result.triangleParameter[i] = (Real)0;
- result.boxParameter[i] = (Real)0;
- result.closestPoint[0][i] = (Real)0;
- result.closestPoint[1][i] = (Real)0;
- }
- result.distance = (Real)0;
- result.sqrDistance = (Real)0;
- }
- result.numLCPIterations = mLCP.GetNumIterations();
- return result;
- }
- private:
- LCPSolver<Real, 9> mLCP;
- };
- }
|