public static void canardEncodeScalar <T>(byte[] destination,
uint bit_offset,
byte bit_length,
T value)
{
union storage = new union(false);
Byte std_byte_length = 0;
// Extra most significant bits can be safely ignored here.
if (bit_length == 1)
{
std_byte_length = sizeof(bool);
storage.boolean = (bool)(dynamic)value;
}
else if (bit_length <= 8)
{
std_byte_length = 1;
storage.u8 = ((Byte)(dynamic)value);
}
else if (bit_length <= 16)
{
std_byte_length = 2;
storage.u16 = ((UInt16)(dynamic)value);
}
else if (bit_length <= 32)
{
if (value is float)
{
std_byte_length = 4;
storage.f32 = ((float)(dynamic)value);
}
else
{
std_byte_length = 4;
storage.u32 = ((UInt32)(dynamic)value);
}
}
else if (bit_length <= 64)
{
std_byte_length = 8;
storage.u64 = ((UInt64)(dynamic)value);
}
/*
* The bit copy algorithm assumes that more significant bits have lower index, so we need to shift some.
* Extra least significant bits will be filled with zeroes, which is fine.
* Extra most significant bits will be discarded here.
* Coverity Scan mistakenly believes that the array may be overrun if bit_length == 64; however, this branch will
* not be taken if bit_length == 64, because 64 % 8 == 0.
*/
if ((bit_length % 8) != 0)
{
// coverity[overrun-local]
storage[bit_length / 8] = (byte)(storage.bytes[bit_length / 8] << ((8 - (bit_length % 8)) & 7));
}
copyBitArray(storage.bytes.ToArray(), 0, bit_length, destination, bit_offset);
}
static extern int Blah(
[MarshalAs(UnmanagedType.I4)]
int i,
string s,
union u,
sparse sp,
[In, Out] string inout,
StringBuilder sb,
IntPtr ip,
out bool ob,
ref bytes bs);
private static int descatterTransferPayload(CanardRxTransfer transfer, uint bit_offset, byte bit_length
, ref union output)
{
if (bit_offset >= (transfer.payload_len * 8))
{
return(0); // Out of range, reading zero bits
}
if (bit_offset + bit_length > (transfer.payload_len * 8))
{
bit_length = (Byte)(transfer.payload_len * 8 - bit_offset);
}
byte[] dest = new byte[8];
copyBitArray(transfer.data, bit_offset, bit_length, dest, 0);
Array.Resize(ref dest, 8);
output.u64 = BitConverter.ToUInt64(dest, 0);
return(bit_length);
}
public static Quaternion Slerp(Quaternion qStart, Quaternion qEnd, float t)
{
float CosOm = qStart.X * qEnd.X + qStart.Y * qEnd.Y + qStart.Z * qEnd.Z + qStart.Re * qEnd.Re;
float Om = (float)Math.Acos(CosOm);
float Rsqrt = 1 - CosOm * CosOm; //for 1/SinOm
union u = new union();
float xhalf = 0.5f * Rsqrt;
u.asFloat = Rsqrt;
u.asInt = 0x5f3759df - (u.asInt >> 1);
Rsqrt = u.asFloat * (1.5f - xhalf * u.asFloat * u.asFloat);
return ((float)Math.Sin((1 - t) * Om) * qStart + (float)Math.Sin(t * Om) * qEnd) * Rsqrt;
}
public static Quaternion LSlerp(Quaternion qStart, Quaternion qEnd, float t)
{
float CosA = qStart.X * qEnd.X + qStart.Y * qEnd.Y + qStart.Z * qEnd.Z + qStart.Re * qEnd.Re;
t *= (0.5069269f - CosA * (0.7987229f + 0.5069269f * CosA)) * (t * (2 * t - 3) + 1) + 1;
Quaternion qRes = qStart + t * (qEnd - qStart);
float Rsqrt = qRes.X * qRes.X + qRes.Y * qRes.Y + qRes.Z * qRes.Z + qRes.Re * qRes.Re;
union u = new union();
float xhalf = 0.5f * Rsqrt;
u.asFloat = Rsqrt;
u.asInt = 0x5f3759df - (u.asInt >> 1);
Rsqrt = u.asFloat * (1.5f - xhalf * u.asFloat * u.asFloat);
return qRes * Rsqrt;
}
public static Quaternion Lerp(Quaternion qStart, Quaternion qEnd, float t)
{
Quaternion qRes = qStart + t * (qEnd - qStart);
float Rsqrt = qRes.X * qRes.X + qRes.Y * qRes.Y + qRes.Z * qRes.Z + qRes.Re * qRes.Re;
union u = new union();
float xhalf = 0.5f * Rsqrt;
u.asFloat = Rsqrt;
u.asInt = 0x5f3759df - (u.asInt >> 1);
Rsqrt = u.asFloat * (1.5f - xhalf * u.asFloat * u.asFloat);
return qRes * Rsqrt;
}
public static int canardDecodeScalar <T>(CanardRxTransfer transfer,
uint bit_offset,
byte bit_length,
bool value_is_signed,
ref T out_value)
{
union storage = new union(false);
memset(storage.bytes.ToArray(), 0, Marshal.SizeOf(storage)); // This is important
int result = descatterTransferPayload(transfer, bit_offset, bit_length, ref storage);
if (result <= 0)
{
return(result);
}
CANARD_ASSERT((result > 0) && (result <= 64) && (result <= bit_length));
/*
* The bit copy algorithm assumes that more significant bits have lower index, so we need to shift some.
* Extra most significant bits will be filled with zeroes, which is fine.
* Coverity Scan mistakenly believes that the array may be overrun if bit_length == 64; however, this branch will
* not be taken if bit_length == 64, because 64 % 8 == 0.
*/
if ((bit_length % 8) != 0)
{
// coverity[overrun-local]
storage[bit_length / 8] = (byte)(storage.bytes[bit_length / 8] >> ((8 - (bit_length % 8)) & 7));
}
/*
* Determining the closest standard byte length - this will be needed for byte reordering and sign bit extension.
*/
Byte std_byte_length = 0;
if (bit_length == 1)
{
std_byte_length = sizeof(bool);
}
else if (bit_length <= 8)
{
std_byte_length = 1;
}
else if (bit_length <= 16)
{
std_byte_length = 2;
}
else if (bit_length <= 32)
{
std_byte_length = 4;
}
else if (bit_length <= 64)
{
std_byte_length = 8;
}
else
{
CANARD_ASSERT(false);
return(-CANARD_ERROR_INTERNAL);
}
CANARD_ASSERT((std_byte_length > 0) && (std_byte_length <= 8));
/*
* Flipping the byte order if needed.
*/
/*if (isBigEndian())
* {
* swapByteOrder(&storage.bytes[0], std_byte_length);
* }*/
/*
* Extending the sign bit if needed. I miss templates.
*/
if (value_is_signed && (std_byte_length * 8 != bit_length))
{
if (bit_length <= 8)
{
if ((storage.s8 & (1U << (bit_length - 1))) != 0) // If the sign bit is set...
{
storage.u8 |=
(byte)((Byte)0xFFU & (Byte) ~((1 << bit_length) - 1U)); // ...set all bits above it.
}
}
else if (bit_length <= 16)
{
if ((storage.s16 & (1U << (bit_length - 1))) != 0)
{
storage.u16 |= (UInt16)((UInt16)0xFFFFU & (UInt16) ~((1 << bit_length) - 1U));
}
}
else if (bit_length <= 32)
{
if ((storage.s32 & (((UInt32)1) << (bit_length - 1))) != 0)
{
storage.u32 |= (UInt32)0xFFFFFFFFU & (UInt32) ~((((UInt32)1U) << bit_length) - 1U);
}
}
else if (bit_length < 64) // Strictly less, this is not a typo
{
if ((storage.u64 & (((UInt64)1) << (bit_length - 1))) != 0)
{
storage.u64 |= (UInt64)0xFFFFFFFFFFFFFFFFU & (UInt64) ~((((UInt64)1) << bit_length) - 1U);
}
}
else
{
CANARD_ASSERT(false);
return(-CANARD_ERROR_INTERNAL);
}
}
/*
* Copying the result out.
*/
if (value_is_signed)
{
if (bit_length <= 8)
{
out_value = (T)(IConvertible)storage.s8;
}
else if (bit_length <= 16)
{
out_value = (T)(dynamic)storage.s16;
}
else if (typeof(T) == typeof(float))
{
out_value = (T)(IConvertible)storage.f32;
}
else if (bit_length <= 32)
{
out_value = (T)(IConvertible)storage.s32;
}
else if (bit_length <= 64)
{
out_value = (T)(IConvertible)storage.s64;
}
else
{
CANARD_ASSERT(false);
return(-CANARD_ERROR_INTERNAL);
}
}
else
{
if (bit_length == 1)
{
out_value = (T)(IConvertible)storage.boolean;
}
else if (bit_length <= 8)
{
out_value = (T)(IConvertible)storage.u8;
}
else if (bit_length <= 16)
{
out_value = (T)(IConvertible)storage.u16;
}
else if (bit_length <= 32)
{
out_value = (T)(IConvertible)storage.u32;
}
else if (bit_length <= 64)
{
out_value = (T)(IConvertible)storage.u64;
}
else
{
CANARD_ASSERT(false);
return(-CANARD_ERROR_INTERNAL);
}
}
CANARD_ASSERT(result <= bit_length);
CANARD_ASSERT(result > 0);
return(result);
}
public static float InvSqrt(float x)
{
union u = new union();
float xhalf = 0.5f * x;
u.asFloat = x;
u.asInt = 0x5f3759df - (u.asInt >> 1);
x = u.asFloat * (1.5f - xhalf * u.asFloat * u.asFloat);
return x;
}
public NonEmptyList(T head, union tail) => (Head, Tail) = (head, tail);
public static case2 NonEmpty(T head, union tail) => new case2(new NonEmptyList(head, tail));
