nuclear@28: #include <math.h>
nuclear@28: #include "geom.h"
nuclear@28: #include "vector.h"
nuclear@28: 
nuclear@28: plane_t plane_cons(scalar_t nx, scalar_t ny, scalar_t nz, scalar_t d)
nuclear@28: {
nuclear@28: 	plane_t p;
nuclear@28: 	p.norm.x = nx;
nuclear@28: 	p.norm.y = ny;
nuclear@28: 	p.norm.z = nz;
nuclear@28: 	p.d = d;
nuclear@28: 	return p;
nuclear@28: }
nuclear@28: 
nuclear@28: plane_t plane_poly(vec3_t v0, vec3_t v1, vec3_t v2)
nuclear@28: {
nuclear@28: 	vec3_t a, b, norm;
nuclear@28: 
nuclear@28: 	a = v3_sub(v1, v0);
nuclear@28: 	b = v3_sub(v2, v0);
nuclear@28: 	norm = v3_cross(a, b);
nuclear@28: 	norm = v3_normalize(norm);
nuclear@28: 
nuclear@28: 	return plane_ptnorm(v0, norm);
nuclear@28: }
nuclear@28: 
nuclear@28: plane_t plane_ptnorm(vec3_t pt, vec3_t normal)
nuclear@28: {
nuclear@28: 	plane_t plane;
nuclear@28: 
nuclear@28: 	plane.norm = normal;
nuclear@28: 	plane.d = v3_dot(pt, normal);
nuclear@28: 
nuclear@28: 	return plane;
nuclear@28: }
nuclear@28: 
nuclear@28: plane_t plane_invert(plane_t p)
nuclear@28: {
nuclear@28: 	p.norm = v3_neg(p.norm);
nuclear@28: 	p.d = -p.d;
nuclear@28: 	return p;
nuclear@28: }
nuclear@28: 
nuclear@28: scalar_t plane_signed_dist(plane_t plane, vec3_t pt)
nuclear@28: {
nuclear@28: 	vec3_t pp = plane_point(plane);
nuclear@28: 	vec3_t pptopt = v3_sub(pt, pp);
nuclear@28: 	return v3_dot(pptopt, plane.norm);
nuclear@28: }
nuclear@28: 
nuclear@28: scalar_t plane_dist(plane_t plane, vec3_t pt)
nuclear@28: {
nuclear@28: 	return fabs(plane_signed_dist(plane, pt));
nuclear@28: }
nuclear@28: 
nuclear@28: vec3_t plane_point(plane_t plane)
nuclear@28: {
nuclear@28: 	return v3_scale(plane.norm, plane.d);
nuclear@28: }
nuclear@28: 
nuclear@28: int plane_ray_intersect(ray_t ray, plane_t plane, scalar_t *pos)
nuclear@28: {
nuclear@28: 	vec3_t pt, orig_to_pt;
nuclear@28: 	scalar_t ndotdir;
nuclear@28: 
nuclear@28: 	pt = plane_point(plane);
nuclear@28: 	ndotdir = v3_dot(plane.norm, ray.dir);
nuclear@28: 
nuclear@28: 	if(fabs(ndotdir) < 1e-7) {
nuclear@28: 		return 0;
nuclear@28: 	}
nuclear@28: 
nuclear@28: 	if(pos) {
nuclear@28: 		orig_to_pt = v3_sub(pt, ray.origin);
nuclear@28: 		*pos = v3_dot(plane.norm, orig_to_pt) / ndotdir;
nuclear@28: 	}
nuclear@28: 	return 1;
nuclear@28: }
nuclear@28: 
nuclear@28: sphere_t sphere_cons(scalar_t x, scalar_t y, scalar_t z, scalar_t rad)
nuclear@28: {
nuclear@28: 	sphere_t sph;
nuclear@28: 	sph.pos.x = x;
nuclear@28: 	sph.pos.y = y;
nuclear@28: 	sph.pos.z = z;
nuclear@28: 	sph.rad = rad;
nuclear@28: 	return sph;
nuclear@28: }
nuclear@28: 
nuclear@28: int sphere_ray_intersect(ray_t ray, sphere_t sph, scalar_t *pos)
nuclear@28: {
nuclear@28: 	scalar_t a, b, c, d, sqrt_d, t1, t2, t;
nuclear@28: 
nuclear@28: 	a = v3_dot(ray.dir, ray.dir);
nuclear@28: 	b = 2.0 * ray.dir.x * (ray.origin.x - sph.pos.x) +
nuclear@28: 		2.0 * ray.dir.y * (ray.origin.y - sph.pos.y) +
nuclear@28: 		2.0 * ray.dir.z * (ray.origin.z - sph.pos.z);
nuclear@28: 	c = v3_dot(sph.pos, sph.pos) + v3_dot(ray.origin, ray.origin) +
nuclear@28: 		2.0 * v3_dot(v3_neg(sph.pos), ray.origin) - sph.rad * sph.rad;
nuclear@28: 
nuclear@28: 	d = b * b - 4.0 * a * c;
nuclear@28: 	if(d < 0.0) {
nuclear@28: 		return 0;
nuclear@28: 	}
nuclear@28: 
nuclear@28: 	sqrt_d = sqrt(d);
nuclear@28: 	t1 = (-b + sqrt_d) / (2.0 * a);
nuclear@28: 	t2 = (-b - sqrt_d) / (2.0 * a);
nuclear@28: 
nuclear@28: 	if(t1 < 1e-7 || t1 > 1.0) {
nuclear@28: 		t1 = t2;
nuclear@28: 	}
nuclear@28: 	if(t2 < 1e-7 || t2 > 1.0) {
nuclear@28: 		t2 = t1;
nuclear@28: 	}
nuclear@28: 	t = t1 < t2 ? t1 : t2;
nuclear@28: 
nuclear@28: 	if(t < 1e-7 || t > 1.0) {
nuclear@28: 		return 0;
nuclear@28: 	}
nuclear@28: 
nuclear@28: 	if(pos) {
nuclear@28: 		*pos = t;
nuclear@28: 	}
nuclear@28: 	return 1;
nuclear@28: }
nuclear@28: 
nuclear@28: int sphere_sphere_intersect(sphere_t sph1, sphere_t sph2, scalar_t *pos, scalar_t *rad)
nuclear@28: {
nuclear@28: 	return -1;
nuclear@28: }