close Warning: Can't use blame annotator: 
svn blame failed on trunk/lib/FlairSimulator/src/Model_impl.cpp: 200029 - Couldn't perform atomic initialization

source: flair-src/trunk/lib/FlairSimulator/src/Model_impl.cpp@ 8

Last change on this file since 8 was 8, checked in by Sanahuja Guillaume, 8 years ago

simulator
File size: 9.5 KB
RevLine 
1// created: 2013/03/25
2// filename: Model_impl.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: classe definissant un modele a simuler
10//
11/*********************************************************************/
12
13#include "Model.h"
14#include "Model_impl.h"
15#include "Simulator.h"
16#include "TabWidget.h"
17#include "Tab.h"
18#include "DoubleSpinBox.h"
19#include "Vector3DSpinBox.h"
20#include "SpinBox.h"
21#include "CheckBox.h"
22#include "cvmatrix.h"
23#include "Euler.h"
24#include <math.h>
25
26#ifdef GL
27#include "ConditionVariable.h"
28#include "Gui.h"
29#include "Gui_impl.h"
30#include <ISceneManager.h>
31#include <ISceneNodeAnimatorCollisionResponse.h>
32#include <IMetaTriangleSelector.h>
33#include <IVideoDriver.h>
34#include <ICameraSceneNode.h>
35#include "AnimPoursuite.h"
36using namespace irr;
37using namespace video;
38using namespace scene;
39using namespace core;
40using namespace io;
41#endif
42
43using namespace flair::core;
44using namespace flair::gui;
45using namespace flair::simulator;
46
47#ifdef GL
48Model_impl::Model_impl(Model* self,std::string name,ISceneManager* scenemanager,vrpn_Connection_IP* vrpn)
49 : ISceneNode(scenemanager->getRootSceneNode(), scenemanager, -1),Thread(self,name,50),vrpn_Tracker( name.c_str(), vrpn )
50
51#else
52Model_impl::Model_impl(Model* self,std::string name,vrpn_Connection_IP* vrpn)
53 : Thread(self,name,50),vrpn_Tracker( name.c_str(), vrpn )
54#endif
55{
56 this->self=self;
57
58#ifdef GL
59 //for sync with gui
60 cond=new ConditionVariable(this,name);
61 sync_count=0;
62
63 //collisions
64 collision_mutex=new Mutex(this);
65 collision_occured=false;
66
67 //selector for collisions
68 selector = getSceneManager()->createTriangleSelectorFromBoundingBox(this);
69 setTriangleSelector(selector);
70 meta_selector = getSceneManager()->createMetaTriangleSelector();
71
72 anim = getSceneManager()->createCollisionResponseAnimator(meta_selector, this,vector3df(1,1,1),vector3df(0,0,0), vector3df(0,0,0));
73 addAnimator(anim);
74
75 //camera
76 camera =getSceneManager()->addCameraSceneNode();
77 camera->setAspectRatio(getGui()->getAspectRatio());//on force a cause du view port
78 camera->setUpVector(vector3df(0,0,1));
79
80 animator=new AnimPoursuite(this);
81 camera->addAnimator(animator);
82 camera->setFarValue(8000);
83
84 position_init=false;
85#endif
86
87 //init user interface
88 Tab* tab=new Tab(getSimulator()->GetTabWidget(),ObjectName());
89 tabwidget=new TabWidget(tab->NewRow(),"tabs");
90 Tab* sampl=new Tab(tabwidget,"sampling");
91 dT=new DoubleSpinBox(sampl->NewRow(),"Tech (s):",0.001,1,0.001,3);
92 Tab* layout=new Tab(tabwidget,"optitrack");
93 enable_opti=new CheckBox(layout->NewRow(),"enabled");
94 Tab* init=new Tab(tabwidget,"init");
95 pos_init=new Vector3DSpinBox(init->NewRow(),"position",-50,50,1);
96 yaw_init=new SpinBox(init->NewRow(),"yaw (deg):",-180,180,10);
97
98 //modele
99 states_mutex=new Mutex(this);
100 self->state[0].Pos=pos_init->Value();
101 self->state[0].Vel.x=0;
102 self->state[0].Vel.y=0;
103 self->state[0].Vel.z=0;
104 self->state[0].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
105 self->state[0].W.x=0;
106 self->state[0].W.y=0;
107 self->state[0].W.z=0;
108
109 self->state[-1]=self->state[0];
110 self->state[-2]=self->state[0];
111
112 cvmatrix_descriptor* desc=new cvmatrix_descriptor(13,1);
113 desc->SetElementName(0,0,"q0");
114 desc->SetElementName(1,0,"q1");
115 desc->SetElementName(2,0,"q2");
116 desc->SetElementName(3,0,"q3");
117 desc->SetElementName(4,0,"x");
118 desc->SetElementName(5,0,"y");
119 desc->SetElementName(6,0,"z");
120 desc->SetElementName(7,0,"wx");
121 desc->SetElementName(8,0,"wy");
122 desc->SetElementName(9,0,"wz");
123 desc->SetElementName(10,0,"vx");
124 desc->SetElementName(11,0,"vy");
125 desc->SetElementName(12,0,"vz");
126 output=new cvmatrix(this,desc,floatType,"state");
127
128 self->AddDataToLog(output);
129}
130
131Model_impl::~Model_impl()
132{
133 SafeStop();
134 Join();
135#ifdef GL
136 remove();//remove ISceneNode
137#endif
138}
139
140Quaternion Model_impl::ComputeInitRotation(Quaternion quat_in) {
141 Quaternion yaw_rot_quat;
142 Euler yaw_rot_euler(0,0,Euler::ToRadian(yaw_init->Value()));
143 yaw_rot_euler.ToQuaternion(yaw_rot_quat);
144 return yaw_rot_quat*quat_in;
145}
146
147void Model_impl::mainloop(void)
148{
149 if(enable_opti->Value()==false) return;
150 vrpn_gettimeofday(&_timestamp, NULL);
151 vrpn_Tracker::timestamp = _timestamp;
152
153 //change to vrpn reference
154 states_mutex->GetMutex();
155 Quaternion quat=getSimulator()->ToVRPNReference(self->state[0].Quat);
156 Vector3D position=getSimulator()->ToVRPNReference(self->state[0].Pos);
157 states_mutex->ReleaseMutex();
158
159 pos[0]=position.x;
160 pos[1]=position.y;
161 pos[2]=position.z;
162 //warning: d_quat is defined as (qx,qy,qz,qw), which is different from flair::core::Quaternion
163 d_quat[0] = quat.q1;
164 d_quat[1] = quat.q2;
165 d_quat[2] = quat.q3;
166 d_quat[3] = quat.q0;
167
168 char msgbuf[1000];
169
170 d_sensor = 0;
171
172 int len = vrpn_Tracker::encode_to(msgbuf);
173
174 if (d_connection->pack_message(len, _timestamp, position_m_id, d_sender_id, msgbuf, vrpn_CONNECTION_LOW_LATENCY))
175 {
176 fprintf(stderr,"can't write message: tossing\n");
177 }
178
179 server_mainloop();
180}
181
182#ifdef GL
183ITriangleSelector* Model_impl::TriangleSelector(void)
184{
185 return selector;
186}
187
188IMetaTriangleSelector* Model_impl::MetaTriangleSelector(void)
189{
190 return meta_selector;
191}
192
193void Model_impl::UpdatePos(void)
194{
195 vector3df nodePosition;
196 Quaternion nodeOrientation;
197 Euler euler;
198
199 states_mutex->GetMutex();
200 nodePosition=ToIrrlichtCoordinates(self->state[0].Pos);
201 nodeOrientation=ToIrrlichtOrientation(self->state[0].Quat);
202 states_mutex->ReleaseMutex();
203
204 setPosition(nodePosition);
205
206 nodeOrientation.ToEuler(euler);
207 ISceneNode::setRotation(Euler::ToDegree(1)*vector3df(euler.roll,euler.pitch,euler.yaw));
208
209 if(position_init==false)
210 {
211 anim->setTargetNode(this); // a faire pour se teleporter sans les collisions
212 position_init=true;
213 }
214
215 self->AnimateModel();
216}
217
218void Model_impl::CheckCollision(void)
219{
220 //TODO: setEllipsoidRadius should be called in Model::setScale
221 //but we need to call recalculateBoundingBox
222 anim->setEllipsoidRadius(getTransformedBoundingBox().getExtent());
223
224 if(anim->collisionOccurred()==true)
225 {
226 vector3df pos;
227 vector3df pos_rel;
228 vector3df nodePosition;
229 pos= anim->getCollisionPoint();
230 nodePosition=getPosition();
231 pos_rel=pos-nodePosition;
232 //printf("collision %f %f %f\n",pos.X,pos.Y,pos.Z);
233 //printf("drone %f %f %f\n",nodePosition.X,nodePosition.Y,nodePosition.Z);
234 //printf("rel %f %f %f\n",pos_rel.X,pos_rel.Z,pos_rel.Y);
235
236 collision_mutex->GetMutex();
237 collision_occured=true;
238 collision_point=ToSimulatorCoordinates(nodePosition);
239 collision_mutex->ReleaseMutex();
240 }
241}
242
243void Model_impl::CollisionHandler(void)
244{
245 collision_mutex->GetMutex();
246 if(collision_occured==true)
247 {
248 collision_occured=false;
249 states_mutex->GetMutex();
250 self->state[0].Pos=collision_point;
251 self->state[-1].Pos=self->state[0].Pos;
252 self->state[-2].Pos=self->state[0].Pos;
253 states_mutex->ReleaseMutex();
254 }
255 collision_mutex->ReleaseMutex();
256}
257
258void Model_impl::OnRegisterSceneNode(void)
259{
260 if (IsVisible)
261 SceneManager->registerNodeForRendering(this);
262
263 ISceneNode::OnRegisterSceneNode();
264}
265
266void Model_impl::render(void)
267{
268 IVideoDriver* driver = SceneManager->getVideoDriver();
269 driver->setTransform(ETS_WORLD, AbsoluteTransformation);
270}
271
272//le premier arrive attend l'autre
273void Model_impl::SynchronizationPoint()
274{
275 cond->GetMutex();
276 sync_count++;
277
278 if (sync_count < 2)
279 {
280 cond->CondWait();
281 }
282 else
283 {
284 cond->CondSignal();
285 }
286
287 cond->ReleaseMutex();
288}
289#endif //GL
290
291void Model_impl::Run(void)
292{
293 // Ask Xenomai to warn us upon switches to secondary mode.
294 WarnUponSwitches(true);
295
296#ifdef GL
297 //synchronize with gui
298 SynchronizationPoint();
299#endif
300
301 SetPeriodMS(dT->Value()*1000.);
302
303 while(!ToBeStopped())
304 {
305 if(dT->ValueChanged()) SetPeriodMS(dT->Value()*1000.);
306 WaitPeriod();
307
308#ifdef GL
309 CollisionHandler();
310#endif
311 states_mutex->GetMutex();
312 self->CalcModel();
313
314 output->GetMutex();
315 output->SetValueNoMutex(0,0,self->state[0].Quat.q0);
316 output->SetValueNoMutex(1,0,self->state[0].Quat.q1);
317 output->SetValueNoMutex(2,0,self->state[0].Quat.q2);
318 output->SetValueNoMutex(3,0,self->state[0].Quat.q3);
319 output->SetValueNoMutex(4,0,self->state[0].Pos.x);
320 output->SetValueNoMutex(5,0,self->state[0].Pos.y);
321 output->SetValueNoMutex(6,0,self->state[0].Pos.z);
322 output->SetValueNoMutex(7,0,self->state[0].W.x);
323 output->SetValueNoMutex(8,0,self->state[0].W.y);
324 output->SetValueNoMutex(9,0,self->state[0].W.z);
325 output->SetValueNoMutex(10,0,self->state[0].Vel.x);
326 output->SetValueNoMutex(11,0,self->state[0].Vel.y);
327 output->SetValueNoMutex(12,0,self->state[0].Vel.z);
328 output->ReleaseMutex();
329 output->SetDataTime(GetTime());
330
331 self->state.Update();
332
333 if(pos_init->ValueChanged() || yaw_init->ValueChanged())
334 {
335 self->state[-1].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
336 self->state[-2].Quat=ComputeInitRotation(Quaternion(1,0,0,0));
337 self->state[-1].Pos=pos_init->Value();
338 self->state[-2].Pos=self->state[-1].Pos;
339#ifdef GL
340 position_init=false;
341#endif
342 }
343
344 states_mutex->ReleaseMutex();
345
346 self->ProcessUpdate(output);
347
348 }
349
350 WarnUponSwitches(false);
351}
Note: See TracBrowser for help on using the repository browser.