void vpSimulator::getExternalCameraPosition(vpHomogeneousMatrix &cMf) { /* SoCamera *camera ; camera = this->externalView->getCamera() ;*/ SoSFVec3f position = externalCamera->position ; // get the rotation SoSFRotation orientation = externalCamera->orientation; SbVec3f axis ; float angle ; orientation.getValue(axis,angle) ; SbRotation rotation(axis,angle) ; // get the translation SbVec3f t ; t = position.getValue() ; SbMatrix matrix ; matrix.setRotate(rotation) ; vpHomogeneousMatrix fMc ; SbMatrix rotX; rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI)); matrix.multLeft (rotX); for(unsigned int i=0;i<4;i++) for(unsigned int j=0;j<4;j++) fMc[j][i]=matrix[(int)i][(int)j]; fMc[0][3] = t[0] ; fMc[1][3] = t[1] ; fMc[2][3] = t[2] ; cMf = fMc.inverse() ; }
/*Funciones callback*/ static void colorCallback(void *data, SoSensor *sensor) { unsigned char *color; SoSFVec3f *background = (SoSFVec3f *)((SoFieldSensor *)sensor)->getAttachedField(); color=(unsigned char *) data; color[0]=(char)(255.0*background->getValue()[0]); color[1]=(char)(255.0*background->getValue()[1]); color[2]=(char)(255.0*background->getValue()[2]); }
void SoXipAnnotation::updateEnumerationPosition( SoSFVec3f& position ) { if( mIsViewInitialized && point.getNum() >= 2 ) { const SbVec3f& p0 = point[0]; const SbVec3f& p1 = point[1]; SbVec3f u = SbVec3f( 1, 0, 0 ); SbVec3f v = p1 - p0; SbVec3f normal = mViewVolume.getPlane(0).getNormal(); double a = angleBetweenVectors( u, v, normal ); bool pos_cos = cos(a) > 0; bool pos_sin = sin(a) > 0; mEnumerationText->justification.setValue( pos_cos ? SoXipText2::LEFT : SoXipText2::RIGHT ); mEnumerationText->vAlignment.setValue( pos_sin ? SoXipText2::TOP : SoXipText2::BOTTOM ); SbVec3f offset = projectScreenOffsetToWorld( SbVec2s( 5, 5 ) ); SbVec3f middle; middle[0] = ( pos_cos ? offset[0] : -offset[0] ) + ( p0[0] + p1[0] ) / 2.; middle[1] = ( !pos_sin ? offset[1] : -offset[1] ) + ( p0[1] + p1[1] ) / 2.; middle[2] = ( p0[2] + p1[2] ) / 2.; position.setValue( middle ); } }
void SoXipAnnotation::updateAnnotationPosition( SoSFVec3f& position ) { if( mIsViewInitialized && point.getNum() == 2 ) { SbVec3f screenPt[2]; mViewVolume.projectToScreen( point[0], screenPt[0] ); mViewVolume.projectToScreen( point[1], screenPt[1] ); if( screenPt[0][0] < screenPt[1][0] ) { mAnnotation->justification.setValue( SoXipText2::LEFT ); position.setValue( point[1] + projectScreenOffsetToWorld( SbVec2s( 5, 0 ) ) ); } else { mAnnotation->justification.setValue( SoXipText2::RIGHT ); position.setValue( point[1] - projectScreenOffsetToWorld( SbVec2s( 5, 0 ) ) ); } mAnnotation->vAlignment.setValue( SoXipText2::TOP ); } }
//////////////////////////////////////////////////////////////////////// // // Called whenever the tracking sensor fires. // // Use: static private // // Uwe Woessner // void InvFullViewer::trackingSensorCB(void *p, SoSensor *) // //////////////////////////////////////////////////////////////////////// { InvFullViewer *v = (InvFullViewer *)p; MouseRecordType record; PolhemusRecord p_record; float P, Y, R; float dP, dY, dR; float dpx, dpy, dpz; // static float angle = 0; static float tdpx = 0; static float tdpy = 0; static float tdpz = 0; static int out_of_range_flag = 0; // static float tdP=0; // static float tdY=0; // static float tdR=0; // SoCamera *camera = InvViewer::viewer->getCamera(); // grab one valid record if (v->trackingDevice < 3) // Logitech on port 1 or 2 { get_record(v->trackingFd, &record); // out of range if ((record.buttons & 0x20) && (!out_of_range_flag)) { v->buttonList[TRACK_PUSH]->select(0); out_of_range_flag = 1; } else if (out_of_range_flag) { v->buttonList[TRACK_PUSH]->select(1); out_of_range_flag = 0; } // move the camera position by the translation amount // in the forward direction. // we will ignore 'y' motion since this is the walk viewer. // first, orient the tracker data to our camera orientation. // first update the position according to current position and tracker position #define TFACT 0.05 // get current transform SoSFVec3f translation; // scale position and create relative position dpx = (record.x - tdpx) * TFACT; dpy = (record.y - tdpy) * TFACT; dpz = (record.z - tdpz) * TFACT; // printf("%f %f, %f\n",dpx,dpy,dpz); // set error boundary if ((fabs(dpx) > 10.0) || (fabs(dpy) > 10.0) || (fabs(dpz) > 10.0)) { cerr << "You moved to fast!\n"; // save values tdpx = record.x; tdpy = record.y; tdpz = record.z; return; // maybe the tracker track is not correct } // save values tdpx = record.x; tdpy = record.y; tdpz = record.z; // printf("r: %f %f %f %f\n",dpx,dpy,dpz,record.xr,record.yr,record.zr,record.ar); translation.setValue(dpx, dpy, dpz); v->camera->position.setValue(v->camera->position.getValue() + translation.getValue()); // now the rotations // // little correction P = (record.pitch + 20.0) * M_PI / 180.0; Y = (record.yaw) * M_PI / 180.0; R = (record.roll) * M_PI / 180.0; #define RFACT 2.0 // scale rotation (better not) dP = P * (RFACT); dY = Y * (RFACT); dR = R * (RFACT / 2.0); float ra = cos(dR / 2) * cos(dP / 2) * cos(dY / 2) + sin(dR / 2) * sin(dP / 2) * sin(dY / 2); float rx = cos(dR / 2) * sin(dP / 2) * cos(dY / 2) + sin(dR / 2) * cos(dP / 2) * sin(dY / 2); float ry = cos(dR / 2) * cos(dP / 2) * sin(dY / 2) + sin(dR / 2) * sin(dP / 2) * cos(dY / 2); float rz = sin(dR / 2) * cos(dP / 2) * cos(dY / 2) + cos(dR / 2) * sin(dP / 2) * sin(dY / 2); v->camera->orientation.setValue(rx, ry, rz, ra); } else // Fastrak is in the house (port 3 or 4) { fastrackGetSingleRecord(v->trackingFd, &p_record); SoSFVec3f translation; // scale position and create relative position dpx = (p_record.x - tdpx) * TFACT; dpy = (p_record.y - tdpy) * TFACT; dpz = (p_record.z - tdpz) * TFACT; // save values tdpx = p_record.x; tdpy = p_record.y; tdpz = p_record.z; // printf("r: %f %f %f %f\n",p_record.q1,p_record.q2,p_record.q3,p_record.w); translation.setValue(dpx, dpy, dpz); v->camera->position.setValue(v->camera->position.getValue() + translation.getValue()); // v->camera->orientation.setValue(p_record.q1,p_record.q2,p_record.q3,(float)(2*facos(p_record.w))); } }
void ComponentHeliostatField::CreateHeliostatZones( std::vector< Point3D > heliostatCenterList, TSeparatorKit* parentNode, TTrackerFactory* heliostatTrackerFactory, Point3D aimingPoint, TSeparatorKit* heliostatComponentNode, int eje ) { SoType separatorType = SoType::fromName( SbName ( "TSeparatorKit" ) ); SoNodeKitListPart* heliostatsNodePartList = static_cast< SoNodeKitListPart* >( parentNode->getPart( "childList", true ) ); if( !heliostatsNodePartList ) return; int nHeliostatCenters = ( int ) heliostatCenterList.size(); if( nHeliostatCenters < 8 ) { for( int nHeliostat = 0; nHeliostat < nHeliostatCenters; nHeliostat++ ) { //TSeparatorKit* heliostatSeparator = new TSeparatorKit; TSeparatorKit* heliostatSeparator = static_cast< TSeparatorKit* > ( separatorType.createInstance() ); heliostatsNodePartList->addChild(heliostatSeparator); QString heliostatName = QString( QLatin1String( "Heliostat%1" ) ).arg( QString::number( nHeliostat ) ); heliostatSeparator->setName( heliostatName.toStdString().c_str() ); SoTransform* nodeTransform = dynamic_cast< SoTransform* >( heliostatSeparator->getPart( "transform", true ) ); nodeTransform->translation.setValue( heliostatCenterList[nHeliostat].x, heliostatCenterList[nHeliostat].y, heliostatCenterList[nHeliostat].z ); SoNodeKitListPart* heliostatPartList = static_cast< SoNodeKitListPart* >( heliostatSeparator->getPart( "childList", true ) ); if( !heliostatPartList ) return; //TSeparatorKit* heliostatTrackerNode= new TSeparatorKit; TSeparatorKit* heliostatTrackerNode = static_cast< TSeparatorKit* > ( separatorType.createInstance() ); heliostatPartList->addChild( heliostatTrackerNode ); heliostatTrackerNode->setName( "HeliostatTrackerNode" ); SoNodeKitListPart* heliostaTrackerNodetPartList = static_cast< SoNodeKitListPart* >( heliostatTrackerNode->getPart( "childList", true ) ); if( !heliostaTrackerNodetPartList ) return; TTracker* tracker = heliostatTrackerFactory->CreateTTracker(); heliostatTrackerNode->setPart( "tracker", tracker ); SoSFVec3f* aimingPointField = dynamic_cast< SoSFVec3f* > ( tracker->getField ( "aimingPoint" ) ); if( aimingPointField ) aimingPointField->setValue( SbVec3f( aimingPoint.x, aimingPoint.y, aimingPoint.z ) ); if( heliostatComponentNode ) heliostaTrackerNodetPartList->addChild( heliostatComponentNode ); } } else { //TSeparatorKit* heliostatSeparator1 = new TSeparatorKit; TSeparatorKit* heliostatSeparator1 = static_cast< TSeparatorKit* > ( separatorType.createInstance() ); heliostatsNodePartList->addChild(heliostatSeparator1); QString heliostatName1 = QString( QLatin1String( "DivisionPor%1_1" ) ).arg( ( eje == 3 )? QString( 'Z' ): QString( 'X' ) ); heliostatSeparator1->setName( heliostatName1.toStdString().c_str() ); //TSeparatorKit* heliostatSeparator2 = new TSeparatorKit; TSeparatorKit* heliostatSeparator2 = static_cast< TSeparatorKit* > ( separatorType.createInstance() ); heliostatsNodePartList->addChild(heliostatSeparator2); QString heliostatName2 = QString( QLatin1String( "DivisionPor%1_2" ) ).arg( ( eje == 3 )? QString( 'Z' ): QString( 'X' ) ); heliostatSeparator2->setName( heliostatName2.toStdString().c_str() ); if( fabs( eje - 3 ) < 0.001 ) { std::sort( heliostatCenterList.begin(), heliostatCenterList.end(), comparePuntosPorZ ); std::vector< Point3D >::iterator it; double zMin = heliostatCenterList[0].z; double zMax = heliostatCenterList[nHeliostatCenters-1].z; double splitPoint = zMin + ( 0.5 * ( zMax - zMin ) ); std::vector< Point3D > hCenterListPart1; double position = 0; while( ( position < heliostatCenterList.size() ) && ( heliostatCenterList[position].z < splitPoint ) ) { hCenterListPart1.push_back( heliostatCenterList[position] ); position++; } std::vector< Point3D > hCenterListPart2; while( position < heliostatCenterList.size() ) { hCenterListPart2.push_back( heliostatCenterList[position] ); position++; } CreateHeliostatZones( hCenterListPart1, heliostatSeparator1, heliostatTrackerFactory, aimingPoint, heliostatComponentNode, 1 ); CreateHeliostatZones( hCenterListPart2, heliostatSeparator2, heliostatTrackerFactory, aimingPoint, heliostatComponentNode, 1 ); } else { std::sort( heliostatCenterList.begin(), heliostatCenterList.end(), comparePuntosPorX ); double xMin = heliostatCenterList[0].x; double xMax = heliostatCenterList[nHeliostatCenters-1].x; double splitPoint = xMin + ( 0.5 * ( xMax - xMin ) ); std::vector< Point3D > hCenterListPart1; double position = 0; while( ( position < heliostatCenterList.size() ) && ( heliostatCenterList[position].x < splitPoint ) ) { hCenterListPart1.push_back( heliostatCenterList[position] ); position++; } std::vector< Point3D > hCenterListPart2; while( position < heliostatCenterList.size() ) { hCenterListPart2.push_back( heliostatCenterList[position] ); position++; } CreateHeliostatZones( hCenterListPart1, heliostatSeparator1, heliostatTrackerFactory, aimingPoint, heliostatComponentNode, 3 ); CreateHeliostatZones( hCenterListPart2, heliostatSeparator2, heliostatTrackerFactory, aimingPoint, heliostatComponentNode, 3 ); } } }