1 /*
2 ===========================================================================
3
4 Doom 3 GPL Source Code
5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
6
7 This file is part of the Doom 3 GPL Source Code ("Doom 3 Source Code").
8
9 Doom 3 Source Code is free software: you can redistribute it and/or modify
10 it under the terms of the GNU General Public License as published by
11 the Free Software Foundation, either version 3 of the License, or
12 (at your option) any later version.
13
14 Doom 3 Source Code is distributed in the hope that it will be useful,
15 but WITHOUT ANY WARRANTY; without even the implied warranty of
16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 GNU General Public License for more details.
18
19 You should have received a copy of the GNU General Public License
20 along with Doom 3 Source Code. If not, see <http://www.gnu.org/licenses/>.
21
22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code. If not, please request a copy in writing from id Software at the address below.
23
24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
25
26 ===========================================================================
27 */
28
29 #ifndef __MATH_PLUECKER_H__
30 #define __MATH_PLUECKER_H__
31
32 #include "idlib/math/Vector.h"
33
34 /*
35 ===============================================================================
36
37 Pluecker coordinate
38
39 ===============================================================================
40 */
41
42 class idPluecker {
43 public:
44 idPluecker( void );
45 explicit idPluecker( const float *a );
46 explicit idPluecker( const idVec3 &start, const idVec3 &end );
47 explicit idPluecker( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
48
49 float operator[]( const int index ) const;
50 float & operator[]( const int index );
51 idPluecker operator-() const; // flips the direction
52 idPluecker operator*( const float a ) const;
53 idPluecker operator/( const float a ) const;
54 float operator*( const idPluecker &a ) const; // permuted inner product
55 idPluecker operator-( const idPluecker &a ) const;
56 idPluecker operator+( const idPluecker &a ) const;
57 idPluecker & operator*=( const float a );
58 idPluecker & operator/=( const float a );
59 idPluecker & operator+=( const idPluecker &a );
60 idPluecker & operator-=( const idPluecker &a );
61
62 bool Compare( const idPluecker &a ) const; // exact compare, no epsilon
63 bool Compare( const idPluecker &a, const float epsilon ) const; // compare with epsilon
64 bool operator==( const idPluecker &a ) const; // exact compare, no epsilon
65 bool operator!=( const idPluecker &a ) const; // exact compare, no epsilon
66
67 void Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 );
68 void Zero( void );
69
70 void FromLine( const idVec3 &start, const idVec3 &end ); // pluecker from line
71 void FromRay( const idVec3 &start, const idVec3 &dir ); // pluecker from ray
72 bool FromPlanes( const idPlane &p1, const idPlane &p2 ); // pluecker from intersection of planes
73 bool ToLine( idVec3 &start, idVec3 &end ) const; // pluecker to line
74 bool ToRay( idVec3 &start, idVec3 &dir ) const; // pluecker to ray
75 void ToDir( idVec3 &dir ) const; // pluecker to direction
76 float PermutedInnerProduct( const idPluecker &a ) const; // pluecker permuted inner product
77 float Distance3DSqr( const idPluecker &a ) const; // pluecker line distance
78
79 float Length( void ) const; // pluecker length
80 float LengthSqr( void ) const; // pluecker squared length
81 idPluecker Normalize( void ) const; // pluecker normalize
82 float NormalizeSelf( void ); // pluecker normalize
83
84 int GetDimension( void ) const;
85
86 const float * ToFloatPtr( void ) const;
87 float * ToFloatPtr( void );
88 const char * ToString( int precision = 2 ) const;
89
90 private:
91 float p[6];
92 };
93
94 extern idPluecker pluecker_origin;
95 #define pluecker_zero pluecker_origin
96
idPluecker(void)97 ID_INLINE idPluecker::idPluecker( void ) {
98 }
99
idPluecker(const float * a)100 ID_INLINE idPluecker::idPluecker( const float *a ) {
101 memcpy( p, a, 6 * sizeof( float ) );
102 }
103
idPluecker(const idVec3 & start,const idVec3 & end)104 ID_INLINE idPluecker::idPluecker( const idVec3 &start, const idVec3 &end ) {
105 FromLine( start, end );
106 }
107
idPluecker(const float a1,const float a2,const float a3,const float a4,const float a5,const float a6)108 ID_INLINE idPluecker::idPluecker( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
109 p[0] = a1;
110 p[1] = a2;
111 p[2] = a3;
112 p[3] = a4;
113 p[4] = a5;
114 p[5] = a6;
115 }
116
117 ID_INLINE idPluecker idPluecker::operator-() const {
118 return idPluecker( -p[0], -p[1], -p[2], -p[3], -p[4], -p[5] );
119 }
120
121 ID_INLINE float idPluecker::operator[]( const int index ) const {
122 return p[index];
123 }
124
125 ID_INLINE float &idPluecker::operator[]( const int index ) {
126 return p[index];
127 }
128
129 ID_INLINE idPluecker idPluecker::operator*( const float a ) const {
130 return idPluecker( p[0]*a, p[1]*a, p[2]*a, p[3]*a, p[4]*a, p[5]*a );
131 }
132
133 ID_INLINE float idPluecker::operator*( const idPluecker &a ) const {
134 return p[0] * a.p[4] + p[1] * a.p[5] + p[2] * a.p[3] + p[4] * a.p[0] + p[5] * a.p[1] + p[3] * a.p[2];
135 }
136
137 ID_INLINE idPluecker idPluecker::operator/( const float a ) const {
138 float inva;
139
140 assert( a != 0.0f );
141 inva = 1.0f / a;
142 return idPluecker( p[0]*inva, p[1]*inva, p[2]*inva, p[3]*inva, p[4]*inva, p[5]*inva );
143 }
144
145 ID_INLINE idPluecker idPluecker::operator+( const idPluecker &a ) const {
146 return idPluecker( p[0] + a[0], p[1] + a[1], p[2] + a[2], p[3] + a[3], p[4] + a[4], p[5] + a[5] );
147 }
148
149 ID_INLINE idPluecker idPluecker::operator-( const idPluecker &a ) const {
150 return idPluecker( p[0] - a[0], p[1] - a[1], p[2] - a[2], p[3] - a[3], p[4] - a[4], p[5] - a[5] );
151 }
152
153 ID_INLINE idPluecker &idPluecker::operator*=( const float a ) {
154 p[0] *= a;
155 p[1] *= a;
156 p[2] *= a;
157 p[3] *= a;
158 p[4] *= a;
159 p[5] *= a;
160 return *this;
161 }
162
163 ID_INLINE idPluecker &idPluecker::operator/=( const float a ) {
164 float inva;
165
166 assert( a != 0.0f );
167 inva = 1.0f / a;
168 p[0] *= inva;
169 p[1] *= inva;
170 p[2] *= inva;
171 p[3] *= inva;
172 p[4] *= inva;
173 p[5] *= inva;
174 return *this;
175 }
176
177 ID_INLINE idPluecker &idPluecker::operator+=( const idPluecker &a ) {
178 p[0] += a[0];
179 p[1] += a[1];
180 p[2] += a[2];
181 p[3] += a[3];
182 p[4] += a[4];
183 p[5] += a[5];
184 return *this;
185 }
186
187 ID_INLINE idPluecker &idPluecker::operator-=( const idPluecker &a ) {
188 p[0] -= a[0];
189 p[1] -= a[1];
190 p[2] -= a[2];
191 p[3] -= a[3];
192 p[4] -= a[4];
193 p[5] -= a[5];
194 return *this;
195 }
196
Compare(const idPluecker & a)197 ID_INLINE bool idPluecker::Compare( const idPluecker &a ) const {
198 return ( ( p[0] == a[0] ) && ( p[1] == a[1] ) && ( p[2] == a[2] ) &&
199 ( p[3] == a[3] ) && ( p[4] == a[4] ) && ( p[5] == a[5] ) );
200 }
201
Compare(const idPluecker & a,const float epsilon)202 ID_INLINE bool idPluecker::Compare( const idPluecker &a, const float epsilon ) const {
203 if ( idMath::Fabs( p[0] - a[0] ) > epsilon ) {
204 return false;
205 }
206
207 if ( idMath::Fabs( p[1] - a[1] ) > epsilon ) {
208 return false;
209 }
210
211 if ( idMath::Fabs( p[2] - a[2] ) > epsilon ) {
212 return false;
213 }
214
215 if ( idMath::Fabs( p[3] - a[3] ) > epsilon ) {
216 return false;
217 }
218
219 if ( idMath::Fabs( p[4] - a[4] ) > epsilon ) {
220 return false;
221 }
222
223 if ( idMath::Fabs( p[5] - a[5] ) > epsilon ) {
224 return false;
225 }
226
227 return true;
228 }
229
230 ID_INLINE bool idPluecker::operator==( const idPluecker &a ) const {
231 return Compare( a );
232 }
233
234 ID_INLINE bool idPluecker::operator!=( const idPluecker &a ) const {
235 return !Compare( a );
236 }
237
Set(const float a1,const float a2,const float a3,const float a4,const float a5,const float a6)238 ID_INLINE void idPluecker::Set( const float a1, const float a2, const float a3, const float a4, const float a5, const float a6 ) {
239 p[0] = a1;
240 p[1] = a2;
241 p[2] = a3;
242 p[3] = a4;
243 p[4] = a5;
244 p[5] = a6;
245 }
246
Zero(void)247 ID_INLINE void idPluecker::Zero( void ) {
248 p[0] = p[1] = p[2] = p[3] = p[4] = p[5] = 0.0f;
249 }
250
FromLine(const idVec3 & start,const idVec3 & end)251 ID_INLINE void idPluecker::FromLine( const idVec3 &start, const idVec3 &end ) {
252 p[0] = start[0] * end[1] - end[0] * start[1];
253 p[1] = start[0] * end[2] - end[0] * start[2];
254 p[2] = start[0] - end[0];
255 p[3] = start[1] * end[2] - end[1] * start[2];
256 p[4] = start[2] - end[2];
257 p[5] = end[1] - start[1];
258 }
259
FromRay(const idVec3 & start,const idVec3 & dir)260 ID_INLINE void idPluecker::FromRay( const idVec3 &start, const idVec3 &dir ) {
261 p[0] = start[0] * dir[1] - dir[0] * start[1];
262 p[1] = start[0] * dir[2] - dir[0] * start[2];
263 p[2] = -dir[0];
264 p[3] = start[1] * dir[2] - dir[1] * start[2];
265 p[4] = -dir[2];
266 p[5] = dir[1];
267 }
268
ToLine(idVec3 & start,idVec3 & end)269 ID_INLINE bool idPluecker::ToLine( idVec3 &start, idVec3 &end ) const {
270 idVec3 dir1, dir2;
271 float d;
272
273 dir1[0] = p[3];
274 dir1[1] = -p[1];
275 dir1[2] = p[0];
276
277 dir2[0] = -p[2];
278 dir2[1] = p[5];
279 dir2[2] = -p[4];
280
281 d = dir2 * dir2;
282 if ( d == 0.0f ) {
283 return false; // pluecker coordinate does not represent a line
284 }
285
286 start = dir2.Cross(dir1) * (1.0f / d);
287 end = start + dir2;
288 return true;
289 }
290
ToRay(idVec3 & start,idVec3 & dir)291 ID_INLINE bool idPluecker::ToRay( idVec3 &start, idVec3 &dir ) const {
292 idVec3 dir1;
293 float d;
294
295 dir1[0] = p[3];
296 dir1[1] = -p[1];
297 dir1[2] = p[0];
298
299 dir[0] = -p[2];
300 dir[1] = p[5];
301 dir[2] = -p[4];
302
303 d = dir * dir;
304 if ( d == 0.0f ) {
305 return false; // pluecker coordinate does not represent a line
306 }
307
308 start = dir.Cross(dir1) * (1.0f / d);
309 return true;
310 }
311
ToDir(idVec3 & dir)312 ID_INLINE void idPluecker::ToDir( idVec3 &dir ) const {
313 dir[0] = -p[2];
314 dir[1] = p[5];
315 dir[2] = -p[4];
316 }
317
PermutedInnerProduct(const idPluecker & a)318 ID_INLINE float idPluecker::PermutedInnerProduct( const idPluecker &a ) const {
319 return p[0] * a.p[4] + p[1] * a.p[5] + p[2] * a.p[3] + p[4] * a.p[0] + p[5] * a.p[1] + p[3] * a.p[2];
320 }
321
Length(void)322 ID_INLINE float idPluecker::Length( void ) const {
323 return ( float )idMath::Sqrt( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
324 }
325
LengthSqr(void)326 ID_INLINE float idPluecker::LengthSqr( void ) const {
327 return ( p[5] * p[5] + p[4] * p[4] + p[2] * p[2] );
328 }
329
NormalizeSelf(void)330 ID_INLINE float idPluecker::NormalizeSelf( void ) {
331 float l, d;
332
333 l = LengthSqr();
334 if ( l == 0.0f ) {
335 return l; // pluecker coordinate does not represent a line
336 }
337 d = idMath::InvSqrt( l );
338 p[0] *= d;
339 p[1] *= d;
340 p[2] *= d;
341 p[3] *= d;
342 p[4] *= d;
343 p[5] *= d;
344 return d * l;
345 }
346
Normalize(void)347 ID_INLINE idPluecker idPluecker::Normalize( void ) const {
348 float d;
349
350 d = LengthSqr();
351 if ( d == 0.0f ) {
352 return *this; // pluecker coordinate does not represent a line
353 }
354 d = idMath::InvSqrt( d );
355 return idPluecker( p[0]*d, p[1]*d, p[2]*d, p[3]*d, p[4]*d, p[5]*d );
356 }
357
GetDimension(void)358 ID_INLINE int idPluecker::GetDimension( void ) const {
359 return 6;
360 }
361
ToFloatPtr(void)362 ID_INLINE const float *idPluecker::ToFloatPtr( void ) const {
363 return p;
364 }
365
ToFloatPtr(void)366 ID_INLINE float *idPluecker::ToFloatPtr( void ) {
367 return p;
368 }
369
370 #endif /* !__MATH_PLUECKER_H__ */
371