ScreenProtectorArea::ScreenProtectorArea(const std::vector<Point>& sVertices)
:numVertices(sVertices.size()),
vertices(new Point[numVertices]),edges(new Vector[numVertices]),edgeLengths(new Scalar[numVertices])
{
/* Copy the polygon vertices: */
for(unsigned int i=0U;i<numVertices;++i)
vertices[i]=sVertices[i];
/* Calculate the polygon's centroid and normal vector by adding cross products of each pair of adjacent edges: */
Point::AffineCombiner ac;
Vector normal=Vector::zero;
Point* p2=&vertices[numVertices-1U];
Vector d1=*p2-vertices[numVertices-2U];
for(unsigned int i=0U;i<numVertices;++i)
{
/* Calculate the cross product of the vertex' two incident edges: */
Point* p3=&vertices[i];
ac.addPoint(*p3);
Vector d2=*p3-*p2;
normal+=d1^d2;
/* Go to the next vertex: */
p2=p3;
d1=d2;
}
normal.normalize();
plane=Plane(normal,ac.getPoint());
/* Project all vertices into the plane: */
for(unsigned int i=0U;i<numVertices;++i)
vertices[i]=plane.project(vertices[i]);
/* Finish initialization: */
init();
}
void BruntonPrimitive::buildBrunton(void)
{
/* Create the root node: */
SceneGraph::TransformNode* rootT=new SceneGraph::TransformNode;
root=rootT;
getSceneGraphRoot().children.appendValue(root);
getSceneGraphRoot().update();
/* Calculate the plane primitive's centroid: */
Point::AffineCombiner cc;
for(int i=0;i<4;++i)
cc.addPoint(getPoint(i));
Point centroid=cc.getPoint();
Scalar bruntonScale=Math::mid(Geometry::dist(getPoint(2),getPoint(0)),Geometry::dist(getPoint(3),getPoint(1)));
/* Calculate the plane primitive's dip angle and strike vector: */
Vector normal=getPlane().getNormal();
if(normal[2]<Scalar(0))
normal=-normal;
normal.normalize();
Vector strike=normal;
Scalar dipAngle=Math::acos(strike[2]/Geometry::mag(strike));
strike[2]=Scalar(0);
strike.normalize();
Scalar strikeAngle=Math::atan2(-strike[0],strike[1]);
/* Set the root node's transformation: */
rootT->translation.setValue(centroid-Point::origin);
/* Create the dip and strike indicator: */
SceneGraph::ShapeNode* s1=new SceneGraph::ShapeNode;
rootT->children.appendValue(s1);
{
SceneGraph::IndexedLineSetNode* ils=new SceneGraph::IndexedLineSetNode;
s1->geometry.setValue(ils);
SceneGraph::ColorNode* color=new SceneGraph::ColorNode;
ils->color.setValue(color);
color->color.appendValue(SceneGraph::Color(0.0f,0.5f,1.0f));
color->color.appendValue(SceneGraph::Color(0.0f,1.0f,0.5f));
color->update();
SceneGraph::CoordinateNode* coord=new SceneGraph::CoordinateNode;
ils->coord.setValue(coord);
coord->point.appendValue(Point::origin);
coord->point.appendValue(Point::origin+normal*bruntonScale);
coord->point.appendValue(Point::origin+strike*bruntonScale);
coord->update();
ils->coordIndex.appendValue(0);
ils->coordIndex.appendValue(1);
ils->coordIndex.appendValue(-1);
ils->coordIndex.appendValue(0);
ils->coordIndex.appendValue(2);
ils->colorPerVertex.setValue(false);
ils->lineWidth.setValue(3.0f);
ils->update();
}
s1->update();
SceneGraph::ShapeNode* s2=new SceneGraph::ShapeNode;
rootT->children.appendValue(s2);
{
SceneGraph::IndexedLineSetNode* ils=new SceneGraph::IndexedLineSetNode;
s2->geometry.setValue(ils);
SceneGraph::ColorNode* color=new SceneGraph::ColorNode;
ils->color.setValue(color);
color->color.appendValue(SceneGraph::Color(0.0f,0.5f,1.0f));
color->color.appendValue(SceneGraph::Color(0.0f,0.5f,1.0f));
color->color.appendValue(SceneGraph::Color(0.0f,1.0f,0.5f));
color->color.appendValue(SceneGraph::Color(0.0f,1.0f,0.5f));
color->update();
SceneGraph::CoordinateNode* coord=new SceneGraph::CoordinateNode;
ils->coord.setValue(coord);
coord->point.appendValue(Point::origin);
coord->point.appendValue(Point(0,0,bruntonScale));
coord->point.appendValue(Point(0,bruntonScale,0));
ils->coordIndex.appendValue(0);
ils->coordIndex.appendValue(1);
ils->coordIndex.appendValue(-1);
for(Scalar a=Scalar(0);a<dipAngle;a+=Math::rad(Scalar(10)))
{
ils->coordIndex.appendValue(coord->point.getNumValues());
coord->point.appendValue(Point::origin+(Vector(0,0,1)*Math::cos(a)+strike*Math::sin(a))*(bruntonScale*Scalar(0.9)));
}
ils->coordIndex.appendValue(coord->point.getNumValues());
coord->point.appendValue(Point::origin+(Vector(0,0,1)*Math::cos(dipAngle)+strike*Math::sin(dipAngle))*(bruntonScale*Scalar(0.9)));
ils->coordIndex.appendValue(-1);
ils->coordIndex.appendValue(0);
ils->coordIndex.appendValue(2);
ils->coordIndex.appendValue(-1);
Scalar aInc=Math::rad(Scalar(10));
if(strikeAngle<Scalar(0))
aInc=-aInc;
for(Scalar a=Scalar(0);Math::abs(a)<Math::abs(strikeAngle);a+=aInc)
{
ils->coordIndex.appendValue(coord->point.getNumValues());
coord->point.appendValue(Point::origin+Vector(-Math::sin(a),Math::cos(a),0)*(bruntonScale*Scalar(0.9)));
}
ils->coordIndex.appendValue(coord->point.getNumValues());
coord->point.appendValue(Point::origin+Vector(-Math::sin(strikeAngle),Math::cos(strikeAngle),0)*(bruntonScale*Scalar(0.9)));
coord->update();
ils->colorPerVertex.setValue(false);
ils->lineWidth.setValue(1.0f);
ils->update();
}
s2->update();
SceneGraph::TransformNode* t2=new SceneGraph::TransformNode;
rootT->children.appendValue(t2);
t2->translation.setValue((Vector(0,0,1)*Math::cos(Math::div2(dipAngle))+strike*Math::sin(Math::div2(dipAngle)))*bruntonScale);
{
SceneGraph::BillboardNode* bb=new SceneGraph::BillboardNode;
t2->children.appendValue(bb);
bb->axisOfRotation.setValue(Vector::zero);
{
SceneGraph::ShapeNode* s=new SceneGraph::ShapeNode;
bb->children.appendValue(s);
SceneGraph::TextNode* text=new SceneGraph::TextNode;
s->geometry.setValue(text);
SceneGraph::FontStyleNode* fs=new SceneGraph::FontStyleNode;
text->fontStyle.setValue(fs);
fs->size.setValue(bruntonScale*Scalar(0.25));
fs->justify.appendValue("MIDDLE");
fs->justify.appendValue("MIDDLE");
fs->update();
char buffer[40];
snprintf(buffer,sizeof(buffer),"%.2f",Math::deg(dipAngle));
text->string.appendValue(buffer);
text->update();
s->update();
}
bb->update();
}
t2->update();
SceneGraph::TransformNode* t3=new SceneGraph::TransformNode;
rootT->children.appendValue(t3);
t3->translation.setValue(Vector(-Math::sin(Math::div2(strikeAngle)),Math::cos(Math::div2(strikeAngle)),0)*bruntonScale);
{
SceneGraph::BillboardNode* bb=new SceneGraph::BillboardNode;
t3->children.appendValue(bb);
bb->axisOfRotation.setValue(Vector::zero);
{
SceneGraph::ShapeNode* s=new SceneGraph::ShapeNode;
bb->children.appendValue(s);
SceneGraph::TextNode* text=new SceneGraph::TextNode;
s->geometry.setValue(text);
SceneGraph::FontStyleNode* fs=new SceneGraph::FontStyleNode;
text->fontStyle.setValue(fs);
fs->size.setValue(bruntonScale*Scalar(0.25));
fs->justify.appendValue("MIDDLE");
fs->justify.appendValue("MIDDLE");
fs->update();
char buffer[40];
Scalar sa=-Math::deg(strikeAngle);
if(sa<Scalar(0))
sa+=360.0;
snprintf(buffer,sizeof(buffer),"%.2f",sa);
text->string.appendValue(buffer);
text->update();
s->update();
}
bb->update();
}
t3->update();
rootT->update();
}
void ScreenCalibrator::readOptitrackSampleFile(const char* fileName,bool flipZ)
{
/* Open the CSV input file: */
IO::TokenSource tok(Vrui::openFile(fileName));
tok.setPunctuation(",\n");
tok.setQuotes("\"");
tok.skipWs();
/* Read all point records from the file: */
double lastTimeStamp=-Math::Constants<double>::min;
Point::AffineCombiner pac;
unsigned int numPoints=0;
unsigned int line=1;
while(!tok.eof())
{
/* Read a point record: */
/* Read the marker index: */
int markerIndex=atoi(tok.readNextToken());
if(strcmp(tok.readNextToken(),",")!=0)
Misc::throwStdErr("readOptitrackSampleFile: missing comma in line %u",line);
/* Read the sample timestamp: */
double timeStamp=atof(tok.readNextToken());
/* Read the point position: */
Point p;
for(int i=0;i<3;++i)
{
if(strcmp(tok.readNextToken(),",")!=0)
Misc::throwStdErr("readOptitrackSampleFile: missing comma in line %u",line);
p[i]=Scalar(atof(tok.readNextToken()));
}
if(flipZ)
{
/* Invert the z component to flip to a right-handed coordinate system: */
p[2]=-p[2];
}
if(strcmp(tok.readNextToken(),"\n")!=0)
Misc::throwStdErr("readOptitrackSampleFile: overlong point record in line %u",line);
/* Check if the point record is valid: */
if(markerIndex==1)
{
/* Check if this record started a new sampling sequence: */
if(timeStamp>=lastTimeStamp+5.0)
{
/* Get the current average point position and reset the accumulator: */
if(numPoints>0)
{
trackingPoints.push_back(pac.getPoint());
pac.reset();
numPoints=0;
}
}
/* Add the point to the current accumulator: */
pac.addPoint(p);
++numPoints;
lastTimeStamp=timeStamp;
}
++line;
}
/* Get the last average point position: */
if(numPoints>0)
trackingPoints.push_back(pac.getPoint());
/* Cull duplicate points from the point list: */
size_t numDupes=cullDuplicates(trackingPoints,Scalar(0.05));
if(numDupes>0)
std::cout<<"ScreenCalibrator::readOptitrackSampleFile: "<<numDupes<<" duplicate points culled from input file"<<std::endl;
}
ScreenCalibrator::ScreenCalibrator(int& argc,char**& argv,char**& appDefaults)
:Vrui::Application(argc,argv,appDefaults),
trackingPointsMover(0)
{
/* Create and register the point query tool class: */
PointQueryToolFactory* pointQueryToolFactory=new PointQueryToolFactory("PointQueryTool","Point Query",0,*Vrui::getToolManager());
pointQueryToolFactory->setNumButtons(1);
pointQueryToolFactory->setButtonFunction(0,"Query Point");
Vrui::getToolManager()->addClass(pointQueryToolFactory,Vrui::ToolManager::defaultToolFactoryDestructor);
/* Parse the command line: */
const char* optitrackFileName=0;
bool optitrackFlipZ=false;
const char* totalstationFileName=0;
int screenPixelSize[2]={-1,-1};
int screenSquareSize=200;
double unitScale=1.0;
for(int i=1;i<argc;++i)
{
if(argv[i][0]=='-')
{
if(strcasecmp(argv[i]+1,"screenSize")==0)
{
for(int j=0;j<2;++j)
{
++i;
screenPixelSize[j]=atoi(argv[i]);
}
}
else if(strcasecmp(argv[i]+1,"squareSize")==0)
{
++i;
screenSquareSize=atoi(argv[i]);
}
else if(strcasecmp(argv[i]+1,"metersToInches")==0)
unitScale=1000.0/25.4;
else if(strcasecmp(argv[i]+1,"unitScale")==0)
{
++i;
unitScale=atof(argv[i]);
}
else if(strcasecmp(argv[i]+1,"flipZ")==0)
optitrackFlipZ=true;
else
{
}
}
else if(totalstationFileName==0)
totalstationFileName=argv[i];
else if(optitrackFileName==0)
optitrackFileName=argv[i];
else
{
}
}
/* Read the Optitrack sample file: */
if(optitrackFileName!=0)
{
readOptitrackSampleFile(optitrackFileName,optitrackFlipZ);
std::cout<<"Read "<<trackingPoints.size()<<" ball points from Optitrack sample file"<<std::endl;
}
/* Read relevant point classes from the Totalstation survey file: */
if(totalstationFileName!=0)
{
screenPoints=readTotalstationSurveyFile(totalstationFileName,"SCREEN");
floorPoints=readTotalstationSurveyFile(totalstationFileName,"FLOOR");
ballPoints=readTotalstationSurveyFile(totalstationFileName,"BALLS");
std::cout<<"Read "<<ballPoints.size()<<" ball points from TotalStation survey file"<<std::endl;
}
/*********************************************************************
Establish a normalized coordinate system with the floor at the z=0
plane, the screen in a plane about orthogonal to the y axis, and the
screen center above the origin.
*********************************************************************/
/* Fit a plane to the floor points: */
Geometry::PCACalculator<3> floorPca;
for(PointList::const_iterator fpIt=floorPoints.begin();fpIt!=floorPoints.end();++fpIt)
floorPca.accumulatePoint(*fpIt);
Point floorCentroid=floorPca.calcCentroid();
floorPca.calcCovariance();
double floorEv[3];
floorPca.calcEigenvalues(floorEv);
Geometry::PCACalculator<3>::Vector floorNormal=floorPca.calcEigenvector(floorEv[2]);
/* Fit a plane to the screen points: */
Geometry::PCACalculator<3> screenPca;
for(PointList::const_iterator spIt=screenPoints.begin();spIt!=screenPoints.end();++spIt)
screenPca.accumulatePoint(*spIt);
Point screenCentroid=screenPca.calcCentroid();
screenPca.calcCovariance();
double screenEv[3];
screenPca.calcEigenvalues(screenEv);
Geometry::PCACalculator<3>::Vector screenNormal=screenPca.calcEigenvector(screenEv[2]);
/* Flip the floor normal such that it points towards the screen points: */
if((screenCentroid-floorCentroid)*floorNormal<Scalar(0))
floorNormal=-floorNormal;
/* Flip the screen normal such that it points away from the ball points: */
Point::AffineCombiner ballC;
for(PointList::const_iterator bpIt=ballPoints.begin();bpIt!=ballPoints.end();++bpIt)
ballC.addPoint(*bpIt);
if((ballC.getPoint()-screenCentroid)*screenNormal>Scalar(0))
screenNormal=-screenNormal;
/* Project the screen centroid onto the floor plane to get the coordinate system origin: */
Point origin=screenCentroid-floorNormal*(((screenCentroid-floorCentroid)*floorNormal)/Geometry::sqr(floorNormal));
/* Orthonormalize the screen normal against the floor normal: */
Vector y=screenNormal-floorNormal*((screenNormal*floorNormal)/Geometry::sqr(floorNormal));
Vector x=Geometry::cross(y,floorNormal);
#if 0
/* Calculate a rotation to align the floor normal with +z and the (horizontal) screen normal with +y: */
ONTransform::Rotation rot=ONTransform::Rotation::fromBaseVectors(x,y);
#endif
/*********************************************************************
Calculate a transformation to move the Totalstation survey points into
the normalized coordinate system:
*********************************************************************/
ONTransform transform(origin-Point::origin,ONTransform::Rotation::fromBaseVectors(x,y));
transform.doInvert();
/* Transform all survey points: */
for(PointList::iterator spIt=screenPoints.begin();spIt!=screenPoints.end();++spIt)
*spIt=transform.transform(*spIt);
for(PointList::iterator fpIt=floorPoints.begin();fpIt!=floorPoints.end();++fpIt)
*fpIt=transform.transform(*fpIt);
for(PointList::iterator bpIt=ballPoints.begin();bpIt!=ballPoints.end();++bpIt)
*bpIt=transform.transform(*bpIt);
if(screenPixelSize[0]>0&&screenPixelSize[1]>0&&screenSquareSize>0)
{
/*********************************************************************
Calculate the optimal projective transformation and screen
transformation (orthonormal transformation plus non-uniform scaling in
x and y) from theoretical screen points to surveyed screen points:
*********************************************************************/
/* Create a list of theoretical screen points: */
PointList screen;
int screenPixelOffset[2];
for(int i=0;i<2;++i)
screenPixelOffset[i]=((screenPixelSize[i]-1)%screenSquareSize)/2;
for(int y=screenPixelOffset[1];y<screenPixelSize[1];y+=screenSquareSize)
for(int x=screenPixelOffset[0];x<screenPixelSize[0];x+=screenSquareSize)
screen.push_back(Point((Scalar(x)+Scalar(0.5))/Scalar(screenPixelSize[0]),Scalar(1)-(Scalar(y)+Scalar(0.5))/Scalar(screenPixelSize[1]),0));
if(screen.size()!=screenPoints.size())
Misc::throwStdErr("Wrong number of screen points, got %d instead of %d",int(screenPoints.size()),int(screen.size()));
/* Find the best-fitting projective transformation for the measured screen points: */
PTransformFitter ptf(screen.size(),&screen[0],&screenPoints[0]);
PTransformFitter::Scalar screenResult2=LevenbergMarquardtMinimizer<PTransformFitter>::minimize(ptf);
std::cout<<"Projective transformation fitting final distance: "<<screenResult2<<std::endl;
pScreenTransform=ptf.getTransform();
/* Print the screen transformation matrix: */
std::cout<<"Projective transformation matrix:"<<std::endl;
std::cout<<std::setprecision(6)<<pScreenTransform<<std::endl;
/* Find the best-fitting screen transformation for the measured screen points: */
ScreenTransformFitter stf(screen.size(),&screen[0],&screenPoints[0]);
ScreenTransformFitter::Scalar screenResult1=LevenbergMarquardtMinimizer<ScreenTransformFitter>::minimize(stf);
std::cout<<"Screen transformation fitting final distance: "<<screenResult1<<std::endl;
screenTransform=stf.getTransform();
screenSize[0]=stf.getSize(0);
screenSize[1]=stf.getSize(1);
std::cout<<"Optimal screen size: "<<screenSize[0]<<", "<<screenSize[1]<<std::endl;
std::cout<<"Optimal screen origin: "<<screenTransform.getOrigin()<<std::endl;
std::cout<<"Optimal horizontal screen axis: "<<screenTransform.getDirection(0)<<std::endl;
std::cout<<"Optimal vertical screen axis: "<<screenTransform.getDirection(1)<<std::endl;
/*********************************************************************
Calculate a homography matrix from the optimal screen transformation
to the optimal projective transformation to correct screen
misalignments:
*********************************************************************/
Point sCorners[4];
Point pCorners[4];
for(int i=0;i<4;++i)
{
sCorners[i][0]=i&0x1?screenSize[0]*unitScale:0.0;
sCorners[i][1]=i&0x2?screenSize[1]*unitScale:0.0;
sCorners[i][2]=0.0;
pCorners[i][0]=i&0x1?1.0:0.0;
pCorners[i][1]=i&0x2?1.0:0.0;
pCorners[i][2]=0.0;
pCorners[i]=screenTransform.inverseTransform(pScreenTransform.transform(pCorners[i]));
pCorners[i][0]*=unitScale;
pCorners[i][1]*=unitScale;
}
Geometry::ProjectiveTransformation<double,2> sHom=calcHomography(sCorners);
Geometry::ProjectiveTransformation<double,2> pHom=calcHomography(pCorners);
Geometry::ProjectiveTransformation<double,2> hom=pHom;
hom.leftMultiply(Geometry::invert(sHom));
for(int i=0;i<3;++i)
for(int j=0;j<3;++j)
hom.getMatrix()(i,j)/=hom.getMatrix()(2,2);
#if 0
std::cout<<"Homography matrix for projective transform: "<<pHom<<std::endl;
std::cout<<"Homography matrix for screen transform: "<<sHom<<std::endl;
std::cout<<"Screen correction homography matrix: "<<hom<<std::endl;
#endif
#if 0
/* Do some experiments: */
Geometry::ProjectiveTransformation<double,3> hom3=Geometry::ProjectiveTransformation<double,3>::identity;
for(int i=0;i<3;++i)
for(int j=0;j<3;++j)
hom3.getMatrix()(i<2?i:3,j<2?j:3)=hom.getMatrix()(i,j);
hom3.getMatrix()(2,0)=hom3.getMatrix()(3,0);
hom3.getMatrix()(2,1)=hom3.getMatrix()(3,1);
std::cout<<hom3<<std::endl;
std::cout<<Geometry::invert(hom3)<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0,-1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0,-1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0, 1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0, 1.0,-1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0,-1.0, 1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0,-1.0, 1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>(-1.0, 1.0, 1.0,1.0)).toPoint()<<std::endl;
std::cout<<hom3.transform(Geometry::HVector<double,3>( 1.0, 1.0, 1.0,1.0)).toPoint()<<std::endl;
#endif
/* Print a configuration file section for the screen: */
std::cout<<std::endl<<"Configuration settings for screen:"<<std::endl;
std::cout<<"origin "<<screenTransform.getTranslation()*unitScale<<std::endl;
std::cout<<"horizontalAxis "<<screenTransform.getDirection(0)<<std::endl;
std::cout<<"width "<<screenSize[0]*unitScale<<std::endl;
std::cout<<"verticalAxis "<<screenTransform.getDirection(1)<<std::endl;
std::cout<<"height "<<screenSize[1]*unitScale<<std::endl;
std::cout<<"offAxis true"<<std::endl;
std::cout<<"homography ( ";
for(int j=0;j<3;++j)
{
if(j>0)
std::cout<<", \\"<<std::endl<<" ";
std::cout<<"( ";
for(int i=0;i<3;++i)
{
if(i>0)
std::cout<<", ";
std::cout<<pHom.getMatrix()(i,j);
}
std::cout<<" )";
}
std::cout<<" )"<<std::endl;
std::cout<<std::endl;
}
if(optitrackFileName!=0&&totalstationFileName!=0)
{
/*********************************************************************
Calculate the optimal orthonormal transformation from tracking system
coordinates to the normalized coordinate system by aligning ball
positions observed by the tracking system with ball positions measured
using the total station:
*********************************************************************/
/* Find an orthonormal transformation to align the tracking points with the ball points: */
size_t numPoints=trackingPoints.size();
if(numPoints>ballPoints.size())
numPoints=ballPoints.size();
/* Calculate the centroid of the tracking points: */
Point::AffineCombiner tpCc;
for(size_t i=0;i<numPoints;++i)
tpCc.addPoint(trackingPoints[i]);
Vector tpTranslation=tpCc.getPoint()-Point::origin;
for(size_t i=0;i<numPoints;++i)
trackingPoints[i]-=tpTranslation;
ONTransformFitter ontf(numPoints,&trackingPoints[0],&ballPoints[0]);
//ontf.setTransform(ONTransformFitter::Transform::rotate(ONTransformFitter::Transform::Rotation::rotateX(Math::rad(Scalar(90)))));
ONTransformFitter::Scalar result=LevenbergMarquardtMinimizer<ONTransformFitter>::minimize(ontf);
ONTransform tsCal=ontf.getTransform();
tsCal*=ONTransform::translate(-tpTranslation);
std::cout<<"Final distance: "<<result<<std::endl;
std::cout<<"Tracking system calibration transformation: "<<tsCal<<std::endl;
std::cout<<"Configuration settings for tracking calibrator: "<<std::endl;
std::cout<<"transformation translate "<<tsCal.getTranslation()*unitScale<<" \\"<<std::endl;
std::cout<<" * scale "<<unitScale<<" \\"<<std::endl;
std::cout<<" * rotate "<<tsCal.getRotation().getAxis()<<", "<<Math::deg(tsCal.getRotation().getAngle())<<std::endl;
/* Transform the tracking points with the result transformation: */
for(PointList::iterator tpIt=trackingPoints.begin();tpIt!=trackingPoints.end();++tpIt)
*tpIt=tsCal.transform(*tpIt+tpTranslation);
}
/* Initialize the navigation transformation: */
Geometry::Box<Scalar,3> bbox=Geometry::Box<Scalar,3>::empty;
for(PointList::const_iterator tpIt=trackingPoints.begin();tpIt!=trackingPoints.end();++tpIt)
bbox.addPoint(*tpIt);
for(PointList::const_iterator spIt=screenPoints.begin();spIt!=screenPoints.end();++spIt)
bbox.addPoint(*spIt);
for(PointList::const_iterator fpIt=floorPoints.begin();fpIt!=floorPoints.end();++fpIt)
bbox.addPoint(*fpIt);
for(PointList::const_iterator bpIt=ballPoints.begin();bpIt!=ballPoints.end();++bpIt)
bbox.addPoint(*bpIt);
Vrui::setNavigationTransformation(Geometry::mid(bbox.min,bbox.max),Geometry::dist(bbox.min,bbox.max));
/* Create a virtual input device to move the tracking points interactively: */
trackingPointsMover=Vrui::addVirtualInputDevice("TrackingPointsMover",0,0);
// Vrui::getInputGraphManager()->setNavigational(trackingPointsMover,true);
Vrui::NavTrackerState scaledDeviceT=Vrui::getInverseNavigationTransformation();
scaledDeviceT*=trackingPointsMover->getTransformation();
trackingPointsTransform=Vrui::TrackerState(scaledDeviceT.getTranslation(),scaledDeviceT.getRotation());
trackingPointsTransform.doInvert();
}
void MultiDeviceNavigationTool::frame(void)
{
/* Do nothing if the tool is inactive: */
if(isActive())
{
/* Calculate the centroid of all devices whose buttons were pressed in the last frame: */
int numLastDevices=0;
Point::AffineCombiner centroidC;
for(int i=0;i<input.getNumButtonSlots();++i)
if(lastDeviceButtonStates[i])
{
++numLastDevices;
centroidC.addPoint(getButtonDevicePosition(i));
}
if(numLastDevices>0)
{
Point currentCentroid=centroidC.getPoint();
/* Calculate the average rotation vector and scaling factor of all devices whose buttons were pressed in the last frame: */
Vector rotation=Vector::zero;
Scalar scaling(1);
int numActiveDevices=0;
for(int i=0;i<input.getNumButtonSlots();++i)
if(lastDeviceButtonStates[i])
{
/* Calculate the previous vector to centroid: */
Vector lastDist=lastDevicePositions[i]-lastCentroid;
Scalar lastLen=Geometry::mag(lastDist);
/* Calculate the new vector to centroid: */
Vector currentDist=getButtonDevicePosition(i)-currentCentroid;
Scalar currentLen=Geometry::mag(currentDist);
if(lastLen>configuration.minRotationScalingDistance&¤tLen>configuration.minRotationScalingDistance)
{
/* Calculate the rotation axis and angle: */
Vector rot=lastDist^currentDist;
Scalar rotLen=Geometry::mag(rot);
if(rotLen>Scalar(0))
{
Scalar angle=Math::asin(rotLen/(lastLen*currentLen));
rot*=angle/rotLen;
/* Accumulate the rotation vector: */
rotation+=rot;
}
/* Calculate the scaling factor: */
Scalar scal=currentLen/lastLen;
/* Accumulate the scaling factor: */
scaling*=scal;
++numActiveDevices;
}
}
/* Navigate: */
NavTransform t=NavTransform::translate((currentCentroid-lastCentroid)*configuration.translationFactor);
if(numActiveDevices>0)
{
/* Average and scale rotation and scaling: */
rotation*=configuration.rotationFactor/Scalar(numActiveDevices);
scaling=Math::pow(scaling,configuration.scalingFactor/Scalar(numActiveDevices));
/* Apply rotation and scaling: */
t*=NavTransform::translateFromOriginTo(currentCentroid);
t*=NavTransform::rotate(Rotation::rotateScaledAxis(rotation));
t*=NavTransform::scale(scaling);
t*=NavTransform::translateToOriginFrom(currentCentroid);
}
concatenateNavigationTransformationLeft(t);
}
/* Update button states and device positions for next frame: */
Point::AffineCombiner newLastCentroidC;
for(int i=0;i<input.getNumButtonSlots();++i)
{
lastDeviceButtonStates[i]=getButtonState(i);
lastDevicePositions[i]=getButtonDevicePosition(i);
if(lastDeviceButtonStates[i])
newLastCentroidC.addPoint(lastDevicePositions[i]);
}
lastCentroid=newLastCentroidC.getPoint();
}
}