public void AddMotionController(MotionControllerModel mc)
{
if (!_camera.Controllers.Contains(mc))
{
_camera.Controllers.Add(mc);
}
}
public MotionControllerModel Initialize(int id)
{
_motionController = new MotionControllerModel();
Init(id);
return _motionController;
}
public RemoveMotionControllerMessage(MotionControllerModel motionController)
{
MotionController = motionController;
}
/// <summary>
/// Initializes a new instance of the MotionControllerViewModel class.
/// </summary>
public MotionControllerViewModel(MotionControllerModel mc, IMotionControllerService mcs)
{
if (!IsInDesignMode)
{
_motionController = mc;
_motionControllerService = mcs;
Palette = Palette.Create(new RGBColorWheel(), System.Windows.Media.Colors.Red, PaletteSchemaType.Analogous, 1);
MotionControllerService.Initialize(MotionController);
DoSelectColor(Palette);
Messenger.Default.Register<AddCameraMessage>(this,
message =>
{
// initialize entries for newly added camera
if (!MotionController.Tracking.ContainsKey(message.Camera))
{
MotionController.TrackerStatus.Add(message.Camera, PSMoveTrackerStatus.NotCalibrated);
MotionController.Tracking.Add(message.Camera, false);
MotionController.RawPosition.Add(message.Camera, Float3.Zero);
MotionController.WorldPosition.Add(message.Camera, Float3.Zero);
MotionController.CameraPosition.Add(message.Camera, Float3.Zero);
MotionController.FusionPosition.Add(message.Camera, Float3.Zero);
MotionController.PositionHistory.Add(message.Camera, new Vector3[5]);
}
});
Messenger.Default.Register<RemoveCameraMessage>(this,
message =>
{
// remove entries corresponding to recently removed camera
MotionController.TrackerStatus.Remove(message.Camera);
MotionController.Tracking.Remove(message.Camera);
MotionController.RawPosition.Remove(message.Camera);
MotionController.WorldPosition.Remove(message.Camera);
MotionController.CameraPosition.Remove(message.Camera);
MotionController.FusionPosition.Remove(message.Camera);
MotionController.PositionHistory.Remove(message.Camera);
});
if (mc.Serial != null)
{
// register with service locator
SimpleIoc.Default.Register(() => this, mc.Serial, true);
// notify existing cameras to add this controller
Messenger.Default.Send(new AddMotionControllerMessage(MotionController));
}
// initialize existing cameras
foreach (CameraViewModel scvm in SimpleIoc.Default.GetAllCreatedInstances<CameraViewModel>())
{
MotionController.RawPosition.Add(scvm.Camera, Float3.Zero);
MotionController.FusionPosition.Add(scvm.Camera, Float3.Zero);
MotionController.CameraPosition.Add(scvm.Camera, Float3.Zero);
MotionController.WorldPosition.Add(scvm.Camera, Float3.Zero);
MotionController.Tracking.Add(scvm.Camera, false);
MotionController.TrackerStatus.Add(scvm.Camera, PSMoveTrackerStatus.NotCalibrated);
MotionController.PositionHistory.Add(scvm.Camera, new Vector3[5]);
}
}
}
/// <summary>
/// for design purposes only
/// </summary>
public MotionControllerViewModel()
: this(new MotionControllerModel
{
Design = true,
Id = MotionControllerModel.COUNTER,
Name = "Design " + MotionControllerModel.COUNTER,
Serial = "00:00:00:00:00:0" + MotionControllerModel.COUNTER++,
},
new DesignMotionControllerService())
{
#if DEBUG
Random rnd = new Random();
if (IsInDesignMode)
{
CameraModel cam0 = new CameraModel { Name = "cam0" };
CameraModel cam1 = new CameraModel { Name = "cam1" };
MotionController = new MotionControllerModel
{
Name = "Design " + rnd.Next(10),
Serial = "00:00:00:00:00:0" + rnd.Next(10),
RawPosition = new ObservableConcurrentDictionary<CameraModel, Vector3>
{
{cam0, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))},
{cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))}
},
FusionPosition = new ObservableConcurrentDictionary<CameraModel, Vector3>
{
{cam0, new Vector3(rnd.Next(-50, 50), rnd.Next(-50, 50), rnd.Next(-50, 50))},
{cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))}
},
CameraPosition = new ObservableConcurrentDictionary<CameraModel, Vector3>
{
{cam0, new Vector3(rnd.Next(-50, 50), rnd.Next(-50, 50), rnd.Next(-50, 50))},
{cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))}
},
WorldPosition = new ObservableConcurrentDictionary<CameraModel, Vector3>
{
{cam0, new Vector3(rnd.Next(-50, 50), rnd.Next(-50, 50), rnd.Next(-50, 50))},
{cam1, new Vector3(rnd.Next(640), rnd.Next(480), rnd.Next(30))}
}
};
}
MotionController.Circle = rnd.Next(2) > 0;
MotionController.Cross = rnd.Next(2) > 0;
MotionController.Triangle = rnd.Next(2) > 0;
MotionController.Square = rnd.Next(2) > 0;
MotionController.Start = rnd.Next(2) > 0;
MotionController.Select = rnd.Next(2) > 0;
MotionController.Move = rnd.Next(2) > 0;
MotionController.PS = rnd.Next(2) > 0;
MotionController.Trigger = rnd.Next(256);
#endif
}
public AddMotionControllerMessage(MotionControllerModel motionController)
{
MotionController = motionController;
}
public MotionControllerModel Initialize(MotionControllerModel motionController)
{
return _motionController = motionController;
}
public void DoRefresh()
{
ObservableCollection<MotionControllerModel> existingControllers = new ObservableCollection<MotionControllerModel>();
AvailableMotionControllers.Clear();
NewMotionController = new MotionControllerModel();
NewMotionController.Name = null;
NewControllersDetected = false;
int connectedCount = PsMoveApi.count_connected();
if(connectedCount > 0)
{
foreach (MotionControllerViewModel mcvw in SimpleIoc.Default.GetAllCreatedInstances<MotionControllerViewModel>())
{
existingControllers.Add(mcvw.MotionController);
}
MotionControllerService motionControllerService = new MotionControllerService();
for (int i = 0; i < connectedCount; i++)
{
MotionControllerModel tmp = motionControllerService.Initialize(i);
if(existingControllers.Count > 0)
{
bool duplicate = false;
foreach (MotionControllerModel mcw in existingControllers)
{
if (tmp.ConnectStatus == PSMoveConnectStatus.OK)
{
if (tmp.Serial.Equals(mcw.Serial))
{
duplicate = true;
break;
}
}
} // end foreach
if (!duplicate) AvailableMotionControllers.Add(tmp);
}
else
{
if (tmp.ConnectStatus == PSMoveConnectStatus.OK)
{
AvailableMotionControllers.Add(tmp);
}
}
} // end for
}
if (AvailableMotionControllers.Count > 0) NewControllersDetected = true;
}
public void RemoveMotionController(MotionControllerModel mc)
{
_camera.Controllers.Remove(mc);
}
public void ProcessData(MotionControllerModel mc)
{
if (_camera.Handle != IntPtr.Zero)
{
Vector3 rawPosition = Vector3.zero;
Vector3 fusionPosition = Vector3.zero;
PSMoveTrackerStatus trackerStatus = mc.TrackerStatus[_camera];
if(!mc.Design) trackerStatus = PsMoveApi.psmove_tracker_get_status(_camera.Handle, mc.Handle);
if (trackerStatus == PSMoveTrackerStatus.Tracking)
{
float rx, ry, rrad;
float fx, fy, fz;
PsMoveApi.psmove_tracker_get_position(_camera.Handle, mc.Handle, out rx, out ry, out rrad);
PsMoveApi.psmove_fusion_get_position(_camera.Fusion, mc.Handle, out fx, out fy, out fz);
rx = (int) (rx + 0.5);
ry = (int) (ry + 0.5);
rawPosition = new Vector3(rx, ry, rrad);
fusionPosition = new Vector3(fx, fy, fz);
}
else if (mc.Design)
{
switch (_camera.Calibration.Index)
{
case 0:
rawPosition = new Vector3(129, 280, 8.970074f);
break;
case 1:
rawPosition = new Vector3(180, 293, 11.9714022f);
break;
case 2:
rawPosition = new Vector3(528, 286, 9.038924f);
break;
case 3:
rawPosition = new Vector3(389, 275, 6.530668f);
break;
}
}
mc.TrackerStatus[_camera] = trackerStatus;
mc.RawPosition[_camera] = rawPosition;
mc.FusionPosition[_camera] = fusionPosition;
if (trackerStatus == PSMoveTrackerStatus.Tracking || mc.Design)
{
// controller position -> rectangle in surrounding the sphere in image coordinates
PointF[] imgPts = CvHelper.GetImagePointsF(mc.RawPosition[_camera]);
ExtrinsicCameraParameters ex = CameraCalibration.FindExtrinsicCameraParams2(
_camera.Calibration.ObjectPoints2D,
imgPts,
_camera.Calibration.IntrinsicParameters);
Matrix<double> coordinatesInCameraSpace_homo = new Matrix<double>(new double[]
{
ex.TranslationVector[0, 0],
ex.TranslationVector[1, 0],
ex.TranslationVector[2, 0],
1
});
mc.CameraPosition[_camera] = new Vector3(
(float)coordinatesInCameraSpace_homo[0, 0],
(float)coordinatesInCameraSpace_homo[1, 0],
(float)coordinatesInCameraSpace_homo[2, 0]);
ex.RotationVector[0, 0] += (Math.PI / 180) * (_camera.Calibration.RotX + _camera.Calibration.XAngle);
ex.RotationVector[1, 0] += (Math.PI / 180) * (_camera.Calibration.RotY + _camera.Calibration.YAngle);
ex.RotationVector[2, 0] += (Math.PI / 180) * (_camera.Calibration.RotZ + _camera.Calibration.ZAngle);
_camera.Calibration.ExtrinsicParameters[mc.Id] = ex;
Matrix<double> minusRotation = new Matrix<double>(3, 3);
minusRotation = CvHelper.Rotate(
-_camera.Calibration.RotX - _camera.Calibration.XAngle,
-_camera.Calibration.RotY - _camera.Calibration.YAngle,
-_camera.Calibration.RotZ - _camera.Calibration.ZAngle);
Matrix<double> R3x3_cameraToWorld = new Matrix<double>(3, 3);
R3x3_cameraToWorld = CvHelper.Rotate(
_camera.Calibration.RotX,
_camera.Calibration.RotY + _camera.Calibration.YAngle,
_camera.Calibration.RotZ);
Matrix<double> rotInv = new Matrix<double>(3, 3);
CvInvoke.cvInvert(ex.RotationVector.RotationMatrix.Ptr, rotInv, SOLVE_METHOD.CV_LU);
Matrix<double> test = CvHelper.ConvertToHomogenous(-1*R3x3_cameraToWorld);
_camera.Calibration.ObjectPointsProjected = CameraCalibration.ProjectPoints(
_camera.Calibration.ObjectPoints3D,
_camera.Calibration.ExtrinsicParameters[mc.Id],
_camera.Calibration.IntrinsicParameters);
Matrix<double> cameraPositionInWorldSpace4x4 = new Matrix<double>(new double[,]
{
{1, 0, 0, _camera.Calibration.TranslationToWorld[0, 0]},
{0, 1, 0, _camera.Calibration.TranslationToWorld[1, 0]},
{0, 0, 1, _camera.Calibration.TranslationToWorld[2, 0]},
{0, 0, 0, 1},
});
Matrix<double> Rt_homo = CvHelper.ConvertToHomogenous(R3x3_cameraToWorld);
Matrix<double> x_world_homo = CvHelper.ConvertToHomogenous(minusRotation) * coordinatesInCameraSpace_homo;
Rt_homo[0, 3] = x_world_homo[0, 0];
Rt_homo[1, 3] = x_world_homo[1, 0];
Rt_homo[2, 3] = x_world_homo[2, 0];
x_world_homo = cameraPositionInWorldSpace4x4 * x_world_homo;
Vector3 v3world = new Vector3((float) x_world_homo[0, 0], (float) x_world_homo[1, 0],
(float) x_world_homo[2, 0]);
mc.WorldPosition[_camera] = v3world;
for (int i = mc.PositionHistory[_camera].Length - 1; i > 0; --i)
{
mc.PositionHistory[_camera][i] = mc.PositionHistory[_camera][i - 1];
}
mc.PositionHistory[_camera][0] = v3world;
}
}
} // ProcessData
public void DisableTracking(MotionControllerModel mc)
{
if (mc.Design) return;
PsMoveApi.psmove_tracker_disable(_camera.Handle, mc.Handle);
mc.Tracking[_camera] = false;
mc.TrackerStatus[_camera] = PSMoveTrackerStatus.NotCalibrated;
ConsoleService.Write(string.Format("[Tracker, {0}] Tracking of Motion Controller ({1}) disabled.",
_camera.GUID, mc.Serial));
}
public void EnableTracking(MotionControllerModel mc)
{
if (mc.Design) return;
if (_camera.Handle == IntPtr.Zero) StartTracker(PSMoveTrackerExposure.Low);
ConsoleService.Write(string.Format("[Tracker, {0}] Calibrating Motion Controller ({1}).", _camera.GUID, mc.Serial));
byte r = (byte)((mc.Color.r * 255) + 0.5f);
byte g = (byte)((mc.Color.g * 255) + 0.5f);
byte b = (byte)((mc.Color.b * 255) + 0.5f);
mc.TrackerStatus[_camera] = PsMoveApi.psmove_tracker_enable_with_color(_camera.Handle, mc.Handle, r, g, b);
if (mc.TrackerStatus[_camera] == PSMoveTrackerStatus.Tracking
|| mc.TrackerStatus[_camera] == PSMoveTrackerStatus.Calibrated)
{
PsMoveApi.psmove_tracker_update_image(_camera.Handle);
PsMoveApi.psmove_tracker_update(_camera.Handle, mc.Handle);
mc.TrackerStatus[_camera] = PsMoveApi.psmove_tracker_get_status(_camera.Handle, mc.Handle);
PsMoveApi.psmove_enable_orientation(mc.Handle, PSMoveBool.True);
PsMoveApi.psmove_reset_orientation(mc.Handle);
}
//Matrix4x4 proj = new Matrix4x4();
//for (int row = 0; row < 4; row++)
//{
// for (int col = 0; col < 4; col++)
// {
// proj[row, col] = PsMoveApi.PSMoveMatrix4x4_at(PsMoveApi.psmove_fusion_get_projection_matrix(_camera.Fusion), row * 4 + col);
// }
//}
//mc.ProjectionMatrix[_camera] = proj;
ConsoleService.Write(string.Format("[Tracker, {0}] Tracker Status of {1} = {2}",
_camera.GUID, mc.Name, Enum.GetName(typeof(PSMoveTrackerStatus), mc.TrackerStatus[_camera])));
}
public void RemoveMotionController(MotionControllerModel mc)
{
if(_camera.Controllers.Contains(mc))
{
if(mc.Tracking[_camera])
{
DisableTracking(mc);
}
_camera.Controllers.Remove(mc);
}
}
public MotionControllerModel Initialize(int id)
{
return MotionController = new MotionControllerModel();
}
