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
00013
00014
00015
00016
00017
00018
00019
00020
00021 collptp=0;
00022 dyna = new SIG_Dyna((void*)this);
00023
00024
00025
00026
00027
00028
00029
00030 dyna->set_position(&position);
00031 dyna->set_orientation(&orientation);
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
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
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
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
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
00136 };
00137
00138 void SIGEL_Simulation::SIG_DynaLink::checkInertiaTensor(DL_dyna* d)
00139 {
00140
00141 };