bool CameraProject(Vector3 p, Matrix4x4 cameraToWorld, Matrix4x4 worldToClip, Rect viewport, out Vector3 outP)
{
Vector3 clipPoint;
outP = new Vector3(0, 0, 0);
if (worldToClip.PerspectiveMultiplyPoint3(p, out clipPoint))
{
Vector3 cameraPos = cameraToWorld.GetPosition();
Vector3 dir = p - cameraPos;
// The camera/projection matrices follow OpenGL convention: positive Z is towards the viewer.
// So negate it to get into Unity convention.
Vector3 forward = -cameraToWorld.GetAxisZ();
float dist = Vector3.Dot(dir, forward);
outP.x = viewport.x + (1.0f + clipPoint.x) * viewport.width * 0.5f;
outP.y = viewport.y + (1.0f + clipPoint.y) * viewport.height * 0.5f;
//outP.z = (1.0f + clipPoint.z) * 0.5f;
outP.z = dist;
return(true);
}
outP.Set(0.0f, 0.0f, 0.0f);
return(false);
}
public static void LocalFromMatrix4x4(this Transform transform, Matrix4x4 matrix)
{
transform.localScale = matrix.GetScale();
transform.localRotation = matrix.GetRotation();
transform.localPosition = matrix.GetPosition();
}
/// <summary>
/// Extract Translation, Rotation, and Scale given a Matrix.
/// </summary>
/// <param name="m">Input Matrix.</param>
/// <param name="trans">Output translation.</param>
/// <param name="rot">Output rotation.</param>
/// <param name="scale">Output scale.</param>
public static void GetTRS(this Matrix m, out Vector3 trans, out Quaternion rot, out Vector3 scale)
{
trans = m.GetPosition();
rot = m.GetRotation();
scale = m.ScaleFromMatrix();
}
/// <summary>
/// Assign matrix into Transform's local transformation.
/// </summary>
/// <param name="gameObject">Target GameObject.</param>
/// <param name="mat">Values from this matrix will be applied to the GameObject.</param>
public static void AssignLocalTransform(GameObject gameObject, Matrix mat)
{
gameObject.transform.localPosition = mat.GetPosition();
gameObject.transform.localRotation = mat.GetRotation();
gameObject.transform.localScale = mat.ScaleFromMatrix();
}
public static void SetMatrixLocal(this UnityEngine.Transform transform, UnityEngine.Matrix4x4 matrix)
{
transform.localScale = matrix.GetScale();
transform.localRotation = matrix.GetRotation();
transform.localPosition = matrix.GetPosition();
}
private void ProcessMarker(MarkerInfo marker)
{
var pose = marker.transform_matrix;
var transformMatrix = new Matrix4x4();
transformMatrix.m00 = pose.m00;
transformMatrix.m01 = pose.m01;
transformMatrix.m02 = pose.m02;
transformMatrix.m03 = pose.m03;
transformMatrix.m10 = pose.m10;
transformMatrix.m11 = pose.m11;
transformMatrix.m12 = pose.m12;
transformMatrix.m13 = pose.m13;
transformMatrix.m20 = pose.m20;
transformMatrix.m21 = pose.m21;
transformMatrix.m22 = pose.m22;
transformMatrix.m23 = pose.m23;
transformMatrix.m30 = 0;
transformMatrix.m31 = 0;
transformMatrix.m32 = 0;
transformMatrix.m33 = 1;
if (NewPoseDetected != null)
{
NewPoseDetected(new MarkerPose
{
Id = marker.id,
Name = marker.name,
Position = transformMatrix.GetPosition(),
Rotation = transformMatrix.GetRotation()
});
}
}
//TODO:fix it
internal void SetPosition(Matrix4x4 matrix)
{
this.gameObject.transform.position = matrix.GetPosition();
this.gameObject.transform.rotation = matrix.GetUnityRotation();
this.gameObject.transform.localScale = matrix.GetScale();
}