void RestShapeSpringsForceField<Rigid3Types>::addForce(const core::MechanicalParams* /* mparams */, DataVecDeriv& f, const DataVecCoord& x, const DataVecDeriv& /* v */)
{
sofa::helper::WriteAccessor< DataVecDeriv > f1 = f;
sofa::helper::ReadAccessor< DataVecCoord > p1 = x;
sofa::helper::ReadAccessor< DataVecCoord > p0 = *getExtPosition();
f1.resize(p1.size());
if (recompute_indices.getValue())
{
recomputeIndices();
}
const VecReal& k = stiffness.getValue();
const VecReal& k_a = angularStiffness.getValue();
for (unsigned int i = 0; i < m_indices.size(); i++)
{
const unsigned int index = m_indices[i];
unsigned int ext_index = m_indices[i];
if(useRestMState)
ext_index= m_ext_indices[i];
// translation
if (i >= m_pivots.size())
{
Vec3d dx = p1[index].getCenter() - p0[ext_index].getCenter();
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
}
else
{
CPos localPivot = p0[ext_index].getOrientation().inverseRotate(m_pivots[i] - p0[ext_index].getCenter());
CPos rotatedPivot = p1[index].getOrientation().rotate(localPivot);
CPos pivot2 = p1[index].getCenter() + rotatedPivot;
CPos dx = pivot2 - m_pivots[i];
getVCenter(f1[index]) -= dx * (i < k.size() ? k[i] : k[0]) ;
}
// rotation
Quatd dq = p1[index].getOrientation() * p0[ext_index].getOrientation().inverse();
Vec3d dir;
double angle=0;
dq.normalize();
if (dq[3] < 0)
{
dq = dq * -1.0;
}
if (dq[3] < 0.999999999999999)
dq.quatToAxis(dir, angle);
getVOrientation(f1[index]) -= dir * angle * (i < k_a.size() ? k_a[i] : k_a[0]);
}
}
TEST(Boxes, ConstraintViolation)
{
const double TOL = 1e-6;
const double DT = 1e-2;
const std::string FNAME("box.xml");
const std::string BOX_ID("box");
shared_ptr<TimeSteppingSimulator> sim;
shared_ptr<RigidBody> box;
const unsigned NTIMES = 100;
// log contact
Moby::Log<Moby::OutputToFile>::reporting_level = (LOG_SIMULATOR | LOG_CONSTRAINT);
Moby::OutputToFile::stream.open("logging.out");
// log energy
std::ofstream energy_out("energy.dat");
// do this multiple times
for (unsigned j=0; j< NTIMES; j++)
{
// load in the box file
map<std::string, BasePtr> READ_MAP = XMLReader::read(FNAME);
// look for the simulator
find(READ_MAP, sim);
// look for the box
find(READ_MAP, box, BOX_ID);
// set tolerance for constraint stabilization and minimum step size
// sim->min_step_size = TOL/10000.0;
// sim->min_step_size = 1e-5;
sim->min_step_size = 1e-1;
sim->cstab.eps = -NEAR_ZERO;
// modify the initial orientation of the box
Quatd q;
q.x = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.y = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.z = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.w = (double) rand() / RAND_MAX * 2.0 - 1.0;
q.normalize();
Pose3d P = *box->get_pose();
P.q = q;
box->set_pose(P);
// modify the velocity of the box
Vector3d xd(box->get_inertial_pose());
Vector3d w(box->get_inertial_pose());
SVelocityd v(box->get_inertial_pose());
for (unsigned i=0; i< 3; i++)
{
xd[i] = (double) rand() / RAND_MAX * 2.0 - 1.0;
w[i] = (double) rand() / RAND_MAX * 2.0 - 1.0;
}
v.set_linear(xd);
v.set_angular(w);
box->set_velocity(v);
// set the first no K.E. time
double no_KE_time = 0.0;
// setup the maximum violation
double max_vio = 0.0;
// reset steps
unsigned step = 0;
// integrate the box until it has no kinetic energy for 1/2 sec
while (true)
{
// execute the step
sim->step(DT);
// get the constraint violation
const vector<PairwiseDistInfo>& pdi = sim->get_pairwise_distances();
for (unsigned i=0; i< pdi.size(); i++)
max_vio = std::min(max_vio, pdi[i].dist);
// get the kinetic energy of the box
double KE = box->calc_kinetic_energy();
energy_out << KE << std::endl;
// see whether there is no kinetic energy
if (KE < 1e-6)
{
if (sim->current_time - no_KE_time > 0.5)
break;
}
else
no_KE_time = sim->current_time;
if (step++ % 1000 == 0)
std::cerr << "." << std::flush;
}
// only want to print out one message about violation
EXPECT_GT(max_vio, -TOL);
std::cerr << "+" << std::flush;
energy_out.close();
}
}
void UniformMass<RigidTypes, MassType>::loadFromFileRigidImpl(const string& filename)
{
TemporaryLocale locale(LC_ALL, "C") ;
if (!filename.empty())
{
MassType m = getMass();
string unconstingFilenameQuirck = filename ;
if (!DataRepository.findFile(unconstingFilenameQuirck))
msg_error(this) << "cannot find file '" << filename << "'.\n" ;
else
{
char cmd[64];
FILE* file;
if ((file = fopen(filename.c_str(), "r")) == NULL)
{
msg_error(this) << "cannot open file '" << filename << "'.\n" ;
}
else
{
{
skipToEOL(file);
ostringstream cmdScanFormat;
cmdScanFormat << "%" << (sizeof(cmd) - 1) << "s";
while (fscanf(file, cmdScanFormat.str().c_str(), cmd) != EOF)
{
if (!strcmp(cmd,"inrt"))
{
for (int i = 0; i < 3; i++){
for (int j = 0; j < 3; j++){
double tmp = 0;
if( fscanf(file, "%lf", &(tmp)) < 1 ){
msg_error(this) << "error while reading file '" << filename << "'.\n";
}
m.inertiaMatrix[i][j]=tmp;
}
}
}
else if (!strcmp(cmd,"cntr") || !strcmp(cmd,"center") )
{
Vec3d center;
for (int i = 0; i < 3; ++i)
{
if( fscanf(file, "%lf", &(center[i])) < 1 ){
msg_error(this) << "error reading file '" << filename << "'.";
}
}
}
else if (!strcmp(cmd,"mass"))
{
double mass;
if( fscanf(file, "%lf", &mass) > 0 )
{
if (!this->d_mass.isSet())
m.mass = mass;
}
else
msg_error(this) << "error reading file '" << filename << "'." << msgendl
<< "Unable to decode command 'mass'";
}
else if (!strcmp(cmd,"volm"))
{
double tmp;
if( fscanf(file, "%lf", &(tmp)) < 1 )
msg_error(this) << "error reading file '" << filename << "'." << msgendl
<< "Unable to decode command 'volm'." << msgendl;
m.volume = tmp ;
}
else if (!strcmp(cmd,"frme"))
{
Quatd orient;
for (int i = 0; i < 4; ++i)
{
if( fscanf(file, "%lf", &(orient[i])) < 1 )
msg_error(this) << "error reading file '" << filename << "'." << msgendl
<< "Unable to decode command 'frme' at index " << i ;
}
orient.normalize();
}
else if (!strcmp(cmd,"grav"))
{
Vec3d gravity;
if( fscanf(file, "%lf %lf %lf\n", &(gravity.x()), &(gravity.y()), &(gravity.z())) < 3 )
msg_warning(this) << "error reading file '" << filename << "'." << msgendl
<< " Unable to decode command 'gravity'.";
}
else if (!strcmp(cmd,"visc"))
{
double viscosity = 0;
if( fscanf(file, "%lf", &viscosity) < 1 )
msg_warning(this) << "error reading file '" << filename << "'.\n"
" Unable to decode command 'visc'. \n";
}
else if (!strcmp(cmd,"stck"))
{
double tmp;
if( fscanf(file, "%lf", &tmp) < 1 ) //&(MSparams.default_stick));
msg_warning(this) << "error reading file '" << filename << "'.\n"
<< "Unable to decode command 'stck'. \n";
}
else if (!strcmp(cmd,"step"))
{
double tmp;
if( fscanf(file, "%lf", &tmp) < 1 ) //&(MSparams.default_dt));
msg_warning(this) << "error reading file '" << filename << "'.\n"
<< "Unable to decode command 'step'. \n";
}
else if (!strcmp(cmd,"prec"))
{
double tmp;
if( fscanf(file, "%lf", &tmp) < 1 ) //&(MSparams.default_prec));
{
msg_warning(this) << "error reading file '" << filename << "'.\n"
<< "Unable to decode command 'prec'. \n" ;
}
}
else if (cmd[0] == '#') // it's a comment
{
skipToEOL(file);
}
else // it's an unknown keyword
{
msg_warning(this) << "error reading file '" << filename << "'. \n"
<< "Unable to decode an unknow command '"<< cmd << "'. \n" ;
skipToEOL(file);
}
}
}
fclose(file);
}
}
setMass(m);
}
else if (d_totalMass.getValue()>0 && mstate!=NULL) d_mass.setValue((Real)d_totalMass.getValue() / mstate->getSize());
}