Properly moving (teleporting) a physics entity with its children

Background
If a GameObject (parent) with a GameObject child is converted to ECS, then moving (changing Transform component of) or rotating (changing Rotation component of) the parent entity results in the child moving/rotating with it, as you'd expect.

Now imaging having those GameObjects mentioned above as non-DOTS Unity physics (PhysX) rigidbodies. If you move or rotate the parent transform -- or move or rotate the parent rigidbody using Rigidbody.MovePosition() or Rigidbody.MoveRotation() -- the child rigid bodies move as you'd expect.

Now have all those GameObjects convert into DOTS physics bodies. Moving or rotating the parent entity does not move or rotate the children as one would expect. Each body moves and acts independently. Moving the parent body does not result in the child body moving with it. This is DOTS physics engine (Unity Physics or Havok) agnostic.

Question
How does one move the root of a hierarchy of DOTS physics bodies, and have the child bodies move with it?

Setting the position of the parent entity using the following SetComponentData call results in the parent moving, but the child bodies continuing to stay where they were.

Code (CSharp):

1. entityManager.SetComponentData(parentEntity, new Translation { Value = new float3 (10, 20, 10) });

A solution could be to get the relative distance (vector) between the two bodies, and set the transform of the child body to the new location but offset by that vector. Issue is that if there also is a target rotation one would have to essentially rewrite the code of the transform systems under TransformSystemGroup to get the child body to rotate around the parent body properly (if the body is at an odd offset). There must be a better (correct) way to do this.

 

From a physics perspective marking an body as dynamic is marking it as independent, i.e. to move freely based on its own velocity. So they are unparented in conversion, becoming independent of the hierarchy they used to belong to.
If a child shouldn't become independent then their colliders should really make up part of the parent bodies compound collider. Alternatively, if the 'child' could be constrained to the 'parent' body with a joint, which could be removed later. A single body with a Compound Collider will naturally act very differently to two constrained bodies. A third option is to keyframe the 'child' body. This extra code (which we'll add to the ComponentExtensions in the next release) could help there:

Spoiler

Code (CSharp):

1.  
2.         // Returns the Euler angles (in radians) between two quaternions
3.         public static float3 EstimateAnglesBetween(quaternion from, quaternion to)
4.         {
5.             float3 fromImag = new float3(from.value.x, from.value.y, from.value.z);
6.             float3 toImag = new float3(to.value.x, to.value.y, to.value.z);
7.  
8.             float3 angle = math.cross(fromImag, toImag);
9.             angle -= to.value.w * fromImag;
10.             angle += from.value.w * toImag;
11.             angle += angle;
12.             return math.dot(toImag, fromImag) < 0 ? -angle : angle;
13.         }
14.  
15.  
16.         public static void ApplyKeyframe(ref this PhysicsVelocity pv, PhysicsMass pm,
17.             ref Translation t, ref Rotation r,
18.             float3 desiredPosition, quaternion desiredRotation,
19.             float deltaTime)
20.         {
21.  
22.             float3 currentPosition = t.Value;
23.             quaternion currentRotation = r.Value;
24.  
25.             float invDeltaTime = math.rcp(deltaTime);
26.  
27.             if(!desiredPosition.Equals(currentPosition))
28.             {
29.                 //float3 currentLinearVelocity = pv.Linear;
30.                 float3 desiredLinearVelocity = (desiredPosition - currentPosition) * invDeltaTime;
31.  
32.                 pv.Linear = desiredLinearVelocity;
33.             }
34.  
35.             if (!desiredRotation.Equals(currentRotation))
36.             {
37.                 //float3 currentAngularVelocity = pv.GetAngularVelocity(pm, r);
38.                 float3 desiredAngularVelocity = Math.EstimateAnglesBetween(currentRotation, desiredRotation) * invDeltaTime;
39.  
40.                 pv.SetAngularVelocity(pm, r, desiredAngularVelocity);
41.             }
42.         }

Usually I ask what is the actual use case is that you are trying to achieve with parenting the bodies together?

 

I am trying to move the parent body to a location, and have all child bodies preserve their local location when the parent body is moved. I was thinking the easiest way to do this was parenting the child bodies so that TransformSystems do the work for me, but it shows to not be the case, especially due to how all bodies act independently. Thank you for that explanation.

My context is that I have a body (the "parent" body) with multiple bodies attached to it using a combination of a Prismatic Joint and my own custom linear spring + dashpot, essentially creating a "shock absorber."

