17 #include "libopenrave.h"
38 if( _info._bIsEnabled != bEnable ) {
41 _info._bIsEnabled = bEnable;
48 return _info._bIsEnabled;
53 bool bchanged =
false;
54 FOREACH(itgeom,_vGeometries) {
55 if( (*itgeom)->_info._bVisible != visible ) {
56 (*itgeom)->_info._bVisible = visible;
69 FOREACHC(itgeom,_vGeometries) {
70 if( (*itgeom)->IsVisible() ) {
80 vParentLinks.resize(_vParentLinks.size());
81 for(
size_t i = 0; i < _vParentLinks.size(); ++i) {
82 vParentLinks[i] = parent->GetLinks().at(_vParentLinks[i]);
88 return find(_vParentLinks.begin(),_vParentLinks.end(),plink->GetIndex()) != _vParentLinks.end();
103 dReal i0 = vinertiamoments[0], i1 = vinertiamoments[1], i2 = vinertiamoments[2];
104 dReal q0=tMassFrame.rot[0], q1=tMassFrame.rot[1], q2=tMassFrame.rot[2], q3=tMassFrame.rot[3];
105 dReal q1_2 = q1*q1, q2_2 = q2*q2, q3_2 = q3*q3;
107 minertia.m[1] = i0*(2*q0*q3 + 2*q1*q2)*(1 - 2*q2_2 - 2*q3_2) + i1*(-2*q0*q3 + 2*q1*q2)*(1 - 2*q1_2 - 2*q3_2) + i2*(-2*q0*q1 + 2*q2*q3)*(2*q0*q2 + 2*q1*q3);
108 minertia.m[2] = i0*(-2*q0*q2 + 2*q1*q3)*(1 - 2*q2_2 - 2*q3_2) + i1*(-2*q0*q3 + 2*q1*q2)*(2*q0*q1 + 2*q2*q3) + i2*(2*q0*q2 + 2*q1*q3)*(1 - 2*q1_2 - 2*q2_2);
110 minertia.m[4] = minertia.m[1];
112 minertia.m[6] = i0*(-2*q0*q2 + 2*q1*q3)*(2*q0*q3 + 2*q1*q2) + i1*(2*q0*q1 + 2*q2*q3)*(1 - 2*q1_2 - 2*q3_2) + i2*(-2*q0*q1 + 2*q2*q3)*(1 - 2*q1_2 - 2*q2_2);
114 minertia.m[8] = minertia.m[2];
115 minertia.m[9] = minertia.m[6];
127 return ComputeInertia(_info._t*_info._tMassFrame, _info._vinertiamoments);
132 _info._tMassFrame=massframe;
138 _info._vinertiamoments = inertiamoments;
150 if( _vGeometries.size() == 1) {
151 return _vGeometries.front()->ComputeAABB(
Transform());
153 else if( _vGeometries.size() > 1 ) {
155 bool binitialized=
false;
157 FOREACHC(itgeom,_vGeometries) {
158 ab = (*itgeom)->ComputeAABB(
Transform());
164 if( !binitialized ) {
170 if( vmin.x > vnmin.x ) {
173 if( vmin.y > vnmin.y ) {
176 if( vmin.z > vnmin.z ) {
179 if( vmax.x < vnmax.x ) {
182 if( vmax.y < vnmax.y ) {
185 if( vmax.z < vnmax.z ) {
190 if( !binitialized ) {
191 ab.
pos = _info._t.trans;
195 ab.
pos = (
dReal)0.5 * (vmin + vmax);
205 if( _vGeometries.size() == 1) {
206 return _vGeometries.front()->ComputeAABB(_info._t);
208 else if( _vGeometries.size() > 1 ) {
210 bool binitialized=
false;
212 FOREACHC(itgeom,_vGeometries) {
213 ab = (*itgeom)->ComputeAABB(_info._t);
219 if( !binitialized ) {
225 if( vmin.x > vnmin.x ) {
228 if( vmin.y > vnmin.y ) {
231 if( vmin.z > vnmin.z ) {
234 if( vmax.x < vnmax.x ) {
237 if( vmax.y < vnmax.y ) {
240 if( vmax.z < vnmax.z ) {
245 if( !binitialized ) {
246 ab.
pos = _info._t.trans;
250 ab.
pos = (
dReal)0.5 * (vmin + vmax);
263 o << _vGeometries.size() <<
" ";
264 FOREACHC(it,_vGeometries) {
265 (*it)->serialize(o,options);
269 SerializeRound(o,_info._tMassFrame);
270 SerializeRound(o,_info._mass);
271 SerializeRound3(o,_info._vinertiamoments);
277 if( _info._bStatic != bStatic ) {
278 _info._bStatic = bStatic;
286 GetParent()->_nUpdateStampId++;
291 GetParent()->GetEnv()->GetPhysicsEngine()->SetBodyForce(shared_from_this(), force, pos, bAdd);
296 GetParent()->GetEnv()->GetPhysicsEngine()->SetBodyTorque(shared_from_this(), torque, bAdd);
301 GetParent()->GetEnv()->GetPhysicsEngine()->SetLinkVelocity(shared_from_this(), linearvel, angularvel);
306 GetParent()->GetEnv()->GetPhysicsEngine()->GetLinkVelocity(shared_from_this(), linearvel, angularvel);
312 std::pair<Vector,Vector> velocities;
313 GetParent()->GetEnv()->GetPhysicsEngine()->GetLinkVelocity(shared_from_this(), velocities.first, velocities.second);
319 return _vGeometries.at(index);
324 _vGeometries.resize(geometries.size());
325 for(
size_t i = 0; i < geometries.size(); ++i) {
326 _vGeometries[i].reset(
new Geometry(shared_from_this(),*geometries[i]));
334 _vGeometries.resize(geometries.size());
336 FOREACH(itinfo,geometries) {
337 _vGeometries[i].reset(
new Geometry(shared_from_this(),*itinfo));
346 _vGeometries.swap(link->_vGeometries);
347 FOREACH(itgeom,_vGeometries) {
348 (*itgeom)->_parent = shared_from_this();
350 FOREACH(itgeom,link->_vGeometries) {
351 (*itgeom)->_parent = link;
361 if( _vGeometries.size() == 1) {
362 return _vGeometries.front()->ValidateContactNormal(position,normal);
364 else if( _vGeometries.size() > 1 ) {
365 RAVELOG_VERBOSE(str(boost::format(
"cannot validate normal when there is more than one geometry in link '%s(%d)' (do not know colliding geometry)")%_info._name%GetIndex()));
373 vattachedlinks.resize(0);
374 FOREACHC(it, _vRigidlyAttachedLinks) {
375 vattachedlinks.push_back(parent->GetLinks().at(*it));
381 if( parameters.size() > 0 ) {
382 _info._mapFloatParameters[key] = parameters;
385 _info._mapFloatParameters.erase(key);
392 if( parameters.size() > 0 ) {
393 _info._mapIntParameters[key] = parameters;
396 _info._mapIntParameters.erase(key);
403 return find(_vRigidlyAttachedLinks.begin(),_vRigidlyAttachedLinks.end(),plink->GetIndex()) != _vRigidlyAttachedLinks.end();
408 if( _info._vgeometryinfos.size() != _vGeometries.size() ) {
410 _info._vgeometryinfos.resize(_vGeometries.size());
411 for(
size_t i = 0; i < _info._vgeometryinfos.size(); ++i) {
412 if( !_info._vgeometryinfos[i] ) {
415 *_info._vgeometryinfos[i] = _vGeometries[i]->GetInfo();
422 _collision.vertices.resize(0);
423 _collision.indices.resize(0);
424 FOREACH(itgeom,_vGeometries) {
425 _collision.Append((*itgeom)->GetCollisionMesh(),(*itgeom)->GetTransform());