bool testBezierTetrahedronGeometry()
{
BezierTetrahedronSetTopologyContainer *container=root->get<BezierTetrahedronSetTopologyContainer>(root->SearchDown);
typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(std::string("BezierTetrahedronTopology/"));
typename MechanicalObject::WriteVecCoord coords = dofs->writePositions();
size_t i,j;
BezierDegreeType degree=container->getDegree();
sofa::component::topology::VecPointID indexArray;
sofa::helper::vector<TetrahedronBezierIndex> tbiArray=container->getTetrahedronBezierIndexArray();
for ( i = 0; i<container->getNumberOfTetrahedra(); i++)
{
indexArray.clear();
container->getGlobalIndexArrayOfBezierPointsInTetrahedron(i, indexArray);
for (j=0;j<tbiArray.size();++j) {
if (j>=4) {
// test if the position is correct
Coord pos=coords[indexArray[0]]*(Real)tbiArray[j][0]/degree+coords[indexArray[1]]*(Real)tbiArray[j][1]/degree+coords[indexArray[2]]*(Real)tbiArray[j][2]/degree+coords[indexArray[3]]*(Real)tbiArray[j][3]/degree;
if ((pos-coords[indexArray[j]]).norm()>1e-5) {
ADD_FAILURE() << "Wrong control point position in tetrahedron no : "<<i <<" for point of local index " <<j
<< " Got point position="<<coords[indexArray[j]]<<" instead of "<<pos<<std::endl;
return false;
}
}
}
}
return true;
}
/** Constrain one particle, and not the last one.
Detects bugs like not setting the projection matrix entries beyond the last constrained particle
*/
void init_2bones()
{
joints.clear();
typename MechanicalObject::WriteVecCoord x = dofs->writePositions();
x.resize(2);
VecCoord rigids(2);
DataTypes::setCPos(x[1], CPos(0,1,0));
DataTypes::setCRot(x[1], CRot(0.707107,0, 0, 0.707107)); // rotation x: 90 degree
Coord target(CPos(1,1,1), CRot(0,0.382683,0,0.92388)); //rotation y : 45 degrees
joints.resize(2);
joints[0].addChannel(x[0], 0);
joints[0].addChannel(target, 1);
joints[0].setRestPosition(x[0]);
joints[1].addChannel(x[1], 0);
joints[1].setRestPosition(x[1]);
joints[1].mParentIndex = 0;
helper::vector<int> bones(2,0); bones[1] = 1;
projection->setSkeletalMotion(joints, bones);
/// Init
sofa::simulation::getSimulation()->init(root.get());
simulation->animate(root.get(),0.25);
simulation->animate(root.get(),0.25);
}
bool testBezierTetrahedronMass()
{
BezierTetrahedronSetTopologyContainer *container=root->get<BezierTetrahedronSetTopologyContainer>(root->SearchDown);
BezierTetrahedronSetGeometryAlgorithms *geo=root->get<BezierTetrahedronSetGeometryAlgorithms>(root->SearchDown);
typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(std::string("BezierTetrahedronTopology/"));
typename MechanicalObject::WriteVecCoord coords = dofs->writePositions();
MeshMatrixMass *mass=root->get<MeshMatrixMass>(root->SearchDown);
const sofa::helper::vector<typename MeshMatrixMass::MassVector> & mv=mass->tetrahedronMassInfo.getValue();
const sofa::helper::vector<typename MeshMatrixMass::MassType> &ma =mass->vertexMassInfo.getValue();
size_t i,j,k,rank;
BezierDegreeType degree=container->getDegree();
Real tetraVol1,tetraVol2,totalVol1,totalVol2;
sofa::helper::vector<TetrahedronBezierIndex> tbiArray=container->getTetrahedronBezierIndexArray();
size_t nbControlPoints=(degree+1)*(degree+2)*(degree+3)/6;
totalVol1=0;
for ( i = 0; i<container->getNumberOfTetrahedra(); i++)
{
// const BezierTetrahedronSetTopologyContainer::VecPointID &indexArray=container->getGlobalIndexArrayOfBezierPoints(i);
/// get the volume of the tetrahedron
tetraVol1=geo->computeTetrahedronVolume(i);
tetraVol2=0;
// compute the total volume
totalVol1+=tetraVol1;
/// check that the sum of the volume matrix elements is equal to the volume of the tetrahedron
for (rank=0,j=0;j<nbControlPoints;j++) {
for (k=j;k<nbControlPoints;k++,rank++) {
if (k==j)
// add diagonal term
tetraVol2+=mv[i][rank];
else
// add 2 times off-diagonal term
tetraVol2+=2*mv[i][rank];
}
}
if (fabs(tetraVol1-tetraVol2)>1e-5) {
ADD_FAILURE() << "Wrong mass matrix in tetrahedron no : "<<i
<< " Got total mass="<<tetraVol2<<" instead of "<<tetraVol1<<std::endl;
return false;
}
}
// compute totalVol2 as the total of the lumped volume
totalVol2=0;
for ( i = 0; i<ma.size(); i++)
{
totalVol2+=ma[i];
}
if (fabs(totalVol1-totalVol2)>1e-5) {
ADD_FAILURE() << "Wrong total vertex mass value."
<< " Got total vertex mass="<<totalVol2<<" instead of "<<totalVol1<<std::endl;
return false;
}
return true;
}
bool test_projectPosition()
{
VecCoord xprev(numNodes);
typename MechanicalObject::WriteVecCoord x = dofs->writePositions();
for (unsigned i=0; i<numNodes; i++){
xprev[i] = x[i] = CPos(i,0,0);
}
// cerr<<"test_projectPosition, x before = " << x << endl;
projection->projectPosition(core::MechanicalParams::defaultInstance(), *dofs->write(core::VecCoordId::position()) );
// cerr<<"test_projectPosition, x after = " << x << endl;
bool succeed=true;
typename Indices::const_iterator it = indices.begin(); // must be sorted
for(unsigned i=0; i<numNodes; i++ )
{
if ((it!=indices.end()) && ( i==*it )) // constrained particle
{
CPos crossprod = (x[i]-origin).cross(direction); // should be parallel
Real scal = crossprod*crossprod; // null if x is on the line
// cerr<<"scal = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Position of constrained particle " << i << " is wrong: " << x[i] ;
}
it++;
}
else // unconstrained particle: check that it has not changed
{
CPos dx = x[i]-xprev[i];
Real scal = dx*dx;
// cerr<<"scal gap = "<< scal << endl;
if( !Sofa_test<typename _DataTypes::Real>::isSmall(scal,100) ){
succeed = false;
ADD_FAILURE() << "Position of unconstrained particle " << i << " is wrong: " << x[i] ;
}
}
}
return succeed;
}
/// After simulation compare the positions of points to the theoretical positions.
bool compareSimulatedToTheoreticalPositions(double convergenceAccuracy, double diffMaxBetweenSimulatedAndTheoreticalPosition)
{
// Init simulation
sofa::simulation::getSimulation()->init(root.get());
// Compute the theoretical final positions
VecCoord finalPos;
typename AffineMovementConstraint::SPtr affineConstraint = root->get<AffineMovementConstraint>(root->SearchDown);
typename MechanicalObject::SPtr dofs = root->get<MechanicalObject>(root->SearchDown);
typename MechanicalObject::WriteVecCoord x = dofs->writePositions();
affineConstraint->getFinalPositions( finalPos,*dofs->write(core::VecCoordId::position()) );
// Set random rotation and translation for affine constraint
randomGenerator.initSeed(seed);
this->SetRandomTestedRotationAndTranslation(seed);
// Set data values of affine movement constraint
affineConstraint->m_rotation.setValue(testedRotation);
affineConstraint->m_translation.setValue(testedTranslation);
// Initialize
size_t numNodes = finalPos.size();
VecCoord xprev(numNodes);
VecDeriv dx(numNodes);
bool hasConverged = true;
for (size_t i=0; i<numNodes; i++)
{
xprev[i] = CPos(0,0,0);
}
// Animate
do
{
hasConverged = true;
sofa::simulation::getSimulation()->animate(root.get(),0.5);
typename MechanicalObject::ReadVecCoord x = dofs->readPositions();
// Compute dx
for (size_t i=0; i<x.size(); i++)
{
dx[i] = x[i]-xprev[i];
// Test convergence
if(dx[i].norm()>convergenceAccuracy) hasConverged = false;
}
// xprev = x
for (size_t i=0; i<numNodes; i++)
{
xprev[i]=x[i];
}
}
while(!hasConverged); // not converged
// Compare the theoretical positions and the simulated positions
bool succeed=true;
for(size_t i=0; i<finalPos.size(); i++ )
{
if((finalPos[i]-x[i]).norm()>diffMaxBetweenSimulatedAndTheoreticalPosition)
{
succeed = false;
ADD_FAILURE() << "final Position of point " << i << " is wrong: " << x[i] << std::endl <<"the expected Position is " << finalPos[i] << std::endl
<< "difference = " <<(finalPos[i]-x[i]).norm() << std::endl;
}
}
return succeed;
}