diff --git a/src/base/base_math.c b/src/base/base_math.c index f5a96b7e..722ad4ec 100644 --- a/src/base/base_math.c +++ b/src/base/base_math.c @@ -40,6 +40,19 @@ u64 SaturateU64(u64 v) { return v < 0 ? 0 : v > 1 ? 1 : v; } i64 SaturateI64(i64 v) { return v < 0 ? 0 : v > 1 ? 1 : v; } f64 SaturateF64(f64 v) { return v < 0 ? 0 : v > 1 ? 1 : v; } +//////////////////////////////////////////////////////////// +//~ Float ops + +i32 SignF32(f32 v) +{ + return (v >= 0) - (v < 0); +} + +i32 SignF64(f64 v) +{ + return (v >= 0) - (v < 0); +} + //////////////////////////////////////////////////////////// //~ Exponential ops diff --git a/src/base/base_math.h b/src/base/base_math.h index 960ea2be..1a4c309a 100644 --- a/src/base/base_math.h +++ b/src/base/base_math.h @@ -226,7 +226,7 @@ i64 SaturateI64(i64 v); f64 SaturateF64(f64 v); //////////////////////////////////////////////////////////// -//~ Float helpers +//~ Float ops //- Round #define RoundF32(v) roundf(v) @@ -252,6 +252,10 @@ f64 SaturateF64(f64 v); #define AbsF32(v) fabsf(v) #define AbsF64(v) fabs(v) +//- Sign +i32 SignF32(f32 v); +i32 SignF64(f64 v); + //////////////////////////////////////////////////////////// //~ Exponential ops diff --git a/src/pp/pp_sim/pp_sim_core.c b/src/pp/pp_sim/pp_sim_core.c index 88fc5210..1416d03d 100644 --- a/src/pp/pp_sim/pp_sim_core.c +++ b/src/pp/pp_sim/pp_sim_core.c @@ -931,33 +931,169 @@ S_CollisionResult S_CollisionResultFromShapes(S_Shape shape0, S_Shape shape1, Ve return result; } -S_RaycastResult S_RaycastShape(S_Shape shape, Vec2 ray_start, Vec2 ray_dir, b32 extend) +S_RaycastResult S_RaycastShape(S_Shape shape, Vec2 ray_start, Vec2 ray_dir) { - S_RaycastResult result = Zi; + f32 radius = shape.radius; - // HACK: Create line shape as large as the world and perform a collision test - Vec2 ray_shape_p0 = ray_start; - Vec2 ray_shape_p1; - if (extend) + f32 ray_len = Vec2Len(ray_dir); + Vec2 ray_dir_norm = DivVec2(ray_dir, ray_len); + + Vec2 s = ray_start; + Vec2 e = AddVec2(ray_start, ray_dir_norm); + Vec2 vse = ray_dir_norm; + + Vec2 isect = Zi; + Vec2 isect_normal = Zi; + b32 isect_is_round = 0; + b32 isect_found = 0; + if (shape.points_count > 1) { - ray_shape_p1 = AddVec2(ray_start, MulVec2(NormVec2(ray_dir), S_WorldPitch * 1.414213562)); + // Find expanded line intersections with ray + for (i32 p_idx = 0; p_idx < shape.points_count && !isect_found; ++p_idx) + { + Vec2 a = Zi; + Vec2 b = Zi; + Vec2 vab = Zi; + Vec2 normal = Zi; + { + i32 a_idx = p_idx; + i32 b_idx = a_idx + 1; + if (b_idx >= shape.points_count) + { + b_idx = 0; + } + + Vec2 a_orig = shape.points[a_idx]; + Vec2 b_orig = shape.points[b_idx]; + vab = SubVec2(b_orig, a_orig); + normal = NegVec2(PerpVec2(NormVec2(vab))); + Vec2 radius_add = MulVec2(normal, radius); + + a = AddVec2(a_orig, radius_add); + b = AddVec2(b_orig, radius_add); + } + Vec2 vsa = SubVec2(a, s); + Vec2 vsb = SubVec2(b, s); + f32 wa = WedgeVec2(vse, vsa); + f32 wb = WedgeVec2(vse, vsb); + if (wa > 0 && wb < 0) + { + f32 t = -wa / (wb - wa); + isect = AddVec2(a, MulVec2(vab, t)); + isect_normal = normal; + isect_found = 1; + } + } + + // Find closest rounded corner + if (!isect_found && radius != 0) + { + isect_is_round = 1; + for (i32 f_idx = 0; f_idx < shape.points_count && !isect_found; ++f_idx) + { + Vec2 f_orig = shape.points[f_idx]; + Vec2 a = Zi; + Vec2 b = Zi; + Vec2 vab = Zi; + { + i32 prev_idx = f_idx - 1; + i32 next_idx = f_idx + 1; + if (prev_idx < 0) + { + prev_idx = shape.points_count - 1; + } + if (next_idx >= shape.points_count) + { + next_idx = 0; + } + Vec2 prev_orig = shape.points[prev_idx]; + Vec2 next_orig = shape.points[next_idx]; + Vec2 vpf = SubVec2(f_orig, prev_orig); + Vec2 vfn = SubVec2(next_orig, f_orig); + Vec2 vpf_norm = NormVec2(vpf); + Vec2 vfn_norm = NormVec2(vfn); + Vec2 radius_add_a = MulVec2(PerpVec2(vpf_norm), -radius); + Vec2 radius_add_b = MulVec2(PerpVec2(vfn_norm), -radius); + a = AddVec2(f_orig, radius_add_a); + b = AddVec2(f_orig, radius_add_b); + } + Vec2 vsa = SubVec2(a, s); + Vec2 vsb = SubVec2(b, s); + f32 wa = WedgeVec2(vse, vsa); + f32 wb = WedgeVec2(vse, vsb); + if (wa > 0 && wb < 0) + { + isect = f_orig; + isect_found = 1; + } + } + } + + // Find closest corner + if (!isect_found) + { + f32 min_dist = Inf; + for (i32 p_idx = 0; p_idx < shape.points_count && !isect_found; ++p_idx) + { + Vec2 p = shape.points[p_idx]; + f32 dist = AbsF32(WedgeVec2(vse, SubVec2(p, s))); + if (dist < min_dist) + { + isect = p; + min_dist = dist; + } + } + } + } + else if (shape.points_count == 1 && radius != 0) + { + isect = shape.points[0]; + isect_is_round = 1; + } + + + // Find round intersection + b32 is_intersecting = 0; + if (isect_is_round || !isect_found) + { + Vec2 vsi = SubVec2(isect, s); + + f32 dot = DotVec2(vse, vsi); + f32 wedge = WedgeVec2(vse, vsi); + + is_intersecting = AbsF32(wedge) < radius; + if (is_intersecting) + { + f32 diff = SqrtF32(radius * radius - wedge * wedge); + f32 entrance_t = dot - diff; + f32 exit_t = dot + diff; + { + Vec2 old_isect = isect; + isect = AddVec2(s, MulVec2(vse, entrance_t)); + isect_normal = NormVec2(SubVec2(isect, old_isect)); + } + } } else { - ray_shape_p1 = AddVec2(ray_shape_p0, ray_dir); + is_intersecting = isect_found; } - S_Shape ray_shape = S_ShapeFromDesc(.count = 2, .points = { ray_shape_p0, ray_shape_p1 }); - S_DebugDrawShape(ray_shape, Color_Cyan); - - S_CollisionResult cls = S_CollisionResultFromShapes(ray_shape, shape, ray_dir); - result.is_intersecting = cls.collision_points_count > 0; - result.normal = NegVec2(cls.collision_normal); - result.p = cls.closest_p1; + S_RaycastResult result = Zi; + result.is_intersecting = is_intersecting; + if (is_intersecting) + { + result.p = isect; + result.normal = isect_normal; + } return result; } + + + + //////////////////////////////////////////////////////////// //~ Lookup helpers @@ -1356,91 +1492,91 @@ void S_TickForever(WaveLaneCtx *lane) ////////////////////////////// //- Solve constraints - for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx) - { - S_Constraint *constraint = &constraints[constraint_idx]; + // for (i64 constraint_idx = 0; constraint_idx < constraints_count; ++constraint_idx) + // { + // S_Constraint *constraint = &constraints[constraint_idx]; - S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); - S_Ent *ent1 = S_EntFromKey(world, constraint->ent1); + // S_Ent *ent0 = S_EntFromKey(world, constraint->ent0); + // S_Ent *ent1 = S_EntFromKey(world, constraint->ent1); - Vec2 old_dv0 = ent0->solved_dv; - Vec2 old_dv1 = ent1->solved_dv; + // Vec2 old_dv0 = ent0->solved_dv; + // Vec2 old_dv1 = ent1->solved_dv; - Vec2 dv0 = VEC2(0, 0); - Vec2 dv1 = VEC2(0, 0); - f32 dw0 = 0; - f32 dw1 = 0; - { - // // Get shapes - // S_Shape shape0 = S_ShapeFromDesc(.count = 1); - // S_Shape shape1 = S_ShapeFromDesc(.count = 1); - // if (ent0->active) - // { - // shape0 = S_MulXformShape(ent0->xf, ent0->local_shape); - // } - // if (ent1->active) - // { - // shape1 = S_MulXformShape(ent1->xf, ent1->local_shape); - // } + // Vec2 dv0 = VEC2(0, 0); + // Vec2 dv1 = VEC2(0, 0); + // f32 dw0 = 0; + // f32 dw1 = 0; + // { + // // // Get shapes + // // S_Shape shape0 = S_ShapeFromDesc(.count = 1); + // // S_Shape shape1 = S_ShapeFromDesc(.count = 1); + // // if (ent0->active) + // // { + // // shape0 = S_MulXformShape(ent0->xf, ent0->local_shape); + // // } + // // if (ent1->active) + // // { + // // shape1 = S_MulXformShape(ent1->xf, ent1->local_shape); + // // } - S_Shape shape0 = constraint->shape0; - S_Shape shape1 = constraint->shape1; + // S_Shape shape0 = constraint->shape0; + // S_Shape shape1 = constraint->shape1; - S_DebugDrawShape(shape1, Color_Orange); + // S_DebugDrawShape(shape1, Color_Orange); - // TODO: Real dir - Vec2 shape_dir = NormVec2(SubVec2(shape1.centroid, shape0.centroid)); - Vec2 neg_shape_dir = NegVec2(shape_dir); + // // TODO: Real dir + // Vec2 shape_dir = NormVec2(SubVec2(shape1.centroid, shape0.centroid)); + // Vec2 neg_shape_dir = NegVec2(shape_dir); - // TODO: Real relative velocity - Vec2 rel_vel = SubVec2(old_dv1, old_dv0); + // // TODO: Real relative velocity + // Vec2 rel_vel = SubVec2(old_dv1, old_dv0); - // Vec2 normal = NormVec2(rel_vel); - // Vec2 neg_normal = NegVec2(normal); - Vec2 normal = NormVec2(shape_dir); - Vec2 neg_normal = NegVec2(normal); + // // Vec2 normal = NormVec2(rel_vel); + // // Vec2 neg_normal = NegVec2(normal); + // Vec2 normal = NormVec2(shape_dir); + // Vec2 neg_normal = NegVec2(normal); - // Vec2 shape0_pt = S_SupportPointFromShape(shape0, shape_dir); - // Vec2 shape1_pt = S_SupportPointFromShape(shape1, neg_shape_dir); + // // Vec2 shape0_pt = S_SupportPointFromShape(shape0, shape_dir); + // // Vec2 shape1_pt = S_SupportPointFromShape(shape1, neg_shape_dir); - S_CollisionResult collision_data = S_CollisionResultFromShapes(shape0, shape1, VEC2(0, 0)); - Vec2 shape0_pt = collision_data.closest_p0; - Vec2 shape1_pt = collision_data.closest_p1; + // S_CollisionResult collision_data = S_CollisionResultFromShapes(shape0, shape1, VEC2(0, 0)); + // Vec2 shape0_pt = collision_data.closest_p0; + // Vec2 shape1_pt = collision_data.closest_p1; - Vec2 sep = SubVec2(shape1_pt, shape0_pt); - f32 sep_along_normal = DotVec2(sep, normal); + // Vec2 sep = SubVec2(shape1_pt, shape0_pt); + // f32 sep_along_normal = DotVec2(sep, normal); - f32 rel_vel_along_normal = DotVec2(rel_vel, normal); + // f32 rel_vel_along_normal = DotVec2(rel_vel, normal); - // f32 sep_normal = DotVec2(sep, normal); + // // f32 sep_normal = DotVec2(sep, normal); - S_DebugDrawPoint(shape0_pt, Color_Cyan); - S_DebugDrawPoint(shape1_pt, Color_Cyan); + // S_DebugDrawPoint(shape0_pt, Color_Cyan); + // S_DebugDrawPoint(shape1_pt, Color_Cyan); - S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); - S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); + // S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); + // S_DebugDrawPoint(collision_data.collision_points[0].p, Color_Red); - if (sep_along_normal < 0) - { - dv0 = AddVec2(dv0, MulVec2(normal, sep_along_normal)); - } - // dv0 = VEC2(sep_normal, sep_normal); + // if (sep_along_normal < 0) + // { + // dv0 = AddVec2(dv0, MulVec2(normal, sep_along_normal)); + // } + // // dv0 = VEC2(sep_normal, sep_normal); - // f32 separation = constraint->sep; - } + // // f32 separation = constraint->sep; + // } - // Update solved velocity - if (ent0->valid) - { - ent0->solved_dv = AddVec2(ent0->solved_dv, dv0); - ent0->solved_dw += dw0; - } - if (ent1->valid) - { - ent1->solved_dv = AddVec2(ent1->solved_dv, dv1); - ent1->solved_dw += dw1; - } - } + // // Update solved velocity + // if (ent0->valid) + // { + // ent0->solved_dv = AddVec2(ent0->solved_dv, dv0); + // ent0->solved_dw += dw0; + // } + // if (ent1->valid) + // { + // ent1->solved_dv = AddVec2(ent1->solved_dv, dv1); + // ent1->solved_dw += dw1; + // } + // } ////////////////////////////// //- Integrate velocities @@ -1518,61 +1654,64 @@ void S_TickForever(WaveLaneCtx *lane) for (S_Ent *bullet = S_FirstEnt(world); bullet->valid; bullet = S_NextEnt(bullet)) { - bullet->has_hit = 0; - Vec2 ray_start = bullet->bullet_start; - Vec2 ray_end = bullet->bullet_end; - Vec2 ray_dir = SubVec2(ray_end, ray_start); - - // TODO: Real raycast query - S_Ent *closest_victim = &S_NilEnt; - S_RaycastResult victim_raycast = Zi; + if (bullet->is_bullet) { - f32 closest_len_sq = Inf; - for (S_Ent *victim = S_FirstEnt(world); victim->valid; victim = S_NextEnt(victim)) - { - if (victim->is_player && !S_MatchKey(victim->key, bullet->bullet_firer)) - { - Xform victim_xf = victim->xf; - S_Shape victim_world_shape = S_MulXformShape(victim_xf, victim->local_shape); + bullet->has_hit = 0; + Vec2 ray_start = bullet->bullet_start; + Vec2 ray_end = bullet->bullet_end; + Vec2 ray_dir = SubVec2(ray_end, ray_start); - S_RaycastResult raycast = S_RaycastShape(victim_world_shape, ray_start, ray_dir, 0); - if (raycast.is_intersecting) + // TODO: Real raycast query + S_Ent *closest_victim = &S_NilEnt; + S_RaycastResult victim_raycast = Zi; + { + f32 closest_len_sq = Inf; + for (S_Ent *victim = S_FirstEnt(world); victim->valid; victim = S_NextEnt(victim)) + { + if (victim->is_player && !S_MatchKey(victim->key, bullet->bullet_firer)) { - f32 len_sq = Vec2LenSq(SubVec2(raycast.p, ray_start)); - if (len_sq < closest_len_sq) + Xform victim_xf = victim->xf; + S_Shape victim_world_shape = S_MulXformShape(victim_xf, victim->local_shape); + + S_RaycastResult raycast = S_RaycastShape(victim_world_shape, ray_start, ray_dir); + if (raycast.is_intersecting) { - closest_len_sq = len_sq; - closest_victim = victim; - victim_raycast = raycast; + Vec2 hit = raycast.p; + Vec2 dir_to_hit = SubVec2(hit, ray_start); + if (DotVec2(ray_dir, dir_to_hit) <= 0 || Vec2LenSq(dir_to_hit) <= Vec2LenSq(ray_dir)) + { + f32 len_sq = Vec2LenSq(SubVec2(raycast.p, ray_start)); + if (len_sq < closest_len_sq) + { + closest_len_sq = len_sq; + closest_victim = victim; + victim_raycast = raycast; + } + } } } } } - } - if (closest_victim->valid) - { - bullet->has_hit = 1; - bullet->hit_entry = victim_raycast.p; - bullet->hit_entry_normal = victim_raycast.normal; - // bullet->bullet_end = bullet->hit_entry; - bullet->exists = 0; - S_DebugDrawPoint(bullet->bullet_end, Color_Red); - } - else - { - S_DebugDrawPoint(bullet->bullet_end, Color_Purple); - } + if (closest_victim->valid) + { + bullet->has_hit = 1; + bullet->hit_entry = victim_raycast.p; + bullet->hit_entry_normal = victim_raycast.normal; + // bullet->bullet_end = bullet->hit_entry; + bullet->exists = 0; + } - Rng2 bounds = Zi; - bounds.p0 = VEC2(-S_WorldPitch / 2, -S_WorldPitch / 2); - bounds.p1 = VEC2(S_WorldPitch / 2, S_WorldPitch / 2); - if ( - bullet->bullet_start.x < bounds.p0.x || bullet->bullet_start.y < bounds.p0.y || - bullet->bullet_start.x > bounds.p1.x || bullet->bullet_start.y > bounds.p1.y - ) - { - bullet->exists = 0; + Rng2 bounds = Zi; + bounds.p0 = VEC2(-S_WorldPitch / 2, -S_WorldPitch / 2); + bounds.p1 = VEC2(S_WorldPitch / 2, S_WorldPitch / 2); + if ( + bullet->bullet_start.x < bounds.p0.x || bullet->bullet_start.y < bounds.p0.y || + bullet->bullet_start.x > bounds.p1.x || bullet->bullet_start.y > bounds.p1.y + ) + { + bullet->exists = 0; + } } } @@ -1698,6 +1837,48 @@ void S_TickForever(WaveLaneCtx *lane) + + + + ////////////////////////////// + //- Raycast testing + + // FIXME: Remove this + + + { + S_Ent *player = &S_NilEnt; + for (S_Ent *ent = S_FirstEnt(world); ent->valid; ent = S_NextEnt(ent)) + { + if (ent->is_player) + { + player = ent; + break; + } + } + + PERSIST Vec2 start = { 5, 1 }; + PERSIST Vec2 end = { 4, -1 }; + + Vec2 dir = SubVec2(end, start); + + Xform xf = player->xf; + S_Shape world_shape = S_MulXformShape(xf, player->local_shape); + + S_RaycastResult raycast = S_RaycastShape(world_shape, start, dir); + + S_DebugDrawPoint(raycast.p, Color_Blue); + S_DebugDrawLine(raycast.p, AddVec2(raycast.p, raycast.normal), Color_White); + } + + + + + + + + + ////////////////////////////// //- Debug draw entities diff --git a/src/pp/pp_sim/pp_sim_core.h b/src/pp/pp_sim/pp_sim_core.h index a6a735c0..dd8a0023 100644 --- a/src/pp/pp_sim/pp_sim_core.h +++ b/src/pp/pp_sim/pp_sim_core.h @@ -158,9 +158,9 @@ Struct(S_CollisionResult) Struct(S_RaycastResult) { + b32 is_intersecting; Vec2 p; Vec2 normal; - b32 is_intersecting; }; //////////////////////////////////////////////////////////// @@ -400,7 +400,7 @@ S_ClippedLine S_ClipLineToLine(Vec2 a0, Vec2 b0, Vec2 a1, Vec2 b1, Vec2 normal); Vec2 S_ClipPointToLine(Vec2 a, Vec2 b, Vec2 p, Vec2 normal); S_CollisionResult S_CollisionResultFromShapes(S_Shape shape0, S_Shape shape1, Vec2 sweep); -S_RaycastResult S_RaycastShape(S_Shape shape, Vec2 ray_start, Vec2 ray_dir, b32 extend); +S_RaycastResult S_RaycastShape(S_Shape shape, Vec2 ray_start, Vec2 ray_dir); //////////////////////////////////////////////////////////// //~ Lookup helpers