SCM pip_color_distance (SCM cR, SCM cG, SCM cB, SCM cR2, SCM cG2, SCM cB2)
{
double R = scm_to_double (cR);
double G = scm_to_double (cG);
double B = scm_to_double (cB);
double R2 = scm_to_double (cR2);
double G2 = scm_to_double (cG2);
double B2 = scm_to_double (cB2);
R /= 255.0;
G /= 255.0;
B /= 255.0;
R2 /= 255.0;
G2 /= 255.0;
B2 /= 255.0;
R = transform1 (R);
G = transform1 (G);
B = transform1 (B);
R2 = transform1 (R2);
G2 = transform1 (G2);
B2 = transform1 (B2);
double X = R * 0.4124 + G * 0.3576 + B * 0.1805;
double Y = R * 0.2126 + G * 0.7152 + B * 0.0722;
double Z = R * 0.0193 + G * 0.1192 + B * 0.9505;
double X2 = R2 * 0.4124 + G2 * 0.3576 + B2 * 0.1805;
double Y2 = R2 * 0.2126 + G2 * 0.7152 + B2 * 0.0722;
double Z2 = R2 * 0.0193 + G2 * 0.1192 + B2 * 0.9505;
Y /= REFY;
Z /= REFZ;
X /= REFX;
Y2 /= REFY;
Z2 /= REFZ;
X2 /= REFX;
double LL = 116.0 * transform2(Y) - 16.0;
double AA = 500.0 * (transform2(X) - transform2 (Y));
double BB = 200.0 * (transform2(Y) - transform2 (Z));
double LL2 = 116.0 * transform2(Y2) - 16.0;
double AA2 = 500.0 * (transform2(X2) - transform2 (Y2));
double BB2 = 200.0 * (transform2(Y2) - transform2 (Z2));
double dL = LL - LL2;
double dA = AA - AA2;
double dB = BB - BB2;
return scm_from_double (sqrt (dL * dL + dA * dA + dB * dB));
}
/* ARGSUSED */
static XtCallbackProc
stage2_tform_cback(Widget w, xgobidata *xg, XtPointer cbdata)
{
int tfno, ntform_cols, j, k, groupno;
Boolean state;
for (tfno=0; tfno<N2TFORMS; tfno++)
if (w == stage2_tform_cmd[tfno])
break;
ntform_cols = 0;
for (j=0; j<xg->ncols-1; j++) {
XtVaGetValues(var_cbox[j], XtNstate, &state, NULL);
if (state) {
tform_cols[ntform_cols++] = j;
groupno = xg->vgroup_ids[j];
for (k=j+1; k<xg->ncols-1; k++) {
if (xg->vgroup_ids[k] == groupno)
tform_cols[ntform_cols++] = k;
}
}
}
if (ntform_cols > 0) {
if (transform2(xg, tform_cols, ntform_cols, tfno))
tform_response(xg, tform_cols, ntform_cols);
}
}
void
permute_again(xgobidata *xg) {
if (xg->is_touring && (xg->is_princ_comp || xg->is_pp))
;
else {
if (ntform_cols > 0 && tform_cols != NULL) {
if ( transform2(xg, tform_cols, ntform_cols, PERMUTE) )
tform_response(xg, tform_cols, ntform_cols);
}
}
}
int main(){
int t, type, ctr_g, ctr_b, n;
unsigned long int score1, score2;
int *arr1, *arr2;
char *str, *target1, *target2;
arr1 = (int*)malloc(sizeof(int)*MAX_N);
arr2 = (int*)malloc(sizeof(int)*MAX_N);
str = (char*)malloc(sizeof(char)*MAX_N);
target1 = (char*)malloc(sizeof(char)*MAX_N);
target2 = (char*)malloc(sizeof(char)*MAX_N);
scanf("%d",&t);
while(t--){
scanf("%d", &type);
scanf("%s", str);
n = strlen(str);
ctr_b = ctr_g = 0;
analyze_target(str, target1, target2, &ctr_g, &ctr_b, n);
if(type!=2){
// if(type==1){
// printf("> %lu\n", transform2(str, n, arr1, ctr_g, ctr_b));
// }
if(ctr_b==ctr_g){
if(strcmp(target1,str)==0 || strcmp(target2,str)==0){
printf("0\n");
}else{
score1 = transform(target1,str, n, type, arr1, arr2);
score2 = transform(target2,str, n, type, arr1, arr2);
printf("%lu\n", min(score1, score2));
}
}else if(abs(ctr_b-ctr_g)==1){
if(strcmp(target1,str)==0){
printf("0\n");
}else{
printf("%lu\n", transform(target1,str, n, type, arr1, arr2));
}
}else{
printf("-1\n");
}
}else{
if(abs(ctr_b-ctr_g)>1)
printf("-1\n");
else{
printf("%lu\n", transform2(str, n, arr1, ctr_g, ctr_b));
}
}
}
return 0;
}
static void test_preserves_2d_axis_alignment(skiatest::Reporter* reporter) {
SkMatrix44 transform(SkMatrix44::kUninitialized_Constructor);
SkMatrix44 transform2(SkMatrix44::kUninitialized_Constructor);
static const struct TestCase {
SkMScalar a; // row 1, column 1
SkMScalar b; // row 1, column 2
SkMScalar c; // row 2, column 1
SkMScalar d; // row 2, column 2
bool expected;
} test_cases[] = {
{ 3.f, 0.f,
0.f, 4.f, true
}, // basic case
{ 0.f, 4.f,
3.f, 0.f, true
}, // rotate by 90
{ 0.f, 0.f,
0.f, 4.f, true
}, // degenerate x
{ 3.f, 0.f,
0.f, 0.f, true
}, // degenerate y
{ 0.f, 0.f,
3.f, 0.f, true
}, // degenerate x + rotate by 90
{ 0.f, 4.f,
0.f, 0.f, true
}, // degenerate y + rotate by 90
{ 3.f, 4.f,
0.f, 0.f, false
},
{ 0.f, 0.f,
3.f, 4.f, false
},
{ 0.f, 3.f,
0.f, 4.f, false
},
{ 3.f, 0.f,
4.f, 0.f, false
},
{ 3.f, 4.f,
5.f, 0.f, false
},
{ 3.f, 4.f,
0.f, 5.f, false
},
{ 3.f, 0.f,
4.f, 5.f, false
},
{ 0.f, 3.f,
4.f, 5.f, false
},
{ 2.f, 3.f,
4.f, 5.f, false
},
};
for (size_t i = 0; i < sizeof(test_cases)/sizeof(TestCase); ++i) {
const TestCase& value = test_cases[i];
transform.setIdentity();
transform.set(0, 0, value.a);
transform.set(0, 1, value.b);
transform.set(1, 0, value.c);
transform.set(1, 1, value.d);
test(value.expected, reporter, transform);
}
// Try the same test cases again, but this time make sure that other matrix
// elements (except perspective) have entries, to test that they are ignored.
for (size_t i = 0; i < sizeof(test_cases)/sizeof(TestCase); ++i) {
const TestCase& value = test_cases[i];
transform.setIdentity();
transform.set(0, 0, value.a);
transform.set(0, 1, value.b);
transform.set(1, 0, value.c);
transform.set(1, 1, value.d);
transform.set(0, 2, 1.f);
transform.set(0, 3, 2.f);
transform.set(1, 2, 3.f);
transform.set(1, 3, 4.f);
transform.set(2, 0, 5.f);
transform.set(2, 1, 6.f);
transform.set(2, 2, 7.f);
transform.set(2, 3, 8.f);
test(value.expected, reporter, transform);
}
// Try the same test cases again, but this time add perspective which is
// always assumed to not-preserve axis alignment.
for (size_t i = 0; i < sizeof(test_cases)/sizeof(TestCase); ++i) {
const TestCase& value = test_cases[i];
transform.setIdentity();
transform.set(0, 0, value.a);
transform.set(0, 1, value.b);
transform.set(1, 0, value.c);
transform.set(1, 1, value.d);
transform.set(0, 2, 1.f);
transform.set(0, 3, 2.f);
transform.set(1, 2, 3.f);
transform.set(1, 3, 4.f);
transform.set(2, 0, 5.f);
transform.set(2, 1, 6.f);
transform.set(2, 2, 7.f);
transform.set(2, 3, 8.f);
transform.set(3, 0, 9.f);
transform.set(3, 1, 10.f);
transform.set(3, 2, 11.f);
transform.set(3, 3, 12.f);
test(false, reporter, transform);
}
// Try a few more practical situations to check precision
// Reuse TestCase (a, b, c, d) as (x, y, z, degrees) axis to rotate about.
TestCase rotation_tests[] = {
{ 0.0, 0.0, 1.0, 90.0, true },
{ 0.0, 0.0, 1.0, 180.0, true },
{ 0.0, 0.0, 1.0, 270.0, true },
{ 0.0, 1.0, 0.0, 90.0, true },
{ 1.0, 0.0, 0.0, 90.0, true },
{ 0.0, 0.0, 1.0, 45.0, false },
// In 3d these next two are non-preserving, but we're testing in 2d after
// orthographic projection, where they are.
{ 0.0, 1.0, 0.0, 45.0, true },
{ 1.0, 0.0, 0.0, 45.0, true },
};
for (size_t i = 0; i < sizeof(rotation_tests)/sizeof(TestCase); ++i) {
const TestCase& value = rotation_tests[i];
transform.setRotateDegreesAbout(value.a, value.b, value.c, value.d);
test(value.expected, reporter, transform);
}
static const struct DoubleRotationCase {
SkMScalar x1;
SkMScalar y1;
SkMScalar z1;
SkMScalar degrees1;
SkMScalar x2;
SkMScalar y2;
SkMScalar z2;
SkMScalar degrees2;
bool expected;
} double_rotation_tests[] = {
{ 0.0, 0.0, 1.0, 90.0, 0.0, 1.0, 0.0, 90.0, true },
{ 0.0, 0.0, 1.0, 90.0, 1.0, 0.0, 0.0, 90.0, true },
{ 0.0, 1.0, 0.0, 90.0, 0.0, 0.0, 1.0, 90.0, true },
};
for (size_t i = 0; i < sizeof(double_rotation_tests)/sizeof(DoubleRotationCase); ++i) {
const DoubleRotationCase& value = double_rotation_tests[i];
transform.setRotateDegreesAbout(value.x1, value.y1, value.z1, value.degrees1);
transform2.setRotateDegreesAbout(value.x2, value.y2, value.z2, value.degrees2);
transform.postConcat(transform2);
test(value.expected, reporter, transform);
}
// Perspective cases.
transform.setIdentity();
transform.setDouble(3, 2, -0.1); // Perspective depth 10
transform2.setRotateDegreesAbout(0.0, 1.0, 0.0, 45.0);
transform.preConcat(transform2);
test(false, reporter, transform);
transform.setIdentity();
transform.setDouble(3, 2, -0.1); // Perspective depth 10
transform2.setRotateDegreesAbout(0.0, 0.0, 1.0, 90.0);
transform.preConcat(transform2);
test(true, reporter, transform);
}
void *thread_entry(void *arg)
{
struct fp_img_dev *idev = (struct fp_img_dev *)arg;
//struct libusb_device_handle *udev = idev->udev;
struct etes603_dev *dev = (struct etes603_dev *)idev->priv;
uint8_t *braw = malloc(256000);
uint8_t *brawp = braw;
uint8_t *bframe = malloc(256000);
uint8_t *bimg = malloc(image_width*image_height*4*2);
XExposeEvent event = { Expose, 0, 1, dis, draw_win, 0, 0, image_width, image_height, 0 };
printf("Capturing thread started...\n");
//#define MODE_FRAME
//#define MODE_IMAGE
//#define MODE_LOGGING
#define MODE_IMAGE_FULL
frame_prepare_capture(dev);
if (frame_capture(dev, bframe)) {
printf("frame_capture failed\n");
return (void *)1;
}
#ifdef MODE_IMAGE_FULL
// if (fp_capture(dev, bframe, 64000))
// return (void *)1;
frame_prepare_capture(dev);
#endif
while (stop == 0) {
#ifdef MODE_FRAME
frame_capture(dev, bframe);
if (process_frame_empty(bframe, FRAME_WIDTH * 2, 1))
continue;
do {
brawp = process_frame(brawp, bframe/*, FRAME_WIDTH*/);
//memcpy(brawp, bframe, FRAME_WIDTH * 2);
//brawp += 384 /*FRAME_WIDTH * 2*/;
if (brawp + 384 >= braw + 256000)
break;
frame_capture(dev, bframe);
/* Realtime... */
transform(braw, 96000, (uint32_t *)bimg, image_width * image_height);
if (image != NULL) {
memcpy(image->data, bimg, image_width * image_height * 4);
}
XSendEvent(dis, draw_win /* mainwin*/, False, 0 /*ExposureMask*/, (XEvent *)&event);
XFlush(dis);
if (stop)
goto leave_loop;
} while (!process_frame_empty(bframe, FRAME_WIDTH * 2, 1));
transform(braw, 96000, (uint32_t *)bimg, image_width * image_height);
#endif
#ifdef MODE_IMAGE
if (fp_capture(dev, bframe, 256000))
stop = 1;
braw = bframe;
memset(bimg, 0, 64000*2*4);
transform(braw, 96000, (uint32_t *)bimg, image_width * image_height);
#endif
#ifdef MODE_IMAGE_FULL
frame_capture(dev, bframe);
if (process_frame_empty(bframe, FRAME_WIDTH * 2, 1))
continue;
//fp_capture(dev, bframe);
if (fp_capture(dev, bframe, 64000))
//if (sync_read_buffer_full(udev, bframe))
stop = 1;
memcpy(braw, bframe, 64000);
if (fp_capture(dev, bframe, 64000))
//if (sync_read_buffer_full(udev, bframe))
stop = 1;
memcpy(braw+64000, bframe, 64000);
memset(bimg, 0, 64000*2*4);
transform2(braw, 128000, (uint32_t *)bimg);
frame_prepare_capture(dev);
#endif
#ifdef MODE_LOGGING
static int index = 0;
char filename[255];
frame_capture(dev, bframe);
if (process_frame_empty(bframe, FRAME_WIDTH * 2, 1))
continue;
sprintf (filename, "/tmp/scan%03d", index++);
FILE *f = fopen(filename, "w");
if (!f) perror("fopen");
do {
brawp = process_frame(brawp, bframe);
frame_capture(dev, bframe);
/* writing file */
if (f)
fwrite (bframe, 1, 384, f);
if (stop)
goto leave_loop;
} while (!process_frame_empty(bframe, FRAME_WIDTH * 2, 1)); /* Saving all frames to files */
transform(braw, 96000, (uint32_t *)bimg, image_width * image_height);
if (f) {
fclose(f);
f = NULL;
}
#endif
if (image != NULL) {
memcpy(image->data, bimg, image_width * image_height * 4);
}
memset(braw, 0, 256000);
brawp = braw;
XSendEvent(dis, draw_win /* mainwin*/, False, 0 /*ExposureMask*/, (XEvent *)&event);
XFlush(dis);
} /* stop == 0 */
#if defined(MODE_LOGGING) || defined(MODE_FRAME)
leave_loop:
#endif
printf("Capturing thread leaving...\n");
return NULL;
}
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);
}
}
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;
}
