00001 #ifndef SIGEL_SIMULATION_SIG_DYNASYSTEM_H 00002 #define SIGEL_SIMULATION_SIG_DYNASYSTEM_H 00003 00004 namespace SIGEL_Simulation { class SIG_DynaCallbacks; }; 00005 00006 #include "rungekutta4.h" 00007 #include "constraint.h" 00008 #include "constraint_manager.h" 00009 #include "SOLID/solid.h" 00010 #include <qvector.h> 00011 #include <qstring.h> 00012 #include <qobject.h> 00013 #include "SIGEL_Simulation/SIG_DynaCallbacks.h" 00014 #include "SIGEL_Simulation/SIG_DynaLink.h" 00015 #include "SIGEL_Simulation/SIG_DynaJoint.h" 00016 #include "SIGEL_Simulation/SIG_DynaSensor.h" 00017 #include "SIGEL_Simulation/SIG_DynaDrive.h" 00018 #include "SIGEL_Simulation/SIG_DynaSystemWrongNumberException.h" 00019 #include "SIGEL_Simulation/SIG_SimulationParameters.h" 00020 #include "SIGEL_Environment/SIG_Environment.h" 00021 #include "SIGEL_Robot/SIG_Joint.h" 00022 #include "SIGEL_Robot/SIG_Link.h" 00023 #include "SIGEL_Robot/SIG_Material.h" 00024 00025 namespace SIGEL_Simulation 00026 { 00033 class SIG_DynaSystem : public QObject 00034 { 00035 Q_OBJECT 00036 00037 public: 00038 00039 bool newFrame; 00040 static bool foundNaN; 00041 00042 void doNewFrame(); 00043 void checkCollisionPTPs(); 00044 00045 DL_matrix rotationMatrix(DL_vector axis, DL_Scalar angle); 00046 static DL_Scalar unNaN(DL_Scalar val); 00047 static DL_point unNaN(DL_point val); 00048 static DL_vector unNaN(DL_vector val); 00049 static DL_matrix unNaN(DL_matrix val); 00050 00051 00053 void msgFunction(QString theMessage); 00054 00056 void doCollisionDetection(); 00057 00058 void clearAllDynamics(); 00059 void initializeFloor(); 00060 00062 DL_dyna_system* dynaSystem; 00063 00065 DL_m_integrator* mIntegrator; 00066 00068 DL_constraint_manager* constraintManager; 00069 00071 SIG_DynaCallbacks* dynaCallbacks; 00072 00074 SIGEL_Robot::SIG_Material * floorMaterial; 00075 00081 SIG_DynaSystem(SIGEL_Environment::SIG_Environment const & theEnvironment, 00082 SIG_SimulationParameters const & theSimulationParameters); 00083 00089 ~SIG_DynaSystem(); 00090 00096 void newLink(SIGEL_Robot::SIG_Link& theLink); 00097 00103 void newJoint(SIGEL_Robot::SIG_Joint& theJoint); 00104 00110 void newDrive(SIGEL_Robot::SIG_Drive& theDrive); 00111 00117 void newSensor(SIGEL_Robot::SIG_Sensor& theSensor); 00118 00126 SIG_DynaLink& getLink(int number) 00127 throw(SIG_DynaSystemWrongNumberException); 00128 00136 SIG_DynaJoint& getJoint(int number) 00137 throw(SIG_DynaSystemWrongNumberException); 00138 00146 SIG_DynaDrive& getDrive(int number) 00147 throw(SIG_DynaSystemWrongNumberException); 00148 00156 SIG_DynaSensor& getSensor(int number) 00157 throw(SIG_DynaSystemWrongNumberException); 00158 00160 int rootLinkNo; 00161 00163 SIGEL_Environment::SIG_Environment const & environment; 00164 00166 SIG_SimulationParameters const & simulationParameters; 00167 00169 DtShapeRef floor; 00170 00172 QVector<SIG_DynaLink> dynaLinks; 00173 00175 QVector<SIG_DynaJoint> dynaJoints; 00176 00178 QVector<SIG_DynaSensor> dynaSensors; 00179 00181 QVector<SIG_DynaDrive> dynaDrives; 00182 00187 static void collisionResponse(void * client_data, 00188 DtObjectRef obj1, 00189 DtObjectRef obj2, 00190 const DtCollData *coll_data); 00191 00192 signals: 00197 void signalDynamoMessage(QString theMessage); 00198 00199 }; 00200 00201 } 00202 00203 #endif // SIGEL_SIMULATION_SIG_DYNASYSTEM_H 00204