Issues with simple rotational conversions

I am having some problems with rotation conversions between local/world in a system. I am curious if anyone is able to identify what mistakes I am making.

I have a simple system that I am iterating through a few points at a very basic level. I can't seem to get the rotation to convert properly back into a local space and create the intended result even though I have a few conversion equations that seem to be working. So I must assume it is an inconsistency in my knowledge of quaternion notation and/or operation.

I've tested the sequence of calculations in non-ecs and it is working fine. At a high level it is:

Code (CSharp):

1. Vector3 pos = worldPoints[j].transform.position;
2.                     Vector3 toLastPoint = worldPoints[worldPoints.Length - 1].transform.position - pos;
3.                     Vector3 toTarget = target- pos;
4.  
5.                     Quaternion rotation= worldPoints[j].transform.rotation;
6.  
7.                     Quaternion targetRotation = Quaternion.FromToRotation(toLastPoint, toTarget) * pointRotation;
8.  
9.                     if (w >= 1) worldPoints[j].transform.rotation = targetRotation;
10.                     else worldPoints[j].transform.rotation = Quaternion.Lerp(pointRotation, targetRotation, w);

Of course it is worth noting that these positions and rotations are in world space.

When doing this with entities, the quaternion from the Rotation component and the float3 from the Translation are given in localSpace. I have tried both converting them to world (preferred solution) as well as converting everything to local. Neither seem to have the same result and again, I assume it is something wrong with the way I am handling the quaternions.

with ECS I am doing the following, with the translation and rotation taken from the input for the job they are in.

Code (CSharp):

1. float3 ePos= EntityManager.GetComponentData<Translation>(e).Value;
2. LocalToWorld e1LTW = EntityManager.GetComponentData<LocalToWorld>(e);
3. float3 ePosWorld = GetEntityWorldPosition(e1LTW, ePos);
4. float3 toLastPoint = ePos2 - ePos;
5. float3 toTarget = localTarget - ePosWorld;
6.  
7. quaternion eRot = EntityManager.GetComponentData<Rotation>(e).Value;
8.                                     quaternion eRotWorld = GetWorldRotation(e1LTW, eRot);
9.  
10. quaternion targetRotation = math.mul(FromToRotation(toLastPoint, toTarget), eRotWorld);
11. quaternion targetRotationLocal = GetLocalRotation(e1LTW, eRotations[j]);
12.                                     entityManager.SetComponentData<Rotation>(e, new Rotation { Value = targetRotationLocal });
13.  

with the conversion as:

Code (CSharp):

1. private static float3 GetEntityWorldPosition(LocalToWorld ltw, float3 t)
2.         {
3.             float4 pos = new float4(t, 1);
4.             float3 worldPos= math.mul(ltw.Value, pos).xyz;
5.             return worldPos;
6.         }
7.  
8.         private static float3 GetEntityLocalPosition(LocalToWorld ltw, float3 worldPosition)
9.         {
10.             float4 pos = new float4(worldPosition, 1);
11.             float4x4 worldToLocal = math.inverse(ltw.Value);
12.             float3 localPos = math.mul(worldToLocal, pos).xyz;
13.             return localPos;
14.         }
15.  
16.         public static quaternion GetWorldRotation(LocalToWorld ltw, quaternion q)
17.         {
18.             return math.mul(quaternion.LookRotationSafe(ltw.Forward, ltw.Up), q.value);
19.         }
20.  
21.         public static quaternion GetLocalRotation(LocalToWorld ltw, quaternion q)
22.         {
23.             var invWorld = math.inverse(quaternion.LookRotationSafe(ltw.Forward, ltw.Up));
24.             var result = math.mul(invWorld, q.value);
25.  
26.             return result;
27.         }

For the non-ecs portion the points line up as intended, and for the ecs, it keeps moving every frame and it is recalculating every frame.

Is there any issues with the quaternion conversions between world and local? Is there something wrong with my process of taking local position/rotation, converting to world, calculating and adding a new rotation, converting back to local, and assigning the new quaternion to the rotation?

Any help is greatly appreciated. Thank you.

 

Simplest way to debug this would be to draw rays from both expected and result quaternions step by step.
Same comes for points.

On the first glance, don't use mul for ltw, use math.transform / math.rotate.
That should prevent invalid .w, casts, and simplify things a bit.

If you're doing similar in ECS:

Code (CSharp):

1. if (w >= 1) worldPoints[j].transform.rotation = targetRotation;
2.                     else worldPoints[j].transform.rotation = Quaternion.Lerp(pointRotation, targetRotation, w);

