mirror of
https://github.com/borgesdan/xn65
synced 2024-12-29 21:54:47 +01:00
104 lines
5.2 KiB
C++
104 lines
5.2 KiB
C++
#include "xna/common/collision.hpp"
|
|
|
|
namespace xna {
|
|
Vector3 Gjk::ComputeClosestPoint() {
|
|
auto num1 = 0.0f;
|
|
Vector3 zero = Vector3::Zero();
|
|
maxLengthSq = 0.0f;
|
|
|
|
for (auto bitsToIndex = Gjk::BitsToIndices[simplexBits]; bitsToIndex != 0; bitsToIndex >>= 3)
|
|
{
|
|
auto index = (bitsToIndex & 7) - 1;
|
|
auto num2 = det[simplexBits][index];
|
|
num1 += num2;
|
|
zero += y[index] * num2;
|
|
maxLengthSq = MathHelper::Max(maxLengthSq, yLengthSq[index]);
|
|
}
|
|
|
|
return zero / num1;
|
|
}
|
|
|
|
bool Gjk::UpdateSimplex(Int newIndex) {
|
|
auto yBits = simplexBits | 1 << newIndex;
|
|
auto xBits = 1 << newIndex;
|
|
for (auto sb = simplexBits; sb != 0; --sb)
|
|
{
|
|
if ((sb & yBits) == sb && IsSatisfiesRule(sb | xBits, yBits))
|
|
{
|
|
simplexBits = sb | xBits;
|
|
closestPoint = ComputeClosestPoint();
|
|
return true;
|
|
}
|
|
}
|
|
|
|
bool flag = false;
|
|
|
|
if (IsSatisfiesRule(xBits, yBits)) {
|
|
simplexBits = xBits;
|
|
closestPoint = y[newIndex];
|
|
maxLengthSq = yLengthSq[newIndex];
|
|
flag = true;
|
|
}
|
|
|
|
return flag;
|
|
}
|
|
|
|
void Gjk::UpdateDeterminant(Int xmIdx) {
|
|
auto index1 = 1 << xmIdx;
|
|
det[index1][xmIdx] = 1.0f;
|
|
auto bitsToIndex = Gjk::BitsToIndices[simplexBits];
|
|
auto num1 = bitsToIndex;
|
|
auto num2 = 0;
|
|
while (num1 != 0)
|
|
{
|
|
auto index2 = (num1 & 7) - 1;
|
|
auto num3 = 1 << index2;
|
|
auto index3 = num3 | index1;
|
|
det[index3][index2] = Vector3::Dot(edges[xmIdx][index2], y[xmIdx]);
|
|
det[index3][xmIdx] = Vector3::Dot(edges[index2][xmIdx], y[index2]);
|
|
auto num4 = bitsToIndex;
|
|
for (auto index4 = 0; index4 < num2; ++index4)
|
|
{
|
|
auto index5 = (num4 & 7) - 1;
|
|
auto num5 = 1 << index5;
|
|
auto index6 = index3 | num5;
|
|
auto index7 = Gjk::edgeLengthSq[index2][index5] < Gjk::edgeLengthSq[xmIdx][index5] ? index2 : xmIdx;
|
|
det[index6][index5] = (Gjk::det[index3][index2] * Vector3::Dot(edges[index7][index5], y[index2]) + Gjk::det[index3][xmIdx] * Vector3::Dot(edges[index7][index5], y[xmIdx]));
|
|
auto index8 = Gjk::edgeLengthSq[index5][index2] < Gjk::edgeLengthSq[xmIdx][index2] ? index5 : xmIdx;
|
|
det[index6][index2] = (Gjk::det[num5 | index1][index5] * Vector3::Dot(edges[index8][index2], y[index5]) + Gjk::det[num5 | index1][xmIdx] * Vector3::Dot(edges[index8][index2], y[xmIdx]));
|
|
auto index9 = Gjk::edgeLengthSq[index2][xmIdx] < Gjk::edgeLengthSq[index5][xmIdx] ? index2 : index5;
|
|
det[index6][xmIdx] = (Gjk::det[num3 | num5][index5] * Vector3::Dot(edges[index9][xmIdx], y[index5]) + Gjk::det[num3 | num5][index2] * Vector3::Dot(edges[index9][xmIdx], y[index2]));
|
|
num4 >>= 3;
|
|
}
|
|
num1 >>= 3;
|
|
++num2;
|
|
}
|
|
|
|
if ((simplexBits | index1) != 15)
|
|
return;
|
|
auto index10 = Gjk::edgeLengthSq[1][0] < Gjk::edgeLengthSq[2][0] ? (Gjk::edgeLengthSq[1][0] < Gjk::edgeLengthSq[3][0] ? 1 : 3) : (Gjk::edgeLengthSq[2][0] < Gjk::edgeLengthSq[3][0] ? 2 : 3);
|
|
det[15][0] = (Gjk::det[14][1] * Vector3::Dot(edges[index10][0], y[1]) + Gjk::det[14][2] * Vector3::Dot(edges[index10][0], y[2]) + Gjk::det[14][3] * Vector3::Dot(edges[index10][0], y[3]));
|
|
auto index11 = Gjk::edgeLengthSq[0][1] < Gjk::edgeLengthSq[2][1] ? (Gjk::edgeLengthSq[0][1] < Gjk::edgeLengthSq[3][1] ? 0 : 3) : (Gjk::edgeLengthSq[2][1] < Gjk::edgeLengthSq[3][1] ? 2 : 3);
|
|
det[15][1] = (Gjk::det[13][0] * Vector3::Dot(edges[index11][1], y[0]) + Gjk::det[13][2] * Vector3::Dot(edges[index11][1], y[2]) + Gjk::det[13][3] * Vector3::Dot(edges[index11][1], y[3]));
|
|
auto index12 = Gjk::edgeLengthSq[0][2] < Gjk::edgeLengthSq[1][2] ? (Gjk::edgeLengthSq[0][2] < Gjk::edgeLengthSq[3][2] ? 0 : 3) : (Gjk::edgeLengthSq[1][2] < Gjk::edgeLengthSq[3][2] ? 1 : 3);
|
|
det[15][2] = (Gjk::det[11][0] * Vector3::Dot(edges[index12][2], y[0]) + Gjk::det[11][1] * Vector3::Dot(edges[index12][2], y[1]) + Gjk::det[11][3] * Vector3::Dot(edges[index12][2], y[3]));
|
|
auto index13 = Gjk::edgeLengthSq[0][3] < Gjk::edgeLengthSq[1][3] ? (Gjk::edgeLengthSq[0][3] < Gjk::edgeLengthSq[2][3] ? 0 : 2) : (Gjk::edgeLengthSq[1][3] < Gjk::edgeLengthSq[2][3] ? 1 : 2);
|
|
det[15][3] = (Gjk::det[7][0] * Vector3::Dot(edges[index13][3], y[0]) + Gjk::det[7][1] * Vector3::Dot(edges[index13][3], y[1]) + Gjk::det[7][2] * Vector3::Dot(edges[index13][3], y[2]));
|
|
}
|
|
|
|
bool Gjk::AddSupportPoint(Vector3 const& newPoint) {
|
|
auto index1 = (Gjk::BitsToIndices[simplexBits ^ 15] & 7) - 1;
|
|
y[index1] = newPoint;
|
|
yLengthSq[index1] = newPoint.LengthSquared();
|
|
for (auto bitsToIndex = Gjk::BitsToIndices[simplexBits]; bitsToIndex != 0; bitsToIndex >>= 3)
|
|
{
|
|
auto index2 = (bitsToIndex & 7) - 1;
|
|
Vector3 vector3 = y[index2] - newPoint;
|
|
edges[index2][index1] = vector3;
|
|
edges[index1][index2] = -vector3;
|
|
edgeLengthSq[index1][index2] = edgeLengthSq[index2][index1] = vector3.LengthSquared();
|
|
}
|
|
UpdateDeterminant(index1);
|
|
return UpdateSimplex(index1);
|
|
}
|
|
} |