1
0
mirror of https://github.com/borgesdan/xn65 synced 2024-12-29 21:54:47 +01:00
2024-06-27 20:40:41 -03:00

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);
}
}