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 StartComputing()
{
ToricComputing tc = new ToricComputing(target1, target2);
Vector2 projectedSizeA = tc.DistanceFromProjectedSize(sizeToReachA, 0.5f, target1).ToVector();
Vector2 projectedSizeB = tc.DistanceFromProjectedSize(sizeToReachB, 0.5f, target1).ToVector();
//DEBUG
Debug.Log(projectedSizeA + ";" + projectedSizeB);
Dictionary <float, Interval> alphas = tc._alphaComputer.getIntervalOfAcceptedAlpha(projectedSizeA, projectedSizeB);
foreach (KeyValuePair <float, Interval> a in alphas)
{
Debug.Log(a.ToString());
}
//DEBUG
float[] keys = new float[alphas.Keys.Count];
alphas.Keys.CopyTo(keys, 0);
theta = keys[UnityEngine.Random.Range(0, alphas.Keys.Count - 2)];
Interval alphaRange;
alphas.TryGetValue(theta, out alphaRange);
alpha = alphaRange.getRandom();
Debug.Log(theta);
Debug.Log(alphaRange);
Debug.Log(alpha);
StartDebug();
}
private void testGetAlphaFromDistance()
{
ToricComputing tc = new ToricComputing(target1, target2);
Dictionary <float, Interval> alphaInv = tc._alphaComputer.getIntervalOfAcceptedAlpha(distanceToA, distanceToB);
Interval alphaRange;
float[] keys = new float[alphaInv.Keys.Count];
alphaInv.Keys.CopyTo(keys, 0);
if (alphaInv.Keys.Count - 2 <= 0)
{
throw new Exception("No intersection");
}
theta = keys[UnityEngine.Random.Range(0, alphaInv.Keys.Count - 2)];
alphaInv.TryGetValue(theta, out alphaRange);
alpha = alphaRange.getRandom() * Mathf.Rad2Deg;
theta *= Mathf.Rad2Deg;
Debug.Log(alphaRange);
Toricmanifold test = new Toricmanifold(alpha, theta, phi, target1, target2);
Vector3 posTest = test.ToWorldPosition();
Quaternion rotTest = test.ComputeOrientation(tilt);
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
//DEBUG
Debug.Log("distance: " + (posTest - target1.transform.position).magnitude);
Debug.Log("distance: " + (posTest - target2.transform.position).magnitude);
cube.transform.position = posTest;
cube.transform.rotation = rotTest;
}
/**
* Returns an interval of alpha values based on frames
* inside which the targets can be projected.
*
* @param desPosA the desired onscreen position of target A around the frame is going to be constructed
* @param desPosB the desired onscreen position of target B around the frame is going to be constructed
* @return a interval of accepted alpha values in degrees
*
*/
public Interval getAlphaIntervalFromOnscreenPositions(Vector2 desPosA, Vector2 desPosB)
{
List <Vector2> verticesFrameTargetA = getFrameTarget(desPosA);
List <Vector2> verticesFrameTargetB = getFrameTarget(desPosB);
List <Vector3> VerticesInCamSpaceA = new List <Vector3>();
List <Vector3> VerticesInCamSpaceB = new List <Vector3>();
foreach (Vector2 vec in verticesFrameTargetA)
{
Vector3 inCamSpace = ToricComputing.GetVectorInCameraSpace(vec);
VerticesInCamSpaceA.Add(inCamSpace);
}
foreach (Vector2 vec in verticesFrameTargetB)
{
Vector3 inCamSpace = ToricComputing.GetVectorInCameraSpace(vec);
VerticesInCamSpaceB.Add(inCamSpace);
}
List <float> alphaValues = new List <float>();
foreach (Vector3 pA in VerticesInCamSpaceA)
{
foreach (Vector3 pB in VerticesInCamSpaceB)
{
float alpha = Vector3.Angle(pA, pB) * Mathf.Deg2Rad;
alphaValues.Add(alpha);
}
}
return(new Interval(Mathf.Min(alphaValues.ToArray()), Mathf.Max(alphaValues.ToArray())));
}
private void testIntervalFromOnscreenPos()
{
ToricComputing tc = new ToricComputing(target1, target2);
Interval alphaTest = tc._alphaComputer.getAlphaIntervalFromOnscreenPositions(screenPos1, screenPos2);
alpha = alphaTest.getRandom();
StartCamera();
}
private void testAllConstraints()
{
ToricComputing tc = new ToricComputing(target1, target2);
Toricmanifold tm = tc.FinalConstraintCombination(samplingRate, vantageDirectionA, deviationAngleA, vantageDirectionB, deviationAngleB, distanceToA, distanceToB, screenPos1, screenPos2, visibilityInterval);
//DEBUG
Debug.Log("possiblePosition: " + tm.ToString());
alpha = tm.getAlpha();
theta = tm.getTheta();
phi = tm.getPhi();
StartCamera();
}
//private methods
private Quaternion computeLookAtTransition()
{
Vector3 forward, up;
{
Vector3 pA3 = Vector3.Normalize(ToricComputing.GetVectorInCameraSpace(screenPositionA));
Vector3 pB3 = Vector3.Normalize(ToricComputing.GetVectorInCameraSpace(screenPositionB));
up = Vector3.Cross(pB3, pA3).normalized;
forward = (pA3 + pB3).normalized;
}
return(Quaternion.LookRotation(forward, up));
}
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 void testVantageAngleConstraintB()
{
ToricComputing tc = new ToricComputing(target1, target2);
Dictionary <float, Interval> phisA = tc._vantageCons.getPositionFromVantageOneTarget(target2.transform.position, vantageDirectionB, deviationAngleB);
//DEBUG
float[] keys = new float[phisA.Keys.Count];
phisA.Keys.CopyTo(keys, 0);
phi = keys[UnityEngine.Random.Range(0, keys.Length - 2)];
Interval betaRange;
phisA.TryGetValue(phi, out betaRange);
phi = phi * Mathf.Rad2Deg;
Debug.Log(betaRange);
theta = 2 * (Mathf.PI - alpha * Mathf.Deg2Rad - betaRange.getRandom()) * Mathf.Rad2Deg;
Debug.Log(theta);
Debug.Log(phi);
StartDebug();
}
private void testVantageAngleConstraint()
{
ToricComputing tc = new ToricComputing(target1, target2);
Dictionary <float, Interval> phisA = tc.getThetaIntervallFromVantageBothTargets(vantageDirectionA, deviationAngleA, vantageDirectionB, deviationAngleB, 1 / samplingRate);
//DEBUG
float[] keys = new float[phisA.Keys.Count];
phisA.Keys.CopyTo(keys, 0);
phi = keys[UnityEngine.Random.Range(0, keys.Length - 2)];
Interval betaRange;
phisA.TryGetValue(phi, out betaRange);
phi = phi * Mathf.Rad2Deg;
Debug.Log(betaRange);
theta = betaRange.getRandom() * Mathf.Rad2Deg;
Debug.Log(theta);
Debug.Log(phi);
StartDebug();
}
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))));
}
public String ToString()
{
return("Alpha: " + _alpha.angle() + "Theta: " + _theta.angle() + "Phi: " + _phi.angle() + "Visibility: " + ToricComputing.visibilityCheck(this));
}