I want to move all the bodies to a point instantaneously like a unit, without these joints being "broken" or resulting in undefined behavior. Regardless if I have the "parent" body as a transform parent, if I move the parent body to the location I want, the "child" bodies won't move, and due to the prismatic joint and my linear spring, it creates massive amounts of forces on the parent and child bodies, result in extreme behavior and all the bodies freaking out / exploding.

I had the idea of moving all the bodies including the child bodies to the same location and let the joints deal with reposting them to their relative location, but this also results in unwarranted behavior. Some child bodies move to the joints higher than others which makes the linear spring system add forces, therefore resulting in the parent body rotating. I'd expect this issue to be removed if all bodies are moved to the target position if the "child" bodies retain their relative locations to the "parent."

I'm not exactly sure how this is different than just setting the position of the bodies to the new location after manually calculating the local locations in world space. Wouldn't this method also result in bodies colliding with walls if there are any in the path of the teleport?

 

Ah, so you want to teleport a constrained system. Locally I tweaked the Ragdoll demo to take a positionOffset and rotationOffset and each ragdoll part. This way I didn't need to care about the details of the hierarchy, just used the pelvis position as a center and offset from it.

This code is obviously not optimal doing everything via the entity manager.

Code (CSharp):

1.         // reposition and reorient with offset information
2.         if (entities.Length > 0)
3.         {
4.             for (int i = 0; i < entities.Length; i++)
5.             {
6.                 var e = entities[i];
7.                 Translation positionComponent = entityManager.GetComponentData<Translation>(e);
8.                 Rotation rotationComponent = entityManager.GetComponentData<Rotation>(e);
9.                 PhysicsVelocity velocityComponent = entityManager.GetComponentData<PhysicsVelocity>(e);
10.  
11.                 float3 position = positionComponent.Value;
12.                 quaternion rotation = rotationComponent.Value;
13.  
14.                 float3 localPosition = position - pelvisPosition;
15.                 localPosition = math.rotate(rotationOffset, localPosition);
16.  
17.                 position = localPosition + pelvisPosition + positionOffset;
18.                 rotation = math.mul(rotation, rotationOffset);
19.  
20.                 positionComponent.Value = position;
21.                 rotationComponent.Value = rotation;
22.  
23.                 velocityComponent.Linear = initialVelocity;
24.  
25.                 entityManager.SetComponentData<PhysicsVelocity>(e, velocityComponent);
26.                 entityManager.SetComponentData<Translation>(e, positionComponent);
27.                 entityManager.SetComponentData<Rotation>(e, rotationComponent);
28.             }
29.         }

 

I was pretty confused how to use the implementation @steveeHavok provided, so I sat down and tried to wrap my head around a solution. I believe this solution is a bit more streamlined, and the temporary and input variables are a bit more "self-describing."

The solution I created uses a DynamicBuffer<LinkedEntityGroup> for iterating over all linked entities (which is common for instantiated prefabs), but it'd work identically when using an entities array like Stevee's rag doll implementation.

My implementation requires four other variables: The current position and rotation of the root entity as float3's (rootPosition, rootRotation), and the target position and rotation as quaternions (targetPosition, targetRotation).

For example if your spawning the hierarchy of bodies from a prefab, the root entity is (usually) the entity with the DynamicBuffer<LinkedEntityGroup>. For example, here's getting that information in that context:

Code (CSharp):

1. BlobAssetStore blobAssetStore = World.DefaultGameObjectInjectionWorld.GetExistingSystem<ConvertToEntitySystem>().BlobAssetStore;
2. var settings = GameObjectConversionSettings.FromWorld(World.DefaultGameObjectInjectionWorld, blobAssetStore);
3. settings.ConversionFlags = GameObjectConversionUtility.ConversionFlags.AssignName;
4.  
5. var prefab = GameObjectConversionUtility.ConvertGameObjectHierarchy(gameObjectPrefab, settings);
6. var instance = entityManager.Instantiate(prefab);
7.  
8. float3 rootPosition = entityManager.GetComponentData<Translation>(carInstance).Value;
9. quaternion rootRotation = entityManager.GetComponentData<Rotation>(carInstance).Value;

When the object teleports, the "new root position" and target position will be equal.

Note: I do nothing to the PhysicsVelocity struct, so linear and angular velocity is preserved (of course only if this code is called at the right time in regards to the physics systems).

Here is the solution:

Code (CSharp):

