istereo

view libs/vmath/geom.c @ 28:c0ae8e668447

find changesets by author, revision, files, or words in the commit message
added vmath library
author John Tsiombikas <nuclear@mutantstargoat.com>
date Thu, 08 Sep 2011 08:30:42 +0300
parents
children ff055bff6a15
line source
1 #include <math.h>
2 #include "geom.h"
3 #include "vector.h"
4
5 plane_t plane_cons(scalar_t nx, scalar_t ny, scalar_t nz, scalar_t d)
6 {
7 plane_t p;
8 p.norm.x = nx;
9 p.norm.y = ny;
10 p.norm.z = nz;
11 p.d = d;
12 return p;
13 }
14
15 plane_t plane_poly(vec3_t v0, vec3_t v1, vec3_t v2)
16 {
17 vec3_t a, b, norm;
18
19 a = v3_sub(v1, v0);
20 b = v3_sub(v2, v0);
21 norm = v3_cross(a, b);
22 norm = v3_normalize(norm);
23
24 return plane_ptnorm(v0, norm);
25 }
26
27 plane_t plane_ptnorm(vec3_t pt, vec3_t normal)
28 {
29 plane_t plane;
30
31 plane.norm = normal;
32 plane.d = v3_dot(pt, normal);
33
34 return plane;
35 }
36
37 plane_t plane_invert(plane_t p)
38 {
39 p.norm = v3_neg(p.norm);
40 p.d = -p.d;
41 return p;
42 }
43
44 scalar_t plane_signed_dist(plane_t plane, vec3_t pt)
45 {
46 vec3_t pp = plane_point(plane);
47 vec3_t pptopt = v3_sub(pt, pp);
48 return v3_dot(pptopt, plane.norm);
49 }
50
51 scalar_t plane_dist(plane_t plane, vec3_t pt)
52 {
53 return fabs(plane_signed_dist(plane, pt));
54 }
55
56 vec3_t plane_point(plane_t plane)
57 {
58 return v3_scale(plane.norm, plane.d);
59 }
60
61 int plane_ray_intersect(ray_t ray, plane_t plane, scalar_t *pos)
62 {
63 vec3_t pt, orig_to_pt;
64 scalar_t ndotdir;
65
66 pt = plane_point(plane);
67 ndotdir = v3_dot(plane.norm, ray.dir);
68
69 if(fabs(ndotdir) < 1e-7) {
70 return 0;
71 }
72
73 if(pos) {
74 orig_to_pt = v3_sub(pt, ray.origin);
75 *pos = v3_dot(plane.norm, orig_to_pt) / ndotdir;
76 }
77 return 1;
78 }
79
80 sphere_t sphere_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t rad)
81 {
82 sphere_t sph;
83 sph.pos.x = x;
84 sph.pos.y = y;
85 sph.pos.z = z;
86 sph.rad = rad;
87 return sph;
88 }
89
90 int sphere_ray_intersect(ray_t ray, sphere_t sph, scalar_t *pos)
91 {
92 scalar_t a, b, c, d, sqrt_d, t1, t2, t;
93
94 a = v3_dot(ray.dir, ray.dir);
95 b = 2.0 * ray.dir.x * (ray.origin.x - sph.pos.x) +
96 2.0 * ray.dir.y * (ray.origin.y - sph.pos.y) +
97 2.0 * ray.dir.z * (ray.origin.z - sph.pos.z);
98 c = v3_dot(sph.pos, sph.pos) + v3_dot(ray.origin, ray.origin) +
99 2.0 * v3_dot(v3_neg(sph.pos), ray.origin) - sph.rad * sph.rad;
100
101 d = b * b - 4.0 * a * c;
102 if(d < 0.0) {
103 return 0;
104 }
105
106 sqrt_d = sqrt(d);
107 t1 = (-b + sqrt_d) / (2.0 * a);
108 t2 = (-b - sqrt_d) / (2.0 * a);
109
110 if(t1 < 1e-7 || t1 > 1.0) {
111 t1 = t2;
112 }
113 if(t2 < 1e-7 || t2 > 1.0) {
114 t2 = t1;
115 }
116 t = t1 < t2 ? t1 : t2;
117
118 if(t < 1e-7 || t > 1.0) {
119 return 0;
120 }
121
122 if(pos) {
123 *pos = t;
124 }
125 return 1;
126 }
127
128 int sphere_sphere_intersect(sphere_t sph1, sphere_t sph2, scalar_t *pos, scalar_t *rad)
129 {
130 return -1;
131 }