Improve ray-based character controller

src/game.c

@@ -5,6 +5,8 @@
 #include"resman.h"
 #include<cglm/vec3.h>
 
+#define SUBFEET(cp) (cp->capsule.radius)
+
 struct Game Game;
 
 void game_init() {
@@ -29,8 +31,8 @@ void game_init() {
 }
 
 struct CollisionPair {
-	dGeomID g1; // greater
+	uint16_t e1; // greater
-	dGeomID g2; // lesser
+	uint16_t e2; // lesser
 	uint8_t x;
 } __attribute__((packed));
 static size_t activeCollisionCount, activeCollisionCapacity;
@@ -38,22 +40,22 @@ static struct CollisionPair *activeCollisions;
 int pair_comparator(const void *a_, const void *b_) {
 	const struct CollisionPair *a = a_;
 	const struct CollisionPair *b = b_;
-	if(a->g1 == b->g1) {
+	if(a->e1 == b->e1) {
-		return (uintptr_t) a->g2 - (uintptr_t) b->g2;
+		return (intmax_t) a->e2 - (intmax_t) b->e2;
 	} else {
-		return (uintptr_t) a->g1 - (uintptr_t) b->g1;
+		return (intmax_t) a->e1 - (intmax_t) b->e1;
 	}
 }
-static int activate_pair(dGeomID g1, dGeomID g2) {
+static int activate_pair(uint16_t e1, uint16_t e2) {
 	struct CollisionPair p = {
-		.g1 = g1 > g2 ? g1 : g2,
+		.e1 = e1 > e2 ? e1 : e2,
-		.g2 = g1 > g2 ? g2 : g1,
+		.e2 = e1 > e2 ? e2 : e1,
 	};
 	
 	struct CollisionPair *peepee = bsearch(&p, activeCollisions, activeCollisionCount, sizeof(struct CollisionPair), pair_comparator);
 	
 	if(peepee) {
-		peepee->x++;
+		peepee->x = 2;
 		return TRIGGER_EV_CONTINUOUS;
 	}
 	
@@ -72,8 +74,8 @@ static int activate_pair(dGeomID g1, dGeomID g2) {
 static void tick_pairs() {
 	for(size_t i = 0; i < activeCollisionCount;) {
 		if(--activeCollisions[i].x == 0) {
-			uint16_t e1 = (uintptr_t) dGeomGetData(activeCollisions[i].g1);
+			uint16_t e1 = activeCollisions[i].e1;
-			uint16_t e2 = (uintptr_t) dGeomGetData(activeCollisions[i].g2);
+			uint16_t e2 = activeCollisions[i].e2;
 			
 			if(e1 != ENT_ID_INVALID) {
 				struct CPhysics *p1 = game_getcomponent(e1, physics);
@@ -110,6 +112,8 @@ static void contact_callback(void *data, dGeomID g1, dGeomID g2) {
 	uint16_t e1 = (uintptr_t) dGeomGetData(g1);
 	uint16_t e2 = (uintptr_t) dGeomGetData(g2);
 	
+	if(e1 != ENT_ID_INVALID && e1 == e2) return;
+	
 	dBodyID b1 = dGeomGetBody(g1);
 	dBodyID b2 = dGeomGetBody(g2);
 	
@@ -133,8 +137,6 @@ static void contact_callback(void *data, dGeomID g1, dGeomID g2) {
 		if(movingPlatform) {
 			const dReal *platvel = dBodyGetLinearVel(movingPlatform == 1 ? b1 : b2);
 			
-			//if(i == 0)printf("get %f %f %f\n", platvel[0], platvel[1], platvel[2]);
-			
 			contact[i].surface.mode |= dContactMotion1 | dContactMotion2 | dContactMotionN | dContactFDir1;
 			contact[i].surface.mode |= dContactSoftERP;
 			contact[i].surface.soft_erp = 0.9;
@@ -155,8 +157,11 @@ static void contact_callback(void *data, dGeomID g1, dGeomID g2) {
 	
 	int ghost = 0;
 	
+	if(dGeomGetCategoryBits(g1) & CATEGORY_GHOST) ghost = 1;
+	if(dGeomGetCategoryBits(g2) & CATEGORY_GHOST) ghost = 1;
+	
 	if(numc) {
-		int triggerType = activate_pair(g1, g2);
+		int triggerType = activate_pair(e1, e2);
 		
 		if(e1 != ENT_ID_INVALID) {
 			struct CPhysics *cp = game_getcomponent(e1, physics);
@@ -168,6 +173,11 @@ static void contact_callback(void *data, dGeomID g1, dGeomID g2) {
 			if(cp->dynamics & CPHYSICS_GHOST) {
 				ghost = 1;
 			}
+			
+			struct CMovement *cm = game_getcomponent(e1, movement);
+			if(cm && cm->holding != ENT_ID_INVALID && cm->holding == e2) {
+				ghost = 1;
+			}
 		}
 		
 		if(e2 != ENT_ID_INVALID) {
@@ -180,6 +190,11 @@ static void contact_callback(void *data, dGeomID g1, dGeomID g2) {
 			if(cp->dynamics & CPHYSICS_GHOST) {
 				ghost = 1;
 			}
+			
+			struct CMovement *cm = game_getcomponent(e2, movement);
+			if(cm && cm->holding != ENT_ID_INVALID && cm->holding == e1) {
+				ghost = 1;
+			}
 		}
 		
 		float friction = 1;
@@ -318,30 +333,35 @@ static void game_character_controller_raycast_handler(void *data, dGeomID g1, dG
 		
 		vec3 n = {0, -1, 0};
 		
+		struct CPhysics *cp1 = game_getcomponent(e1, physics);
+		struct CPhysics *cp2 = game_getcomponent(e2, physics);
+		
+		if((cp2 && (cp2->dynamics & CPHYSICS_GHOST)) || (cp1 && (cp1->dynamics & CPHYSICS_GHOST))) {
+			return;
+		}
+		
 		if(dGeomGetClass(g1) == dRayClass) {
+			dBodyID bid = dGeomGetBody(cp1->geom);
+			
 			struct CMovement *cm = game_getcomponent(e1, movement);
 			
-			struct CPhysics *cp2 = game_getcomponent(e2, physics);
+			cm->groundDepth = dGeomRayGetLength(g1) - contact[0].geom.depth;
-			if(cp2 && (cp2->dynamics & CPHYSICS_GHOST)) {
-				return;
-			}
 			
-			if(glm_vec3_dot(contact[0].geom.normal, n) <= -0.7) {
+			if(glm_vec3_dot(contact[0].geom.normal, n) <= -0.7 && cm->groundDepth > SUBFEET(cp1)) {
-				cm->canJump = 1;
-				glm_vec3_scale(contact[0].geom.normal, 1, cm->groundNormal);
-			}
-		} else if(dGeomGetClass(g2) == dRayClass) {
-			struct CMovement *cm = game_getcomponent(e2, movement);
-			
-			struct CPhysics *cp1 = game_getcomponent(e1, physics);
-			if(cp1 && (cp1->dynamics & CPHYSICS_GHOST)) {
-				return;
-			}
-			
-			if(glm_vec3_dot(contact[0].geom.normal, n) <= -0.7) {
 				cm->canJump = 1;
 				glm_vec3_scale(contact[0].geom.normal, -1, cm->groundNormal);
 			}
+		} else if(dGeomGetClass(g2) == dRayClass) {
+			dBodyID bid = dGeomGetBody(cp2->geom);
+			
+			struct CMovement *cm = game_getcomponent(e2, movement);
+			
+			cm->groundDepth = dGeomRayGetLength(g2) - contact[0].geom.depth;
+			
+			if(glm_vec3_dot(contact[0].geom.normal, n) >= +0.7 && cm->groundDepth > SUBFEET(cp2)) {
+				cm->canJump = 1;
+				glm_vec3_scale(contact[0].geom.normal, +1, cm->groundNormal);
+			}
 		}
 		
 	}
@@ -349,6 +369,10 @@ static void game_character_controller_raycast_handler(void *data, dGeomID g1, dG
 void game_character_controller_raycast() {
 	dGeomID rayGeoms[Game.entities.movementCount];
 	
+	for(size_t i = 0; i < Game.entities.movementCount; i++) {
+		Game.entities.movement[i].canJump = 0;
+	}
+	
 	for(int i = 0; i < Game.entities.movementCount; i++) {
 		struct CPhysics *cp = game_getcomponent(Game.entities.movement[i].entity, physics);
 		
@@ -357,9 +381,7 @@ void game_character_controller_raycast() {
 			continue;
 		}
 		
-		const float raylen = 0.5;
+		rayGeoms[i] = dCreateRay(Game.space, cp->capsule.length / 2 + cp->capsule.radius + SUBFEET(cp));
-		
-		rayGeoms[i] = dCreateRay(Game.space, raylen);
 		
 		dGeomSetData(rayGeoms[i], (void*) (uintptr_t) Game.entities.movement[i].entity);
 		
@@ -370,7 +392,7 @@ void game_character_controller_raycast() {
 		
 		const float *position = dBodyGetPosition(bid);
 		
-		dGeomRaySet(rayGeoms[i], position[0], position[1] - cp->capsule.length / 2 - cp->capsule.radius + raylen / 2, position[2], 0, -1, 0);
+		dGeomRaySet(rayGeoms[i], position[0], position[1], position[2], 0, -1, 0);
 	}
 	
 	dSpaceCollide(Game.space, NULL, game_character_controller_raycast_handler);
@@ -430,9 +452,18 @@ void game_update() {
 		if(cp && cp->geom) {
 			dBodyID bid = dGeomGetBody(cp->geom);
 			if(Game.entities.movement[i].canJump) {
-				dBodySetLinearVel(bid, 0, 0, 0);
+				if(dBodyGetLinearVel(bid)[1] <= 0) {
+					Game.entities.movement[i].isJumping = false;
+				}
+				
+				if(!Game.entities.movement[i].isJumping) {
+					const float *pos = dBodyGetPosition(bid);
+					dBodySetPosition(bid, pos[0], pos[1] + Game.entities.movement[i].groundDepth - cp->capsule.radius / 2 - SUBFEET(cp), pos[2]);
+					
+					dBodySetLinearVel(bid, 0, 0, 0);
+				}
 			}
-			dBodySetGravityMode(bid, !Game.entities.movement[i].canJump);
+			dBodySetGravityMode(bid, Game.entities.movement[i].canJump && !Game.entities.movement[i].isJumping ? 0 : 1);
 		}
 	}
 	
@@ -468,14 +499,21 @@ void game_update() {
 					Game.entities.physics[i].box.l);
 				
 				break;
-			case CPHYSICS_CAPSULE:
+			case CPHYSICS_CAPSULE: {
-				Game.entities.physics[i].geom = dCreateCapsule(Game.space,
+				dGeomID top = dCreateCapsule(Game.space,
 					Game.entities.physics[i].capsule.radius,
-					Game.entities.physics[i].capsule.length);
+					Game.entities.physics[i].capsule.length - Game.entities.physics[i].capsule.radius);
+					
+				dGeomID feet = dCreateSphere(Game.space,
+					Game.entities.physics[i].capsule.radius);
+				
+				Game.entities.physics[i].geom = top;
+				Game.entities.physics[i].geom2 = feet;
 				
 				dMassSetCapsuleTotal(&mass, Game.entities.physics[i].mass, 2, Game.entities.physics[i].capsule.radius, Game.entities.physics[i].capsule.length);
 				
 				break;
+			}
 			case CPHYSICS_SPHERE:
 				Game.entities.physics[i].geom = dCreateSphere(Game.space,
 					Game.entities.physics[i].sphere.radius);
@@ -489,6 +527,12 @@ void game_update() {
 			dGeomSetCollideBits(Game.entities.physics[i].geom, Game.entities.physics[i].collide);
 			dGeomSetData(Game.entities.physics[i].geom, (void*) Game.entities.physics[i].entity);
 			
+			if(Game.entities.physics[i].type == CPHYSICS_CAPSULE) {
+				dGeomSetCategoryBits(Game.entities.physics[i].geom2, CATEGORY_ENTITY | CATEGORY_GHOST);
+				dGeomSetCollideBits(Game.entities.physics[i].geom2, Game.entities.physics[i].collide);
+				dGeomSetData(Game.entities.physics[i].geom2, (void*) Game.entities.physics[i].entity);
+			}
+			
 			if((Game.entities.physics[i].dynamics & ~CPHYSICS_GHOST) != CPHYSICS_STATIC) {
 				dBodyID body = dBodyCreate(Game.phys);
 				
@@ -499,10 +543,16 @@ void game_update() {
 				}
 				
 				dGeomSetBody(Game.entities.physics[i].geom, body);
+				if(Game.entities.physics[i].geom2) {
+					dGeomSetBody(Game.entities.physics[i].geom2, body);
+				}
 				
 				if(Game.entities.physics[i].type == CPHYSICS_CAPSULE) {
 					dBodySetMaxAngularSpeed(body, 0);
 					
+					dGeomSetOffsetPosition(Game.entities.physics[i].geom, 0, Game.entities.physics[i].capsule.radius / 2, 0);
+					dGeomSetOffsetPosition(Game.entities.physics[i].geom2, 0, -Game.entities.physics[i].capsule.length / 2, 0);
+					
 					// Rotate to Y-up
 					dQuaternion q;
 					dQFromAxisAndAngle(q, 1, 0, 0, M_PI * 0.5);
@@ -589,23 +639,19 @@ void game_update() {
 			dQFromAxisAndAngle(q, 0, 1, 0, Game.entities.movement[mi].pointing + M_PI);
 			dBodySetQuaternion(bid, q);
 			
-			if(Game.entities.movement[mi].jump && (0||Game.entities.movement[mi].canJump)) {
+			if(Game.entities.movement[mi].jump && Game.entities.movement[mi].canJump) {
-				Game.entities.movement[mi].canJump = 0;
+				dVector3 force = {};
-				
+				dWorldImpulseToForce(Game.phys, 1.f / GAME_TPS, 0, 6.5, 0, force);
-				dVector3 force;
-				dWorldImpulseToForce(Game.phys, 1.f / GAME_TPS, 0, 5, 0, force);
 				
 				if(bid) {
 					dBodyAddForce(bid, force[0], force[1], force[2]);
 				}
+				
+				Game.entities.movement[mi].isJumping = true;
 			}
 		}
 	}
 	
-	for(size_t i = 0; i < Game.entities.movementCount; i++) {
-		Game.entities.movement[i].canJump = 0;
-	}
-	
 	dSpaceCollide(Game.space, 0, &contact_callback);
 	
 	tick_pairs();
@@ -955,10 +1001,14 @@ void game_killentity(uint16_t eid) {
 	if(cp) {
 		dBodyID bid = dGeomGetBody(cp->geom);
 		
-		dGeomDestroy(cp->geom);
-		
 		if(bid) {
+			for(dGeomID gid = dBodyGetFirstGeom(bid); gid; gid = dBodyGetNextGeom(gid)) {
+				dGeomDestroy(gid);
+			}
+			
 			dBodyDestroy(bid);
+		} else {
+			dGeomDestroy(cp->geom);
 		}
 		
 		game_killcomponent_ptr(cp, physics);

src/game.h

@@ -13,6 +13,7 @@
 #define CATEGORY_STATIC 1
 #define CATEGORY_RAY 2
 #define CATEGORY_ENTITY 4
+#define CATEGORY_GHOST 8
 
 #define TRIGGER_INVALID 0
 
@@ -47,7 +48,7 @@ struct TrimeshData {
 #define CPHYSICS_GHOST 128
 struct CPhysics {
 	uint16_t entity;
-	dGeomID geom;
+	dGeomID geom, geom2;
 	uint16_t trigger;
 	
 	uint8_t type;
@@ -87,8 +88,9 @@ struct CMovement {
 	uint16_t entity;
 	vec3 dir;
 	float pointing;
-	char jump, canJump;
+	char jump, canJump, isJumping;
 	vec3 groundNormal;
+	float groundDepth;
 	uint16_t holding;
 };
 

src/main.c

@@ -523,11 +523,16 @@ int main(int argc_, char **argv_) {
 			InstantCamShift--;
 		}
 		if(LuaapiFirstPerson) {
-			struct CRender *c = game_getcomponent(Game.spectated, render);
+			struct CRender *cr = game_getcomponent(Game.spectated, render);
+			struct CPhysics *cp = game_getcomponent(Game.spectated, physics);
 			
-			if(c) {
+			if(cr) {
 				vec3 p;
-				glm_vec3_lerp(c->posLast, c->pos, alpha, p);
+				glm_vec3_lerp(cr->posLast, cr->pos, alpha, p);
+				
+				if(cp && cp->type == CPHYSICS_CAPSULE) {
+					p[1] += cp->capsule.length / 2;
+				}
 				
 				mat4 view;
 				glm_look(p, cameraForwardDir, (vec3) {0, 1, 0}, view);