C++ (Cpp) ArmGetTipOrientationUnitVector示例

示例#1

文件： vrpn_3DMicroscribe.C 项目： ASPePeX/vrpn

// This routine will reset the 3DMicroscribe, zeroing the Origin position,
// mode.
int	vrpn_3DMicroscribe::reset(void) 
{
#ifdef VRPN_USE_MICROSCRIBE
	int iResult;
	// ARM_FULL: ArmDll32 calculates and updates the Cartesian position and orientation of the stylus tip.
	iResult = ArmSetUpdate(ARM_FULL);
	
	if(iResult != ARM_SUCCESS) 
	{ 
		//error setting the update type, disconnect and end the thread
		VRPN_MSG_ERROR( "Unable to set the update type for the MicroScribe." );
		return -1;
	}

	//use mm instead of inches
	ArmSetLengthUnits(ARM_MM);
	//use radians instead of degrees
	//ArmSetAngleUnits(ARM_RADIANS);

	//get the position of the tip
	length_3D tipPosition;
	angle_3D tipVector;

	iResult = ArmGetTipPosition(&tipPosition); //retrieves the current stylus tip position in Cartesian coordinates
	iResult = ArmGetTipOrientationUnitVector(&tipVector); //retrieves the current stylus tip's unit vector orientation

	if(iResult == ARM_NOT_CONNECTED)
	{ 
		//error connecting
		VRPN_MSG_ERROR( "MicroScribe connection lost!" );
		return -1;
	}
	
#endif
	// We're now waiting for a response from the box
	status = STATUS_SYNCING;

	vrpn_gettimeofday(&timestamp, NULL);	// Set watchdog now
	return 0;
}

示例#2

文件： mtsMicroScribeDigitizer.cpp 项目： jhu-saw/sawMicroScribeDigitizer

void mtsMicroScribeDigitizer::Run(void)
{
    // timestamp of last reconnect trial in case of disconnection
    static double lastReconnectTryTime = 0;

    ProcessQueuedCommands();

    // If not connected, try reconnect at every 1 sec
    if (!DeviceConnected) {
        if (lastReconnectTryTime == 0) {
            lastReconnectTryTime = osaGetTime();
        } else if (lastReconnectTryTime + 1.0 * cmn_s < osaGetTime()) {
			CMN_LOG_CLASS_INIT_VERBOSE << "Run: Try reconnecting to digitizer..." << std::endl;
            if (ARM_SUCCESS == ArmReconnect()) {
                OnDeviceConnection();
			} else {
				CMN_LOG_CLASS_INIT_VERBOSE << "Run: Failed to reconnect to digitizer." << std::endl;
			}
        }
        return;
    }

    // read digitizer tip position
    if (ARM_NOT_CONNECTED == ArmGetTipPosition(&TipPositionVendor)) {
        // Disconnection detected
        OnDeviceDisconnection();
        return;
    }
    // read digitizer tip orientation 
    if (ARM_NOT_CONNECTED == ArmGetTipOrientation(&TipOrientationVendor)) {
        // Disconnection detected
        OnDeviceDisconnection();
        return;
    }
    // read digitizer tip orientation unit vector
    if (ARM_NOT_CONNECTED == ArmGetTipOrientationUnitVector(&TipOrientationUnitVectorVendor)) {
        // Disconnection detected
        OnDeviceDisconnection();
        return;
    }
    // read digitizer button states
    if (ARM_NOT_CONNECTED == ArmGetButtonsState(&ButtonStateVendor)) {
        // Disconnection detected
        OnDeviceDisconnection();
        return;
    }
    // read digitizer joint readings
    if (ARM_NOT_CONNECTED == ArmGetJointAngles(ARM_DEGREES, // or ARM_RADIANS
                                               JointReadingsVendor)) 
    {
        // Disconnection detected
        OnDeviceDisconnection();
        return;
    }

    // Convert vendor-defined container to cisst container
    // position
    TipPosition(X) = TipPositionVendor.x;
    TipPosition(Y) = TipPositionVendor.y;
    TipPosition(Z) = TipPositionVendor.z;
    // orientation
    TipOrientation(ROLL) = TipOrientationVendor.x;
    TipOrientation(PITCH) = TipOrientationVendor.y;
    TipOrientation(YAW) = TipOrientationVendor.z;
    // orientation unit vector
    TipOrientationUnitVector(ROLL) = TipOrientationUnitVectorVendor.x;
    TipOrientationUnitVector(PITCH) = TipOrientationUnitVectorVendor.y;
    TipOrientationUnitVector(YAW) = TipOrientationUnitVectorVendor.z;
    // button state
    static DWORD lastButtonStateVendor = 0;
    bool buttonStateChange = false;

    if (!(lastButtonStateVendor & ARM_BUTTON_1) && (ButtonStateVendor & ARM_BUTTON_1)) {
        EventButton1Down();
        ButtonState(BUTTON_1) = DOWN;
        buttonStateChange = true;
    } else if ((lastButtonStateVendor & ARM_BUTTON_1) && !(ButtonStateVendor & ARM_BUTTON_1)) {
        EventButton1Up();
        ButtonState(BUTTON_1) = UP;
        buttonStateChange = true;
    }

    if (!(lastButtonStateVendor & ARM_BUTTON_2) && (ButtonStateVendor & ARM_BUTTON_2)) {
        EventButton2Down();
        ButtonState(BUTTON_2) = DOWN;
        buttonStateChange = true;
    } else if ((lastButtonStateVendor & ARM_BUTTON_2) && !(ButtonStateVendor & ARM_BUTTON_2)) {
        EventButton2Up();
        ButtonState(BUTTON_2) = UP;
        buttonStateChange = true;
    }

    if (buttonStateChange) {
        lastButtonStateVendor = ButtonStateVendor;
    }

    // Joint readings
    for (size_t i = 0; i < NUM_DOF; ++i) {
        JointReadings(i) = JointReadingsVendor[i];
    }
}