87 lines
2.6 KiB
C#
87 lines
2.6 KiB
C#
|
/*===============================================================================
|
||
|
|
Copyright (C) 2024 Immersal - Part of Hexagon. All Rights Reserved.
|
||
|
|
|
||
|
|
This file is part of the Immersal SDK.
|
||
|
|
|
||
|
|
The Immersal SDK cannot be copied, distributed, or made available to
|
||
|
|
third-parties for commercial purposes without written permission of Immersal Ltd.
|
||
|
|
|
||
|
|
Contact sales@immersal.com for licensing requests.
|
||
|
|
===============================================================================*/
|
||
|
|
|
||
|
|
using Immersal.XR;
|
||
|
|
using UnityEngine;
|
||
|
|
|
||
|
|
namespace Immersal.XR
|
||
|
|
{
|
||
|
|
public static class ExtensionMethods
|
||
|
|
{
|
||
|
|
public static Matrix4x4 SwitchHandedness(this ref Matrix4x4 b)
|
||
|
|
{
|
||
|
|
Matrix4x4 D = Matrix4x4.identity;
|
||
|
|
D.m00 = -1;
|
||
|
|
b = D * b * D;
|
||
|
|
return b;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static Quaternion SwitchHandedness(this ref Quaternion b)
|
||
|
|
{
|
||
|
|
Matrix4x4 m = Matrix4x4.Rotate(b);
|
||
|
|
m.SwitchHandedness();
|
||
|
|
b = m.rotation;
|
||
|
|
return b;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static float[] ToFloats(this ref Quaternion q)
|
||
|
|
{
|
||
|
|
float[] result = new float[4] { q.x, q.y, q.z, q.w };
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static Vector3 SwitchHandedness(this ref Vector3 b)
|
||
|
|
{
|
||
|
|
Matrix4x4 m = Matrix4x4.TRS(b, Quaternion.identity, Vector3.one);
|
||
|
|
m.SwitchHandedness();
|
||
|
|
b = m.GetColumn(3);
|
||
|
|
return b;
|
||
|
|
}
|
||
|
|
|
||
|
|
public static double[] QuaternionsToDoubleMatrix3x3(this XRMap.MapAlignment ma)
|
||
|
|
{
|
||
|
|
double[] q = new double[] {ma.qw, ma.qx, ma.qy, ma.qz};
|
||
|
|
double[] m = new double [] {1, 0, 0, 0, 1, 0, 0, 0, 1}; //identity matrix
|
||
|
|
|
||
|
|
// input quaternion should be in WXYZ order
|
||
|
|
double w = q[0];
|
||
|
|
double x = q[1];
|
||
|
|
double y = q[2];
|
||
|
|
double z = q[3];
|
||
|
|
|
||
|
|
double ww = w * w;
|
||
|
|
double xx = x * x;
|
||
|
|
double yy = y * y;
|
||
|
|
double zz = z * z;
|
||
|
|
|
||
|
|
double xy = x * y;
|
||
|
|
double zw = z * w;
|
||
|
|
double xz = x * z;
|
||
|
|
double yw = y * w;
|
||
|
|
double yz = y * z;
|
||
|
|
double xw = x * w;
|
||
|
|
|
||
|
|
double inv = 1.0 / (xx + yy + zz + ww);
|
||
|
|
|
||
|
|
m[0] = ( xx - yy - zz + ww) * inv;
|
||
|
|
m[1] = 2.0 * (xy - zw) * inv;
|
||
|
|
m[2] = 2.0 * (xz + yw) * inv;
|
||
|
|
m[3] = 2.0 * (xy + zw) * inv;
|
||
|
|
m[4] = (-xx + yy - zz + ww) * inv;
|
||
|
|
m[5] = 2.0 * (yz - xw) * inv;
|
||
|
|
m[6] = 2.0 * (xz - yw) * inv;
|
||
|
|
m[7] = 2.0 * (yz + xw) * inv;
|
||
|
|
m[8] = (-xx - yy + zz + ww) * inv;
|
||
|
|
|
||
|
|
return m;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|