CSL 5.2

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List
- File Members

PME.cpp
Go to the documentation of this file.
1 ///
2 /// PME.h -- Ventriloquist
3 /// See the copyright notice and acknowledgment of authors in the file COPYRIGHT
4 /// Doug McCoy, 2004.
5 ///
6
7 #include "PME.h"
8 #include <arpa/inet.h>
9
10 //extern float max_mu, min_mu, max_e, min_e, max_error, min_error;
11
12 double kTwoPi = 2.0 * M_PI;
13
14 void Controller::set_data (controller_str &str ){
15 sync.lock();
16 float x = (float)str.x / 32767.0;
17 float y = (float)str.y / 32767.0;
18 float z = (float)str.z / 32767.0;
19 float dx = (float)str.dx / 32767.0;
20 float dy = (float)str.dy / 32767.0;
21 float dz = (float)str.dz / 32767.0;
22 _position.set(x, y, z);
23 _velocity.set(dx, dy, dz);
24 // glove_state = str.glove_state;
25 sync.unlock();
26 }
27
28 void Controller::get_data(CPoint &p, CPoint &v, GloveState &glove_st ){
29 sync.lock();
30 p = _position;
31 v = _velocity;
32 glove_st = glove_state;
33 sync.unlock();
34 }
35
36 void Controller::get_position(CPoint &p ){
37 sync.lock();
38 p = _position;
39 sync.unlock();
40 }
41
42 //void * Controller::remote_read_func(void *data ){
43 // Controller * myself = (Controller*)data;
44 // controller_str received_data;
45 // string addr_recieved_from;
46 // unsigned short port_recv_from;
47 // string request_command = "send";
48 //
49 // while (1){
50 // // send request for controller data
51 // myself->sock.sendTo(&request_command, request_command.size(), myself->foreign_net_address, myself->foreign_port);
52 //
53 // myself->sock.recvFrom(&received_data, sizeof(received_data), addr_recieved_from, port_recv_from);
54 // myself->set_data(received_data);
55 // csl::sleep_sec(0.01);
56 // }
57 //}
58 //
59 //void Controller::start_reader_thread(){
60 // thread.create_thread(remote_read_func, this);
61 //}
62
63 void Controller::get_remote_data() {
64 controller_str received_data;
65 string addr_recieved_from;
66 unsigned short port_recv_from;
67 char * request_command = "send";
68
69 // send request for controller data
70 sock.sendTo(request_command, 5, foreign_net_address, foreign_port);
71
72 sock.recvFrom(&received_data, sizeof(received_data), addr_recieved_from, port_recv_from);
73 // cout << "recieved data" << endl;
74 received_data.x = ntohl(received_data.x);
75 received_data.y = ntohl(received_data.y);
76 received_data.z = ntohl(received_data.z);
77 received_data.dx = ntohl(received_data.dx);
78 received_data.dy = ntohl(received_data.dy);
79 received_data.dz = ntohl(received_data.dz);
80 received_data.glove_state = ntohl(received_data.glove_state);
81
82 if (1 == received_data.glove_state){
83
84 received_data.glove_state;
85 }
86
87 set_data(received_data);
88 }
89
90 PMESource::PMESource(SpatialSource & s)
91 : source(&s), current_move_type(kStopped),
92 bounce_distance(1.0), current_trace_index(0), trace_length(0) { };
93
94 PMESource::PMESource()
95 : source(NULL), current_move_type(kStopped),
96 bounce_distance(1.0), current_trace_index(0), trace_length(0) { };
97
98 void PMESource::set_position(CPoint &p ){
99 source->setPosition(p);
100 }
101
102 CPoint PMESource::get_position() {
103 return source->getPosition();
104 }
105
106 void PMESource::update_position() {
107 CPoint P, newP;
108 switch (current_move_type){
109 case kOrbit: {
110 orbit.calculate_new_position_in_orbit();
111 CPoint newpos;
112 orbit.calculate_absolute_position(newpos );
113 source->setPosition(newpos.x, newpos.y, newpos.z);
114 break;
115 }
116 case kDraw:
117 if (trace_length>0){
118 source->setPosition(trace[current_trace_index].x,
119 trace[current_trace_index].y,
120 trace[current_trace_index].z);
121 current_trace_index = (current_trace_index + 1) % trace_length;
122 }
123 break;
124
125 #define signof(x) (((x)<0)? -1.0 : 1.0)
126 case kBounce:
127 P = source->getPosition();
128 if (fabs(P.x) > bounce_distance)
129 P.x = signof(P.x) * bounce_distance;
130 if (fabs(P.y) > bounce_distance)
131 P.y = signof(P.y) * bounce_distance;
132 if (fabs(P.z) > bounce_distance)
133 P.z = signof(P.z) * bounce_distance;
134
135 newP = P + bounce_velocity;
136 if (fabs(newP.x) > bounce_distance)
137 bounce_velocity.x = -bounce_velocity.x;
138 if (fabs(newP.y) > bounce_distance)
139 bounce_velocity.y = -bounce_velocity.y;
140 if (fabs(newP.z) > bounce_distance)
141 bounce_velocity.z = -bounce_velocity.z;
142 newP = P + bounce_velocity;
143 source->setPosition(newP.x, newP.y, newP.z);
144 break;
145
146 case kGrabbed:
147 break;
148
149 case kStopped:
150 break;
151 }
152 }
153
154 void PMESource::set_orbit(CPoint &R, CPoint &V){
155 orbit.calculate_orbital_params(R, V );
156 }
157
158 void PMESource::set_next_move_type (MovementType m){
159 next_move_type = m;
160 }
161
162 void PMESource::set_current_move_type (MovementType m){
163 current_move_type = m;
164 }
165
166 void PMESource::set_bounce_distance(float bd){
167 bounce_distance = bd;
168 }
169
170 void PMESource::push_trace(CPoint &pos ){
171 if (trace_length >= MAX_TRACE_LENGTH){
172 cerr << "PMESource:: push_trace() - Trace full!" << endl;
173 return;
174 }
175 trace[trace_length] = pos;
176 trace_length++;
177 }
178
179 unsigned exit_count = 0;
180
181 #define RMAX (1.0) // meters
182 void Orbit::calculate_orbital_params(CPoint R, CPoint V){
183 calculate_eccentricity(R, V);
184 double v = V.len();
185 double r = R.len();
186 a = -0.5 * mu / ((v*v*0.5) - (mu / r) );
187 n = sqrt(mu/(a*a*a)); // radians per unit time
188 CPoint h = R^V; // cross product
189 i = acos(h.z / h.len() ); // h.z = h cross (0,0,1)
190 CPoint K(0, 0, 1);
191 n_vec = K^h;
192 omega = acos(n_vec.x / n_vec.len());
193 if (n_vec.y < 0)
194 omega = kTwoPi - omega;
195 double temp = n_vec * e_vec / (n_vec.len() * e_vec.len() );
196 w = acos(temp );
197 if (e_vec.z < 0)
198 w = kTwoPi - w;
199 nu = acos(e_vec * R / (e_vec.len() * R.len() ) );
200 if (R*V < 0)
201 nu = kTwoPi - nu;
202 }
203
204 void Orbit::calculate_eccentricity(CPoint R, CPoint V){
205 CPoint rtemp = R;
206 CPoint vtemp = V;
207 rtemp.normalize();
208 vtemp.normalize();
209 e = fabs(rtemp * vtemp); // dot product should be between 0 and 1
210 e = (e<0.99) ? e:0.99;
211
212 mu = 0.4; // first guess
213 double vs = V.len();
214 double r = R.len();
215 double error = 100.0; // some large number
216 double delta = -1.0;
217 double sign = -1.0;
218 unsigned c = 0;
219 while (c++ < 200000) {
220 if (c>20000)
221 c=c;
222 rtemp = R;
223 vtemp = V;
224 rtemp *= (vs*vs - (mu/r));
225 vtemp *= R*V;
226 e_vec = rtemp - vtemp;
227 e_vec *= (1.0/mu);
228
229 double old_error = error;
230 error = (e_vec.len() - e);
231 delta = error - old_error;
232
233 if (delta == 0.0){
234 cout << "Delta == 0" << endl << endl;
235 break;
236 }
237
238 if (fabs(error) < (0.0001 * e))
239 break;
240 if (delta >= 0.0)
241 sign = -sign;
242 mu *= 1.0 + (sign * error );
243 }
244 e = e_vec.len(); // correct initial e guess to actual value
245 cout << "e:\t" << e << endl;
246 cout << "error:\t" << error << endl;
247 //
248 // max_mu = max(max_mu, mu);
249 // min_mu = min(min_mu, mu);
250 // max_e = max(max_e, e);
251 // min_e = min(min_e, e);
252 //// max_error = max(max_error, error/e);
253 // min_error = min(min_error, error/e);
254
255 }
256
257 void Orbit::calculate_absolute_position(CPoint &newPosition){
258 double r = (a * (1.0 - e*e)) / (1.0 + e * cos(nu));
259 newPosition.x = r * (cos(omega)*cos(nu+w) - sin(omega)*sin(nu+w)*cos(i) );
260 newPosition.y = r * (sin(omega)*cos(nu+w) - cos(omega)*sin(nu+w)*cos(i) );
261 newPosition.z = r * (sin(nu+w)*sin(i) );
262
263 }
264
265 void Orbit::calculate_new_position_in_orbit(){
266 double E = acos((e+cos(nu)) / (1.0+e*cos(nu)) );
267 if (nu > M_PI)
268 E = kTwoPi - E;
269 double M = E - e*sin(E);
270 M = M + n;
271 while (M > (kTwoPi))
272 M -= kTwoPi;
273 double E_old;
274 E = M;
275 float error = 10.0; // initial large error
276 while (fabs(error) > 0.0001){
277 E_old = E;
278 E = M + e*sin(E);
279 error = E - E_old;
280 }
281 nu = acos((cos(E) - e) / (1.0 - e*cos(E)) );
282 if (E > M_PI)
283 nu = kTwoPi - nu;
284
285 }
286
287
288 void Orbit::dump(){
289 cout << "Orbit:"<< endl;
290 cout << "semi-major axis:\t" << a << endl;
291 cout << "eccentricity:\t" << e << endl;
292 cout << "inclination angle:\t" << i << endl;
293 cout << "longitude of ascending node:\t" << omega << endl;
294 cout << "argument of perigee:\t" << w << endl;
295 cout << "true anomaly:\t" << nu << endl;
296 cout << "mean motion:\t" << sqrt(mu / (a*a*a) ) << endl;
297 cout << "mu:\t\t\t" << mu << endl << endl;
298 }
299
300 PME::PME (string remote_net_addr, unsigned short remote_port)
301 : controller(remote_net_addr, remote_port), grabbed_source(NULL), num_sources(0) {
302 // pme_source_list = (PMESource**) malloc(MAX_NUM_VBAP_SOURCES * siizeof(PMESource*) );
303 pme_source_list = new PMESource*[ MAX_NUM_VBAP_SOURCES ];
304 }
305
306 PME::PME ()
307 : grabbed_source(NULL), num_sources(0) {
308 // pme_source_list = (PMESource**) malloc(MAX_NUM_VBAP_SOURCES * siizeof(PMESource*) );
309 pme_source_list = new PMESource*[ MAX_NUM_VBAP_SOURCES ];
310 }
311
312 PME::~PME(){ delete pme_source_list; }
313
314 void PME::update_grabbed_position(CPoint &p) {
315 // CVector p;
316 // controller.getPosition(p );
317 grabbed_source->set_position(p );
318 }
319
320 bool PME::add_pme_source(PMESource &s ){
321 if (num_sources == MAX_NUM_VBAP_SOURCES){
322 printf("Max number of VBAP sources exceeded. Source not added\n");
323 return false;
324 }
325 pme_source_list[num_sources++] = &s;
326 return true;
327
328 }
329
330 #define MIN_GRAB_DISTANCE_SQ (0.05)
331 bool PME::check_for_grabbed_source(CPoint &p ){
332 for (unsigned i=0; i<num_sources; i++){
333 CPoint pp = (pme_source_list[i]->get_position() );
334 double distance_sq = p.distance2(pp );
335 if (distance_sq < MIN_GRAB_DISTANCE_SQ){
336 grabbed_source = pme_source_list[i];
337 pme_source_list[i]->set_current_move_type(kGrabbed );
338 return true;
339 }
340 }
341 return false; // did not grab source
342 }
343
344 void PME::manage_sources(){
345 struct timeval start, end;
346 CPoint P, V;
347 GloveState gs;
348
349 // pme_move_type = kDraw;
350 keep_processing_sources = true;
351
352 while (keep_processing_sources){
353 GET_TIME(&start);
354 controller.get_remote_data();
355 controller.get_data(P, V, gs );
356 if (gs == kClosed && grabbed_source==NULL){ // glove just grabbed or holding source
357
358 if (check_for_grabbed_source(P ) ){ // finds near source and sets grabbed_source
359 grabbed_source->set_current_move_type(kGrabbed);
360 cout << "grabbed source"<<endl;
361 grabbed_source->reset_trace();
362 }
363 }
364 else if (grabbed_source != NULL ){
365 // cout << "moving grabbed source" << endl;
366 update_grabbed_position(P );
367 MovementType mt = grabbed_source->get_next_move_type();
368 if ((gs == kPoint) && (mt == kDraw) ){
369 grabbed_source->push_trace(P );
370 cout << "making trace" << endl;
371 }
372 }
373
374 if (grabbed_source != NULL && gs == kOpen){ // source just released
375 cout << "glove released" << endl;
376 if (V.len() < 0.005)
377 grabbed_source->set_current_move_type(kStopped);
378 grabbed_source->update_move_type();
379 switch (grabbed_source->get_current_move_type()){
380 case kDraw:
381 break;
382 case kOrbit:
383 grabbed_source->set_orbit(P, V );
384 break;
385 case kBounce:
386 grabbed_source->set_bounce_velocity(V );
387 }
388 grabbed_source = NULL;
389 }
390
391 for (unsigned i=0; i<num_sources; i++){
392 pme_source_list[i]->update_position();
393 }
394
395 GET_TIME(&end);
396
397 struct timeval *s = &start, *e = &end;
398 unsigned diff = SUB_TIMES(e, s );
399 // cout<<"time:\t" << diff<<endl;
400 if (diff < 10000)
401 csl::sleepUsec(10000-diff ); // each loop should now take 0.01 seconds
402 // else
403 // int temp=0;
404 }
405 }
406
407 // thread functions
408
409 void * PME::management_func(void *data){
410 PME *me = (PME*)data;
411 me->manage_sources();
412 return 0;
413 }
414
415 void PME::start_management_thread(){
416 management_thread.createThread(management_func, (void*)this);
417 }
418
419 void PME::stop_management_thread(){
420 keep_processing_sources = false;
421 }