public Vector3D CrossProduct(Vector3D vector)
{
return new Vector3D(
(this.Y * vector.Z - this.Z * vector.Y),
(this.Z * vector.X - this.X * vector.Z),
(this.X * vector.Y - this.Y * vector.X)
);
}
public Vector3D Add(Vector3D vector)
{
if (vector == null)
{
return this;
}
return new Vector3D(
this.X + vector.X,
this.Y + vector.Y,
this.Z + vector.Z
);
}
/// <summary>
/// This method is used to calculate the RealWorldCalculation-coordinates for a given KinectPoint
/// When to use: use only to calculate the corner points' RealWorldCalculation-coordinates. For other KinectPoints use the other "CalculateRealWorldVector"
/// method.
/// How to use: The parameters rwA, rwB and rwC represent the corner points of the detected field. rwB is the RealWorld-point which is a direct
/// of the other RealWorld-points rwA and rwC. Direct means, that those point can be reached from rwB by following the adjacent outlines of the field.
/// Important: The passed KinectPoint must not be NULL!
/// </summary>
/// <param name="p">the KinectPoint which should be transformed to a RealWorldPoint </param>
/// <param name="rA">the RealWorldPoint A which represents a corner point and
/// is used to calculate the coordinates for the new RealWorldPoints </param>
/// <param name="rB">the RealWorldPoint B which represents a corner point and
/// is used to calculate the coordinates for the new RealWorldPoints </param>
/// <param name="rC">the RealWorldPoint C which represents a corner point and
/// is used to calculate the coordinates for the new RealWorldPoints </param>
/// <returns>Returns the RealWorldPoint corresponding to the passed KinectPoint</returns>
private RealWorldPoint CalculateRealWorldPoint(KinectPoint p, RealWorldPoint rA, RealWorldPoint rB, RealWorldPoint rC)
{
var rwPoint = new RealWorldPoint();
var vec_rA = CreateRealWorldVector(rA);
var vec_rB = CreateRealWorldVector(rB);
var vec_rC = CreateRealWorldVector(rC);
var vec_N = (vec_rA.Subtract(vec_rB)).CrossProduct(vec_rC.Subtract(vec_rB));
var q = vec_N.ScalarProduct(vec_rB);
var x = (int)((p.X - (Kinect.KINECT_IMAGE_WIDTH / 2)) /Kinect.WIDTH_CONST);
var y = (int)((p.Y - (Kinect.KINECT_IMAGE_HEIGHT / 2)) /Kinect.HEIGHT_CONST);
var vec_rP = new Vector3D(x,y,1);
rwPoint.Z = (int)(q / (vec_N.ScalarProduct(vec_rP)));
rwPoint.X = (int) ((p.X - (Kinect.KINECT_IMAGE_WIDTH / 2))*rwPoint.Z/Kinect.WIDTH_CONST);
rwPoint.Y = (int)((p.Y - (Kinect.KINECT_IMAGE_HEIGHT / 2)) * rwPoint.Z /Kinect.HEIGHT_CONST);
return rwPoint;
}
public Vector3D Subtract(Vector3D vector)
{
return new Vector3D(
this.X - vector.X,
this.Y - vector.Y,
this.Z - vector.Z
);
}
public double ScalarProduct(Vector3D vector)
{
return (double) (this.X*vector.X + this.Y*vector.Y + this.Z*vector.Z);
}
private Vector3D[] GetOriginPoint(Vector3D rwvA, Vector3D rwvB, Vector3D rwvC, Vector3D rwvD)
{
Vector3D[] coordinateSystemPoints = new Vector3D[3];
var dictVectorsA = new Dictionary<Vector3D, int>();
dictVectorsA.Add(rwvB, (int)(rwvB.Subtract(rwvA)).GetLength());
dictVectorsA.Add(rwvD, (int)(rwvD.Subtract(rwvA)).GetLength());
int sumA = dictVectorsA.Values.Sum();
var dictVectorsB = new Dictionary<Vector3D, int>();
dictVectorsB.Add(rwvA, (int)(rwvA.Subtract(rwvB)).GetLength());
dictVectorsB.Add(rwvC, (int)(rwvC.Subtract(rwvB)).GetLength());
int sumB = dictVectorsB.Values.Sum();
var dictVectorsC = new Dictionary<Vector3D, int>();
dictVectorsC.Add(rwvB, (int)(rwvB.Subtract(rwvC)).GetLength());
dictVectorsC.Add(rwvD, (int)(rwvD.Subtract(rwvC)).GetLength());
int sumC = dictVectorsC.Values.Sum();
var dictVectorsD = new Dictionary<Vector3D, int>();
dictVectorsD.Add(rwvC, (int)(rwvC.Subtract(rwvD)).GetLength());
dictVectorsD.Add(rwvA, (int)(rwvA.Subtract(rwvD)).GetLength());
int sumD = dictVectorsD.Values.Sum();
if (sumA < sumB && sumA < sumC && sumA < sumD)
{
coordinateSystemPoints[0] = rwvA;
coordinateSystemPoints[1] = dictVectorsA.FirstOrDefault((x) => x.Value == dictVectorsA.Values.Max()).Key;
coordinateSystemPoints[2] = dictVectorsA.FirstOrDefault((x) => x.Value == dictVectorsA.Values.Min()).Key;
}
else if (sumB < sumA && sumB < sumC && sumB < sumD)
{
coordinateSystemPoints[0] = rwvB;
coordinateSystemPoints[1] = dictVectorsB.FirstOrDefault((x) => x.Value == dictVectorsB.Values.Max()).Key;
coordinateSystemPoints[2] = dictVectorsB.FirstOrDefault((x) => x.Value == dictVectorsB.Values.Min()).Key;
}
else if (sumC < sumA && sumC < sumB && sumC < sumD)
{
coordinateSystemPoints[0] = rwvC;
coordinateSystemPoints[1] = dictVectorsC.FirstOrDefault((x) => x.Value == dictVectorsC.Values.Max()).Key;
coordinateSystemPoints[2] = dictVectorsC.FirstOrDefault((x) => x.Value == dictVectorsC.Values.Min()).Key;
}
else
{
coordinateSystemPoints[0] = rwvD;
coordinateSystemPoints[1] = dictVectorsD.FirstOrDefault((x) => x.Value == dictVectorsD.Values.Max()).Key;
coordinateSystemPoints[2] = dictVectorsD.FirstOrDefault((x) => x.Value == dictVectorsD.Values.Min()).Key;
}
var originPoint = Calibration.GetEdgePoints().Find((x) => x.RealWorldPoint.Equals(coordinateSystemPoints[0].ToRealWorldPoint()));
var xAxisPoint = Calibration.GetEdgePoints().Find((x) => x.RealWorldPoint.Equals(coordinateSystemPoints[1].ToRealWorldPoint()));
var yAxisPoint = Calibration.GetEdgePoints().Find((x) => x.RealWorldPoint.Equals(coordinateSystemPoints[2].ToRealWorldPoint()));
originPoint.PointType = PointType.Origin;
xAxisPoint.PointType = PointType.xAxis;
yAxisPoint.PointType = PointType.yAxis;
return coordinateSystemPoints;
}
