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