Archive/Portal64
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

Rewrite collision capsule

Browse source
This commit is contained in:
James Lambert 2023-11-27 21:57:22 -07:00
parent 00cc732bd9
commit c4a6ef375c
1 changed files with 59 additions and 44 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

103
src/physics/collision_capsule.c
View file

@ -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 = {