本文整理汇总了C#中IPath.AdvancePoint方法的典型用法代码示例。如果您正苦于以下问题：C# IPath.AdvancePoint方法的具体用法？C# IPath.AdvancePoint怎么用？C# IPath.AdvancePoint使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类IPath的用法示例。
在下文中一共展示了IPath.AdvancePoint方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C#代码示例。
示例1: UpdatePath
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0 || pose == null)
return;
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X) / 2, (gap.P0.Y + gap.P1.Y) / 2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X) + Math.PI / 2;
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta), Math.Sin(theta)));
Vector2 goalPointGlobal = ((goal1.X - lookAhead.pt.X) * (goal1.X - lookAhead.pt.X) + (goal1.Y - lookAhead.pt.Y) * (goal1.Y - lookAhead.pt.Y)
< (goal2.X - lookAhead.pt.X) * (goal2.X - lookAhead.pt.X) + (goal2.Y - lookAhead.pt.Y) * (goal2.Y - lookAhead.pt.Y)) ? goal1 : goal2;
double xg = (goalPointGlobal.X - pose.x) * Math.Cos(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Sin(pose.yaw);
double yg = -(goalPointGlobal.X - pose.x) * Math.Sin(pose.yaw) + (goalPointGlobal.Y - pose.y) * Math.Cos(pose.yaw);
goalPoint = new Vector2(xg, yg);
}示例2: GenrefVWHuniformdt
private void GenrefVWHuniformdt(IPath path, double deltadist, double dt)
{
PointOnPath currentPointOnPath = path[0].ClosestPoint(currentPoint.ToVector2());
int numpoints = (int)((path.Length) / deltadist);
int n = 5; //number of additional points
xr = new double[numpoints+n];
yr = new double[numpoints+n];
double starttimestamp = (DateTime.Now.Ticks - 621355968000000000) / 10000000;
timestamps = new double[numpoints];
for (int i = 0; i < timestamps.Length; i++)
{
timestamps[i] = starttimestamp + i * dt;
}
for (int i = 0; i <= numpoints+3; i++)
{
double d = deltadist*(i);
PointOnPath lookaheadpointi = path.AdvancePoint(currentPointOnPath, ref d);
xr[i] = lookaheadpointi.pt.X;
yr[i] = lookaheadpointi.pt.Y;
}
double[] xrdot = new double[xr.Length - 1];
double[] yrdot = new double[xrdot.Length];
double[] xr2dot = new double[xrdot.Length - 1];
double[] yr2dot = new double[xr2dot.Length];
double[] vr = new double[xr.Length];
double[] wr = new double[xr.Length];
double[] hr = new double[xr.Length];
for (int i = 0; i < xr.Length - 1; i++)
{
xrdot[i] = (xr[i + 1] - xr[i]) / dt;
yrdot[i] = (yr[i + 1] - yr[i]) / dt;
}
for (int i = 0; i < xrdot.Length - 1; i++)
{
xr2dot[i] = (xrdot[i + 1] - xrdot[i]) / dt;
yr2dot[i] = (yrdot[i + 1] - yrdot[i]) / dt;
}
for (int i = 0; i < xrdot.Length; i++)
{
vr[i] = Math.Sqrt(Math.Pow(xrdot[i], 2) + Math.Pow(yrdot[i], 2));
hr[i] = Math.Atan2(yrdot[i], xrdot[i]); // in radians
// unwrap headings
if (i > 0)
{
while (hr[i] - hr[i - 1] >= Math.PI)
{
hr[i] -= 2 * Math.PI; ;
}
while (hr[i] - hr[i - 1] <= -Math.PI)
{
hr[i] += 2 * Math.PI;
}
}
if (i < xr2dot.Length)
{
wr[i] = ((xrdot[i] * yr2dot[i] - yrdot[i] * xr2dot[i]) / (Math.Pow(vr[i], 2))); // in radians/sec
}
}
//pad the reference vectors
for (int i = 1; i < n; i++)
{
wr[numpoints + i] = 0;
vr[numpoints + i] = 0;
hr[numpoints + i] = 0;
}
TextWriter tw = new StreamWriter("parametrization.txt");
for (int i = 0; i < timestamps.Length; i++)
{
tw.WriteLine("index timestamp x y vr wr hr");
tw.WriteLine("{0} {1} {2} {3} {4} {5} {6}",i,timestamps[i],xr[i],yr[i],vr[i],wr[i],hr[i]);
}
tw.Close();
}示例3: UpdatePath
public void UpdatePath(IPath path, LineSegment gap)
{
if (path == null || path.Count == 0) return;
/*double dist = 0.45;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 goalpointGlobal = lookAhead.pt;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);*/
double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
Vector2 mean = new Vector2((gap.P0.X + gap.P1.X)/2, (gap.P0.Y + gap.P1.Y)/2);
double theta = Math.Atan2(gap.P0.Y - gap.P1.Y, gap.P0.X - gap.P1.X);
Vector2 goal1 = mean + 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goal2 = mean - 0.25 * (new Vector2(Math.Cos(theta + Math.PI / 2), Math.Sin(theta + Math.PI / 2)));
Vector2 goalpointGlobal = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;
double xg = (goalpointGlobal.X - currentPoint.x) * Math.Cos(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Sin(currentPoint.yaw);
double yg = -(goalpointGlobal.X - currentPoint.x) * Math.Sin(currentPoint.yaw) + (goalpointGlobal.Y - currentPoint.y) * Math.Cos(currentPoint.yaw);
goalpoint = new Vector2(xg, yg);
/*double dist = 1.5;
PointOnPath lookAhead = path.AdvancePoint(path.StartPoint, ref dist);
double goal1Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * 0.25;
double goal1X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * 0.25;
Vector2 goal1 = new Vector2(goal1X, goal1Y);
double goal2Y = (gap.P0.Y + gap.P1.Y) / 2 + (gap.P0.X - gap.P1.X) / (gap.Length) * -0.25;
double goal2X = (gap.P0.X + gap.P1.X) / 2 + (gap.P0.Y - gap.P1.Y) / (gap.Length) * -0.25;
Vector2 goal2 = new Vector2(goal2X, goal2Y);
goalpoint = (goal1.DistanceTo(lookAhead.pt) < goal2.DistanceTo(lookAhead.pt)) ? goal1 : goal2;*/
}