public static DubinInput LSRInput(List <Pair <Vector2, Vector2> > LRTangents, Circle agentLeft, Circle queryRight)
{
DubinInput next = new DubinInput();
DubinInput.Steer nextInput;
if (LRTangents.Count > 3)
{
double arc1, arc2, arc3;
// L
arc1 = ArcLength(agentLeft.pos, start.pos, LRTangents[3].first, r1, true);
nextInput = new DubinInput.Steer(start.v, (float)o1, (float)(arc1 / start.v));
next.steer.Add(nextInput);
// S
arc2 = (LRTangents[3].first - LRTangents[3].second).magnitude;
nextInput = new DubinInput.Steer(MAX_SPEED, 0, (float)(arc2 / MAX_SPEED));
if (constAcc)
{
var at = computeAT(arc2, start.v, goal.v);
nextInput = new DubinInput.Steer(start.v, 0, (float)at.second, (float)at.first);
}
next.steer.Add(nextInput);
// R
arc3 = ArcLength(queryRight.pos, LRTangents[3].second, goal.pos, r2, false);
nextInput = new DubinInput.Steer(goal.v, (float)-o2, (float)(arc3 / goal.v));
next.steer.Add(nextInput);
next.updateTime();
}
return(next);
}
public static DubinInput bestCSCInput(Circle agentLeft, Circle agentRight, Circle queryLeft, Circle queryRight)
{
List <Pair <Vector2, Vector2> > RRTangents = TangentLines(agentRight, queryRight);
List <Pair <Vector2, Vector2> > LLTangents = TangentLines(agentLeft, queryLeft);
List <Pair <Vector2, Vector2> > RLTangents = TangentLines(agentRight, queryLeft);
List <Pair <Vector2, Vector2> > LRTangents = TangentLines(agentLeft, queryRight);
DubinInput best = new DubinInput(), next;
next = RSRInput(RRTangents, agentRight, queryRight);
if (next.time < best.time)
{
best = next;
}
next = LSLInput(LLTangents, agentLeft, queryLeft);
if (next.time < best.time)
{
best = next;
}
next = RSLInput(RLTangents, agentRight, queryLeft);
if (next.time < best.time)
{
best = next;
}
next = LSRInput(LRTangents, agentLeft, queryRight);
if (next.time < best.time)
{
best = next;
}
return(best);
}
bool pathFeasible(DubinNode from, DubinNode to, DubinInput input)
{
// TODO: check for max condition
foreach (DubinInput.Steer steer in input.steer)
{
if (Mathf.Abs(steer.a) > MAX_ACCEL)
{
return(false);
}
}
var dubinsCollision = DDrive.dubinsTransition(true, null, from, to, input);
return(dubinsCollision);
}
public static DubinInput DubinSP(DubinState start, DubinState goal, bool constAcc = false)
{
DubinUtils.constAcc = constAcc;
DubinUtils.start = start;
DubinUtils.goal = goal;
DubinUtils.o1 = start.omega;
DubinUtils.o2 = goal.omega;
DubinUtils.r1 = start.v / o1;
DubinUtils.r2 = goal.v / o2;
Circle agentLeft;
Circle agentRight;
Circle queryLeft;
Circle queryRight;
double theta = start.theta;
theta += Math.PI / 2.0;
if (theta > Math.PI)
{
theta -= 2.0 * Math.PI;
}
agentLeft = new Circle(start.pos.x + r1 * Math.Cos(theta), start.pos.y + r1 * Math.Sin(theta), r1);
theta = start.theta;
theta -= Math.PI / 2.0;
if (theta < -Math.PI)
{
theta += 2.0 * Math.PI;
}
agentRight = new Circle(start.pos.x + r1 * Math.Cos(theta), start.pos.y + r1 * Math.Sin(theta), r1);
theta = goal.theta;
theta += Math.PI / 2.0;
if (theta > Math.PI)
{
theta -= 2.0 * Math.PI;
}
queryLeft = new Circle(goal.pos.x + r2 * Math.Cos(theta), goal.pos.y + r2 * Math.Sin(theta), r2);
theta = goal.theta;
theta -= Math.PI / 2.0;
if (theta < -Math.PI)
{
theta += 2.0 * Math.PI;
}
queryRight = new Circle(goal.pos.x + r2 * Math.Cos(theta), goal.pos.y + r2 * Math.Sin(theta), r2);
DubinInput bestCSC = bestCSCInput(agentLeft, agentRight, queryLeft, queryRight);
// DubinInput bestCCC = bestCCCInput(agentLeft, agentRight, queryLeft, queryRight);
// DubinInput shortest = bestCCC.time > bestCSC.time ? bestCSC : bestCCC;
DubinInput shortest = bestCSC;
return(shortest);
}
public static DubinInput bestCCCInput(Circle agentLeft, Circle agentRight, Circle queryLeft, Circle queryRight)
{
DubinInput best = new DubinInput();
return(best);
}
/*
* dubins transition function, multiple uses
* 1. if collision param is true: check for collision, will return bool
* 2. else: put interpolated nodes into list
*/
public static bool dubinsTransition(bool collision, List <Node> list, DubinNode from, DubinNode to, DubinInput input)
{
DubinNode prev = from;
double tx = from.pos.x, ty = from.pos.y, ttheta = from.theta; // HACK: to improve precision
foreach (DubinInput.Steer steer in input.steer)
{
float sumT = 0;
double v = steer.v;
while (sumT < steer.time)
{
var dt = Mathf.Min(DELTA_T, steer.time);
tx = tx + dt * v * Math.Cos(prev.theta);
ty = ty + dt * v * Math.Sin(prev.theta);
var pos = new Vector2((float)tx, (float)ty);
v += dt * steer.a;
ttheta = ttheta + dt * steer.omega;
while (ttheta > 2 * Mathf.PI)
{
ttheta -= 2 * Mathf.PI;
}
while (ttheta < 0)
{
ttheta += 2 * Mathf.PI;
}
if (collision && !Cheetah.instance.IsValid(pos))
{
return(false);
}
DubinNode nextPoint = new DubinNode(pos, steer.v, (float)ttheta, steer.omega, prev.time + dt);
if (!collision)
{
list.Add(nextPoint);
}
// if ((nextPoint.pos - prev.pos).magnitude > 0.5) // TODO: debug
// {
// Debug.Log("WTF");
// dubinInput(from, to);
// return false;
// }
prev = nextPoint;
sumT += dt;
}
}
// // TODO: debug
// if ((prev.pos - to.pos).magnitude > 0.5 || Mathf.Abs(prev.theta - to.theta) > 0.3)
// {
// Debug.Log("WTF");
// dubinInput(from, to);
// return false;
// }
return(true);
}
