/// <summary>
/// Calculates the difference in position between the real plane and the virtual plane.
/// </summary>
/// <returns>The difference in posi</returns>
/// <param name="r">The red component.</param>
/// <param name="v">V.</param>
Vector3 CalcDist(CalibrationPlane r, CalibrationPlane v)
{
Vector3 rcenter = GetCenterOfPoints(new Vector3[] { r.i, r.j, r.k });
Vector3 vcenter = GetCenterOfPoints(new Vector3[] { v.i, v.j, v.k });
var dist = rcenter - vcenter;
this.transform.position += dist;
return(dist);
}
/// <summary>
/// Calculates the difference of rotation and position by making an average of the rigid transformations for each of the 3 pivot points.
/// </summary>
/// <returns>The rotation pivot, the difference of rotation and position between two planes.</returns>
/// <param name="r">The real plane.</param>
/// <param name="v">The virtual plane, it should a plane on this game object</param>
/// <param name="vtrans">The position of the 3 points that define the virtual plane</param>
RigidTransformation CalcRotation(CalibrationPlane r, CalibrationPlane v, Transform[] vtrans)
{
var rigidTransformations = new RigidTransformation [3];
rigidTransformations [0] = CalcRotationPivot(r, v, vtrans, CalibrationPlane.Pivot.I);
rigidTransformations [1] = CalcRotationPivot(r, v, vtrans, CalibrationPlane.Pivot.J);
rigidTransformations [2] = CalcRotationPivot(r, v, vtrans, CalibrationPlane.Pivot.K);
var average = RigidTransformation.Average(rigidTransformations);
this.transform.rotation *= average.rotation;
this.transform.position += average.dist;
return(average);
}
/// <summary>
/// Calculates the difference of rotation and position between two planes around a specific pivot point
/// </summary>
/// <returns>The rotation pivot, the difference of rotation and position between two planes.</returns>
/// <param name="r">The real plane.</param>
/// <param name="v">The virtual plane, it should a plane on this game object</param>
/// <param name="vtrans">The position of the 3 points that define the virtual plane</param>
/// <param name="pivot">The pivot point of the rotation.</param>
RigidTransformation CalcRotationPivot(CalibrationPlane r, CalibrationPlane v, Transform[] vtrans, CalibrationPlane.Pivot pivot)
{
// We save the values of the current rotation and position to restore them at the end of the method
Quaternion startRotation = this.transform.rotation;
Vector3 startPosition = this.transform.position;
// We calculate n, the normal vector between the two normal of the calibration planes
Vector3 n = Vector3.Cross(r.normal, v.normal);
// We make sure that all the points in the virtual calibration plane are corresponding to their transform value
v.SetPoints(vtrans);
// We compute the 3 euler angles.
// Alpha is the rotation to align the virtual plane to the intersection of the two planes.
// Beta is the rotation to make the two planes parallel.
// Gamma is the rotation to align the points of the two planes now that the planes are parralel to each other.
float alpha = (n != Vector3.zero && v.PivotVect(pivot) != Vector3.zero) ? Vector3.SignedAngle(v.PivotVect(pivot), n, v.normal) : 0.0f;
float beta = (v.normal != Vector3.zero && r.normal != Vector3.zero) ? Vector3.SignedAngle(v.normal, r.normal, n) : 0.0f;
float gamma = (n != Vector3.zero && r.PivotVect(pivot) != Vector3.zero) ? Vector3.SignedAngle(n, r.PivotVect(pivot), r.normal) : 0.0f;
// Alpha rotation around the normal axis
this.transform.RotateAround(v.PivotPoint(pivot), v.normal, alpha);
// Once the rotation is done, we update the points
v.SetPoints(vtrans);
// Beta rotation around the pivot vect
this.transform.RotateAround(v.PivotPoint(pivot), v.PivotVect(pivot), beta);
// Once the rotation is done
v.SetPoints(vtrans);
// Gamma rotation
this.transform.RotateAround(v.PivotPoint(pivot), v.normal, gamma);
// The two planes are now parallel and aligned, we can compute the difference in rotation between the start and now.
var rigidTransformation = new RigidTransformation(Quaternion.Inverse(startRotation) * this.transform.rotation, this.transform.position - startPosition);
// We set the position and rotation back to where it was.
this.transform.rotation = startRotation;
this.transform.position = startPosition;
// We then reset the point in the virtual plane
v.SetPoints(vtrans);
return(rigidTransformation);
}
/// <summary>
/// Calibrate the objet to change its rotation, position and scale to match its position tags to the given positions tags.
/// </summary>
/// <param name="realPositionTags">Real position tags.</param>
void CalcCalibration(PositionTag[] realPositionTags)
{
int length = Mathf.Min(realPositionTags.Length, virtualPositionTags.Length);
int count = 0;
var startScale = this.transform.localScale;
var startRotation = this.transform.rotation;
var startPosition = this.transform.position;
var scaleList = new List <float> ();
var rotationList = new List <Quaternion> ();
var distList = new List <Vector3> ();
// To have the most accurate result we create two planes from all the possible unique combinations of 3 points and calculate an
// average of all transformations in rotation, position and scale.
for (int i = 0; i < length; i++)
{
for (int j = i + 1; j < length; j++)
{
for (int k = j + 1; k < length; k++)
{
var rtrans = new Transform[] {
realPositionTags [i].transform,
realPositionTags [j].transform,
realPositionTags [k].transform
};
var vtrans = new Transform[] {
virtualPositionTags [i].transform,
virtualPositionTags [j].transform,
virtualPositionTags [k].transform
};
var r = new CalibrationPlane(rtrans);
var v = new CalibrationPlane(vtrans);
var rigidTransformation = CalcRotation(r, v, vtrans);
var scale = CalcScale(r.normal, v.normal);
v.SetPoints(vtrans);
var dist = CalcDist(r, v);
count++;
// We add the calculated transformation to the corresponding lists
rotationList.Add(rigidTransformation.rotation);
scaleList.Add(scale);
distList.Add(rigidTransformation.dist + dist);
// Once the transformations are saved, we can go back to the starting position
this.transform.localScale = startScale;
this.transform.rotation = startRotation;
this.transform.position = startPosition;
}
}
}
this.transform.rotation *= MathHelper.AverageQuaternion(rotationList);
this.transform.localScale *= scaleList.Average();
this.transform.position += MathHelper.AverageVector3(distList);
}
