Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_DynaLink.cpp

00001 #include "SIGEL_Simulation/SIG_DynaLink.h"
00002 #include "SIGEL_Simulation/SIG_Dyna.h"
00003 #include "SIGEL_Simulation/SIG_DynaSystem.h"
00004 #include "SIGEL_Tools/SIG_IO.h"
00005 #include <qarray.h>
00006 #include "NaN.h"
00007 
00008 SIGEL_Simulation::SIG_DynaLink::SIG_DynaLink(DL_point thePosition, DL_matrix theOrientation)
00009 {
00010   position=thePosition;
00011   orientation.assign(&theOrientation);
00012 /*  SIGEL_Tools::SIG_IO::cerr << "Orient " << orientation.c0.x
00013                                << " " << orientation.c1.x
00014                                << " " << orientation.c2.x << "\n";
00015   SIGEL_Tools::SIG_IO::cerr << " " << orientation.c0.y
00016                                << " " << orientation.c1.y
00017                                << " " << orientation.c2.y << "\n";
00018   SIGEL_Tools::SIG_IO::cerr << " " << orientation.c0.z
00019                                << " " << orientation.c1.z
00020                                << " " << orientation.c2.z << "\n";*/
00021   collptp=0;
00022   dyna = new SIG_Dyna((void*)this);
00023 /*  DL_vector v(0,0,0);
00024   dyna->set_next_orientation(&orientation);
00025   dyna->set_next_position(&position);
00026   dyna->set_next_velocity(&v);
00027   dyna->move(&position,&orientation);
00028   dyna->set_next_angvelocity(&v);
00029 */
00030   dyna->set_position(&position);
00031   dyna->set_orientation(&orientation);
00032 /*  SIGEL_Tools::SIG_IO::cerr << "Orient " << dyna->get_orientation()->c0.x
00033                                << " " << dyna->get_orientation()->c1.x
00034                                << " " << dyna->get_orientation()->c2.x << "\n";
00035   SIGEL_Tools::SIG_IO::cerr << " " << dyna->get_orientation()->c0.y
00036                                << " " << dyna->get_orientation()->c1.y
00037                                << " " << dyna->get_orientation()->c2.y << "\n";
00038   SIGEL_Tools::SIG_IO::cerr << " " << dyna->get_orientation()->c0.z
00039                                << " " << dyna->get_orientation()->c1.z
00040                                << " " << dyna->get_orientation()->c2.z << "\n";*/
00041 /*dyna->set_velocity(&v);
00042   dyna->set_angvelocity(&v);*/
00043 #ifdef SIG_DEBUG  
00044   SIGEL_Tools::SIG_IO::cerr << " ************ DynaLink **********************************\n";
00045   SIGEL_Tools::SIG_IO::cerr << "Dyna: " << dyna << "\n";
00046   SIGEL_Tools::SIG_IO::cerr << "Masse: " << dyna->get_mass() << "\n";
00047   SIGEL_Tools::SIG_IO::cerr << "Inertia X:" << static_cast<SIG_Dyna*>(dyna)->get_inertiatensor()->x
00048                                << " Y:" << static_cast<SIG_Dyna*>(dyna)->get_inertiatensor()->y
00049                                << " Z:" << static_cast<SIG_Dyna*>(dyna)->get_inertiatensor()->z << "\n";
00050   SIGEL_Tools::SIG_IO::cerr << "Vel X:" << dyna->get_velocity()->x
00051                                << " Y:" << dyna->get_velocity()->y
00052                                << " Z:" << dyna->get_velocity()->z << "\n";
00053   SIGEL_Tools::SIG_IO::cerr << "AVel X:" << dyna->get_angvelocity()->x
00054                                 << " Y:" << dyna->get_angvelocity()->y
00055                                 << " Z:" << dyna->get_angvelocity()->z << "\n";
00056   SIGEL_Tools::SIG_IO::cerr << "Pos X:" << dyna->get_position()->x
00057                                << " Y:" << dyna->get_position()->y
00058                                << " Z:" << dyna->get_position()->z << "\n";
00059   SIGEL_Tools::SIG_IO::cerr << "Orient " << dyna->get_orientation()->c0.x
00060                                << " " << dyna->get_orientation()->c1.x
00061                                << " " << dyna->get_orientation()->c2.x << "\n";
00062   SIGEL_Tools::SIG_IO::cerr << " " << dyna->get_orientation()->c0.y
00063                                << " " << dyna->get_orientation()->c1.y
00064                                << " " << dyna->get_orientation()->c2.y << "\n";
00065   SIGEL_Tools::SIG_IO::cerr << " " << dyna->get_orientation()->c0.z
00066                                << " " << dyna->get_orientation()->c1.z
00067                                << " " << dyna->get_orientation()->c2.z << "\n";
00068   SIGEL_Tools::SIG_IO::cerr << "Next Vel X:" << dyna->get_next_velocity()->x
00069                                << " Y:" << dyna->get_next_velocity()->y
00070                                << " Z:" << dyna->get_next_velocity()->z << "\n";
00071   SIGEL_Tools::SIG_IO::cerr << "Next AVel X:" << dyna->get_next_angvelocity()->x
00072                                 << " Y:" << dyna->get_next_angvelocity()->y
00073                                 << " Z:" << dyna->get_next_angvelocity()->z << "\n";
00074   SIGEL_Tools::SIG_IO::cerr << "Next Pos X:" << dyna->get_next_position()->x
00075                                << " Y:" << dyna->get_next_position()->y
00076                                << " Z:" << dyna->get_next_position()->z << "\n";
00077 #endif
00078 };
00079 
00080 SIGEL_Simulation::SIG_DynaLink::~SIG_DynaLink()
00081 {
00082   delete dyna;
00083   if (collptp!=0)
00084    delete collptp;
00085 };
00086 
00087 void SIGEL_Simulation::SIG_DynaLink::getNewGeoInfo(DL_geo* g)
00088 {
00089 /*  DL_vector v;
00090   v=(g->get_velocity());
00091   if (isnan(v.x)) v.x=0;
00092   if (isnan(v.y)) v.y=0;
00093   if (isnan(v.z)) v.z=0;
00094   g->set_angvelocity(&v);
00095   v=(g->get_angvelocity());
00096   if (isnan(v.x)) v.x=0;
00097   if (isnan(v.y)) v.y=0;
00098   if (isnan(v.z)) v.z=0;
00099   g->set_angvelocity(&v);
00100   g->move(&position,&orientation);*/
00101   g->set_position(&position);
00102   g->set_orientation(&orientation);
00103 };
00104 
00105 void SIGEL_Simulation::SIG_DynaLink::updateDynaCompanion(DL_dyna* d)
00106 {
00107   position.assign(d->get_position());
00108   orientation.assign(d->get_orientation());
00109   
00110   // SOLID updaten (fuer naechsten Frame)
00111   DL_point nextPosition;
00112   nextPosition.assign(d->get_next_position());
00113   DL_matrix nextOrientation;
00114   nextOrientation.assign(d->get_next_orientation());
00115   SIG_DynaSystem::unNaN(nextOrientation);
00116   DL_vector pos2;
00117   nextPosition.tovector(&pos2);
00118   dtSelectObject(this);
00119   dtLoadIdentity();
00120   QArray<double> rotmat(16);
00121   for (int j=0; j<3; j++)
00122     rotmat[ (j * 4) + 3 ] = 0;
00123     rotmat[ 15 ] = 1; 
00124   for (int i=0; i<3; i++)
00125     for (int j=0; j<3; j++)
00126       rotmat[ i + (4 * j) ] = nextOrientation.get(i,j);  
00127   for (int i=0; i<3; i++)
00128     rotmat[ 12 + i ] = pos2.get(i);
00129   dtMultMatrixd(rotmat.data());
00130 };
00131 
00132 void SIGEL_Simulation::SIG_DynaLink::getFirstGeoInfo(DL_geo* g)
00133 {
00134   getNewGeoInfo(g);
00135   // hier koennten spaeter initiale Geschwindigkeiten eingestellt werden.
00136 };
00137 
00138 void SIGEL_Simulation::SIG_DynaLink::checkInertiaTensor(DL_dyna* d)
00139 {
00140   // Wird noch nicht implementiert, wird nur fuer Verformungen benoetigt
00141 };

Generated at Mon Sep 3 01:32:19 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000