polyadvent

A game engine from scratch in C
git clone git://jb55.com/polyadvent
Log | Files | Refs | README

terrain_collision.c (5625B)


      1 
      2 #include "terrain_collision.h"
      3 #include <stdint.h>
      4 
      5 struct grid_query {
      6     struct terrain_cell *cell;
      7     int cell_vert_index;
      8     float distance;
      9 };
     10 
     11 struct tri_query {
     12     struct vert_tri *vtri;
     13 };
     14 
     15 /** Get the top 3 closest terrain vertices to a specific world position `pos`
     16  */
     17 static void get_closest_verts(struct terrain *terrain,
     18                               vec3 *pos,
     19                               struct grid_query closest[3],
     20                               struct terrain_cell *cells[9]
     21                               )
     22 {
     23     for (int i = 0; i < 9; i++) {
     24         struct terrain_cell *cell = cells[i];
     25         for (int j = 0; j < cell->vert_count; j++) {
     26             vec3 *vpos = &terrain->verts[cell->verts_index[j]];
     27             float d = vec3_distsq(pos, vpos);
     28 
     29             if (d < closest[0].distance) {
     30                 closest[2] = closest[1];
     31                 closest[1] = closest[0];
     32                 closest[0] = (struct grid_query) {
     33                   .distance = d,
     34                   .cell = cell,
     35                   .cell_vert_index = j
     36                 };
     37             }
     38             else if (d < closest[1].distance) {
     39                 closest[2] = closest[1];
     40                 closest[1] = (struct grid_query) {
     41                    .distance = d,
     42                    .cell = cell,
     43                    .cell_vert_index = j
     44                 };
     45             }
     46             else if (d < closest[2].distance) {
     47                 closest[2].distance = d;
     48                 closest[2].cell_vert_index = j;
     49                 closest[2].cell = cell;
     50             }
     51         }
     52     }
     53 }
     54 
     55 
     56 
     57 static inline float sign (float *p1, float *p2, float *p3)
     58 {
     59     return (p1[0] - p3[0]) * (p2[1] - p3[1]) - (p2[0] - p3[0]) * (p1[1] - p3[1]);
     60 }
     61 
     62 bool point_in_tri(float *pt, float *v1, float *v2, float *v3)
     63 {
     64     float d1, d2, d3;
     65     bool has_neg, has_pos;
     66 
     67     d1 = sign(pt, v1, v2);
     68     d2 = sign(pt, v2, v3);
     69     d3 = sign(pt, v3, v1);
     70 
     71     has_neg = (d1 < 0) || (d2 < 0) || (d3 < 0);
     72     has_pos = (d1 > 0) || (d2 > 0) || (d3 > 0);
     73 
     74     return !(has_neg && has_pos);
     75 }
     76 
     77 
     78 
     79 #ifdef DEBUG
     80 static void terrain_tri_debug(float *verts, struct tri *tri)
     81 {
     82 
     83     if (is_null_id(&tri->debug_id)) {
     84 
     85         float tmp[3], tmp2[3], normal[3];
     86 
     87         float *v1 = &verts[tri->vert_indices[0]];
     88         float *v2 = &verts[tri->vert_indices[1]];
     89         float *v3 = &verts[tri->vert_indices[2]];
     90 
     91         /* debug("v1 %f,%f v2 %f,%f v3 %f,%f\n", */
     92         /*       v1[0], v1[1], */
     93         /*       v2[0], v2[1], */
     94         /*       v3[0], v3[1]); */
     95 
     96         vec3_subtract(v1, v2, tmp);
     97         vec3_subtract(v2, v3, tmp2);
     98 
     99         vec3_cross(tmp, tmp2, normal);
    100         /* debug("tmp %f %f\n", tmp[0], tmp[1]); */
    101 
    102         vec3_scale(tmp, 0.5, tmp);
    103         vec3_subtract(v1, tmp, tmp);
    104         vec3_subtract(v3, tmp, tmp);
    105         vec3_scale(tmp, 0.5, tmp);
    106         vec3_subtract(v3, tmp, tmp);
    107 
    108         /* debug("creating new grid_debug entity at %f %f %f\n", tmp[0], tmp[1], tmp[2]); */
    109 
    110         struct entity *ent = new_debug_entity(&tri->debug_id, tmp);
    111         struct node *node = get_node(&ent->node_id);
    112 
    113         vec3_normalize(normal, normal);
    114         /* vec3_scale(normal, 180/PI, normal); */
    115         node_rotate(node, normal);
    116         node_recalc(node);
    117     }
    118 
    119 }
    120 #else
    121 #define terrain_tri_debug(...)
    122 #endif
    123 
    124 
    125 /** Test all of the triangles around a specific terrain vertex, looking to see if
    126  *  we are inside any of them. Return the intersecting triangle. This is all
    127  *  done in a 2d way.
    128  */
    129 static struct tri *point_in_vert_tris(float *verts, struct vert_tris *vtris, float *point)
    130 {
    131     for (int i = 0; i < vtris->tri_count; i++) {
    132         struct tri *tri = &vtris->tris[i];
    133         float *v1 = &verts[tri->vert_indices[0]];
    134         float *v2 = &verts[tri->vert_indices[1]];
    135         float *v3 = &verts[tri->vert_indices[2]];
    136         if (point_in_tri(point, v1, v2, v3))
    137             return tri;
    138     }
    139 
    140     return NULL;
    141 }
    142 
    143 
    144 
    145 static float get_terrain_penetration(float *verts, struct tri *tri, float *pos, float *move)
    146 {
    147     float tmp[3], normal[3];
    148 
    149     float *v1 = &verts[tri->vert_indices[0]];
    150     float *v2 = &verts[tri->vert_indices[1]];
    151     float *v3 = &verts[tri->vert_indices[2]];
    152 
    153     vec3_subtract(v3, v2, tmp);
    154     vec3_subtract(v2, v1, normal);
    155     vec3_cross(tmp, normal, normal);
    156     vec3_normalize(normal, normal);
    157     vec3_subtract(v1, pos, tmp);
    158     float d = vec3_dot(tmp, normal);
    159     vec3_scale(normal, d, move);
    160     /* vec3_add(pos, tmp, move); */
    161     return d;
    162 }
    163 
    164 
    165 struct tri *collide_terrain(struct terrain *terrain, float *pos, float *move, float *pen)
    166 {
    167     struct terrain_cell *cells[9] = {0};
    168     struct grid_query queries[3];
    169     for (int i = 0; i < 3; i++) {
    170         queries[i].distance = 3.402823e+38;
    171         queries[i].cell = NULL;
    172         queries[i].cell_vert_index = -1;
    173     }
    174 
    175     query_terrain_grid(terrain, pos[0], pos[1], cells);
    176     get_closest_verts(terrain, pos, queries, cells);
    177 
    178     for (int j = 0; j < ARRAY_SIZE(queries); j++) {
    179         if (!queries[j].cell)
    180             continue;
    181         int vind = queries[j].cell->verts_index[queries[j].cell_vert_index];
    182         struct vert_tris *vtris = &terrain->vtris[vind / 3];
    183 
    184         for (int i = 0; i < vtris->tri_count; i++) {;
    185             /* terrain_cell_debug(terrain, queries[i].cell, queries[i].cell_vert_index, pos); */
    186 
    187             struct tri *tri;
    188             if ((tri = point_in_vert_tris(terrain->verts, vtris, pos))) {
    189                 terrain_tri_debug(terrain->verts, tri);
    190                 *pen = get_terrain_penetration(terrain->verts, tri, pos, move);
    191                 return tri;
    192             }
    193         }
    194     }
    195 
    196     return NULL;
    197 }