From fa0bd95f3cc9a21ab4cc5bc2c8fafbadd1f246fb Mon Sep 17 00:00:00 2001 From: originalnicodr Date: Fri, 3 Mar 2023 13:03:52 -0300 Subject: [PATCH] Updated orientation interpolation in cam paths to use Splines. --- src/ExplorerBehaviour.cs | 226 ++++++++++----------------------------- 1 file changed, 57 insertions(+), 169 deletions(-) diff --git a/src/ExplorerBehaviour.cs b/src/ExplorerBehaviour.cs index e9de170..2d94683 100644 --- a/src/ExplorerBehaviour.cs +++ b/src/ExplorerBehaviour.cs @@ -301,20 +301,22 @@ namespace UnityExplorer { InitializeProperties(); - Vector3 p0, p1; //Start point, end point - Quaternion r0, r1; //Start rotation, end rotation - Vector3 m0, m1; //Tangents + Vector3 p0, p1; //Start position, end position + Vector3 m0, m1; //Position Tangents + Vector4 r0, r1; //Start rotation, end rotation + Vector4 qm0, qm1; //Rotation Tangents float fov0, fov1; + for(int i = 0; i < controlPoints.Length; i++){ + if(i>0 && Dot(controlPoints[i - 1].rotation, controlPoints[i].rotation) < 0){ + Quaternion q = controlPoints[i].rotation; + controlPoints[i].rotation = new Quaternion(- q.x, - q.y, - q.z, - q.w); + ExplorerCore.Log($"cambio orientacion n° {i}"); + } + } + // First for loop goes through each individual control point and connects it to the next, so 0-1, 1-2, 2-3 and so on int closedAdjustment = closedLoop ? 0 : 1; - - /* - Quaternion[] rotationArray = new Quaternion[controlPoints.Length]; - for(int i = 0; i < controlPoints.Length - closedAdjustment; i++) - rotationArray[i] = controlPoints[i].rotation; - */ - int totalframes = 0;//count the total number of frames so we can save the points in the right splinePoints position for (int currentPoint = 0; currentPoint < controlPoints.Length - closedAdjustment; currentPoint++) @@ -322,53 +324,64 @@ namespace UnityExplorer bool closedLoopFinalPoint = (closedLoop && currentPoint == controlPoints.Length - 1); p0 = controlPoints[currentPoint].position; - r0 = controlPoints[currentPoint].rotation; + r0 = QuaternionToVector4(controlPoints[currentPoint].rotation); fov0 = controlPoints[currentPoint].fov; if(closedLoopFinalPoint) { p1 = controlPoints[0].position; - r1 = controlPoints[0].rotation; + r1 = QuaternionToVector4(controlPoints[0].rotation); + + //Check if we are using the shortest path on the rotation. If not, change r1 to represent that shortest path. + if (Vector4.Dot(r0, r1) < 0) + r1 = - r1; + fov1 = controlPoints[0].fov; } else { p1 = controlPoints[currentPoint + 1].position; - r1 = controlPoints[currentPoint + 1].rotation; + r1 = QuaternionToVector4(controlPoints[currentPoint + 1].rotation); fov1 = controlPoints[currentPoint + 1].fov; } - // m0 + // m0, qm0 if (currentPoint == 0) // Tangent M[k] = (P[k+1] - P[k-1]) / 2 { if(closedLoop) { m0 = p1 - controlPoints[controlPoints.Length - 1].position; + qm0 = r1 - QuaternionToVector4(controlPoints[controlPoints.Length - 1].rotation); } else { m0 = p1 - p0; + qm0 = r1 - r0; } } else { m0 = p1 - controlPoints[currentPoint - 1].position; + qm0 = r1 - QuaternionToVector4(controlPoints[currentPoint - 1].rotation); } - // m1 + // m1, qm1 if (closedLoop) { if (currentPoint == controlPoints.Length - 1) //Last point case { m1 = controlPoints[(currentPoint + 2) % controlPoints.Length].position - p0; + qm1 = QuaternionToVector4(controlPoints[(currentPoint + 2) % controlPoints.Length].rotation) - r0; } else if (currentPoint == 0) //First point case { m1 = controlPoints[currentPoint + 2].position - p0; + qm1 = QuaternionToVector4(controlPoints[currentPoint + 2].rotation) - r0; } else { m1 = controlPoints[(currentPoint + 2) % controlPoints.Length].position - p0; + qm1 = QuaternionToVector4(controlPoints[(currentPoint + 2) % controlPoints.Length].rotation) - r0; } } else @@ -376,15 +389,19 @@ namespace UnityExplorer if (currentPoint < controlPoints.Length - 2) { m1 = controlPoints[(currentPoint + 2) % controlPoints.Length].position - p0; + qm1 = QuaternionToVector4(controlPoints[(currentPoint + 2) % controlPoints.Length].rotation) - r0; } else { m1 = p1 - p0; + qm1 = r1 - r0; } } m0 *= 0.5f; //Doing this here instead of in every single above statement m1 *= 0.5f; + qm0 *= 0.5f; + qm1 *= 0.5f; int frames = controlPoints[currentPoint].frames; //resolution and time the node takes to get to the other node float pointStep = 1.0f / frames; @@ -399,7 +416,7 @@ namespace UnityExplorer { float t = tesselatedPoint * pointStep; - CatmullRomPoint point = Evaluate(p0, p1, r0, r1, m0, m1, fov0, fov1, t); + CatmullRomPoint point = Evaluate(p0, p1, m0, m1, r0, r1, qm0, qm1, fov0, fov1, t); splinePoints[totalframes + tesselatedPoint] = point; } @@ -407,11 +424,21 @@ namespace UnityExplorer } } + //Old games dont have a definition for a dot product between quaternions. + static float Dot(Quaternion a, Quaternion b) + { + return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; + } + + static Vector4 QuaternionToVector4(Quaternion q){ + return new Vector4(q.x, q.y, q.z, q.w); + } + //Evaluates curve at t[0, 1]. Returns point/rotation/normal/tan struct. [0, 1] means clamped between 0 and 1. - public static CatmullRomPoint Evaluate(Vector3 posStart, Vector3 posEnd, Quaternion r0, Quaternion r1, Vector3 tanPoint1, Vector3 tanPoint2, float fovStart, float fovEnd, float t) + public static CatmullRomPoint Evaluate(Vector3 posStart, Vector3 posEnd, Vector3 tanPoint1, Vector3 tanPoint2, Vector4 rotStart, Vector4 rotEnd, Vector4 tanRot1, Vector4 tanRot2, float fovStart, float fovEnd, float t) { Vector3 position = CalculatePosition(posStart, posEnd, tanPoint1, tanPoint2, t); - Quaternion rotation = Quaternion.Slerp(r0, r1, t); + Quaternion rotation = CalculateRotation(rotStart, rotEnd, tanRot1, tanRot2, t); Vector3 tangent = CalculateTangent(posStart, posEnd, tanPoint1, tanPoint2, t); Vector3 normal = NormalFromTangent(tangent); float fov = Mathf.SmoothStep(fovStart, fovEnd, t); @@ -432,6 +459,18 @@ namespace UnityExplorer return position; } + public static Quaternion CalculateRotation(Vector4 start, Vector4 end, Vector4 tanPoint1, Vector4 tanPoint2, float t) + { + // Hermite curve formula: + // (2t^3 - 3t^2 + 1) * p0 + (t^3 - 2t^2 + t) * m0 + (-2t^3 + 3t^2) * p1 + (t^3 - t^2) * m1 + Vector4 rotation = (2.0f * t * t * t - 3.0f * t * t + 1.0f) * start + + (t * t * t - 2.0f * t * t + t) * tanPoint1 + + (-2.0f * t * t * t + 3.0f * t * t) * end + + (t * t * t - t * t) * tanPoint2; + + return new Quaternion(rotation.x, rotation.y, rotation.z, rotation.w); + } + //Calculates tangent at t[0, 1] public static Vector3 CalculateTangent(Vector3 start, Vector3 end, Vector3 tanPoint1, Vector3 tanPoint2, float t) { @@ -485,155 +524,4 @@ namespace UnityExplorer playingPath = true; } } - - //SQUAD - //Ported by CodeKiwi - //https://forum.unity.com/threads/need-help-implementing-quaternion-squad.273931/#post-4447033 - public class Squad { - // Returns a smoothed quaternion along the set of quaternions making up the spline, each quaternion is along an equidistant value in t - public static Quaternion Spline(Quaternion[] quaternions , float t ) - { - var section = (int)((quaternions.Length - 1) * t); - - var alongLine = (quaternions.Length - 1) * t - section; - - if (section == 0) - { - return SplineSegment(quaternions[section], quaternions[section], quaternions[section + 1], quaternions[section + 2], alongLine); - } - else if( section == quaternions.Length - 2 && section > 0) - { - return SplineSegment(quaternions[section - 1], quaternions[section], quaternions[section + 1], quaternions[section + 1], alongLine); - } - else if ( section >= 1 && section 0.9999f) - { - return fro; - } - - float theta = Mathf.Acos(dot); - var sinT = 1.0f / Mathf.Sin(theta); - - var newFactor = Mathf.Sin(factor * theta) * sinT; - - var invFactor = Mathf.Sin((1.0f - factor) * theta) * sinT; - - return new Quaternion(invFactor * fro.x + newFactor * to.x, invFactor * fro.y + newFactor * to.y, invFactor * fro.z + newFactor * to.z, invFactor * fro.w + newFactor * to.w); - - } - - - - // Returns a smooth approximation between q1 and q2 using t1 and t2 as 'tangents' - static Quaternion SQUAD(Quaternion q1 , Quaternion t1 , Quaternion t2 , Quaternion q2 , float t) - { - float slerpT = 2.0f * t * (1.0f - t); - - Quaternion slerp1 = SlerpNoInvert(q1, q2, t); - - Quaternion slerp2 = SlerpNoInvert(t1, t2, t); - - return SlerpNoInvert(slerp1, slerp2, slerpT); - } - - - - // Returns a quaternion between q1 and q2 as part of a smooth SQUAD segment - public static Quaternion SplineSegment(Quaternion q0, Quaternion q1, Quaternion q2, Quaternion q3, float t) - { - Quaternion qa = Intermediate(q0, q1, q2); - Quaternion qb = Intermediate(q1, q2, q3); - return SQUAD(q1, qa, qb, q2, t); - - } - - static public void Exp(ref Quaternion a ) - { - float angle = Mathf.Sqrt(a.x * a.x + a.y * a.y + a.z * a.z); - - float sinAngle = Mathf.Sin(angle); - a.w = Mathf.Cos(angle); - - if (Mathf.Abs(sinAngle) >= 1.0e-15) - { - float coeff = sinAngle / angle; - a.x *= coeff; - a.y *= coeff; - a.z *= coeff; - } - - } - static Quaternion Add(Quaternion a , Quaternion b ) - { - var r = new Quaternion(); - r.w = a.w + b.w; - r.x = a.x + b.x; - r.y = a.y + b.y; - r.z = a.z + b.z; - return r; - } - - static void Scale(ref Quaternion a, float s) - { - a.w *= s; - a.x *= s; - a.y *= s; - a.z *= s; - } - - // Tries to compute sensible tangent values for the quaternion - static Quaternion Intermediate(Quaternion q0, Quaternion q1, Quaternion q2 ) - { - Quaternion q1inv = Quaternion.Inverse(q1); - - Quaternion c1 = q1inv * q2; - - Quaternion c2 = q1inv * q0; - - //c1.Log(); - //c2.Log(); - - Quaternion c3 = Add(c2, c1);// c2 + c1; - Scale(ref c3, -0.25f);// c3.Scale(-0.25f); - - Exp(ref c3);// c3.Exp(); - - Quaternion r = q1 * c3; -#if MONO - return Normalize(r); -#else - r.Normalize(); - return r; -#endif - } -#if MONO - public static float Dot(Quaternion a, Quaternion b) - { - return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; - } - - public static Quaternion Normalize(Quaternion q) - { - float mag = Mathf.Sqrt(Dot(q, q)); - - if (mag < Mathf.Epsilon) - return Quaternion.identity; - - return new Quaternion(q.x / mag, q.y / mag, q.z / mag, q.w / mag); - } -#endif - - } }