/// <summary>
/// Calculates the min and max x y and z values for the bounding box that encapsulates the trackable
/// </summary>
/// <param name="trackable"></param>
/// <param name="xMin"></param>
/// <param name="yMin"></param>
/// <param name="zMin"></param>
/// <param name="xMax"></param>
/// <param name="yMax"></param>
/// <param name="zMax"></param>
protected void CalculateMinMaxXYZ(StroMoHab_Objects.Objects.Trackable trackable, out float xMin, out float yMin, out float zMin, out float xMax, out float yMax, out float zMax)
{
// Calculate the min and max X Y and Z values
StroMoHab_Matrix.RotationMatrix rotationMatrix = new StroMoHab_Matrix.RotationMatrix(+Math.PI * trackable.Pitch / 180, +Math.PI * trackable.Yaw / 180, +Math.PI * trackable.Roll / 180);
List <StroMoHab_Matrix.PointMatrix> pointMatrixList = BuildCuboidCorners(scaleX, scaleY, scaleZ);
List <StroMoHab_Matrix.PointMatrix> rotatedPointMatrixList = RotateCuboidCorners(pointMatrixList, rotationMatrix);
FindMaxMinCorners(rotatedPointMatrixList, out xMin, out yMin, out zMin, out xMax, out yMax, out zMax);
}
/// <summary>
/// Returns the Trackable for the index specified
/// </summary>
/// <param name="TrackableIndex">The index of the trackable</param>
/// <returns>The result</returns>
public static Trackable GetTrackableLocation(int TrackableIndex)
{
Trackable trackable = new Trackable();
float x, y, z;
float qx, qy, qz, qw;
float yaw, pitch, roll;
GetTrackableLocation(TrackableIndex, out x, out y, out z, out qx, out qy, out qz, out qw, out yaw, out pitch, out roll);
trackable.xCoordinate = (int)(x * 1000); // Positive faceing the screen going right
trackable.yCoordinate = (int)(y * 1000); // Positive Up
trackable.zCoordinate = (int)(z * 1000); // Positive Towards the screen
trackable.Yaw = -Math.Round(yaw, 1); // invert yaw
trackable.Pitch = Math.Round(roll, 1); // Swap around pitch and roll
trackable.Roll = Math.Round(pitch, 1);
trackable.TimeStamp = System.DateTime.Now.Ticks;
trackable.ID = TrackableID(TrackableIndex);
trackable.Name = TT_TrackableName(TrackableIndex);
trackable.TrackableIndex = TrackableIndex;
trackable.QW = qw;
trackable.QX = qx;
trackable.QY = qy;
trackable.QZ = qz;
Marker trackableMarker = null;
trackable.TrackableMarkers = new List <Marker>();
// Build the rotation matrix
StroMoHab_Matrix.RotationMatrix rotationMatix = new StroMoHab_Matrix.RotationMatrix(Math.PI * trackable.Pitch / 180, Math.PI * trackable.Yaw / 180, Math.PI * trackable.Roll / 180);
//loop through and get all of the markers that are part of the trackable, apply the rotation matrix, and add them
for (int j = 0; j < TrackingTools.TrackableMarkerCount(TrackableIndex); j++)
{
trackableMarker = TrackingTools.TrackableMarker(TrackableIndex, j); // get the marker
// rotate the marker
StroMoHab_Matrix.PointMatrix newPointMatrix = StroMoHab_Matrix.Operations.Rotate(new StroMoHab_Matrix.PointMatrix(trackableMarker.xCoordinate, trackableMarker.yCoordinate, trackableMarker.zCoordinate), rotationMatix);
//get the new marker position
trackableMarker.xCoordinate = Math.Round(newPointMatrix.XCoordinate, 1);
trackableMarker.yCoordinate = Math.Round(newPointMatrix.YCoordinate, 1);
trackableMarker.zCoordinate = Math.Round(newPointMatrix.ZCoordinate, 1);
trackable.TrackableMarkers.Add(trackableMarker);
}
return(trackable);
}
/// <summary>
/// Calculates the scalling and offset values for the trackable which determin its size and position
/// </summary>
/// <param name="trackable"></param>
/// <param name="joint1"></param>
/// <param name="joint2"></param>
protected override void CalculateScallingAndOffsets(StroMoHab_Objects.Objects.Trackable trackable, StroMoHab_Objects.Objects.Joint joint1, StroMoHab_Objects.Objects.Joint joint2)
{
if (joint1.Exists)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint1PointVector = new StroMoHab_Matrix.PointMatrix(joint1.xCoordinate - trackable.xCoordinate, joint1.yCoordinate - trackable.yCoordinate, joint1.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint1NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint1PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
scaleZ = (float)joint1NewPointVector.ZCoordinate / SCALLINGFACTOR;
scaleY = (float)joint1NewPointVector.YCoordinate / SCALLINGFACTOR;
// Protect against really small limbs
if (joint1NewPointVector.ZCoordinate < 100)
scaleZ = Math.Abs((float)100 / SCALLINGFACTOR);
offsetY = -scaleY;
}
}
/// <summary>
/// Calculates the scalling and offset values for the trackable which determin its size and position
/// </summary>
/// <param name="trackable"></param>
/// <param name="joint1"></param>
/// <param name="joint2"></param>
protected override void CalculateScallingAndOffsets(StroMoHab_Objects.Objects.Trackable trackable, StroMoHab_Objects.Objects.Joint joint1, StroMoHab_Objects.Objects.Joint joint2)
{
if (joint1.Exists)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint1PointVector = new StroMoHab_Matrix.PointMatrix(joint1.xCoordinate - trackable.xCoordinate, joint1.yCoordinate - trackable.yCoordinate, joint1.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint1NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint1PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
scaleZ = (float)joint1NewPointVector.ZCoordinate / SCALLINGFACTOR;
scaleY = (float)joint1NewPointVector.YCoordinate / SCALLINGFACTOR;
// Protect against really small limbs
if (joint1NewPointVector.ZCoordinate < 100)
{
scaleZ = Math.Abs((float)100 / SCALLINGFACTOR);
}
offsetY = -scaleY;
}
}
/// <summary>
/// Calculates the scalling and offset values for the trackable which determin its size and position
/// </summary>
/// <param name="trackable"></param>
/// <param name="joint1"></param>
/// <param name="joint2"></param>
protected override void CalculateScallingAndOffsets(StroMoHab_Objects.Objects.Trackable trackable, StroMoHab_Objects.Objects.Joint joint1, StroMoHab_Objects.Objects.Joint joint2)
{
if (joint1.Exists && joint2.Exists == false)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint1PointVector = new StroMoHab_Matrix.PointMatrix(joint1.xCoordinate - trackable.xCoordinate, joint1.yCoordinate - trackable.yCoordinate, joint1.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint1NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint1PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
scaleZ = (float)joint1NewPointVector.ZCoordinate / SCALLINGFACTOR;
scaleY = (float)joint1NewPointVector.YCoordinate / SCALLINGFACTOR;
if (joint1NewPointVector.ZCoordinate < 150)
scaleZ = Math.Abs((float)150 / SCALLINGFACTOR);
if (joint1NewPointVector.YCoordinate < 30)
scaleY = Math.Abs((float)30 / SCALLINGFACTOR);
offsetY = -scaleY;
}
else if (joint2.Exists && joint1.Exists == false)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint2PointVector = new StroMoHab_Matrix.PointMatrix(joint2.xCoordinate - trackable.xCoordinate, joint2.yCoordinate - trackable.yCoordinate, joint2.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint2NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint2PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
scaleZ = Math.Abs((float)joint2NewPointVector.ZCoordinate / SCALLINGFACTOR);
scaleY = Math.Abs((float)joint2NewPointVector.YCoordinate / SCALLINGFACTOR);
if (joint2NewPointVector.ZCoordinate < 150)
scaleZ = Math.Abs((float)150 / SCALLINGFACTOR);
if (joint2NewPointVector.YCoordinate < 30)
scaleY = Math.Abs((float)30 / SCALLINGFACTOR);
offsetY = -scaleY;
}
else if (joint1.Exists && joint2.Exists)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint2PointVector = new StroMoHab_Matrix.PointMatrix(joint2.xCoordinate - trackable.xCoordinate, joint2.yCoordinate - trackable.yCoordinate, joint2.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
StroMoHab_Matrix.PointMatrix joint1PointVector = new StroMoHab_Matrix.PointMatrix(joint1.xCoordinate - trackable.xCoordinate, joint1.yCoordinate - trackable.yCoordinate, joint1.zCoordinate - trackable.zCoordinate);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint1NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint1PointVector, rotationMatrixReverse);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint2NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint2PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
//Half of the distance between the two joints
scaleZ = (float)(Math.Abs(joint1NewPointVector.ZCoordinate) + Math.Abs(joint2NewPointVector.ZCoordinate) / 2) / SCALLINGFACTOR;
if(scaleZ < Math.Abs((float)150 / SCALLINGFACTOR))
scaleZ = Math.Abs((float)150 / SCALLINGFACTOR);
scaleY = Math.Abs((float)joint1NewPointVector.YCoordinate / SCALLINGFACTOR);
offsetY = -scaleY;
}
}
/// <summary>
/// Calculates the min and max x y and z values for the bounding box that encapsulates the trackable
/// </summary>
/// <param name="trackable"></param>
/// <param name="xMin"></param>
/// <param name="yMin"></param>
/// <param name="zMin"></param>
/// <param name="xMax"></param>
/// <param name="yMax"></param>
/// <param name="zMax"></param>
protected void CalculateMinMaxXYZ(StroMoHab_Objects.Objects.Trackable trackable, out float xMin, out float yMin, out float zMin, out float xMax, out float yMax, out float zMax)
{
// Calculate the min and max X Y and Z values
StroMoHab_Matrix.RotationMatrix rotationMatrix = new StroMoHab_Matrix.RotationMatrix(+Math.PI * trackable.Pitch / 180, +Math.PI * trackable.Yaw / 180, +Math.PI * trackable.Roll / 180);
List<StroMoHab_Matrix.PointMatrix> pointMatrixList = BuildCuboidCorners(scaleX, scaleY, scaleZ);
List<StroMoHab_Matrix.PointMatrix> rotatedPointMatrixList = RotateCuboidCorners(pointMatrixList, rotationMatrix);
FindMaxMinCorners(rotatedPointMatrixList, out xMin, out yMin, out zMin, out xMax, out yMax, out zMax);
}
/// <summary>
/// Calculates the scalling and offset values for the trackable which determin its size and position
/// </summary>
/// <param name="trackable"></param>
/// <param name="joint1"></param>
/// <param name="joint2"></param>
protected override void CalculateScallingAndOffsets(StroMoHab_Objects.Objects.Trackable trackable, StroMoHab_Objects.Objects.Joint joint1, StroMoHab_Objects.Objects.Joint joint2)
{
if (joint1.Exists && joint2.Exists == false)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint1PointVector = new StroMoHab_Matrix.PointMatrix(joint1.xCoordinate - trackable.xCoordinate, joint1.yCoordinate - trackable.yCoordinate, joint1.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint1NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint1PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
scaleZ = (float)joint1NewPointVector.ZCoordinate / SCALLINGFACTOR;
scaleY = (float)joint1NewPointVector.YCoordinate / SCALLINGFACTOR;
if (joint1NewPointVector.ZCoordinate < 150)
{
scaleZ = Math.Abs((float)150 / SCALLINGFACTOR);
}
if (joint1NewPointVector.YCoordinate < 30)
{
scaleY = Math.Abs((float)30 / SCALLINGFACTOR);
}
offsetY = -scaleY;
}
else if (joint2.Exists && joint1.Exists == false)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint2PointVector = new StroMoHab_Matrix.PointMatrix(joint2.xCoordinate - trackable.xCoordinate, joint2.yCoordinate - trackable.yCoordinate, joint2.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint2NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint2PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
scaleZ = Math.Abs((float)joint2NewPointVector.ZCoordinate / SCALLINGFACTOR);
scaleY = Math.Abs((float)joint2NewPointVector.YCoordinate / SCALLINGFACTOR);
if (joint2NewPointVector.ZCoordinate < 150)
{
scaleZ = Math.Abs((float)150 / SCALLINGFACTOR);
}
if (joint2NewPointVector.YCoordinate < 30)
{
scaleY = Math.Abs((float)30 / SCALLINGFACTOR);
}
offsetY = -scaleY;
}
else if (joint1.Exists && joint2.Exists)
{
// Set up matricies
StroMoHab_Matrix.PointMatrix joint2PointVector = new StroMoHab_Matrix.PointMatrix(joint2.xCoordinate - trackable.xCoordinate, joint2.yCoordinate - trackable.yCoordinate, joint2.zCoordinate - trackable.zCoordinate);
StroMoHab_Matrix.RotationMatrix rotationMatrixReverse = new StroMoHab_Matrix.RotationMatrix(-Math.PI * trackable.Pitch / 180, -Math.PI * trackable.Yaw / 180, -Math.PI * trackable.Roll / 180);
StroMoHab_Matrix.PointMatrix joint1PointVector = new StroMoHab_Matrix.PointMatrix(joint1.xCoordinate - trackable.xCoordinate, joint1.yCoordinate - trackable.yCoordinate, joint1.zCoordinate - trackable.zCoordinate);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint1NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint1PointVector, rotationMatrixReverse);
// Rotate the joint
StroMoHab_Matrix.PointMatrix joint2NewPointVector = StroMoHab_Matrix.Operations.Rotate(joint2PointVector, rotationMatrixReverse);
// Calculate the scaling and offsets based on the vector from the trackable to the joint
//Half of the distance between the two joints
scaleZ = (float)(Math.Abs(joint1NewPointVector.ZCoordinate) + Math.Abs(joint2NewPointVector.ZCoordinate) / 2) / SCALLINGFACTOR;
if (scaleZ < Math.Abs((float)150 / SCALLINGFACTOR))
{
scaleZ = Math.Abs((float)150 / SCALLINGFACTOR);
}
scaleY = Math.Abs((float)joint1NewPointVector.YCoordinate / SCALLINGFACTOR);
offsetY = -scaleY;
}
}
/// <summary>
/// Returns the Trackable for the index specified
/// </summary>
/// <param name="TrackableIndex">The index of the trackable</param>
/// <returns>The result</returns>
public static Trackable GetTrackableLocation(int TrackableIndex)
{
Trackable trackable = new Trackable();
float x, y, z;
float qx, qy, qz, qw;
float yaw, pitch, roll;
GetTrackableLocation(TrackableIndex, out x, out y, out z, out qx, out qy, out qz, out qw, out yaw, out pitch, out roll);
trackable.xCoordinate = (int)(x * 1000); // Positive faceing the screen going right
trackable.yCoordinate = (int)(y * 1000); // Positive Up
trackable.zCoordinate = (int)(z * 1000); // Positive Towards the screen
trackable.Yaw = -Math.Round(yaw, 1); // invert yaw
trackable.Pitch = Math.Round(roll, 1); // Swap around pitch and roll
trackable.Roll = Math.Round(pitch, 1);
trackable.TimeStamp = System.DateTime.Now.Ticks;
trackable.ID = TrackableID(TrackableIndex);
trackable.Name = TT_TrackableName(TrackableIndex);
trackable.TrackableIndex = TrackableIndex;
trackable.QW = qw;
trackable.QX = qx;
trackable.QY = qy;
trackable.QZ = qz;
Marker trackableMarker = null;
trackable.TrackableMarkers = new List<Marker>();
// Build the rotation matrix
StroMoHab_Matrix.RotationMatrix rotationMatix = new StroMoHab_Matrix.RotationMatrix(Math.PI * trackable.Pitch / 180, Math.PI * trackable.Yaw / 180, Math.PI * trackable.Roll / 180);
//loop through and get all of the markers that are part of the trackable, apply the rotation matrix, and add them
for (int j = 0; j < TrackingTools.TrackableMarkerCount(TrackableIndex); j++)
{
trackableMarker = TrackingTools.TrackableMarker(TrackableIndex, j); // get the marker
// rotate the marker
StroMoHab_Matrix.PointMatrix newPointMatrix = StroMoHab_Matrix.Operations.Rotate(new StroMoHab_Matrix.PointMatrix(trackableMarker.xCoordinate, trackableMarker.yCoordinate, trackableMarker.zCoordinate), rotationMatix);
//get the new marker position
trackableMarker.xCoordinate = Math.Round(newPointMatrix.XCoordinate, 1);
trackableMarker.yCoordinate = Math.Round(newPointMatrix.YCoordinate, 1);
trackableMarker.zCoordinate = Math.Round(newPointMatrix.ZCoordinate, 1);
trackable.TrackableMarkers.Add(trackableMarker);
}
return trackable;
}
/// <summary>
/// Takes a list of corner points and rotates them
/// </summary>
protected virtual List <StroMoHab_Matrix.PointMatrix> RotateCuboidCorners(List <StroMoHab_Matrix.PointMatrix> pointMatrixList, StroMoHab_Matrix.RotationMatrix rotationMatrix)
{
List <StroMoHab_Matrix.PointMatrix> rotatedPointMatrixList = new List <StroMoHab_Matrix.PointMatrix>();
foreach (StroMoHab_Matrix.PointMatrix pointMatrix in pointMatrixList)
{
StroMoHab_Matrix.PointMatrix rotatedPointMatrix = StroMoHab_Matrix.Operations.Rotate(pointMatrix, rotationMatrix);
rotatedPointMatrixList.Add(rotatedPointMatrix);
}
return(rotatedPointMatrixList);
}
/// <summary>
/// Rotates the given point by the given rotation matrix and returns a point matrix
/// </summary>
/// <param name="pointMatrix">The 1x3 point matrix</param>
/// <param name="rotationMatrix">The 3 3x3 rotation matricies</param>
/// <returns>The new 1x3 point matrix</returns>
public static StroMoHab_Matrix.PointMatrix Rotate(StroMoHab_Matrix.PointMatrix pointMatrix, StroMoHab_Matrix.RotationMatrix rotationMatrix)
{
Matrix newPointMatrix = newPointMatrix = rotationMatrix.rotationMatrixZ * pointMatrix.pointMatrix;
newPointMatrix = rotationMatrix.rotationMatrixY * newPointMatrix;
newPointMatrix = rotationMatrix.rotationMatrixX * newPointMatrix;
StroMoHab_Matrix.PointMatrix returnedPointMatrix = new PointMatrix();
returnedPointMatrix.pointMatrix = newPointMatrix;
return(returnedPointMatrix);
}
