dbf-halloween2015

annotate libs/vmath/geom.c @ 1:c3f5c32cb210

find changesets by author, revision, files, or words in the commit message
barfed all the libraries in the source tree to make porting easier
author John Tsiombikas <nuclear@member.fsf.org>
date Sun, 01 Nov 2015 00:36:56 +0200
parents
children
rev   line source
nuclear@1 1 /*
nuclear@1 2 libvmath - a vector math library
nuclear@1 3 Copyright (C) 2004-2011 John Tsiombikas <nuclear@member.fsf.org>
nuclear@1 4
nuclear@1 5 This program is free software: you can redistribute it and/or modify
nuclear@1 6 it under the terms of the GNU Lesser General Public License as published
nuclear@1 7 by the Free Software Foundation, either version 3 of the License, or
nuclear@1 8 (at your option) any later version.
nuclear@1 9
nuclear@1 10 This program is distributed in the hope that it will be useful,
nuclear@1 11 but WITHOUT ANY WARRANTY; without even the implied warranty of
nuclear@1 12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
nuclear@1 13 GNU Lesser General Public License for more details.
nuclear@1 14
nuclear@1 15 You should have received a copy of the GNU Lesser General Public License
nuclear@1 16 along with this program. If not, see <http://www.gnu.org/licenses/>.
nuclear@1 17 */
nuclear@1 18
nuclear@1 19
nuclear@1 20 #include <math.h>
nuclear@1 21 #include "geom.h"
nuclear@1 22 #include "vector.h"
nuclear@1 23
nuclear@1 24 plane_t plane_cons(scalar_t nx, scalar_t ny, scalar_t nz, scalar_t d)
nuclear@1 25 {
nuclear@1 26 plane_t p;
nuclear@1 27 p.norm.x = nx;
nuclear@1 28 p.norm.y = ny;
nuclear@1 29 p.norm.z = nz;
nuclear@1 30 p.d = d;
nuclear@1 31 return p;
nuclear@1 32 }
nuclear@1 33
nuclear@1 34 plane_t plane_poly(vec3_t v0, vec3_t v1, vec3_t v2)
nuclear@1 35 {
nuclear@1 36 vec3_t a, b, norm;
nuclear@1 37
nuclear@1 38 a = v3_sub(v1, v0);
nuclear@1 39 b = v3_sub(v2, v0);
nuclear@1 40 norm = v3_cross(a, b);
nuclear@1 41 norm = v3_normalize(norm);
nuclear@1 42
nuclear@1 43 return plane_ptnorm(v0, norm);
nuclear@1 44 }
nuclear@1 45
nuclear@1 46 plane_t plane_ptnorm(vec3_t pt, vec3_t normal)
nuclear@1 47 {
nuclear@1 48 plane_t plane;
nuclear@1 49
nuclear@1 50 plane.norm = normal;
nuclear@1 51 plane.d = v3_dot(pt, normal);
nuclear@1 52
nuclear@1 53 return plane;
nuclear@1 54 }
nuclear@1 55
nuclear@1 56 plane_t plane_invert(plane_t p)
nuclear@1 57 {
nuclear@1 58 p.norm = v3_neg(p.norm);
nuclear@1 59 p.d = -p.d;
nuclear@1 60 return p;
nuclear@1 61 }
nuclear@1 62
nuclear@1 63 scalar_t plane_signed_dist(plane_t plane, vec3_t pt)
nuclear@1 64 {
nuclear@1 65 vec3_t pp = plane_point(plane);
nuclear@1 66 vec3_t pptopt = v3_sub(pt, pp);
nuclear@1 67 return v3_dot(pptopt, plane.norm);
nuclear@1 68 }
nuclear@1 69
nuclear@1 70 scalar_t plane_dist(plane_t plane, vec3_t pt)
nuclear@1 71 {
nuclear@1 72 return fabs(plane_signed_dist(plane, pt));
nuclear@1 73 }
nuclear@1 74
nuclear@1 75 vec3_t plane_point(plane_t plane)
nuclear@1 76 {
nuclear@1 77 return v3_scale(plane.norm, plane.d);
nuclear@1 78 }
nuclear@1 79
nuclear@1 80 int plane_ray_intersect(ray_t ray, plane_t plane, scalar_t *pos)
nuclear@1 81 {
nuclear@1 82 vec3_t pt, orig_to_pt;
nuclear@1 83 scalar_t ndotdir;
nuclear@1 84
nuclear@1 85 pt = plane_point(plane);
nuclear@1 86 ndotdir = v3_dot(plane.norm, ray.dir);
nuclear@1 87
nuclear@1 88 if(fabs(ndotdir) < 1e-7) {
nuclear@1 89 return 0;
nuclear@1 90 }
nuclear@1 91
nuclear@1 92 if(pos) {
nuclear@1 93 orig_to_pt = v3_sub(pt, ray.origin);
nuclear@1 94 *pos = v3_dot(plane.norm, orig_to_pt) / ndotdir;
nuclear@1 95 }
nuclear@1 96 return 1;
nuclear@1 97 }
nuclear@1 98
nuclear@1 99 sphere_t sphere_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t rad)
nuclear@1 100 {
nuclear@1 101 sphere_t sph;
nuclear@1 102 sph.pos.x = x;
nuclear@1 103 sph.pos.y = y;
nuclear@1 104 sph.pos.z = z;
nuclear@1 105 sph.rad = rad;
nuclear@1 106 return sph;
nuclear@1 107 }
nuclear@1 108
nuclear@1 109 int sphere_ray_intersect(ray_t ray, sphere_t sph, scalar_t *pos)
nuclear@1 110 {
nuclear@1 111 scalar_t a, b, c, d, sqrt_d, t1, t2, t;
nuclear@1 112
nuclear@1 113 a = v3_dot(ray.dir, ray.dir);
nuclear@1 114 b = 2.0 * ray.dir.x * (ray.origin.x - sph.pos.x) +
nuclear@1 115 2.0 * ray.dir.y * (ray.origin.y - sph.pos.y) +
nuclear@1 116 2.0 * ray.dir.z * (ray.origin.z - sph.pos.z);
nuclear@1 117 c = v3_dot(sph.pos, sph.pos) + v3_dot(ray.origin, ray.origin) +
nuclear@1 118 2.0 * v3_dot(v3_neg(sph.pos), ray.origin) - sph.rad * sph.rad;
nuclear@1 119
nuclear@1 120 d = b * b - 4.0 * a * c;
nuclear@1 121 if(d < 0.0) {
nuclear@1 122 return 0;
nuclear@1 123 }
nuclear@1 124
nuclear@1 125 sqrt_d = sqrt(d);
nuclear@1 126 t1 = (-b + sqrt_d) / (2.0 * a);
nuclear@1 127 t2 = (-b - sqrt_d) / (2.0 * a);
nuclear@1 128
nuclear@1 129 if(t1 < 1e-7 || t1 > 1.0) {
nuclear@1 130 t1 = t2;
nuclear@1 131 }
nuclear@1 132 if(t2 < 1e-7 || t2 > 1.0) {
nuclear@1 133 t2 = t1;
nuclear@1 134 }
nuclear@1 135 t = t1 < t2 ? t1 : t2;
nuclear@1 136
nuclear@1 137 if(t < 1e-7 || t > 1.0) {
nuclear@1 138 return 0;
nuclear@1 139 }
nuclear@1 140
nuclear@1 141 if(pos) {
nuclear@1 142 *pos = t;
nuclear@1 143 }
nuclear@1 144 return 1;
nuclear@1 145 }
nuclear@1 146
nuclear@1 147 int sphere_sphere_intersect(sphere_t sph1, sphere_t sph2, scalar_t *pos, scalar_t *rad)
nuclear@1 148 {
nuclear@1 149 return -1;
nuclear@1 150 }