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; }