SReal UniformMass<VecTypes, MassType>::getPotentialEnergyRigidImpl(const core::MechanicalParams* mparams,
const DataVecCoord& p_x) const
{
SOFA_UNUSED(mparams) ;
SReal e = 0;
ReadAccessor< DataVecCoord > x = p_x;
ReadAccessor<Data<vector<int> > > indices = d_indices;
typename Coord::Pos g ( getContext()->getGravity() );
for (unsigned int i=0; i<indices.size(); i++)
e -= g*d_mass.getValue().mass*x[indices[i]].getCenter();
return e;
}
void UniformMass<RigidTypes, MassType>::drawRigid3DImpl(const VisualParams* vparams)
{
if (!vparams->displayFlags().getShowBehaviorModels())
return;
const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
ReadAccessor<Data<vector<int> > > indices = d_indices;
typename RigidTypes::Vec3 gravityCenter;
defaulttype::Vec3d len;
// The moment of inertia of a box is:
// m->_I(0,0) = M/REAL(12.0) * (ly*ly + lz*lz);
// m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz);
// m->_I(2,2) = M/REAL(12.0) * (lx*lx + ly*ly);
// So to get lx,ly,lz back we need to do
// lx = sqrt(12/M * (m->_I(1,1)+m->_I(2,2)-m->_I(0,0)))
// Note that RigidMass inertiaMatrix is already divided by M
double m00 = d_mass.getValue().inertiaMatrix[0][0];
double m11 = d_mass.getValue().inertiaMatrix[1][1];
double m22 = d_mass.getValue().inertiaMatrix[2][2];
len[0] = sqrt(m11+m22-m00);
len[1] = sqrt(m00+m22-m11);
len[2] = sqrt(m00+m11-m22);
for (unsigned int i=0; i<indices.size(); i++)
{
if (getContext()->isSleeping())
vparams->drawTool()->drawFrame(x[indices[i]].getCenter(), x[indices[i]].getOrientation(), len*d_showAxisSize.getValue(), Vec4f(0.5,0.5,0.5,1) );
else
vparams->drawTool()->drawFrame(x[indices[i]].getCenter(), x[indices[i]].getOrientation(), len*d_showAxisSize.getValue() );
gravityCenter += (x[indices[i]].getCenter());
}
if (d_showInitialCenterOfGravity.getValue())
{
const VecCoord& x0 = mstate->read(core::ConstVecCoordId::restPosition())->getValue();
for (unsigned int i=0; i<indices.size(); i++)
vparams->drawTool()->drawFrame(x0[indices[i]].getCenter(), x0[indices[i]].getOrientation(), len*d_showAxisSize.getValue());
}
if(d_showCenterOfGravity.getValue())
{
gravityCenter /= x.size();
const sofa::defaulttype::Vec4f color(1.0,1.0,0.0,1.0);
vparams->drawTool()->drawCross(gravityCenter, d_showAxisSize.getValue(), color);
}
}
void UniformMass<Vec6Types, MassType>::drawVec6Impl(const core::visual::VisualParams* vparams)
{
if (!vparams->displayFlags().getShowBehaviorModels())
return;
const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
const VecCoord& x0 = mstate->read(core::ConstVecCoordId::restPosition())->getValue();
ReadAccessor<Data<vector<int> > > indices = d_indices;
Mat3x3d R; R.identity();
std::vector<Vector3> vertices;
std::vector<sofa::defaulttype::Vec4f> colors;
const sofa::defaulttype::Vec4f red(1.0,0.0,0.0,1.0);
const sofa::defaulttype::Vec4f green(0.0,1.0,0.0,1.0);
const sofa::defaulttype::Vec4f blue(0.0,0.0,1.0,1.0);
sofa::defaulttype::Vec4f colorSet[3];
colorSet[0] = red;
colorSet[1] = green;
colorSet[2] = blue;
for (unsigned int i=0; i<indices.size(); i++)
{
defaulttype::Vec3d len(1,1,1);
int a = (i<indices.size()-1)?i : i-1;
int b = a+1;
defaulttype::Vec3d dp; dp = x0[b]-x0[a];
defaulttype::Vec3d p; p = x[indices[i]];
len[0] = dp.norm();
len[1] = len[0];
len[2] = len[0];
R = R * MatrixFromEulerXYZ(x[indices[i]][3], x[indices[i]][4], x[indices[i]][5]);
for(unsigned int j=0 ; j<3 ; j++)
{
vertices.push_back(p);
vertices.push_back(p + R.col(j)*len[j]);
colors.push_back(colorSet[j]);
colors.push_back(colorSet[j]);;
}
}
vparams->drawTool()->drawLines(vertices, 1, colors);
}
void UniformMass<RigidTypes, MassType>::drawRigid2DImpl(const VisualParams* vparams)
{
if (!vparams->displayFlags().getShowBehaviorModels())
return;
const VecCoord& x =mstate->read(core::ConstVecCoordId::position())->getValue();
ReadAccessor<Data<vector<int> > > indices = d_indices;
defaulttype::Vec3d len;
len[0] = len[1] = sqrt(d_mass.getValue().inertiaMatrix);
len[2] = 0;
for (unsigned int i=0; i<indices.size(); i++)
{
Quat orient(Vec3d(0,0,1), x[indices[i]].getOrientation());
Vec3d center; center = x[indices[i]].getCenter();
vparams->drawTool()->drawFrame(center, orient, len*d_showAxisSize.getValue() );
}
}
void mitk::VtkModel::DrawGroups(bool transparent)
{
using sofa::core::objectmodel::Data;
using sofa::helper::ReadAccessor;
using sofa::helper::vector;
ReadAccessor<Data<vector<FaceGroup> > > groups = this->groups;
if (groups.empty())
{
this->DrawGroup(-1, transparent);
}
else
{
int numGroups = static_cast<int>(groups.size());
for (int i = 0; i < numGroups; ++i)
this->DrawGroup(i, transparent);
}
}
void UniformMass<VecTypes, MassType>::addMDxToVectorVecImpl(defaulttype::BaseVector *resVect,
const VecDeriv* dx,
SReal mFact,
unsigned int& offset)
{
unsigned int derivDim = (unsigned)Deriv::size();
double m = d_mass.getValue();
ReadAccessor<Data<vector<int> > > indices = d_indices;
const SReal* g = getContext()->getGravity().ptr();
for (unsigned int i=0; i<indices.size(); i++)
for (unsigned int j=0; j<derivDim; j++)
{
if (dx != NULL)
resVect->add(offset + indices[i] * derivDim + j, mFact * m * g[j] * (*dx)[indices[i]][0]);
else
resVect->add(offset + indices[i] * derivDim + j, mFact * m * g[j]);
}
}
Vector6 DiagonalMass<Vec3Types, Vec3Mass>::getMomentumVec3Impl( const MechanicalParams*,
const DataVecCoord& vx,
const DataVecDeriv& vv ) const
{
ReadAccessor<DataVecDeriv> v = vv;
ReadAccessor<DataVecCoord> x = vx;
const MassVector &masses = d_vertexMass.getValue();
Vector6 momentum;
for ( unsigned int i=0 ; i<v.size() ; i++ )
{
Deriv linearMomentum = v[i] * masses[i];
for( int j=0 ; j<3 ; ++j ) momentum[j] += linearMomentum[j];
Deriv angularMomentum = cross( x[i], linearMomentum );
for( int j=0 ; j<3 ; ++j ) momentum[3+j] += angularMomentum[j];
}
return momentum;
}
Vector6 DiagonalMass<RigidTypes,RigidMass>::getMomentumRigid3Impl ( const MechanicalParams*,
const DataVecCoord& vx,
const DataVecDeriv& vv ) const
{
ReadAccessor<DataVecDeriv> v = vv;
ReadAccessor<DataVecCoord> x = vx;
const MassVector &masses = d_vertexMass.getValue();
defaulttype::Vector6 momentum;
for ( unsigned int i=0 ; i<v.size() ; i++ )
{
typename RigidTypes::Vec3 linearMomentum = v[i].getLinear() * masses[i].mass;
for( int j=0 ; j<3 ; ++j ) momentum[j] += linearMomentum[j];
typename RigidTypes::Vec3 angularMomentum = cross( x[i].getCenter(), linearMomentum ) + ( masses[i].inertiaMassMatrix * v[i].getAngular() );
for( int j=0 ; j<3 ; ++j ) momentum[3+j] += angularMomentum[j];
}
return momentum;
}
Vector6 UniformMass<Vec3Types, MassType>::getMomentumVec3DImpl ( const MechanicalParams*,
const DataVecCoord& d_x,
const DataVecDeriv& d_v ) const
{
ReadAccessor<DataVecDeriv> v = d_v;
ReadAccessor<DataVecCoord> x = d_x;
ReadAccessor<Data<vector<int> > > indices = d_indices;
const MassType& m = d_mass.getValue();
defaulttype::Vec6d momentum;
for ( unsigned int i=0 ; i<indices.size() ; i++ )
{
Deriv linearMomentum = m*v[indices[i]];
for( int j=0 ; j<3 ; ++j ) momentum[j] += linearMomentum[j];
Deriv angularMomentum = cross( x[indices[i]], linearMomentum );
for( int j=0 ; j<3 ; ++j ) momentum[3+j] += angularMomentum[j];
}
return momentum;
}
Vector6 UniformMass<RigidTypes,MassType>::getMomentumRigid3DImpl( const MechanicalParams*,
const DataVecCoord& d_x,
const DataVecDeriv& d_v ) const
{
ReadAccessor<DataVecDeriv> v = d_v;
ReadAccessor<DataVecCoord> x = d_x;
ReadAccessor<Data<vector<int> > > indices = d_indices;
Real m = d_mass.getValue().mass;
const typename MassType::Mat3x3& I = d_mass.getValue().inertiaMassMatrix;
defaulttype::Vec6d momentum;
for ( unsigned int i=0 ; i<indices.size() ; i++ )
{
typename RigidTypes::Vec3 linearMomentum = m*v[indices[i]].getLinear();
for( int j=0 ; j< 3 ; ++j ) momentum[j] += linearMomentum[j];
typename RigidTypes::Vec3 angularMomentum = cross( x[indices[i]].getCenter(), linearMomentum ) + ( I * v[indices[i]].getAngular() );
for( int j=0 ; j< 3 ; ++j ) momentum[3+j] += angularMomentum[j];
}
return momentum;
}
