dbf-halloween2015

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