private static Vector3D GetStrokeAccel_Velocity(NearPointResult destination, double radius)
{
if (destination.DistanceSquared > radius * radius)
{
return new Vector3D(0, 0, 0);
}
Vector3D retVal;
if (destination.IsSegmentHit)
{
retVal = Math3D.LERP(destination.FullSegment[destination.SegmentFrom].Item2, destination.FullSegment[destination.SegmentTo].Item2, destination.PercentAlongSegment);
}
else
{
retVal = destination.FullSegment[destination.SegmentFrom].Item2;
}
double percent = UtilityCore.GetScaledValue(1, 0, 0, radius, Math.Sqrt(destination.DistanceSquared));
return retVal * percent;
}
private static Vector3D GetStrokeAccel_Segment(NearPointResult segment, Point3D position, double accel, double velocityInfluenceRadius, double towardRadiusMult, bool shouldAttractToward)
{
// LERP Radius
double fromRadius = segment.FullSegment[segment.SegmentFrom].Item2.Length * velocityInfluenceRadius;
double toRadius = segment.FullSegment[segment.SegmentTo].Item2.Length * velocityInfluenceRadius;
double radius = UtilityCore.GetScaledValue(fromRadius, toRadius, 0d, 1d, segment.PercentAlongSegment);
// Acceleration toward
Vector3D toward = shouldAttractToward ?
GetStrokeAccel_Toward(segment, position, accel, radius * towardRadiusMult) :
new Vector3D(0, 0, 0);
// Velocity influence
Vector3D velocity = GetStrokeAccel_Velocity(segment, radius);
return toward + velocity;
}
private static Vector3D GetStrokeAccel_Toward(NearPointResult destination, Point3D position, double accel, double radius)
{
if (destination.DistanceSquared.IsNearZero())
{
return new Vector3D(0, 0, 0);
}
// Go full acceleration directly toward
Vector3D retVal = (destination.NearestPoint - position).ToUnit(false) * accel;
if (destination.DistanceSquared < radius * radius)
{
// Influence drops to zero when really close to the point
double percent = UtilityCore.GetScaledValue(0, 1, 0, radius, Math.Sqrt(destination.DistanceSquared));
retVal *= percent;
}
return retVal;
}
private static Vector3D GetStrokeAccel_Point(NearPointResult point, Point3D position, double accel, double velocityInfluenceRadius, double towardRadiusMult)
{
double radius = point.FullSegment[point.SegmentFrom].Item2.Length * velocityInfluenceRadius;
// Acceleration toward
Vector3D toward = GetStrokeAccel_Toward(point, position, accel, radius * towardRadiusMult);
// Velocity influence
Vector3D velocity = GetStrokeAccel_Velocity(point, radius);
return toward + velocity;
}
