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

SIG_TranslationalController.cpp

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   /* Hier muss nun der Abstand zwischen den beiden Fix-Vektoren in Weltkoordinaten gemessen werden,
00028    * danach sollte ueberprueft werden, ob eine der Grenzen ueberschritten ist.
00029    * Zum Schluss sollte auf die beiden Dynas eine Kraft einwirken 
00030    * Diese Kraft errechnet sich irgendwie aus der in den beiden Punkten wirkenden Geschwindigkeit
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 };

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