Created
August 20, 2020 17:19
-
-
Save thelebaron/9e8e8cb63f2cbea0426ef94839727ecb to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
using System.Runtime.CompilerServices; | |
using Unity.Collections; | |
using Unity.Entities; | |
using Unity.Mathematics; | |
using Unity.Physics; | |
using UnityEngine; | |
using FloatRange = Unity.Physics.Math.FloatRange; | |
using LegacyCharacter = UnityEngine.CharacterJoint; | |
using LegacyConfigurable = UnityEngine.ConfigurableJoint; | |
using LegacyFixed = UnityEngine.FixedJoint; | |
using LegacyHinge = UnityEngine.HingeJoint; | |
using LegacyJoint = UnityEngine.Joint; | |
using LegacySpring = UnityEngine.SpringJoint; | |
// ReSharper disable Unity.InefficientPropertyAccess | |
namespace Modules.PhysicsConversion | |
{ | |
/// <summary> This class is based on LegacyJointConversionSystem(UnityPhysicsPackageFolder\Unity.Physics.Hybrid\Conversion\LegacyJointConversionSystem) | |
/// and just reconfigured for manual conversion. Basically to get around Inject's limiations. </summary> | |
public static class LegacyJointUtility | |
{ | |
public static World DefaultWorld => World.DefaultGameObjectInjectionWorld; | |
static Entity CreateJointEntity(GameObjectConversionSystem system, GameObject primaryGameObject, GameObject gameObject, PhysicsJoint joint, Entity entityA, Entity entityB, bool enableCollision = false) | |
{ | |
var entityManager = system.DstEntityManager; | |
Entity jointEntity = system.CreateAdditionalEntity(primaryGameObject); | |
entityManager.AddComponentData(jointEntity, new PhysicsConstrainedBodyPair(entityA, entityB, enableCollision)); | |
entityManager.AddComponentData(jointEntity, joint); | |
#if UNITY_EDITOR | |
var nameEntityA = system.DstEntityManager.GetName(entityA); | |
var nameEntityB = entityB == Entity.Null ? "PhysicsWorld" : system.DstEntityManager.GetName(entityB); | |
system.DstEntityManager.SetName(jointEntity, $"Joint {nameEntityA} + {nameEntityB}"); | |
#endif | |
return jointEntity; | |
} | |
private static void AddOrSetComponent<T>(GameObjectConversionSystem system, Entity entity, T value) | |
where T : struct, IComponentData | |
{ | |
if (!system.DstEntityManager.HasComponent<T>(entity)) | |
system.DstEntityManager.AddComponentData(entity, value); | |
else if (!TypeManager.IsZeroSized(TypeManager.GetTypeIndex<T>())) | |
system.DstEntityManager.SetComponentData(entity, value); | |
} | |
static PhysicsJoint CreateConfigurableJoint( | |
GameObjectConversionSystem system, Entity connectedEntity, | |
quaternion jointFrameOrientation, | |
LegacyJoint joint, bool3 linearLocks, bool3 linearLimited, SoftJointLimit linearLimit, SoftJointLimitSpring linearSpring, bool3 angularFree, bool3 angularLocks, | |
bool3 angularLimited, SoftJointLimit lowAngularXLimit, SoftJointLimit highAngularXLimit, SoftJointLimitSpring angularXLimitSpring, SoftJointLimit angularYLimit, | |
SoftJointLimit angularZLimit, SoftJointLimitSpring angularYZLimitSpring) | |
{ | |
//var constraints = new NativeList<Constraint>(Allocator.Temp); | |
var constraints = new FixedList128<Constraint>(); | |
// TODO: investigate mapping PhysX spring and damping to Unity Physics SpringFrequency and SpringDamping | |
var springFrequency = Constraint.DefaultSpringFrequency; | |
var springDamping = Constraint.DefaultSpringDamping; | |
if (angularLimited[0]) | |
{ | |
constraints.Add(Constraint.Twist(0, math.radians(new FloatRange(-highAngularXLimit.limit, -lowAngularXLimit.limit)), springFrequency, springDamping)); | |
} | |
if (angularLimited[1]) | |
{ | |
constraints.Add(Constraint.Twist(1, math.radians(new FloatRange(-angularYLimit.limit, angularYLimit.limit)), springFrequency, springDamping)); | |
} | |
if (angularLimited[2]) | |
{ | |
constraints.Add(Constraint.Twist(2, math.radians(new FloatRange(-angularZLimit.limit, angularZLimit.limit)), springFrequency, springDamping)); | |
} | |
if (math.any(linearLimited)) | |
{ | |
var distanceRange = new FloatRange(-linearLimit.limit, linearLimit.limit).Sorted(); | |
constraints.Add(new Constraint | |
{ | |
ConstrainedAxes = linearLimited, | |
Type = ConstraintType.Linear, | |
Min = math.csum((int3)linearLimited) == 1 ? distanceRange.Min : 0f, | |
Max = distanceRange.Max, | |
SpringFrequency = springFrequency, | |
SpringDamping = springDamping | |
}); | |
} | |
if (math.any(linearLocks)) | |
{ | |
constraints.Add(new Constraint | |
{ | |
ConstrainedAxes = linearLocks, | |
Type = ConstraintType.Linear, | |
Min = 0, | |
Max = 0, | |
SpringFrequency = springFrequency, | |
SpringDamping = springDamping | |
}); | |
} | |
if (math.any(angularLocks)) | |
{ | |
constraints.Add(new Constraint | |
{ | |
ConstrainedAxes = angularLocks, | |
Type = ConstraintType.Angular, | |
Min = 0, | |
Max = 0, | |
SpringFrequency = springFrequency, | |
SpringDamping = springDamping | |
}); | |
} | |
RigidTransform worldFromBodyA = Math.DecomposeRigidBodyTransform(joint.transform.localToWorldMatrix); | |
RigidTransform worldFromBodyB = joint.connectedBody == null | |
? RigidTransform.identity | |
: Math.DecomposeRigidBodyTransform(joint.connectedBody.transform.localToWorldMatrix); | |
var legacyWorldFromJointA = math.mul( | |
new RigidTransform(joint.transform.rotation, joint.transform.position), | |
new RigidTransform(jointFrameOrientation, joint.anchor) | |
); | |
var bodyAFromJoint = new BodyFrame(math.mul(math.inverse(worldFromBodyA), legacyWorldFromJointA)); | |
//var connectedEntity = system.GetPrimaryEntity(joint.connectedBody); | |
var isConnectedBodyConverted = joint.connectedBody == null || connectedEntity != Entity.Null; | |
RigidTransform bFromA = isConnectedBodyConverted ? math.mul(math.inverse(worldFromBodyB), worldFromBodyA) : worldFromBodyA; | |
RigidTransform bFromBSource = isConnectedBodyConverted ? RigidTransform.identity : worldFromBodyB; | |
var bodyBFromJoint = new BodyFrame | |
{ | |
Axis = math.mul(bFromA.rot, bodyAFromJoint.Axis), | |
PerpendicularAxis = math.mul(bFromA.rot, bodyAFromJoint.PerpendicularAxis), | |
Position = math.mul(bFromBSource, new float4(joint.connectedAnchor, 1f)).xyz | |
}; | |
// deprecated | |
//var jointData = JointData.Create(bodyAFromJoint, bodyBFromJoint, constraints); | |
//constraints.Dispose(); | |
//return jointData; | |
var jointData = new PhysicsJoint | |
{ | |
BodyAFromJoint = bodyAFromJoint, | |
BodyBFromJoint = bodyBFromJoint | |
}; | |
jointData.SetConstraints(constraints); | |
return jointData; | |
} | |
static bool IsMotionFree(ConfigurableJointMotion motion) | |
{ | |
return motion == ConfigurableJointMotion.Free; | |
} | |
static bool IsMotionLocked(ConfigurableJointMotion motion) | |
{ | |
return motion == ConfigurableJointMotion.Locked; | |
} | |
static bool IsMotionLimited(ConfigurableJointMotion motion) | |
{ | |
return motion == ConfigurableJointMotion.Limited; | |
} | |
[MethodImpl(MethodImplOptions.AggressiveInlining)] | |
static quaternion GetJointFrameOrientation(float3 axis, float3 secondaryAxis) => | |
new BodyFrame { Axis = axis, PerpendicularAxis = secondaryAxis }.AsRigidTransform().rot; | |
[MethodImpl(MethodImplOptions.AggressiveInlining)] | |
static BodyFrame GetFrame(float3 axis, float3 secondaryAxis) | |
{ | |
return new BodyFrame | |
{ | |
Axis = axis, PerpendicularAxis = secondaryAxis | |
}; | |
} | |
public static void ConvertCharacterJoint( | |
GameObjectConversionSystem system, | |
GameObject primaryGameObject, | |
LegacyCharacter joint, | |
Entity jointEntity, | |
Entity connectedEntity) | |
{ | |
var linearLocks = new bool3(true); | |
var linearLimited = new bool3(false); | |
var angularFree = new bool3(false); | |
var angularLocks = new bool3(false); | |
var angularLimited = new bool3(true); | |
var jointFrameOrientation = GetJointFrameOrientation(joint.axis, joint.swingAxis); | |
var jointData = CreateConfigurableJoint(system, connectedEntity, jointFrameOrientation, joint, linearLocks, linearLimited, new SoftJointLimit { limit = 0f, bounciness = 0f }, new SoftJointLimitSpring { spring = 0f, damper = 0f }, angularFree, angularLocks, angularLimited, | |
joint.lowTwistLimit, joint.highTwistLimit, joint.twistLimitSpring, joint.swing1Limit, joint.swing2Limit, joint.swingLimitSpring); | |
CreateJointEntity(system, primaryGameObject, joint.gameObject, jointData, jointEntity, joint.connectedBody == null ? Entity.Null : connectedEntity, joint.enableCollision); | |
} | |
/*public static void ConvertSpringJoint(GameObjectConversionSystem system, LegacySpring joint) | |
{ | |
var constraints = new NativeList<Constraint>(Allocator.Temp); | |
constraints.Add(new Constraint { | |
ConstrainedAxes = new bool3(true), | |
Type = ConstraintType.Linear, | |
Min = joint.minDistance, | |
Max = joint.maxDistance, | |
SpringFrequency = 1f, // ? | |
SpringDamping = 0.1f // ? | |
}); | |
var jointFrameA = BodyFrame.Identity; | |
jointFrameA.Position = joint.anchor; | |
var connectedEntity = system.GetPrimaryEntity(joint.connectedBody); | |
var isConnectedBodyConverted = | |
joint.connectedBody == null || connectedEntity != Entity.Null; | |
RigidTransform bFromBSource = | |
isConnectedBodyConverted ? RigidTransform.identity : Math.DecomposeRigidBodyTransform(joint.connectedBody.transform.localToWorldMatrix); | |
var jointFrameB = BodyFrame.Identity; | |
jointFrameB.Position = math.mul(bFromBSource, new float4(joint.connectedAnchor, 1f)).xyz; | |
var jointData = JointData.Create(jointFrameA, jointFrameB, constraints); | |
//CreateJointEntity(system, joint.gameObject, jointData, system.GetPrimaryEntity(joint.gameObject), joint.connectedBody == null ? Entity.Null : connectedEntity, joint.enableCollision); | |
constraints.Dispose(); | |
} | |
public static void ConvertFixedJoint(GameObjectConversionSystem system, LegacyFixed joint) | |
{ | |
var legacyWorldFromJointA = math.mul( | |
new RigidTransform(joint.transform.rotation, joint.transform.position), | |
new RigidTransform(quaternion.identity, joint.anchor) | |
); | |
RigidTransform worldFromBodyA = Math.DecomposeRigidBodyTransform(joint.transform.localToWorldMatrix); | |
var connectedEntity = system.GetPrimaryEntity(joint.connectedBody); | |
RigidTransform worldFromBodyB = connectedEntity == Entity.Null | |
? RigidTransform.identity | |
: Math.DecomposeRigidBodyTransform(joint.connectedBody.transform.localToWorldMatrix); | |
var bodyAFromJoint = new BodyFrame(math.mul(math.inverse(worldFromBodyA), legacyWorldFromJointA)); | |
var bodyBFromJoint = new BodyFrame(math.mul(math.inverse(worldFromBodyB), legacyWorldFromJointA)); | |
var jointData = JointData.CreateFixed(bodyAFromJoint, bodyBFromJoint); | |
//CreateJointEntity(system, joint.gameObject, jointData, system.GetPrimaryEntity(joint.gameObject), joint.connectedBody == null ? Entity.Null : connectedEntity, joint.enableCollision); | |
} | |
public static void ConvertHingeJoint(GameObjectConversionSystem system, GameObject primaryGameObject, HingeJoint joint, Entity jointEntity, Entity connectedEntity) | |
{ | |
RigidTransform worldFromBodyA = Math.DecomposeRigidBodyTransform(joint.transform.localToWorldMatrix); | |
RigidTransform worldFromBodyB = joint.connectedBody == null | |
? RigidTransform.identity | |
: Math.DecomposeRigidBodyTransform(joint.connectedBody.transform.localToWorldMatrix); | |
Math.CalculatePerpendicularNormalized(joint.axis, out float3 perpendicularA, out _); | |
var bodyAFromJoint = new BodyFrame | |
{ | |
Axis = joint.axis, | |
PerpendicularAxis = perpendicularA, | |
Position = joint.anchor | |
}; | |
//var connectedEntity = system.GetPrimaryEntity(joint.connectedBody); | |
var isConnectedBodyConverted = joint.connectedBody == null || connectedEntity != Entity.Null; | |
RigidTransform bFromA = isConnectedBodyConverted ? math.mul(math.inverse(worldFromBodyB), worldFromBodyA) : worldFromBodyA; | |
RigidTransform bFromBSource = | |
isConnectedBodyConverted ? RigidTransform.identity : worldFromBodyB; | |
var bodyBFromJoint = new BodyFrame | |
{ | |
Axis = math.mul(bFromA.rot, joint.axis), | |
PerpendicularAxis = math.mul(bFromA.rot, perpendicularA), | |
Position = math.mul(bFromBSource, new float4(joint.connectedAnchor, 1f)).xyz | |
}; | |
var limits = math.radians(new FloatRange(joint.limits.min, joint.limits.max)); | |
var jointData = joint.useLimits | |
? JointData.CreateLimitedHinge(bodyAFromJoint, bodyBFromJoint, limits) | |
: JointData.CreateHinge(bodyAFromJoint, bodyBFromJoint); | |
CreateJointEntity(system, primaryGameObject, joint.gameObject, jointData, jointEntity, joint.connectedBody == null ? Entity.Null : connectedEntity, joint.enableCollision); | |
} | |
public static void ConvertConfigurableJoint(GameObjectConversionSystem system, LegacyConfigurable joint) | |
{ | |
var linearLocks = new bool3 | |
( | |
IsMotionLocked(joint.xMotion), | |
IsMotionLocked(joint.yMotion), | |
IsMotionLocked(joint.zMotion) | |
); | |
var linearLimited = new bool3 | |
( | |
IsMotionLimited(joint.xMotion), | |
IsMotionLimited(joint.yMotion), | |
IsMotionLimited(joint.zMotion) | |
); | |
var angularFree = new bool3 | |
( | |
IsMotionFree(joint.angularXMotion), | |
IsMotionFree(joint.angularYMotion), | |
IsMotionFree(joint.angularZMotion) | |
); | |
var angularLocks = new bool3 | |
( | |
IsMotionLocked(joint.angularXMotion), | |
IsMotionLocked(joint.angularYMotion), | |
IsMotionLocked(joint.angularZMotion) | |
); | |
var angularLimited = new bool3 | |
( | |
IsMotionLimited(joint.angularXMotion), | |
IsMotionLimited(joint.angularYMotion), | |
IsMotionLimited(joint.angularZMotion) | |
); | |
var jointFrameOrientation = GetJointFrameOrientation(joint.axis, joint.secondaryAxis); | |
var jointData = CreateConfigurableJoint(system, jointFrameOrientation, joint, linearLocks, linearLimited, joint.linearLimit, joint.linearLimitSpring, angularFree, angularLocks, angularLimited, | |
joint.lowAngularXLimit, joint.highAngularXLimit, joint.angularXLimitSpring, joint.angularYLimit, joint.angularZLimit, joint.angularYZLimitSpring); | |
//CreateJointEntity(system, joint.gameObject, jointData, system.GetPrimaryEntity(joint.gameObject), joint.connectedBody == null ? Entity.Null : system.GetPrimaryEntity(joint.connectedBody), joint.enableCollision); | |
} | |
// from physics legacyconversionsystem 0.4.0p5 | |
// i didnt need to convert this particular method | |
static void ConvertConfigurableJoint(LegacyConfigurable joint) | |
{ | |
var linearLocks = | |
GetAxesWithMotionType(ConfigurableJointMotion.Locked, joint.xMotion, joint.yMotion, joint.zMotion); | |
var linearLimited = | |
GetAxesWithMotionType(ConfigurableJointMotion.Limited, joint.xMotion, joint.yMotion, joint.zMotion); | |
var angularFree = | |
GetAxesWithMotionType(ConfigurableJointMotion.Free, joint.angularXMotion, joint.angularYMotion, joint.angularZMotion); | |
var angularLocks = | |
GetAxesWithMotionType(ConfigurableJointMotion.Locked, joint.angularXMotion, joint.angularYMotion, joint.angularZMotion); | |
var angularLimited = | |
GetAxesWithMotionType(ConfigurableJointMotion.Limited, joint.angularXMotion, joint.angularYMotion, joint.angularZMotion); | |
var jointFrameOrientation = GetJointFrameOrientation(joint.axis, joint.secondaryAxis); | |
var jointData = CreateConfigurableJoint(jointFrameOrientation, joint, linearLocks, linearLimited, joint.linearLimit, joint.linearLimitSpring, angularFree, angularLocks, angularLimited, | |
joint.lowAngularXLimit, joint.highAngularXLimit, joint.angularXLimitSpring, joint.angularYLimit, joint.angularZLimit, joint.angularYZLimitSpring); | |
//m_EndJointConversionSystem.CreateJointEntity(joint, GetConstrainedBodyPair(joint), jointData); | |
}*/ | |
static bool3 GetAxesWithMotionType( | |
ConfigurableJointMotion motionType, | |
ConfigurableJointMotion x, ConfigurableJointMotion y, ConfigurableJointMotion z | |
) => | |
new bool3(x == motionType, y == motionType, z == motionType); | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment