void RunFloat2Or3Tests(std::string const& typeName)
{
RunCommonVectorTests<T>(typeName);
RunPerfTest<T, T>(typeName + " reflect", [](T* value, T const& param)
{
*value = reflect(*value, param);
});
RunPerfTest<T, float4x4>(typeName + " transform_normal (float4x4)", [](T* value, float4x4 const& param)
{
*value = transform_normal(*value, param);
});
RunPerfTest<T, float4x4>(typeName + " transform4 (float4x4)", [](T* value, float4x4 const& param)
{
float4 t = transform4(*value, param);
value->x = t.x + t.y + t.z + t.w;
});
RunPerfTest<T, quaternion>(typeName + " transform4 (quaternion)", [](T* value, quaternion const& param)
{
float4 t = transform4(*value, param);
value->x = t.x + t.y + t.z + t.w;
});
}
void HomogeneousMatrixTest::testRPYConversions() {
double xExpected, yExpected, zExpected, rollExpected, pitchExpected, yawExpected;
double xActual, yActual, zActual, rollActual, pitchActual, yawActual;
IHomogeneousMatrix44::IHomogeneousMatrix44Ptr transform1(new HomogeneousMatrix44());
xExpected = 1.0;
yExpected = 2.0;
zExpected = 3.0;
rollExpected = 0.5*M_PI;
pitchExpected = 0.0;
yawExpected = 0.0;
HomogeneousMatrix44::xyzRollPitchYawToMatrix(xExpected, yExpected, zExpected, rollExpected, pitchExpected, yawExpected, transform1);
std::cout << "---> transform1 " << std::endl <<*transform1;
HomogeneousMatrix44::matrixToXyzRollPitchYaw(transform1, xActual, yActual, zActual, rollActual, pitchActual, yawActual);
CPPUNIT_ASSERT_DOUBLES_EQUAL(xExpected, xActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yExpected, yActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(zExpected, zActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rollExpected, rollActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(pitchExpected, pitchActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yawExpected, yawActual, maxTolerance);
xExpected = 1.0;
yExpected = 2.0;
zExpected = 3.0;
rollExpected = 0.0;
pitchExpected = 0.0;
yawExpected = 0.5 * M_PI;
Eigen::AngleAxis<double> rotation2(yawExpected, Eigen::Vector3d(0,0,1));
Transform3d transformation2;
transformation2 = Eigen::Affine3d::Identity();
transformation2.translate(Eigen::Vector3d(xExpected,yExpected,zExpected));
transformation2.rotate(rotation2);
IHomogeneousMatrix44::IHomogeneousMatrix44Ptr transform2(new HomogeneousMatrix44(&transformation2));
HomogeneousMatrix44::matrixToXyzRollPitchYaw(transform2, xActual, yActual, zActual, rollActual, pitchActual, yawActual);
CPPUNIT_ASSERT_DOUBLES_EQUAL(xExpected, xActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yExpected, yActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(zExpected, zActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rollExpected, rollActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(pitchExpected, pitchActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yawExpected, yawActual, maxTolerance);
xExpected = 1.0;
yExpected = 2.0;
zExpected = 3.0;
rollExpected = 0.0;
pitchExpected = 0.5 * M_PI;
yawExpected = 0;
Eigen::AngleAxis<double> rotation3(pitchExpected, Eigen::Vector3d(0,1,0));
Transform3d transformation3;
transformation3 = Eigen::Affine3d::Identity();
transformation3.translate(Eigen::Vector3d(xExpected,yExpected,zExpected));
transformation3.rotate(rotation3);
IHomogeneousMatrix44::IHomogeneousMatrix44Ptr transform3(new HomogeneousMatrix44(&transformation3));
HomogeneousMatrix44::matrixToXyzRollPitchYaw(transform3, xActual, yActual, zActual, rollActual, pitchActual, yawActual);
CPPUNIT_ASSERT_DOUBLES_EQUAL(xExpected, xActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yExpected, yActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(zExpected, zActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rollExpected, rollActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(pitchExpected, pitchActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yawExpected, yawActual, maxTolerance);
xExpected = 1.0;
yExpected = 2.0;
zExpected = 3.0;
rollExpected = 0.5 * M_PI;
pitchExpected = 0.0;
yawExpected = 0;
Eigen::AngleAxis<double> rotation4(rollExpected, Eigen::Vector3d(1,0,0));
Transform3d transformation4;
transformation4 = Eigen::Affine3d::Identity();
transformation4.translate(Eigen::Vector3d(xExpected,yExpected,zExpected));
transformation4.rotate(rotation4);
IHomogeneousMatrix44::IHomogeneousMatrix44Ptr transform4(new HomogeneousMatrix44(&transformation4));
HomogeneousMatrix44::matrixToXyzRollPitchYaw(transform4, xActual, yActual, zActual, rollActual, pitchActual, yawActual);
std::cout << "---> transform4 " << std::endl <<*transform4;
CPPUNIT_ASSERT_DOUBLES_EQUAL(xExpected, xActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yExpected, yActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(zExpected, zActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(rollExpected, rollActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(pitchExpected, pitchActual, maxTolerance);
CPPUNIT_ASSERT_DOUBLES_EQUAL(yawExpected, yawActual, maxTolerance);
/* CHecke if Eigen generated and xyzRPY generated _matices_ are similar */
const double* matrixData1 = transform1->getRawData();
const double* matrixData4 = transform4->getRawData();
for (unsigned int i = 0; i < 16; ++i) {
CPPUNIT_ASSERT_DOUBLES_EQUAL(matrixData1[i], matrixData4[i], maxTolerance);
}
}
/**
* Program entry-point.
*
*/
int main(int argc, char **argv) {
// parse arguments
while (true) {
int index = -1;
getopt_long(argc, argv, "", options, &index);
if (index == -1) {
if (argc != optind + 2) {
usage();
return 1;
}
input_file = argv[optind++];
if (access(input_file, R_OK)) {
fprintf(stderr, "Error: input file not readable: %s\n", input_file);
return 2;
}
output_file = argv[optind++];
if (access(output_file, W_OK) && errno == EACCES) {
fprintf(stderr, "Error: output file not writable: %s\n", output_file);
return 2;
}
break;
}
switch (index) {
case OPTION_WIDTH:
sample_width = atoi(optarg);
break;
case OPTION_HEIGHT:
sample_height = atoi(optarg);
break;
case OPTION_COUNT:
sample_count = atoi(optarg);
break;
case OPTION_ROTATE_STDDEV_X:
rotate_stddev_x = atof(optarg) / 180.0 * M_PI;
break;
case OPTION_ROTATE_STDDEV_Y:
rotate_stddev_y = atof(optarg) / 180.0 * M_PI;
break;
case OPTION_ROTATE_STDDEV_Z:
rotate_stddev_z = atof(optarg) / 180.0 * M_PI;
break;
case OPTION_LUMINOSITY_STDDEV:
luminosity_stddev = atof(optarg);
break;
case OPTION_BACKGROUNDS:
backgrounds_file = optarg;
if (access(backgrounds_file, R_OK)) {
fprintf(stderr, "Error: backgrounds file not readable: %s\n", backgrounds_file);
return 2;
}
break;
default:
usage();
return 1;
}
}
// read input files
std::vector<std::string> samples;
if (!parseFiles(input_file, samples)) {
fprintf(stderr, "Error: cannot parse file listing: %s\n", input_file);
return 2;
}
// read background files
std::vector<std::string> backgrounds;
if (backgrounds_file != NULL && !parseFiles(backgrounds_file, backgrounds)) {
fprintf(stderr, "Error: cannot parse file listing: %s\n", backgrounds_file);
return 2;
}
// create output file
FILE *fp = fopen(output_file, "wb");
if (fp == NULL) {
fprintf(stderr, "Error: cannot open output file for writing: %s\n", output_file);
return 2;
}
icvWriteVecHeader(fp, sample_count, sample_width, sample_height);
// generate distortions
std::default_random_engine generator(time(NULL));
std::normal_distribution<double> xdist(0.0, rotate_stddev_x / 3.0);
std::normal_distribution<double> ydist(0.0, rotate_stddev_y / 3.0);
std::normal_distribution<double> zdist(0.0, rotate_stddev_z / 3.0);
std::normal_distribution<double> ldist(0.0, luminosity_stddev / 3.0);
cv::Mat el = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
int variations = MAX(1, (int)floor((double)sample_count / (double)samples.size()));
int idx = 0;
int i = 0;
while (i < sample_count) {
// suffle the input lists
if (idx % samples.size() == 0) {
std::shuffle(samples.begin(), samples.end(), generator);
std::shuffle(backgrounds.begin(), backgrounds.end(), generator);
}
// read sample image
auto const &sample_file(samples[idx % samples.size()]);
cv::Mat sample = cv::imread(sample_file);
double sampleRatio = (double)sample.cols / (double)sample.rows;
double outputRatio = (double)sample_width / (double)sample_height;
// normalize sample
cv::Mat greySample = sample;
double min, max;
if (sample.channels() != 1) {
cv::cvtColor(sample, greySample, cv::COLOR_RGB2GRAY);
}
cv::minMaxIdx(greySample, &min, &max);
greySample -= min;
greySample /= (max - min) / 255.0;
// generate mask
cv::Mat mask(cv::Mat::ones(greySample.rows, greySample.cols, greySample.type()));
// enlarge canvas to fit output ratio
cv::Mat resizedSample, resizedMask;
if (backgrounds.size() > 0 && sampleRatio < outputRatio) {
int width = (int)((double)greySample.rows * outputRatio);
cv::Rect area(
(width - greySample.cols) / 2,
0,
greySample.cols,
greySample.rows
);
resizedSample = cv::Mat::zeros(greySample.rows, width, greySample.type());
resizedMask = cv::Mat::zeros(greySample.rows, width, greySample.type());
greySample.copyTo(resizedSample(area));
mask.copyTo(resizedMask(area));
} else if (backgrounds.size() > 0 && sampleRatio > outputRatio) {
int height = (int)((double)greySample.cols / outputRatio);
cv::Rect area(
0,
(height - greySample.rows) / 2,
greySample.cols,
greySample.rows
);
resizedSample = cv::Mat::zeros(height, greySample.cols, greySample.type());
resizedMask = cv::Mat::zeros(height, greySample.cols, greySample.type());
greySample.copyTo(resizedSample(area));
mask.copyTo(resizedMask(area));
} else {
resizedSample = greySample;
resizedMask = mask;
}
// apply distortions
cv::Mat target(resizedSample.rows, resizedSample.cols, resizedSample.type());
cv::Mat targetMask(resizedSample.rows, resizedSample.cols, resizedSample.type());
double halfWidth = resizedSample.cols / 2.0;
double halfHeight = resizedSample.rows / 2.0;
cv::Mat rotationVector(3, 1, CV_64FC1);
cv::Mat rotation4(cv::Mat::eye(4, 4, CV_64FC1));
cv::Mat translate4(cv::Mat::eye(4, 4, CV_64FC1));
cv::Mat translate3(cv::Mat::eye(3, 3, CV_64FC1));
cv::Mat scale3(cv::Mat::eye(3, 3, CV_64FC1));
int dx = (resizedSample.cols - greySample.cols) / 2;
int dy = (resizedSample.rows - greySample.rows) / 2;
cv::Point2f points1[4] = {
cv::Point2f(dx, dy),
cv::Point2f(dx, greySample.rows),
cv::Point2f(greySample.cols, greySample.rows),
cv::Point2f(greySample.cols, dy)
};
cv::Point2f points2[4];
translate4.at<double>(0, 3) = -halfWidth;
translate4.at<double>(1, 3) = -halfHeight;
for (int k = 0; k < variations; k++) {
double rx = k > 0 && rotate_stddev_x > 0.0 ? xdist(generator) : 0.0;
double ry = k > 0 && rotate_stddev_y > 0.0 ? ydist(generator) : 0.0;
double rz = k > 0 && rotate_stddev_z > 0.0 ? zdist(generator) : 0.0;
double rl = k > 0 && luminosity_stddev > 0.0 ? ldist(generator) : 0.0;
// compute rotation in 3d
rotationVector.at<double>(0) = rx;
rotationVector.at<double>(1) = ry;
rotationVector.at<double>(2) = rz;
cv::Rodrigues(rotationVector, cv::Mat(rotation4, cv::Rect(0, 0, 3, 3)));
// compute transformation in 3d
cv::Mat transform4(rotation4 * translate4);
double minx = DBL_MAX, miny = DBL_MAX;
double maxx = DBL_MIN, maxy = DBL_MIN;
for (int j = 0; j < 4; j++) {
cv::Mat point(4, 1, CV_64FC1);
point.at<double>(0) = points1[j].x;
point.at<double>(1) = points1[j].y;
point.at<double>(2) = 0.0;
point.at<double>(3) = 1.0;
point = transform4 * point;
points2[j].x = point.at<double>(0);
points2[j].y = point.at<double>(1);
if (points2[j].x < minx) {
minx = points2[j].x;
}
if (points2[j].x > maxx) {
maxx = points2[j].x;
}
if (points2[j].y < miny) {
miny = points2[j].y;
}
if (points2[j].y > maxy) {
maxy = points2[j].y;
}
}
// compute transformation in 2d
cv::Mat projection3(cv::getPerspectiveTransform(points1, points2));
double scalex = (resizedSample.cols - dx) / (maxx - minx);
double scaley = (resizedSample.rows - dy) / (maxy - miny);
translate3.at<double>(0, 2) = halfWidth;
translate3.at<double>(1, 2) = halfHeight;
scale3.at<double>(0, 0) = scalex; //MIN(scalex, scaley);
scale3.at<double>(1, 1) = scaley; //MIN(scalex, scaley);
// transform sample and mask in 2d
cv::Mat transform3(translate3 * scale3 * projection3);
cv::warpPerspective(resizedSample, target, transform3, target.size());
cv::warpPerspective(resizedMask, targetMask, transform3, targetMask.size());
// apply luminosity change
if (rl != 0.0) {
rl += 1.0;
target *= rl;
}
// read background image
cv::Mat greyBackground;
if (backgrounds.size() > 0) {
auto const &background_file(backgrounds[i % backgrounds.size()]);
cv::Mat background = cv::imread(background_file);
// normalize background image
if (background.channels() != 1) {
cv::cvtColor(background, greyBackground, cv::COLOR_RGB2GRAY);
} else {
greyBackground = background;
}
cv::minMaxIdx(greyBackground, &min, &max);
greyBackground -= min;
greyBackground /= (max - min) / 255.0;
// reshape background to fit output ratio
double backgroundRatio = (double)greyBackground.cols / (double)greyBackground.rows;
cv::Mat tmp;
if (backgroundRatio < outputRatio) {
int height = (int)((double)greyBackground.cols / outputRatio);
std::uniform_int_distribution<int> hdist(0, greyBackground.rows - height);
tmp = greyBackground(
cv::Rect(
0,
hdist(generator),
greyBackground.cols,
height
)
);
} else if (backgroundRatio > outputRatio) {
int width = (int)((double)greyBackground.rows * outputRatio);
std::uniform_int_distribution<int> wdist(0, greyBackground.cols - width);
tmp = greyBackground(
cv::Rect(
wdist(generator),
0,
width,
greyBackground.rows
)
);
} else {
tmp = greyBackground;
}
cv::resize(tmp, greyBackground, resizedSample.size(), 0, 0, cv::INTER_CUBIC);
} else {
// random noise background
greyBackground = cv::Mat(target.rows, target.cols, CV_8UC1);
cv::randn(greyBackground, 255.0 / 2, 255.0 / 2 / 3);
cv::GaussianBlur(greyBackground, greyBackground, cv::Size(5, 5), 10);
}
// blend background
cv::Mat sampleMask, backgroundMask, tmp;
cv::threshold(targetMask, sampleMask, 0.1, 255.0, cv::THRESH_BINARY);
cv::erode(sampleMask, tmp, el);
cv::blur(tmp, sampleMask, cv::Size(5, 5));
cv::threshold(targetMask, backgroundMask, 0.1, 255.0, cv::THRESH_BINARY_INV);
cv::dilate(backgroundMask, tmp, el);
cv::blur(tmp, backgroundMask, cv::Size(5, 5));
cv::multiply(target, sampleMask, target, 1.0 / 255.0);
cv::multiply(greyBackground, backgroundMask, greyBackground, 1.0 / 255.0);
target += greyBackground;
// cv::namedWindow("preview", cv::WINDOW_NORMAL);
// cv::imshow("preview", target);
// while ((cv::waitKey(0) & 0xff) != '\n');
// cv::namedWindow("preview", cv::WINDOW_NORMAL);
// cv::imshow("preview", greyBackground);
// while ((cv::waitKey(0) & 0xff) != '\n');
// sample resize
cv::Mat finalSample;
cv::resize(target, finalSample, cv::Size(sample_width, sample_height), 0, 0, cv::INTER_CUBIC);
// cv::namedWindow("preview", cv::WINDOW_NORMAL);
// cv::imshow("preview", finalSample);
// while ((cv::waitKey(0) & 0xff) != '\n');
// sample save
CvMat targetfinal_ = finalSample;
icvWriteVecSample(fp, &targetfinal_);
i++;
if (i % 100 == 0) {
fprintf(stdout, "processed %d images, %d samples\n", idx, i);
fflush(stdout);
}
}
idx++;
}
// close output file
fclose(fp);
return 0;
}
void IHPDriver::handleEvent(core::objectmodel::Event *event)
{
//std::cout<<"IHPDriver::handleEvent() called:"<<std::endl;//////////////////////////////////////////////////////
static double time_prev;
if (firstDevice && dynamic_cast<sofa::simulation::AnimateEndEvent *> (event))
{
// force the simulation to be "real-time"
CTime *timer;
timer = new CTime();
double time = 0.001*timer->getRefTime()* PaceMaker::time_scale; // in sec
// if the computation time is shorter than the Dt set in the simulation... it waits !
if ((time- time_prev) < getContext()->getDt() )
{
double wait_time = getContext()->getDt() - time + time_prev;
timer->sleep(wait_time);
}
time_prev=time;
}
if (dynamic_cast<sofa::simulation::AnimateBeginEvent *>(event))
{
//turnOn xitactVisu
if(!visuActif && xitactVisu.getValue() && initVisu)
{
simulation::Node *parent = dynamic_cast<simulation::Node *>(this->getContext());
parent->addChild(nodeXitactVisual);
nodeXitactVisual->updateContext();
visuActif = true;
}
//turnOff xitactVisu
else if(initVisu && visuActif && !xitactVisu.getValue())
{
simulation::Node *parent = dynamic_cast<simulation::Node *>(this->getContext());
parent->removeChild(nodeXitactVisual);
nodeXitactVisual->updateContext();
visuActif=false;
}
// calcul des angles à partir de la direction proposée par l'interface...
// cos(ThetaX) = cx sin(ThetaX) = sx cos(ThetaZ) = cz sin(ThetaZ) = sz .
// au repos (si cx=1 et cz=1) on a Axe y
// on commence par tourner autour de x puis autour de z
// [cz -sz 0] [1 0 0 ] [0] [ -sz*cx]
// [sz cz 0]*[0 cx -sx]*[1] = [ cx*cz ]
// [0 0 1] [0 sx cx] [0] [ sx ]
xiTrocarAcquire();
XiToolState state;
xiTrocarQueryStates();
xiTrocarGetState(indexTool.getValue(), &state);
// saving informations in class structure.
data.simuState = state;
Vector3 dir;
dir[0] = -(double)state.trocarDir[0];
dir[1] = (double)state.trocarDir[2];
dir[2] = -(double)state.trocarDir[1];
double pi = 3.1415926535;
double thetaY;
double thetaX;
thetaY = (atan2(dir[0],-sqrt(1-dir[0]*dir[0])));
thetaX = (pi-acos(dir[2]*sqrt(1-dir[0]*dir[0])/(dir[0]*dir[0]-1)));
//look if thetaX and thetaY are NaN
if(!(thetaX == thetaX))
{
cout<<"ratrapage X"<<endl;
thetaX=pi;
}
if(!(thetaY == thetaY))
{
cout<<"ratrapage Y"<<endl;
thetaY=pi;
}
if(dir[1]>=0)
thetaX*=-1;
while(thetaY<=0)
thetaY+=2*pi;
while(thetaX<=0)
thetaX+=2*pi;
while(thetaY>2*pi)
thetaY-=2*pi;
while(thetaX>2*pi)
thetaX-=2*pi;
if (showToolStates.getValue()) // print tool state
this->displayState();
if (testFF.getValue()) // try FF when closing handle
this->updateForce();
// Button and grasp handling event
XiStateFlags stateFlag;
stateFlag = state.flags - data.restState.flags;
if (stateFlag == XI_ToolButtonLeft)
this->leftButtonPushed();
else if (stateFlag == XI_ToolButtonRight)
this->rightButtonPushed();
if (state.opening < graspThreshold.getValue())
{
this->graspClosed();
}
//XitactVisu
VecCoord& posD =(*visualXitactDOF->x.beginEdit());
posD.resize(4);
VecCoord& posA =(*visualAxesDOF->x.beginEdit());
posA.resize(3);
VecCoord& posB =(*positionBase.beginEdit());
//data.positionBaseGlobal[0]=posB[0];
data.posBase=posB[0].getCenter();
data.quatBase=posB[0].getOrientation();
SolidTypes<double>::Transform tampon(posB[0].getCenter(),posB[0].getOrientation());
positionBase.endEdit();
posD[0].getCenter() = tampon.getOrigin();
posD[0].getOrientation() = tampon.getOrientation();
sofa::helper::Quater<double> qRotX(Vec3d(1,0,0),pi/2);
sofa::helper::Quater<double> qRotY(Vec3d(0,0,-1),pi/2);
SolidTypes<double>::Transform transformRotX(Vec3d(0.0,0.0,0.0),qRotX);
SolidTypes<double>::Transform transformRotY(Vec3d(0.0,0.0,0.0),qRotY);
SolidTypes<double>::Transform tamponAxes=tampon;
posA[0].getCenter() = tamponAxes.getOrigin();
posA[0].getOrientation() = tamponAxes.getOrientation();
tamponAxes*=transformRotX;
posA[1].getCenter() = tamponAxes.getOrigin();
posA[1].getOrientation() = tamponAxes.getOrientation();
tamponAxes*=transformRotY;
posA[2].getCenter() = tamponAxes.getOrigin();
posA[2].getOrientation() = tamponAxes.getOrientation();
sofa::helper::Quater<double> qy(Vec3d(0,1,0),thetaY);
sofa::helper::Quater<double> qx(Vec3d(1,0,0),thetaX);
SolidTypes<double>::Transform transform2(Vec3d(0.0,0.0,0.0),qx*qy);
tampon*=transform2;
posD[1].getCenter() = tampon.getOrigin();
posD[1].getOrientation() = tampon.getOrientation();
sofa::helper::Quater<float> quarter3(Vec3d(0.0,0.0,1.0),-state.toolRoll);
SolidTypes<double>::Transform transform3(Vec3d(0.0,0.0,-state.toolDepth*Scale.getValue()),quarter3);
tampon*=transform3;
posD[2].getCenter() = tampon.getOrigin();
posD[2].getOrientation() = tampon.getOrientation();
if(posTool)
{
VecCoord& posT = *(posTool->x0.beginEdit());
//cout<<"xitact "<<deviceIndex.getValue()<<" "<<posD[2]<<endl;
posT[deviceIndex.getValue()]=posD[2];
posTool->x0.endEdit();
}
sofa::helper::Quater<float> quarter4(Vec3d(0.0,1.0,0.0),-data.simuState.opening/(float)2.0);
SolidTypes<double>::Transform transform4(Vec3d(0.0,0.0,0.44*Scale.getValue()),quarter4);
tampon*=transform4;
posD[3].getCenter() = tampon.getOrigin();
posD[3].getOrientation() = tampon.getOrientation();
visualXitactDOF->x.endEdit();
Vec1d& openT = (*openTool.beginEdit());
openT[0]=(data.simuState.opening)*(maxTool.getValue()-minTool.getValue())+minTool.getValue();
openTool.endEdit();
if(changeScale)
{
float rapport=((float)Scale.getValue())/oldScale;
for(int j = 0; j<4 ; j++)
{
sofa::defaulttype::ResizableExtVector<sofa::defaulttype::Vec<3,float>> &scaleMapping = *(visualNode[j].mapping->points.beginEdit());
for(unsigned int i=0; i<scaleMapping.size(); i++)
{
for(int p=0; p<3; p++)
scaleMapping[i].at(p)*=rapport;
}
visualNode[j].mapping->points.endEdit();
}
oldScale=(float)Scale.getValue();
changeScale=false;
}
}
if (dynamic_cast<core::objectmodel::KeypressedEvent *>(event))
{
core::objectmodel::KeypressedEvent *kpe = dynamic_cast<core::objectmodel::KeypressedEvent *>(event);
onKeyPressedEvent(kpe);
}
else if (dynamic_cast<core::objectmodel::KeyreleasedEvent *>(event))
{
core::objectmodel::KeyreleasedEvent *kre = dynamic_cast<core::objectmodel::KeyreleasedEvent *>(event);
onKeyReleasedEvent(kre);
}
//std::cout<<"IHPDriver::handleEvent() ended:"<<std::endl;
}
