|
mbsimcontrol
4.0.0
|
00001 /* Copyright (C) 2004-2009 MBSim Development Team 00002 * 00003 * This library is free software; you can redistribute it and/or 00004 * modify it under the terms of the GNU Lesser General Public 00005 * License as published by the Free Software Foundation; either 00006 * version 2.1 of the License, or (at your option) any later version. 00007 * 00008 * This library is distributed in the hope that it will be useful, 00009 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00010 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00011 * Lesser General Public License for more details. 00012 * 00013 * You should have received a copy of the GNU Lesser General Public 00014 * License along with this library; if not, write to the Free Software 00015 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00016 * 00017 * Contact: markus.ms.schneider@gmail.com 00018 */ 00019 00020 #ifndef _FRAME_SENSORS_H_ 00021 #define _FRAME_SENSORS_H_ 00022 00023 #include "mbsimControl/sensor.h" 00024 00025 namespace MBSim { 00026 class Frame; 00027 } 00028 00029 namespace MBSimControl { 00030 00035 class AbsolutCoordinateSensor : public Sensor { 00036 public: 00037 AbsolutCoordinateSensor(const std::string &name) : Sensor(name), frame(NULL), direction(), frameString("") {} 00038 std::string getType() const { return "AbsolutCoordinateSensor"; } 00039 void initializeUsingXML(xercesc::DOMElement *element); 00040 void init(InitStage stage); 00041 void setFrame(MBSim::Frame * frame_) {frame=frame_; } 00042 void setDirection(fmatvec::Mat direction_) { 00043 direction=direction_; 00044 for (int i=0; i<direction_.cols(); i++) 00045 direction.col(i)=direction.col(i)/nrm2(direction.col(i)); 00046 assert(direction.rows()==3); 00047 } 00048 protected: 00049 MBSim::Frame * frame; 00050 fmatvec::Mat direction; 00051 std::string frameString; 00052 }; 00053 00058 class AbsolutePositionSensor : public AbsolutCoordinateSensor { 00059 public: 00060 AbsolutePositionSensor(const std::string &name="") : AbsolutCoordinateSensor(name) {} 00061 std::string getType() const { return "AbsolutePositionSensor"; } 00062 fmatvec::Vec getSignal(); 00063 }; 00064 00069 class AbsoluteVelocitySensor : public AbsolutCoordinateSensor { 00070 public: 00071 AbsoluteVelocitySensor(const std::string &name="") : AbsolutCoordinateSensor(name) {} 00072 std::string getType() const { return "AbsoluteVelocitySensor"; } 00073 fmatvec::Vec getSignal(); 00074 }; 00075 00080 class AbsoluteAngularPositionSensor : public AbsolutCoordinateSensor { 00081 public: 00082 AbsoluteAngularPositionSensor(const std::string &name="") : AbsolutCoordinateSensor(name) {} 00083 std::string getType() const { return "AbsoluteAngularPositionSensor"; } 00084 fmatvec::Vec getSignal(); 00085 00086 void calcxSize() {xSize=direction.cols(); } 00087 void init(InitStage stage) { 00088 if (stage==resize) { 00089 AbsolutCoordinateSensor::init(stage); 00090 g.resize(direction.cols()); 00091 gd.resize(direction.cols()); 00092 x.resize(direction.cols()); 00093 } 00094 else 00095 AbsolutCoordinateSensor::init(stage); 00096 } 00097 void updateg(double t) {g=x; } 00098 void updategd(double t); 00099 void updatexd(double t) {xd=gd; } 00100 void updatedx(double t, double dt) {xd=gd*dt; } 00101 }; 00102 00107 class AbsoluteAngularVelocitySensor : public AbsolutCoordinateSensor { 00108 public: 00109 AbsoluteAngularVelocitySensor(const std::string &name="") : AbsolutCoordinateSensor(name) {} 00110 std::string getType() const { return "AbsoluteAngularVelocitySensor"; } 00111 fmatvec::Vec getSignal(); 00112 }; 00113 00114 00119 class RelativeCoordinateSensor : public Sensor { 00120 public: 00121 RelativeCoordinateSensor(const std::string &name) : Sensor(name), refFrame(NULL), relFrame(NULL), direction(), refFrameString(""), relFrameString("") {} 00122 std::string getType() const { return "RelativeCoordinateSensor"; } 00123 void initializeUsingXML(xercesc::DOMElement *element); 00124 void init(InitStage stage); 00125 void setReferenceFrame(MBSim::Frame * refFrame_) {refFrame=refFrame_; } 00126 void setRelativeFrame(MBSim::Frame * relFrame_) {relFrame=relFrame_; } 00127 void setDirection(fmatvec::Mat direction_) { 00128 direction=direction_; 00129 for (int i=0; i<direction_.cols(); i++) 00130 direction.col(i)=direction.col(i)/nrm2(direction.col(i)); 00131 assert(direction.rows()==3); 00132 } 00133 protected: 00134 MBSim::Frame * refFrame; 00135 MBSim::Frame * relFrame; 00136 fmatvec::Mat direction; 00137 std::string refFrameString, relFrameString; 00138 }; 00139 00144 class RelativePositionSensor : public RelativeCoordinateSensor { 00145 public: 00146 RelativePositionSensor(const std::string &name="") : RelativeCoordinateSensor(name) {} 00147 std::string getType() const { return "RelativePositionSensor"; } 00148 fmatvec::Vec getSignal(); 00149 }; 00150 00155 class RelativeVelocitySensor : public RelativeCoordinateSensor { 00156 public: 00157 RelativeVelocitySensor(const std::string &name="") : RelativeCoordinateSensor(name) {} 00158 std::string getType() const { return "RelativeVelocitySensor"; } 00159 fmatvec::Vec getSignal(); 00160 }; 00161 00166 class RelativeAngularPositionSensor : public RelativeCoordinateSensor { 00167 public: 00168 RelativeAngularPositionSensor(const std::string &name="") : RelativeCoordinateSensor(name) {} 00169 std::string getType() const { return "RelativeAngularPositionSensor"; } 00170 fmatvec::Vec getSignal(); 00171 00172 void calcxSize() {xSize=direction.cols(); } 00173 void init(InitStage stage) { 00174 if (stage==resize) { 00175 RelativeCoordinateSensor::init(stage); 00176 g.resize(direction.cols()); 00177 gd.resize(direction.cols()); 00178 x.resize(direction.cols()); 00179 } 00180 else 00181 RelativeCoordinateSensor::init(stage); 00182 } 00183 void updateg(double t) {g=x; } 00184 void updategd(double t); 00185 void updatexd(double t) {xd=gd; } 00186 void updatedx(double t, double dt) {xd=gd*dt; } 00187 }; 00188 00193 class RelativeAngularVelocitySensor : public RelativeCoordinateSensor { 00194 public: 00195 RelativeAngularVelocitySensor(const std::string &name="") : RelativeCoordinateSensor(name) {} 00196 std::string getType() const { return "RelativeAngularVelocitySensor"; } 00197 fmatvec::Vec getSignal(); 00198 }; 00199 00200 } 00201 00202 #endif /* _FRAME_SENSORS_H_ */
1.8.0