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