Skip to content

Commit

Permalink
Implement (experimental) contact caching
Browse files Browse the repository at this point in the history
  • Loading branch information
@jdeokkim
jdeokkim committed Oct 2, 2024
1 parent 5647100 commit 1cd6bac
Show file tree
Hide file tree
Showing 4 changed files with 109 additions and 61 deletions.
26 changes: 13 additions & 13 deletions examples/src/basic.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,8 @@
static const Rectangle SCREEN_BOUNDS = { .width = SCREEN_WIDTH,
.height = SCREEN_HEIGHT };

static const float BOX_SIZE = 40.0f, CELL_SIZE = 2.0f,
DELTA_TIME = 1.0f / TARGET_FPS;
static const float BOX_WIDTH = 40.0f, BOX_HEIGHT = 40.0f;
static const float CELL_SIZE = 2.0f, DELTA_TIME = 1.0f / (TARGET_FPS << 1);

/* Private Variables ======================================================= */

Expand Down Expand Up @@ -96,31 +96,31 @@ int main(void) {

static void InitExample(void) {
world = frCreateWorld(frVector2ScalarMultiply(FR_WORLD_DEFAULT_GRAVITY,
2.0f),
1.0f),
CELL_SIZE);

ground = frCreateBodyFromShape(
FR_BODY_STATIC,
frVector2PixelsToUnits((frVector2) { .x = 0.5f * SCREEN_WIDTH,
.y = 0.85f * SCREEN_HEIGHT }),
frCreateRectangle((frMaterial) { .density = 1.25f, .friction = 0.5f },
frCreateRectangle((frMaterial) { .density = 1.25f, .friction = 0.75f },
frPixelsToUnits(0.75f * SCREEN_WIDTH),
frPixelsToUnits(0.1f * SCREEN_HEIGHT)));

frAddBodyToWorld(world, ground);

boxShape = frCreateRectangle((frMaterial) { .density = 1.0f,
.friction = 0.35f },
frPixelsToUnits(BOX_SIZE),
frPixelsToUnits(BOX_SIZE));
.friction = 0.75f },
frPixelsToUnits(BOX_WIDTH),
frPixelsToUnits(BOX_HEIGHT));

for (int i = 0; i < BOX_COUNT; i++) {
boxes[i] = frCreateBodyFromShape(FR_BODY_DYNAMIC,
frVector2PixelsToUnits((frVector2) {
.x = 0.5f * SCREEN_WIDTH,
.y = (0.75f * SCREEN_HEIGHT)
- (i * BOX_SIZE) }),
boxShape);
boxes[i] = frCreateBodyFromShape(
FR_BODY_DYNAMIC,
frVector2PixelsToUnits((frVector2) {
.x = 0.5f * SCREEN_WIDTH,
.y = (0.74f * SCREEN_HEIGHT) - (i * (BOX_HEIGHT + 1.0f)) }),
boxShape);

frAddBodyToWorld(world, boxes[i]);
}
Expand Down
28 changes: 17 additions & 11 deletions include/ferox.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,20 +121,26 @@ typedef bool (*frHashQueryFunc)(frContextNode ctx);

/* (From 'collision.c') ==================================================== */

/* A structure that represents the contact points of two colliding bodies. */
/* A structure that represents a contact point. */
typedef struct frContact_ {
int id;
float depth;
float timestamp;
frVector2 point;
struct {
float normalMass, normalScalar;
float tangentMass, tangentScalar;
} cache;
} frContact;

/*
A structure that represents the contact points
between two colliding bodies.
*/
typedef struct frCollision_ {
int count;
struct {
int id;
float depth;
float timestamp;
frVector2 point;
struct {
float normalMass, normalScalar;
float tangentMass, tangentScalar;
} cache;
} contacts[2];
frVector2 direction;
frContact contacts[2];
float friction, restitution;
} frCollision;

