public void BuildSpline(float3 inPosition, float3 inTangent, NativeArray <float3> controlPoints, float3 outTangent, float curvature)
{
int pointCount = controlPoints.Length + 1;
int segmentCount = pointCount - 1;
segments[0] = HermitCurve.Create(
inPosition,
inTangent,
controlPoints[0],
segmentCount == 1 ? outTangent : (controlPoints[1] - inPosition),
curvature);
for (int i = 1; i < segmentCount; ++i)
{
float3 previousPoint = i == 1 ? inPosition : controlPoints[i - 2];
float3 segmentInTangent = controlPoints[i] - previousPoint;
float3 segmentOutTangent = i == (segmentCount - 1) ? outTangent : (controlPoints[i + 1] - controlPoints[i - 1]);
segments[i] = HermitCurve.Create(
controlPoints[i - 1],
segmentInTangent,
controlPoints[i],
segmentOutTangent,
curvature);
}
}
public bool UpdateAgentTransform(AffineTransform agentTransform)
{
if (GoalReached)
{
return(false);
}
float3 nextControlPointPos = pathSpline.segments[nextControlPoint].OutPosition;
float controlPointRadius = nextControlPoint >= NumControlPoints - 1 ? navParams.finalControlPointRadius : navParams.intermediateControlPointRadius;
if (math.distancesq(agentTransform.t, nextControlPointPos) <= controlPointRadius * controlPointRadius)
{
++nextControlPoint;
if (GoalReached)
{
return(false);
}
}
// use agent transform as first control point of the spline (starting from nextControlPoint, segments before are discarded)
HermitCurve segment = pathSpline.segments[nextControlPoint];
segment = HermitCurve.Create(
agentTransform.t,
agentTransform.Forward,
segment.OutPosition,
segment.OutTangent,
navParams.pathCurvature);
pathSpline.segments[nextControlPoint] = segment;
return(true);
}