public void ReadFromStream(Unity.SnapshotDebugger.Buffer buffer)
{
startTransform = buffer.ReadAffineTransform();
controlPoints = buffer.ReadNativeArray <float3>(out allocator);
navParams = buffer.ReadBlittable <NavigationParams>();
pathSpline.ReadFromStream(buffer);
nextControlPoint = buffer.Read32();
isBuilt = buffer.ReadBlittable <BlittableBool>();
outputTrajectory = buffer.ReadBlittable <DebugIdentifier>();
}
public static NavigationPath Create(float3[] controlPoints, AffineTransform startTransform, NavigationParams navParams, Allocator allocator)
{
return(new NavigationPath()
{
startTransform = startTransform,
controlPoints = new NativeArray <float3>(controlPoints, allocator),
allocator = allocator,
navParams = navParams,
pathSpline = Spline.Create(controlPoints.Length, allocator),
nextControlPoint = 0,
isBuilt = false
});
}
public void GenerateTrajectory(ref MotionSynthesizer synthesizer, ref Trajectory trajectory)
{
if (GoalReached)
{
return;
}
Assert.IsTrue(trajectory.Length > 0);
if (trajectory.Length == 0)
{
return;
}
AffineTransform rootTransform = synthesizer.WorldRootTransform;
float maxSpeedAtCorner = navParams.desiredSpeed;
if (nextControlPoint < pathSpline.segments.Length - 1)
{
float3 curSegmentDir = pathSpline.segments[nextControlPoint].SegmentDirection;
float3 nextSegmentDir = pathSpline.segments[nextControlPoint + 1].SegmentDirection;
float alignment = math.max(math.dot(curSegmentDir, nextSegmentDir), 0.0f);
maxSpeedAtCorner = math.lerp(navParams.maxSpeedAtRightAngle, navParams.desiredSpeed, alignment);
}
int halfTrajectoryLength = trajectory.Length / 2;
float deltaTime = synthesizer.Binary.TimeHorizon / halfTrajectoryLength;
float distance = 0.0f;
float speed = math.length(synthesizer.CurrentVelocity);
float remainingDistOnSpline = pathSpline.ComputeCurveLength(nextControlPoint);
float remainingDistOnSegment = pathSpline.segments[nextControlPoint].CurveLength;
for (int index = halfTrajectoryLength; index < trajectory.Length; ++index)
{
if (remainingDistOnSpline > 0.0f)
{
// acceleration to reach desired speed
float acceleration = math.clamp((navParams.desiredSpeed - speed) / deltaTime,
-navParams.maximumDeceleration,
navParams.maximumAcceleration);
// decelerate if needed to reach maxSpeedAtCorner
float brakingDistance = 0.0f;
if (remainingDistOnSegment > 0.0f && speed > maxSpeedAtCorner)
{
brakingDistance = NavigationParams.ComputeDistanceToReachSpeed(speed, maxSpeedAtCorner, -navParams.maximumDeceleration);
if (remainingDistOnSegment <= brakingDistance)
{
acceleration = math.min(acceleration, NavigationParams.ComputeAccelerationToReachSpeed(speed, maxSpeedAtCorner, remainingDistOnSegment));
}
}
// decelerate if needed to stop when last control point is reached
brakingDistance = NavigationParams.ComputeDistanceToReachSpeed(speed, 0.0f, -navParams.maximumDeceleration);
if (remainingDistOnSpline <= brakingDistance)
{
acceleration = math.min(acceleration, NavigationParams.ComputeAccelerationToReachSpeed(speed, 0.0f, remainingDistOnSpline));
}
speed += acceleration * deltaTime;
}
else
{
speed = 0.0f;
}
float moveDist = speed * deltaTime;
remainingDistOnSegment -= moveDist;
remainingDistOnSpline -= moveDist;
distance += moveDist;
AffineTransform point = EvaluatePointAtDistance(distance);
trajectory[index] = rootTransform.inverseTimes(point);
}
synthesizer.DebugPushGroup();
synthesizer.DebugWriteUnblittableObject(ref trajectory);
outputTrajectory = trajectory.debugIdentifier;
synthesizer.DebugWriteUnblittableObject(ref this);
}
