/// <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); }
/// <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> /// 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> /// 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); }
/// <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> /// 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> /// Implement code to calculate the size of the bounding box /// </summary> protected abstract void CalculateBoundingBox(StroMoHab_Objects.Objects.Trackable trackable);