/// <summary>
/// Attracts the p to curve. Default implementation uses a binary search
/// </summary>
/// <returns>If the curve is not in radius distance, then just return p</returns>
public virtual Vector2 GetAttractedPoint(Vector2 p, float attract_radius)
{
Algebra.Del _Distance = (t) => (_GetTwodPos(t) - p).sqrMagnitude;
float close_scaled_t = Algebra.MinArg(_Distance, 0.5f, 0f, 1f);
Vector2 closest_p = _GetTwodPos(close_scaled_t);
if ((closest_p - p).sqrMagnitude <= attract_radius * attract_radius)
{
return(closest_p);
}
else
{
return(p);
}
}
public override Vector3 AttouchPoint(Vector3 p)
{
Vector2 targetP = new Vector2(p.x, p.z);
Algebra.Del deriveOfDistance = delegate(float t) {
float a2_x = P0.x - 2 * P1.x + P2.x;
float a1_x = -2 * P0.x + 2 * P1.x;
float a0_x = P0.x - targetP.x;
float a2_y = P0.y - 2 * P1.y + P2.y;
float a1_y = -2 * P0.y + 2 * P1.y;
float a0_y = P0.y - targetP.y;
float[] distParams = new float[5];
distParams[4] = Mathf.Pow(a2_x, 2f) + Mathf.Pow(a2_y, 2f);
distParams[3] = 2 * (a1_x * a2_x + a1_y * a2_y);
distParams[2] = Mathf.Pow(a1_x, 2f) + Mathf.Pow(a1_y, 2f) + 2 * (a0_x * a2_x + a0_y * a2_y);
distParams[1] = 2 * (a0_x * a1_x + a0_y * a1_y);
distParams[0] = Mathf.Pow(a0_x, 2f) + Mathf.Pow(a0_y, 2f);
return(4 * Mathf.Pow(t, 3) * distParams[4] + 3 * Mathf.Pow(t, 2) * distParams[3] + 2 * t * distParams[2] + distParams[1]);
};
Algebra.Del deriveOfDeriveOfDistance = delegate(float t){
float a2_x = P0.x - 2 * P1.x + P2.x;
float a1_x = -2 * P0.x + 2 * P1.x;
float a0_x = P0.x - targetP.x;
float a2_y = P0.y - 2 * P1.y + P2.y;
float a1_y = -2 * P0.y + 2 * P1.y;
float a0_y = P0.y - targetP.y;
float[] distParams = new float[5];
distParams[4] = Mathf.Pow(a2_x, 2f) + Mathf.Pow(a2_y, 2f);
distParams[3] = 2 * (a1_x * a2_x + a1_y * a2_y);
distParams[2] = Mathf.Pow(a1_x, 2f) + Mathf.Pow(a1_y, 2f) + 2 * (a0_x * a2_x + a0_y * a2_y);
return(4 * 3 * Mathf.Pow(t, 2) * distParams[4] + 3 * 2 * t * distParams[3] + 2 * distParams[2]);
};
float t1 = Algebra.newTown(deriveOfDistance, deriveOfDeriveOfDistance, 0f, t_start);
float t2 = Algebra.newTown(deriveOfDistance, deriveOfDeriveOfDistance, 0f, t_end);
t1 = toLocalParam(t1);
t2 = toLocalParam(t2);
List <float> candidateParams = new List <float>();
if (0 < t1 && t1 < 1)
{
candidateParams.Add(t1);
}
if (0 < t2 && t2 < 1)
{
candidateParams.Add(t2);
}
candidateParams.Add(0f);
candidateParams.Add(1f);
var sortedParams = candidateParams.OrderBy((param) => (this.at_2d(param) - targetP).magnitude);
return(this.at(sortedParams.First()));
}
