AnimatLab  2
Test
BulletAnimatSim/Bullet_UnitTests/Rotation_Tests.cpp
1 #include "stdafx.h"
2 #include <boost/test/unit_test.hpp>
3 #include <boost/shared_ptr.hpp>
4 #include <boost/date_time/posix_time/posix_time.hpp>
5 #include <boost/thread/thread.hpp>
6 
7 BOOST_AUTO_TEST_SUITE( Rotation_Suite )
8 
9 bool is_critical( CStdErrorInfo const& ex ) { return ex.m_lError < 0; }
10 
11 
12 void normalise(float &x, float &y, float &z, float &w, float fltTolerance)
13 {
14  // Don't normalize if we don't have to
15  float mag2 = w * w + x * x + y * y + z * z;
16  if (fabs(mag2) > fltTolerance && fabs(mag2 - 1.0f) > fltTolerance) {
17  float mag = sqrt(mag2);
18  w /= mag;
19  x /= mag;
20  y /= mag;
21  z /= mag;
22  }
23 }
24 
25 // Convert from Euler Angles
26 osg::Quat FromEuler(float pitch, float yaw, float roll)
27 {
28  float rollOver2 = roll * 0.5f;
29  float sinRollOver2 = (float)sin((double)rollOver2);
30  float cosRollOver2 = (float)cos((double)rollOver2);
31  float pitchOver2 = pitch * 0.5f;
32  float sinPitchOver2 = (float)sin((double)pitchOver2);
33  float cosPitchOver2 = (float)cos((double)pitchOver2);
34  float yawOver2 = yaw * 0.5f;
35  float sinYawOver2 = (float)sin((double)yawOver2);
36  float cosYawOver2 = (float)cos((double)yawOver2);
37 
38  float x = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
39  float y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
40  float z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
41  float w = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
42 
43  osg::Quat result(x, y, z, w);
44 
45  return result;
46 }
47 
48 //osg::Matrix NewSetupMatrix(CStdFPoint &localPos, CStdFPoint &localRot)
49 //{
50 // osg::Matrix osgLocalMatrix;
51 // osg::Vec3d vLoc(localPos.x, localPos.y, localPos.z);
52 // osg::Vec3d vEuler(localRot.x, localRot.y, localRot.z);
53 // OsgMatrixUtil::PositionAndHprRadToMatrix(osgLocalMatrix, vLoc, vEuler);
54 //
55 // return osgLocalMatrix;
56 //}
57 
58 
59 osg::Matrix NewSetupMatrix1(CStdFPoint &localPos, osg::Quat qRot)
60 {
61  osg::Matrix osgLocalMatrix;
62  osgLocalMatrix.makeIdentity();
63 
64  //convert cstdpoint to osg::Vec3
65  osg::Vec3 vPos(localPos.x, localPos.y, localPos.z);
66 
67  //build the matrix
68  osgLocalMatrix.makeRotate(qRot);
69  osgLocalMatrix.setTrans(vPos);
70 
71  return osgLocalMatrix;
72 }
73 
74 osg::Matrix NewSetupMatrix(CStdFPoint &localPos, CStdFPoint &localRot)
75 {
76  //osg::Quat q = FromEuler(localRot.y, localRot.z, localRot.x);
77 
78  osg::Vec3 vPos(localPos.x, localPos.y, localPos.z);
79  osg::Matrix m;
80  m.makeIdentity();
81  m.makeRotate(localRot.z, osg::Vec3d(0, 0, 1), localRot.y, osg::Vec3d(0, 1, 0), localRot.x, osg::Vec3d(1, 0, 0));
82  m.setTrans(vPos);
83  return m;
84 
85  //return NewSetupMatrix1(localPos, q);
86 }
87 
88 osg::Matrix OldSetupMatrix(CStdFPoint &localPos, CStdFPoint &localRot)
89 {
90  //Vx::VxReal3 vLoc = {localPos.x, localPos.y, localPos.z};
91  //Vx::VxReal3 vRot = {localRot.x, localRot.y, localRot.z};
92  //Vx::VxTransform vTrans = Vx::VxTransform::createFromTranslationAndEulerAngles(vLoc, vRot);
93 
94  osg::Matrix osgLocalMatrix;
95  //VxOSG::copyVxReal44_to_OsgMatrix(osgLocalMatrix, vTrans.m);
96 
97  // Vx::VxQuaternion vQuat(vTrans);
98  //
99  // osg::Quat q(vQuat[0], vQuat[1], vQuat[2], vQuat[3]);
100  // osg::Matrix matT = NewSetupMatrix1(localPos, q);
101 
102  return osgLocalMatrix;
103 }
104 
105 
106 osg::Quat OldSetupMatrixQuat(CStdFPoint &localPos, CStdFPoint &localRot)
107 {
108  //Vx::VxReal3 vLoc = {localPos.x, localPos.y, localPos.z};
109  //Vx::VxReal3 vRot = {localRot.x, localRot.y, localRot.z};
110  //Vx::VxTransform vTrans = Vx::VxTransform::createFromTranslationAndEulerAngles(vLoc, vRot);
111 
112  //osg::Matrix osgLocalMatrix;
113  //VxOSG::copyVxReal44_to_OsgMatrix(osgLocalMatrix, vTrans.m);
114 
115  // Vx::VxQuaternion vQuat(vTrans);
116 
117  // Vx::VxQuaternion vQuat2;
118  // vQuat2.fromEulerXYZ(localRot.x, localRot.y, localRot.z);
119 
120  osg::Quat q; //(vQuat2[0], vQuat2[1], vQuat2[2], vQuat2[3]);
121  return q;
122 }
123 
124 CStdFPoint OldEulerRotationFromMatrix(osg::Matrix osgMT)
125 {
126  //Now lets get the euler angle rotation
127  //Vx::VxReal44 vxTM;
128  //VxOSG::copyOsgMatrix_to_VxReal44(osgMT, vxTM);
129  //Vx::VxTransform vTrans(vxTM);
130  //Vx::VxReal3 vEuler;
131  //vTrans.getRotationEulerAngles(vEuler);
132  CStdFPoint vRot; //(vEuler[0], vEuler[1] ,vEuler[2]);
133 
134  vRot.ClearNearZero();
135  return vRot;
136 }
137 
138 bool QuatEqual(osg::Quat q1, osg::Quat q2)
139 {
140  if( (fabs(q1.x()-q2.x()) < 1e-5) && (fabs(q1.y()-q2.y()) < 1e-5) && (fabs(q1.z()-q2.z()) < 1e-5) && (fabs(q1.w()-q2.w()) < 1e-5) )
141  return true;
142  else
143  return false;
144 }
145 
146 
147 //----------------------------------------------------------------------------
148 CStdFPoint ExtractEulerXYZ (osg::Matrix osgMT)
149 {
150  //We need to transpose the matrix that osg provides for us in
151  //order to do the following calculations.
152  osg::Matrix3 osgMT3(osgMT(0, 0), osgMT(1, 0), osgMT(2, 0),
153  osgMT(0, 1), osgMT(1, 1), osgMT(2, 1),
154  osgMT(0, 2), osgMT(1, 2), osgMT(2, 2));
155 
156  // +- -+ +- -+
157  // | r00 r01 r02 | | cy*cz -cy*sz sy |
158  // | r10 r11 r12 | = | cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx |
159  // | r20 r21 r22 | | -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy |
160  // +- -+ +- -+
161  float xAngle=0, yAngle=0, zAngle=0;
162 
163  if (osgMT3(0, 2) < 1)
164  {
165  if (osgMT3(0, 2) > -1)
166  {
167  // y_angle = asin(r02)
168  // x_angle = atan2(-r12,r22)
169  // z_angle = atan2(-r01,r00)
170  yAngle = (float) asin((double) osgMT3(0, 2));
171  xAngle = (float) atan2((double) -osgMT3(1, 2), (double) osgMT3(2, 2));
172  zAngle = (float) atan2((double) -osgMT3(0, 1), (double) osgMT3(0, 0));
173  //return EA_UNIQUE;
174  }
175  else
176  {
177  // y_angle = -pi/2
178  // z_angle - x_angle = atan2(r10,r11)
179  // WARNING. The solution is not unique. Choosing z_angle = 0.
180  yAngle = -(osg::PI/2);
181  xAngle = -atan2((double) osgMT3(1, 0), (double) osgMT3(1, 1));
182  zAngle = 0;
183  //return EA_NOT_UNIQUE_DIF;
184  }
185  }
186  else
187  {
188  // y_angle = +pi/2
189  // z_angle + x_angle = atan2(r10,r11)
190  // WARNING. The solutions is not unique. Choosing z_angle = 0.
191  yAngle = osg::PI/2;
192  xAngle = atan2((double) osgMT3(1, 0), (double) osgMT3(1, 1));
193  zAngle = 0;
194  //return EA_NOT_UNIQUE_SUM;
195  }
196 
197  CStdFPoint vRot(xAngle, yAngle, zAngle);
198  vRot.ClearNearZero();
199 
200  return vRot;
201 }
202 //
203 //BOOST_AUTO_TEST_CASE( CompareOldNewSetupMatrix )
204 //{
205 // //CStdFPoint vRot(0, 0, 0);
206 //
207 // //CStdPtrArray<CStdFPoint> m_aryPos;
208 // //m_aryPos.Add(new CStdFPoint(0, 0, 0));
209 // //m_aryPos.Add(new CStdFPoint(-10, 0, 0));
210 // //m_aryPos.Add(new CStdFPoint(10, 0, 10));
211 // //m_aryPos.Add(new CStdFPoint(10, 10, 10));
212 //
213 // ////int iPosCount = m_aryPos.GetSize();
214 // ////for(int iPosIdx=0; iPosIdx<iPosCount; iPosIdx++)
215 // ////{
216 // //// CStdFPoint &vPos = *m_aryPos[iPosIdx];
217 // //CStdFPoint vPos(0, 0, 0);
218 //
219 // // float fltDiv = osg::PI/100;
220 // // float fltStart = -osg::PI*2;
221 // // float fltEnd = osg::PI*2;
222 //
223 // // //vRot.Set(osg::PI/4, osg::PI/4, -osg::PI/6);
224 //
225 // // for(float fltXRot = fltStart; fltXRot<fltEnd; fltXRot+=fltDiv)
226 // // for(float fltYRot = fltStart; fltYRot<fltEnd; fltYRot+=fltDiv)
227 // // for(float fltZRot = fltStart; fltZRot<fltEnd; fltZRot+=fltDiv)
228 // // {
229 // // osg::Matrix vNew, vOld;
230 // // //osg::Quat vNew, vOld;
231 // // //vRot.Set(fltXRot, fltYRot, fltZRot);
232 //
233 // // //vOld = OldSetupMatrixQuat(vPos, vRot);
234 // // //vNew = FromEuler(vRot.y, vRot.z, vRot.x);
235 //
236 // // vOld = OldSetupMatrix(vPos, vRot);
237 // // vNew = NewSetupMatrix(vPos, vRot);
238 //
239 // // //OsgMatrixUtil::Print(vOld);
240 // // //OsgMatrixUtil::Print(vNew);
241 //
242 // // int i=5;
243 // // if(!(vOld == vNew))
244 // // i=6;
245 // // //if(!QuatEqual(vOld, vNew))
246 // // // i=6;
247 // // //BOOST_ASSERT();
248 // // }
249 // ////}
250 //}
251 //
252 //
253 //BOOST_AUTO_TEST_CASE( CompareOldNewEulerRotationFromMatrix )
254 //{
255 // CStdFPoint vRot(0, 0, 0);
256 // CStdFPoint vPos(0, 0, 0);
257 //
258 // //OsgMatrixUtil osgUtil;
259 // //BlMatrixUtil vsUtil;
260 //
261 // ////SetMatrixUtil(&osgUtil);
262 // //SetMatrixUtil(&vsUtil);
263 //
264 // float fltDiv = osg::PI/20;
265 // float fltStart = -osg::PI*2;
266 // float fltEnd = osg::PI*2;
267 //
268 // //for(float fltXRot = fltStart; fltXRot<fltEnd; fltXRot+=fltDiv)
269 // // for(float fltYRot = -((osg::PI/2)-0.001); fltYRot<((osg::PI/2)-0.001); fltYRot+=fltDiv)
270 // // for(float fltZRot = fltStart; fltZRot<fltEnd; fltZRot+=fltDiv)
271 // // {
272 // // osg::Matrix osgMT;
273 // // vRot.Set(fltXRot, fltYRot, fltZRot);
274 //
275 // // osgMT = SetupMatrix(vPos, vRot);
276 //
277 //
278 // // CStdFPoint vOld = EulerRotationFromMatrix(osgMT);
279 // // CStdFPoint vNew = ExtractEulerXYZ(osgMT);
280 // //
281 // // int i=5;
282 // // if(!vOld.Equal(vNew, 1e-4))
283 // // i=6;
284 //
285 // //BOOST_ASSERT(vOld == vNew);
286 // // }
287 //}
288 //
289 //BOOST_AUTO_TEST_CASE( CompareOldNewEulerRotationFromMatrix_Specific )
290 //{
291 // osg::Matrix osgMT(-0.041392912621901916, 2.2185430073521640e-016, -0.99914294612166255, 0.00000000000000000,
292 // -2.2185430073521640e-016, 0.99999999999999989, 2.3123567785485781e-016, 0.00000000000000000,
293 // 0.99914294612166255, 2.3123567785485781e-016, -0.041392912621901916, 0.00000000000000000,
294 // 0.00000000000000000, 1.5000000000000000, 0, 1);
295 //
296 // osg::Matrix3 osgMT3(-0.041392912621901916, 2.2185430073521640e-016, -0.99914294612166255,
297 // -2.2185430073521640e-016, 0.99999999999999989, 2.3123567785485781e-016,
298 // 0.99914294612166255, 2.3123567785485781e-016, -0.041392912621901916);
299 //
300 // osg::Matrix3 osgMT3T(-0.041392912621901916, -2.2185430073521640e-016, 0.99914294612166255,
301 // 2.2185430073521640e-016, 0.99999999999999989, 2.3123567785485781e-016,
302 // -0.99914294612166255, 2.3123567785485781e-016, -0.041392912621901916);
303 //
304 //
305 // OsgMatrixUtil osgUtil;
306 // //BlMatrixUtil vsUtil;
307 //
308 // SetMatrixUtil(&osgUtil);
309 // CStdFPoint vOsgRot = EulerRotationFromMatrix(osgMT);
310 //
311 // //SetMatrixUtil(&vsUtil);
312 // CStdFPoint vVsRot = EulerRotationFromMatrix(osgMT);
313 //
314 // CStdFPoint vtRot = ExtractEulerXYZ(osgMT);
315 //
316 // int i=5;
317 // if(vOsgRot != vVsRot)
318 // i=6;
319 //
320 // //osgUItil.
321 //}
322 //
337 //
343 //
349 //
355 //
356 //BOOST_AUTO_TEST_CASE( Dynamic_Loading )
357 //{
358 // std::string strExePath = Std_ExecutablePath();
359 // std::string strExecutablePath, strExeFile;
360 // Std_SplitPathAndFile(strExePath, strExecutablePath, strExeFile);
361 //
362 // std::string strProjFile = strExecutablePath + "../Libraries/BulletAnimatSim/Bullet_UnitTests/TestResources/SingleJoint_StandaloneD.asim";
363 //
364 // Simulator *lpSim = Simulator::CreateSimulator("", strProjFile);
365 //
366 // RigidBody *lpBody = dynamic_cast<RigidBody *>(lpSim->CreateObject("", "RigidBody", "Box"));
367 // if(lpBody)
368 // delete lpBody;
369 //
370 // if(lpSim)
371 // delete lpSim;
372 //}
373 //
374 //BOOST_AUTO_TEST_CASE( Falling_Shapes_Sim )
375 //{
376 // std::string strExePath = Std_ExecutablePath();
377 // std::string strExecutablePath, strExeFile;
378 // Std_SplitPathAndFile(strExePath, strExecutablePath, strExeFile);
379 //
380 // std::string strProjFile = strExecutablePath + "../Libraries/BulletAnimatSim/Bullet_UnitTests/TestResources/FallingBodies/FallingBodies_StandaloneD.asim";
381 //
382 // Simulator *lpSim = Simulator::CreateSimulator("", strProjFile);
383 //
384 // lpSim->Load();
385 // lpSim->Initialize(0, 0);
386 // lpSim->Simulate();
387 //
388 // if(lpSim)
389 // delete lpSim;
390 //}
391 //
392 //
393 //BOOST_AUTO_TEST_CASE( VolumeOfBox )
394 //{
395 // //osg::ref_ptr<osg::Geometry> osgBox = OsgAnimatSim::Environment::CreateBoxGeometry(2, 2, 2, 1, 1, 1);
396 // osg::ref_ptr<osg::Geometry> osgBox = OsgAnimatSim::Environment::CreateConeGeometry(3, 2, 2, 15, true, true, true);
397 // //osg::ref_ptr<osg::Geometry> osgBox = OsgAnimatSim::Environment::CreateSphereGeometry(15, 15, 1);
398 //
399 // osg::ref_ptr<osg::Geode> osgGeode = new osg::Geode;
400 //
401 // osgGeode->addDrawable(osgBox.get());
402 //
403 // btConvexHullShape *btHull = OsgMeshToConvexHull(osgGeode.get(), true, 0);
404 //
405 // //osg::ref_ptr<osg::Node> osgDebugNode = osgbCollision::osgNodeFromBtCollisionShape( btHull );
406 //
407 // //osgDB::writeNodeFile(*(osgDebugNode.get()), "C:\\Temp\\Test.osg");
408 //
409 // float fltVolume = OsgConvexHullVolume(osgGeode.get());
410 // fltVolume = fltVolume + 1;
411 //}
412 //
413 //BOOST_AUTO_TEST_CASE( PyramidVolume )
414 //{
415 // osg::Vec3d v1(0,0,0);
416 // osg::Vec3d v2(1,0,0);
417 // osg::Vec3d v3(1,2,0);
418 // osg::Vec3d vCenterPoint(0.5,1.5,1);
419 //
420 // OsgAnimatSim::Visualization::OsgPyramid p(vCenterPoint, v1, v2, v3);
421 //
422 // float fltHeight = p.Height();
423 // float fltArea = p.BaseArea();
424 // float fltVolume = p.Volume();
425 //}
426 
427 
428 BOOST_AUTO_TEST_CASE( SimulationMgr_CreateModifyShutdownWithWindow )
429 {
430  std::string strExePath = Std_ExecutablePath();
431  std::string strExecutablePath, strExeFile;
432  Std_SplitPathAndFile(strExePath, strExecutablePath, strExeFile);
433  std::string strProjFile = "";
434 
435 #ifdef _DEBUG
436  strProjFile = strExecutablePath + "../Tutorials/Examples/StandAloneSimTest/Bullet_Single_x32_debug.asim";
437 #else
438  strProjFile = strExecutablePath + "../Tutorials/Examples/StandAloneSimTest/Bullet_Single_x32.asim";
439 #endif
440 
441  SimulationThread *lpThread = SimulationMgr::Instance().CreateSimulation(strProjFile);
442 
443  AnimatBase *lpNeuron = lpThread->Sim()->FindByID("0825d6e6-ebc4-4d4e-8c87-1fd06f821916");
444 
445  float fltVth = -0.050;
446  for(int iIdx=0; iIdx<2; iIdx++)
447  {
448  lpNeuron->SetData("Vth", fltVth);
449 
450  lpThread->Simulate(2.0);
451 
452  while(lpThread->Sim()->SimRunning())
453  {
454  boost::this_thread::sleep(boost::posix_time::milliseconds(100));
455  }
456 
457  fltVth-=-0.02;
458  }
459 
460  SimulationMgr::Instance().ShutdownAllSimulations();
461 }
462 
463 BOOST_AUTO_TEST_SUITE_END()
StdUtils::Std_SplitPathAndFile
void Std_SplitPathAndFile(std::string &strFullPath, std::string &strPath, std::string &strFile)
Splits the path from the actual filename.
Definition: StdUtilFunctions.cpp:3240
StdUtils::Std_ExecutablePath
std::string Std_ExecutablePath()
Finds the name and path of the current executable.
Definition: StdUtilFunctions.cpp:3312