IMHOTEP Framework
 All Classes Namespaces Functions Variables Enumerations Enumerator Properties Pages
SteamVR_Utils.cs
1 //======= Copyright (c) Valve Corporation, All rights reserved. ===============
2 //
3 // Purpose: Utilities for working with SteamVR
4 //
5 //=============================================================================
6 
7 using UnityEngine;
8 using System.Collections;
9 using System.Runtime.InteropServices;
10 using Valve.VR;
11 
12 public static class SteamVR_Utils
13 {
14  // this version does not clamp [0..1]
15  public static Quaternion Slerp(Quaternion A, Quaternion B, float t)
16  {
17  var cosom = Mathf.Clamp(A.x * B.x + A.y * B.y + A.z * B.z + A.w * B.w, -1.0f, 1.0f);
18  if (cosom < 0.0f)
19  {
20  B = new Quaternion(-B.x, -B.y, -B.z, -B.w);
21  cosom = -cosom;
22  }
23 
24  float sclp, sclq;
25  if ((1.0f - cosom) > 0.0001f)
26  {
27  var omega = Mathf.Acos(cosom);
28  var sinom = Mathf.Sin(omega);
29  sclp = Mathf.Sin((1.0f - t) * omega) / sinom;
30  sclq = Mathf.Sin(t * omega) / sinom;
31  }
32  else
33  {
34  // "from" and "to" very close, so do linear interp
35  sclp = 1.0f - t;
36  sclq = t;
37  }
38 
39  return new Quaternion(
40  sclp * A.x + sclq * B.x,
41  sclp * A.y + sclq * B.y,
42  sclp * A.z + sclq * B.z,
43  sclp * A.w + sclq * B.w);
44  }
45 
46  public static Vector3 Lerp(Vector3 A, Vector3 B, float t)
47  {
48  return new Vector3(
49  Lerp(A.x, B.x, t),
50  Lerp(A.y, B.y, t),
51  Lerp(A.z, B.z, t));
52  }
53 
54  public static float Lerp(float A, float B, float t)
55  {
56  return A + (B - A) * t;
57  }
58 
59  public static double Lerp(double A, double B, double t)
60  {
61  return A + (B - A) * t;
62  }
63 
64  public static float InverseLerp(Vector3 A, Vector3 B, Vector3 result)
65  {
66  return Vector3.Dot(result - A, B - A);
67  }
68 
69  public static float InverseLerp(float A, float B, float result)
70  {
71  return (result - A) / (B - A);
72  }
73 
74  public static double InverseLerp(double A, double B, double result)
75  {
76  return (result - A) / (B - A);
77  }
78 
79  public static float Saturate(float A)
80  {
81  return (A < 0) ? 0 : (A > 1) ? 1 : A;
82  }
83 
84  public static Vector2 Saturate(Vector2 A)
85  {
86  return new Vector2(Saturate(A.x), Saturate(A.y));
87  }
88 
89  public static float Abs(float A)
90  {
91  return (A < 0) ? -A : A;
92  }
93 
94  public static Vector2 Abs(Vector2 A)
95  {
96  return new Vector2(Abs(A.x), Abs(A.y));
97  }
98 
99  private static float _copysign(float sizeval, float signval)
100  {
101  return Mathf.Sign(signval) == 1 ? Mathf.Abs(sizeval) : -Mathf.Abs(sizeval);
102  }
103 
104  public static Quaternion GetRotation(this Matrix4x4 matrix)
105  {
106  Quaternion q = new Quaternion();
107  q.w = Mathf.Sqrt(Mathf.Max(0, 1 + matrix.m00 + matrix.m11 + matrix.m22)) / 2;
108  q.x = Mathf.Sqrt(Mathf.Max(0, 1 + matrix.m00 - matrix.m11 - matrix.m22)) / 2;
109  q.y = Mathf.Sqrt(Mathf.Max(0, 1 - matrix.m00 + matrix.m11 - matrix.m22)) / 2;
110  q.z = Mathf.Sqrt(Mathf.Max(0, 1 - matrix.m00 - matrix.m11 + matrix.m22)) / 2;
111  q.x = _copysign(q.x, matrix.m21 - matrix.m12);
112  q.y = _copysign(q.y, matrix.m02 - matrix.m20);
113  q.z = _copysign(q.z, matrix.m10 - matrix.m01);
114  return q;
115  }
116 
117  public static Vector3 GetPosition(this Matrix4x4 matrix)
118  {
119  var x = matrix.m03;
120  var y = matrix.m13;
121  var z = matrix.m23;
122 
123  return new Vector3(x, y, z);
124  }
125 
126  public static Vector3 GetScale(this Matrix4x4 m)
127  {
128  var x = Mathf.Sqrt(m.m00 * m.m00 + m.m01 * m.m01 + m.m02 * m.m02);
129  var y = Mathf.Sqrt(m.m10 * m.m10 + m.m11 * m.m11 + m.m12 * m.m12);
130  var z = Mathf.Sqrt(m.m20 * m.m20 + m.m21 * m.m21 + m.m22 * m.m22);
131 
132  return new Vector3(x, y, z);
133  }
134 
135  [System.Serializable]
136  public struct RigidTransform
137  {
138  public Vector3 pos;
139  public Quaternion rot;
140 
141  public static RigidTransform identity
142  {
143  get { return new RigidTransform(Vector3.zero, Quaternion.identity); }
144  }
145 
146  public static RigidTransform FromLocal(Transform t)
147  {
148  return new RigidTransform(t.localPosition, t.localRotation);
149  }
150 
151  public RigidTransform(Vector3 pos, Quaternion rot)
152  {
153  this.pos = pos;
154  this.rot = rot;
155  }
156 
157  public RigidTransform(Transform t)
158  {
159  this.pos = t.position;
160  this.rot = t.rotation;
161  }
162 
163  public RigidTransform(Transform from, Transform to)
164  {
165  var inv = Quaternion.Inverse(from.rotation);
166  rot = inv * to.rotation;
167  pos = inv * (to.position - from.position);
168  }
169 
170  public RigidTransform(HmdMatrix34_t pose)
171  {
172  var m = Matrix4x4.identity;
173 
174  m[0, 0] = pose.m0;
175  m[0, 1] = pose.m1;
176  m[0, 2] = -pose.m2;
177  m[0, 3] = pose.m3;
178 
179  m[1, 0] = pose.m4;
180  m[1, 1] = pose.m5;
181  m[1, 2] = -pose.m6;
182  m[1, 3] = pose.m7;
183 
184  m[2, 0] = -pose.m8;
185  m[2, 1] = -pose.m9;
186  m[2, 2] = pose.m10;
187  m[2, 3] = -pose.m11;
188 
189  this.pos = m.GetPosition();
190  this.rot = m.GetRotation();
191  }
192 
193  public RigidTransform(HmdMatrix44_t pose)
194  {
195  var m = Matrix4x4.identity;
196 
197  m[0, 0] = pose.m0;
198  m[0, 1] = pose.m1;
199  m[0, 2] = -pose.m2;
200  m[0, 3] = pose.m3;
201 
202  m[1, 0] = pose.m4;
203  m[1, 1] = pose.m5;
204  m[1, 2] = -pose.m6;
205  m[1, 3] = pose.m7;
206 
207  m[2, 0] = -pose.m8;
208  m[2, 1] = -pose.m9;
209  m[2, 2] = pose.m10;
210  m[2, 3] = -pose.m11;
211 
212  m[3, 0] = pose.m12;
213  m[3, 1] = pose.m13;
214  m[3, 2] = -pose.m14;
215  m[3, 3] = pose.m15;
216 
217  this.pos = m.GetPosition();
218  this.rot = m.GetRotation();
219  }
220 
221  public HmdMatrix44_t ToHmdMatrix44()
222  {
223  var m = Matrix4x4.TRS(pos, rot, Vector3.one);
224  var pose = new HmdMatrix44_t();
225 
226  pose.m0 = m[0, 0];
227  pose.m1 = m[0, 1];
228  pose.m2 = -m[0, 2];
229  pose.m3 = m[0, 3];
230 
231  pose.m4 = m[1, 0];
232  pose.m5 = m[1, 1];
233  pose.m6 = -m[1, 2];
234  pose.m7 = m[1, 3];
235 
236  pose.m8 = -m[2, 0];
237  pose.m9 = -m[2, 1];
238  pose.m10 = m[2, 2];
239  pose.m11 = -m[2, 3];
240 
241  pose.m12 = m[3, 0];
242  pose.m13 = m[3, 1];
243  pose.m14 = -m[3, 2];
244  pose.m15 = m[3, 3];
245 
246  return pose;
247  }
248 
249  public HmdMatrix34_t ToHmdMatrix34()
250  {
251  var m = Matrix4x4.TRS(pos, rot, Vector3.one);
252  var pose = new HmdMatrix34_t();
253 
254  pose.m0 = m[0, 0];
255  pose.m1 = m[0, 1];
256  pose.m2 = -m[0, 2];
257  pose.m3 = m[0, 3];
258 
259  pose.m4 = m[1, 0];
260  pose.m5 = m[1, 1];
261  pose.m6 = -m[1, 2];
262  pose.m7 = m[1, 3];
263 
264  pose.m8 = -m[2, 0];
265  pose.m9 = -m[2, 1];
266  pose.m10 = m[2, 2];
267  pose.m11 = -m[2, 3];
268 
269  return pose;
270  }
271 
272  public override bool Equals(object o)
273  {
274  if (o is RigidTransform)
275  {
276  RigidTransform t = (RigidTransform)o;
277  return pos == t.pos && rot == t.rot;
278  }
279  return false;
280  }
281 
282  public override int GetHashCode()
283  {
284  return pos.GetHashCode() ^ rot.GetHashCode();
285  }
286 
287  public static bool operator ==(RigidTransform a, RigidTransform b)
288  {
289  return a.pos == b.pos && a.rot == b.rot;
290  }
291 
292  public static bool operator !=(RigidTransform a, RigidTransform b)
293  {
294  return a.pos != b.pos || a.rot != b.rot;
295  }
296 
297  public static RigidTransform operator *(RigidTransform a, RigidTransform b)
298  {
299  return new RigidTransform
300  {
301  rot = a.rot * b.rot,
302  pos = a.pos + a.rot * b.pos
303  };
304  }
305 
306  public void Inverse()
307  {
308  rot = Quaternion.Inverse(rot);
309  pos = -(rot * pos);
310  }
311 
312  public RigidTransform GetInverse()
313  {
314  var t = new RigidTransform(pos, rot);
315  t.Inverse();
316  return t;
317  }
318 
319  public void Multiply(RigidTransform a, RigidTransform b)
320  {
321  rot = a.rot * b.rot;
322  pos = a.pos + a.rot * b.pos;
323  }
324 
325  public Vector3 InverseTransformPoint(Vector3 point)
326  {
327  return Quaternion.Inverse(rot) * (point - pos);
328  }
329 
330  public Vector3 TransformPoint(Vector3 point)
331  {
332  return pos + (rot * point);
333  }
334 
335  public static Vector3 operator *(RigidTransform t, Vector3 v)
336  {
337  return t.TransformPoint(v);
338  }
339 
340  public static RigidTransform Interpolate(RigidTransform a, RigidTransform b, float t)
341  {
342  return new RigidTransform(Vector3.Lerp(a.pos, b.pos, t), Quaternion.Slerp(a.rot, b.rot, t));
343  }
344 
345  public void Interpolate(RigidTransform to, float t)
346  {
347  pos = SteamVR_Utils.Lerp(pos, to.pos, t);
348  rot = SteamVR_Utils.Slerp(rot, to.rot, t);
349  }
350  }
351 
352  public delegate object SystemFn(CVRSystem system, params object[] args);
353 
354  public static object CallSystemFn(SystemFn fn, params object[] args)
355  {
356  var initOpenVR = (!SteamVR.active && !SteamVR.usingNativeSupport);
357  if (initOpenVR)
358  {
359  var error = EVRInitError.None;
360  OpenVR.Init(ref error, EVRApplicationType.VRApplication_Utility);
361  }
362 
363  var system = OpenVR.System;
364  var result = (system != null) ? fn(system, args) : null;
365 
366  if (initOpenVR)
367  OpenVR.Shutdown();
368 
369  return result;
370  }
371 
372  public static void TakeStereoScreenshot(uint screenshotHandle, GameObject target, int cellSize, float ipd, ref string previewFilename, ref string VRFilename)
373  {
374  const int width = 4096;
375  const int height = width / 2;
376  const int halfHeight = height / 2;
377 
378  var texture = new Texture2D(width, height * 2, TextureFormat.ARGB32, false);
379 
380  var timer = new System.Diagnostics.Stopwatch();
381 
382  Camera tempCamera = null;
383 
384  timer.Start();
385 
386  var camera = target.GetComponent<Camera>();
387  if (camera == null)
388  {
389  if (tempCamera == null)
390  tempCamera = new GameObject().AddComponent<Camera>();
391  camera = tempCamera;
392  }
393 
394  // Render preview texture
395  const int previewWidth = 2048;
396  const int previewHeight = 2048;
397  var previewTexture = new Texture2D(previewWidth, previewHeight, TextureFormat.ARGB32, false);
398  var targetPreviewTexture = new RenderTexture(previewWidth, previewHeight, 24);
399 
400  var oldTargetTexture = camera.targetTexture;
401  var oldOrthographic = camera.orthographic;
402  var oldFieldOfView = camera.fieldOfView;
403  var oldAspect = camera.aspect;
404  var oldstereoTargetEye = camera.stereoTargetEye;
405  camera.stereoTargetEye = StereoTargetEyeMask.None;
406  camera.fieldOfView = 60.0f;
407  camera.orthographic = false;
408  camera.targetTexture = targetPreviewTexture;
409  camera.aspect = 1.0f;
410  camera.Render();
411 
412  // copy preview texture
413  RenderTexture.active = targetPreviewTexture;
414  previewTexture.ReadPixels(new Rect(0, 0, targetPreviewTexture.width, targetPreviewTexture.height), 0, 0);
415  RenderTexture.active = null;
416  camera.targetTexture = null;
417  Object.DestroyImmediate(targetPreviewTexture);
418 
419  var fx = camera.gameObject.AddComponent<SteamVR_SphericalProjection>();
420 
421  var oldPosition = target.transform.localPosition;
422  var oldRotation = target.transform.localRotation;
423  var basePosition = target.transform.position;
424  var baseRotation = Quaternion.Euler(0, target.transform.rotation.eulerAngles.y, 0);
425 
426  var transform = camera.transform;
427 
428  int vTotal = halfHeight / cellSize;
429  float dv = 90.0f / vTotal; // vertical degrees per segment
430  float dvHalf = dv / 2.0f;
431 
432  var targetTexture = new RenderTexture(cellSize, cellSize, 24);
433  targetTexture.wrapMode = TextureWrapMode.Clamp;
434  targetTexture.antiAliasing = 8;
435 
436  camera.fieldOfView = dv;
437  camera.orthographic = false;
438  camera.targetTexture = targetTexture;
439  camera.aspect = oldAspect;
440  camera.stereoTargetEye = StereoTargetEyeMask.None;
441 
442  // Render sections of a sphere using a rectilinear projection
443  // and resample using a sphereical projection into a single panorama
444  // texture per eye. We break into sections in order to keep the eye
445  // separation similar around the sphere. Rendering alternates between
446  // top and bottom sections, sweeping horizontally around the sphere,
447  // alternating left and right eyes.
448  for (int v = 0; v < vTotal; v++)
449  {
450  var pitch = 90.0f - (v * dv) - dvHalf;
451  var uTotal = width / targetTexture.width;
452  var du = 360.0f / uTotal; // horizontal degrees per segment
453  var duHalf = du / 2.0f;
454 
455  var vTarget = v * halfHeight / vTotal;
456 
457  for (int i = 0; i < 2; i++) // top, bottom
458  {
459  if (i == 1)
460  {
461  pitch = -pitch;
462  vTarget = height - vTarget - cellSize;
463  }
464 
465  for (int u = 0; u < uTotal; u++)
466  {
467  var yaw = -180.0f + (u * du) + duHalf;
468 
469  var uTarget = u * width / uTotal;
470 
471  var vTargetOffset = 0;
472  var xOffset = -ipd / 2 * Mathf.Cos(pitch * Mathf.Deg2Rad);
473 
474  for (int j = 0; j < 2; j++) // left, right
475  {
476  if (j == 1)
477  {
478  vTargetOffset = height;
479  xOffset = -xOffset;
480  }
481 
482  var offset = baseRotation * Quaternion.Euler(0, yaw, 0) * new Vector3(xOffset, 0, 0);
483  transform.position = basePosition + offset;
484 
485  var direction = Quaternion.Euler(pitch, yaw, 0.0f);
486  transform.rotation = baseRotation * direction;
487 
488  // vector pointing to center of this section
489  var N = direction * Vector3.forward;
490 
491  // horizontal span of this section in degrees
492  var phi0 = yaw - (du / 2);
493  var phi1 = phi0 + du;
494 
495  // vertical span of this section in degrees
496  var theta0 = pitch + (dv / 2);
497  var theta1 = theta0 - dv;
498 
499  var midPhi = (phi0 + phi1) / 2;
500  var baseTheta = Mathf.Abs(theta0) < Mathf.Abs(theta1) ? theta0 : theta1;
501 
502  // vectors pointing to corners of image closes to the equator
503  var V00 = Quaternion.Euler(baseTheta, phi0, 0.0f) * Vector3.forward;
504  var V01 = Quaternion.Euler(baseTheta, phi1, 0.0f) * Vector3.forward;
505 
506  // vectors pointing to top and bottom midsection of image
507  var V0M = Quaternion.Euler(theta0, midPhi, 0.0f) * Vector3.forward;
508  var V1M = Quaternion.Euler(theta1, midPhi, 0.0f) * Vector3.forward;
509 
510  // intersection points for each of the above
511  var P00 = V00 / Vector3.Dot(V00, N);
512  var P01 = V01 / Vector3.Dot(V01, N);
513  var P0M = V0M / Vector3.Dot(V0M, N);
514  var P1M = V1M / Vector3.Dot(V1M, N);
515 
516  // calculate basis vectors for plane
517  var P00_P01 = P01 - P00;
518  var P0M_P1M = P1M - P0M;
519 
520  var uMag = P00_P01.magnitude;
521  var vMag = P0M_P1M.magnitude;
522 
523  var uScale = 1.0f / uMag;
524  var vScale = 1.0f / vMag;
525 
526  var uAxis = P00_P01 * uScale;
527  var vAxis = P0M_P1M * vScale;
528 
529  // update material constant buffer
530  fx.Set(N, phi0, phi1, theta0, theta1,
531  uAxis, P00, uScale,
532  vAxis, P0M, vScale);
533 
534  camera.aspect = uMag / vMag;
535  camera.Render();
536 
537  RenderTexture.active = targetTexture;
538  texture.ReadPixels(new Rect(0, 0, targetTexture.width, targetTexture.height), uTarget, vTarget + vTargetOffset);
539  RenderTexture.active = null;
540  }
541 
542  // Update progress
543  var progress = (float)( v * ( uTotal * 2.0f ) + u + i*uTotal) / (float)(vTotal * ( uTotal * 2.0f ) );
544  OpenVR.Screenshots.UpdateScreenshotProgress(screenshotHandle, progress);
545  }
546  }
547  }
548 
549  // 100% flush
550  OpenVR.Screenshots.UpdateScreenshotProgress(screenshotHandle, 1.0f);
551 
552  // Save textures to disk.
553  // Add extensions
554  previewFilename += ".png";
555  VRFilename += ".png";
556 
557  // Preview
558  previewTexture.Apply();
559  System.IO.File.WriteAllBytes(previewFilename, previewTexture.EncodeToPNG());
560 
561  // VR
562  texture.Apply();
563  System.IO.File.WriteAllBytes(VRFilename, texture.EncodeToPNG());
564 
565  // Cleanup.
566  if (camera != tempCamera)
567  {
568  camera.targetTexture = oldTargetTexture;
569  camera.orthographic = oldOrthographic;
570  camera.fieldOfView = oldFieldOfView;
571  camera.aspect = oldAspect;
572  camera.stereoTargetEye = oldstereoTargetEye;
573 
574  target.transform.localPosition = oldPosition;
575  target.transform.localRotation = oldRotation;
576  }
577  else
578  {
579  tempCamera.targetTexture = null;
580  }
581 
582  Object.DestroyImmediate(targetTexture);
583  Object.DestroyImmediate(fx);
584 
585  timer.Stop();
586  Debug.Log(string.Format("Screenshot took {0} seconds.", timer.Elapsed));
587 
588  if (tempCamera != null)
589  {
590  Object.DestroyImmediate(tempCamera.gameObject);
591  }
592 
593  Object.DestroyImmediate(previewTexture);
594  Object.DestroyImmediate(texture);
595  }
596 }
597