void SixAxisTransformTool::frame(void)
{
/* Assemble translation from translation vectors and current valuator values: */
Vector translation=Vector::zero;
for(int i=0; i<3; ++i)
translation+=translations[i]*Scalar(getValuatorState(i));
translation*=getCurrentFrameTime();
/* Assemble rotation from scaled rotation axes and current valuator values: */
Vector rotation=Vector::zero;
for(int i=0; i<3; ++i)
rotation+=config.rotations[i]*Scalar(getValuatorState(3+i));
rotation*=getCurrentFrameTime();
/* Calculate an incremental transformation for the virtual input device: */
ONTransform deltaT=ONTransform::translate(translation);
Point pos=transformedDevice->getPosition();
deltaT*=ONTransform::translateFromOriginTo(pos);
deltaT*=ONTransform::rotate(ONTransform::Rotation::rotateScaledAxis(rotation));
deltaT*=ONTransform::translateToOriginFrom(pos);
/* Update the virtual input device's transformation: */
deltaT*=transformedDevice->getTransformation();
deltaT.renormalize();
transformedDevice->setTransformation(deltaT);
/* Request another frame if the input device has moved: */
if(translation!=Vector::zero||rotation!=Vector::zero)
{
/* Request another frame: */
scheduleUpdate(getApplicationTime()+1.0/125.0);
}
}
void RevolverTool::buttonCallback(int buttonSlotIndex,InputDevice::ButtonCallbackData* cbData)
{
if(buttonSlotIndex==0)
{
if(cbData->newButtonState)
{
/* Change the currently mapped button set: */
currentChamber=(currentChamber+1)%factory->numChambers;
/* Set the newly mapped chamber's state to the input device's buttons' and valuators' states: */
for(int i=1;i<input.getNumButtonSlots();++i)
transformedDevice->setButtonState((i-1)*factory->numChambers+currentChamber,getButtonState(i));
for(int i=0;i<input.getNumValuatorSlots();++i)
transformedDevice->setValuator(i*factory->numChambers+currentChamber,getValuatorState(i));
/* Show the current button assignment for one second: */
showNumbersTime=getApplicationTime()+1.0;
}
}
else
{
/* Pass the button event through to the virtual input device: */
transformedDevice->setButtonState((buttonSlotIndex-1)*factory->numChambers+currentChamber,cbData->newButtonState);
}
}
void SixAxisNavigationTool::frame(void)
{
if(isActive())
{
/* Assemble translation from translation vectors and current valuator values: */
Vector translation=Vector::zero;
for(int i=0;i<3;++i)
translation+=translations[i]*Scalar(getValuatorState(i));
translation*=getCurrentFrameTime();
/* Assemble rotation from scaled rotation axes and current valuator values: */
Vector rotation=Vector::zero;
for(int i=0;i<3;++i)
rotation+=rotations[i]*Scalar(getValuatorState(3+i));
rotation*=getCurrentFrameTime();
/* Calculate incremental zoom factor: */
Scalar zoom=config.zoomFactor*Scalar(getValuatorState(6))*getCurrentFrameTime();
/* Apply proper navigation mode: */
if(config.invertNavigation)
{
translation=-translation;
rotation=-rotation;
zoom=-zoom;
}
/* Calculate an incremental transformation based on the translation and rotation: */
NavTrackerState deltaT=NavTrackerState::translateFromOriginTo(config.followDisplayCenter?getDisplayCenter():config.navigationCenter);
deltaT*=NavTrackerState::translate(translation);
deltaT*=NavTrackerState::rotate(NavTrackerState::Rotation::rotateScaledAxis(rotation));
deltaT*=NavTrackerState::scale(Math::exp(-zoom));
deltaT*=NavTrackerState::translateToOriginFrom(config.followDisplayCenter?getDisplayCenter():config.navigationCenter);
/* Update the accumulated transformation: */
navTransform.leftMultiply(deltaT);
navTransform.renormalize();
/* Update the navigation transformation: */
setNavigationTransformation(navTransform);
/* Request another frame: */
scheduleUpdate(getApplicationTime()+1.0/125.0);
}
}
void SixAxisSurfaceNavigationTool::frame(void)
{
/* Act depending on this tool's current state: */
if(isActive())
{
/* Use the average frame time as simulation time: */
Scalar dt=getCurrentFrameTime();
/* Update rotation angles based on current rotation valuator states: */
for(int i=0;i<3;++i)
angles[i]=wrapAngle(angles[i]+getValuatorState(i+3)*factory->rotateFactors[i]*dt);
if(angles[1]<Math::rad(Scalar(-90)))
angles[1]=Math::rad(Scalar(-90));
else if(angles[1]>Math::rad(Scalar(90)))
angles[1]=Math::rad(Scalar(90));
if(!factory->canRoll||factory->bankTurns)
{
Scalar targetRoll=factory->bankTurns?getValuatorState(3)*factory->rotateFactors[2]:Scalar(0);
Scalar t=Math::exp(-factory->levelSpeed*dt);
angles[2]=angles[2]*t+targetRoll*(Scalar(1)-t);
if(Math::abs(angles[2]-targetRoll)<Scalar(1.0e-3))
angles[2]=targetRoll;
}
/* Calculate the new head position: */
Point newHeadPos=getMainViewer()->getHeadPosition();
/* Create a physical navigation frame around the new foot position: */
calcPhysicalFrame(newHeadPos);
/* Calculate movement from head position change: */
Vector move=newHeadPos-headPos;
headPos=newHeadPos;
/* Add movement velocity based on the current translation valuator states: */
for(int i=0;i<3;++i)
move[i]+=getValuatorState(i)*factory->translateFactors[i]*dt;
/* Transform the movement vector from physical space to the physical navigation frame: */
move=physicalFrame.inverseTransform(move);
/* Rotate by the current azimuth and elevation angles: */
move=Rotation::rotateX(-angles[1]).transform(move);
move=Rotation::rotateZ(-angles[0]).transform(move);
/* Move the surface frame: */
NavTransform newSurfaceFrame=surfaceFrame;
newSurfaceFrame*=NavTransform::translate(move);
/* Re-align the surface frame with the surface: */
Point initialOrigin=newSurfaceFrame.getOrigin();
Rotation initialOrientation=newSurfaceFrame.getRotation();
AlignmentData ad(surfaceFrame,newSurfaceFrame,factory->probeSize,factory->maxClimb);
align(ad);
if(!factory->fixAzimuth)
{
/* Have the azimuth angle track changes in the surface frame's rotation: */
Rotation rot=Geometry::invert(initialOrientation)*newSurfaceFrame.getRotation();
rot.leftMultiply(Rotation::rotateFromTo(rot.getDirection(2),Vector(0,0,1)));
Vector x=rot.getDirection(0);
angles[0]=wrapAngle(angles[0]+Math::atan2(x[1],x[0]));
}
/* If flying is allowed and the initial surface frame was above the surface, lift it back up: */
Scalar z=newSurfaceFrame.inverseTransform(initialOrigin)[2];
if(!factory->canFly||z<factory->probeSize)
z=factory->probeSize;
newSurfaceFrame*=NavTransform::translate(Vector(Scalar(0),Scalar(0),z));
/* Apply the newly aligned surface frame: */
surfaceFrame=newSurfaceFrame;
applyNavState();
/* Deactivate the tool if it is done: */
if(numActiveAxes==0&&Math::abs(angles[2])<Math::Constants<Scalar>::epsilon)
deactivate();
else
{
/* Request another frame: */
scheduleUpdate(getApplicationTime()+1.0/125.0);
}
}
}
