private void SendPacketNotification(string jsonPacket)
{
var packet = JsonUtility.FromJson<OptitrackPacket>(jsonPacket);
var poses = new List<OptitrackPose>();
foreach (var body in packet.Rigidbodies)
{
var pose = new OptitrackPose();
pose.DetectionTime = Time.unscaledTime;
pose.Id = body.Id;
pose.RigidbodyName = body.Name;
pose.Position = new Vector3(body.X, body.Y, body.Z);
pose.Rotation = new Quaternion(body.QX, body.QY, body.QZ, body.QW);
foreach (var marker in body.Markers)
{
pose.Markers.Add(new OptitrackPose.Marker
{
Id = marker.Id,
Position = new Vector3(marker.X, marker.Y, marker.Z)
});
}
}
if (PosesReceived != null)
{
PosesReceived(poses);
}
}
private void CalibrateMarker(OptitrackPose pose)
{
var anchorMarker = pose.Markers.First((m) => m.Id == AnchorId);
var span1Marker = pose.Markers.First((m) => m.Id == Span1Id);
var span2Marker = pose.Markers.First((m) => m.Id == Span2Id);
var normalVector = Vector3.Normalize(Vector3.Cross(span1Marker.Position - anchorMarker.Position, span2Marker.Position - anchorMarker.Position));
var rotationAxis = Vector3.Normalize(Vector3.Cross(normalVector, -OptitrackCalibratorObject.up));
var rotationAngle = Vector3.Dot(normalVector, -OptitrackCalibratorObject.up);
Quaternion rotation = Quaternion.AngleAxis(rotationAngle, rotationAxis);
//var ground = OptitrackCalibratorObject;
//var groundPlane = new Plane(ground.position, ground.position + ground.forward, ground.position + ground.right);
//var localUpVector = OptitrackCalibratorObject.up;
//// assume that point is *above* plain, no idea if that's important in unity's algorithm
//float distanceAnchorPlane = 0f;
//groundPlane.Raycast(new Ray(anchorMarker.Position, -localUpVector), out distanceAnchorPlane);
//float distanceSpan1Plane = 0f;
//groundPlane.Raycast(new Ray(span1Marker.Position, -localUpVector), out distanceSpan1Plane);
//// bring span1marker position to the same height (based on groundplane) as anchorMarker
//var angleA1 =
var markerSetup = GetComponent<MultiMarker_MarkerSetup>();
markerSetup.CalibratorToMarkerRotOffset = rotation.eulerAngles;
var markerSize = ArucoListener.Instance.MarkerSizeInMeter;
markerSetup.CalibratorToMarkerPosOffset = new Vector3(-markerSize/2f, 0, -markerSize/2f);
}
private void OnOptitrackPose(List <OptitrackPose> poses)
{
var matchingPose = poses.FirstOrDefault((p) => p.RigidbodyName == CalibratorName);
if (matchingPose != null)
{
_calibratorPose = matchingPose;
}
}
private static void SendPacketNotification(string jsonPacket)
{
var packet = JsonUtility.FromJson <OptitrackPacket>(jsonPacket);
var poses = new List <OptitrackPose>();
foreach (var body in packet.Rigidbodies)
{
var pose = new OptitrackPose();
pose.DetectionTime = Time.unscaledTime;
pose.Id = body.Id;
pose.RigidbodyName = body.Name;
// right to left handed conversion
pose.Position = new Vector3(body.X, body.Y, -body.Z) * Globals.OptitrackScaling; // z -> -z
pose.Rotation = new Quaternion(body.QX, body.QY, -body.QZ, -body.QW); // qz -> -qz, qw -> -qw
foreach (var marker in body.Markers)
{
pose.Markers.Add(new OptitrackPose.Marker
{
Id = marker.Id,
// right to left handed conversion
Position = new Vector3(marker.X, marker.Y, -marker.Z) * Globals.OptitrackScaling
});
}
poses.Add(pose);
_detectedPoses[body.Name] = pose;
}
if (PosesReceived != null)
{
PosesReceived(poses);
}
}
private void OnOptitrackPose(List<OptitrackPose> poses)
{
foreach (var pose in poses)
{
if (pose.RigidbodyName == OptitrackCameraName)
{
_optitrackCameraPose = pose;
}
}
}
private void OnOptitrackPose(List<OptitrackPose> poses)
{
var matchingPose = poses.FirstOrDefault((p) => p.RigidbodyName == CalibratorName);
if (matchingPose != null)
{
_calibratorPose = matchingPose;
}
}
private void OnOptitrackPose(List<OptitrackPose> poses)
{
foreach (var pose in poses)
{
if (pose.RigidbodyName == OptitrackCameraName)
{
_optitrackCameraPose = pose;
_optitrackCameraDetectionTime = Time.unscaledTime;
}
}
}
private void OnOptitrackPose(List<OptitrackPose> poses)
{
foreach (var pose in poses)
{
if (pose.RigidbodyName == OptitrackCameraName)
{
// TODO: compare markers etc to determine steadiness
if (_optitrackCameraPose == null)
{
HasSteadyOptitrackCameraPose = false;
}
else
{
HasSteadyOptitrackCameraPose = true;
}
_optitrackCameraPose = pose;
// TODO thresholding etc
HasSteadyOptitrackCameraPose = true;
break;
}
}
}
private void SaveOptitrackPose(OptitrackPose newPose, ref OptitrackPose memberPose, ref bool steadyIndicator)
{
if (memberPose == null)
{
steadyIndicator = false;
}
else
{
//var prevPose = memberPose;
// TODO: compare markers etc...
steadyIndicator = true;
}
memberPose = newPose;
}
