// Use this for initialization
void Start()
{
controller = FindObjectOfType <TrackingController>();
VRPNTracker = GetComponent <TrackerTransform>();
VRPNButton = GetComponent <Tracker_Button_Server>();
sarTracker = GetComponent <SARTracker>();
}
void handleMsg(string message)
{
TrackerTransform tt = JsonUtility.FromJson <TrackerTransform>(message);
//RIGHT-Handed coordinate system coming from vive
//Converting to Unity's LEFT-handed ccords system
tt.position.x *= -1.0f;
tt.rotation = new Quaternion(-tt.rotation.x, tt.rotation.y, tt.rotation.z, -tt.rotation.w);
if (tt.id.Equals("tracker0"))
{
bTT_0 = true;
lastTrackerTransform_0 = tt;
canReadCam_0 = true;
//Debug.Log("HOLO " + ht.position.ToString() + " : " + ht.rotation.eulerAngles.ToString() + " from " + source);
++count_sps;
}
if (tt.id.Equals("tracker1"))
{
bTT_1 = true;
lastTrackerTransform_1 = tt;
canReadCam_1 = true;
//Debug.Log("HOLO " + ht.position.ToString() + " : " + ht.rotation.eulerAngles.ToString() + " from " + source);
++count_sps;
}
//Debug.Log("HOLO " + tt.id + " " + tt.position.ToString() + " : " + tt.rotation.eulerAngles.ToString() + " from " + source);
//++count_sps;
}
public override void StartTracker(HumanoidControl _humanoid)
{
humanoid = _humanoid;
if (!enabled)
{
return;
}
if (useLeapPackage && isHeadMounted)
{
AddXRServiceProvider();
if (leapProvider == null)
{
leapProvider = Leap.Unity.Hands.Provider;
}
leapProvider.OnUpdateFrame -= OnUpdateFrame;
leapProvider.OnUpdateFrame += OnUpdateFrame;
}
else
{
device = new LeapDevice();
device.Init();
leapTransform = device.GetTracker();
AddTracker(humanoid, "LeapMotion");
if (isHeadMounted)
{
SetTrackerOnCamera();
}
}
}
void UpdateViveHoloTracker()
{
//LEFT-handed (converted from RIGHT-handed when receiving data)
TrackerTransform TrackerHoloT = Instance.lastTrackerTransform_0;
ViveHoloTracker.transform.position = ViveEmitter.transform.position + ViveEmitter.transform.TransformVector(TrackerHoloT.position);
ViveHoloTracker.transform.rotation = ViveEmitter.transform.rotation * TrackerHoloT.rotation;
}
void UpdateViveEmitterCalibration()
{
//LEFT-handed (converted from RIGHT-handed when receiving data)
TrackerTransform TrackerTableT = Instance.lastTrackerTransform_1;
//Debug.Log (TrackerTableT.position);
//Vive emitter transform according to tracker 1
ViveEmitter.transform.rotation = ViveCASTTracker.transform.rotation * Quaternion.Inverse(TrackerTableT.rotation);
ViveEmitter.transform.position = ViveCASTTracker.transform.position + ViveEmitter.transform.TransformVector(-TrackerTableT.position);
}
public void StoreViveEmitterCalibration()
{
//LEFT-handed (converted from RIGHT-handed when receiving data)
TrackerTransform TrackerTableT = Instance.lastTrackerTransform_1;
//Debug.Log (TrackerTableT.position);
//Vive emitter transform according to tracker 1
ViveEmitter.transform.rotation = ViveCASTTracker.transform.rotation * Quaternion.Inverse(TrackerTableT.rotation);
ViveEmitter.transform.position = ViveCASTTracker.transform.position + ViveEmitter.transform.TransformVector(-TrackerTableT.position);
string jsonData = JsonUtility.ToJson(TrackerTableT);
File.WriteAllText(filePath, jsonData);
ViveEmitter_calibrated = true;
}
public override void StartTracker(HumanoidControl _humanoid)
{
humanoid = _humanoid;
if (!enabled)
{
return;
}
device = new NeuronDevice();
device.Init(address, port);
neuronTransform = device.GetTracker();
AddTracker(humanoid, "PerceptionNeuron");
}
public override void StartTracker(HumanoidControl _humanoid)
{
humanoid = _humanoid;
if (!enabled)
{
return;
}
device = new Kinect1Device();
device.Init();
kinect1Transform = device.GetTracker();
AddTracker(humanoid, "Microsoft Kinect 1");
}
public override void StartTracker(HumanoidControl _humanoid)
{
humanoid = _humanoid;
if (!enabled)
{
return;
}
device = new AstraDevice();
device.Init();
astraTransform = device.GetTracker();
AddTracker(humanoid, "Orbbec Astra");
}
public void LoadViveEmitterVCalibration()
{
if (File.Exists(filePath))
{
StreamReader reader = new StreamReader(filePath);
string jsonData = reader.ReadToEnd();
TrackerTransform tt = JsonUtility.FromJson <TrackerTransform>(jsonData);
ViveEmitter.transform.rotation = ViveCASTTracker.transform.rotation * Quaternion.Inverse(tt.rotation);
ViveEmitter.transform.position = ViveCASTTracker.transform.position + ViveEmitter.transform.TransformVector(-tt.position);
ViveEmitter_calibrated = true;
}
else
{
Debug.Log("Calibration File does not exist.");
}
}
private void AdjustPointcloud(TrackerTransform targetTransform, Pose refPose)
{
Pose controllerPose = VivePose.GetPose(calibrationController);
// The delta rotation of the hand controller
Quaternion rotationOffset = controllerPose.rotation * Quaternion.Inverse(calibrationControllerReferencePose.rotation);
print("rotationOffset: w=" + rotationOffset.w + " x=" + rotationOffset.x + " y=" + rotationOffset.y + " z=" + rotationOffset.z);
// Force Unity to use "small" angles (below 180 degrees)
if (rotationOffset.w < 0f)
{
rotationOffset.w = -rotationOffset.w;
rotationOffset.x = -rotationOffset.x;
rotationOffset.y = -rotationOffset.y;
rotationOffset.z = -rotationOffset.z;
}
rotationOffset.ToAngleAxis(out float deltaRotationAngle, out Vector3 deltaRotationAxis);
// Force Unity to not switch the axis after 180 degrees rotation. We simply set current rotation as reference
if (deltaRotationAngle > 150)
{
SetAdjustmentReferencePoses();
return;
}
print("rotationOffset angle: " + deltaRotationAngle + "\t axis: " + deltaRotationAxis);
Vector3 positionOffset = controllerPose.position - calibrationControllerReferencePose.position;
// Always rotate and position from the reference
targetTransform.transform.rotation = refPose.rotation;
targetTransform.transform.position = refPose.position;
Quaternion scaledRotationOffset = Quaternion.Slerp(Quaternion.identity, rotationOffset, adjustmentRatio);
print("scaledRotationOffset: " + scaledRotationOffset);
scaledRotationOffset.ToAngleAxis(out float scaledDeltaRotationAngle, out Vector3 scaledDeltaRotationAxis);
print("scaledRotationOffset angle: " + scaledDeltaRotationAngle + "\t axis: " + scaledDeltaRotationAxis);
targetTransform.transform.RotateAround(calibrationControllerReferencePose.position, scaledDeltaRotationAxis, scaledDeltaRotationAngle);
targetTransform.transform.position += (positionOffset * adjustmentRatio);
}
private void SaveTransform(TrackerTransform slot, int slotNum)
{
if (slot == null)
{
slot = new TrackerTransform();
}
slot.ConfVersion = ConfigVersion;
slot.Chest_Position = V_Chest.transform.position;
slot.Chest_Rotation = V_Chest.transform.rotation;
slot.Foot_L_Position = V_Foot_L.transform.position;
slot.Foot_L_Rotation = V_Foot_L.transform.rotation;
slot.Foot_R_Position = V_Foot_R.transform.position;
slot.Foot_R_Rotation = V_Foot_R.transform.rotation;
var json = JsonUtility.ToJson(slot, true);
if (!Directory.Exists(SavePath))
{
Directory.CreateDirectory(SavePath);
}
File.WriteAllText(SavePath + "\\Slot" + slotNum + ".json", json, new UTF8Encoding(false));
}
private static void Init()
{
if (_inGamePlayerStats != null)
{
return;
}
_doneInitializing = false;
_inGamePlayerStats = new Dictionary <string, InGameWorldVariable>(StringComparer.OrdinalIgnoreCase);
if (TrackerTransform == null)
{
return;
}
// set up variables for use
for (var i = 0; i < TrackerTransform.childCount; i++)
{
var oTrans = TrackerTransform.GetChild(i);
var oStat = oTrans.GetComponent <WorldVariable>();
if (oStat == null)
{
LevelSettings.LogIfNew("Transform '" + oTrans.name +
"' under WorldVariables does not contain a WorldVariable script. Please fix this.");
continue;
}
if (_inGamePlayerStats.ContainsKey(oStat.name))
{
LevelSettings.LogIfNew("You have more than one World Variable named '" + oStat.name +
"' in this Scene. Please make sure the names are unique.");
continue;
}
var newStatTracker = new InGameWorldVariable(oStat, oStat.name, oStat.varType);
if (Application.isPlaying)
{
// do not update values when we're in edit mode!
switch (oStat.persistanceMode)
{
case WorldVariable.StatPersistanceMode.ResetToStartingValue:
switch (oStat.varType)
{
case VariableType._integer:
newStatTracker.CurrentIntValue = oStat.startingValue;
break;
case VariableType._float:
newStatTracker.CurrentFloatValue = oStat.startingValueFloat;
break;
}
break;
case WorldVariable.StatPersistanceMode.KeepFromPrevious:
// set to value in player prefs
break;
}
if (oStat.listenerPrefab != null)
{
var variable = GetExistingWorldVariableIntValue(oStat.name, oStat.startingValue);
if (variable != null)
{
oStat.listenerPrefab.UpdateValue(variable.Value, variable.Value);
}
}
}
_inGamePlayerStats.Add(oStat.name, newStatTracker);
}
_doneInitializing = true;
}
// Update is called once per frame
void LateUpdate()
{
//TrackerTransform tt = new TrackerTransform();
//string st = JsonUtility.ToJson(tt);
////Debug.Log("UPDATE");
//Debug.Log(st);
if (Input.GetKeyDown(KeyCode.C) && !calibrating)
{
//this.StartCalibrateOrigin();
//this.CalibrateViveWithHololens();
if (Instance.bTT_0 && !calibrating)
{
if (ViveEmitter_calibrated)
{
CalibrateViveWithHololens();
}
}
Debug.Log("C pressed");
}
if (Input.GetKeyDown(KeyCode.H) && !calibrating)
{
this.CalibrateRotation();
Debug.Log("H pressed");
}
if (Input.GetKeyDown(KeyCode.S) && !calibrating)
{
Debug.Log("Confirm calibration of table, store the position and orientation of table");
//this.StoreTableCalibration();
this.StoreViveEmitterCalibration();
}
if (Input.GetKeyDown(KeyCode.L) && !calibrating)
{
Debug.Log("Load calibration data from previous one");
//this.LoadTableCalibration();
this.LoadViveEmitterVCalibration();
}
if (Input.GetKeyDown(KeyCode.E) && !calibrating)
{
Debug.Log("E pressed");
this.CalibrateViveEmitter();
}
if (Input.GetKeyDown(KeyCode.UpArrow))
{
//Debug.Log("Up Arrow Key pressed");
HoloToTrackerDisplacement += new Vector3(0, 0.002f, 0);
Debug.Log(HoloToTrackerDisplacement.ToString("F4"));
}
if (Input.GetKeyDown(KeyCode.DownArrow))
{
//Debug.Log("Down Arrow Key pressed");
HoloToTrackerDisplacement += new Vector3(0, -0.002f, 0);
Debug.Log(HoloToTrackerDisplacement.ToString("F4"));
}
if (Input.GetKeyDown(KeyCode.RightArrow))
{
//Debug.Log("Right Arrow Key pressed");
//backward
HoloToTrackerDisplacement += new Vector3(0, 0, 0.002f);
Debug.Log(HoloToTrackerDisplacement.ToString("F4"));
}
if (Input.GetKeyDown(KeyCode.LeftArrow))
{
//Debug.Log("Left Arrow Key pressed");
//forward
HoloToTrackerDisplacement += new Vector3(0, 0, -0.002f);
Debug.Log(HoloToTrackerDisplacement.ToString("F4"));
}
//if (Instance.bTT_1 && !calibrating)
//{
// Vector3 pos = Instance.lastTrackerTransform_1.position;
// Vector3 mid_pos = this.rotateAroundAxis(pos, new Vector3(0, 0, 0), re_from0toTable);
// //position relative to vive meter
// Vector3 mid2_pos = new Vector3(-mid_pos.x, -mid_pos.z, mid_pos.y);
// //Vector3 final_pos = meter_position + mid2_pos;
// //Transform localToCamera = Camera.main.transform.Find("TrackerLocalToCamera");
// //localToCamera.position = meter_position;
// //localToCamera.localPosition = localToCamera.localPosition + mid2_pos;
// //this.transform.position = localToCamera.position;
// Transform calibratedChildTrans = calibratedCamChild.transform;
// calibratedChildTrans.position = meter_position;
// calibratedChildTrans.localPosition = calibratedChildTrans.localPosition + mid2_pos;
// this.transform.position = calibratedChildTrans.position;
// //Quaternion rot = Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(Instance.lastTrackerTransform.rotation);
// Quaternion rot = (Instance.lastTrackerTransform_1.rotation);
// //version1 by far best (y axis works)
// //rot = new Quaternion(rot.x, -rot.y, -rot.z, rot.w);
// //Quaternion re_from0toTable_left = new Quaternion(re_from0toTable.x, -re_from0toTable.y, -re_from0toTable.z, re_from0toTable.w);
// //version2 not working
// //rot = new Quaternion(rot.x, -rot.z, -rot.y, rot.w);
// //Quaternion re_from0toTable_left = new Quaternion(re_from0toTable.x, -re_from0toTable.z, -re_from0toTable.y, re_from0toTable.w);
// //version3 not working
// //rot = new Quaternion(-rot.x, rot.y, -rot.z, rot.w);
// //Quaternion re_from0toTable_left = new Quaternion(-re_from0toTable.x, re_from0toTable.y, -re_from0toTable.z, re_from0toTable.w);
// //version4
// rot = new Quaternion(rot.x, rot.z, -rot.y, rot.w);
// Quaternion change = rot_calib * meter_rot * rot;
// this.transform.localRotation = camera_rot * change;
//}
if (Instance.bTT_0 && Instance.bTT_1 && !calibrating)
{
TrackerTransform TT0 = Instance.lastTrackerTransform_0;
TrackerTransform TT1 = Instance.lastTrackerTransform_1;
if (TT0.position.x == 0 && TT0.position.y == 0 && TT0.position.z == 0 && TT0.rotation.x == 0 && TT0.rotation.y == 0 && TT0.rotation.z == 0 && TT0.rotation.w == 1.0f)
{
//Tracker0 Hololens one lose tracking
Debug.Log("Tracker 0 lost tracking");
//this.GetComponent<Renderer>().material.color = Color.blue;
first_tracking = true;
return;
}
else
{
//this.GetComponent<Renderer>().material.color = Color.white;
}
if (TT1.position.x == 0 && TT1.position.y == 0 && TT1.position.z == 0 && TT1.rotation.x == 0 && TT1.rotation.y == 0 && TT1.rotation.z == 0 && TT1.rotation.w == 1.0f)
{
//Tracker1 lose tracking
Debug.Log("Tracker 1 lost tracking");
//this.GetComponent<Renderer>().material.color = Color.red;
first_tracking = true;
return;
}
else
{
//this.GetComponent<Renderer>().material.color = Color.white;
}
}
if (Instance.bTT_1 && !calibrating)
{
if (!ViveEmitter_calibrated)
{
UpdateViveEmitterCalibration();
}
UpdateViveHoloTracker();
}
if (Instance.bTT_0 && !calibrating)
{
if (ViveEmitter_calibrated)
{
CalibrateViveWithHololens();
}
}
}
