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(); }