Note percentages do not clamp in 0..1 ranges.

 

Thanks. While going through and checking / comparing results between two identically placed systems, the part that they become different in the calculations are at:

Quaternion.FromToRotation(toLastBone, toTarget);

I revisited the code I had for this and checked some posts for other implementations. I found a post with some help from @andrew-lukasik and @Bunny83 : https://answers.unity.com/questions/1750909/unitymathematics-equivalent-to-quaternionfromtorot.html
so thank you for that.

and it seems to work the same as the Quaternion.FromToRotation so it seems I am on the right track.

 

In ecs there doesn't seem to be a quaternion.lerp, so I am using .slerp. I will make sure to clamp as I didn't know it didn't clamp, so thanks I appreciate that comment.
Slerp is fine in my use case but is there any reason they don't have lerp for quaternions?

 

There's nlerp in math:

Code (CSharp):

1. /// <summary>Returns the result of a normalized linear interpolation between two quaternions q1 and a2 using an interpolation parameter t.</summary>
2.         [MethodImpl(MethodImplOptions.AggressiveInlining)]
3.         public static quaternion nlerp(quaternion q1, quaternion q2, float t)
4.         {
5.             float dt = dot(q1, q2);
6.             if(dt < 0.0f)
7.             {
8.                 q2.value = -q2.value;
9.             }
10.  
11.             return normalize(quaternion(lerp(q1.value, q2.value, t)));
12.         }
13.  
14.         /// <summary>Returns the result of a spherical interpolation between two quaternions q1 and a2 using an interpolation parameter t.</summary>
15.         [MethodImpl(MethodImplOptions.AggressiveInlining)]
16.         public static quaternion slerp(quaternion q1, quaternion q2, float t)
17.         {
18.             float dt = dot(q1, q2);
19.             if (dt < 0.0f)
20.             {
21.                 dt = -dt;
22.                 q2.value = -q2.value;
23.             }
24.  
25.             if (dt < 0.9995f)
26.             {
27.                 float angle = acos(dt);
28.                 float s = rsqrt(1.0f - dt * dt);    // 1.0f / sin(angle)
29.                 float w1 = sin(angle * (1.0f - t)) * s;
30.                 float w2 = sin(angle * t) * s;
31.                 return quaternion(q1.value * w1 + q2.value * w2);
32.             }
33.             else
34.             {
35.                 // if the angle is small, use linear interpolation
36.                 return nlerp(q1, q2, t);
37.             }
38.         }

Probably because quaternions values when interpolated directly could produce invalid values, and as a result - has to be normalized. Plus its vectorized this way.

 

Awesome, thank you so much! Wasn't aware of nLerp.

 

Hi. Looking at your ecs variable naming choices I sense some possible confusion there, please let me clear some things out:

Code (CSharp):

1. float3 localPosition = EntityManager.GetComponentData<Translation>(entity).Value;
2. quaternion localRotation = EntityManager.GetComponentData<Rotation>(entity).Value;
3. LocalToWorld transform = EntityManager.GetComponentData<LocalToWorld>(entity);
4. float3 worldPosition = transform.Position;
5. quaternion worldRotation = transform.Rotation;

LocalToWorld

is your final transformation matrix for given entity. Both

Translation

(local translation) and

Rotation

(local rotation) are inputs for

TransformSystemGroup

's systems to calculate

LocalToWorld

.

 

My first iteration when translating this code into dots would probably be something like this (not tested at all, just an initial sketch):

Code (CSharp):

1. struct JobData : IComponentData
2. {
3.     public float3 lastPoint;
4.     public float3 target;
5.     public float w;
6.     public quaternion pointRotation;
7. }
8.  
9. Entities
10.     .WithName("some_kind_of_rotation_job")
11.     .WithChangeFilter<JobData,Rotation>()
12.     .ForEach( ( ref Rotation localRotation , in JobData data , in LocalToWorld transform ) =>
13.     {
14.         float3 worldPosition = transform.Position;
15.         float3 toLastPoint = data.lastPoint - worldPosition;
16.         float3 toTarget = data.target - worldPosition;
17.         quaternion targetRotation = math.mul( FromToRotation(toLastPoint,toTarget) , data.pointRotation );
18.         quaternion newWorldRotation = math.slerp( data.pointRotation , targetRotation , math.saturate(data.w) );
19.  
20.         localRotation.Value = math.mul( newWorldRotation , math.inverse(transform.Rotation) );// idk
21.     })
22.     .WithBurst().ScheduleParallel();