public void PushRBodyReading(PolarisRigidBody rbody)
{
rigidBodyReads.Enqueue(rbody);
if (rigidBodyReads.Count > maxSize)
{
rigidBodyReads.Dequeue();
}
rigid = GetAverageFromQueue();
}
PolarisRigidBody ConvertRigidBodyIntoURSpace(PolarisRigidBody polaris)
{
//positive x is toward the camera
//positive y is to the right of the camera (camera pov)
//camera z is out toward the robot
//z -950 at closent limit to cam, 2400 is outer limit
//camera x is up negative up
//camera y is left and right positive right
//on rigid bodies ndi must be on top
//camera y positive is left and right
PolarisRigidBody converted = new PolarisRigidBody();
converted.x = (polaris.z / 1000);
converted.y = (polaris.y / 1000);
converted.z = ((-polaris.x) / 1000);
return(converted);
}
private PolarisRigidBody GetAverageFromQueue()
{
PolarisRigidBody returnBody = new PolarisRigidBody();
bool firstIter = true;
foreach (PolarisRigidBody body in rigidBodyReads)
{
if (firstIter)
{
firstIter = false;
returnBody.x = body.x;
returnBody.y = body.y;
returnBody.z = body.z;
returnBody.qx = body.qx;
returnBody.qy = body.qy;
returnBody.qz = body.qz;
returnBody.qo = body.qo;
}
else
{
returnBody.x += body.x;
returnBody.y += body.y;
returnBody.z += body.z;
returnBody.qx += body.qx;
returnBody.qy += body.qy;
returnBody.qz += body.qz;
returnBody.qo += body.qo;
}
}
returnBody.x = returnBody.x / rigidBodyReads.Count;
returnBody.y = returnBody.y / rigidBodyReads.Count;
returnBody.z = returnBody.z / rigidBodyReads.Count;
returnBody.qx = returnBody.qx / rigidBodyReads.Count;
returnBody.qy = returnBody.qy / rigidBodyReads.Count;
returnBody.qz = returnBody.qz / rigidBodyReads.Count;
returnBody.qo = returnBody.qo / rigidBodyReads.Count;
returnBody.isInRange = true;
returnBody.numberOfAverages = rigidBodyReads.Count;
return(returnBody);
}