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); }