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);
}
}
/// <summary>
/// A0 A1 A2 ...
/// +---------+---------+---
/// | | |
/// +---------+---------+---
/// B0 B1 B2 ...
/// </summary>
void GetABPoint(Vector3 loc, Vector3 tangent,
out Vector3 A, out Vector3 B)
{
var angle = RouteMath.GetPolarEular(tangent);
var quat = Quaternion.AngleAxis(angle, RouteMath.kAxisY);
var iquat = Quaternion.AngleAxis(angle + 180, RouteMath.kAxisY);
loc += position;
if (eular == 0)
{
A = (quat * RouteMath.kAxisX) * width / 2 + loc;
B = (iquat * RouteMath.kAxisX) * width / 2 + loc;
}
else
{
A = (quat * RouteMath.kAxisX) * width / 2;
B = (iquat * RouteMath.kAxisX) * width / 2;
A = Quaternion.AngleAxis(eular, tangent) * A + loc;
B = Quaternion.AngleAxis(eular, tangent) * B + loc;
}
}