264 lines
11 KiB
C
264 lines
11 KiB
C
#include <physics.h>
|
|
|
|
short checkLineW( VECTOR * pointA, VECTOR * pointB, MESH * mesh ) {
|
|
long val1 = ( ( mesh->body->position.vx + mesh->body->min.vx ) - pointA->vx ) * ( pointB->vy - pointA->vy ) - ( ( mesh->body->position.vz + mesh->body->min.vy ) - pointA->vy ) * ( pointB->vx - pointA->vx ) ;
|
|
long val2 = ( ( mesh->body->position.vx + mesh->body->max.vx ) - pointA->vx ) * ( pointB->vy - pointA->vy ) - ( ( mesh->body->position.vz + mesh->body->max.vy ) - pointA->vy ) * ( pointB->vx - pointA->vx ) ;
|
|
if ( val1 > 0 && val2 > 0 ) {
|
|
// right
|
|
return 1;
|
|
}
|
|
else if ( val1 < 0 && val2 < 0 ) {
|
|
// left
|
|
return -1;
|
|
}
|
|
else if ( val1 == 0 && val2 == 0 ) {
|
|
// identical
|
|
return 0;
|
|
}
|
|
else if (
|
|
( val1 > 0 && val2 == 0 ) ||
|
|
( val1 == 0 && val2 > 0 )
|
|
) {
|
|
// right
|
|
return 1;
|
|
}
|
|
else if (
|
|
( val1 < 0 && val2 == 0 ) ||
|
|
( val1 == 0 && val2 < 0 )
|
|
) {
|
|
// left
|
|
return -1;
|
|
}
|
|
else if (
|
|
( val1 < 0 && val2 > 0 ) ||
|
|
( val1 > 0 && val2 < 0 )
|
|
) {
|
|
// intersect
|
|
return 3;
|
|
}
|
|
};
|
|
// Screen space variant
|
|
short checkLineS( VECTOR * pointA, VECTOR * pointB, MESH * mesh ) {
|
|
// FIXME : mesh->body->min.vx is not in screen space
|
|
int val1 = ( ( mesh->pos2D.vx + mesh->body->min.vx ) - pointA->vx ) * ( pointB->vy - pointA->vy ) - ( ( mesh->pos2D.vy + mesh->body->min.vy ) - pointA->vy ) * ( pointB->vx - pointA->vx ) ;
|
|
int val2 = ( ( mesh->pos2D.vx + mesh->body->max.vx ) - pointA->vx ) * ( pointB->vy - pointA->vy ) - ( ( mesh->pos2D.vy + mesh->body->max.vy ) - pointA->vy ) * ( pointB->vx - pointA->vx ) ;
|
|
if ( val1 > 0 && val2 > 0 ) {
|
|
// right
|
|
return 1;
|
|
}
|
|
else if ( val1 < 0 && val2 < 0 ) {
|
|
// left
|
|
return -1;
|
|
}
|
|
else if ( val1 == 0 && val2 == 0 ) {
|
|
// identical
|
|
return 2;
|
|
}
|
|
else if (
|
|
( val1 > 0 && val2 == 0 ) ||
|
|
( val1 == 0 && val2 > 0 )
|
|
) {
|
|
// right
|
|
return 1;
|
|
}
|
|
else if (
|
|
( val1 < 0 && val2 == 0 ) ||
|
|
( val1 == 0 && val2 < 0 )
|
|
) {
|
|
// left
|
|
return -1;
|
|
}
|
|
else if (
|
|
( val1 < 0 && val2 > 0 ) ||
|
|
( val1 > 0 && val2 < 0 )
|
|
) {
|
|
// intersect
|
|
return 3;
|
|
}
|
|
};
|
|
// Physics
|
|
VECTOR getIntCollision(BODY one, BODY two){
|
|
VECTOR d1, d2, col;
|
|
short correction = 50;
|
|
d1.vx = (one.position.vx + one.max.vx) - (two.position.vx + two.min.vx);
|
|
d1.vy = (one.position.vy + one.max.vy) - (two.position.vy + two.min.vy);
|
|
d1.vz = (one.position.vz + one.max.vz) - (two.position.vz + two.min.vz);
|
|
d2.vx = (two.position.vx + two.max.vx) - (one.position.vx - one.max.vx);
|
|
d2.vy = (two.position.vy + two.max.vy) - (one.position.vy + one.min.vy);
|
|
d2.vz = (two.position.vz + two.max.vz) - (one.position.vz - one.max.vz);
|
|
col.vx = !(d1.vx > 0 && d2.vx > 0);
|
|
col.vy = !(d1.vy > 0 && d2.vy > 0);
|
|
col.vz = !(d1.vz > 0 && d2.vz > 0);
|
|
return col;
|
|
};
|
|
VECTOR getExtCollision(BODY one, BODY two){
|
|
VECTOR d1, d2, col;
|
|
d1.vx = (one.position.vx + one.max.vx) - (two.position.vx + two.min.vx);
|
|
d1.vy = (one.position.vy + one.max.vy) - (two.position.vy + two.min.vy);
|
|
d1.vz = (one.position.vz + one.max.vz) - (two.position.vz + two.min.vz);
|
|
d2.vx = (two.position.vx + two.max.vx) - (one.position.vx + one.min.vx);
|
|
d2.vy = (two.position.vy + two.max.vy) - (one.position.vy + one.min.vy);
|
|
d2.vz = (two.position.vz + two.max.vz) - (one.position.vz + one.min.vz);
|
|
col.vx = d1.vx > 0 && d2.vx > 0;
|
|
col.vy = d1.vy > 0 && d2.vy > 0;
|
|
col.vz = d1.vz > 0 && d2.vz > 0;
|
|
return col;
|
|
};
|
|
VECTOR checkBodyCol(BODY * one, BODY * two){
|
|
VECTOR colInt;
|
|
colInt = getIntCollision( *one , *two );
|
|
// If collisiton on Y axis,
|
|
if ( !(colInt.vy) ) {
|
|
// and above plane
|
|
if ( !colInt.vx && !colInt.vz ) {
|
|
// collide
|
|
short slopeX = two->normal.vx > 0 ? 1 : -1;
|
|
//~ short slopeZ = two->normal.vz > 0 ? 1 : -1;
|
|
long o = two->max.vy - two->min.vy ;
|
|
long a = two->max.vx - two->min.vx;
|
|
long aa = (two->position.vx + two->min.vx) - (one->position.vx + one->min.vx) ;
|
|
//~ long b = two->max.vz - two->min.vz;
|
|
//~ long ab = (two->position.vz + two->min.vz) - (one->position.vz + one->min.vz) ;
|
|
// Avoid div/0
|
|
if (a){
|
|
long y = (( (slopeX * o) * ONE) / a) * aa;
|
|
// long y = (( (slopeZ * o) * ONE) / b) * ab;
|
|
// FntPrint("sly: %d", y >> 12);
|
|
if (y) {
|
|
one->position.vy = (y >> 12) - ( slopeX < 0 ? 128 : 0 );
|
|
// one->position.vy = (y >> 12) - ( slopeZ < 0 ? 64 : 0 ) ;
|
|
} else {
|
|
one->position.vy = two->max.vy - one->max.vy;
|
|
}
|
|
}
|
|
one->velocity.vy = 0;
|
|
two->velocity.vy = 0;
|
|
//~ FntPrint("col: %d %d %d %d\nY: %d, Z: %d, X: %d\nSlope: %d", o, a, aa, ab, one->position.vy, one->position.vz, one->position.vx, slopeX);
|
|
}
|
|
}
|
|
return colInt;
|
|
};
|
|
void applyAngMom(LEVEL curLvl ){
|
|
if (curLvl.propPtr->isRound){
|
|
if ( curLvl.propPtr->body->velocity.vx ) {
|
|
VECTOR L = angularMom( *curLvl.propPtr->body );
|
|
curLvl.propPtr->rot.vz -= L.vx;
|
|
}
|
|
if ( curLvl.propPtr->body->velocity.vz ) {
|
|
VECTOR L = angularMom( *curLvl.propPtr->body );
|
|
curLvl.propPtr->rot.vx -= L.vz;
|
|
}
|
|
}
|
|
};
|
|
//~ void applyAcceleration(BODY * actor, int dt){
|
|
//~ dt = 1;
|
|
//~ VECTOR acceleration = {actor->invMass * actor->gForce.vx , (actor->invMass * actor->gForce.vy) + (GRAVITY * ONE), actor->invMass * actor->gForce.vz};
|
|
//FntPrint("acc: %d %d %d\n", acceleration.vx, acceleration.vy, acceleration.vz );
|
|
//~ actor->velocity.vx += (acceleration.vx * dt) >> 12;
|
|
//~ actor->velocity.vy += (acceleration.vy * dt) >> 12;
|
|
//~ actor->velocity.vz += (acceleration.vz * dt) >> 12;
|
|
//FntPrint("acc: %d %d %d\n", acceleration.vx / ONE, acceleration.vy / ONE, acceleration.vz / ONE );
|
|
//~ actor->position.vx += (actor->velocity.vx * dt);
|
|
//~ actor->position.vy += (actor->velocity.vy * dt);
|
|
//~ actor->position.vz += (actor->velocity.vz * dt);
|
|
//FntPrint("vel: %d %d %d\n", actor->velocity.vx, actor->velocity.vy, actor->velocity.vz );
|
|
//~ };
|
|
|
|
void applyAcceleration(BODY * actor, int dt){
|
|
// Make sure no div by 0 occurs
|
|
if (dt < 1) { dt = 1; }
|
|
int dtfx = ONE/dt;
|
|
if (dtfx < 1) { dtfx = 1; }
|
|
VECTOR acceleration = {actor->invMass * actor->gForce.vx , (actor->invMass * actor->gForce.vy) + (GRAVITY * ONE), actor->invMass * actor->gForce.vz};
|
|
//~ FntPrint("acc: %d %d %d %d\n", acceleration.vx, acceleration.vy, acceleration.vz , delta);
|
|
//~ actor->velocity.vx += (acceleration.vx * dt) >> 12;
|
|
//~ actor->velocity.vy += (acceleration.vy * dt) >> 12;
|
|
//~ actor->velocity.vz += (acceleration.vz * dt) >> 12;
|
|
actor->velocity.vx += (acceleration.vx / dtfx) / 256;
|
|
actor->velocity.vy += (acceleration.vy / dtfx) / 256;
|
|
actor->velocity.vz += (acceleration.vz / dtfx) / 256;
|
|
//~ FntPrint("acc: %d %d %d\n", (acceleration.vx / (ONE/dt)) , (acceleration.vy / (ONE/dt)), (acceleration.vz / (ONE/dt)) );
|
|
//~ actor->position.vx += (actor->velocity.vx * dt);
|
|
//~ actor->position.vy += (actor->velocity.vy * dt);
|
|
//~ actor->position.vz += (actor->velocity.vz * dt);
|
|
actor->position.vx += (actor->velocity.vx );
|
|
actor->position.vy += (actor->velocity.vy );
|
|
actor->position.vz += (actor->velocity.vz );
|
|
//~ FntPrint("vel: %d %d %d\n", actor->velocity.vx, actor->velocity.vy, actor->velocity.vz );
|
|
};
|
|
//~ // https://gamedevelopment.tutsplus.com/tutorials/how-to-create-a-custom-2d-physics-engine-the-basics-and-impulse-resolution--gamedev-6331
|
|
void ResolveCollision( BODY * one, BODY * two ){
|
|
//~ FntPrint("rv: %d, %d, %d\n", one->velocity.vx, one->velocity.vy, one->velocity.vz);
|
|
// Calculate relative velocity
|
|
VECTOR rv = { subVector( one->velocity, two->velocity) };
|
|
//~ FntPrint("rv: %d, %d, %d\n", rv.vx,rv.vy,rv.vz);
|
|
// Collision normal
|
|
VECTOR normal = { subVector( two->position, one->position ) };
|
|
// Normalize collision normal
|
|
normal.vx = normal.vx > 0 ? 1 : normal.vx < 0 ? -1 : 0 ;
|
|
normal.vy = normal.vy > 256 ? 1 : normal.vy < -256 ? -1 : 0 ;
|
|
normal.vz = normal.vz > 0 ? 1 : normal.vz < 0 ? -1 : 0 ;
|
|
//~ FntPrint("norm: %d, %d, %d\n", normal.vx,normal.vy,normal.vz);
|
|
// Calculate relative velocity in terms of the normal direction
|
|
long velAlongNormal = dotProduct( rv, normal );
|
|
//~ FntPrint("velN: %d\n", velAlongNormal);
|
|
// Do not resolve if velocities are separating
|
|
if(velAlongNormal > 0)
|
|
return;
|
|
// Calculate restitution
|
|
long e = min( one->restitution, two->restitution );
|
|
//~ FntPrint("e: %d\n", e);
|
|
//~ // Calculate impulse scalar
|
|
long j = -(1 + e) * velAlongNormal * ONE;
|
|
j /= one->invMass + two->invMass;
|
|
//~ j /= ONE;
|
|
//~ FntPrint("j: %d\n", j);
|
|
// Apply impulse
|
|
applyVector(&normal, j, j, j, *=);
|
|
//~ FntPrint("Cnormal %d %d %d\n",normal.vx,normal.vy,normal.vz);
|
|
VECTOR velOne = normal;
|
|
VECTOR velTwo = normal;
|
|
applyVector(&velOne,one->invMass,one->invMass,one->invMass, *=);
|
|
applyVector(&velTwo,two->invMass,two->invMass,two->invMass, *=);
|
|
//~ FntPrint("V1 %d %d %d\n", velOne.vx/4096,velOne.vy/4096,velOne.vz/4096);
|
|
//~ FntPrint("V2 %d %d %d\n", velTwo.vx/4096,velTwo.vy/4096,velTwo.vz/4096);
|
|
applyVector(&one->velocity, velOne.vx/4096/4096, velOne.vy/4096/4096, velOne.vz/4096/4096, +=);
|
|
applyVector(&two->velocity, velTwo.vx/4096/4096, velTwo.vy/4096/4096, velTwo.vz/4096/4096, -=);
|
|
//~ FntPrint("V1 %d %d %d\n", velOne.vx/4096/4096,velOne.vy/4096/4096,velOne.vz/4096/4096);
|
|
//~ FntPrint("V2 %d %d %d\n", velTwo.vx/4096/4096,velTwo.vy/4096/4096,velTwo.vz/4096/4096);
|
|
};
|
|
VECTOR angularMom(BODY body){
|
|
// L = r * p
|
|
// p = m * v
|
|
VECTOR w = {0,0,0,0};
|
|
int r = (body.max.vx - body.min.vx) >> 1;
|
|
w.vx = (r * body.mass * body.velocity.vx) >> 2;
|
|
w.vy = (r * body.mass * body.velocity.vy) >> 2;
|
|
w.vz = (r * body.mass * body.velocity.vz) >> 2;
|
|
//~ FntPrint("v: %d, r:%d, w:%d\n", body.velocity.vz * r, r * r, w.vz);
|
|
return w;
|
|
};
|
|
u_int jump(BODY * actor, int dt){
|
|
// https://medium.com/@brazmogu/physics-for-game-dev-a-platformer-physics-cheatsheet-f34b09064558
|
|
// y = -0.5gt² + v't
|
|
// vt = sqrt( (vel.vx)² + (vel.vz)² + (vel.vy - 9,81t)² )
|
|
//~ long long velocity = patan(actor->velocity.vx, actor->velocity.vy);
|
|
//~ int y = (GRAVITY/2 * dt) + (velocity * dt );
|
|
u_int vt = psqrt( (actor->velocity.vx * actor->velocity.vx) +
|
|
(actor->velocity.vz * actor->velocity.vz) +
|
|
( (actor->velocity.vy - (GRAVITY * dt)) * (actor->velocity.vy - (GRAVITY * dt)) )
|
|
);
|
|
return vt;
|
|
};
|
|
void respawnMesh(LEVEL * level, MESH * mesh, VECTOR * rot, VECTOR * pos, NODE * node){
|
|
// Actor
|
|
if( mesh->isActor ){
|
|
level->curNode = node;
|
|
level->levelPtr = level->curNode->plane;
|
|
} else {
|
|
level->propPtr->node = node;
|
|
}
|
|
copyVector(&mesh->body->position, pos );
|
|
copyVector(&mesh->rot, rot );
|
|
};
|