1. for (int i = 0; i < entityGroup.Length; i++)
2. {
3.   Entity e = entityGroup[i].Value;
4.  
5.   // Physics bodies are the only entities that need to have the teleportation calculation applied to them
6.   if (entityManager.HasComponent<PhysicsVelocity>(e))
7.   {
8.  
9.     Translation positionComponent = entityManager.GetComponentData<Translation>(e);
10.     Rotation rotationComponent = entityManager.GetComponentData<Rotation>(e);
11.  
12.     // offset to destination = destination - source
13.     float3 localPosition = positionComponent.Value - rootPosition;
14.     // Global rotation quaternion * local vector = global vector
15.     //    Though, the local vector is in the local space of the objects, so the inital rotation must be taken into account.
16.     //    So therefore in reality, the equation is:
17.     // Relative rotation quaternion from Inital rotation to Final rotation * local vector = final vector
18.     //    This relative rotation quaternion can be found not by dest - source like with vectors, but by
19.     // Quaternion relative = Quaternion.Inverse(a) * b;
20.     //    where the relative vector represents the relative rotation from A to B.
21.     //    Source:  https://answers.unity.com/questions/35541/problem-finding-relative-rotation-from-one-quatern.html
22.     // Note that the relative rotation is from the root to the target, not from the individual body to the target.
23.     // Doing the relative rotation from individual to target results in different relative rotations per body.
24.     quaternion rootRelativeRotation = math.mul(math.inverse(rootRotation), targetRotation);
25.     float3 rotatedLocalPosition = math.mul(rootRelativeRotation, localPosition);
26.  
27.     // Move the entire local position to the target position.
28.     // This can be done by adding the offset from the root position to the target position to the local position.
29.     // As usual, offset vector can be found by:
30.     //    float3 offset = destination - source
31.     float3 rootOffset = targetPosition - rootPosition;
32.     float3 finalPosition = rootPosition + rotatedLocalPosition + rootOffset;
33.  
34.     // The root relative rotation to the target can be now applied to the body's rotation.
35.     // This can be thought of like adding two quaternions like one would "add a vector and an offset vector."
36.     // Though, actually adding quaternions like vectors is not what we want, and does not produce the expected result (nor is it possible).
37.     // Source:  http://answers.unity.com/answers/659240/view.html
38.     // Just like with the operation earlier where Global rotation quaternion * local vector = global vector,
39.     //    a rotation quaternion can be rotated by a rotation offset by multiplying them together.
40.     quaternion finalRotation = math.mul(rotationComponent.Value, rootRelativeRotation);
41.  
42.     positionComponent.Value = finalPosition;
43.     rotationComponent.Value = finalRotation;
44.  
45.     entityManager.SetComponentData<Translation>(e, positionComponent);
46.     entityManager.SetComponentData<Rotation>(e, rotationComponent);
47.   }
48. }

(CODE EDIT 6/7/20: Root position was not being added to final position, creating wild, ping-ponging teleportation)

 

What changes would be required to make the code optimal? Would the solution have to be moved into a ComponentSystem, and potentially jobified?

 

I'm not sure if I got what are you trying to achieve, but what stops you from making a kinematic body, connect all those joints on it, an then make that kinematic body follow a parent?

 

My goal is to move an already existing set of bodies to a new location. All "children" bodies must move in relation to the "root" or "parent" body. With Unity Physics and Havok Physics, this movement (teleportation) is done simply by setting all the bodies position and rotation components in a specific way at a specific time (in relation to systems). It cannot be done by setting velocity and/or angular velocity components, as the bodies must pass through wall during the teleporting process. If teleporting was done using velocity, bodies would interact with collision during the teleportation.

I'm not sure how this translates to the Unity Physics paradigm. What do you mean by connecting all the joints? Are you saying to disable joints during the teleportation process?

Setting bodies to be kinematic in Unity Physics is certainly not as straight forward as PhysX. Unity Physics requires setting Inverse Mass, Inverse Inertia, and PhysicsGravityFactor to 0. Even if setting bodies to kinematic can result in child bodies moving in relation to the parent, I can see the solution becoming really bloated. You'd have to store the starting Inverse Mass, Inverse Inertia, and PhysicsGravityFactor values on the frame of the teleport, and reset them at the end of the teleport.

 

Heh...I missed the fact that actual joints should still be part of physics simulation. I meant not to use physics for that "surrogate parent" at all, rather setting its LocalToWorld directly and let the child bodies follow it, but I suppose this wont work with physics joints.