ref: 2c2b67808907102d662da2cb8f44dc0eb67a9a7b
parent: 5d2ade97bde945dd2cc5b3ce47b692b433b1378b
author: MHS <gek@katherine>
date: Tue Mar 23 18:42:17 EDT 2021
Automatic commit.
--- a/include-demo/chadphys.h
+++ b/include-demo/chadphys.h
@@ -15,115 +15,13 @@
} phys_body;
typedef struct{vec3 g; //gravity
- phys_body* bodies;
+ phys_body** bodies;
f_ ms; //max speed
- uint nbodies; //number of bodies
+ int nbodies; //number of bodies
} phys_world;
-typedef struct{- phys_body** data;
- uint capacity;
-} phys_cell;
-typedef struct{- phys_cell* data;
- f_ celldim;
- uint xcells;
- uint ycells;
- uint zcells;
- vec3 offset;
-} phys_spatialhash;
-static inline phys_spatialhash spatialhash_init(uint x, uint y, uint z, f_ celldim, vec3 offset){- phys_spatialhash ret = {0};- ret.xcells = x;
- ret.ycells = y;
- ret.zcells = z;
- ret.celldim = celldim;
- ret.offset = offset;
- ret.data = calloc(1,sizeof(phys_cell) * x * y * z);
- return ret;
-}
-static inline void destroy_spatialhash(phys_spatialhash* p){- for(size_t i = 0; i < p->xcells * p->ycells * p->zcells; i++)
- if(p->data[i].data) free(p->data[i].data);
- if(p->data) free(p->data);
- p->data = NULL;
-}
-static inline uint phys_cell_insert(phys_cell* c, phys_body* b){ //1 = error.- uint inserted = 0;
- while(!inserted){- for(uint b_ = 0; b_ < c->capacity; b_++)
- if(c->data[b_]==NULL)
- {- c->data[b_] = b;
- inserted = 1;
- return 0;
- }
- phys_body** old = c->data;
-
-
- c->data = calloc(1,((c->capacity)<<1) * sizeof(phys_body*));
- if(c->data == NULL) { //The malloc failed! Wowza!- if(old)//In case we had something there...
- free(old);
- return 1;
- }
- memcpy(c->data, old, c->capacity);
- c->capacity <<= 1;
- free(old);
- }
- return 1; //Unreachable.
-}
-//update the placement of this body b in the spatial hash.
-static inline void spatialhash_clear(phys_spatialhash* h){-#pragma omp parallel for collapse(4)
- for(uint i = 0; i < h->xcells; i++)
- for(uint j = 0; j < h->ycells; j++)
- for(uint k = 0; k < h->zcells; k++)
- for(uint b_cell = 0; b_cell < h->data[i+ j*h->xcells + k*h->xcells*h->ycells].capacity; b_cell++)
- h->data[i+ j*h->xcells + k*h->xcells*h->ycells].data[b_cell] = NULL;
-}
-static inline uint spatialhash_update(phys_spatialhash* h, phys_body* b){- //Assumes that the spatial hash was cleared this frame- no duplicates.
- vec3 b1c = downv4(b->shape.c);
- f_ rad = 0;
- if(b->shape.c.d[3] == 0){- vec3 b1max = addv3(b1c,b->shape.e);
- rad = MAX(MAX(b1max.d[0],b1max.d[1]),b1max.d[2]);
- } else {- rad = b->shape.c.d[3];
- }
- rad /= h->celldim;
- b1c = addv3(h->offset, b1c);
- b1c = scalev3(1.0/h->celldim, b1c);
- ivec3 beg, end;
-#pragma omp simd
- for(int i = 0; i < 3; i++){- beg.d[i] = b1c.d[i] - rad;
- end.d[i] = b1c.d[i] + rad;
- }
-//error checking.
- for(int i = 0; i < 3; i++)
- if(beg.d[i] < 0 || end.d[i] < 0) return 1;
- if( end.d[0] > h->xcells ||
- end.d[1] > h->ycells ||
- end.d[2] > h->zcells)
- return 1;
- if( beg.d[0] > h->xcells ||
- beg.d[1] > h->ycells ||
- beg.d[2] > h->zcells)
- return 1;
-#pragma omp parallel for collapse(3)
- for(uint i = beg.d[0]; i < h->xcells && i<=end.d[0]; i++)
- for(uint j = beg.d[1]; j < h->ycells && j<=end.d[1]; j++)
- for(uint k = beg.d[2]; k < h->zcells && k<=end.d[2]; k++){- phys_cell_insert(h->data+i+ j*h->xcells + k*h->xcells*h->ycells, b);
- }
- return 0;
-}
-
-
static inline void initPhysBody(phys_body* body){ body->shape = (aabb){ .c=(vec4){.d[0] = 0,.d[1] = 0,.d[2] = 0,.d[3] = 0},@@ -169,18 +67,20 @@
penvec.d[0] *= -1;
penvec.d[1] *= -1;
penvec.d[2] *= -1;
- } else {+ }
#ifdef CHADPHYS_DEBUG
+ else { puts("\nInvalid configuration. Error.\n");-#endif
}
+#endif
if(penvec.d[3] <= 0) return; //No penetration detected, or invalid configuration.
vec3 penvecnormalized = scalev3(1.0/penvec.d[3], downv4(penvec)); //the penetration vector points into B...
- float friction = a->friction * b->friction;
+ f_ friction = a->friction * b->friction;
//We now have the penetration vector. There is a penetration.
//determine how much each should be displaced by.
//The penvec points INTO A and is of length penvec.d[3]
- float bdisplacefactor = a->mass / (a->mass + b->mass), adisplacefactor = b->mass / (a->mass + b->mass);
+ f_ bdisplacefactor = a->mass / (a->mass + b->mass);
+ f_ adisplacefactor = b->mass / (a->mass + b->mass);
vec3 comvel;
if(!(a->mass > 0)) { adisplacefactor = 0; bdisplacefactor = 1;comvel = (vec3){{0,0,0}};@@ -213,11 +113,29 @@
penvecnormalized //that direction
)
);
- b->shape.c.d[0] += displaceb.d[0];
- b->shape.c.d[1] += displaceb.d[1];
- b->shape.c.d[2] += displaceb.d[2];
+#pragma omp simd
+ for(int i = 0; i < 3; i++)
+ b->shape.c.d[i] += displaceb.d[i];
b->v = addv3(comvel, scalev3(1-friction, b_planarvel) ); //The center of mass velocity, plus a portion of coplanar velocity.
b->v = addv3(b->v, scalev3( b->bounciness, downv4(displaceb) ) );
}
+}
+
+static inline void stepPhysWorld(phys_world* world, const int collisioniter){+ for(int i = 0; i < world->nbodies; i++)
+ if(world->bodies[i]){+ phys_body* body = world->bodies[i];
+ vec3 bodypos = addv3(downv4(body->shape.c),body->v);
+ body->shape.c.d[0] = bodypos.d[0];
+ body->shape.c.d[1] = bodypos.d[1];
+ body->shape.c.d[2] = bodypos.d[2];
+ body->v = addv3(body->v, body->a);
+ body->v = addv3(body->v, world->g);
+ }
+ //Resolve collisions (if any)
+ for(int iter = 0; iter < collisioniter; iter++)
+ for(int i = 0; i < (int)(world->nbodies-1); i++)
+ for(int j = i+1; j < (int)world->nbodies; j++)
+ resolveBodies(world->bodies[i], world->bodies[j]);
}
#endif