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