Rewrite collision capsule
This commit is contained in:
parent
00cc732bd9
commit
c4a6ef375c
|
@ -21,67 +21,82 @@ void collisionCapsuleBoundingBox(struct ColliderTypeData* typeData, struct Trans
|
|||
}
|
||||
|
||||
#define SQRT_3 0.577350269f
|
||||
#define SQRT_2 0.707106781f
|
||||
|
||||
static struct Vector2 gUnitCircle[] = {
|
||||
{1.0f, 0.0f},
|
||||
{SQRT_2, SQRT_2},
|
||||
{0.0f, 1.0f},
|
||||
{-SQRT_2, SQRT_2},
|
||||
{-1.0f, 0.0f},
|
||||
{-SQRT_2, -SQRT_2},
|
||||
{0.0f, -1.0f},
|
||||
{SQRT_2, -SQRT_2},
|
||||
};
|
||||
|
||||
#define OFFSET_IN_CIRCLE(current, amount) (((current) + (amount)) & 0x7)
|
||||
|
||||
int collisionCapsuleMinkowsiSum(void* data, struct Basis* basis, struct Vector3* direction, struct Vector3* output) {
|
||||
struct CollisionCapsule* capsule = (struct CollisionCapsule*)data;
|
||||
|
||||
float distance = fabsf(direction->x);
|
||||
output->x = direction->x > 0.0f ? capsule->radius : -capsule->radius;
|
||||
output->y = 0.0f;
|
||||
output->z = 0.0f;
|
||||
float directionY = vector3Dot(&basis->y, direction);
|
||||
|
||||
int result = direction->x > 0.0f ? 0x1 : 0x8;
|
||||
if (directionY > SQRT_2) {
|
||||
vector3Scale(&basis->y, output, capsule->radius);
|
||||
return 0xFF;
|
||||
} else if (directionY < -SQRT_2) {
|
||||
vector3Scale(&basis->y, output, -capsule->radius - capsule->extendDownward);
|
||||
return 0xFF00;
|
||||
} else {
|
||||
struct Vector2 horizontalBasis;
|
||||
|
||||
for (int i = 1; i < 3; ++i) {
|
||||
float distanceCheck = fabsf(VECTOR3_AS_ARRAY(direction)[i]);
|
||||
horizontalBasis.x = vector3Dot(&basis->x, direction);
|
||||
horizontalBasis.y = vector3Dot(&basis->z, direction);
|
||||
|
||||
if (distanceCheck > distance) {
|
||||
distance = distanceCheck;
|
||||
*output = gZeroVec;
|
||||
if (VECTOR3_AS_ARRAY(direction)[i] > 0.0f) {
|
||||
VECTOR3_AS_ARRAY(output)[i] = capsule->radius;
|
||||
result = 0x1 << i;
|
||||
} else {
|
||||
VECTOR3_AS_ARRAY(output)[i] = -capsule->radius;
|
||||
result = 0x8 << i;
|
||||
int circleIndex = 0;
|
||||
float currentDot = vector2Dot(&gUnitCircle[0], &horizontalBasis);
|
||||
|
||||
if (currentDot < 0.0f) {
|
||||
circleIndex = 4;
|
||||
currentDot = -currentDot;
|
||||
}
|
||||
|
||||
for (int i = 2; i >=1; --i) {
|
||||
int nextIndex = OFFSET_IN_CIRCLE(circleIndex, i);
|
||||
int prevIndex = OFFSET_IN_CIRCLE(circleIndex, -i);
|
||||
|
||||
float nextDot = vector2Dot(&gUnitCircle[nextIndex], &horizontalBasis);
|
||||
float prevDot = vector2Dot(&gUnitCircle[prevIndex], &horizontalBasis);
|
||||
|
||||
if (nextDot > currentDot) {
|
||||
circleIndex = nextIndex;
|
||||
currentDot = nextDot;
|
||||
}
|
||||
|
||||
if (prevDot > currentDot) {
|
||||
circleIndex = prevIndex;
|
||||
currentDot = prevDot;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float distanceCheck = fabsf(direction->x + direction->y + direction->z) * SQRT_3;
|
||||
int result;
|
||||
|
||||
if (distanceCheck > distance) {
|
||||
float scaledRadius = capsule->radius * SQRT_3;
|
||||
|
||||
result = 64;
|
||||
|
||||
if (output->x > 0.0f) {
|
||||
output->x = scaledRadius;
|
||||
result <<= 1;
|
||||
if (circleIndex == 0) {
|
||||
result = 0x81;
|
||||
} else {
|
||||
output->x = -scaledRadius;
|
||||
result = 0xC0 >> (7 - circleIndex);
|
||||
}
|
||||
|
||||
if (output->y > 0.0f) {
|
||||
output->y = scaledRadius;
|
||||
result <<= 2;
|
||||
} else {
|
||||
output->y = -scaledRadius;
|
||||
vector3Scale(&basis->x, output, gUnitCircle[circleIndex].x * capsule->radius);
|
||||
vector3AddScaled(output, &basis->z, gUnitCircle[circleIndex].y * capsule->radius, output);
|
||||
|
||||
if (directionY < 0.0f) {
|
||||
vector3AddScaled(output, &basis->y, -capsule->extendDownward, output);
|
||||
result <<= 8;
|
||||
}
|
||||
|
||||
if (output->z > 0.0f) {
|
||||
output->z = scaledRadius;
|
||||
result <<= 4;
|
||||
} else {
|
||||
output->z = -scaledRadius;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
if (direction->y < 0.0f) {
|
||||
vector3AddScaled(output, &basis->y, -capsule->extendDownward, output);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
struct ColliderCallbacks gCollisionCapsuleCallbacks = {
|
||||
|
|
Loading…
Reference in a new issue