| 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_geom3 4 #ifndef tools_geom3
5 #define tools_geom3 5 #define tools_geom3
6 6
7 #include "line" 7 #include "line"
8 #include "plane" 8 #include "plane"
9 #include "vec3" 9 #include "vec3"
10 10
11 namespace tools { 11 namespace tools {
12 12
13 template <class T> 13 template <class T>
14 class cubic { 14 class cubic {
15 public: 15 public:
16 cubic(const vec3<T>& a_p0,const vec3<T>& a_v 16 cubic(const vec3<T>& a_p0,const vec3<T>& a_v0,
17 const vec3<T>& a_p1,const vec3<T>& a_v 17 const vec3<T>& a_p1,const vec3<T>& a_v1) {
18 // Construct a cubic given 2 points and th 18 // Construct a cubic given 2 points and their tangents.
19 initialize(a_p0.x(),a_p0.y(),a_p0.z(), 19 initialize(a_p0.x(),a_p0.y(),a_p0.z(),
20 a_v0.x(),a_v0.y(),a_v0.z(), 20 a_v0.x(),a_v0.y(),a_v0.z(),
21 a_p1.x(),a_p1.y(),a_p1.z(), 21 a_p1.x(),a_p1.y(),a_p1.z(),
22 a_v1.x(),a_v1.y(),a_v1.z()); 22 a_v1.x(),a_v1.y(),a_v1.z());
23 } 23 }
24 cubic(const T& a_p0_x,const T& a_p0_y,const 24 cubic(const T& a_p0_x,const T& a_p0_y,const T& a_p0_z,
25 const T& a_v0_x,const T& a_v0_y,const 25 const T& a_v0_x,const T& a_v0_y,const T& a_v0_z,
26 const T& a_p1_x,const T& a_p1_y,const 26 const T& a_p1_x,const T& a_p1_y,const T& a_p1_z,
27 const T& a_v1_x,const T& a_v1_y,const 27 const T& a_v1_x,const T& a_v1_y,const T& a_v1_z){
28 initialize(a_p0_x,a_p0_y,a_p0_z, 28 initialize(a_p0_x,a_p0_y,a_p0_z,
29 a_v0_x,a_v0_y,a_v0_z, 29 a_v0_x,a_v0_y,a_v0_z,
30 a_p1_x,a_p1_y,a_p1_z, 30 a_p1_x,a_p1_y,a_p1_z,
31 a_v1_x,a_v1_y,a_v1_z); 31 a_v1_x,a_v1_y,a_v1_z);
32 } 32 }
33 33
34 virtual ~cubic() {} 34 virtual ~cubic() {}
35 public: 35 public:
36 cubic(const cubic& a_from) 36 cubic(const cubic& a_from)
37 :m_a(a_from.m_a) 37 :m_a(a_from.m_a)
38 ,m_b(a_from.m_b) 38 ,m_b(a_from.m_b)
39 ,m_c(a_from.m_c) 39 ,m_c(a_from.m_c)
40 ,m_d(a_from.m_d) 40 ,m_d(a_from.m_d)
41 {} 41 {}
42 cubic& operator=(const cubic& a_from) { 42 cubic& operator=(const cubic& a_from) {
43 m_a = a_from.m_a; 43 m_a = a_from.m_a;
44 m_b = a_from.m_b; 44 m_b = a_from.m_b;
45 m_c = a_from.m_c; 45 m_c = a_from.m_c;
46 m_d = a_from.m_d; 46 m_d = a_from.m_d;
47 return *this; 47 return *this;
48 } 48 }
49 public: 49 public:
50 void get_point(unsigned int a_index,unsigned 50 void get_point(unsigned int a_index,unsigned int a_num,vec3<T>& a_p){
51 //a_index = 0 is a_p0 51 //a_index = 0 is a_p0
52 //a_index = a_num-1 is a_p1 52 //a_index = a_num-1 is a_p1
53 T _s = T(a_index)/T(a_num-1); << 53 T s = T(a_index)/T(a_num-1);
54 T s2 = _s*_s; << 54 T s2 = s*s;
55 T s3 = s2*_s; << 55 T s3 = s2*s;
56 a_p = m_a*s3 + m_b*s2 + m_c*_s + m_d; << 56 a_p = m_a*s3 + m_b*s2 + m_c*s + m_d;
57 } 57 }
58 void get_point(unsigned int a_index,unsigned 58 void get_point(unsigned int a_index,unsigned int a_num,T& a_x,T& a_y,T& a_z){
59 //a_index = 0 is a_p0 59 //a_index = 0 is a_p0
60 //a_index = a_num-1 is a_p1 60 //a_index = a_num-1 is a_p1
61 T s = T(a_index)/T(a_num-1); 61 T s = T(a_index)/T(a_num-1);
62 T s2 = s*s; 62 T s2 = s*s;
63 T s3 = s2*s; 63 T s3 = s2*s;
64 a_x = m_a.x()*s3 + m_b.x()*s2 + m_c.x()*s 64 a_x = m_a.x()*s3 + m_b.x()*s2 + m_c.x()*s + m_d.x();
65 a_y = m_a.y()*s3 + m_b.y()*s2 + m_c.y()*s 65 a_y = m_a.y()*s3 + m_b.y()*s2 + m_c.y()*s + m_d.y();
66 a_z = m_a.z()*s3 + m_b.z()*s2 + m_c.z()*s 66 a_z = m_a.z()*s3 + m_b.z()*s2 + m_c.z()*s + m_d.z();
67 } 67 }
68 protected: 68 protected:
69 void initialize(const T& a_p0_x,const T& a_p 69 void initialize(const T& a_p0_x,const T& a_p0_y,const T& a_p0_z,
70 const T& a_v0_x,const T& a_v 70 const T& a_v0_x,const T& a_v0_y,const T& a_v0_z,
71 const T& a_p1_x,const T& a_p 71 const T& a_p1_x,const T& a_p1_y,const T& a_p1_z,
72 const T& a_v1_x,const T& a_v 72 const T& a_v1_x,const T& a_v1_y,const T& a_v1_z){
73 // Construct a cubic given 2 points and th 73 // Construct a cubic given 2 points and their tangents.
74 74
75 // f(s) = a s**3 + b s**2 + c s + d 75 // f(s) = a s**3 + b s**2 + c s + d
76 // f'(s) = 3 a s**2 + 2 b s + c 76 // f'(s) = 3 a s**2 + 2 b s + c
77 77
78 // f(0) = d = p0 78 // f(0) = d = p0
79 // f'(0) = c = v0 79 // f'(0) = c = v0
80 80
81 // f(1) = a + b + v0 + p0 = p1 81 // f(1) = a + b + v0 + p0 = p1
82 // f'(1) = 3 a + 2 b + v0 = v1 82 // f'(1) = 3 a + 2 b + v0 = v1
83 83
84 // f(1) = a + b = p1 - v0 - p0 84 // f(1) = a + b = p1 - v0 - p0
85 // f'(1) = 3 a + 2 b = v1 - v0 85 // f'(1) = 3 a + 2 b = v1 - v0
86 86
87 // b = 3(p1-v0-p0)-(v1-v0) 87 // b = 3(p1-v0-p0)-(v1-v0)
88 // a = p1-v0-p0 - b = p1-v0-p0-3(p1-v0-p0) 88 // a = p1-v0-p0 - b = p1-v0-p0-3(p1-v0-p0)+(v1-v0)
89 // a = -2p1 + v0 + 2p0 + v1 89 // a = -2p1 + v0 + 2p0 + v1
90 90
91 T a_x = -2*a_p1_x + a_v0_x + 2*a_p0_x + a_ 91 T a_x = -2*a_p1_x + a_v0_x + 2*a_p0_x + a_v1_x;
92 T a_y = -2*a_p1_y + a_v0_y + 2*a_p0_y + a_ 92 T a_y = -2*a_p1_y + a_v0_y + 2*a_p0_y + a_v1_y;
93 T a_z = -2*a_p1_z + a_v0_z + 2*a_p0_z + a_ 93 T a_z = -2*a_p1_z + a_v0_z + 2*a_p0_z + a_v1_z;
94 m_a.set_value(a_x,a_y,a_z); 94 m_a.set_value(a_x,a_y,a_z);
95 95
96 T b_x = 3*(a_p1_x - a_v0_x - a_p0_x) - (a_ 96 T b_x = 3*(a_p1_x - a_v0_x - a_p0_x) - (a_v1_x - a_v0_x);
97 T b_y = 3*(a_p1_y - a_v0_y - a_p0_y) - (a_ 97 T b_y = 3*(a_p1_y - a_v0_y - a_p0_y) - (a_v1_y - a_v0_y);
98 T b_z = 3*(a_p1_z - a_v0_z - a_p0_z) - (a_ 98 T b_z = 3*(a_p1_z - a_v0_z - a_p0_z) - (a_v1_z - a_v0_z);
99 m_b.set_value(b_x,b_y,b_z); 99 m_b.set_value(b_x,b_y,b_z);
100 100
101 m_c.set_value(a_v0_x,a_v0_y,a_v0_z); 101 m_c.set_value(a_v0_x,a_v0_y,a_v0_z);
102 m_d.set_value(a_p0_x,a_p0_y,a_p0_z); 102 m_d.set_value(a_p0_x,a_p0_y,a_p0_z);
103 103
104 } 104 }
105 105
106 protected: 106 protected:
107 vec3<T> m_a; 107 vec3<T> m_a;
108 vec3<T> m_b; 108 vec3<T> m_b;
109 vec3<T> m_c; 109 vec3<T> m_c;
110 vec3<T> m_d; 110 vec3<T> m_d;
111 }; 111 };
112 112
113 } 113 }
114 114
115 #include <vector> 115 #include <vector>
116 116
117 namespace tools { 117 namespace tools {
118 118
119 template <class VEC3> 119 template <class VEC3>
120 class clip { 120 class clip {
121 protected: 121 protected:
122 typedef typename VEC3::elem_t T; 122 typedef typename VEC3::elem_t T;
123 public: 123 public:
124 clip():m_cur(0){} 124 clip():m_cur(0){}
125 virtual ~clip() {} 125 virtual ~clip() {}
126 private: 126 private:
127 clip(const clip&):m_cur(0){} 127 clip(const clip&):m_cur(0){}
128 clip& operator=(const clip&){return *this;} 128 clip& operator=(const clip&){return *this;}
129 public: 129 public:
130 void reset() { 130 void reset() {
131 m_data[0].clear(); 131 m_data[0].clear();
132 m_data[1].clear(); 132 m_data[1].clear();
133 m_cur = 0; 133 m_cur = 0;
134 } 134 }
135 135
136 void add(const VEC3& a_point) {m_data[m_cur] 136 void add(const VEC3& a_point) {m_data[m_cur].push_back(a_point);}
137 137
138 void execute(const plane<VEC3>& plane) { 138 void execute(const plane<VEC3>& plane) {
139 //Clip polygon against plane. This might c 139 //Clip polygon against plane. This might change the number of
140 //vertices in the polygon. 140 //vertices in the polygon.
141 141
142 size_t n = m_data[m_cur].size(); 142 size_t n = m_data[m_cur].size();
143 if(!n) return; 143 if(!n) return;
144 144
145 // create a loop : 145 // create a loop :
146 VEC3 dummy = m_data[m_cur][0]; 146 VEC3 dummy = m_data[m_cur][0];
147 m_data[m_cur].push_back(dummy); 147 m_data[m_cur].push_back(dummy);
148 148
149 const VEC3& planeN = plane.normal(); 149 const VEC3& planeN = plane.normal();
150 150
151 for(size_t i = 0; i < n; i++) { 151 for(size_t i = 0; i < n; i++) {
152 VEC3 v0 = m_data[m_cur][i]; 152 VEC3 v0 = m_data[m_cur][i];
153 VEC3 v1 = m_data[m_cur][i+1]; 153 VEC3 v1 = m_data[m_cur][i+1];
154 154
155 T d0 = plane.distance(v0); 155 T d0 = plane.distance(v0);
156 T d1 = plane.distance(v1); 156 T d1 = plane.distance(v1);
157 157
158 if (d0 >= 0.0f && d1 < 0.0f) { // exit p 158 if (d0 >= 0.0f && d1 < 0.0f) { // exit plane
159 VEC3 dir = v1-v0; 159 VEC3 dir = v1-v0;
160 // we know that v0 != v1 since we got 160 // we know that v0 != v1 since we got here
161 dir.normalize(); 161 dir.normalize();
162 T dot = dir.dot(planeN); 162 T dot = dir.dot(planeN);
163 VEC3 newvertex = v0 - dir * (d0/dot); 163 VEC3 newvertex = v0 - dir * (d0/dot);
164 out_point(newvertex); 164 out_point(newvertex);
165 } else if (d0 < 0.0f && d1 >= 0.0f) { // 165 } else if (d0 < 0.0f && d1 >= 0.0f) { // enter plane
166 VEC3 dir = v1-v0; 166 VEC3 dir = v1-v0;
167 // we know that v0 != v1 since we got 167 // we know that v0 != v1 since we got here
168 dir.normalize(); 168 dir.normalize();
169 T dot = dir.dot(planeN); 169 T dot = dir.dot(planeN);
170 VEC3 newvertex = v0 - dir * (d0/dot); 170 VEC3 newvertex = v0 - dir * (d0/dot);
171 out_point(newvertex); 171 out_point(newvertex);
172 out_point(v1); 172 out_point(v1);
173 } else if (d0 >= 0.0f && d1 >= 0.0f) { / 173 } else if (d0 >= 0.0f && d1 >= 0.0f) { // in plane
174 out_point(v1); 174 out_point(v1);
175 } 175 }
176 } 176 }
177 m_data[m_cur].clear(); 177 m_data[m_cur].clear();
178 m_cur ^= 1; 178 m_cur ^= 1;
179 } 179 }
180 180
181 const std::vector<VEC3>& result() const {ret 181 const std::vector<VEC3>& result() const {return m_data[m_cur];}
182 182
183 protected: 183 protected:
184 void out_point(const VEC3& a_p) {m_data[m_cu 184 void out_point(const VEC3& a_p) {m_data[m_cur ^ 1].push_back(a_p);}
185 185
186 protected: 186 protected:
187 std::vector<VEC3> m_data[2]; 187 std::vector<VEC3> m_data[2];
188 unsigned int m_cur; 188 unsigned int m_cur;
189 }; 189 };
190 190
191 } 191 }
192 192
193 #endif 193 #endif