C++ (Cpp) RigidBody::addFrame Examples

Example #1

File: ClientHandler.cpp Project: mdfeist/ProjectDR

// DataHandler receives data from the server
void __cdecl DataHandler(sFrameOfMocapData* data, void* pUserData) {
	// Cast user data as ClientHandler
	ClientHandler* pClient = (ClientHandler*) pUserData;

	// Lock the ClientHandler so data isn't changed
	// by another thread.
	if (!pClient->lock())
		return;

	// Unlock the ClientHandler
	pClient->unlock();

	// Rigid Bodies
	for(int i = 0; i < data->nRigidBodies; i++) {
		// Get pointer to Rigid Body
		RigidBody* body = pClient->getRigidBody(data->RigidBodies[i].ID);

		// Check if Rigid Body with the given ID exists
		if ( body ) {
			// Lock the ClientHandler so data isn't changed
			// by another thread.
			if (!pClient->lock())
				continue;

			// Update the Position and Orientation of the Rigid Body
			float x, y, z;
			float qx, qy, qz, qw;

			if (pClient->coordinateSystem()) {
				x = data->RigidBodies[i].x;
				y = data->RigidBodies[i].y;
				z = data->RigidBodies[i].z;

				qx = data->RigidBodies[i].qx;
				qy = data->RigidBodies[i].qy;
				qz = data->RigidBodies[i].qz;
				qw = data->RigidBodies[i].qw;
			} else {
				x = -data->RigidBodies[i].x;
				y = data->RigidBodies[i].z;
				z = data->RigidBodies[i].y;

				qx = -data->RigidBodies[i].qx;
				qy = data->RigidBodies[i].qz;
				qz = data->RigidBodies[i].qy;
				qw = data->RigidBodies[i].qw;
			}
				
			body->addFrame(Eigen::Vector3f(x, y, z),
				Eigen::Quaternionf(qw, qx, qy, qz));

			// Clear all the previous markers that were attached to the Rigid Body
			body->clearMarkers();

			// Gather all the Markers attached to the Rigid Body
			for(int iMarker = 0; iMarker < data->RigidBodies[i].nMarkers; iMarker++) {
				// Create a new marker
				Marker marker;

				// Get the information about the marker
				if(data->RigidBodies[i].MarkerIDs)
					marker.setID(data->RigidBodies[i].MarkerIDs[iMarker]);

				if(data->RigidBodies[i].Markers) {
					if (pClient->coordinateSystem()) {
						x = -data->RigidBodies[i].Markers[iMarker][0];
						y = data->RigidBodies[i].Markers[iMarker][1];
						z = data->RigidBodies[i].Markers[iMarker][2];
					} else {
						x = -data->RigidBodies[i].Markers[iMarker][0];
						z = data->RigidBodies[i].Markers[iMarker][1];
						y = data->RigidBodies[i].Markers[iMarker][2];
					}

					marker.setPosition(Eigen::Vector3f(x, y, z));
				}

				// Add the marker to the Rigid Body
				body->addMarker(marker);
			}

			// Unlock the ClientHandler
			pClient->unlock();
		}
	}

	// Update the OptiTrack data
	pClient->updateData();

}