public Quaternion finalOrientation(float time, Vector3 position)
{
float gF = findXOverTimeOrientation(_time0, _time1, time);
position = ToricComputing.FromWorldPosition(position, _keypoint1._target1.transform.position, _keypoint1._target2.transform.position);
Toricmanifold T1 = new Toricmanifold(position.x * Mathf.Rad2Deg, position.y * Mathf.Rad2Deg, position.z * Mathf.Rad2Deg, _keypoint1._target1, _keypoint1._target2);
Vector2[] desPosT1 = _keypoint1.getDesiredScreenPositions();
T1.SetDesiredPosition(desPosT1[0], desPosT1[1]);
position = ToricComputing.FromWorldPosition(position, _keypoint2._target1.transform.position, _keypoint2._target2.transform.position);
Toricmanifold T2 = new Toricmanifold(position.x * Mathf.Rad2Deg, position.y * Mathf.Rad2Deg, position.z * Mathf.Rad2Deg, _keypoint2._target1, _keypoint2._target2);
Vector2[] desPosT2 = _keypoint2.getDesiredScreenPositions();
T2.SetDesiredPosition(desPosT2[0], desPosT2[1]);
Quaternion qT1 = T1.ComputeOrientation();
Quaternion qT2 = T2.ComputeOrientation();
//controlling motion along time
Quaternion rotation = Quaternion.Slerp(qT1, qT2, gF);
return(rotation);
}
private void testFromWorld()
{
Toricmanifold test = new Toricmanifold(alpha, theta, phi, target1, target2);
test.SetDesiredPosition(screenPos1, screenPos2);
test.visualize(Color.red);
Vector3 worldPos = test.ToWorldPosition();
Vector3 toricRep = ToricComputing.FromWorldPosition(worldPos, target1.transform.position, target2.transform.position);
Toricmanifold test2 = new Toricmanifold(toricRep.x * Mathf.Rad2Deg, toricRep.y * Mathf.Rad2Deg, toricRep.z * Mathf.Rad2Deg, target1, target2);
test2.SetDesiredPosition(screenPos1, screenPos2);
test2.visualize(Color.green);
}
private Vector3 calculateTrajectory(float x, Toricmanifold targetAB)
{
Vector3[] targets = { targetAB._target1.transform.position, targetAB._target2.transform.position };
Vector3 AB = targets[1] - targets[0];
Vector3 p0 = ToricComputing.FromWorldPosition(_keypoint1.ToWorldPosition(), targets[0], targets[1]);
Vector3 p1 = ToricComputing.FromWorldPosition(_keypoint2.ToWorldPosition(), targets[0], targets[1]);
float alphaInterpolated = p0.x * (1 - x) + p1.x * x;
alphaInterpolated *= Mathf.Rad2Deg;
Vector3 vantageA0 = _keypoint1.ToWorldPosition() - targets[0];
float distanceA0 = vantageA0.magnitude;
vantageA0 = vantageA0.normalized;
Vector3 vantageA1 = _keypoint2.ToWorldPosition() - targets[0];
float distanceA1 = vantageA1.magnitude;
vantageA1 = vantageA1.normalized;
Vector3 interpolatedVa = (1 - x) * vantageA0 + x * vantageA1;
float interpolatedDistanceA = (1 - x) * distanceA0 + x * distanceA1;
Vector3 vantageB0 = _keypoint1.ToWorldPosition() - targets[1];
float distanceB0 = vantageB0.magnitude;
vantageB0 = vantageB0.normalized;
Vector3 vantageB1 = _keypoint2.ToWorldPosition() - targets[1];
float distanceB1 = vantageB1.magnitude;
vantageB1 = vantageB1.normalized;
Vector3 interpolatedVb = (1 - x) * vantageB0 + x * vantageB1;
float interpolatedDistanceB = (1 - x) * distanceB0 + x * distanceB1;
float thetaInterpolatedA = 2 * Vector3.Angle(interpolatedVa, AB);
float thetaInterpolatedB = 2 * (180 - Vector3.Angle(-AB, interpolatedVb) - alphaInterpolated);
Vector3 upOnPlane = Vector3.ProjectOnPlane(Vector3.up, AB);
Vector3 VAonPlane = Vector3.ProjectOnPlane(interpolatedVa, AB);
Vector3 phiZero = Vector3.Cross(upOnPlane, AB);
float phiInterpolatedA = Vector3.SignedAngle(phiZero, VAonPlane, -AB);
Toricmanifold intersectionTa = new Toricmanifold(alphaInterpolated, thetaInterpolatedA, phiInterpolatedA, targetAB._target1, targetAB._target2);
float distanceAT = (intersectionTa.ToWorldPosition() - targets[0]).magnitude;
upOnPlane = Vector3.ProjectOnPlane(Vector3.up, AB);
Vector3 VBonPlane = Vector3.ProjectOnPlane(interpolatedVa, AB);
phiZero = Vector3.Cross(upOnPlane, AB);
float phiInterpolatedB = Vector3.SignedAngle(phiZero, VBonPlane, -AB);
Toricmanifold intersectionTb = new Toricmanifold(alphaInterpolated, thetaInterpolatedB, phiInterpolatedB, targetAB._target1, targetAB._target2);
float distanceBT = (intersectionTb.ToWorldPosition() - targets[1]).magnitude;
float lambdaA = Mathf.Sin(Vector3.Angle(AB, interpolatedVa));
float lambdaB = Mathf.Sin(Vector3.Angle(AB, interpolatedVb));
Vector3 interpolatedTargets1 = targets[0] + interpolatedVa * 0.5f * (interpolatedDistanceA + distanceAT);
Vector3 interpolatedeTargets2 = targets[1] + interpolatedVb * 0.5f * (interpolatedDistanceB + distanceBT);
return(0.5f * (interpolatedTargets1 + interpolatedeTargets2));
}
private Vector3 calculateTrajectory(float x, Toricmanifold targetAB)
{
Vector3[] targets = { targetAB._target1.transform.position, targetAB._target2.transform.position };
Vector3 AB = targets[1] - targets[0];
Vector3 p0 = ToricComputing.FromWorldPosition(_keypoint1.ToWorldPosition(), targets[0], targets[1]);
Vector3 p1 = ToricComputing.FromWorldPosition(_keypoint2.ToWorldPosition(), targets[0], targets[1]);
float alphaInterpolated = p0.x * x + p1.x * (1 - x);
Vector3 vantageA0 = _keypoint1.ToWorldPosition() - targets[0];
float distanceA0 = vantageA0.magnitude;
vantageA0 = vantageA0.normalized;
Vector3 vantageA1 = _keypoint2.ToWorldPosition() - targets[0];
float distanceA1 = vantageA1.magnitude;
vantageA0 = vantageA1.normalized;
Vector3 interpolatedVa = x * vantageA0 + (1 - x) * vantageA1;
float interpolatedDistanceA = x * distanceA0 + (1 - x) * distanceA1;
Vector3 vantageB0 = _keypoint1.ToWorldPosition() - targets[1];
float distanceB0 = vantageB0.magnitude;
vantageB0 = vantageB0.normalized;
Vector3 vantageB1 = _keypoint2.ToWorldPosition() - targets[1];
float distanceB1 = vantageB1.magnitude;
vantageB1 = vantageB1.normalized;
Vector3 interpolatedVb = x * vantageB0 + (1 - x) * vantageB1;
float interpolatedDistanceB = x * distanceB0 + (1 - x) * distanceB1;
float thetaInterpolatedA = 2 * Vector3.Angle(interpolatedVa, AB);
float thetaInterpolatedB = 2 * (Mathf.PI - Vector3.Angle(interpolatedVb, AB) - alphaInterpolated);
Vector3 n = Vector3.Cross(interpolatedVa, AB).normalized;
Vector3 z;
Vector2 n2 = new Vector2(AB.x, AB.z);
float tmp = n2[0];
n2[0] = n2[1];
n2[1] = -tmp;
z = new Vector3(n2.x, 0, n2.y).normalized;
float phiInterpolatedA = Vector3.SignedAngle(n, -AB, z) * Mathf.Deg2Rad - Mathf.PI / 2;
Toricmanifold intersectionTa = new Toricmanifold(alphaInterpolated * Mathf.Rad2Deg, thetaInterpolatedA * Mathf.Rad2Deg, phiInterpolatedA * Mathf.Rad2Deg, targetAB._target1, targetAB._target2);
float distanceAT = (intersectionTa.ToWorldPosition() - targets[0]).magnitude;
n = Vector3.Cross(interpolatedVb, AB).normalized;
n2 = new Vector2(AB.x, AB.z);
tmp = n2[0];
n2[0] = n2[1];
n2[1] = -tmp;
z = new Vector3(n2.x, 0, n2.y).normalized;
float phiInterpolatedB = Vector3.SignedAngle(n, -AB, z) * Mathf.Deg2Rad - Mathf.PI / 2;
Toricmanifold intersectionTb = new Toricmanifold(alphaInterpolated * Mathf.Rad2Deg, thetaInterpolatedB * Mathf.Rad2Deg, phiInterpolatedB * Mathf.Rad2Deg, targetAB._target1, targetAB._target2);
float distanceBT = (intersectionTb.ToWorldPosition() - targets[1]).magnitude;
float lambdaA = Mathf.Sin(Vector3.Angle(AB, interpolatedVa));
float lambdaB = Mathf.Sin(Vector3.Angle(AB, interpolatedVb));
return(0.5f * (targets[0] + targets[1] + interpolatedVa * ((interpolatedDistanceA + distanceAT * lambdaA) / (1 + lambdaA)) + interpolatedVb * ((interpolatedDistanceB + distanceBT * lambdaB) / (1 + lambdaB))));
}
