/// <summary>
/// Sends the command to the driver.
/// </summary>
/// <param name="command">Command to be sent to the driver.</param>
/// <param name="responseType">Type of the response. This parameter may be <see langword="null"/>.</param>
/// <returns>Response received from the driver, or <see langword="null"/>, if the <paramref name="responseType"/> is <see langword="null"/>.</returns>
/// <exception cref="ArgumentNullException"><paramref name="command"/> is <see langword="null"/>.</exception>
/// <exception cref="ArgumentException"><paramref name="responseType"/> is not a structure type.</exception>
/// <exception cref="InvalidOperationException">
/// Client is not connected.
/// <para>-or-</para>
/// Memory for the command or response was not allocated.
/// <para>-or-</para>
/// Message was not sent to the driver.
/// </exception>
private object ExecuteCommand(IDriverCommand command, Type responseType)
{
if (command == null)
{
throw new ArgumentNullException(nameof(command));
}
if (responseType != null && (!responseType.IsValueType || responseType.IsPrimitive))
{
throw new ArgumentException("Response type is not a structure type.", nameof(responseType));
}
lock (this.syncRoot)
{
if (this.state != ConnectionState.Connected)
{
throw new InvalidOperationException("Client is not connected.");
}
// We want to have two separate buffers, because the driver can update the response buffer
// while parsing the command.
IntPtr commandBuffer = IntPtr.Zero;
IntPtr responseBuffer = IntPtr.Zero;
try
{
// Allocate buffer for the command.
// Command header contains 'Type' and 'DataLength' (Int32) values.
// See the 'DRIVER_COMMAND' structure in the 'CommunicationData.h' for details.
int commandHeaderSize = Marshal.SizeOf(command.Type) + Marshal.SizeOf(typeof(int));
int commandDataSize = command.Data?.Length ?? 0;
int commandSize = commandHeaderSize + commandDataSize;
try
{
commandBuffer = Marshal.AllocHGlobal(commandSize);
}
catch (OutOfMemoryException oom)
{
string message = "Unable to allocate memory for the command.";
DriverClientBase.Logger.Error(message, oom);
throw new InvalidOperationException(message, oom);
}
// Marshal command to the buffer allocated, if the command.Data is NULL, it'll be ignored.
MarshalingHelper.MarshalObjectsToPointer(commandBuffer, commandSize, command.Type, commandDataSize, command.Data);
//
// Allocate the response buffer, if needed.
//
int responseSize = 0;
if (responseType != null)
{
responseSize = Marshal.SizeOf(responseType);
try
{
responseBuffer = Marshal.AllocHGlobal(responseSize);
}
catch (OutOfMemoryException oom)
{
string message = "Unable to allocate memory for the response.";
DriverClientBase.Logger.Error(message, oom);
throw new InvalidOperationException(message, oom);
}
}
//
// Send command to the driver.
//
uint bytesReceived;
uint hr = NativeMethods.FilterSendMessage(this.filterPortHandle, commandBuffer, (uint)commandSize, responseBuffer, (uint)responseSize, out bytesReceived);
if (hr != NativeMethods.Ok)
{
string message = string.Format(CultureInfo.InvariantCulture, "Unable to send message to the driver: 0x{0:X8}", hr);
Exception innerException = Marshal.GetExceptionForHR((int)hr);
DriverClientBase.Logger.Error(innerException, message);
throw new InvalidOperationException(message, innerException);
}
// Return NULL, if response type is not specified.
return(responseType == null ? null : Marshal.PtrToStructure(responseBuffer, responseType));
}
catch
{
this.Disconnect();
this.state = ConnectionState.Faulted;
throw;
}
finally
{
if (commandBuffer != IntPtr.Zero)
{
Marshal.FreeHGlobal(commandBuffer);
}
if (responseBuffer != IntPtr.Zero)
{
Marshal.FreeHGlobal(responseBuffer);
}
}
}
}
private void _Draw3DBody()
{
m_Imagesize = m_cameraConfig.GetImageDimensions();
m_Texturesize = m_cameraConfig.GetTextureDimensions();
m_Confidence_first = MarshalingHelper.GetValueOfUnmanagedArrayElement <float>(m_body.GetMaskConfidence(), 0);
m_Depth_first = MarshalingHelper.GetValueOfUnmanagedArrayElement <short>(m_body.GetMaskDepth(), 0);
m_Confidence_last = MarshalingHelper.GetValueOfUnmanagedArrayElement <float>(m_body.GetMaskConfidence(), m_Texturesize[0] * m_Texturesize[1] - 1);
m_Depth_last = MarshalingHelper.GetValueOfUnmanagedArrayElement <short>(m_body.GetMaskDepth(), m_Texturesize[0] * m_Texturesize[1] - 1);
sb.Append("ImageSize: " + m_Imagesize + "\n");
sb.Append("TextureSize: " + m_Texturesize + "\n");
sb.Append("Mask_Confidence_first: " + m_Confidence_first + "\n");
sb.Append("Mask_Depth_first: " + m_Depth_first + "\n");
sb.Append("Mask_Confidence_last: " + m_Confidence_last + "\n");
sb.Append("Mask_Depth_last: " + m_Depth_last + "\n\n");
sb.Append("Skeleton Confidence: \n");
m_body.GetSkeletons(m_bodySkeletons);
Matrix4x4 camera2WorldMatrix = m_skeletonCamera.cameraToWorldMatrix;
foreach (var pair in m_bodySkeletons)
{
if (!pair.Value.Is3DValid)
{
continue;
}
m_skeletonPointObject[(int)pair.Key].name = pair.Key.ToString();
Vector3 positionInCameraSpace = pair.Value.Coordinate3D;
Vector3 positionInWorldSpace = camera2WorldMatrix.MultiplyPoint(positionInCameraSpace);
m_skeletonPointObject[(int)pair.Key].transform.position = positionInWorldSpace;
m_skeletonPointObject[(int)pair.Key].GetComponent <MeshRenderer>().material = m_skeletonMaterial;
m_skeletonPointObject[(int)pair.Key].SetActive(true);
sb.Append(pair.Key.ToString());
sb.Append(": ");
sb.Append(pair.Value.Confidence);
sb.Append("\n");
}
m_body.GetSkeletonConnection(m_connections);
for (int i = 0; i < m_connections.Count; i++)
{
ARBody.SkeletonPointEntry skpStart;
ARBody.SkeletonPointEntry skpEnd;
if (!m_bodySkeletons.TryGetValue(m_connections[i].Key, out skpStart) ||
!m_bodySkeletons.TryGetValue(m_connections[i].Value, out skpEnd) ||
!skpStart.Is3DValid || !skpEnd.Is3DValid)
{
continue;
}
Vector3 startGLScreenCoord = skpStart.Coordinate3D;
Vector3 endGLScreenCoord = skpEnd.Coordinate3D;
m_skeletonConnectionRenderer[i].SetPosition(0, camera2WorldMatrix.MultiplyPoint(startGLScreenCoord));
m_skeletonConnectionRenderer[i].SetPosition(1, camera2WorldMatrix.MultiplyPoint(endGLScreenCoord));
m_skeletonConnectionRenderer[i].gameObject.SetActive(true);
}
}
private void ProcessNotification(ResizableBuffer resizableBuffer)
{
IntPtr bufferPointer = resizableBuffer.DangerousGetPointer();
// Marshal the notification received.
DriverNotificationHeader header = (DriverNotificationHeader)Marshal.PtrToStructure(bufferPointer, typeof(DriverNotificationHeader));
DriverNotification notification = new DriverNotification {
Type = header.Type, DataLength = header.DataLength, Data = bufferPointer + this.notificationHeaderSize
};
// Get the reply object from the user-defined handler.
object reply = null;
int handlerResult = (int)NativeMethods.Ok;
try
{
reply = this.handler(notification);
}
catch (Exception e)
{
handlerResult = Marshal.GetHRForException(e);
NotificationsMonitor.Logger.Error(e, "Notification handler threw an exception.");
}
// Driver is not expecting any reply.
if (header.ReplyLength == 0)
{
if (reply != null)
{
NotificationsMonitor.Logger.Warn(CultureInfo.InvariantCulture, "Driver is not expecting any reply, but reply object is returned from handler: {0}", reply.GetType());
}
return;
}
int replySize = this.replyHeaderSize + MarshalingHelper.GetObjectSize(reply);
if (replySize > header.ReplyLength)
{
throw new InvalidOperationException(string.Format(CultureInfo.InvariantCulture, "Reply ({0} bytes) is bigger than the one expected by the driver ({1} bytes).", replySize, header.ReplyLength));
}
DriverReplyHeader replyHeader = new DriverReplyHeader
{
MessageId = header.MessageId,
// Notify driver about the exception thrown, if any.
Status = handlerResult
};
// Adjust the buffer to fit the reply.
resizableBuffer.Resize(replySize);
bufferPointer = resizableBuffer.DangerousGetPointer();
// Marshal reply to the output buffer.
MarshalingHelper.MarshalObjectsToPointer(bufferPointer, replySize, replyHeader, reply);
// And send it to the driver.
uint hr = NativeMethods.FilterReplyMessage(this.filterPortHandle, bufferPointer, (uint)replySize);
if (hr != NativeMethods.Ok)
{
throw new InvalidOperationException(string.Format(CultureInfo.InvariantCulture, "Unable to send reply: 0x{0:X8}", hr), Marshal.GetExceptionForHR(unchecked ((int)hr)));
}
}
