public static Point3D AdjustLocationToLaser(Point3D current, Vector3D lookDirection, double currentDistanceMM, double tarGetZDistanceMM, double biasMovement = 1)
{
if (double.IsNaN(currentDistanceMM) ||
double.IsNaN(tarGetZDistanceMM))
{
return(current);
}
double zMoveMM = Math.Round((currentDistanceMM - tarGetZDistanceMM), 1);
double zMove = zMoveMM / 1000;
if (zMove == 0 ||
Math.Abs(zMoveMM) < 0.25)
{
return(current);
}
zMove = zMove * biasMovement;
CameraURCoordinateTranslator.Direction direction = CameraURCoordinateTranslator.Direction.Forward;
if (zMove < 0)
{
direction = CameraURCoordinateTranslator.Direction.Backward;
zMove = Math.Abs(zMove);
}
else
{
direction = CameraURCoordinateTranslator.Direction.Forward;
}
Point3D returnPoint = App.Current.CoordinateTranslator.GetPointRelativeToolRoation(current, lookDirection, zMove, direction);
return(returnPoint);
}
void MoveZ(CameraURCoordinateTranslator.Direction direction)
{
Vector3D copy = GetCurrentLookDirection();
Point3D translatedPoint = App.Current.CoordinateTranslator.GetPointRelativeToolRoation(
GetCurrentPoint(),
copy,
(GetZDistance() / 1000),
direction);
MoveToPoint(translatedPoint, GetCurrentLookDirectionAsPoint(), false);
}
public static Point3D AdjustLocationToForceSensor(Point3D current, Vector3D lookDirection, Vector3D forceSensorDirection)
{
CameraURCoordinateTranslator.Direction direction = CameraURCoordinateTranslator.Direction.Forward;
//Z
double move = App.Current.ApplicationSettings.ForceRetractDistanceMM / 1000;//ConvertForceToMove(forceSensorDirection.Z);
//if (move < 0)
//{
direction = CameraURCoordinateTranslator.Direction.Backward;
// move = Math.Abs(move);
//}
//else
//{
// direction = CameraURCoordinateTranslator.Direction.Forward;
//}
Point3D returnPointZ = App.Current.CoordinateTranslator.GetPointRelativeToolRoation(current, lookDirection, move, direction);
////X
//move = ConvertForceToMove(forceSensorDirection.X);
//if (move < 0)
//{
// direction = CameraURCoordinateTranslator.Direction.Left;
// move = Math.Abs(move);
//}
//else
//{
// direction = CameraURCoordinateTranslator.Direction.Right;
//}
//Point3D returnPointXZ = App.Current.CoordinateTranslator.GetPointRelativeToolRoation(returnPointZ, lookDirection, move, direction);
////Y
//move = ConvertForceToMove(forceSensorDirection.Y);
//if (move < 0)
//{
// direction = CameraURCoordinateTranslator.Direction.Down;
// move = Math.Abs(move);
//}
//else
//{
// direction = CameraURCoordinateTranslator.Direction.Up;
//}
//Point3D returnPointXYZ = App.Current.CoordinateTranslator.GetPointRelativeToolRoation(returnPointXZ, lookDirection, move, direction);
//return returnPointXYZ;
return(returnPointZ);
}