Expand Down
90 changes: 54 additions & 36 deletions src/rigid-body.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -347,7 +347,7 @@ void frApplyAccumulatedImpulses(frBody *b1, frBody *b2, frCollision *ctx) {
return;
}

frVector2 ctxTangent = { .x = ctx->direction.y, .y = -ctx->direction.x };
frVector2 ctxTangent = frVector2RightNormal(ctx->direction);

for (int i = 0; i < ctx->count; i++) {
frVector2 contactPoint = ctx->contacts[i].point;
Expand All @@ -368,6 +368,9 @@ void frApplyAccumulatedImpulses(frBody *b1, frBody *b2, frCollision *ctx) {

ctx->contacts[i].cache.normalMass = 1.0f / normalMass;

frVector2 accNormalImpulse = frVector2ScalarMultiply(
ctx->direction, ctx->contacts[i].cache.normalScalar);

relPositionCross1 = frVector2Cross(relPosition1, ctxTangent);
relPositionCross2 = frVector2Cross(relPosition2, ctxTangent);

Expand All @@ -379,7 +382,29 @@ void frApplyAccumulatedImpulses(frBody *b1, frBody *b2, frCollision *ctx) {

ctx->contacts[i].cache.tangentMass = 1.0f / tangentMass;

/* TODO: ... */
frVector2 accTangentImpulse = frVector2ScalarMultiply(
ctxTangent, ctx->contacts[i].cache.tangentScalar);

{
frVector2 accImpulse = frVector2Add(accNormalImpulse,
accTangentImpulse);

b1->mtn.velocity = frVector2Subtract(
b1->mtn.velocity,
frVector2ScalarMultiply(accImpulse, b1->mtn.inverseMass));

b1->mtn.angularVelocity -= b1->mtn.inverseInertia
* frVector2Cross(relPosition1,
accImpulse);

b2->mtn.velocity = frVector2Add(
b2->mtn.velocity,
frVector2ScalarMultiply(accImpulse, b2->mtn.inverseMass));

b2->mtn.angularVelocity += b2->mtn.inverseInertia
* frVector2Cross(relPosition2,
accImpulse);
}
}
}

Expand Down Expand Up @@ -425,7 +450,7 @@ void frResolveCollision(frBody *b1,
|| inverseDt <= 0.0f)
return;

frVector2 ctxTangent = { .x = ctx->direction.y, .y = -ctx->direction.x };
frVector2 ctxTangent = frVector2RightNormal(ctx->direction);

for (int i = 0; i < ctx->count; i++) {
frVector2 contactPoint = ctx->contacts[i].point;
Expand All @@ -435,8 +460,8 @@ void frResolveCollision(frBody *b1,
frVector2 relPosition2 = frVector2Subtract(contactPoint,
frGetBodyPosition(b2));

frVector2 relNormal1 = { .x = -relPosition1.y, .y = relPosition1.x };
frVector2 relNormal2 = { .x = -relPosition2.y, .y = relPosition2.x };
frVector2 relNormal1 = frVector2LeftNormal(relPosition1);
frVector2 relNormal2 = frVector2LeftNormal(relPosition2);

frVector2 relVelocity = frVector2Subtract(
frVector2Add(b2->mtn.velocity,
Expand All @@ -458,33 +483,19 @@ void frResolveCollision(frBody *b1,
* ctx->contacts[i].cache.normalMass;

{
if (normalScalar < 0.0f) normalScalar = 0.0f;

// TODO: ...
ctx->contacts[i].cache.normalScalar = normalScalar;
float oldNormalScalar = ctx->contacts[i].cache.normalScalar;

ctx->contacts[i].cache.normalScalar = fmaxf(0.0f,
oldNormalScalar
+ normalScalar);

normalScalar = ctx->contacts[i].cache.normalScalar
- oldNormalScalar;
}

frVector2 normalImpulse = frVector2ScalarMultiply(ctx->direction,
normalScalar);

{
b1->mtn.velocity = frVector2Subtract(
b1->mtn.velocity,
frVector2ScalarMultiply(normalImpulse, b1->mtn.inverseMass));

b1->mtn.angularVelocity -= b1->mtn.inverseInertia
* frVector2Cross(relPosition1,
normalImpulse);

b2->mtn.velocity = frVector2Add(
b2->mtn.velocity,
frVector2ScalarMultiply(normalImpulse, b2->mtn.inverseMass));

b2->mtn.angularVelocity += b2->mtn.inverseInertia
* frVector2Cross(relPosition2,
normalImpulse);
}

relVelocity = frVector2Subtract(
frVector2Add(b2->mtn.velocity,
frVector2ScalarMultiply(relNormal2,
Expand All @@ -497,34 +508,41 @@ void frResolveCollision(frBody *b1,
* ctx->contacts[i].cache.tangentMass;

{
float maxTangentScalar = fabsf(ctx->friction * normalScalar);
float maxTangentScalar = fabsf(
ctx->friction * ctx->contacts[i].cache.normalScalar);

tangentScalar = fminf(fmaxf(tangentScalar, -maxTangentScalar),
maxTangentScalar);
float oldTangentScalar = ctx->contacts[i].cache.tangentScalar;

// TODO: ...
ctx->contacts[i].cache.tangentScalar = tangentScalar;
ctx->contacts[i].cache.tangentScalar = fminf(
fmaxf(oldTangentScalar + tangentScalar, -maxTangentScalar),
maxTangentScalar);

tangentScalar = ctx->contacts[i].cache.tangentScalar
- oldTangentScalar;
}

frVector2 tangentImpulse = frVector2ScalarMultiply(ctxTangent,
tangentScalar);

{
frVector2 totalImpulse = frVector2Add(normalImpulse,
tangentImpulse);

b1->mtn.velocity = frVector2Subtract(
b1->mtn.velocity,
frVector2ScalarMultiply(tangentImpulse, b1->mtn.inverseMass));
frVector2ScalarMultiply(totalImpulse, b1->mtn.inverseMass));

b1->mtn.angularVelocity -= b1->mtn.inverseInertia
* frVector2Cross(relPosition1,
tangentImpulse);
totalImpulse);

b2->mtn.velocity = frVector2Add(
b2->mtn.velocity,
frVector2ScalarMultiply(tangentImpulse, b2->mtn.inverseMass));
frVector2ScalarMultiply(totalImpulse, b2->mtn.inverseMass));

b2->mtn.angularVelocity += b2->mtn.inverseInertia
* frVector2Cross(relPosition2,
tangentImpulse);
totalImpulse);
}
}
}
Expand Down
26 changes: 25 additions & 1 deletion src/world.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -346,7 +346,31 @@ static bool frPreStepHashQueryCallback(frContextNode ctx) {
collision.friction = entry->value.friction;
collision.restitution = entry->value.restitution;

/* TODO: ... */
for (int i = 0; i < collision.count; i++) {
int newContactIndex = i, oldContactIndex = -1;

for (int j = 0; j < entry->value.count; j++) {
int newContactId = collision.contacts[newContactIndex].id;
int oldContactId = entry->value.contacts[j].id;

if (newContactId == oldContactId) {
oldContactIndex = j;

break;
}
}

if (oldContactIndex < 0) continue;

frContact *newContact = &(collision.contacts[newContactIndex]);
frContact *oldContact = &(entry->value.contacts[oldContactIndex]);

float oldNormalScalar = oldContact->cache.normalScalar;
float oldTangentScalar = oldContact->cache.tangentScalar;

newContact->cache.normalScalar = oldNormalScalar;
newContact->cache.tangentScalar = oldTangentScalar;
}
} else {
collision.friction = 0.5f
* (frGetShapeFriction(s1)
Expand Down

0 comments on commit 1cd6bac

Please sign in to comment.