Merge remote-tracking branch 'illwieckz/fastallocate'
[xonotic/netradiant.git] / libs / mathlib / ray.c
1 /*
2    Copyright (C) 2001-2006, William Joseph.
3    All Rights Reserved.
4
5    This file is part of GtkRadiant.
6
7    GtkRadiant is free software; you can redistribute it and/or modify
8    it under the terms of the GNU General Public License as published by
9    the Free Software Foundation; either version 2 of the License, or
10    (at your option) any later version.
11
12    GtkRadiant is distributed in the hope that it will be useful,
13    but WITHOUT ANY WARRANTY; without even the implied warranty of
14    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15    GNU General Public License for more details.
16
17    You should have received a copy of the GNU General Public License
18    along with GtkRadiant; if not, write to the Free Software
19    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
20  */
21
22 #include "mathlib.h"
23 #include <float.h>
24
25 vec3_t identity = { 0,0,0 };
26
27 void ray_construct_for_vec3( ray_t *ray, const vec3_t origin, const vec3_t direction ){
28         VectorCopy( origin, ray->origin );
29         VectorCopy( direction, ray->direction );
30 }
31
32 void ray_transform( ray_t *ray, const m4x4_t matrix ){
33         m4x4_transform_point( matrix, ray->origin );
34         m4x4_transform_normal( matrix, ray->direction );
35 }
36
37 vec_t ray_intersect_point( const ray_t *ray, const vec3_t point, vec_t epsilon, vec_t divergence ){
38         vec3_t displacement;
39         vec_t depth;
40
41         // calc displacement of test point from ray origin
42         VectorSubtract( point, ray->origin, displacement );
43         // calc length of displacement vector along ray direction
44         depth = DotProduct( displacement, ray->direction );
45         if ( depth < 0.0f ) {
46                 return (vec_t)FLT_MAX;
47         }
48         // calc position of closest point on ray to test point
49         VectorMA( ray->origin, depth, ray->direction, displacement );
50         // calc displacement of test point from closest point
51         VectorSubtract( point, displacement, displacement );
52         // calc length of displacement, subtract depth-dependant epsilon
53         if ( VectorLength( displacement ) - ( epsilon + ( depth * divergence ) ) > 0.0f ) {
54                 return (vec_t)FLT_MAX;
55         }
56         return depth;
57 }
58
59 // Tomas Moller and Ben Trumbore. Fast, minimum storage ray-triangle intersection. Journal of graphics tools, 2(1):21-28, 1997
60
61 #define EPSILON 0.000001
62
63 vec_t ray_intersect_triangle( const ray_t *ray, qboolean bCullBack, const vec3_t vert0, const vec3_t vert1, const vec3_t vert2 ){
64         float edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
65         float det,inv_det;
66         float u, v;
67         vec_t depth = (vec_t)FLT_MAX;
68
69         /* find vectors for two edges sharing vert0 */
70         VectorSubtract( vert1, vert0, edge1 );
71         VectorSubtract( vert2, vert0, edge2 );
72
73         /* begin calculating determinant - also used to calculate U parameter */
74         CrossProduct( ray->direction, edge2, pvec );
75
76         /* if determinant is near zero, ray lies in plane of triangle */
77         det = DotProduct( edge1, pvec );
78
79         if ( bCullBack == qtrue ) {
80                 if ( det < EPSILON ) {
81                         return depth;
82                 }
83
84                 // calculate distance from vert0 to ray origin
85                 VectorSubtract( ray->origin, vert0, tvec );
86
87                 // calculate U parameter and test bounds
88                 u = DotProduct( tvec, pvec );
89                 if ( u < 0.0 || u > det ) {
90                         return depth;
91                 }
92
93                 // prepare to test V parameter
94                 CrossProduct( tvec, edge1, qvec );
95
96                 // calculate V parameter and test bounds
97                 v = DotProduct( ray->direction, qvec );
98                 if ( v < 0.0 || u + v > det ) {
99                         return depth;
100                 }
101
102                 // calculate t, scale parameters, ray intersects triangle
103                 depth = DotProduct( edge2, qvec );
104                 inv_det = 1.0f / det;
105                 depth *= inv_det;
106                 //u *= inv_det;
107                 //v *= inv_det;
108         }
109         else
110         {
111                 /* the non-culling branch */
112                 if ( det > -EPSILON && det < EPSILON ) {
113                         return depth;
114                 }
115                 inv_det = 1.0f / det;
116
117                 /* calculate distance from vert0 to ray origin */
118                 VectorSubtract( ray->origin, vert0, tvec );
119
120                 /* calculate U parameter and test bounds */
121                 u = DotProduct( tvec, pvec ) * inv_det;
122                 if ( u < 0.0 || u > 1.0 ) {
123                         return depth;
124                 }
125
126                 /* prepare to test V parameter */
127                 CrossProduct( tvec, edge1, qvec );
128
129                 /* calculate V parameter and test bounds */
130                 v = DotProduct( ray->direction, qvec ) * inv_det;
131                 if ( v < 0.0 || u + v > 1.0 ) {
132                         return depth;
133                 }
134
135                 /* calculate t, ray intersects triangle */
136                 depth = DotProduct( edge2, qvec ) * inv_det;
137         }
138         return depth;
139 }
140
141 vec_t ray_intersect_plane( const ray_t* ray, const vec3_t normal, vec_t dist ){
142         return -( DotProduct( normal, ray->origin ) - dist ) / DotProduct( ray->direction, normal );
143 }