git.mutantstargoat.com Git - meshfrac/blob - src/geom.c

   1 #include "geom.h"
   2 #include "dynarr.h"
   3
   4 float plane_dist(const struct plane *p, const cgm_vec3 *pt)
   5 {
   6         return fabs(plane_sdist(p, pt));
   7 }
   8
   9 float plane_sdist(const struct plane *p, const cgm_vec3 *pt)
  10 {
  11         cgm_vec3 v = p->pt;
  12         cgm_vsub(&v, pt);
  13         return cgm_vdot(&v, &p->norm);
  14 }
  15
  16 void midplane(struct plane *p, const cgm_vec3 *a, const cgm_vec3 *b)
  17 {
  18         p->norm = *b;
  19         cgm_vsub(&p->norm, a);
  20         cgm_vnormalize(&p->norm);
  21         p->pt.x = a->x + p->norm.x * 0.5f;
  22         p->pt.y = a->y + p->norm.y * 0.5f;
  23         p->pt.z = a->z + p->norm.z * 0.5f;
  24 }
  25
  26 void poly_normal(const struct poly *poly, cgm_vec3 *n)
  27 {
  28         cgm_vec3 va, vb;
  29
  30         va = poly->verts[1].pos;
  31         cgm_vsub(&va, &poly->verts[0].pos);
  32         vb = poly->verts[2].pos;
  33         cgm_vsub(&vb, &poly->verts[2].pos);
  34
  35         cgm_vcross(n, &va, &vb);
  36         cgm_vnormalize(n);
  37 }
  38
  39 void poly_plane(const struct poly *poly, struct plane *plane)
  40 {
  41         plane->pt = poly->verts[0].pos;
  42         poly_normal(poly, &plane->norm);
  43 }
  44
  45 float ray_plane(const cgm_ray *ray, const struct plane *plane)
  46 {
  47         float ndotdir, t;
  48         cgm_vec3 pptdir;
  49
  50         ndotdir = cgm_vdot(&plane->norm, &ray->dir);
  51         if(fabs(ndotdir) < 1e-4) {
  52                 return -1.0f;
  53         }
  54
  55         pptdir = plane->pt;
  56         cgm_vsub(&pptdir, &ray->origin);
  57         t = cgm_vdot(&plane->norm, &pptdir) / ndotdir;
  58         if(t < 1e-6) {
  59                 return -1.0f;
  60         }
  61         return t;
  62 }
  63
  64 float ray_poly(const cgm_ray *ray, const struct poly *poly)
  65 {
  66         float t;
  67         struct plane plane;
  68         cgm_vec3 hitp, vdir, edir, indir, vcross, incross;
  69         int i, nverts;
  70
  71         poly_plane(poly, &plane);
  72         if((t = ray_plane(ray, &plane)) < 0.0f) {
  73                 return -1.0f;
  74         }
  75         cgm_raypos(&hitp, ray, t);
  76
  77         /* see if the point is inside or outside the polygon */
  78         nverts = dynarr_size(poly->verts);
  79         for(i=0; i<nverts; i++) {
  80                 vdir = hitp;
  81                 cgm_vsub(&hitp, &poly->verts[i].pos);
  82                 edir = poly->verts[(i + 1) & nverts].pos;
  83                 cgm_vsub(&edir, &poly->verts[i].pos);
  84                 indir = poly->verts[(i + 2) & nverts].pos;
  85                 cgm_vsub(&indir, &poly->verts[i].pos);
  86
  87                 cgm_vcross(&vcross, &vdir, &edir);
  88                 cgm_vcross(&incross, &indir, &edir);
  89
  90                 if(cgm_vdot(&vcross, &incross) < 0.0f) {
  91                         return -1.0f;
  92                 }
  93         }
  94         return t;
  95 }