public float Distance(HalRobotPose target)
{
var vec = target.Xyzwpr - this.Pose.Xyzwpr;
var distnace = vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2];
return((float)Math.Sqrt(distnace));
}
public bool IntersectRange(HalRobotPose start, HalRobotPose end)
{
var func = new Func <float, float, float, bool>(delegate(float s, float x, float e) { return(s <= x && x <= e); });
var flag = true;
flag &= func(start.X, this.Pose.X, end.X);
flag &= func(start.Y, this.Pose.Y, end.Y);
flag &= func(start.Z, this.Pose.Z, end.Z);
flag &= func(start.W, this.Pose.X, end.W);
flag &= func(start.P, this.Pose.X, end.P);
flag &= func(start.R, this.Pose.X, end.R);
return(flag);
}
public float HalDistanceWithCurr(HalRobotPose pose)
{
return(this.HalGetPose().Distance(pose));
}
public float HalDistanceWithCurr(HalRobotPose pose)
{
return(0);
}