des.math.linear.ray source code

1 /+
2 The MIT License (MIT)
3 
4     Copyright (c) <2013> <Oleg Butko (deviator), Anton Akzhigitov (Akzwar)>
5 
6     Permission is hereby granted, free of charge, to any person obtaining a copy
7     of this software and associated documentation files (the "Software"), to deal
8     in the Software without restriction, including without limitation the rights
9     to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
10     copies of the Software, and to permit persons to whom the Software is
11     furnished to do so, subject to the following conditions:
12 
13     The above copyright notice and this permission notice shall be included in
14     all copies or substantial portions of the Software.
15 
16     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
17     IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
18     FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
19     AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
20     LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
21     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
22     THE SOFTWARE.
23 +/
24 
25 module des.math.linear.ray;
26 
27 import std.math;
28 import std.traits;
29 import des.util.testsuite;
30 import des.math.linear.vector;
31 import des.math.linear.matrix;
32 import des.math.basic;
33 
34 version(unittest)
35 {
36     bool eq_seg(A,B,E=float)( in Ray!A a, in Ray!B b, in E eps=E.epsilon )
37     { return eq_approx( a.pos.data ~ a.dir.data, b.pos.data ~ b.dir.data, eps ); }
38 }
39 
40 ///
41 struct Ray(T) if( isFloatingPoint!T )
42 {
43     ///
44     alias Vector3!T vectype;
45 
46     ///
47     vectype pos, dir;
48 
49     mixin( BasicMathOp!"pos dir" );
50 
51 pure:
52     ///
53     static auto fromPoints(A,B)( in A start, in B end )
54         if( isCompatibleVector!(3,T,A) && isCompatibleVector!(3,T,B) )
55     { return Ray!T( start, end - start ); }
56 
57     ///
58     this(A,B)( in A a, in B b )
59         if( isCompatibleVector!(3,T,A) && isCompatibleVector!(3,T,B) )
60     {
61         pos = a;
62         dir = b;
63     }
64 
65     @property
66     {
67         ///
68         ref vectype start() { return pos; }
69         ///
70         ref const(vectype) start() const { return pos; }
71         ///
72         vectype end() const { return pos + dir; }
73         ///
74         vectype end( in vectype p )
75         {
76             dir = p - pos;
77             return p;
78         }
79 
80         ///
81         auto revert() const
82         { return Ray!(T).fromPoints( end, start ); }
83 
84         ///
85         T len2() const { return dir.len2; }
86         ///
87         T len() const { return dir.len; }
88     }
89 
90     /// affine transform
91     auto tr(X)( in Matrix!(4,4,X) mtr ) const
92     {
93         return Ray!T( (mtr * Vector4!T( pos, 1 )).xyz,
94                       (mtr * Vector4!T( dir, 0 )).xyz );
95     }
96 
97     /+ высота проведённая из точки это отрезок, 
98        соединяющий проекцию точки на прямую и 
99        саму точку (Ray) +/
100     ///
101     auto altitude( in vectype pp ) const
102     {
103         auto n = dir.e;
104         auto dd = pos + n * dot(n,(pp-pos));
105         return Ray!T( dd, pp - dd );
106     }
107 
108     /+ общий перпендикуляр +/
109     ///
110     auto altitude(F)( in Ray!F seg ) const
111     {
112         /+ находим нормаль для паралельных 
113         плоскостей в которых лежат s1 и s2 +/
114         auto norm = cross(dir,seg.dir).e;
115 
116         /+ расстояние между началами точками на прямых +/
117         auto mv = pos - seg.pos;
118 
119         /+ нормальный вектор, длиной в расстояние между плоскостями +/
120         auto dist = norm * dot(norm,mv);
121 
122         /+ переносим отрезок на плоскость первой прямой
123            и сразу находим пересечение +/
124         auto pp = calcIntersection( Ray!T( seg.pos + dist, seg.dir ) );
125 
126         return Ray!T( pp, -dist );
127     }
128 
129     /+ пересечение с другой прямой 
130        если она в той же плоскости +/
131     ///
132     auto intersect(F)( in Ray!F seg ) const
133     {
134         auto a = altitude( seg );
135         return a.pos + a.dir * 0.5;
136     }
137 
138     ///
139     auto calcIntersection(F)( in Ray!F seg ) const
140     {
141         auto a = pos;
142         auto v = dir;
143 
144         auto b = seg.pos;
145         auto w = seg.dir;
146 
147         static T resolve( T a0, T r, T a1, T q, T b0, T p, T b1, T s )
148         {
149             T pq = p * q, rs = r * s;
150             return ( a0 * pq + ( b1 - b0 ) * r * q - a1 * rs ) / ( pq - rs );
151         }
152 
153         auto x = resolve( a.x, v.x, b.x, w.x,  a.y, v.y, b.y, w.y );
154         auto y = resolve( a.y, v.y, b.y, w.y,  a.x, v.x, b.x, w.x );
155         auto z = resolve( a.z, v.z, b.z, w.z,  a.y, v.y, b.y, w.y );
156 
157         x = isFinite(x) ? x : resolve( a.x, v.x, b.x, w.x,  a.z, v.z, b.z, w.z );
158         y = isFinite(y) ? y : resolve( a.y, v.y, b.y, w.y,  a.z, v.z, b.z, w.z );
159         z = isFinite(z) ? z : resolve( a.z, v.z, b.z, w.z,  a.x, v.x, b.x, w.x );
160 
161         if( !isFinite(x) ) x = pos.x;
162         if( !isFinite(y) ) y = pos.y;
163         if( !isFinite(z) ) z = pos.z;
164 
165         return vectype( x, y, z );
166     }
167 }
168 
169 ///
170 alias Ray!float  fRay;
171 ///
172 alias Ray!double dRay;
173 ///
174 alias Ray!real   rRay;
175 
176 ///
177 unittest
178 {
179     auto r1 = fRay( vec3(1,2,3), vec3(2,3,4) );
180     auto r2 = fRay( vec3(4,5,6), vec3(5,2,3) );
181     auto rs = fRay( vec3(5,7,9), vec3(7,5,7) );
182     assert( r1 + r2 == rs );
183 }
184 
185 unittest
186 {
187     auto a = vec3(1,2,3);
188     auto b = vec3(2,3,4);
189     auto r1 = fRay( a, b );
190     assert( r1.start == a );
191     assert( r1.end == a + b );
192     r1.start = b;
193     assert( r1.start == b );
194     r1.start = a;
195     assert( r1.len == b.len );
196     r1.end = a;
197     assert( r1.len == 0 );
198 }
199 
200 unittest
201 {
202     import des.math.linear.matrix;
203     auto mtr = mat4( 2, 0.1, 0.04, 2,
204                      0.3, 5, 0.01, 5,
205                      0.1, 0.02, 3, 1,
206                      0, 0, 0, 1 );
207 
208     auto s = fRay( vec3(1,2,3), vec3(2,3,4) );
209     
210     auto ta = (mtr * vec4(s.start,1)).xyz;
211     auto tb = (mtr * vec4(s.end,1)).xyz;
212     auto ts = s.tr( mtr );
213 
214     assert( eq( ts.start, ta ) );
215     assert( eq_approx( ts.end, tb, 1e-5 ) );
216 }
217 
218 ///
219 unittest
220 {
221     auto s = fRay( vec3(2,0,0), vec3(-4,4,0) );
222     auto p = vec3( 0,0,0 );
223     auto r = s.altitude(p);
224     assert( eq( p, r.end ) );
225     assert( eq( r.pos, vec3(1,1,0) ) );
226 }
227 
228 unittest
229 {
230     auto s = fRay( vec3(2,0,0), vec3(0,4,0) );
231     auto p = vec3( 0,0,0 );
232     auto r = s.altitude(p).len;
233     assert( r == 2.0f );
234 }
235 
236 unittest
237 {
238     auto s1 = fRay( vec3(0,0,1), vec3(2,2,0) );
239     auto s2 = fRay( vec3(2,0,-1), vec3(-4,4,0) );
240     auto a1 = s1.altitude(s2);
241     auto a2 = s2.altitude(s1);
242     assert( eq_seg( a1, a2.revert ) );
243     assert( a1.len == 2 );
244     assert( eq( a1.pos, vec3(1,1,1) ) );
245     assert( eq( a1.dir, vec3(0,0,-2) ) );
246 }
247 
248 ///
249 unittest
250 {
251     auto s1 = fRay( vec3(-2,0,0), vec3(1,0,0) );
252     auto s2 = fRay( vec3(0,0,2), vec3(0,1,-1) );
253 
254     auto a1 = s1.altitude(s2);
255     assert( eq_seg( a1, fRay(vec3(0,0,0), vec3(0,1,1)) ) );
256 }
257 
258 ///
259 unittest
260 {
261     auto s1 = fRay( vec3(0,0,0), vec3(2,2,0) );
262     auto s2 = fRay( vec3(2,0,0), vec3(-4,4,0) );
263     assert( eq( s1.intersect(s2), s2.intersect(s1) ) );
264     assert( eq( s1.intersect(s2), vec3(1,1,0) ) );
265 }