/// <summary>
/// Calculates the bounding box for the given trackable and updates it
/// </summary>
/// <param name="trackable"></param>
protected override void CalculateBoundingBox(StroMoHab_Objects.Objects.Trackable trackable)
{
float xMin, yMin, zMin, xMax, yMax, zMax;
CalculateMinMaxXYZ(trackable, out xMin, out yMin, out zMin, out xMax, out yMax, out zMax);
// Update the bounding box based on the calculated values and any offsets
UpdateBoundingBox(trackable.xCoordinate / SCALLINGFACTOR + xMin, trackable.yCoordinate / SCALLINGFACTOR + yMin + offsetY/2, -trackable.zCoordinate / SCALLINGFACTOR + zMin, trackable.xCoordinate / SCALLINGFACTOR + xMax, trackable.yCoordinate / SCALLINGFACTOR + yMax + offsetY/2, -trackable.zCoordinate / SCALLINGFACTOR + zMax);
}
private int SPHERERESOLUTION = 20; // Number of lines to build up spheres from
/// <summary>
/// Draws the Joint
/// </summary>
/// <param name="joint">The joint</param>
public void Draw(StroMoHab_Objects.Objects.Joint joint)
{
Gl.glTranslatef(joint.xCoordinate / SCALLINGFACTOR, joint.yCoordinate / SCALLINGFACTOR, -joint.zCoordinate / SCALLINGFACTOR);
Gl.glScalef(40f / SCALLINGFACTOR, 40f / SCALLINGFACTOR, 40f / SCALLINGFACTOR);
SetDrawingMode(true);
DrawUnitSphere();
SetDrawingMode(false);
DrawUnitSphere();
Gl.glScalef(1/40f / SCALLINGFACTOR, 1/40f / SCALLINGFACTOR, 1/40f / SCALLINGFACTOR);
Gl.glTranslatef(-joint.xCoordinate / SCALLINGFACTOR, -joint.yCoordinate / SCALLINGFACTOR, joint.zCoordinate / SCALLINGFACTOR);
SetDrawingMode(true);
}
/// <summary>
/// Draws the body part based on the trackable
/// </summary>
/// <param name="trackable">The trackable</param>
/// <param name="joint1">The first joint</param>
/// <param name="joint2">The second joint</param>
public virtual void Draw(StroMoHab_Objects.Objects.Trackable trackable, StroMoHab_Objects.Objects.Joint joint1, StroMoHab_Objects.Objects.Joint joint2)
{
//CalculateScallingAndOffsets(trackable, joint1, joint2);
CalculateBoundingBox(trackable);
MoveIn(trackable);
ScaleIn();
SetDrawingMode(true);
DrawBodyPart();
SetDrawingMode(false);
DrawBodyPart();
ScaleOut();
MoveOut(trackable);
SetDrawingMode(true);
}
/// <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>
/// Moves to the center of the trackalbe and sets the orientation so that the body part can be drawn
/// </summary>
protected virtual void MoveIn(StroMoHab_Objects.Objects.Trackable trackable)
{
// Move to center point of the trackalbe
Gl.glTranslatef(trackable.xCoordinate / SCALLINGFACTOR + offsetX, trackable.yCoordinate / SCALLINGFACTOR + offsetY, -trackable.zCoordinate / SCALLINGFACTOR - offsetZ);
// Rotate based on Pitch Yaw and Roll
Gl.glRotatef((float)trackable.Pitch, 1f, 0f, 0f);
Gl.glRotatef((float)trackable.Yaw, 0f, -1f, 0f);
Gl.glRotatef((float)trackable.Roll, 0f, 0f, -1f);
}
/// <summary> /// Implement code to calculate to size of the body part so that the unit shape can be scalled correctly /// and so it can be offset correctly based on connecting joint data /// </summary> /// <param name="trackable"></param> /// <param name="joint1"></param> /// <param name="joint2"></param> protected abstract void CalculateScallingAndOffsets(StroMoHab_Objects.Objects.Trackable trackable, StroMoHab_Objects.Objects.Joint joint1, StroMoHab_Objects.Objects.Joint joint2);
/// <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> /// Implement code to calculate the size of the bounding box /// </summary> protected abstract void CalculateBoundingBox(StroMoHab_Objects.Objects.Trackable trackable);
/// <summary>
/// Moves back to x/y/z = 0 and resets the orientation
/// </summary>
protected virtual void MoveOut(StroMoHab_Objects.Objects.Trackable trackable)
{
// Rotate back
Gl.glRotatef((float)trackable.Roll, 0f, 0f, 1f);
Gl.glRotatef((float)trackable.Yaw, 0f, 1f, 0f);
Gl.glRotatef((float)trackable.Pitch, -1f, 0f, 0f);
// Move back
Gl.glTranslatef(-trackable.xCoordinate / SCALLINGFACTOR - offsetX, -trackable.yCoordinate / SCALLINGFACTOR - offsetY, trackable.zCoordinate / SCALLINGFACTOR + offsetZ);
}
/// <summary>
/// Finds the Rotation matrix of a Trackable by using the Quaternion data
/// </summary>
/// <param name="Trackable"></param>
/// <returns></returns>
public static Matrix BuildRotorFromQuaternionData(StroMoHab_Objects.Objects.Trackable Trackable)
{
// Use the standard conversion from quaternion data into a single 3x3 3 axis of rotation rotation matrix
Matrix rotationMatrix;
rotationMatrix = Matrix.Create(new double[,] { { 1 - 2 * Trackable.QY * Trackable.QY - 2 * Trackable.QZ * Trackable.QZ, 2 * Trackable.QX * Trackable.QY - 2 * Trackable.QZ * Trackable.QW, 2 * Trackable.QX * Trackable.QZ + 2 * Trackable.QY * Trackable.QW },
{ 2 * Trackable.QX * Trackable.QY + 2 * Trackable.QZ * Trackable.QW, 1 - 2 * Trackable.QX * Trackable.QX - 2 * Trackable.QZ * Trackable.QZ, 2 * Trackable.QY * Trackable.QZ - 2 * Trackable.QX * Trackable.QW },
{ 2 * Trackable.QX * Trackable.QZ - 2 * Trackable.QY * Trackable.QW, 2 * Trackable.QY * Trackable.QZ + 2 * Trackable.QX * Trackable.QW, 1 - 2 * Trackable.QX * Trackable.QX - 2 * Trackable.QY * Trackable.QY } });
return rotationMatrix;
}
