private void CalCulateTransformationMatrix(Point3D estimateOrigin)
{
CalibrateTransform = Matrix3D.Identity;
for (int i = 0; i < captureCalibrationPositions.Length; i++)
{
Vector3D vector = new Vector3D(captureCalibrationPositions[i].X - estimateOrigin.X, captureCalibrationPositions[i].Y - estimateOrigin.Y, captureCalibrationPositions[i].Z - estimateOrigin.Z);
vector.Normalize();
vector *= standardCalibrationPositions[i].ToVector3D().Length;
captureCalibrationPositions[i] = new Point3D(estimateOrigin.X + vector.X, estimateOrigin.Y + vector.Y, estimateOrigin.Z + vector.Z);
}
Vector3D captureAxisX = new Vector3D()
{
X = captureCalibrationPositions[1].X - captureCalibrationPositions[0].X,
Y = captureCalibrationPositions[1].Y - captureCalibrationPositions[0].Y,
Z = captureCalibrationPositions[1].Z - captureCalibrationPositions[0].Z
};
Vector3D captureAxisZ = new Vector3D()
{
X = captureCalibrationPositions[0].X - captureCalibrationPositions[2].X,
Y = captureCalibrationPositions[0].Y - captureCalibrationPositions[2].Y,
Z = captureCalibrationPositions[0].Z - captureCalibrationPositions[2].Z
};
Vector3D captureAxisY = Vector3D.CrossProduct(captureAxisX, captureAxisZ);
CalibrateTransform = Microsoft.Xna.Framework.Matrix.CreateWorld(estimateOrigin.ToVector3D().ToVector3(), captureAxisZ.ToVector3(), captureAxisY.ToVector3()).ToMatrix3D();
CalibrateTransform.Invert();
}
