private static void ProcessSegment(List <byte> results, Waypoint first, Waypoint second) { // -- Header -- // /* Dimensions are swapped to account for difference between * Unity and crazyflie coordinate systems */ BezierDegree xDegree = GetDegree(first, second, AxisType.Z); BezierDegree yDegree = GetDegree(first, second, AxisType.X); BezierDegree zDegree = GetDegree(first, second, AxisType.Y); BezierDegree yawDegree = BezierDegree.Constant; byte curveFormat = ByteUtils.Coalesce(yawDegree.Value(), zDegree.Value(), yDegree.Value(), xDegree.Value()); results.Add(curveFormat); short duration = GetDuration(first, second); ByteUtils.Pack(results, duration, true); // -- Body -- // PackControlPoints(results, first, second, AxisType.X, xDegree); PackControlPoints(results, first, second, AxisType.Y, yDegree); PackControlPoints(results, first, second, AxisType.Z, zDegree); }
private static void PackControlPoints(List <byte> data, Waypoint first, Waypoint second, AxisType axis, BezierDegree degree) { switch (degree) { case BezierDegree.Constant: return; case BezierDegree.Linear: { short endPosition = ConvertPosition(second.Position).Get(axis); ByteUtils.Pack(data, endPosition, true); break; } case BezierDegree.Cubic: { short startTangent = ConvertPosition(first.WorldTangent).Get(axis); short endTangent = ConvertPosition(second.InverseWorldTangent).Get(axis); short endPosition = ConvertPosition(second.Position).Get(axis); ByteUtils.Pack(data, startTangent, true); ByteUtils.Pack(data, endTangent, true); ByteUtils.Pack(data, endPosition, true); break; } } }
public static byte Value(this BezierDegree bezierDegree) { return((byte)bezierDegree); }