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