/// <summary>
/// Update frustum graphics transform
/// </summary>
/// <param name="worldToCameraMatrix4">The transformation.</param>
public void UpdateFrustumTransformMatrix4(Matrix4 worldToCameraMatrix4)
{
// Update the frustum graphics if we have created them
if (null != this.CameraFrustum)
{
Matrix3D transform = KinectFusionHelper.ConvertMatrix4ToMatrix3D(worldToCameraMatrix4);
Quaternion q = KinectFusionHelper.Matrix3DToQuaternion(transform);
double angle = q.Angle;
Vector3D axis = q.Axis;
axis.X = -axis.X; // negate x rotation
Quaternion newq = new Quaternion(axis, angle);
Transform3DGroup cumulativeFrustumViewTransform = new Transform3DGroup();
cumulativeFrustumViewTransform.Children.Add(new TranslateTransform3D(new Vector3D(-transform.OffsetX, transform.OffsetY, transform.OffsetZ)));
cumulativeFrustumViewTransform.Children.Add(new RotateTransform3D(new AxisAngleRotation3D(axis, angle)));
this.cameraFrustumTransform3D.Matrix = cumulativeFrustumViewTransform.Value; // Set the camera transform to the frustum graphics
}
}
/// <summary>
/// Camera frustum 3D graphics
/// </summary>
/// <param name="viewport">The viewport.</param>
/// <param name="near">The near plane of the frustum.</param>
/// <param name="far">The far plane of the frustum.</param>
/// <param name="depthWidth">The width of the depth image.</param>
/// <param name="depthHeight">The height of the depth image.</param>
/// <param name="color">The color to draw the frustum.</param>
/// <param name="thickness">The line thickness to use when drawing the frustum.</param>
public void CreateFrustum3DGraphics(Viewport3D viewport, float near, float far, float depthWidth, float depthHeight, System.Windows.Media.Color color, int thickness)
{
if (null == viewport)
{
return;
}
this.graphicsViewport = viewport;
// De-normalize default camera params
float px = camParams.PrincipalPointX * depthWidth;
float py = camParams.PrincipalPointY * depthHeight;
float fx = camParams.FocalLengthX * depthWidth;
float fy = camParams.FocalLengthY * depthHeight;
float iflx = 1.0f / fx;
float ifly = 1.0f / fy;
this.CameraFrustum = new ScreenSpaceLines3D();
this.CameraFrustum.Points = new Point3DCollection();
Point3DCollection pts = this.CameraFrustum.Points;
// Near plane rectangle
pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly));
// Far plane rectangle
pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly));
// Connecting lines
pts.Add(KinectFusionHelper.BackProject(0, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, 0, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(depthWidth, depthHeight, far, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, near, px, py, iflx, ifly));
pts.Add(KinectFusionHelper.BackProject(0, depthHeight, far, px, py, iflx, ifly));
this.CameraFrustum.Thickness = thickness;
this.CameraFrustum.Color = color;
// Add a fixed rotation around the Y axis to look back down +Z towards origin
Matrix3D fixedRotY180 = new Matrix3D();
fixedRotY180.Rotate(new Quaternion(new Vector3D(0, 1, 0), 180));
this.cumulativeCameraFrustumTransform.Children.Add(new MatrixTransform3D(fixedRotY180));
this.cumulativeCameraFrustumTransform.Children.Add(this.cameraFrustumTransform3D);
this.CameraFrustum.Transform = this.cumulativeCameraFrustumTransform;
}
