Exemplo n.º 1
0
    /// <summary>
    /// Returns whether the connecting succeeded or not.
    ///
    /// NOTE! This function does NOT pair the controller by Bluetooth.
    /// If the controller is not already paired, it can only be connected by USB.
    /// See README for more information.
    /// </summary>
    public bool Init(int index)
    {
        handle = psmove_connect_by_id(index);
        if (handle == IntPtr.Zero)
        {
            return(false);
        }

        if (useTracking)
        {
            tracker = psmove_tracker_new();
            if (tracker == IntPtr.Zero)
            {
                return(false);
            }

            fusion = psmove_fusion_new(tracker, 1.0f, 1000.0f);

            while (trackerStatus != PSMoveTrackerStatus.Tracker_CALIBRATED)
            {
                trackerStatus = psmove_tracker_enable(tracker, handle);                 // Automatic color
                //trackerStatus = psmove_tracker_enable_with_color(tracker, handle, 255, 0, 255); // Magenta
                //trackerStatus = psmove_tracker_enable_with_color(tracker, handle, 0, 255, 255); // Cyan
                //trackerStatus = psmove_tracker_enable_with_color(tracker, handle, 255, 255, 0); // Yellow
                //trackerStatus = psmove_tracker_enable_with_color(tracker, handle, 255, 0, 0); // Red
                //	trackerStatus = psmove_tracker_enable_with_color(tracker, handle, 0, 255, 0); // Green
                //trackerStatus = psmove_tracker_enable_with_color(tracker, handle, 0, 0, 255); // Blue
                //trackerStatus = psmove_tracker_enable_with_color(tracker, handle, 255, 255, 255); // White
            }
        }

        hasCalibration = psmove_has_calibration(handle);
        if (hasCalibration != 1)
        {
            Debug.Log("Error: no calibration ! (result: " + hasCalibration + ")");
        }

        psmove_enable_orientation(handle, 1);

        // Make sure the connection is actually sending data. If not, this is probably a controller
        // you need to remove manually from the OSX Bluetooth Control Panel, then re-connect.
        return(psmove_update_leds(handle) != 0);
    }
Exemplo n.º 2
0
        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