void DrawCurve(Bezier3D bezier, Quaternion rot,
RoutePoint pt0, RoutePoint pt1)
{
var p0 = pt0.transform.position;
var p1 = p0 + rot * pt0.next_weight_point;
var p3 = pt1.transform.position;
var p2 = p3 + rot * pt1.prev_weight_point;
bezier.Set(p0, p1, p2, p3);
float iv = 1 / ((p0 - p3).magnitude / (step_length * 2));
if (iv < 0.001f)
{
//间隔太大
iv = 0.001f;
}
Gizmos.color = pt0.color;
if ((pt0.delta_next.magnitude == 0 && pt1.delta_next.magnitude == 0))
{
Gizmos.DrawLine(p0, p3);
}
else
{
int arrow = 0;
float t = 0;
Vector3 lp = p0;
Color dc = pt1.color - pt0.color;
do
{
t += iv;
if (t > 1.0f)
{
t = 1.0f;
}
Vector3 p = bezier.Get(t);
Gizmos.color = pt0.color + dc * t;
Gizmos.DrawLine(lp, p);
if (arrow++ % 2 == 0)
{
Vector3 tangent = bezier.GetTangent(t);
var angle = RouteMath.GetPolarEular(tangent);
var quat = Quaternion.AngleAxis(angle, RouteMath.kAxisY);
Vector3 v0 = quat * RouteMath.kAxisX;
Vector3 axis = Vector3.Cross(p - lp, v0);
v0 *= 0.3f;
Vector3 v1 = Quaternion.AngleAxis(-70, axis) * v0;
Gizmos.DrawLine(p, p - v1);
v1 = Quaternion.AngleAxis(-110, axis) * v0;
Gizmos.DrawLine(p, p - v1);
}
lp = p;
} while(t < 1.0f);
}
}
float RefreshLength(Bezier3D bezier, Quaternion rot, RoutePoint pt0, RoutePoint pt1)
{
var length = 0.0f;
var p0 = pt0.transform.position;
var p1 = p0 + rot * pt0.next_weight_point;
var p3 = pt1.transform.position;
var p2 = p3 + rot * pt1.prev_weight_point;
bezier.Set(p0, p1, p2, p3);
float microstep = step_length / kStepParts;
float step = 0;
pt0.steps_ = new float[0];
float t = 0;
Vector3 lp = p0;
do
{
t += microstep;
if (t > 1.0f)
{
t = 1.0f;
}
Vector3 p = bezier.Get(t);
var l = (lp - p).magnitude;
step += l;
if (step >= step_length)
{
UnityEditor.ArrayUtility.Add(ref pt0.steps_, t);
step -= step_length;
}
length += l;
lp = p;
} while(t < 1.0f);
if (pt0.velocity > 0 && length / pt0.velocity < 0.001f)
{
pt0.steps_ = new float[0];
}
return(length);
}