public static BlobAssetReference <JointData> CreateKnee(GameObject upperLeg, GameObject lowerLeg)
{
float upperLegHeight = 2.0f * upperLeg.transform.localScale.y;
float3 pivotUpperLeg = new float3(0, -upperLegHeight / 2.0f, 0);
var lowerLegTransform = new RigidTransform(lowerLeg.transform.rotation, lowerLeg.transform.position);
var upperLegTransform = new RigidTransform(upperLeg.transform.rotation, upperLeg.transform.position);
float3 pivotLowerLeg = math.transform(math.inverse(lowerLegTransform), math.transform(upperLegTransform, pivotUpperLeg));
float3 axis = new float3(1, 0, 0);
float3 perpendicular = new float3(0, 0, 1);
var jointFrameUpperLeg = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotUpperLeg
};
var jointFrameLowerLeg = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotLowerLeg
};
return(JointData.CreateLimitedHinge(jointFrameUpperLeg, jointFrameLowerLeg, new FloatRange {
Min = -1.2f
}));
}
public static BlobAssetReference <JointData> CreateElbow(GameObject upperArm, GameObject lowerArm)
{
float upperArmLength = 2 * upperArm.transform.localScale.y;
float lowerArmLength = 2 * lowerArm.transform.localScale.y;
float sign = math.sign(-1.0f * upperArm.transform.position.x);
float3 pivotUpper = new float3(-1.0f * sign * upperArmLength / 2.0f, 0, 0);
float3 pivotLower = new float3(sign * lowerArmLength / 2.0f, 0, 0);
pivotUpper = math.rotate(math.inverse(upperArm.transform.rotation), pivotUpper);
pivotLower = math.rotate(math.inverse(lowerArm.transform.rotation), pivotLower);
float3 axis = new float3(0, 0, 1);
float3 perpendicular = new float3(0, 1, 0);
var jointFrameUpperArm = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotUpper
};
var jointFrameLowerArm = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotLower
};
return(JointData.CreateLimitedHinge(jointFrameUpperArm, jointFrameLowerArm, new FloatRange {
Max = 3f
}));
}
public static void CreateWaist(GameObject torso, GameObject pelvis, out BlobAssetReference <JointData> jointData0, out BlobAssetReference <JointData> jointData1)
{
float3 pivotTorso = float3.zero;
RigidTransform pelvisTransform = new RigidTransform(pelvis.transform.rotation, pelvis.transform.position);
RigidTransform torsoTransform = new RigidTransform(torso.transform.rotation, torso.transform.position);
float3 pivotPelvis = math.transform(math.inverse(pelvisTransform), math.transform(torsoTransform, pivotTorso));
float3 axisPelvis = new float3(0, 0, -1);
float3 axisTorso = axisPelvis;
float3 axisPerpendicular = new float3(1, 0, 0);
float3 perpendicularPelvis = math.rotate(math.inverse(pelvisTransform), axisPerpendicular);
float3 perpendicularTorso = math.rotate(math.inverse(torsoTransform), axisPerpendicular);
float coneAngle = 0.0872665f;
var perpendicularAngle = new FloatRange {
Max = math.PI
};
var twistAngle = new FloatRange(-0.01f, 0.1f);
var jointFrameTorso = new JointFrame {
Axis = axisTorso, PerpendicularAxis = perpendicularTorso, Position = pivotTorso
};
var jointFramePelvis = new JointFrame {
Axis = axisPelvis, PerpendicularAxis = perpendicularPelvis, Position = pivotPelvis
};
JointData.CreateRagdoll(jointFrameTorso, jointFramePelvis, coneAngle, perpendicularAngle, twistAngle, out jointData0, out jointData1);
}
public static void CreateHip(GameObject pelvis, GameObject upperLeg, out BlobAssetReference <JointData> jointData0, out BlobAssetReference <JointData> jointData1)
{
float upperLegHeight = 2.0f * upperLeg.transform.localScale.y;
var pelvisTransform = new RigidTransform(pelvis.transform.rotation, pelvis.transform.position);
var upperLegTransform = new RigidTransform(upperLeg.transform.rotation, upperLeg.transform.position);
float3 pivotLeg = new float3(0, upperLegHeight / 2.0f, 0);
float3 pivotPelvis = math.transform(math.inverse(pelvisTransform), math.transform(upperLegTransform, pivotLeg));
float3 twistAxis = new float3(0, -1, 0);
float3 perpendicularAxis = new float3(1, 0, 0);
float3 twistAxisLeg = math.rotate(math.inverse(upperLegTransform), twistAxis);
float3 twistAxisPelvis = math.rotate(math.inverse(pelvisTransform), twistAxis);
float3 perpendicularAxisLeg = math.rotate(math.inverse(upperLegTransform), perpendicularAxis);
float3 perpendicularAxisPelvis = math.rotate(math.inverse(pelvisTransform), perpendicularAxis);
float coneAngle = math.PI / 4.0f;
var perpendicularAngle = new FloatRange(math.PI / 3f, math.PI * 2f / 3f);
var twistAngle = new FloatRange(-0.2f, 0.2f);
var jointFramePelvis = new JointFrame {
Axis = twistAxisPelvis, PerpendicularAxis = perpendicularAxisPelvis, Position = pivotPelvis
};
var jointFrameLeg = new JointFrame {
Axis = twistAxisLeg, PerpendicularAxis = perpendicularAxisLeg, Position = pivotLeg
};
JointData.CreateRagdoll(jointFramePelvis, jointFrameLeg, coneAngle, perpendicularAngle, twistAngle, out jointData0, out jointData1);
}
public static void CreateNeck(GameObject torso, GameObject head, out BlobAssetReference <JointData> jointData0, out BlobAssetReference <JointData> jointData1)
{
var headTransform = head.GetComponent <Transform>();
float headRadius = 0.5f * headTransform.localScale.x;
float3 pivotHead = new float3(0, -headRadius, 0);
var torsoTransform = torso.GetComponent <Transform>();
var torsoRigidTransform = new RigidTransform(torsoTransform.rotation, torsoTransform.position);
var headRigidTransform = new RigidTransform(headTransform.rotation, headTransform.position);
float3 pivotTorso = math.transform(math.inverse(torsoRigidTransform), math.transform(headRigidTransform, pivotHead));
float3 axis = new float3(0, 1, 0);
float3 perpendicular = new float3(0, 0, 1);
float coneAngle = math.PI / 5.0f;
var perpendicularAngle = new FloatRange {
Max = math.PI
};
var twistAngle = new FloatRange(-math.PI / 3f, math.PI / 3f);
var jointFrameTorso = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotTorso
};
var jointFrameHead = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotHead
};
JointData.CreateRagdoll(jointFrameTorso, jointFrameHead, coneAngle, perpendicularAngle, twistAngle, out jointData0, out jointData1);
}
private static void ConvertJointFrame(JointFrame jfDest, string jointName, FrameElement srcFrame)
{
Matrix3D matrix;
switch (jointName)
{
case "hip":
matrix = jfDest.Matrix;
JointFrame jfRoot = srcFrame.GetJointFrame("root");
Matrix3D rootMatrix = jfRoot.Matrix;
rootMatrix.OffsetX *= 4;
rootMatrix.OffsetY *= 4;
rootMatrix.OffsetZ *= 4;
rootMatrix.OffsetY += 12;
matrix.Append(rootMatrix);
jfDest.Matrix = matrix;
break;
case "abdomen":
matrix = jfDest.Matrix;
JointFrame jfLower = srcFrame.GetJointFrame("lower_body");
Matrix3D lowerMatrix = jfLower.Matrix;
lowerMatrix.Invert();
matrix.Append(lowerMatrix);
jfDest.Matrix = matrix;
break;
case "chest":
matrix = jfDest.Matrix;
//JointFrame jfUpper = srcFrame.GetJointFrame("upper_body");
//Matrix3D upperMatrix = jfUpper.Matrix;
//matrix.Append(upperMatrix);
jfDest.Matrix = matrix;
break;
case "lShldr":
matrix = jfDest.Matrix;
matrix.RotatePrepend(new Quaternion(new Vector3D(0, 0, 1), -35));
jfDest.Matrix = matrix;
break;
case "lForeArm":
case "lHand":
matrix = jfDest.Matrix;
matrix.RotatePrepend(new Quaternion(new Vector3D(0, 0, 1), -35));
matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1), 35));
jfDest.Matrix = matrix;
break;
case "rShldr":
// jfDest.AddValue("Zrotation", 35);
matrix = jfDest.Matrix;
matrix.RotatePrepend(new Quaternion(new Vector3D(0, 0, 1), 35));
jfDest.Matrix = matrix;
break;
case "rForeArm":
case "rHand":
matrix = jfDest.Matrix;
matrix.RotatePrepend(new Quaternion(new Vector3D(0, 0, 1), 35));
matrix.Rotate(new Quaternion(new Vector3D(0, 0, 1), -35));
jfDest.Matrix = matrix;
break;
}
}
protected override void Start()
{
base.Start();
// Enable the joint viewer
SetDebugDisplay(new Unity.Physics.Authoring.PhysicsDebugDisplayData
{
DrawJoints = 1
});
BlobAssetReference <Unity.Physics.Collider> collider = Unity.Physics.BoxCollider.Create(new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = new float3(0.2f, 1.0f, 0.2f),
BevelRadius = 0.0f
});
// Make some ragdoll joints
for (int i = 0; i < 10; i++)
{
// Create a body
Entity body = CreateDynamicBody(
new float3((i - 4.5f) * 1.0f, 0, 0), quaternion.identity, collider, float3.zero, float3.zero, 1.0f);
// Create the ragdoll joint
float3 pivotLocal = new float3(0, 0.5f, 0);
float3 pivotInWorld = math.transform(GetBodyTransform(body), pivotLocal);
float3 axisLocal = new float3(0, 1, 0);
float3 perpendicularLocal = new float3(0, 0, 1);
quaternion worldFromLocal = Quaternion.AngleAxis((i - 4.5f) * 20.0f, new float3(0, 0, 1));
float3 axisWorld = math.mul(worldFromLocal, axisLocal);
float3 perpendicularWorld = math.mul(worldFromLocal, perpendicularLocal);
float maxConeAngle = (float)math.PI / 4.0f;
var perpendicularAngle = new FloatRange(-math.PI / 2f, math.PI / 2f);
var twistAngle = new FloatRange(-math.PI / 8f, math.PI / 8f);
BlobAssetReference <JointData> ragdoll0, ragdoll1;
var localFrame = new JointFrame {
Axis = axisLocal, PerpendicularAxis = perpendicularLocal, Position = pivotLocal
};
var worldFrame = new JointFrame {
Axis = axisWorld, PerpendicularAxis = perpendicularWorld, Position = pivotInWorld
};
JointData.CreateRagdoll(localFrame, worldFrame, maxConeAngle, perpendicularAngle, twistAngle, out ragdoll0, out ragdoll1);
CreateJoint(ragdoll0, body, Entity.Null);
CreateJoint(ragdoll1, body, Entity.Null);
}
}
void ConvertFixedJoint(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 = GetPrimaryEntity(joint.connectedBody);
RigidTransform worldFromBodyB = connectedEntity == Entity.Null
? RigidTransform.identity
: Math.DecomposeRigidBodyTransform(joint.connectedBody.transform.localToWorldMatrix);
var bodyAFromJoint = new JointFrame(math.mul(math.inverse(worldFromBodyA), legacyWorldFromJointA));
var bodyBFromJoint = new JointFrame(math.mul(math.inverse(worldFromBodyB), legacyWorldFromJointA));
var jointData = JointData.CreateFixed(bodyAFromJoint, bodyBFromJoint);
CreateJointEntity(joint.gameObject, jointData, GetPrimaryEntity(joint.gameObject), joint.connectedBody == null ? Entity.Null : connectedEntity, joint.enableCollision);
}
public static BlobAssetReference <JointData> CreateWrist(GameObject lowerArm, GameObject hand)
{
float armLength = 2.0f * lowerArm.transform.localScale.y;
float handLength = 2.0f * hand.transform.localScale.y;
float sign = math.sign(-1.0f * lowerArm.transform.position.x);
float3 pivotFore = new float3(0, -1.0f * sign * armLength / 2.0f, 0);
float3 pivotHand = new float3(0, sign * handLength / 2.0f, 0);
float3 axis = new float3(0, 0, 1);
float3 perpendicular = new float3(0, 1, 0);
var jointFrameForearm = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotFore
};
var jointFrameHand = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotHand
};
return(JointData.CreateLimitedHinge(jointFrameForearm, jointFrameHand, new FloatRange(-0.3f, 0.6f)));
}
protected override unsafe void Start()
{
init(float3.zero); // no gravity
// Enable the joint viewer
SetDebugDisplay(new Unity.Physics.Authoring.PhysicsDebugDisplayData
{
DrawJoints = 1
});
Entity *entities = stackalloc Entity[2];
entities[1] = Entity.Null;
for (int i = 0; i < 1; i++)
{
CollisionFilter filter = new CollisionFilter
{
CollidesWith = (uint)(1 << i),
BelongsTo = (uint)~(1 << (1 - i))
};
BlobAssetReference <Collider> collider = BoxCollider.Create(
new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = new float3(1.0f, 0.2f, 0.2f),
BevelRadius = 0.0f
},
filter, Material.Default);
entities[i] = CreateDynamicBody(float3.zero, quaternion.identity, collider, float3.zero, new float3(0, 1 - i, 0), 1.0f);
}
var jointFrame = new JointFrame {
Axis = new float3(0, 1, 0), PerpendicularAxis = new float3(0, 0, 1)
};
BlobAssetReference <JointData> hingeData =
JointData.CreateLimitedHinge(jointFrame, jointFrame, new FloatRange(-math.PI, -0.2f));
CreateJoint(hingeData, entities[0], entities[1]);
}
public static BlobAssetReference <JointData> CreateAnkle(GameObject lowerLeg, GameObject foot)
{
float lowerLegLength = 2.0f * lowerLeg.transform.localScale.y;
var lowerLegTransform = new RigidTransform(lowerLeg.transform.rotation, lowerLeg.transform.position);
var footTransform = new RigidTransform(foot.transform.rotation, foot.transform.position);
float3 pivotLowerLeg = new float3(0, -lowerLegLength / 2.0f, 0);
float3 pivotFoot = math.transform(math.inverse(footTransform), math.transform(lowerLegTransform, pivotLowerLeg));
float3 axis = new float3(1, 0, 0);
float3 perpendicular = new float3(0, 0, 1);
var jointFrameLowerLeg = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotLowerLeg
};
var jointFrameFoot = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotFoot
};
return(JointData.CreateLimitedHinge(jointFrameLowerLeg, jointFrameFoot, new FloatRange(-0.4f, 0.1f)));
}
void ConvertHingeJoint(LegacyHinge joint)
{
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 JointFrame
{
Axis = joint.axis,
PerpendicularAxis = perpendicularA,
Position = joint.anchor
};
var connectedEntity = 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 JointFrame
{
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(joint.gameObject, jointData, GetPrimaryEntity(joint.gameObject), joint.connectedBody == null ? Entity.Null : connectedEntity, joint.enableCollision);
}
public static void CreateShoulder(GameObject torso, GameObject upperArm, out BlobAssetReference <JointData> jointData0, out BlobAssetReference <JointData> jointData1)
{
float armLength = 2 * upperArm.transform.localScale.y;
float sign = math.sign(-upperArm.transform.position.x);
var torsoRigidTransform = new RigidTransform(torso.transform.rotation, torso.transform.position);
var upperArmRigidTransform = new RigidTransform(upperArm.transform.rotation, upperArm.transform.position);
float3 pivotArm = new float3(sign * armLength / 2.0f, 0, 0);
pivotArm = math.rotate(math.inverse(upperArmRigidTransform.rot), pivotArm);
float3 pivotBody = math.transform(math.inverse(torsoRigidTransform), math.transform(upperArmRigidTransform, pivotArm));
float3 twistAxis = new float3(-1.0f * sign, 0, 0);
float3 perpendicularAxis = new float3(0, 0, 1);
float3 twistAxisArm = math.rotate(math.inverse(upperArmRigidTransform), twistAxis);
float3 twistAxisTorso = math.rotate(math.inverse(torsoRigidTransform), twistAxis);
float3 perpendicularAxisArm = math.rotate(math.inverse(upperArmRigidTransform), perpendicularAxis);
float3 perpendicularAxisTorso = math.rotate(math.inverse(torsoRigidTransform), perpendicularAxis);
float coneAngle = math.PI / 4.0f;
var perpendicularAngle = new FloatRange(math.PI / 6f, math.PI * 5f / 6f);
var twistAngle = new FloatRange(-0.0872665f, 0.0872665f);
var jointFrameBody = new JointFrame {
Axis = twistAxisTorso, PerpendicularAxis = perpendicularAxisTorso, Position = pivotBody
};
var jointFrameArm = new JointFrame {
Axis = twistAxisArm, PerpendicularAxis = perpendicularAxisArm, Position = pivotArm
};
JointData.CreateRagdoll(jointFrameBody, jointFrameArm, coneAngle, perpendicularAngle, twistAngle, out jointData0, out jointData1);
}
protected override void Start()
{
float3 gravity = float3.zero;
base.init(gravity);
// Enable the joint viewer
SetDebugDisplay(new Unity.Physics.Authoring.PhysicsDebugDisplayData
{
DrawJoints = 1
});
// Make soft ball and sockets
{
BlobAssetReference <Unity.Physics.Collider> collider = Unity.Physics.BoxCollider.Create(new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = new float3(0.2f, 0.2f, 0.2f),
BevelRadius = 0.0f
});
// Make joints with different spring frequency. The leftmost joint should oscillate at 0.5hz, the next at 1hz, the next at 1.5hz, etc.
for (int i = 0; i < 10; i++)
{
// Create a body
float3 position = new float3((i - 4.5f) * 1.0f, 0, 0);
float3 velocity = new float3(0, -10.0f, 0);
Entity body = CreateDynamicBody(
position, quaternion.identity, collider, velocity, float3.zero, 1.0f);
// Create the ball and socket joint
float3 pivotLocal = float3.zero;
float3 pivotInWorld = math.transform(GetBodyTransform(body), pivotLocal);
BlobAssetReference <JointData> jointData;
jointData = JointData.CreateBallAndSocket(pivotLocal, pivotInWorld);
var constraint = jointData.Value.Constraints[0];
constraint.SpringDamping = 0.0f;
constraint.SpringFrequency = 0.5f * (float)(i + 1);
jointData.Value.Constraints[0] = constraint;
CreateJoint(jointData, body, Entity.Null);
}
}
// Make soft limited hinges
{
BlobAssetReference <Unity.Physics.Collider> collider = Unity.Physics.BoxCollider.Create(new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = new float3(0.4f, 0.1f, 0.6f),
BevelRadius = 0.0f
});
// First row has soft limit with hard hinge + pivot, second row has everything soft
for (int j = 0; j < 2; j++)
{
for (int i = 0; i < 10; i++)
{
// Create a body
float3 position = new float3((i - 4.5f) * 1.0f, 0, (j + 1) * 3.0f);
float3 velocity = new float3(0, -10.0f, 0);
float3 angularVelocity = new float3(0, 0, -10.0f);
Entity body = CreateDynamicBody(
position, quaternion.identity, collider, velocity, angularVelocity, 1.0f);
// Create the limited hinge joint
float3 pivotLocal = new float3(0, 0, 0);
float3 pivotInWorld = math.transform(GetBodyTransform(body), pivotLocal);
float3 axisLocal = new float3(0, 0, 1);
float3 axisInWorld = axisLocal;
float3 perpendicularLocal = new float3(0, 1, 0);
float3 perpendicularInWorld = perpendicularLocal;
BlobAssetReference <JointData> jointData;
var frameLocal = new JointFrame {
Axis = axisLocal, PerpendicularAxis = perpendicularLocal, Position = pivotLocal
};
var frameWorld = new JointFrame {
Axis = axisInWorld, PerpendicularAxis = perpendicularInWorld, Position = pivotInWorld
};
jointData = JointData.CreateLimitedHinge(frameLocal, frameWorld, default);
// First constraint is the limit, next two are the hinge and pivot
for (int k = 0; k < 1 + 2 * j; k++)
{
var constraint = jointData.Value.Constraints[k];
constraint.SpringDamping = 0.0f;
constraint.SpringFrequency = 0.5f * (float)(i + 1);
jointData.Value.Constraints[k] = constraint;
}
CreateJoint(jointData, body, Entity.Null);
}
}
}
// Make a soft prismatic
{
BlobAssetReference <Unity.Physics.Collider> collider = Unity.Physics.BoxCollider.Create(new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = new float3(0.2f, 0.2f, 0.2f),
BevelRadius = 0.0f
});
// Create a body
float3 position = new float3(0, 0, 9.0f);
float3 velocity = new float3(50.0f, 0, 0);
Entity body = CreateDynamicBody(
position, quaternion.identity, collider, velocity, float3.zero, 1.0f);
// Create the prismatic joint
float3 pivotLocal = float3.zero;
float3 pivotInWorld = math.transform(GetBodyTransform(body), pivotLocal);
float3 axisLocal = new float3(1, 0, 0);
float3 axisInWorld = axisLocal;
float3 perpendicularLocal = new float3(0, 1, 0);
float3 perpendicularInWorld = perpendicularLocal;
BlobAssetReference <JointData> jointData;
var localFrame = new JointFrame {
Axis = axisLocal, PerpendicularAxis = perpendicularLocal, Position = pivotLocal
};
var worldFrame = new JointFrame {
Axis = axisInWorld, PerpendicularAxis = perpendicularInWorld, Position = pivotInWorld
};
jointData = JointData.CreatePrismatic(localFrame, worldFrame, new FloatRange(-2f, 2f), default);
var constraint = jointData.Value.Constraints[0];
constraint.SpringDamping = 0.0f;
constraint.SpringFrequency = 5.0f;
jointData.Value.Constraints[0] = constraint;
CreateJoint(jointData, body, Entity.Null);
}
}
private void CreateRagdoll(float3 positionOffset, quaternion rotationOffset, int ragdollIndex = 1, bool internalCollisions = false)
{
var entities = new NativeList <Entity>(Allocator.Temp);
// Head
float headRadius = 0.1f;
float3 headPosition = new float3(0, 1.8f, 0);
Entity head;
{
CollisionFilter filter = internalCollisions ? layerFilter(layer.Head, layer.Torso) : groupFilter(-ragdollIndex);
BlobAssetReference <Unity.Physics.Collider> collider = Unity.Physics.SphereCollider.Create(new SphereGeometry
{
Center = float3.zero,
Radius = headRadius
}, filter);
head = CreateDynamicBody(headPosition, quaternion.identity, collider, float3.zero, float3.zero, 5.0f);
}
entities.Add(head);
// Torso
float3 torsoSize;
float3 torsoPosition;
Entity torso;
{
//UnityEngine.Mesh torsoMesh = (UnityEngine.Mesh)Resources.Load("torso", typeof(UnityEngine.Mesh));
torsoSize = torsoMesh.bounds.size;
torsoPosition = headPosition - new float3(0, headRadius * 3.0f / 4.0f + torsoSize.y, 0);
CollisionFilter filter = internalCollisions ? layerFilter(layer.Torso, layer.Thigh | layer.Head | layer.UpperArm | layer.Pelvis) : groupFilter(-ragdollIndex);
NativeArray <float3> points = new NativeArray <float3>(torsoMesh.vertices.Length, Allocator.TempJob);
for (int i = 0; i < torsoMesh.vertices.Length; i++)
{
points[i] = torsoMesh.vertices[i];
}
BlobAssetReference <Unity.Physics.Collider> collider = ConvexCollider.Create(
points, ConvexHullGenerationParameters.Default, CollisionFilter.Default
);
points.Dispose();
collider.Value.Filter = filter;
torso = CreateDynamicBody(torsoPosition, quaternion.identity, collider, float3.zero, float3.zero, 20.0f);
}
entities.Add(torso);
// Neck
{
float3 pivotHead = new float3(0, -headRadius, 0);
float3 pivotBody = math.transform(math.inverse(GetBodyTransform(torso)), math.transform(GetBodyTransform(head), pivotHead));
float3 axis = new float3(0, 1, 0);
float3 perpendicular = new float3(0, 0, 1);
float coneAngle = (float)math.PI / 5.0f;
var perpendicularAngle = new FloatRange {
Max = math.PI
}; // unlimited
var twistAngle = new FloatRange(-math.PI / 3f, math.PI / 3f);
var headFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotHead
};
var bodyFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotBody
};
BlobAssetReference <JointData> ragdoll0, ragdoll1;
JointData.CreateRagdoll(headFrame, bodyFrame, coneAngle, perpendicularAngle, twistAngle, out ragdoll0, out ragdoll1);
CreateJoint(ragdoll0, head, torso);
CreateJoint(ragdoll1, head, torso);
}
// Arms
{
float armLength = 0.25f;
float armRadius = 0.05f;
CollisionFilter armUpperFilter = internalCollisions ? layerFilter(layer.UpperArm, layer.Torso | layer.Forearm) : groupFilter(-ragdollIndex);
CollisionFilter armLowerFilter = internalCollisions ? layerFilter(layer.Forearm, layer.UpperArm | layer.Hand) : groupFilter(-ragdollIndex);
BlobAssetReference <Unity.Physics.Collider> upperArmCollider = Unity.Physics.CapsuleCollider.Create(new CapsuleGeometry
{
Vertex0 = new float3(-armLength / 2, 0, 0),
Vertex1 = new float3(armLength / 2, 0, 0),
Radius = armRadius
}, armUpperFilter);
BlobAssetReference <Unity.Physics.Collider> foreArmCollider = Unity.Physics.CapsuleCollider.Create(new CapsuleGeometry
{
Vertex0 = new float3(-armLength / 2, 0, 0),
Vertex1 = new float3(armLength / 2, 0, 0),
Radius = armRadius
}, armLowerFilter);
float handLength = 0.025f;
float handRadius = 0.055f;
CollisionFilter handFilter = internalCollisions ? layerFilter(layer.Hand, layer.Forearm) : groupFilter(-ragdollIndex);
BlobAssetReference <Unity.Physics.Collider> handCollider = Unity.Physics.CapsuleCollider.Create(new CapsuleGeometry
{
Vertex0 = new float3(-handLength / 2, 0, 0),
Vertex1 = new float3(handLength / 2, 0, 0),
Radius = handRadius
}, handFilter);
for (int i = 0; i < 2; i++)
{
float s = i * 2 - 1.0f;
float3 upperArmPosition = torsoPosition + new float3(s * (torsoSize.x + armLength) / 2.0f, 0.9f * torsoSize.y - armRadius, 0.0f);
Entity upperArm = CreateDynamicBody(upperArmPosition, quaternion.identity, upperArmCollider, float3.zero, float3.zero, 10.0f);
float3 foreArmPosition = upperArmPosition + new float3(armLength * s, 0, 0);
Entity foreArm = CreateDynamicBody(foreArmPosition, quaternion.identity, foreArmCollider, float3.zero, float3.zero, 5.0f);
float3 handPosition = foreArmPosition + new float3((armLength + handLength) / 2.0f * s, 0, 0);
Entity hand = CreateDynamicBody(handPosition, quaternion.identity, handCollider, float3.zero, float3.zero, 2.0f);
entities.Add(upperArm);
entities.Add(foreArm);
entities.Add(hand);
// shoulder
{
float3 pivotArm = new float3(-s * armLength / 2.0f, 0, 0);
float3 pivotBody = math.transform(math.inverse(GetBodyTransform(torso)), math.transform(GetBodyTransform(upperArm), pivotArm));
float3 axis = new float3(s, 0, 0);
float3 perpendicular = new float3(0, 0, 1);
float coneAngle = (float)math.PI / 2.0f;
var perpendicularAngle = new FloatRange {
Max = math.PI / 2f
};
var twistAngle = new FloatRange(-math.PI / 4f, math.PI / 4f);
var armFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotArm
};
var bodyFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotBody
};
BlobAssetReference <JointData> ragdoll0, ragdoll1;
JointData.CreateRagdoll(armFrame, bodyFrame, coneAngle, perpendicularAngle, twistAngle, out ragdoll0, out ragdoll1);
CreateJoint(ragdoll0, upperArm, torso);
CreateJoint(ragdoll1, upperArm, torso);
}
// elbow
{
float3 pivotUpper = new float3(s * armLength / 2.0f, 0, 0);
float3 pivotFore = -pivotUpper;
float3 axis = new float3(0, -s, 0);
float3 perpendicular = new float3(s, 0, 0);
var lowerArmFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotFore
};
var upperArmFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotUpper
};
BlobAssetReference <JointData> hinge =
JointData.CreateLimitedHinge(lowerArmFrame, upperArmFrame, new FloatRange {
Max = 3f
});
CreateJoint(hinge, foreArm, upperArm);
}
// wrist
{
float3 pivotFore = new float3(s * armLength / 2.0f, 0, 0);
float3 pivotHand = new float3(-s * handLength / 2.0f, 0, 0);
float3 axis = new float3(0, -s, 0);
float3 perpendicular = new float3(s, 0, 0);
var handFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotHand
};
var forearmFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotFore
};
BlobAssetReference <JointData> hinge =
JointData.CreateLimitedHinge(handFrame, forearmFrame, new FloatRange(-0.3f, 0.6f));
CreateJoint(hinge, hand, foreArm);
}
}
}
// Pelvis
float pelvisRadius = 0.08f;
float pelvisLength = 0.22f;
float3 pelvisPosition = torsoPosition - new float3(0, pelvisRadius * 0.75f, 0.0f);
Entity pelvis;
{
CollisionFilter filter = internalCollisions ? layerFilter(layer.Pelvis, layer.Torso | layer.Thigh) : groupFilter(-ragdollIndex);
BlobAssetReference <Unity.Physics.Collider> collider = Unity.Physics.CapsuleCollider.Create(new CapsuleGeometry
{
Vertex0 = new float3(-pelvisLength / 2, 0, 0),
Vertex1 = new float3(pelvisLength / 2, 0, 0),
Radius = pelvisRadius
}, filter);
pelvis = CreateDynamicBody(pelvisPosition, quaternion.identity, collider, float3.zero, float3.zero, 15.0f);
}
entities.Add(pelvis);
// Waist
{
float3 pivotTorso = float3.zero;
float3 pivotPelvis = math.transform(math.inverse(GetBodyTransform(pelvis)), math.transform(GetBodyTransform(torso), pivotTorso));
float3 axis = new float3(0, -1, 0);
float3 perpendicular = new float3(0, 0, 1);
float coneAngle = 0.1f;
var perpendicularAngle = new FloatRange(-0.1f, math.PI);
var twistAngle = new FloatRange(-0.1f, 0.1f);
BlobAssetReference <JointData> ragdoll0, ragdoll1;
var pelvisFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotPelvis
};
var torsoFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotTorso
};
JointData.CreateRagdoll(pelvisFrame, torsoFrame, coneAngle, perpendicularAngle, twistAngle, out ragdoll0, out ragdoll1);
CreateJoint(ragdoll0, pelvis, torso);
CreateJoint(ragdoll1, pelvis, torso);
}
// Legs
{
float thighLength = 0.32f;
float thighRadius = 0.08f;
CollisionFilter thighFilter = internalCollisions ? layerFilter(layer.Thigh, layer.Pelvis | layer.Calf) : groupFilter(-ragdollIndex);
BlobAssetReference <Unity.Physics.Collider> thighCollider = Unity.Physics.CapsuleCollider.Create(new CapsuleGeometry
{
Vertex0 = new float3(0, -thighLength / 2, 0),
Vertex1 = new float3(0, thighLength / 2, 0),
Radius = thighRadius
}, thighFilter);
float calfLength = 0.32f;
float calfRadius = 0.06f;
CollisionFilter calfFilter = internalCollisions ? layerFilter(layer.Calf, layer.Thigh | layer.Foot) : groupFilter(-ragdollIndex);
BlobAssetReference <Unity.Physics.Collider> calfCollider = Unity.Physics.CapsuleCollider.Create(new CapsuleGeometry
{
Vertex0 = new float3(0, -calfLength / 2, 0),
Vertex1 = new float3(0, calfLength / 2, 0),
Radius = calfRadius
}, calfFilter);
float footLength = 0.08f;
float footRadius = 0.06f;
CollisionFilter footFilter = internalCollisions ? layerFilter(layer.Foot, layer.Calf) : groupFilter(-ragdollIndex);
BlobAssetReference <Unity.Physics.Collider> footCollider = Unity.Physics.CapsuleCollider.Create(new CapsuleGeometry
{
Vertex0 = new float3(0),
Vertex1 = new float3(0, 0, footLength),
Radius = footRadius
}, footFilter);
for (int i = 0; i < 2; i++)
{
float s = i * 2 - 1.0f;
float3 thighPosition = pelvisPosition + new float3(s * pelvisLength / 2.0f, -thighLength / 2.0f, 0.0f);
Entity thigh = CreateDynamicBody(thighPosition, quaternion.identity, thighCollider, float3.zero, float3.zero, 10.0f);
float3 calfPosition = thighPosition + new float3(0, -(thighLength + calfLength) / 2.0f, 0);
Entity calf = CreateDynamicBody(calfPosition, quaternion.identity, calfCollider, float3.zero, float3.zero, 5.0f);
float3 footPosition = calfPosition + new float3(0, -calfLength / 2.0f, 0);
Entity foot = CreateDynamicBody(footPosition, quaternion.identity, footCollider, float3.zero, float3.zero, 2.0f);
entities.Add(thigh);
entities.Add(calf);
entities.Add(foot);
// hip
{
float3 pivotThigh = new float3(0, thighLength / 2.0f, 0);
float3 pivotBody = math.transform(math.inverse(GetBodyTransform(torso)), math.transform(GetBodyTransform(thigh), pivotThigh));
float3 axis = new float3(0, -1, 0);
float3 perpendicular = new float3(s, 0, 0);
float coneAngle = (float)math.PI / 4.0f;
var perpendicularAngle = new FloatRange {
Max = 0.2f + math.PI / 2.0f
};
var twistAngle = new FloatRange(-0.2f, 0.2f);
var upperLegFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotThigh
};
var bodyFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotBody
};
BlobAssetReference <JointData> ragdoll0, ragdoll1;
JointData.CreateRagdoll(upperLegFrame, bodyFrame, coneAngle, perpendicularAngle, twistAngle, out ragdoll0, out ragdoll1);
CreateJoint(ragdoll0, thigh, torso);
CreateJoint(ragdoll1, thigh, torso);
}
// knee
{
float3 pivotThigh = new float3(0, -thighLength / 2.0f, 0);
float3 pivotCalf = math.transform(math.inverse(GetBodyTransform(calf)), math.transform(GetBodyTransform(thigh), pivotThigh));
float3 axis = new float3(-1, 0, 0);
float3 perpendicular = new float3(0, 0, 1);
var lowerLegFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotCalf
};
var upperLegFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotThigh
};
BlobAssetReference <JointData> hinge =
JointData.CreateLimitedHinge(lowerLegFrame, upperLegFrame, new FloatRange {
Min = -1.2f
});
CreateJoint(hinge, calf, thigh);
}
// ankle
{
float3 pivotCalf = new float3(0, -calfLength / 2.0f, 0);
float3 pivotFoot = float3.zero;
float3 axis = new float3(-1, 0, 0);
float3 perpendicular = new float3(0, 0, 1);
var footFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotFoot
};
var lowerLegFrame = new JointFrame {
Axis = axis, PerpendicularAxis = perpendicular, Position = pivotCalf
};
BlobAssetReference <JointData> hinge =
JointData.CreateLimitedHinge(footFrame, lowerLegFrame, new FloatRange(-0.4f, 0.1f));
CreateJoint(hinge, foot, calf);
}
}
}
var entityManager = BasePhysicsDemo.DefaultWorld.EntityManager;
// reposition with offset information
if (entities.Length > 0)
{
float3 center = float3.zero;
for (int i = 0; i < entities.Length; i++)
{
var e = entities[i];
center += entityManager.GetComponentData <Translation>(e).Value;
}
center /= entities.Length;
for (int i = 0; i < entities.Length; i++)
{
var e = entities[i];
Translation positionComponent = entityManager.GetComponentData <Translation>(e);
Rotation rotationComponent = entityManager.GetComponentData <Rotation>(e);
float3 position = positionComponent.Value;
quaternion rotation = rotationComponent.Value;
float3 localPosition = position - center;
localPosition = math.rotate(rotationOffset, localPosition);
position = localPosition + center + positionOffset;
rotation = math.mul(rotation, rotationOffset);
positionComponent.Value = position;
rotationComponent.Value = rotation;
entityManager.SetComponentData <Translation>(e, positionComponent);
entityManager.SetComponentData <Rotation>(e, rotationComponent);
}
}
}
BlobAssetReference <JointData> CreateConfigurableJoint(
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);
// 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))
{
constraints.Add(new Constraint
{
ConstrainedAxes = linearLimited,
Type = ConstraintType.Linear,
Min = math.csum((int3)linearLimited) == 1 ? -linearLimit.limit : 0f,
Max = linearLimit.limit,
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 JointFrame(math.mul(math.inverse(worldFromBodyA), legacyWorldFromJointA));
var connectedEntity = 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 JointFrame
{
Axis = math.mul(bFromA.rot, bodyAFromJoint.Axis),
PerpendicularAxis = math.mul(bFromA.rot, bodyAFromJoint.PerpendicularAxis),
Position = math.mul(bFromBSource, new float4(joint.connectedAnchor, 1f)).xyz
};
var jointData = JointData.Create(bodyAFromJoint, bodyBFromJoint, constraints);
constraints.Dispose();
return(jointData);
}
