| Geant4 Cross Reference |
1 // Copyright (C) 2010, Guy Barrand. All rights 1 // Copyright (C) 2010, Guy Barrand. All rights reserved.
2 // See the file tools.license for terms. 2 // See the file tools.license for terms.
3 3
4 #ifndef tools_line 4 #ifndef tools_line
5 #define tools_line 5 #define tools_line
6 6
7 namespace tools { 7 namespace tools {
8 8
9 // Parametric description: 9 // Parametric description:
10 // l(t) = pos + t * dir 10 // l(t) = pos + t * dir
11 11
12 template <class VEC3> 12 template <class VEC3>
13 class line { 13 class line {
14 protected: 14 protected:
15 typedef typename VEC3::elem_t T; 15 typedef typename VEC3::elem_t T;
16 public: 16 public:
17 line(){} 17 line(){}
18 line(const VEC3& a_p0,const VEC3& a_p1) { 18 line(const VEC3& a_p0,const VEC3& a_p1) {
19 // Construct a line from two points lying 19 // Construct a line from two points lying on the line. If you
20 // want to construct a line from a positio 20 // want to construct a line from a position and a direction, use
21 // line(p, p + d). 21 // line(p, p + d).
22 // line is directed from p0 to p1. 22 // line is directed from p0 to p1.
23 m_pos = a_p0; 23 m_pos = a_p0;
24 //m_dir = a_p1-a_p0; 24 //m_dir = a_p1-a_p0;
25 m_dir = a_p0; 25 m_dir = a_p0;
26 m_dir.multiply(-1); 26 m_dir.multiply(-1);
27 m_dir.add(a_p1); 27 m_dir.add(a_p1);
28 m_dir.normalize(); 28 m_dir.normalize();
29 } 29 }
30 line(const T& a_0_x,const T& a_0_y,const T& 30 line(const T& a_0_x,const T& a_0_y,const T& a_0_z,
31 const T& a_1_x,const T& a_1_y,const T& 31 const T& a_1_x,const T& a_1_y,const T& a_1_z) {
32 m_pos.set_value(a_0_x,a_0_y,a_0_z); 32 m_pos.set_value(a_0_x,a_0_y,a_0_z);
33 m_dir.set_value(a_1_x-a_0_x,a_1_y-a_0_y,a_ 33 m_dir.set_value(a_1_x-a_0_x,a_1_y-a_0_y,a_1_z-a_0_z);
34 m_dir.normalize(); 34 m_dir.normalize();
35 } 35 }
36 virtual ~line() {} 36 virtual ~line() {}
37 public: 37 public:
38 line(const line& a_from) 38 line(const line& a_from)
39 :m_pos(a_from.m_pos) 39 :m_pos(a_from.m_pos)
40 ,m_dir(a_from.m_dir) 40 ,m_dir(a_from.m_dir)
41 {} 41 {}
42 line& operator=(const line& a_from) { 42 line& operator=(const line& a_from) {
43 m_pos = a_from.m_pos; 43 m_pos = a_from.m_pos;
44 m_dir = a_from.m_dir; 44 m_dir = a_from.m_dir;
45 return *this; 45 return *this;
46 } 46 }
47 public: 47 public:
48 void set_value(const VEC3& a_p0,const VEC3& 48 void set_value(const VEC3& a_p0,const VEC3& a_p1) {
49 m_pos = a_p0; 49 m_pos = a_p0;
50 m_dir = a_p0; 50 m_dir = a_p0;
51 m_dir.multiply(-1); 51 m_dir.multiply(-1);
52 m_dir.add(a_p1); 52 m_dir.add(a_p1);
53 m_dir.normalize(); 53 m_dir.normalize();
54 } 54 }
55 void set_value(const T& a_0_x,const T& a_0_y 55 void set_value(const T& a_0_x,const T& a_0_y,const T& a_0_z,
56 const T& a_1_x,const T& a_1_y 56 const T& a_1_x,const T& a_1_y,const T& a_1_z) {
57 m_pos.set_value(a_0_x,a_0_y,a_0_z); 57 m_pos.set_value(a_0_x,a_0_y,a_0_z);
58 m_dir.set_value(a_1_x-a_0_x,a_1_y-a_0_y,a_ 58 m_dir.set_value(a_1_x-a_0_x,a_1_y-a_0_y,a_1_z-a_0_z);
59 m_dir.normalize(); 59 m_dir.normalize();
60 } 60 }
61 61
62 const VEC3& position() const {return m_pos;} 62 const VEC3& position() const {return m_pos;}
63 const VEC3& direction() const {return m_dir; 63 const VEC3& direction() const {return m_dir;}
64 64
65 /* not tested : 65 /* not tested :
66 void closest_point(const VEC3& a_point,VEC& 66 void closest_point(const VEC3& a_point,VEC& a_out) const {
67 //from coin3d/SbLine.cpp. 67 //from coin3d/SbLine.cpp.
68 68
69 // 69 //
70 // a_out 70 // a_out
71 // m_pos x-----x-------> m_dir 71 // m_pos x-----x-------> m_dir
72 // \ | 72 // \ |
73 // \ | 73 // \ |
74 // \ | 74 // \ |
75 // \ | 75 // \ |
76 // \| 76 // \|
77 // x a_point 77 // x a_point
78 78
79 a_out = m_pos + m_dir * ((a_point - m_pos) 79 a_out = m_pos + m_dir * ((a_point - m_pos).dot(m_dir));
80 } 80 }
81 81
82 82
83 bool closest_points(const line<T>& a_line,VE 83 bool closest_points(const line<T>& a_line,VEC3& a_on_this,VEC3& a_on_line) const {
84 //from coin3d/SbLine.cpp. 84 //from coin3d/SbLine.cpp.
85 85
86 //WARNING : if ret false, a_on_this, a_on_ 86 //WARNING : if ret false, a_on_this, a_on_line not set.
87 87
88 // Check if the lines are parallel. 88 // Check if the lines are parallel.
89 // FIXME: should probably use equals() her 89 // FIXME: should probably use equals() here.
90 if(a_line.m_dir == m_dir) return false; 90 if(a_line.m_dir == m_dir) return false;
91 if(a_line.m_dir == T(-1)*m_dir) return fal 91 if(a_line.m_dir == T(-1)*m_dir) return false;
92 92
93 VEC3 P0 = m_pos; 93 VEC3 P0 = m_pos;
94 VEC3 P1 = a_line.m_pos; 94 VEC3 P1 = a_line.m_pos;
95 VEC3 D0 = m_dir; 95 VEC3 D0 = m_dir;
96 VEC3 D1 = a_line.m_dir; 96 VEC3 D1 = a_line.m_dir;
97 VEC3 D0N = D0; 97 VEC3 D0N = D0;
98 98
99 T c[3], d[3]; 99 T c[3], d[3];
100 100
101 for(unsigned int i=0;i<3;i++) { 101 for(unsigned int i=0;i<3;i++) {
102 d[i] = D1[i] - D0N[i]*(D0[0]*D1[0] + D0[ 102 d[i] = D1[i] - D0N[i]*(D0[0]*D1[0] + D0[1]*D1[1] + D0[2]*D1[2]);
103 c[i] = P1[i] - P0[i] + D0N[i]*(D0[0]*P0[ 103 c[i] = P1[i] - P0[i] + D0N[i]*(D0[0]*P0[0] + D0[1]*P0[1] + D0[2]*P0[2]);
104 } 104 }
105 105
106 T den = d[0]*d[0]+d[1]*d[1]+d[2]*d[2]; 106 T den = d[0]*d[0]+d[1]*d[1]+d[2]*d[2];
107 if(den==T()) return false; 107 if(den==T()) return false;
108 108
109 T t = -(c[0]*d[0]+c[1]*d[1]+c[2]*d[2]) / d 109 T t = -(c[0]*d[0]+c[1]*d[1]+c[2]*d[2]) / den;
110 110
111 a_on_line = a_line.m_pos + a_line.m_dir * 111 a_on_line = a_line.m_pos + a_line.m_dir * t;
112 closest_point(a_on_line,a_on_this); 112 closest_point(a_on_line,a_on_this);
113 return true; 113 return true;
114 } 114 }
115 115
116 bool intersect(const line<T>& a_line,VEC3& a 116 bool intersect(const line<T>& a_line,VEC3& a_out,const T& a_prec) const {
117 VEC3 p,q; 117 VEC3 p,q;
118 if(!closest_points(a_line,p,q)) return fal 118 if(!closest_points(a_line,p,q)) return false;
119 if((q-p).length()>a_prec) return false; 119 if((q-p).length()>a_prec) return false;
120 a_out = p; 120 a_out = p;
121 return true; 121 return true;
122 } 122 }
123 */ 123 */
124 124
125 protected: 125 protected:
126 VEC3 m_pos; 126 VEC3 m_pos;
127 VEC3 m_dir; //normalized. 127 VEC3 m_dir; //normalized.
128 }; 128 };
129 129
130 } 130 }
131 131
132 #endif 132 #endif