00001 #include "SIGEL_Simulation/SIG_TranslationalController.h"
00002 #include "SIGEL_Tools/SIG_IO.h"
00003 #include "SIGEL_Simulation/SIG_Dyna.h"
00004 #include <cmath>
00005
00006 SIGEL_Simulation::SIG_TranslationalController::SIG_TranslationalController()
00007 {
00008 minimum=0;
00009 maximum=0;
00010 joint=0;
00011 };
00012
00013 SIGEL_Simulation::SIG_TranslationalController::~SIG_TranslationalController()
00014 {
00015 };
00016
00017 void SIGEL_Simulation::SIG_TranslationalController::init(SIG_DynaJoint * theJoint, DL_Scalar theMinimum, DL_Scalar theMaximum)
00018 {
00019 minimum=theMinimum;
00020 maximum=theMaximum;
00021 joint=theJoint;
00022 activate();
00023 };
00024
00025 void SIGEL_Simulation::SIG_TranslationalController::calculate_and_apply()
00026 {
00027
00028
00029
00030
00031
00032 DL_point lFix,rFix,nextlFix,nextrFix;
00033 joint->leftDyna->dyna->to_world(&joint->leftFix,&lFix);
00034 joint->rightDyna->dyna->to_world(&joint->rightFix,&rFix);
00035 joint->leftDyna->dyna->new_toworld(&joint->leftFix,&nextlFix);
00036 joint->rightDyna->dyna->new_toworld(&joint->rightFix,&nextrFix);
00037 DL_vector diff,nextdiff;
00038 lFix.minus(&rFix,&diff);
00039 nextlFix.minus(&nextrFix,&nextdiff);
00040 double deflect=diff.norm();
00041 double nextdeflect=nextdiff.norm();
00042
00043 #ifdef SIG_DEBUG
00044 SIGEL_Tools::SIG_IO::cerr << "Abstand " << deflect << " Einheiten.\n";
00045 #endif
00046
00047 DL_point llFix,lrFix;
00048 joint->leftDyna->dyna->to_local(&lFix,0,&llFix);
00049 joint->rightDyna->dyna->to_local(&rFix,0,&lrFix);
00050 DL_vector forcel=(dynamic_cast<SIG_Dyna*>(joint->leftDyna->dyna))->getPointForce(&llFix);
00051 DL_vector forcer=(dynamic_cast<SIG_Dyna*>(joint->rightDyna->dyna))->getPointForce(&lrFix);
00052 DL_vector wforcel,wforcer;
00053 joint->leftDyna->dyna->to_world(&forcel,&wforcel);
00054 joint->rightDyna->dyna->to_world(&forcer,&wforcer);
00055 wforcel.inprod(&diff);
00056 wforcer.inprod(&diff);
00057 DL_Scalar nforcel=wforcel.norm();
00058 DL_Scalar nforcer=wforcer.norm();
00059 DL_Scalar reagent=(nforcel+nforcer);
00060 DL_Scalar border=(maximum-minimum)*0.0;
00061
00062 if (deflect<(minimum+border))
00063 {
00064 DL_Scalar faktor=reagent*((minimum+border)-deflect);
00065 if (nextdeflect>deflect)
00066 faktor=faktor/2;
00067 #ifdef SIG_DEBUG
00068 SIGEL_Tools::SIG_IO::cerr << "#### ZU KLEIN ###################################################\n";
00069 #endif
00070 diff.normalize();
00071 diff.timesis(faktor);
00072 joint->leftDyna->dyna->applyforce(&llFix,joint->leftDyna->dyna,&diff);
00073 diff.timesis(-1);
00074 joint->rightDyna->dyna->applyforce(&lrFix,joint->rightDyna->dyna,&diff);
00075 };
00076 if (deflect>(maximum-border))
00077 {
00078 DL_Scalar faktor=reagent*(deflect-(maximum-border));
00079 if (nextdeflect<deflect)
00080 faktor=faktor/2;
00081 #ifdef SIG_DEBUG
00082 SIGEL_Tools::SIG_IO::cerr << "#### ZU GROSS ###################################################\n";
00083 #endif
00084 diff.normalize();
00085 diff.timesis(faktor);
00086 joint->rightDyna->dyna->applyforce(&lrFix,joint->rightDyna->dyna,&diff);
00087 diff.timesis(-1);
00088 joint->leftDyna->dyna->applyforce(&llFix,joint->leftDyna->dyna,&diff);
00089 };
00090 };