des.math.linear.segment 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.segment;
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 struct Segment(T) if( isFloatingPoint!T )
35 {
36     alias Vector!(3,T,"x y z") vectype;
37     vectype pnt, dir;
38     mixin( BasicMathOp!"pnt dir" );
39 
40     static auto fromPoints( in vectype s, in vectype e )
41     { return Segment!T( s, e - s ); }
42 
43     @property
44     {
45         ref vectype start() { return pnt; }
46         ref const(vectype) start() const { return pnt; }
47         vectype end() const { return pnt + dir; }
48         vectype end( in vectype p )
49         {
50             dir = p - pnt;
51             return p;
52         }
53 
54         auto revert() const
55         { return Segment!(T).fromPoints( end, start ); }
56 
57         T len2() const { return dir.len2; }
58         T len() const { return dir.len; }
59     }
60 
61     /+ аффинное преобразование +/
62     auto tr(X)( in Matrix!(4,4,X) mtr ) const
63     {
64         return Segment!T( (mtr * Vector!(4,T,"x y z w")( pnt, 1 )).xyz,
65                           (mtr * Vector!(4,T,"x y z w")( dir, 0 )).xyz );
66     }
67 
68     /+ высота проведённая из точки это отрезок, 
69        соединяющий проекцию точки на прямую и 
70        саму точку (Segment) +/
71     auto altitude( in vectype pp ) const
72     {
73         auto n = dir.e;
74         auto dd = pnt + n * dot(n,(pp-pnt));
75         return Segment!T( dd, pp - dd );
76     }
77 
78     /+ общий перпендикуляр +/
79     auto altitude(F)( in Segment!F seg ) const
80     {
81         /+ находим нормаль для паралельных 
82         плоскостей в которых лежат s1 и s2 +/
83         auto norm = cross(dir,seg.dir).e;
84 
85         /+ расстояние между началами точками на прямых +/
86         auto mv = pnt - seg.pnt;
87 
88         /+ нормальный вектор, длиной в расстояние между плоскостями +/
89         auto dist = norm * dot(norm,mv);
90 
91         /+ переносим отрезок на плоскость первой прямой
92            и сразу находим пересечение +/
93         auto pp = intersect( Segment!T( seg.pnt + dist, seg.dir ) );
94 
95         return Segment!T( pp, -dist );
96     }
97 
98     /+ пересечение с другой прямой 
99        если она в той же плоскости +/
100     auto intersect(F)( in Segment!F seg ) const
101     {
102         auto a = pnt;
103         auto v = dir;
104 
105         auto b = seg.pnt;
106         auto w = seg.dir;
107 
108         static T resolve( T a0, T r, T a1, T q, T b0, T p, T b1, T s )
109         {
110             T pq = p * q, rs = r * s;
111             return ( a0 * pq + ( b1 - b0 ) * r * q - a1 * rs ) / ( pq - rs );
112         }
113 
114         auto x = resolve( a.x, v.x, b.x, w.x,  a.y, v.y, b.y, w.y );
115         auto y = resolve( a.y, v.y, b.y, w.y,  a.x, v.x, b.x, w.x );
116         auto z = resolve( a.z, v.z, b.z, w.z,  a.y, v.y, b.y, w.y );
117 
118         x = isFinite(x) ? x : resolve( a.x, v.x, b.x, w.x,  a.z, v.z, b.z, w.z );
119         y = isFinite(y) ? y : resolve( a.y, v.y, b.y, w.y,  a.z, v.z, b.z, w.z );
120         z = isFinite(z) ? z : resolve( a.z, v.z, b.z, w.z,  a.x, v.x, b.x, w.x );
121 
122         if( !isFinite(x) ) x = pnt.x;
123         if( !isFinite(y) ) y = pnt.y;
124         if( !isFinite(z) ) z = pnt.z;
125 
126         return vectype( x, y, z );
127     }
128 }
129 
130 alias Segment!float  fSeg;
131 alias Segment!double dSeg;
132 alias Segment!real   rSeg;
133 
134 version(unittest)
135 {
136     bool eq_seg(A,B,E=float)( in Segment!A a, in Segment!B b, in E eps=E.epsilon )
137     { return eq_approx( a.pnt.data ~ a.dir.data, b.pnt.data ~ b.dir.data, eps ); }
138 }
139 
140 unittest
141 {
142     auto r1 = fSeg( vec3(1,2,3), vec3(2,3,4) );
143     auto r2 = fSeg( vec3(4,5,6), vec3(5,2,3) );
144     auto rs = fSeg( vec3(5,7,9), vec3(7,5,7) );
145     assert( r1 + r2 == rs );
146 }
147 
148 unittest
149 {
150     auto a = vec3(1,2,3);
151     auto b = vec3(2,3,4);
152     auto r1 = fSeg( a, b );
153     assert( r1.start == a );
154     assert( r1.end == a + b );
155     r1.start = b;
156     assert( r1.start == b );
157     r1.start = a;
158     assert( r1.len == b.len );
159     r1.end = a;
160     assert( r1.len == 0 );
161 }
162 
163 unittest
164 {
165     import des.math.linear.matrix;
166     auto mtr = mat4( 2, 0.1, 0.04, 2,
167                      0.3, 5, 0.01, 5,
168                      0.1, 0.02, 3, 1,
169                      0, 0, 0, 1 );
170 
171     auto s = fSeg( vec3(1,2,3), vec3(2,3,4) );
172     
173     auto ta = (mtr * vec4(s.start,1)).xyz;
174     auto tb = (mtr * vec4(s.end,1)).xyz;
175     auto ts = s.tr( mtr );
176 
177     assert( eq( ts.start, ta ) );
178     assert( eq_approx( ts.end, tb, 1e-5 ) );
179 }
180 
181 unittest
182 {
183     auto s = fSeg( vec3(2,0,0), vec3(-4,4,0) );
184     auto p = vec3( 0,0,0 );
185     auto r = s.altitude(p);
186     assert( eq( p, r.end ) );
187     assert( eq( r.pnt, vec3(1,1,0) ) );
188 }
189 
190 unittest
191 {
192     auto s = fSeg( vec3(2,0,0), vec3(0,4,0) );
193     auto p = vec3( 0,0,0 );
194     auto r = s.altitude(p).len;
195     assert( r == 2.0f );
196 }
197 
198 unittest
199 {
200     auto s1 = fSeg( vec3(0,0,1), vec3(2,2,0) );
201     auto s2 = fSeg( vec3(2,0,-1), vec3(-4,4,0) );
202     auto a1 = s1.altitude(s2);
203     auto a2 = s2.altitude(s1);
204     assert( eq_seg( a1, a2.revert ) );
205     assert( a1.len == 2 );
206     assert( eq( a1.pnt, vec3(1,1,1) ) );
207     assert( eq( a1.dir, vec3(0,0,-2) ) );
208 }
209 
210 unittest
211 {
212     auto s1 = fSeg( vec3(-2,0,0), vec3(1,0,0) );
213     auto s2 = fSeg( vec3(0,0,2), vec3(0,1,-1) );
214 
215     auto a1 = s1.altitude(s2);
216     assert( eq_seg( a1, fSeg(vec3(0,0,0), vec3(0,1,1)) ) );
217 }
218 
219 unittest
220 {
221     auto s1 = fSeg( vec3(0,0,0), vec3(2,2,0) );
222     auto s2 = fSeg( vec3(2,0,0), vec3(-4,4,0) );
223     assert( eq( s1.intersect(s2), s2.intersect(s1) ) );
224     assert( eq( s1.intersect(s2), vec3(1,1,0) ) );
225 }