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);
}
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 static HermitCurve Create(float3 inPos, float3 inTangent, float3 outPos, float3 outTangent, float curvature)
{
float length = math.distance(inPos, outPos);
HermitCurve curve = new HermitCurve()
{
p0 = inPos,
p1 = outPos,
m0 = math.normalizesafe(inTangent, float3.zero) * math.min(length, curvature),
m1 = math.normalizesafe(outTangent, float3.zero) * math.min(length, curvature)
};
curve.subdivs = math.max((int)math.floor(length / SubdivLength), 3);
curve.ComputeCurveLength();
return(curve);
}