void Transform3DWidget::prePaintEvent()
{
QString M = qstring_cast(this->getMatrixInternal());
mTextEdit->blockSignals(true);
int textPos = mTextEdit->textCursor().position();
mTextEdit->setText(M);
QTextCursor cursor = mTextEdit->textCursor();
cursor.setPosition(textPos);
mTextEdit->setTextCursor(cursor);
mTextEdit->blockSignals(false);
Vector3D xyz = mDecomposition.getAngles();
mBlockChanges = true;
mAngleAdapter[0]->setValue(wrapAngle(xyz[0]));
mAngleAdapter[1]->setValue(wrapAngle(xyz[1]));
mAngleAdapter[2]->setValue(wrapAngle(xyz[2]));
Vector3D t = mDecomposition.getPosition();
mTranslationAdapter[0]->setValue(t[0]);
mTranslationAdapter[1]->setValue(t[1]);
mTranslationAdapter[2]->setValue(t[2]);
this->updateInvertAction();
mBlockChanges = false;
}
void MathInterface::functionWrapAngle(void) {
Any a = cb->popValue();
if (a.type() == Any::Float) {
cb->pushValue(wrapAngle(a.getFloat()));
}
else { // Has to be int
cb->pushValue(wrapAngle(a.getInt()));
}
}
StraightPathSegment::StraightPathSegment(Segment& segment, bool dir) {
double angle = segment.getOrientation();
if (dir) {
start = Configuration(segment.getA(), angle);
end = Configuration(segment.getB(), angle);
} else {
start = Configuration(segment.getA(), wrapAngle(angle + M_PI));
end = Configuration(segment.getB(), wrapAngle(angle + M_PI));
}
this->dir = dir;
length = Point::distance(start.position, end.position);
}
/**
@brief Normalise the selected rotations to be within the +/-180 degree range.
@details The normalise uses a wrap around, so for example a yaw of 360 degrees becomes 0 degrees, and -190 degrees becomes 170.
@param normYaw If false, the yaw isn't normalized.
@param normPitch If false, the pitch isn't normalized.
@param normRoll If false, the roll isn't normalized.
*/
inline Euler &normalise(bool normYaw = true, bool normPitch = true,
bool normRoll = true) {
if (normYaw)
wrapAngle(mYaw);
if (normPitch)
wrapAngle(mPitch);
if (normRoll)
wrapAngle(mRoll);
return *this;
}
double computeMerit(const std::vector< Matrix<C_DIM> >& X, const std::vector< Matrix<U_DIM> >& U, double penalty_coeff)
{
double merit = 0;
Matrix<X_DIM> x;
Matrix<C_DIM> dynviol;
Matrix<X_DIM, X_DIM> SqrtSigma;
Matrix<B_DIM> b, b_tp1;
vec(x0, SqrtSigma0, b);
for(int t = 0; t < T-1; ++t) {
unVec(b, x, SqrtSigma);
merit += alpha_belief*tr(SqrtSigma*SqrtSigma) + alpha_control*tr(~U[t]*U[t]);
b_tp1 = beliefDynamics(b, U[t]);
dynviol = (X[t+1] - b_tp1.subMatrix<C_DIM,1>(0,0) );
for(int i = 0; i < C_DIM; ++i) {
if (i != P_DIM) {
merit += penalty_coeff*fabs(dynviol[i]);
} else {
merit += penalty_coeff*wrapAngle(fabs(dynviol[i]));
}
}
b = b_tp1;
}
unVec(b, x, SqrtSigma);
merit += alpha_final_belief*tr(SqrtSigma*SqrtSigma);
return merit;
}
double casadiComputeMerit(const std::vector< Matrix<C_DIM> >& X, const std::vector< Matrix<U_DIM> >& U, double penalty_coeff)
{
double merit = 0;
merit = casadiComputeCost(X, U);
Matrix<X_DIM> x;
Matrix<C_DIM> dynviol;
Matrix<X_DIM, X_DIM> SqrtSigma;
Matrix<B_DIM> b, b_tp1;
vec(x0, SqrtSigma0, b);
for(int t = 0; t < T-1; ++t) {
unVec(b, x, SqrtSigma);
b_tp1 = beliefDynamics(b, U[t]);
dynviol = (X[t+1] - b_tp1.subMatrix<C_DIM,1>(0,0) );
for(int i = 0; i < C_DIM; ++i) {
if (i != P_DIM) {
merit += penalty_coeff*fabs(dynviol[i]);
} else {
merit += penalty_coeff*wrapAngle(fabs(dynviol[i])); // since angles wrap
}
}
b = b_tp1;
}
return merit;
}
float Utilities::wrapAngle360(float angle)
{
angle = wrapAngle(angle);
if (angle < 0) {
angle += 360;
}
return angle;
}
double statePenaltyCollocation(std::vector< Matrix<C_DIM> >& X, std::vector< Matrix<U_DIM> >& U, stateMPC_params& problem, stateMPC_output& output, stateMPC_info& info)
{
resetStateMPCVars();
double penalty_coeff = cfg::initial_penalty_coeff_factor * cfg::initial_penalty_coeff;
//double trust_box_size = cfg::initial_trust_box_size;
int penalty_increases = 0;
Matrix<X_DIM> dynviol;
// penalty loop
while(penalty_increases < cfg::max_penalty_coeff_increases)
{
bool success = minimizeMeritFunction(X, U, problem, output, info, penalty_coeff);
double cntviol = 0;
Matrix<X_DIM> x_t, x_tp1;
x_t.insert(0, 0, X[0]);
x_t.insert(0, 0, x0.subMatrix<L_DIM,1>(C_DIM,0));
for(int t = 0; t < T-1; ++t) {
x_t.insert(0, 0, X[t]);
x_t.insert(C_DIM, 0, x0.subMatrix<L_DIM,1>(C_DIM,0));
x_tp1.insert(0, 0, X[t+1]);
x_tp1.insert(C_DIM, 0, x0.subMatrix<L_DIM,1>(C_DIM,0));
dynviol = x_tp1 - dynfunc(x_t, U[t], zeros<Q_DIM,1>());
for(int i = 0; i < C_DIM; ++i) {
if (i != P_DIM) {
cntviol += fabs(dynviol[i]);
} else {
cntviol += wrapAngle(fabs(dynviol[i]));
}
}
}
success = success && (cntviol < cfg::cnt_tolerance);
LOG_DEBUG("Constraint violations: %2.10f",cntviol);
if (!success) {
penalty_increases++;
penalty_coeff = penalty_coeff*cfg::penalty_coeff_increase_ratio;
//trust_box_size = cfg::initial_trust_box_size;
}
else {
//return computeCost(X, U);
return casadiComputeCost(X, U);
}
}
//return computeCost(X, U);
return casadiComputeCost(X, U);
}
void lookHelper_tick(struct entity* entity) {
if (entity->ai == NULL || entity->ai->lookHelper_speedPitch == 0. || entity->ai->lookHelper_speedYaw == 0.) return;
double dx = entity->ai->lookHelper_x - entity->x;
struct boundingbox bb;
getEntityCollision(entity, &bb);
double dy = entity->ai->lookHelper_y - entity->y - ((bb.maxY - bb.minY) * .9); // TODO: real eye height
double dz = entity->ai->lookHelper_z - entity->z;
double horiz_dist = sqrt(dx * dx + dz * dz);
float dp = -(atan2(dy, horiz_dist) * 180. / M_PI);
float dya = (atan2(dz, dx) * 180. / M_PI) - 90.;
dp = wrapAngle(dp - entity->pitch);
if (dp > entity->ai->lookHelper_speedPitch) dp = entity->ai->lookHelper_speedPitch;
if (dp < -entity->ai->lookHelper_speedPitch) dp = -entity->ai->lookHelper_speedPitch;
entity->pitch += dp;
dya = wrapAngle(dya - entity->yaw);
if (dya > entity->ai->lookHelper_speedYaw) dya = entity->ai->lookHelper_speedYaw;
if (dya < -entity->ai->lookHelper_speedYaw) dya = -entity->ai->lookHelper_speedYaw;
entity->yaw += dya;
if (fabs(dp) < 0.5 && fabs(dya) < .5) {
entity->ai->lookHelper_speedPitch = 0.;
entity->ai->lookHelper_speedYaw = 0.;
}
}
void MathInterface::functionCurveAngle(void) {
float smoothness = cb->popValue().toFloat();
float oldA = cb->popValue().toFloat();
float newA = cb->popValue().toFloat();
float diff = oldA - newA;
while (diff > 180.0f) {
diff -= 360.0f;
}
while (diff < -180.0f) {
diff += 360.0f;
}
cb->pushValue(wrapAngle(oldA - diff / smoothness));
}
void moveCell(Grid* W,Cell* moveMe, double* newLoc){/*Cells moved by summing move ability vectors and bumping into other cells (cellBump)*/
if(insideWorld(newLoc,0)){
Square* currSq=getSquare(W,moveMe->CellPos);
Square* nextSq=getSquare(W,newLoc);
if(!nextSq){
fprintf(stderr,"cell detected it has moved off the world, this should never happen, the body should catch this first!");
//fprintf(stderr,"cell trying to move outside of world");
}
//addColor(W,moveMe->color,0,.7,0,MOVEMENT);
if(currSq!=nextSq){
remLL(&currSq->cells,&moveMe->mySqElem);
addLL(&nextSq->cells,&moveMe->mySqElem);
}
/* cells lose energy based on how far they have moved */
double moveCost=pow(getDist(moveMe->CellPos,newLoc)/CELLRAD,MOVE_EXP)*MOVE_COST_MULT;
EnTransfer(&W->SUN,&moveMe->myBody->energy,moveCost*((1-MOVE_COST_DROP_PROP)+(MOVE_COST_DROP_PROP*(1.0/moveMe->myBody->cellSize))));//getTerrain(W,moveMe->CellPos)->moveCost);///log(moveMe->myBody->cellSize+2));//,__func__,__LINE__);
//EnTransfer(&W->SUN,&moveMe->myBody->energy,pow(getDist(moveMe->CellPos,newLoc)/CELLRAD,MOVE_EXP)*getTerrain(W,moveMe->CellPos)->moveCost/moveMe->myBody->cellSize);//,__func__,__LINE__);
moveMe->CellPos[0]=newLoc[0];
moveMe->CellPos[1]=newLoc[1];
}
moveMe->CellPos[2]=wrapAngle(newLoc[2]);
calcBehindPoint(moveMe->behindPoint,moveMe->CellPos);
};
void CameraFlyer::update()
{
bool goingForward = gVirtualInput().isButtonHeld(mMoveForward);
bool goingBack = gVirtualInput().isButtonHeld(mMoveBack);
bool goingLeft = gVirtualInput().isButtonHeld(mMoveLeft);
bool goingRight = gVirtualInput().isButtonHeld(mMoveRight);
bool fastMove = gVirtualInput().isButtonHeld(mFastMove);
bool camRotating = gVirtualInput().isButtonHeld(mRotateCam);
if (camRotating != mLastButtonState)
{
if (camRotating)
Cursor::instance().hide();
else
Cursor::instance().show();
mLastButtonState = camRotating;
}
float frameDelta = gTime().getFrameDelta();
if (camRotating)
{
mYaw += Degree(gVirtualInput().getAxisValue(mHorizontalAxis) * ROTATION_SPEED * frameDelta);
mPitch += Degree(gVirtualInput().getAxisValue(mVerticalAxis) * ROTATION_SPEED * frameDelta);
mYaw = wrapAngle(mYaw);
mPitch = wrapAngle(mPitch);
Quaternion yRot;
yRot.fromAxisAngle(Vector3::UNIT_Y, Radian(mYaw));
Quaternion xRot;
xRot.fromAxisAngle(Vector3::UNIT_X, Radian(mPitch));
Quaternion camRot = yRot * xRot;
camRot.normalize();
SO()->setRotation(camRot);
}
Vector3 direction = Vector3::ZERO;
if (goingForward) direction += SO()->getForward();
if (goingBack) direction -= SO()->getForward();
if (goingRight) direction += SO()->getRight();
if (goingLeft) direction -= SO()->getRight();
if (direction.squaredLength() != 0)
{
direction.normalize();
float multiplier = 1.0f;
if (fastMove)
multiplier = FAST_MODE_MULTIPLIER;
mCurrentSpeed = Math::clamp(mCurrentSpeed + ACCELERATION * frameDelta, START_SPEED, TOP_SPEED);
mCurrentSpeed *= multiplier;
}
else
{
mCurrentSpeed = 0.0f;
}
float tooSmall = std::numeric_limits<float>::epsilon();
if (mCurrentSpeed > tooSmall)
{
Vector3 velocity = direction * mCurrentSpeed;
SO()->move(velocity * frameDelta);
}
}
void CameraFlyer::update()
{
// Check if any movement or rotation keys are being held
bool goingForward = gVirtualInput().isButtonHeld(mMoveForward);
bool goingBack = gVirtualInput().isButtonHeld(mMoveBack);
bool goingLeft = gVirtualInput().isButtonHeld(mMoveLeft);
bool goingRight = gVirtualInput().isButtonHeld(mMoveRight);
bool fastMove = gVirtualInput().isButtonHeld(mFastMove);
bool camRotating = gVirtualInput().isButtonHeld(mRotateCam);
// If switch to or from rotation mode, hide or show the cursor
if (camRotating != mLastButtonState)
{
if (camRotating)
Cursor::instance().hide();
else
Cursor::instance().show();
mLastButtonState = camRotating;
}
// If camera is rotating, apply new pitch/yaw rotation values depending on the amount of rotation from the
// vertical/horizontal axes.
float frameDelta = gTime().getFrameDelta();
if (camRotating)
{
mYaw += Degree(gVirtualInput().getAxisValue(mHorizontalAxis) * ROTATION_SPEED * frameDelta);
mPitch += Degree(gVirtualInput().getAxisValue(mVerticalAxis) * ROTATION_SPEED * frameDelta);
mYaw = wrapAngle(mYaw);
mPitch = wrapAngle(mPitch);
Quaternion yRot;
yRot.fromAxisAngle(Vector3::UNIT_Y, Radian(mYaw));
Quaternion xRot;
xRot.fromAxisAngle(Vector3::UNIT_X, Radian(mPitch));
Quaternion camRot = yRot * xRot;
camRot.normalize();
SO()->setRotation(camRot);
}
// If the movement button is pressed, determine direction to move in
Vector3 direction = Vector3::ZERO;
if (goingForward) direction += SO()->getForward();
if (goingBack) direction -= SO()->getForward();
if (goingRight) direction += SO()->getRight();
if (goingLeft) direction -= SO()->getRight();
// If a direction is chosen, normalize it to determine final direction.
if (direction.squaredLength() != 0)
{
direction.normalize();
// Apply fast move multiplier if the fast move button is held.
float multiplier = 1.0f;
if (fastMove)
multiplier = FAST_MODE_MULTIPLIER;
// Calculate current speed of the camera
mCurrentSpeed = Math::clamp(mCurrentSpeed + ACCELERATION * frameDelta, START_SPEED, TOP_SPEED);
mCurrentSpeed *= multiplier;
}
else
{
mCurrentSpeed = 0.0f;
}
// If the current speed isn't too small, move the camera in the wanted direction
float tooSmall = std::numeric_limits<float>::epsilon();
if (mCurrentSpeed > tooSmall)
{
Vector3 velocity = direction * mCurrentSpeed;
SO()->move(velocity * frameDelta);
}
}
//----------------------------------------------------------
void ofPolyline::arc(const ofPoint & center, float radiusX, float radiusY, float angleBegin, float angleEnd, bool clockwise, int circleResolution){
if(circleResolution<=1) circleResolution=2;
setCircleResolution(circleResolution);
points.reserve(points.size()+circleResolution);
const float epsilon = 0.0001f;
const size_t nCirclePoints = circlePoints.size();
float segmentArcSize = M_TWO_PI / (float)nCirclePoints;
// convert angles to radians and wrap them into the range 0-M_TWO_PI and
float angleBeginRad = wrapAngle(ofDegToRad(angleBegin));
float angleEndRad = wrapAngle(ofDegToRad(angleEnd));
while(angleBeginRad >= angleEndRad) angleEndRad += M_TWO_PI;
// determine the directional angle delta
float d = clockwise ? angleEndRad - angleBeginRad : angleBeginRad - angleEndRad;
// find the shortest angle delta, clockwise delta direction yeilds POSITIVE values
float deltaAngle = atan2(sin(d),cos(d));
// establish the remaining angle that we have to work through
float remainingAngle = deltaAngle;
// if the delta angle is in the CCW direction OR the start and stop angles are
// effectively the same adjust the remaining angle to be a be a full rotation
if(deltaAngle < 0 || abs(deltaAngle) < epsilon) remainingAngle += M_TWO_PI;
ofPoint radii(radiusX,radiusY);
ofPoint point;
int currentLUTIndex = 0;
bool isFirstPoint = true; // special case for the first point
while(remainingAngle > 0) {
if(isFirstPoint) {
// TODO: should this be the exact point on the circle or
// should it be an intersecting point on the line that connects two
// surrounding LUT points?
//
// get the EXACT first point requested (for points that
// don't fall precisely on a LUT entry)
point = ofPoint(cos(angleBeginRad),sin(angleBeginRad));
// set up the get any in between points from the LUT
float ratio = angleBeginRad / M_TWO_PI * (float)nCirclePoints;
currentLUTIndex = clockwise ? (int)ceil(ratio) : (int)floor(ratio);
float lutAngleAtIndex = currentLUTIndex * segmentArcSize;
// the angle between the beginning angle and the next angle in the LUT table
float d = clockwise ? (lutAngleAtIndex - angleBeginRad) : (angleBeginRad - lutAngleAtIndex);
float firstPointDelta = atan2(sin(d),cos(d)); // negative is in the clockwise direction
// if the are "equal", get the next one CCW
if(abs(firstPointDelta) < epsilon) {
currentLUTIndex = clockwise ? (currentLUTIndex + 1) : (currentLUTIndex - 1);
firstPointDelta = segmentArcSize; // we start at the next lut point
}
// start counting from the offset
remainingAngle -= firstPointDelta;
isFirstPoint = false;
} else {
point = ofPoint(circlePoints[currentLUTIndex].x,circlePoints[currentLUTIndex].y);
if(clockwise) {
currentLUTIndex++; // go to the next LUT point
remainingAngle -= segmentArcSize; // account for next point
// if the angle overshoots, then the while loop will fail next time
} else {
currentLUTIndex--; // go to the next LUT point
remainingAngle -= segmentArcSize; // account for next point
// if the angle overshoots, then the while loop will fail next time
}
}
// keep the current lut index in range
if(clockwise) {
currentLUTIndex = currentLUTIndex % nCirclePoints;
} else {
if(currentLUTIndex < 0) currentLUTIndex = nCirclePoints + currentLUTIndex;
}
// add the point to the poly line
point = point * radii + center;
points.push_back(point);
// if the next LUT point moves us past the end angle then
// add a a point a the exact end angle and call it finished
if(remainingAngle < epsilon) {
point = ofPoint(cos(angleEndRad),sin(angleEndRad));
point = point * radii + center;
points.push_back(point);
remainingAngle = 0; // call it finished, the next while loop test will fail
}
}
flagHasChanged();
}
/*!
* Subtract one angle from another accounting for circular wrap
* \param left is the first angle in radians in the range -PI to PI
* \param serightcond is the second angle in radians in the range -PI to PI
* \return the correctly wrapped difference of left minus right, in the range -PI to PI
*/
double subtractAngles(double left, double right)
{
return wrapAngle(left - right);
}
/*!
* Add two angles together accounting for circular wrap
* \param first is the first angle in radians in the range -PI to PI
* \param second is the second angle in radians in the range -PI to PI
* \return the correctly wrapped sum of first and second, in the range -PI to PI
*/
double addAngles(double first, double second)
{
return wrapAngle(first + second);
}
void callback(const sensor_msgs::JointState::ConstPtr& jointStatePtr)
{
ROS_INFO_STREAM("I'm in the callback");
float L1 = 0.3;
float L2 = 0.31;
float theta1_L, theta1_R, theta2_L, theta2_R;
float mPError1, mPError2;
float r = sqrt(pow(x_estimate, 2)+pow(y_estimate, 2));
float alpha = acosf((powf(L1,2)+powf(L2,2)-powf(r,2))/(2.0*L1*L2));
float beta = acosf((powf(L1,2)-powf(L2,2)+powf(r,2))/(2.0*L1*r));
theta2_L = M_PI + alpha;
theta2_R = M_PI - alpha;
theta1_L = atan2f(y_estimate, x_estimate) + beta;
theta1_R = atan2f(y_estimate, x_estimate) - beta;
theta1_L = wrapAngle(theta1_L);
theta1_R = wrapAngle(theta1_R);
theta2_L = wrapAngle(theta2_L);
theta2_R = wrapAngle(theta2_R);
sensor_msgs::JointState output;
output.header.stamp = ros::Time::now();
output.header.frame_id = "";
if (direction == "left")
{
mPError1 = theta1_L-jointStatePtr->position[0];
//mIError1 += theta1_L-jointStatePtr->position[0];
mPError2 = theta2_L-jointStatePtr->position[2];
//mIError2 += theta2_L-jointStatePtr->position[2];
}
else if (direction == "right")
{
mPError1 = theta1_R-jointStatePtr->position[0];
mPError2 = theta2_R-jointStatePtr->position[2];
}
else
ROS_FATAL("COMMAND WAS NEITHER LEFT NOR RIGHT!!!");
ROS_INFO("******************************GOING FOR IT!!*****************************************\n");
ROS_INFO("x_board=%f, y_board=%f \n", x_estimate, y_estimate);
ROS_INFO("r = %f, alpha = %f, beta = %f\n", r, alpha, beta);
ROS_INFO("theta1_L = %f, theta1_R = %f, theta2_L = %f, theta2_R = %f\n", theta1_L, theta1_R, theta2_L, theta2_R);
ROS_INFO("Joint Positions: (%f, %f, %f)\n", jointStatePtr->position[0], jointStatePtr->position[1], jointStatePtr->position[2]);
ROS_INFO("P Error_1 = %f, P Error_2 = %f\n", mPError1, mPError2);
ROS_INFO("***********************************************************************\n");
//ROS_INFO_STREAM("Back in main, global counter is: " << globalcounter);
float gain=0.2;
float command1 = jointStatePtr->position[0]+gain*mPError1;
float command2 = jointStatePtr->position[2]+gain*mPError2;
ROS_INFO_STREAM("Command to motor 21: " << command1 << " Command to motor 23: " << command2);
//std::cout << "commanding: " << x_estimate << ", " << y_estimate << std::endl;
if ((finalMoveFlag == 1) && (idleFlag ==0))
{
ROS_INFO("finalMoveFlag is 1");
output.position.push_back(jointStatePtr->position[0]+mPError1);
output.position.push_back(1.0);
output.position.push_back(jointStatePtr->position[2]+mPError2);
}
else if (idleFlag == 0)
{
output.position.push_back(command1);
output.position.push_back(1.0);
output.position.push_back(command2);
}
else
{
ROS_INFO("Idling the motors");
output.position.push_back(NAN);
output.position.push_back(1.0);
output.position.push_back(NAN);
output.velocity.push_back(0);
output.velocity.push_back(0);
output.velocity.push_back(0);
}
//.... do something with the input and generate the output...
pub_.publish(output);
ROS_INFO_STREAM("Just published!");
}
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);
}
}
}
