private float CalcSegmentLengthAccurate(SplinePoint3D a, SplinePoint3D b, int approximateCount) { var length = 0f; var transform = GlobalTransform; var prevPosition = transform.TransformVector(a.Position); for (int i = 1; i < approximateCount; i++) { var currentPosition = Interpolate((float)i / approximateCount, a, b).Translation; length += (currentPosition - prevPosition).Length; prevPosition = currentPosition; } length += (transform.TransformVector(b.Position) - prevPosition).Length; return(length); }
private Matrix44 Interpolate(float amount, SplinePoint3D point1, SplinePoint3D point2) { var transform = GlobalTransform; var position1 = transform.TransformVector(point1.Position); var position2 = transform.TransformVector(point2.Position); Vector3 position, direction; if (point1.Interpolation == SplineInterpolation.Linear) { position = Mathf.Lerp(amount, position1, position2); direction = position2 - position1; } else { var tangent1 = position1 + transform.TransformNormal(point1.TangentA); var tangent2 = position2 + transform.TransformNormal(point2.TangentB); position = Mathf.BezierSpline(amount, position1, tangent1, tangent2, position2); direction = Mathf.BezierTangent(amount, position1, tangent1, tangent2, position2); } return(CalcRotationMatrix(direction, Vector3.UnitY) * Matrix44.CreateTranslation(position)); }