45 #include "btBulletDynamicsCommon.h"
62 throw std::invalid_argument(
"Called the constructor for tgCompoundRigidSensor with only a pointer, NOT a tag. That's not allowed, a specific compound tag must be passed in also.");
76 throw std::invalid_argument(
"Pointer to pModel is NULL inside tgCompoundRigidSensor.");
82 if( m_rigids.empty() ) {
83 throw std::runtime_error(
"tgCompoundRigidSensor found no rigid bodies with its tag - something is wrong (inside constructor.)");
93 btRigidBody* firstRigid = m_rigids[0]->getPRigidBody();
95 origOrientQuat = firstRigid->getOrientation();
98 origOrientQuatInv = origOrientQuat.inverse();
120 btVector3 tgCompoundRigidSensor::getCenterOfMass()
126 btVector3* pCom =
new btVector3(0.0, 0.0, 0.0);
128 for(
size_t i=0; i < m_rigids.size(); i++){
129 *pCom += m_rigids[i]->centerOfMass();
132 *pCom /= m_rigids.size();
137 btVector3 tgCompoundRigidSensor::getOrientation()
145 btQuaternion currentOrientQuat = m_rigids[0]->getPRigidBody()->getOrientation();
147 btQuaternion diffOrientQuat = currentOrientQuat * origOrientQuatInv;
149 btMatrix3x3 rotMat = btMatrix3x3( diffOrientQuat );
151 btScalar pitch = 0.0;
153 rotMat.getEulerYPR(yaw, pitch, roll);
156 yaw = 180/M_PI * yaw;
157 pitch = 180/M_PI * pitch;
158 roll = 180/M_PI * roll;
161 btVector3 orient = btVector3(yaw, pitch, roll);
170 double tgCompoundRigidSensor::getMass()
174 for(
size_t i=0; i < m_rigids.size(); i++){
175 mass += m_rigids[i]->mass();
194 assert( m_pModel != 0);
197 std::vector<std::string> headings;
210 std::string prefix =
"compound(";
213 headings.push_back( prefix + m_tag +
").X" );
214 headings.push_back( prefix + m_tag +
").Y" );
215 headings.push_back( prefix + m_tag +
").Z" );
216 headings.push_back( prefix + m_tag +
").YawDegrees" );
217 headings.push_back( prefix + m_tag +
").PitchDegrees" );
218 headings.push_back( prefix + m_tag +
").RollDegrees" );
219 headings.push_back( prefix + m_tag +
").mass" );
234 std::stringstream ss;
237 std::vector<std::string> sensordata;
241 btVector3 com = getCenterOfMass();
242 btVector3 orient = getOrientation();
246 sensordata.push_back( ss.str() );
252 sensordata.push_back( ss.str() );
256 sensordata.push_back( ss.str() );
261 sensordata.push_back( ss.str() );
266 sensordata.push_back( ss.str() );
271 sensordata.push_back( ss.str() );
280 sensordata.push_back( ss.str() );
Create a box shape as an obstacle or add it to your tensegrity.
Constains definition of concrete class tgCompoundRigidSensor.
Utility class for class casting and filtering collections by type.
tgCompoundRigidSensor(tgModel *pModel)
virtual std::vector< std::string > getSensorData()
virtual ~tgCompoundRigidSensor()
std::vector< T * > find(const tgTagSearch &tagSearch)
virtual std::vector< std::string > getSensorDataHeadings()
Constains the implementation of mixin class tgSenseable.