Point TrackballNavigationTool::calcTrackballPosition(void) const
{
/* Get device ray equation: */
Ray ray=getButtonDeviceRay(0);
/* Intersect ray with trackball sphere: */
Vector d=getDisplayCenter()-ray.getOrigin();
Scalar dLen2=Geometry::sqr(d);
Scalar ph=ray.getDirection()*d;
Scalar radius=getDisplaySize();
Scalar det=Math::sqr(ph)+Math::sqr(radius)-dLen2;
if(det>=Scalar(0))
{
/* Find first intersection of ray with sphere (even if behind start point): */
det=Math::sqrt(det);
Scalar lambda=ph-det;
return ray(lambda);
}
else
{
/* Find closest point on sphere to ray: */
Vector ctop=ray.getDirection()*((d*ray.getDirection())/Geometry::sqr(ray.getDirection()))-d;
ctop*=radius/Geometry::mag(ctop);
return getDisplayCenter()+ctop;
}
}
void CalibrationCheckTool::frame(void)
{
if(getButtonState(0))
{
/* Get the interaction point: */
haveDepthPoint=false;
Vrui::Point p(application->calcImagePoint(getButtonDeviceRay(0)).getComponents());
if(p[0]>=-640.0&&p[0]<0.0&&p[1]>=0.0&&p[1]<480.0)
{
int px=int(Math::floor(p[0]+640.0));
int py=int(Math::floor(p[1]));
if(application->averageFrameForeground[py*640+px]>=float(application->averageNumFrames)*0.5f)
{
/* Remember the depth point: */
depthPoint=Point2(double(px)+0.5-640.0,double(py)+0.5);
/* Calculate the color point: */
double depth=application->depthCorrection[py*640+px].correct(application->averageFrameDepth[py*640+px]/application->averageFrameForeground[py*640+px]);
int cx=int(Math::floor(depthToColor[(479-py)*640+px][0]+dtcOffset+dtcScale*depth));
int cy=479-int(Math::floor(depthToColor[(479-py)*640+px][1]))-rowOffset;
colorPoint[0]=double(cx)+0.5;
colorPoint[1]=double(cy)+0.5;
haveDepthPoint=true;
}
}
}
}
void LaserpointerTool::frame(void)
{
if(active)
{
/* Update the laser ray: */
ray=getButtonDeviceRay(0);
}
}
void MeasurementTool::buttonCallback(int buttonSlotIndex,Vrui::InputDevice::ButtonCallbackData* cbData)
{
if(cbData->newButtonState)
{
selectedPoint=Point(application->calcImagePoint(getButtonDeviceRay(0)));
#if 0
if(selectedPoint[0]>=-double(application->depthFrameSize[0])&&selectedPoint[0]<0.0&&selectedPoint[1]>=0.0&&selectedPoint[1]<double(application->depthFrameSize[1]))
{
/* Request an average depth frame from the main application: */
application->requestAverageFrame(Misc::createFunctionCall(this,&MeasurementTool::averageDepthFrameReady));
}
#else
RawKinectViewer::CPoint imagePoint=application->getDepthImagePoint(selectedPoint);
if(imagePoint[2]>=RawKinectViewer::CPoint::Scalar(0))
{
/* Transform the image point to world space and print it: */
RawKinectViewer::CPoint worldPoint=application->intrinsicParameters.depthProjection.transform(imagePoint);
std::cout<<std::setw(20)<<worldPoint<<std::endl;
}
#endif
}
}
Point MouseNavigationTool::calcScreenPos(void) const
{
/* Calculate the ray equation: */
Ray ray=getButtonDeviceRay(0);
/* Get the transformation of the screen currently containing the input device: */
Scalar viewport[4];
ONTransform screenT=getMouseScreenTransform(mouseAdapter,viewport);
/* Intersect the device ray with the screen: */
Vector normal=screenT.getDirection(2);
Scalar d=normal*screenT.getOrigin();
Scalar divisor=normal*ray.getDirection();
if(divisor==Scalar(0))
return Point::origin;
Scalar lambda=(d-ray.getOrigin()*normal)/divisor;
if(lambda<Scalar(0))
return Point::origin;
return ray(lambda);
}
