// Update is called once per frame
void Update()
{
if (isMoving)
{
// How long has it been since we last updated our position
float distanceIncrement = this.moveRate * Time.deltaTime;
// find out how far we should move
// convert to T value 0...1
this.currentDistanceAlongDubinPath += distanceIncrement;
if (currentDistanceAlongDubinPath >= finalDistanceAlongDubinPath)
{
distanceIncrement = currentDistanceAlongDubinPath - finalDistanceAlongDubinPath;
currentDistanceAlongDubinPath = finalDistanceAlongDubinPath;
isMoving = false;
}
else
{
this.CurrentMoveDistance -= distanceIncrement;
}
this.CurrentMoveDistance = Mathf.Max(0.0f, CurrentMoveDistance);
DubinCSC currentDubin = CurrentPath.path[this.currentDubinIdx];
float relativeDistance = currentDistanceAlongDubinPath - runningDubinDistanceTotal;
if (relativeDistance > currentDubin.totalLength)
{
this.runningDubinDistanceTotal += currentDubin.totalLength;
relativeDistance -= currentDubin.totalLength;
currentDubinIdx++;
currentDubin = this.currentPath.path[currentDubinIdx];
}
Vector3 nextPos;
Vector3 nextDir;
currentDubin.GetPositionAndHeadingFromDistance(relativeDistance, this.currentPath.turnRadius, out nextPos, out nextDir);
// move to that spot.
this.transform.position = nextPos;
this.transform.forward = this.CurrentPath.isReverse ? -nextDir : nextDir;
gameEventSystem.RaiseEvent(GameEventType.ObjectPropertyChanged, this.gameObject, this.gameObject);
// TODO:
// Do something with tile grid?
// Do we care for in motion stuff, or just the end position?
if (!isMoving)
{
////TODO: Do we really want to clear the entire, or just from the last point?
//// in which case, we'd need to clip the dubin, basicaly....or repath
//this.CurrentPath.path.Clear();
this.CurrentPath.Clip(currentDistanceAlongDubinPath);
gameEventSystem.RaiseEvent(GameEventType.PathChanged, this.gameObject, CurrentPath);
gameEventSystem.RaiseEvent(GameEventType.MoveEnded, this.gameObject, CurrentPath);
}
}
}
public void Clip(float startDistance)
{
// find the new starting dubin:
float pathDistance = 0.0f;
DubinCSC newStartDubin = DubinCSC.NullDubin;
int startIdx = -1;
// TODO make this into a function, though it's an awkward one
// Figure out which Dubin in the list we should start on
for (int i = 0; i < path.Count; ++i)
{
pathDistance += path[i].totalLength;
if (startDistance < pathDistance)
{
newStartDubin = path[i];
startIdx = i;
pathDistance -= path[i].totalLength;
break; //Found our dubin
}
}
if (startIdx == -1 || MyMath.IsNearZero(pathDistance))
{
this.path.Clear();
}
else
{
float relativeDistanceStart = startDistance - pathDistance;
newStartDubin.Clip(relativeDistanceStart, turnRadius);
// remove all previous dubins
this.path.RemoveRange(0, startIdx);
}
}
// Generates four dubins
static public DubinCSC FindDubins(DirectionalCircle orgLeft,
DirectionalCircle orgRight,
DirectionalCircle dstLeft,
DirectionalCircle dstRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius,
DubinCSC[] dubins)
{
dubins = new DubinCSC[4];
// RSR
///////
DubinCSC shortestDubins = dubins[0] = DubinCSC.RSRCourse(orgRight, dstRight, ptStart, ptDest, turnRadius);
// LSL
////////
dubins[1] = DubinCSC.LSLCourse(orgLeft, dstLeft, ptStart, ptDest, turnRadius);
shortestDubins = DubinCSC.TakeShortest(shortestDubins, dubins[1]);
// RSL
////////
dubins[2] = DubinCSC.RSLCourse(orgRight, dstLeft, ptStart, ptDest, turnRadius);
shortestDubins = DubinCSC.TakeShortest(shortestDubins, dubins[2]);
// LSR
///////
dubins[3] = DubinCSC.LSRCourse(orgLeft, dstRight, ptStart, ptDest, turnRadius);
shortestDubins = DubinCSC.TakeShortest(shortestDubins, dubins[3]);
return(shortestDubins);
}
static public DubinCSC LSRCourse(
DirectionalCircle orgLeft,
DirectionalCircle dstRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
float x1 = orgLeft.center.x;
float y1 = orgLeft.center.y;
float x2 = dstRight.center.x;
float y2 = dstRight.center.y;
Vector2 v = dstRight.center - orgLeft.center;
float dSqr = v.sqrMagnitude;
if (dSqr < turnRadius)
{
// Circles overlap
return(NullDubin);
}
float d = Mathf.Sqrt(dSqr);
v /= d;
// For Sign1 = -1
float c = (turnRadius + turnRadius) / d;
if (c * c > 1.0f)
{
return(NullDubin);
}
float h = Mathf.Sqrt(1.0f - c * c);
// Sign1 = -1, Sign2 = -1
float nx = (v.x * c + h * v.y) * turnRadius;
float ny = (v.y * c - h * v.x) * turnRadius;
DubinCSC course = new DubinCSC();
course.line = (new Line2D()
{
start = new Vector2(x1 + nx, y1 + ny),
end = new Vector2(x2 - nx, y2 - ny)
});
course.sLength = d;
course.startArc = new Arc(orgLeft, orgLeft.startTheta, course.line.start, turnRadius);
course.endArc = new Arc(dstRight, course.line.end, ptDest, turnRadius);
course.totalLength = course.sLength + course.startArc.length + course.endArc.length;
return(course);
}
static public DubinCSC RSRCourse(
DirectionalCircle orgRight,
DirectionalCircle dstRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
// Compute tangent line
float x1 = orgRight.center.x;
float y1 = orgRight.center.y;
float x2 = dstRight.center.x;
float y2 = dstRight.center.y;
Vector2 v = dstRight.center - orgRight.center;
float dSqr = v.sqrMagnitude;
if (dSqr < turnRadius)
{
// Circles overlap
return(NullDubin);
}
float d = Mathf.Sqrt(dSqr);
v /= d; // note to self, i believe this is overkill, and just there for circles of different radius, normal doesn't need to be computed.
// and i could get away with a simple x1,y1 + -vy,vx
// And I might be able to get away with checking each individial arc/line/arc distance separately
// Sign1 = 1, Sign2 = 1
float nx = /*v.x * c*/ -v.y;
float ny = /*v.y * c*/ +v.x;
nx *= turnRadius;
ny *= turnRadius;
DubinCSC course = new DubinCSC();
course.line = new Line2D()
{
start = new Vector2(x1 + nx, y1 + ny),
end = new Vector2(x2 + nx, y2 + ny)
};
course.sLength = d;
course.startArc = new Arc(orgRight, orgRight.startTheta, course.line.start, turnRadius);
course.endArc = new Arc(dstRight, course.line.end, ptDest, turnRadius);
course.totalLength = course.sLength + course.startArc.length + course.endArc.length;
return(course);
}
static public DubinCSC LSLCourse(
DirectionalCircle orgLeft,
DirectionalCircle dstLeft,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
float x1 = orgLeft.center.x;
float y1 = orgLeft.center.y;
float x2 = dstLeft.center.x;
float y2 = dstLeft.center.y;
Vector2 v = dstLeft.center - orgLeft.center;
float dSqr = v.sqrMagnitude;
if (dSqr < turnRadius)
{
// Circles overlap
return(NullDubin);
}
float d = Mathf.Sqrt(dSqr);
v /= d;
float nx = /*v.x * c*/ -v.y;
float ny = /*v.y * c*/ +v.x;
// Sign1 = 1, Sign2 = -1
nx = -nx * turnRadius;
ny = -ny * turnRadius;
DubinCSC course = new DubinCSC();
course.line = (new Line2D()
{
start = new Vector2(x1 + nx, y1 + ny),
end = new Vector2(x2 + nx, y2 + ny)
});
course.sLength = d;
course.startArc = new Arc(orgLeft, orgLeft.startTheta, course.line.start, turnRadius);
course.endArc = new Arc(dstLeft, course.line.end, ptDest, turnRadius);
course.totalLength = course.sLength + course.startArc.length + course.endArc.length;
return(course);
}
static public DubinCSC Translate(DubinCSC dubin, Vector2 transVec)
{
// Lengths remain the same, just the positions change
DubinCSC transDubin = new DubinCSC()
{
startArc = dubin.startArc,
line = dubin.line,
endArc = dubin.endArc,
sLength = dubin.sLength,
totalLength = dubin.totalLength
};
transDubin.line.start += transVec;
transDubin.line.end += transVec;
transDubin.startArc.circle.center += transVec;
transDubin.endArc.circle.center += transVec;
return(transDubin);
}
public void GetEndDirAndPos(out Vector3 dir, out Vector3 pos)
{
DubinCSC lastDubin = path[path.Count - 1];
if (isDirectional)
{
lastDubin.endArc.GetPositionAndHeadingFromDistance(
lastDubin.endArc.length,
turnRadius,
out pos,
out dir);
}
else
{
Vector2 normDir2D = (lastDubin.line.end - lastDubin.line.start).normalized;
dir = new Vector3(normDir2D.x, 0.0f, normDir2D.y);
pos = new Vector3(lastDubin.line.end.x, 0.0f, lastDubin.line.end.y);
}
if (isReverse)
{
dir = -dir;
}
}
static public DubinCSC FindDubin(DirectionalCircle orgLeft,
DirectionalCircle orgRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius,
DirectionalCircle dstLeft,
DirectionalCircle dstRight)
{
// RSR
///////
DubinCSC shortestDubins = DubinCSC.RSRCourse(orgRight, dstRight, ptStart, ptDest, turnRadius);
// LSL
////////
shortestDubins = DubinCSC.TakeShortest(shortestDubins, DubinCSC.LSLCourse(orgLeft, dstLeft, ptStart, ptDest, turnRadius));
// RSL
////////
shortestDubins = DubinCSC.TakeShortest(shortestDubins, DubinCSC.RSLCourse(orgRight, dstLeft, ptStart, ptDest, turnRadius));
// LSR
///////
return(DubinCSC.TakeShortest(shortestDubins, DubinCSC.LSRCourse(orgLeft, dstRight, ptStart, ptDest, turnRadius)));
}
public List <HeadingPositionPair> AsPointsList(LineRenderArgs args, float distInc)
{
Vector3 nextPos = Vector3.zero;
Vector3 nextDir;
int dubinIndex = path.Count - 1;
float totalLength = this.CalculateTotalDistance();
float pathDistance = 0.0f;
DubinCSC currentDubin = path[path.Count - 1];
// Figure out which Dubin in the list we should start on
for (int i = 0; i < path.Count; ++i)
{
pathDistance += path[i].totalLength;
if (args.StartDistance <= pathDistance)
{
currentDubin = path[i];
dubinIndex = i;
pathDistance -= path[i].totalLength;
break; //Found our dubin
}
}
// const int numSegmentsPerDubin = 100;
// int totalNumSegments = numSegmentsPerDubin * (path.Count - dubinIndex);
//float distInc = Mathf.Ceil(currentDubin.totalLength / numSegmentsPerDubin);
int numPairs = (int)(totalLength / distInc);
List <HeadingPositionPair> pathLines = new List <HeadingPositionPair>(numPairs);
for (float distance = args.StartDistance; distance < args.EndDistance;)
{
float relativeDistanceStart = distance - pathDistance;
float relativeDistanceEnd = Mathf.Min(currentDubin.totalLength, args.EndDistance - pathDistance);
//float distInc = Mathf.Ceil(currentDubin.totalLength / numSegmentsPerDubin);
for (float relativeDistance = relativeDistanceStart;
relativeDistance <= relativeDistanceEnd;
relativeDistance += distInc)
{
currentDubin.GetPositionAndHeadingFromDistance(relativeDistance, turnRadius, out nextPos, out nextDir);
pathLines.Add(new HeadingPositionPair(nextPos, nextDir));
}
// ensure that the last spot is included
currentDubin.GetPositionAndHeadingFromDistance(relativeDistanceEnd, turnRadius, out nextPos, out nextDir);
pathLines.Add(new HeadingPositionPair(nextPos, nextDir));
distance += currentDubin.totalLength;
pathDistance += currentDubin.totalLength;
dubinIndex++;
if (dubinIndex >= path.Count)
{
break;
}
currentDubin = path[dubinIndex];
}
return(pathLines);
}
static public DubinCSC FindDegenerateDubins(
DirectionalCircle orgLeft,
DirectionalCircle orgRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius)
{
Vector2 vLeft = orgLeft.center - ptDest;
Vector2 vRight = orgRight.center - ptDest;
DubinCSC dubinLS = new DubinCSC();
DubinCSC dubinRS = new DubinCSC();
float rSq = turnRadius * turnRadius;
if (rSq < vLeft.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vLeft.magnitude);
float b = Mathf.Atan2(vLeft.y, vLeft.x);
float t = b + a;
Vector2 Q = new Vector2(orgLeft.center.x + turnRadius * -Mathf.Sin(t),
orgLeft.center.y + turnRadius * Mathf.Cos(t));
// TODO: I think I'm doing one too many Atan2s here....
dubinLS.startArc = new Arc(orgLeft, orgLeft.startTheta, Q, turnRadius);
dubinLS.line.start = Q;
dubinLS.line.end = ptDest;
dubinLS.sLength = (dubinLS.line.end - dubinLS.line.start).magnitude;
dubinLS.endArc.startTheta = 0.0f;
dubinLS.endArc.endTheta = 0.0f;
dubinLS.endArc.length = 0.0f;
dubinLS.totalLength = dubinLS.startArc.length + dubinLS.sLength;
}
else
{
dubinLS.totalLength = NullDubin.totalLength;
}
if (rSq < vRight.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vRight.magnitude);
float b = Mathf.Atan2(vRight.y, vRight.x);
float t = b - a;
Vector2 Q = new Vector2(orgRight.center.x + turnRadius * Mathf.Sin(t),
orgRight.center.y + turnRadius * -Mathf.Cos(t));
dubinRS.startArc = new Arc(orgRight, orgRight.startTheta, Q, turnRadius);
dubinRS.line.start = Q;
dubinRS.line.end = ptDest;
dubinRS.sLength = (dubinRS.line.end - dubinRS.line.start).magnitude;
dubinRS.endArc.startTheta = 0.0f;
dubinRS.endArc.endTheta = 0.0f;
dubinRS.endArc.length = 0.0f;
dubinRS.totalLength = dubinRS.startArc.length + dubinRS.sLength;
}
else
{
dubinRS.totalLength = NullDubin.totalLength;
}
return(DubinCSC.TakeShortest(dubinLS, dubinRS));
}
// note: This form doesn't care about ending direction, just tries to get to the point
// Generates two dubins
static public DubinCSC FindDegenerateDubins(
DirectionalCircle orgLeft,
DirectionalCircle orgRight,
Vector2 ptStart,
Vector2 ptDest,
float turnRadius,
DubinCSC[] dubins)
{
Vector2 vLeft = orgLeft.center - ptDest; // lines from center of circle to destination
Vector2 vRight = orgRight.center - ptDest;
DubinCSC dubinLS = NullDubin;
DubinCSC dubinRS = NullDubin;
float rSq = turnRadius * turnRadius; // this could probably be pulled out to a member or argument
if (rSq < vLeft.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vLeft.magnitude); // vLeft.Mag = h
float b = Mathf.Atan2(vLeft.y, vLeft.x);
float t = b + a;
Vector2 Q = new Vector2(orgLeft.center.x + turnRadius * -Mathf.Sin(t),
orgLeft.center.y + turnRadius * Mathf.Cos(t));
dubinLS = new DubinCSC();
dubinLS.startArc = new Arc(orgLeft, orgLeft.startTheta, Q, turnRadius);
dubinLS.line.start = Q;
dubinLS.line.end = ptDest;
dubinLS.sLength = (dubinLS.line.end - dubinLS.line.start).magnitude;
dubinLS.endArc.startTheta = 0.0f;
dubinLS.endArc.endTheta = 0.0f;
dubinLS.endArc.length = 0.0f;
dubinLS.totalLength = dubinLS.startArc.length + dubinLS.sLength;
}
if (rSq < vRight.sqrMagnitude)
{
float a = Mathf.Asin(turnRadius / vRight.magnitude);
float b = Mathf.Atan2(vRight.y, vRight.x);
float t = b - a;
Vector2 Q = new Vector2(orgRight.center.x + turnRadius * Mathf.Sin(t),
orgRight.center.y + turnRadius * -Mathf.Cos(t));
dubinRS = new DubinCSC();
dubinRS.startArc = new Arc(orgRight, orgRight.startTheta, Q, turnRadius);
dubinRS.line.start = Q;
dubinRS.line.end = ptDest;
dubinRS.sLength = (dubinRS.line.end - dubinRS.line.start).magnitude;
dubinRS.endArc.startTheta = 0.0f;
dubinRS.endArc.endTheta = 0.0f;
dubinRS.endArc.length = 0.0f;
dubinRS.totalLength = dubinRS.startArc.length + dubinRS.sLength;
}
DubinCSC shortDubin = DubinCSC.TakeShortest(dubinLS, dubinRS);
dubins[0] = dubinLS;
dubins[1] = dubinRS;
return(shortDubin);
}
public void DrawDubin(AccessibleLineRenderer lineRenderer, LineRenderArgs args)
{
if (path == null || path.Count == 0)
{
//TEMP
lineRenderer.enabled = false;
return;
}
lineRenderer.UnderlyingLineRenderer.SetWidth(args.LineWidth, args.LineWidth);
//TEMP
if (lineRenderer.enabled == false)
{
lineRenderer.enabled = true;
}
Vector3 nextPos = Vector3.zero;
Vector3 nextDir;
int dubinIndex = path.Count - 1;
float totalLength = this.CalculateTotalDistance();
float pathDistance = 0.0f;
DubinCSC currentDubin = path[path.Count - 1];
// Figure out which Dubin in the list we should start on
for (int i = 0; i < path.Count; ++i)
{
pathDistance += path[i].totalLength;
if (args.StartDistance <= pathDistance)
{
currentDubin = path[i];
dubinIndex = i;
pathDistance -= path[i].totalLength;
break; //Found our dubin
}
}
const int numSegmentsPerDubin = 100;
int totalNumSegments = numSegmentsPerDubin * (path.Count - dubinIndex);
lineRenderer.SetVertexCount(totalNumSegments); //hackery
int idx = totalNumSegments - 1;
for (float distance = args.StartDistance; distance < args.EndDistance;)
{
float relativeDistanceStart = distance - pathDistance;
float relativeDistanceEnd = Mathf.Min(currentDubin.totalLength, args.EndDistance - pathDistance);
float distInc = Mathf.Ceil(currentDubin.totalLength / numSegmentsPerDubin);
for (float relativeDistance = relativeDistanceStart;
relativeDistance <= relativeDistanceEnd;
relativeDistance += distInc)
{
currentDubin.GetPositionAndHeadingFromDistance(relativeDistance, turnRadius, out nextPos, out nextDir);
nextPos.y += 1.0f;
lineRenderer.SetPosition(idx--, nextPos);
}
// ensure that the last spot is included
currentDubin.GetPositionAndHeadingFromDistance(relativeDistanceEnd, turnRadius, out nextPos, out nextDir);
nextPos.y += 1.0f;
lineRenderer.SetPosition(idx, nextPos);
distance += currentDubin.totalLength;
pathDistance += currentDubin.totalLength;
dubinIndex++;
if (dubinIndex >= path.Count)
{
break;
}
currentDubin = path[dubinIndex];
}
int badMath = 0;
//for (int i = idx; i < totalNumSegments; i++)
for (int i = idx; i >= 0; --i)
{
badMath++;
lineRenderer.SetPosition(i, nextPos);
}
// Debug.Log("BadMath: " + badMath);
}
public static DubinCSC TakeShortest(DubinCSC du1, DubinCSC du2)
{
return(du1.totalLength > du2.totalLength ? du2 : du1);
}
