...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.

cs 1
1 // -----------------------------------------------------------------------
2 // <copyright file="MathUtils.cs" company="Microsoft">
3 // Copyright (c) Microsoft Corporation. All rights reserved.
4 // </copyright>
5 // -----------------------------------------------------------------------
6 // This source is subject to the Microsoft Limited Permissive License.
7 // See http://www.microsoft.com/resources/sharedsource/licensingbasics/
limitedpermissivelicense.mspx
8 // This file is based on the 3D Tools for Windows Presentation Foundation
9 // project. For more information, see:
10 // http://CodePlex.com/Wiki/View.aspx?ProjectName=3DTools
11 //---------------------------------------------------------------------------
12
13 namespace Wpf3DTools
14 {
15 using System;
16 using System.Diagnostics;
17 using System.Globalization;
18 using System.Windows;
19 using System.Windows.Media;
20 using System.Windows.Media.Media3D;
21
22 /// <summary>
23 /// A helper class for common math operations.
24 /// </summary>
25 public static class MathUtils
26 {
27 /// <summary>
28 /// The zero matrix definition
29 /// </summary>
30 public static readonly Matrix3D ZeroMatrix = new Matrix3D(0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
31
32 /// <summary>
33 /// The X axis definition
34 /// </summary>
35 public static readonly Vector3D XAxis = new Vector3D(1, 0, 0);
36
37 /// <summary>
38 /// The Y axis definition
39 /// </summary>
40 public static readonly Vector3D YAxis = new Vector3D(0, 1, 0);
41
42 /// <summary>
43 /// The Z axis definition
44 /// </summary>
45 public static readonly Vector3D ZAxis = new Vector3D(0, 0, 1);
46
47 /// <summary>
48 /// Get the aspect ratio
49 /// </summary>
50 /// <param name="size">The image or window extent.</param>
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 2
51 /// <returns>Returns the aspect ratio.</returns>
52 public static double GetAspectRatio(Size size)
53 {
54 return size.Width / size.Height;
55 }
56
57 /// <summary>
58 /// Convert degrees to radians
59 /// </summary>
60 /// <param name="degrees">The angle in degrees.</param>
61 /// <returns>Returns the angle in radians.</returns>
62 public static double DegreesToRadians(double degrees)
63 {
64 return degrees * (Math.PI / 180.0);
65 }
66
67 /// <summary>
68 /// Computes the effective view matrix for the given camera.
69 /// </summary>
70 /// <param name="camera">The perspective, orthogonal or matrix camera.</
param>
71 /// <returns>A Matrix3D containing the View.</returns>
72 public static Matrix3D GetViewMatrix(Camera camera)
73 {
74 if (camera == null)
75 {
76 throw new ArgumentNullException("camera");
77 }
78
79 ProjectionCamera projectionCamera = camera as ProjectionCamera;
80
81 if (projectionCamera != null)
82 {
83 return GetViewMatrix(projectionCamera);
84 }
85
86 MatrixCamera matrixCamera = camera as MatrixCamera;
87
88 if (matrixCamera != null)
89 {
90 return matrixCamera.ViewMatrix;
91 }
92
93 throw new ArgumentException(string.Format(CultureInfo.CurrentCulture,
"Unsupported camera type '{0}'.", camera.GetType().FullName),
"camera");
94 }
95
96 /// <summary>
97 /// Computes the effective projection matrix for the given camera.
98 /// </summary>
99 /// <param name="camera">The perspective, orthogonal or matrix camera.</
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 3
param>
100 /// <param name="aspectRatio">The aspect ratio of the image or window.</
param>
101 /// <returns>A Matrix3D containing the Projection matrix.</returns>
102 public static Matrix3D GetProjectionMatrix(Camera camera, double
aspectRatio)
103 {
104 if (camera == null)
105 {
106 throw new ArgumentNullException("camera");
107 }
108
109 PerspectiveCamera perspectiveCamera = camera as PerspectiveCamera;
110
111 if (perspectiveCamera != null)
112 {
113 return GetProjectionMatrix(perspectiveCamera, aspectRatio);
114 }
115
116 OrthographicCamera orthographicCamera = camera as OrthographicCamera;
117
118 if (orthographicCamera != null)
119 {
120 return GetProjectionMatrix(orthographicCamera, aspectRatio);
121 }
122
123 MatrixCamera matrixCamera = camera as MatrixCamera;
124
125 if (matrixCamera != null)
126 {
127 return matrixCamera.ProjectionMatrix;
128 }
129
130 throw new ArgumentException(string.Format(CultureInfo.CurrentCulture,
"Unsupported camera type '{0}'.", camera.GetType().FullName),
"camera");
131 }
132
133 /// <summary>
134 /// Computes the transform from world space to the Viewport3DVisual's
inner 2D space.
135 /// This method can fail if Camera.Transform is non-invertible in which
case the camera clip
136 /// planes will be coincident and nothing will render. In this case
success will be false.
137 /// </summary>
138 /// <param name="visual">The input visual.</param>
139 /// <param name="success">Whether the call succeeded.</param>
140 /// <returns>A Matrix3D containing the world-to-viewport transform.</
returns>
141 public static Matrix3D TryWorldToViewportTransform(Viewport3DVisual
visual, out bool success)
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 4
142 {
143 success = false;
144 Matrix3D result = TryWorldToCameraTransform(visual, out success);
145
146 if (null == visual)
147 {
148 return ZeroMatrix;
149 }
150
151 if (success)
152 {
153 result.Append(GetProjectionMatrix(visual.Camera,
MathUtils.GetAspectRatio(visual.Viewport.Size)));
154 result.Append(GetHomogeneousToViewportTransform
(visual.Viewport));
155 success = true;
156 }
157
158 return result;
159 }
160
161 /// <summary>
162 /// Computes the transform from world space to camera space
163 /// This method can fail if Camera.Transform is non-invertible, in which
case the camera clip
164 /// planes will be coincident and nothing will render. In this case
success will be false.
165 /// </summary>
166 /// <param name="visual">The input visual.</param>
167 /// <param name="success">Whether the call succeeded.</param>
168 /// <returns>A Matrix3D containing the world-to-camera transform.</
returns>
169 public static Matrix3D TryWorldToCameraTransform(Viewport3DVisual visual,
out bool success)
170 {
171 success = false;
172 Matrix3D result = Matrix3D.Identity;
173
174 if (null == visual)
175 {
176 return ZeroMatrix;
177 }
178
179 Camera camera = visual.Camera;
180
181 if (null == camera)
182 {
183 return ZeroMatrix;
184 }
185
186 Rect viewport = visual.Viewport;
187
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 5
188 if (viewport == Rect.Empty)
189 {
190 return ZeroMatrix;
191 }
192
193 Transform3D cameraTransform = camera.Transform;
194
195 if (cameraTransform != null)
196 {
197 Matrix3D m = cameraTransform.Value;
198
199 if (!m.HasInverse)
200 {
201 return ZeroMatrix;
202 }
203
204 m.Invert();
205 result.Append(m);
206 }
207
208 result.Append(GetViewMatrix(camera));
209
210 success = true;
211 return result;
212 }
213
214 /// <summary>
215 /// Computes the transform from the inner space of the given Visual3D to
the 2D space of
216 /// the Viewport3DVisual which contains it. The result will contain the
transform of the
217 /// given visual. This method can fail if Camera.Transform is non-
invertible in which
218 /// case the camera clip planes will be coincident and nothing will
render. In this case
219 /// success will be false.
220 /// </summary>
221 /// <param name="visual">The visual.</param>
222 /// <param name="viewport">The viewport.</param>
223 /// <param name="success">Set true if successful</param>
224 /// <returns>A Matrix3D.</returns>
225 public static Matrix3D TryTransformTo2DAncestor(DependencyObject visual,
out Viewport3DVisual viewport, out bool success)
226 {
227 Matrix3D to2D = GetWorldTransformationMatrix(visual, out viewport);
228 to2D.Append(MathUtils.TryWorldToViewportTransform(viewport, out
success));
229
230 if (!success)
231 {
232 return ZeroMatrix;
233 }
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 6
234
235 return to2D;
236 }
237
238 /// <summary>
239 /// Computes the transform from the inner space of the given Visual3D to
the camera
240 /// coordinate space. The result will contain the transform of the given
visual.
241 /// This method can fail if Camera.Transform is non-invertible in which
case the
242 /// camera clip planes will be coincident and nothing will render. In
this case success
243 /// will be false.
244 /// </summary>
245 /// <param name="visual">The visual.</param>
246 /// <param name="viewport">The viewport.</param>
247 /// <param name="success">Set true if successful.</param>
248 /// <returns>A Matrix3D containing.</returns>
249 public static Matrix3D TryTransformToCameraSpace(DependencyObject visual,
out Viewport3DVisual viewport, out bool success)
250 {
251 Matrix3D toViewSpace = GetWorldTransformationMatrix(visual, out
viewport);
252 toViewSpace.Append(MathUtils.TryWorldToCameraTransform(viewport, out
success));
253
254 if (!success)
255 {
256 return ZeroMatrix;
257 }
258
259 return toViewSpace;
260 }
261
262 /// <summary>
263 /// Transforms the axis-aligned bounding box 'bounds' by 'transform'
264 /// </summary>
265 /// <param name="bounds">The AABB to transform</param>
266 /// <param name="transform">The transform to apply</param>
267 /// <returns>Transformed AABB</returns>
268 public static Rect3D TransformBounds(Rect3D bounds, Matrix3D transform)
269 {
270 double x1 = bounds.X;
271 double y1 = bounds.Y;
272 double z1 = bounds.Z;
273 double x2 = bounds.X + bounds.SizeX;
274 double y2 = bounds.Y + bounds.SizeY;
275 double z2 = bounds.Z + bounds.SizeZ;
276
277 Point3D[] points = new Point3D[]
278 {
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 7
279 new Point3D(x1, y1, z1),
280 new Point3D(x1, y1, z2),
281 new Point3D(x1, y2, z1),
282 new Point3D(x1, y2, z2),
283 new Point3D(x2, y1, z1),
284 new Point3D(x2, y1, z2),
285 new Point3D(x2, y2, z1),
286 new Point3D(x2, y2, z2),
287 };
288
289 transform.Transform(points);
290
291 // reuse the 1 and 2 variables to stand for smallest and largest
292 Point3D p = points[0];
293 x1 = x2 = p.X;
294 y1 = y2 = p.Y;
295 z1 = z2 = p.Z;
296
297 for (int i = 1; i < points.Length; i++)
298 {
299 p = points[i];
300
301 x1 = Math.Min(x1, p.X);
302 y1 = Math.Min(y1, p.Y);
303 z1 = Math.Min(z1, p.Z);
304 x2 = Math.Max(x2, p.X);
305 y2 = Math.Max(y2, p.Y);
306 z2 = Math.Max(z2, p.Z);
307 }
308
309 return new Rect3D(x1, y1, z1, x2 - x1, y2 - y1, z2 - z1);
310 }
311
312 /// <summary>
313 /// Normalizes vector if |vector| > 0.
314 /// This normalization is slightly different from Vector3D.Normalize.
Here we just divide
315 /// by the length but Vector3D.Normalize tries to avoid overflow when
finding the length.
316 /// </summary>
317 /// <param name="vector">The vector to normalize</param>
318 /// <returns>'true' if vector was normalized</returns>
319 public static bool TryNormalize(ref Vector3D vector)
320 {
321 double length = vector.Length;
322
323 if (length != 0)
324 {
325 vector /= length;
326 return true;
327 }
328
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 8
329 return false;
330 }
331
332 /// <summary>
333 /// Computes the center of 'box'
334 /// </summary>
335 /// <param name="box">The rectangle we want the center of.</param>
336 /// <returns>The center point</returns>
337 public static Point3D GetCenter(Rect3D box)
338 {
339 return new Point3D(box.X + (box.SizeX / 2), box.Y + (box.SizeY / 2),
box.Z + (box.SizeZ / 2));
340 }
341
342 /// <summary>
343 /// Get the view matrix from a camera.
344 /// </summary>
345 /// <param name="camera">The camera we want the view matrix of.</param>
346 /// <returns>Returns the view matrix.</returns>
347 private static Matrix3D GetViewMatrix(ProjectionCamera camera)
348 {
349 Debug.Assert(camera != null, "Caller needs to ensure camera is non-
null.");
350
351 // This math is identical to what you find documented for
352 // D3DXMatrixLookAtRH with the exception that WPF uses a
353 // LookDirection vector rather than a LookAt point.
354 Vector3D axisZ = -camera.LookDirection;
355 axisZ.Normalize();
356
357 Vector3D axisX = Vector3D.CrossProduct(camera.UpDirection, axisZ);
358 axisX.Normalize();
359
360 Vector3D axisY = Vector3D.CrossProduct(axisZ, axisX);
361
362 Vector3D position = (Vector3D)camera.Position;
363 double offsetX = -Vector3D.DotProduct(axisX, position);
364 double offsetY = -Vector3D.DotProduct(axisY, position);
365 double offsetZ = -Vector3D.DotProduct(axisZ, position);
366
367 return new Matrix3D(
368 axisX.X,
369 axisY.X,
370 axisZ.X,
371 0,
372 axisX.Y,
373 axisY.Y,
374 axisZ.Y,
375 0,
376 axisX.Z,
377 axisY.Z,
378 axisZ.Z,
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 9
379 0,
380 offsetX,
381 offsetY,
382 offsetZ,
383 1);
384 }
385
386 /// <summary>
387 /// Get the projection matrix from an orthographic camera.
388 /// </summary>
389 /// <param name="camera">The camera we want the projection matrix of.</
param>
390 /// <param name="aspectRatio">The aspect ratio of the image or window.</
param>
391 /// <returns>Returns the projection matrix.</returns>
392 private static Matrix3D GetProjectionMatrix(OrthographicCamera camera,
double aspectRatio)
393 {
394 Debug.Assert(camera != null, "Caller needs to ensure camera is non-
null.");
395
396 // This math is identical to what you find documented for
397 // D3DXMatrixOrthoRH with the exception that in WPF only
398 // the camera's width is specified. Height is calculated
399 // from width and the aspect ratio.
400 double w = camera.Width;
401 double h = w / aspectRatio;
402 double zn = camera.NearPlaneDistance;
403 double zf = camera.FarPlaneDistance;
404
405 double m33 = 1 / (zn - zf);
406 double m43 = zn * m33;
407
408 return new Matrix3D(
409 2 / w,
410 0,
411 0,
412 0,
413 0,
414 2 / h,
415 0,
416 0,
417 0,
418 0,
419 m33,
420 0,
421 0,
422 0,
423 m43,
424 1);
425 }
426
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 10
427 /// <summary>
428 /// Get the projection matrix from a perspective camera.
429 /// </summary>
430 /// <param name="camera">The camera we want the projection matrix of.</
param>
431 /// <param name="aspectRatio">The aspect ratio of the image or window.</
param>
432 /// <returns>Returns the projection matrix.</returns>
433 private static Matrix3D GetProjectionMatrix(PerspectiveCamera camera,
double aspectRatio)
434 {
435 Debug.Assert(camera != null, "Caller needs to ensure camera is non-
null.");
436
437 // This math is identical to what you find documented for
438 // D3DXMatrixPerspectiveFovRH with the exception that in
439 // WPF the camera's horizontal rather the vertical
440 // field-of-view is specified.
441 double horizFoV = MathUtils.DegreesToRadians(camera.FieldOfView);
442 double zn = camera.NearPlaneDistance;
443 double zf = camera.FarPlaneDistance;
444
445 double scaleX = 1 / Math.Tan(horizFoV / 2);
446 double scaleY = aspectRatio * scaleX;
447 double m33 = (zf == double.PositiveInfinity) ? -1 : (zf / (zn - zf));
448 double m43 = zn * m33;
449
450 return new Matrix3D(
451 scaleX,
452 0,
453 0,
454 0,
455 0,
456 scaleY,
457 0,
458 0,
459 0,
460 0,
461 m33,
462 -1,
463 0,
464 0,
465 m43,
466 0);
467 }
468
469 /// <summary>
470 /// Get the 3D to viewport transformation.
471 /// </summary>
472 /// <param name="viewport">The viewport rectangle.</param>
473 /// <returns>Returns the 3D to viewport transform matrix.</returns>
474 private static Matrix3D GetHomogeneousToViewportTransform(Rect viewport)
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 11
475 {
476 double scaleX = viewport.Width / 2;
477 double scaleY = viewport.Height / 2;
478 double offsetX = viewport.X + scaleX;
479 double offsetY = viewport.Y + scaleY;
480
481 return new Matrix3D(
482 scaleX,
483 0,
484 0,
485 0,
486 0,
487 -scaleY,
488 0,
489 0,
490 0,
491 0,
492 1,
493 0,
494 offsetX,
495 offsetY,
496 0,
497 1);
498 }
499
500 /// <summary>
501 /// Gets the object space to world space transformation for the given
DependencyObject
502 /// </summary>
503 /// <param name="visual">The visual whose world space transform should be
found</param>
504 /// <param name="viewport">The Viewport3DVisual the Visual is contained
within</param>
505 /// <returns>The world space transformation</returns>
506 private static Matrix3D GetWorldTransformationMatrix(DependencyObject
visual, out Viewport3DVisual viewport)
507 {
508 Matrix3D worldTransform = Matrix3D.Identity;
509 viewport = null;
510
511 if (!(visual is Visual3D))
512 {
513 throw new ArgumentException("Must be of type Visual3D.",
"visual");
514 }
515
516 while (visual != null)
517 {
518 if (!(visual is ModelVisual3D))
519 {
520 break;
521 }
...tudio\Kinect-master\KinectFusionExplorer-WPF\MathUtils.cs 12
522
523 Transform3D transform = (Transform3D)visual.GetValue
(ModelVisual3D.TransformProperty);
524
525 if (transform != null)
526 {
527 worldTransform.Append(transform.Value);
528 }
529
530 visual = VisualTreeHelper.GetParent(visual);
531 }
532
533 viewport = visual as Viewport3DVisual;
534
535 if (viewport == null)
536 {
537 if (visual != null)
538 {
539 // In WPF 3D v1 the only possible configuration is a chain of
540 // ModelVisual3Ds leading up to a Viewport3DVisual.
541 throw new InvalidOperationException(
542 string.Format(CultureInfo.CurrentCulture, "Unsupported
type: '{0}'. Expected tree of ModelVisual3Ds leading up to a
Viewport3DVisual.", visual.GetType().FullName));
543 }
544
545 return ZeroMatrix;
546 }
547
548 return worldTransform;
549 }
550 }
551 }
552