public void JointDataCreateBallAndSocketTest()
{
var positionAinA = float3.zero;
var positionBinB = float3.zero;
var jointDataRef = JointData.CreateBallAndSocket(positionAinA, positionBinB);
var jointData = jointDataRef.Value;
Assert.AreEqual(MTransform.Identity, jointData.AFromJoint);
Assert.AreEqual(MTransform.Identity, jointData.BFromJoint);
Assert.AreEqual(1, jointData.Version);
Assert.AreEqual(1, jointData.NumConstraints);
var constraint = jointDataRef.Value.Constraints[0];
Assert.AreEqual(new bool3(true), constraint.ConstrainedAxes);
Assert.AreEqual(ConstraintType.Linear, constraint.Type);
Assert.AreEqual(0.0f, constraint.Min);
Assert.AreEqual(0.0f, constraint.Max);
Assert.AreEqual(Constraint.DefaultSpringFrequency, constraint.SpringFrequency);
Assert.AreEqual(Constraint.DefaultSpringDamping, constraint.SpringDamping);
}
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);
}
// HACK for filtering Kinect 2 arm rotations
//void filterJointRotation(ref Quaternion measuredRotation, ref Quaternion previousRotation, ref KalmanFilteredRotation kalmanFilter, float kalmanDeltaTime)
void filterJointRotation(ref Skeleton skeleton, ref JointData joint, int jointID, float kalmanDeltaTime)
{
float updateAngle = 0.01f;
float angleThreshold = 15;
float covarianceMultiplier = 1;
// float rotateSpeed = 30;
// if (jointID < 2) // Shoulders
// {
// joint.rotation = Quaternion.Slerp (skeleton.previousRotation [jointID], joint.rotation, rotateSpeed * kalmanDeltaTime );
// return;
// }
if (jointID == 8 || jointID == 9) // Heuristic for leftHand, rightHand
{
covarianceMultiplier = 2;
}
updateAngle = Mathf.Abs(Quaternion.Angle(joint.rotation, skeleton.previousRotation[jointID])); // New measurement vs previous rotation
if (updateAngle < angleThreshold)
{
// joint.rotation = Quaternion.Slerp (skeleton.previousRotation [jointID], joint.rotation, rotateSpeed * kalmanDeltaTime );
skeleton.filterRot[jointID].rotationNoiseCovariance = covarianceMultiplier * skeleton.rotationNoiseCovariance * (0.05f + 0.95f * (angleThreshold - updateAngle) / angleThreshold);
joint.rotation = skeleton.filterRot[jointID].Update(joint.rotation, kalmanDeltaTime);
// if(Time.time < 10)
// Debug.Log(1);
// joint.rotation = skeleton.filterRot[jointID].Update(joint.rotation, kalmanDeltaTime);
// print(Time.time);
}
else
{
skeleton.filterRot[jointID].rotationNoiseCovariance = covarianceMultiplier * 0.05f * skeleton.rotationNoiseCovariance;
joint.rotation = skeleton.filterRot[jointID].Update(joint.rotation, kalmanDeltaTime);
}
skeleton.previousRotation[jointID] = joint.rotation;
}
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);
}
/// @brief updates a single joint
///
/// This method updates a single joint. The decision of what to update (orientation, position)
/// depends on m_updateOrientation and m_updateJointPositions. Only joints with high confidence
/// are updated. @note it is possible to update only position or only orientation even though both
/// are expected if the confidence of one is low.
/// @param centerOffset the new central position
/// @param joint the joint we want to update
/// @param skelTrans the new transformation of the joint
protected void UpdateJoint(Joint joint)
{
// make sure something is hooked up to this joint
if ((int)joint >= m_jointTransforms.Length || !m_jointTransforms[(int)joint])
{
return;
}
JointData jd = m_jointData[(int)joint];
Quaternion rot = CalcRotationForJoint(joint);
Vector3 pos0 = (jd.m_pos - m_centerOffset) * m_scale;
// if we have debug lines to draw we need to collect the data.
if (m_isDebugLineActive & m_linesDebugger != null)
{
Vector3 pos = pos0 * 1.2f + // magnify the scale of the debugger by 1.2 times
transform.position +
new Vector3(1, 0, 0); // add a offset, from the avatar
float posConf = jd.m_posConf;
float rotConf = jd.m_orientConf;
m_linesDebugger.UpdateJointInfoForJoint(joint, pos, posConf, rot, rotConf);
}
// modify orientation (if needed and confidence is high enough)
// the confidence of the hands are always 0, maybe also ankles
if (m_updateOrientation && jd.m_orientConf >= 0.5 && joint != Joint.LeftHand && joint != Joint.RightHand)
{
m_jointTransforms[(int)joint].rotation = rot;
}
// modify position (if needed, and confidence is high enough)
if (m_updateJointPositions && jd.m_posConf >= 0.5)
{
m_jointTransforms[(int)joint].localPosition = pos0; // CalcJointPosition(jd);
}
}
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);
jointData.Value.Constraints[0].SpringDamping = 0.0f;
jointData.Value.Constraints[0].SpringFrequency = 0.5f * (float)(i + 1);
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;
jointData = JointData.CreateLimitedHinge(pivotLocal, pivotInWorld, axisLocal, axisInWorld, perpendicularLocal, perpendicularInWorld, 0.0f, 0.0f);
// First constraint is the limit, next two are the hinge and pivot
for (int k = 0; k < 1 + 2 * j; k++)
{
jointData.Value.Constraints[k].SpringDamping = 0.0f;
jointData.Value.Constraints[k].SpringFrequency = 0.5f * (float)(i + 1);
}
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;
jointData = JointData.CreatePrismatic(pivotLocal, pivotInWorld, axisLocal, axisInWorld, perpendicularLocal, perpendicularInWorld, -2.0f, 2.0f, 0.0f, 0.0f);
jointData.Value.Constraints[0].SpringDamping = 0.0f;
jointData.Value.Constraints[0].SpringFrequency = 5.0f;
CreateJoint(jointData, body, Entity.Null);
}
}
// Use this for initialization
void Start () {
sm = PXCMSenseManager.CreateInstance ();
if (sm == null)
Debug.LogError ("SenseManager Initialization Failed");
sm.QuerySession ().CreateImpl<PXCMDataSmoothing> (out ds);
joints = new JointData[4];
smoother3D = new PXCMDataSmoothing.Smoother3D[4];
LineRenderGameObject = GameObject.Find ("line2");
lineRenderer = (LineRenderer)LineRenderGameObject.GetComponent ("LineRenderer");
lineRenderer.SetVertexCount(lineLength);
for (int i=0; i<=3; i++)
{
joints [i] = new JointData ();
smoother3D [i] = ds.Create3DWeighted (10);
}
smoother4D = ds.Create3DWeighted (10);
var senseManager = GameObject.FindObjectOfType(typeof(SenseToolkitManager));
if (senseManager == null)
{
Debug.LogWarning("Sense Manager Object not found and was added automatically");
senseManager = (GameObject)Instantiate(Resources.Load("SenseManager"));
senseManager.name = "SenseManager";
}
SetSenseOptions();
}
public void Set_JointData(JointData j_data)
{
joint_position = j_data.joint_position;
joint_rotation = j_data.joint_rotation;
}
// 生成されたジョイントエンティティを返す。
public NativeArray <Entity> Convert
(EntityManager em, Entity posturePrefab, NameAndEntity[] bonePrefabs)
{
//var motionClip = this.GetComponent<MotionAuthoring>().MotionClip;//
var rbTop = this.GetComponentInChildren <Rigidbody>();//
if (rbTop == null)
{
return(new NativeArray <Entity>()); //
}
var srcColliders = this.GetComponentsInChildren <UnityEngine.Collider>();
// 名前とボーンエンティティの組を配列化
//var qNameAndBone = motionClip.StreamPaths
// .Select( x => System.IO.Path.GetFileName( x ) )
// .Select( ( name, i ) => (name, i: motionClip.IndexMapFbxToMotion[ i ]) )
// .Where( x => x.i != -1 )
// .Select( x => (Name:x.name, Entity:bonePrefabs[ x.i ].Entity) )
// .Append( (Name:rbTop.name, Entity:posturePrefab) );
//var namesAndBones = qNameAndBone.ToArray();
var namesAndBones = bonePrefabs
.Append(new NameAndEntity(rbTop.name, posturePrefab));
// クエリ用コライダの生成
// ・マテリアルが "xxx overlap collider" という名前になっているものを抽出
// ・対応するボーンエンティティに専用のコンポーネントデータを付加
var qQueryableCollider =
from x in srcColliders
where x.sharedMaterial != null
where x.sharedMaterial.name.StartsWith("overlap ")
join bone in namesAndBones
on x.name equals bone.Name
select(bone.Entity, c : x)
;
foreach (var(ent, c) in qQueryableCollider)
{
addQuearyableColliderBlobs_(ent, c, 0);
}
// コライダとそれが付くべき剛体の組を配列化（同じ剛体に複数のコライダもあり）
// ・有効でないコライダは除外する
var qColliderWithParent =
from collider in srcColliders
where collider.enabled
let parent = collider.gameObject
.AncestorsAndSelf()
.Where(anc => anc.GetComponent <Rigidbody>() != null)
.First()
select(collider, parent)
;
var collidersWithParent = qColliderWithParent.ToArray();
// 同じ剛体を親とするコライダをグループ化するクエリ
var qColliderGroup =
from x in collidersWithParent
group x.collider by x.parent
;
// 剛体を持たない子コライダを合成して、コライダコンポーネントデータを生成するクエリ
var qCompounds =
from g in qColliderGroup
select new PhysicsCollider
{
Value = createBlobCollider_(srcColliders_: g, parent: g.Key, groupIndex: 0),
};
// コライダがついているオブジェクトに相当するボーンのエンティティに、コライダコンポーネントデータを付加
// ・コライダ不要なら、質量プロパティだけ生成してコライダはつけないようにしたい（未実装）
var qEntAndComponent =
from c in (qColliderGroup, qCompounds).Zip()
join b in namesAndBones
on c.x.Key.name equals b.Name
select(b.Entity, c : c.y)
;
foreach (var(ent, c) in qEntAndComponent)
{
em.AddComponentData(ent, c);
}
// 剛体がついているオブジェクトに相当するボーンのエンティティに、各種コンポーネントデータを付加
// ・キネマティックは質量ゼロにするが、速度や質量プロパティは付加する。
// ・コライダがない場合は、球の質量プロパティになる。
var qRbAndBone =
from x in this.GetComponentsInChildren <Rigidbody>()
join b in namesAndBones
on x.name equals b.Name
select(rb : x, b.Entity)
;
foreach (var(rb, ent) in qRbAndBone)
{
addDynamicComponentData_ByRigidbody_(ent, rb, posturePrefab);
}
//return new NativeArray<Entity>(0,Allocator.Temp);
// ジョイントの生成。両端のオブジェクトに相当するエンティティを特定する。
// ・ジョイントはエンティティとして生成する。
// （コライダと同じエンティティに着けても動作したが、サンプルではこうしている）
// （また、ラグドールジョイントはなぜか２つジョイントを返してくるので、同じエンティティには付けられない）
var qJoint =
from j in this.GetComponentsInChildren <UnityEngine.Joint>()
//.Do( x=>Debug.Log(x.name))
join a in namesAndBones
on j.name equals a.Name
join b in namesAndBones
on j.connectedBody.name equals b.Name
let jointData = createJointBlob_(j)
//select (a, b, j, jointData)
select addJointComponentData_(a.Entity, jointData, a.Entity, b.Entity, j.enableCollision)
;
return(qJoint.SelectMany().ToNativeArray(Allocator.Temp));
// 物理材質に着けられた名前から、特定のコライダを持つコンポーネントデータを生成する。
void addQuearyableColliderBlobs_
(Entity ent, UnityEngine.Collider srcCollider, int groupIndex)
{
switch (srcCollider.sharedMaterial.name)
{
//case "overlap cast":汎用だと１つしかもてない、用途ごとにコンポーネントデータを定義しないといけない
//{
// var blob = createBlobCollider_( new[] { srcCollider }, srcCollider.gameObject, groupIndex );
// em.AddComponentData( ent, new GroundHitColliderData { Collider = blob } );
// break;
//}
case "overlap ground ray":
{
if (srcCollider is UnityEngine.SphereCollider srcSphere)
{
em.AddComponentData(ent, new GroundHitRayData
{
Start = srcSphere.center,
Ray = new DirectionAndLength {
value = new float4(math.up() * -1, srcSphere.radius)
}, // 向きは暫定
Filter = LegacyColliderProducer.GetFilter(srcSphere, groupIndex),
});
}
break;
}
case "overlap ground sphere":
{
if (srcCollider is UnityEngine.SphereCollider srcSphere)
{
em.AddComponentData(ent, new GroundHitSphereData
{
Center = srcSphere.center,
Distance = srcSphere.radius,
Filter = LegacyColliderProducer.GetFilter(srcSphere, groupIndex),
});
}
break;
}
}
}
BlobAssetReference <Collider> compoundColliderBlobsFromEnumerable_
(IEnumerable <CompoundCollider.ColliderBlobInstance> src)
{
using (var arr = src.ToNativeArray(Allocator.Temp))
return(CompoundCollider.Create(arr));
}
BlobAssetReference <Collider> createBlobCollider_
(IEnumerable <UnityEngine.Collider> srcColliders_, GameObject parent, int groupIndex)
{
return((srcColliders_.Count() > 1 || srcColliders_.First().gameObject != parent)
? queryBlobInstances_(srcColliders_, parent.transform, groupIndex)
.To(compoundColliderBlobsFromEnumerable_)
: createColliderBlob_(srcColliders_.First(), groupIndex)
);
IEnumerable <CompoundCollider.ColliderBlobInstance> queryBlobInstances_
(IEnumerable <UnityEngine.Collider> srcColliders__, Transform tfParent, int groupIndex_)
{
return
(from x in srcColliders__
let tfCollider = x.transform
let rtf = new RigidTransform
{
pos = tfCollider.position - tfParent.position,
rot = tfCollider.rotation * Quaternion.Inverse(tfParent.rotation),
}
select new CompoundCollider.ColliderBlobInstance
{
Collider = createColliderBlob_(x, groupIndex_),
CompoundFromChild = rtf,
});
}
}
BlobAssetReference <Collider> createColliderBlob_(UnityEngine.Collider srcCollider, int groupIndex)
{
switch (srcCollider)
{
case UnityEngine.SphereCollider srcSphere:
return(srcSphere.ProduceColliderBlob(groupIndex));
case UnityEngine.CapsuleCollider srcCapsule:
return(srcCapsule.ProduceColliderBlob(groupIndex));
case UnityEngine.BoxCollider srcBox:
return(srcBox.ProduceColliderBlob(groupIndex));
}
return(BlobAssetReference <Collider> .Null);
}
void addDynamicComponentData_ByRigidbody_(Entity ent, Rigidbody rb, Entity postureEnt)
{
var massProp = em.HasComponent <PhysicsCollider>(ent)
? em.GetComponentData <PhysicsCollider>(ent).MassProperties
: MassProperties.UnitSphere;
var physicsMass = rb.isKinematic
? PhysicsMass.CreateKinematic(massProp)
: PhysicsMass.CreateDynamic(massProp, rb.mass);
// ＸＹ回転拘束だけ特例で設定する
var freez_xy = RigidbodyConstraints.FreezeRotationX | RigidbodyConstraints.FreezeRotationZ;
if (rb.constraints == freez_xy)
{
physicsMass.InverseInertia = new float3(0, 1, 0);
}
//if( !rb.isKinematic )
em.AddComponentData(ent, physicsMass);
// キネマティックの場合は、つけなくても大丈夫みたい（主にジョイントにとって）
// が、いちおうつけておく
//if( !rb.isKinematic )
em.AddComponentData(ent, new PhysicsVelocity());
// ジョイント付けると、質量ゼロにしても速度が必要みたい（ないと荒ぶる）
// もしくは、コライダがあると安定したように見えた
if (rb.isKinematic || !rb.useGravity)
{
em.AddComponentData(ent, new PhysicsGravityFactor {
Value = 0.0f
});
}
// 質量ゼロにしても、なぜか重力の影響を受け続けるのでオフる
//if( !rb.isKinematic )
{
em.AddComponentData(ent, new Bone.InitializeData {
PostureEntity = postureEnt
});
em.SetComponentData(ent, new Translation {
Value = rb.position
});
em.SetComponentData(ent, new Rotation {
Value = rb.rotation
});
}
}
BlobAssetReference <JointData>[] createJointBlob_(UnityEngine.Joint srcJoint)
{
switch (srcJoint)
{
//case UnityEngine.CharacterJoint srcChJoint when srcChJoint.GetComponent<RagdollJointAuthoring>() != null:
//{
// var srcRagdollJoint = srcChJoint.GetComponent<RagdollJointAuthoring>();
// JointData.CreateRagdoll
// (
// srcRagdollJoint.positionAinA,
// srcRagdollJoint.positionBinB,
// srcRagdollJoint.twistAxisInA,
// srcRagdollJoint.twistAxisInB,
// srcRagdollJoint.perpendicularAxisInA,
// srcRagdollJoint.perpendicularAxisInB,
// math.radians( srcRagdollJoint.maxConeAngle ),
// math.radians( srcRagdollJoint.minPerpendicularAngle ),
// math.radians( srcRagdollJoint.maxPerpendicularAngle ),
// math.radians( srcRagdollJoint.minTwistAngle ),
// math.radians( srcRagdollJoint.maxTwistAngle ),
// out var jointData0,
// out var jointData1
// );
// return new[] { jointData0, jointData1 };
//}
case UnityEngine.CharacterJoint srcChJoint2:
{
var blob = JointData.CreateBallAndSocket(srcChJoint2.anchor, srcChJoint2.connectedAnchor);
return(new[] { blob });
}
}
return(new BlobAssetReference <JointData>[] {});
}
//unsafe Entity createJoint_
unsafe Entity[] addJointComponentData_(
Entity jointEntity,
BlobAssetReference <JointData>[] jointDataArray,
Entity entityA, Entity entityB,
bool isEnableCollision = false
)
{
return((from x in jointDataArray select createJointEntity_(x)).ToArray());
Entity createJointEntity_(BlobAssetReference <JointData> jd)
{
var ent = em.CreateEntity(typeof(Prefab), typeof(PhysicsJoint));
em.SetComponentData(ent,
new PhysicsJoint
{
//JointData = jd,
//EntityA = entityA,
//EntityB = entityB,
//EnableCollision = ( isEnableCollision ? 1 : 0 )
}
);
return(ent);
}
}
}
}
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.25f),
BevelRadius = 0.0f
});
quaternion orientationA = quaternion.identity;
bool identityA = true;
if (!identityA)
{
orientationA = quaternion.AxisAngle(new float3(0, 1, 0), (float)math.PI * 3.0f / 2.0f);
}
quaternion orientationB = quaternion.identity;
bool identityB = true;
if (!identityB)
{
orientationB = quaternion.AxisAngle(math.normalize(new float3(1)), (float)math.PI / 4.0f);
}
// Make some joints with fixed position, limited 3D angle
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 = float3.zero;
float3 pivotInWorld = math.transform(GetBodyTransform(body), pivotLocal);
quaternion worldFromLocal = Quaternion.AngleAxis((i - 4.5f) * 20.0f, new float3(0, 0, 1));
BlobAssetReference <JointData> jointData = JointData.Create(
new MTransform(orientationA, pivotLocal),
new MTransform(orientationB, pivotInWorld),
new Constraint[]
{
Constraint.BallAndSocket(),
new Constraint
{
ConstrainedAxes = new bool3(true, true, true),
Type = ConstraintType.Angular,
Min = math.max(i - 5, 0) * 0.1f,
Max = i * 0.1f,
SpringDamping = Constraint.DefaultSpringDamping,
SpringFrequency = Constraint.DefaultSpringFrequency
}
});
CreateJoint(jointData, body, Entity.Null);
}
}
private void UpdateKinect2JointData(JointData joint, int player, ref JointData jointData)
{
jointData.position = coordinateSystem.ConvertLocation (coordinateSystem.ConvertRawKinect2Location(joint.position), RUISDevice.Kinect_2);
jointData.positionConfidence = joint.positionConfidence;
jointData.rotation = coordinateSystem.ConvertRotation (coordinateSystem.ConvertRawKinect2Rotation(joint.rotation), RUISDevice.Kinect_2);
jointData.rotationConfidence = joint.rotationConfidence;
}
public void updateDirParent(JointData _parent)
{
//Update Direction
dir = pos - _parent.pos;
}
public void updateDirChild(JointData _child)
{
//Update Direction
dir = _child.pos - pos;
}
void SetUpJoint(JointData data)
{
SetUpJoint(data, "Skeleton");
}
public HumanPoseData(demo capturePose)
{
keypoints = new JointData(capturePose);
}
public void JointDataCreateRagdollTest()
{
var positionAinA = new float3(0.0f, 1.0f, 2.0f);
var positionBinB = new float3(1.0f, 0.0f, 3.0f);
var twistAxisInA = new float3(1.0f, 0.0f, 0.0f);
var twistAxisInB = new float3(1.0f, 0.0f, 0.0f);
var perpendicularInA = new float3(0.0f, 1.0f, 0.0f);
var perpendicularInB = new float3(0.0f, 1.0f, 0.0f);
var maxConeAngle = 0.8f;
var minPerpendicularAngle = 0.1f;
var maxPerpendicularAngle = 1.1f;
var minTwistAngle = 0.2f;
var maxTwistAngle = 1.2f;
BlobAssetReference <JointData> jointData0;
BlobAssetReference <JointData> jointData1;
JointData.CreateRagdoll(positionAinA, positionBinB, twistAxisInA, twistAxisInB, perpendicularInA, perpendicularInB,
maxConeAngle, minPerpendicularAngle, maxPerpendicularAngle, minTwistAngle, maxTwistAngle,
out jointData0, out jointData1);
var joint0 = jointData0.Value;
Assert.AreEqual(positionAinA, joint0.AFromJoint.Translation);
Assert.AreEqual(positionBinB, joint0.BFromJoint.Translation);
Assert.AreEqual(1, joint0.Version);
Assert.AreEqual(2, joint0.NumConstraints);
var twistConstraint = jointData0.Value.Constraints[0];
Assert.AreEqual(new bool3(true, false, false), twistConstraint.ConstrainedAxes);
Assert.AreEqual(ConstraintType.Angular, twistConstraint.Type);
Assert.AreEqual(0.2f, twistConstraint.Min);
Assert.AreEqual(1.2f, twistConstraint.Max);
Assert.AreEqual(Constraint.DefaultSpringFrequency, twistConstraint.SpringFrequency);
Assert.AreEqual(Constraint.DefaultSpringDamping, twistConstraint.SpringDamping);
var coneConstraint0 = jointData0.Value.Constraints[1];
Assert.AreEqual(new bool3(false, true, true), coneConstraint0.ConstrainedAxes);
Assert.AreEqual(ConstraintType.Angular, coneConstraint0.Type);
Assert.AreEqual(0.0f, coneConstraint0.Min);
Assert.AreEqual(0.8f, coneConstraint0.Max);
Assert.AreEqual(Constraint.DefaultSpringFrequency, coneConstraint0.SpringFrequency);
Assert.AreEqual(Constraint.DefaultSpringDamping, coneConstraint0.SpringDamping);
var joint1 = jointData1.Value;
Assert.AreEqual(positionAinA, joint1.AFromJoint.Translation);
Assert.AreEqual(positionBinB, joint1.BFromJoint.Translation);
Assert.AreEqual(1, joint1.Version);
Assert.AreEqual(2, joint1.NumConstraints);
var coneConstraint1 = jointData0.Value.Constraints[0];
Assert.AreEqual(new bool3(true, false, false), coneConstraint1.ConstrainedAxes);
Assert.AreEqual(ConstraintType.Angular, coneConstraint1.Type);
Assert.AreEqual(0.2f, coneConstraint1.Min);
Assert.AreEqual(1.2f, coneConstraint1.Max);
Assert.AreEqual(Constraint.DefaultSpringFrequency, coneConstraint1.SpringFrequency);
Assert.AreEqual(Constraint.DefaultSpringDamping, coneConstraint1.SpringDamping);
var ballAndSocketConstraint = jointData0.Value.Constraints[1];
Assert.AreEqual(new bool3(false, true, true), ballAndSocketConstraint.ConstrainedAxes);
Assert.AreEqual(ConstraintType.Angular, ballAndSocketConstraint.Type);
Assert.AreEqual(0.0f, ballAndSocketConstraint.Min);
Assert.AreEqual(0.8f, ballAndSocketConstraint.Max);
Assert.AreEqual(Constraint.DefaultSpringFrequency, ballAndSocketConstraint.SpringFrequency);
Assert.AreEqual(Constraint.DefaultSpringDamping, ballAndSocketConstraint.SpringDamping);
}
private void UpdateKinectJointData(OpenNI.SkeletonJoint joint, int player, ref JointData jointData)
{
OpenNI.SkeletonJointTransformation data;
if (!playerManager.GetPlayer(player).GetSkeletonJoint(joint, out data))
{
return;
}
jointData.position = coordinateSystem.ConvertLocation (coordinateSystem.ConvertRawKinectLocation(data.Position.Position), RUISDevice.Kinect_1);
jointData.positionConfidence = data.Position.Confidence;
jointData.rotation = coordinateSystem.ConvertRotation (coordinateSystem.ConvertRawKinectRotation(data.Orientation), RUISDevice.Kinect_1);
jointData.rotationConfidence = data.Orientation.Confidence;
}
/// copy constructor
public JointData(JointData old)
{
this = old;
this.m_orient = (Vector3[])old.m_orient.Clone();
}
void MakeKeyFramesAndJointData()
{
var keyBlobs = animation.keyFrames;
keyframes = new List <List <KeyFrame> >(10);
for (int i = 0; i < 10; i++)
{
keyframes.Add(new List <KeyFrame>());
}
jointData = new JointData[numJoints];
List <QJoint> indexedJoints = new List <QJoint>(numJoints);
for (int i = 0; i < numJoints; i++)
{
indexedJoints.Add(null);
}
foreach (KeyValuePair <string, KeyBlob> kv in keyBlobs)
{
QJoint curJoint = FindJointByName(kv.Key);
int curIdx = curJoint.index;
indexedJoints[curIdx] = curJoint;
}
for (int i = 0; i < numJoints; i++)
{
QJoint joint = indexedJoints[i];
KeyBlob blob = keyBlobs[joint.name];
int[] jointDataArgs = new int[20];
for (int j = 0; j < 10; j++)
{
string attribName = "m_LocalPosition.x";
switch (j)
{
case (1):
attribName = "m_LocalPosition.y";
break;
case (2):
attribName = "m_LocalPosition.z";
break;
case (3):
attribName = "m_LocalRotation.x";
break;
case (4):
attribName = "m_LocalRotation.y";
break;
case (5):
attribName = "m_LocalRotation.z";
break;
case (6):
attribName = "m_LocalRotation.w";
break;
case (7):
attribName = "m_LocalScale.x";
break;
case (8):
attribName = "m_LocalScale.y";
break;
case (9):
attribName = "m_LocalScale.z";
break;
}
SortedList <float, ScalarFrame> frames = blob.keyedAttributes[attribName].values;
jointDataArgs[j * 2] = keyframes[j].Count;
jointDataArgs[j * 2 + 1] = frames.Count;
for (int k = 0; k < frames.Count; k++)
{
float time = frames.Keys[k];
ScalarFrame frame = frames.Values[k];
KeyFrame newFrame = new KeyFrame(time, frame.value, frame.inTangent, frame.outTangent);
keyframes[j].Add(newFrame);
}
}
int numChildren = NumChildren(i);
Matrix4x4 bindPose = bindTransforms[i];//.inverse;
jointData[i] = new JointData(jointDataArgs, numChildren, bindPose);
}
}
public static extern void SetJointData(int id, JointData data, int parentID);
/// Note: For a perfect save/restore in ODE, the "warm starting"
/// data should be stored in JointData. However, there is
/// currently no easy way to get this data.
public override void Init(JointData data)
{
if (initCalled)
{
// If this Joint has already been initialized, destroy the ODE
// Joint ID.
jointID.Destroy();
if (aMotorID != null)
{
aMotorID.Destroy();
}
// Reset event handler.
jointBreakEventHandler = null;
}
base.Init(data);
initCalled = true;
switch (data.Type)
{
case JointType.Hinge:
// Create an ODE hinge Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateHinge(worldID, IntPtr.Zero));
numAxes = 1;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = false;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Universal:
// Create an ODE universal Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateUniversal(worldID, IntPtr.Zero));
numAxes = 2;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = true;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor2,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Ball:
// Create an ODE ball Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateBall(worldID, IntPtr.Zero));
numAxes = 3;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = true;
axisRotational[2] = true;
// ODE ball Joints need this special "angular motor" to
// restrain their movement e.g. when limits are used.
aMotorID = new dJointID(Tao.Ode.Ode.dJointCreateAMotor(worldID, IntPtr.Zero));
Tao.Ode.Ode.dJointSetAMotorMode(aMotorID, (int)Tao.Ode.Ode.dAMotorMode.dAMotorEuler);
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor2,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor3,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Slider:
// Create an ODE slider Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateSlider(worldID, IntPtr.Zero));
numAxes = 1;
// Set the rotational property of each axis.
axisRotational[0] = false;
axisRotational[1] = false;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Wheel:
// Create an ODE hinge2 Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateHinge2(worldID, IntPtr.Zero));
numAxes = 2;
// Set the rotational property of each axis.
axisRotational[0] = true;
axisRotational[1] = true;
axisRotational[2] = false;
// Set the ODE fudge factor for each axis.
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor,
Defaults.Ode.JointFudgeFactor);
SetJointParam((int)Tao.Ode.Ode.dJointParams.dParamFudgeFactor2,
Defaults.Ode.JointFudgeFactor);
break;
case JointType.Fixed:
// Create an ODE fixed Joint.
jointID = new dJointID(Tao.Ode.Ode.dJointCreateFixed(worldID, IntPtr.Zero));
numAxes = 0;
// Set the rotational property of each axis.
axisRotational[0] = false;
axisRotational[1] = false;
axisRotational[2] = false;
break;
default:
throw new PhysicsException("Unknown bug");
//break;
}
// Tell ODE about the JointFeedback struct for this Joint.
jointFeedback = new Tao.Ode.Ode.dJointFeedback();
Tao.Ode.Ode.dJointSetFeedback(jointID, ref jointFeedback);
// Setup the Solids.
FilterSolidForStaticness(data.Solid0, data.Solid1);
if (!data.IsBroken)
{
// Attach the Joint to the ODE bodies.
AttachODEBodies(data.Solid0, data.Solid1);
}
// Setup the Joint's anchor.
SetAnchor(data.Anchor);
// Setup the Joint's axes.
SetAxis(0, data.Axis[0]);
SetAxis(1, data.Axis[1]);
SetAxis(2, data.Axis[2]);
// Set the ODE joint's userdata pointer.
if (JointType.Ball == data.Type)
{
Tao.Ode.Ode.dJointSetData(aMotorID, GCHandle.ToIntPtr(GCHandle.Alloc(this)));
}
else
{
Tao.Ode.Ode.dJointSetData(jointID, GCHandle.ToIntPtr(GCHandle.Alloc(this)));
}
}
private static extern pxcmStatus PXCMHandData_IHand_QueryNormalizedJoint(IntPtr instance, JointType jointLabel, [Out] JointData jointData);
/// <summary>
/// Places a new joint at the specified point.
/// </summary>
private void PlaceJoint(Vector3 position)
{
var jointData = new JointData(idCounter++, position, 1f);
joints.Add(Joint.CreateFromData(jointData));
}
internal static pxcmStatus QueryNormalizedJointINT(IntPtr instance, JointType jointLabel, out JointData jointData)
{
jointData = new JointData();
return(PXCMHandData_IHand_QueryNormalizedJoint(instance, jointLabel, jointData));
}
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.25f),
BevelRadius = 0.0f
});
var manager = DefaultWorld.EntityManager;
// Add a dynamic body constrained to the world that will die
// Once the dynamic body is destroyed the joint will be invalid
{
// Create a dynamic body
float3 pivotWorld = new float3(-2f, 0, 0);
Entity body = CreateDynamicBody(pivotWorld, quaternion.identity, collider, float3.zero, float3.zero, 1.0f);
// add timeout on dynamic body after 15 frames.
manager.AddComponentData <EntityKiller>(body, new EntityKiller()
{
TimeToDie = 15
});
// Create the joint
float3 pivotLocal = float3.zero;
BlobAssetReference <JointData> jointData = JointData.CreateBallAndSocket(pivotLocal, pivotWorld);
var jointEntity = CreateJoint(jointData, body, Entity.Null);
// add timeout on joint entity after 30 frames.
manager.AddComponentData <EntityKiller>(jointEntity, new EntityKiller()
{
TimeToDie = 30
});
}
// Add two static bodies constrained together
// The joint is invalid immediately
{
// Create a body
Entity bodyA = CreateStaticBody(new float3(0, 0.0f, 0), quaternion.identity, collider);
Entity bodyB = CreateStaticBody(new float3(0, 1.0f, 0), quaternion.identity, collider);
// Create the joint
float3 pivotLocal = float3.zero;
BlobAssetReference <JointData> jointData = JointData.CreateBallAndSocket(pivotLocal, pivotLocal);
var jointEntity = CreateJoint(jointData, bodyA, bodyB);
// add timeout on joint entity after 15 frames.
manager.AddComponentData <EntityKiller>(jointEntity, new EntityKiller()
{
TimeToDie = 15
});
}
}
void SetUpJoint(JointData data)
{
SetUpJoint(data, "Ragdoll");
}
/**
@brief Return the tracking data of a single normalized-hand joint.
@note This information is available only in full-hand tracking mode, when normalized-skeleton is enabled.
@see PXCHandConfiguration::SetTrackingMode
@see PXCHandConfiguration::EnableNormalizedJoints
@param[in] jointLabel - the ID of the requested joint.
@param[out] jointData - the tracking data of the requested normalized-hand joint.
@return PXCM_STATUS_NO_ERROR - operation succeeded.
@see JointType
@see JointData
*/
public pxcmStatus QueryNormalizedJoint(JointType jointLabel, out JointData jointData)
{
return QueryNormalizedJointINT(instance, jointLabel, out jointData);
}
protected bool IsGesture(KinectPlayer p, JointData startJoint, JointData nowJoint)
{
if (startJoint.TrackingState == JointTrackingState.Tracked &&
nowJoint.TrackingState == JointTrackingState.Tracked &&
startJoint.Z > this.PlayerZDistance &&
nowJoint.Z > this.PlayerZDistance &&
p.Z > this.PlayerZDistance )
{
this.GestureDistance = MovedDistance(startJoint, nowJoint);
if (nowJoint.X < startJoint.X)
{
this.GestureDistance *= -1;
}
if (Math.Abs(this.GestureDistance) > GestureGateDistance)
{
p.PlayerJoints.Clear();
return true;
}
}
return false;
}
// Polls for new skeleton data
public static bool PollBodyData(SensorData sensorData, ref BodyFrameData bodyFrame)
{
bool bNewFrame = false;
// BodyFrameData tempBodyFrame = new BodyFrameData();
// int hr = GetBodyFrameData(ref tempBodyFrame, true, true);
//
//// if((hr == 0) && ((sensorData.colorImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastColorFrameTime)) &&
//// ((sensorData.depthImage == null) || (tempBodyFrame.liRelativeTime >= sensorData.lastDepthFrameTime)))
// {
// if(hr == 0 /**&& (bodyFrame.liRelativeTime > lastFrameTime)*/)
// {
// bodyFrame = tempBodyFrame;
// sensorData.lastBodyFrameTime = bodyFrame.liRelativeTime;
// bNewFrame = true;
// }
// }
if (sensorData.bodyFrameReader != null)
{
var frame = sensorData.bodyFrameReader.AcquireLatestFrame();
if (frame != null)
{
frame.GetAndRefreshBodyData(sensorData.bodyData);
bodyFrame.liRelativeTime = frame.RelativeTime;
frame.Dispose();
frame = null;
for (int i = 0; i < sensorData.bodyCount; i++)
{
Body body = sensorData.bodyData[i];
if (body == null)
{
bodyFrame.bodyData[i].bIsTracked = 0;
continue;
}
bodyFrame.bodyData[i].bIsTracked = (short)(body.IsTracked ? 1 : 0);
if (body.IsTracked)
{
bodyFrame.bodyData[i].liTrackingID = (long)body.TrackingId;
for (int j = 0; j < Constants.JointCount; j++)
{
Windows.Kinect.Joint joint = body.Joints[(JointType)j];
JointData jointData = bodyFrame.bodyData[i].joint[j];
jointData.jointType = (JointType)j;
jointData.trackingState = joint.TrackingState;
jointData.position = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
jointData.kinectPos = jointData.position;
//Debug.Log(jointData.jointType + " - " + joint.TrackingState + " " + jointData.kinectPos);
if (j == 0)
{
jointData.direction = Vector3.zero;
}
else
{
int jParent = (int)GetParentJoint(jointData.jointType);
if (jointData.trackingState != TrackingState.NotTracked && bodyFrame.bodyData[i].joint[jParent].trackingState != TrackingState.NotTracked)
{
jointData.direction = jointData.kinectPos - bodyFrame.bodyData[i].joint[jParent].kinectPos;
}
}
jointData.orientation = Quaternion.identity;
// Windows.Kinect.Vector4 vQ = body.JointOrientations[jointData.jointType].Orientation;
// Quaternion orientation = new Quaternion(vQ.X, vQ.Y, vQ.Z, vQ.W);
//
// if(j != 0)
// {
// Quaternion parentOri = bodyFrame.bodyData[i].joint[jParent].orientation;
// jointData.orientation = parentOri * orientation;
// }
// else
// {
// jointData.orientation = orientation;
// }
if (j == 0)
{
bodyFrame.bodyData[i].position = jointData.position;
bodyFrame.bodyData[i].orientation = jointData.orientation;
}
bodyFrame.bodyData[i].joint[j] = jointData;
}
bodyFrame.bodyData[i].leftHandState = body.HandLeftState;
bodyFrame.bodyData[i].leftHandConfidence = body.HandLeftConfidence;
bodyFrame.bodyData[i].rightHandState = body.HandRightState;
bodyFrame.bodyData[i].rightHandConfidence = body.HandRightConfidence;
}
}
bNewFrame = true;
}
}
return(bNewFrame);
}
protected float MovedDistance(JointData start, JointData end)
{
float X = start.X - end.X;
float Y = start.Y - end.Y;
float Z = start.Z - end.Z;
return (float)Math.Sqrt(X* X + Y * Y + Z * Z);
}
/// <summary>
/// Modify object command
/// </summary>
/// <param name="objectController">Controller</param>
/// <param name="gameObject">Modifided gameObject</param>
/// <param name="newTransform">New RootGameObject transform</param>
/// <param name="saveTransform">Saved transform to redo action</param>
/// <param name="jointData">Saved joints</param>
public ModifyCommand(ObjectController objectController, GameObject gameObject, TransformDT newTransform = null, TransformDT saveTransform = null, JointData jointData = null, Action <ObjectController> callback = null)
{
//ToDo сохратять так же ObjectId
_saveTransformDt = saveTransform;
_newTransformDt = newTransform;
_gameObject = gameObject;
_objectController = objectController;
_saveJointData = jointData;
_callback = callback;
SaveObject(objectController);
CommandsManager.AddCommand(this);
}
private byte[] processJointAttributes(JointData jointData)
{
List <byte> jointAttributeBytes = new List <byte>();
foreach (byte byteID in BitConverter.GetBytes((uint)0004))
{
jointAttributeBytes.Add(byteID);
}
switch ((int)jointData.Driver)
{
case 0:
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0000))
{
jointAttributeBytes.Add(IDByte);
}
break;
case 1:
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0001))
{
jointAttributeBytes.Add(IDByte);
}
foreach (byte PWMByte in BitConverter.GetBytes(jointData.PWM))
{
jointAttributeBytes.Add(PWMByte);
}
if (jointData.PWM)
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.PWMport))
{
jointAttributeBytes.Add(Port1Byte);
}
}
else
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.CANport))
{
jointAttributeBytes.Add(Port1Byte);
}
}
foreach (byte fricBoolByte in BitConverter.GetBytes(jointData.HasJointFriction))
{
jointAttributeBytes.Add(fricBoolByte);
}
if (jointData.HasJointFriction)
{
foreach (byte fricByte in BitConverter.GetBytes((UInt16)jointData.Friction))
{
jointAttributeBytes.Add(fricByte);
}
}
foreach (byte driveBoolByte in BitConverter.GetBytes(jointData.DriveWheel))
{
jointAttributeBytes.Add(driveBoolByte);
}
foreach (byte driveByte in BitConverter.GetBytes((UInt16)jointData.Driver))
{
jointAttributeBytes.Add(driveByte);
}
foreach (byte inGearByte in BitConverter.GetBytes((float)jointData.InputGear))
{
jointAttributeBytes.Add(inGearByte);
}
foreach (byte outGearByte in BitConverter.GetBytes((float)jointData.OutputGear))
{
jointAttributeBytes.Add(outGearByte);
}
break;
case 2:
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0002))
{
jointAttributeBytes.Add(IDByte);
}
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.CANport))
{
jointAttributeBytes.Add(Port1Byte);
}
foreach (byte fricBoolByte in BitConverter.GetBytes(jointData.HasJointFriction))
{
jointAttributeBytes.Add(fricBoolByte);
}
if (jointData.HasJointFriction)
{
foreach (byte fricByte in BitConverter.GetBytes((UInt16)jointData.Friction))
{
jointAttributeBytes.Add(fricByte);
}
}
break;
case 3:
foreach (byte Port1 in BitConverter.GetBytes((float)jointData.SolenoidPortA))
{
jointAttributeBytes.Add(Port1);
}
foreach (byte Port2 in BitConverter.GetBytes((float)jointData.SolenoidPortB))
{
jointAttributeBytes.Add(Port2);
}
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0003))
{
jointAttributeBytes.Add(IDByte);
}
if (jointData.HasJointFriction)
{
foreach (byte fricByte in BitConverter.GetBytes((UInt16)jointData.Friction))
{
jointAttributeBytes.Add(fricByte);
}
}
break;
case 4:
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0004))
{
jointAttributeBytes.Add(IDByte);
}
if (jointData.HasJointFriction)
{
foreach (byte fricByte in BitConverter.GetBytes((UInt16)jointData.Friction))
{
jointAttributeBytes.Add(fricByte);
}
}
break;
case 5:
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0005))
{
jointAttributeBytes.Add(IDByte);
}
foreach (byte PWMByte in BitConverter.GetBytes(jointData.PWM))
{
jointAttributeBytes.Add(PWMByte);
}
if (jointData.PWM)
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.PWMport))
{
jointAttributeBytes.Add(Port1Byte);
}
}
else
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.CANport))
{
jointAttributeBytes.Add(Port1Byte);
}
}
if (jointData.HasJointFriction)
{
foreach (byte fricByte in BitConverter.GetBytes((UInt16)jointData.Friction))
{
jointAttributeBytes.Add(fricByte);
}
}
break;
case 6:
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0006))
{
jointAttributeBytes.Add(IDByte);
}
foreach (byte PWMByte in BitConverter.GetBytes(jointData.PWM))
{
jointAttributeBytes.Add(PWMByte);
}
if (jointData.PWM)
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.PWMport))
{
jointAttributeBytes.Add(Port1Byte);
}
foreach (byte Port2Byte in BitConverter.GetBytes(jointData.PWMport2))
{
jointAttributeBytes.Add(Port2Byte);
}
}
else
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.CANport))
{
jointAttributeBytes.Add(Port1Byte);
}
foreach (byte Port2Byte in BitConverter.GetBytes(jointData.CANport2))
{
jointAttributeBytes.Add(Port2Byte);
}
}
if (jointData.HasJointFriction)
{
foreach (byte fricByte in BitConverter.GetBytes((UInt16)jointData.Friction))
{
jointAttributeBytes.Add(fricByte);
}
}
foreach (byte driveBoolByte in BitConverter.GetBytes(jointData.DriveWheel))
{
jointAttributeBytes.Add(driveBoolByte);
}
foreach (byte driveByte in BitConverter.GetBytes((UInt16)jointData.Driver))
{
jointAttributeBytes.Add(driveByte);
}
foreach (byte inGearByte in BitConverter.GetBytes((float)jointData.InputGear))
{
jointAttributeBytes.Add(inGearByte);
}
foreach (byte outGearByte in BitConverter.GetBytes((float)jointData.OutputGear))
{
jointAttributeBytes.Add(outGearByte);
}
break;
case 7:
foreach (byte IDByte in BitConverter.GetBytes((UInt16)0007))
{
jointAttributeBytes.Add(IDByte);
}
foreach (byte PWMByte in BitConverter.GetBytes(jointData.PWM))
{
jointAttributeBytes.Add(PWMByte);
}
if (jointData.PWM)
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.PWMport))
{
jointAttributeBytes.Add(Port1Byte);
}
}
else
{
foreach (byte Port1Byte in BitConverter.GetBytes(jointData.CANport))
{
jointAttributeBytes.Add(Port1Byte);
}
}
if (jointData.HasJointFriction)
{
foreach (byte fricByte in BitConverter.GetBytes((UInt16)jointData.Friction))
{
jointAttributeBytes.Add(fricByte);
}
}
foreach (byte brakeBoolByte in BitConverter.GetBytes(jointData.HasBrake))
{
jointAttributeBytes.Add(brakeBoolByte);
}
if (jointData.HasBrake)
{
foreach (byte brake1Byte in BitConverter.GetBytes((float)jointData.BrakePortA))
{
jointAttributeBytes.Add(brake1Byte);
}
foreach (byte brake2Byte in BitConverter.GetBytes((float)jointData.BrakePortB))
{
jointAttributeBytes.Add(brake2Byte);
}
}
foreach (byte inGearByte in BitConverter.GetBytes((float)jointData.InputGear))
{
jointAttributeBytes.Add(inGearByte);
}
foreach (byte outGearByte in BitConverter.GetBytes((float)jointData.OutputGear))
{
jointAttributeBytes.Add(outGearByte);
}
break;
}
return(jointAttributeBytes.ToArray());
}
// HACK for filtering Kinect 2 arm rotations
//void filterJointRotation(ref Quaternion measuredRotation, ref Quaternion previousRotation, ref KalmanFilteredRotation kalmanFilter, float kalmanDeltaTime)
void filterJointRotation(ref Skeleton skeleton, ref JointData joint, int jointID, float kalmanDeltaTime)
{
float updateAngle = 0.01f;
float angleThreshold = 15;
float covarianceMultiplier = 1;
// float rotateSpeed = 30;
// if (jointID < 2) // Shoulders
// {
// joint.rotation = Quaternion.Slerp (skeleton.previousRotation [jointID], joint.rotation, rotateSpeed * kalmanDeltaTime );
// return;
// }
if (jointID == 8 || jointID == 9) // Heuristic for leftHand, rightHand
covarianceMultiplier = 2;
updateAngle = Mathf.Abs(Quaternion.Angle(joint.rotation, skeleton.previousRotation[jointID])); // New measurement vs previous rotation
if (updateAngle < angleThreshold)
{
// joint.rotation = Quaternion.Slerp (skeleton.previousRotation [jointID], joint.rotation, rotateSpeed * kalmanDeltaTime );
skeleton.filterRot[jointID].rotationNoiseCovariance = covarianceMultiplier*skeleton.rotationNoiseCovariance*(0.05f + 0.95f*(angleThreshold-updateAngle)/angleThreshold);
joint.rotation = skeleton.filterRot[jointID].Update(joint.rotation, kalmanDeltaTime);
// if(Time.time < 10)
// Debug.Log(1);
// joint.rotation = skeleton.filterRot[jointID].Update(joint.rotation, kalmanDeltaTime);
// print(Time.time);
}
else
{
skeleton.filterRot[jointID].rotationNoiseCovariance = covarianceMultiplier * 0.05f * skeleton.rotationNoiseCovariance;
joint.rotation = skeleton.filterRot[jointID].Update(joint.rotation, kalmanDeltaTime);
}
skeleton.previousRotation[jointID] = joint.rotation;
}
private byte[] ProcessRotationalJoint(RigidBodyJoint linearJoint, JointData jointDataObject)
{
try {
ArrayList rotationalJointBytes = new ArrayList();
//Adds ID to signify that it's a rotational joint
ushort ID = 0;
byte[] rotationalJointID = BitConverter.GetBytes(ID);
rotationalJointBytes.Add(rotationalJointID);
foreach (AssemblyJoint joint in linearJoint.Joints)
{
//Adds the ID of the parent STL to the output array
STLData parentSTL = new STLData();
STLDictionary.TryGetValue(NameFilter(joint.OccurrenceOne.Name), out parentSTL);
byte[] parentIDBytes = BitConverter.GetBytes((uint)(parentSTL.getID()));
rotationalJointBytes.Add(parentIDBytes);
//Adds the ID of the child STL to the output array
STLData childSTL = new STLData();
STLDictionary.TryGetValue(NameFilter(joint.OccurrenceTwo.Name), out childSTL);
byte[] childIDBytes = BitConverter.GetBytes((uint)(childSTL.getID()));
rotationalJointBytes.Add(childIDBytes);
//Adds the vector parallel to the movement onto the array of bytes
object GeometryOne, GeometryTwo;
NameValueMap nameMap;
linearJoint.GetJointData(out GeometryOne, out GeometryTwo, out nameMap);
Circle vectorCircle = (Circle)GeometryTwo;
byte[] vectorJointData = new byte[12];
BitConverter.GetBytes(vectorCircle.Center.X).CopyTo(vectorJointData, 0);
BitConverter.GetBytes(vectorCircle.Center.Y).CopyTo(vectorJointData, 4);
BitConverter.GetBytes(vectorCircle.Center.Z).CopyTo(vectorJointData, 8);
rotationalJointBytes.Add(vectorJointData);
//Adds the point of connection relative to the parent part
byte[] transJointData = new byte[12];
AssemblyJointDefinition jointData = joint.Definition;
BitConverter.GetBytes(jointData.OriginTwo.Point.X).CopyTo(transJointData, 0);
BitConverter.GetBytes(jointData.OriginTwo.Point.Y).CopyTo(transJointData, 4);
BitConverter.GetBytes(jointData.OriginTwo.Point.Z).CopyTo(transJointData, 8);
rotationalJointBytes.Add(transJointData);
//Adds the degrees of freedom
byte[] freedomData = new byte[8];
ModelParameter positionData = (ModelParameter)jointData.AngularPosition;
double relataivePosition = positionData._Value;
positionData = (ModelParameter)jointData.AngularPositionEndLimit;
BitConverter.GetBytes((float)(Math.Abs(relataivePosition) - Math.Abs(positionData._Value))).CopyTo(freedomData, 0);
positionData = (ModelParameter)jointData.AngularPositionStartLimit;
BitConverter.GetBytes((float)(Math.Abs(relataivePosition) - Math.Abs(positionData._Value))).CopyTo(freedomData, 4);
rotationalJointBytes.Add(freedomData);
rotationalJointBytes.Add(processJointAttributes(jointDataObject));
}
object[] tempArray = rotationalJointBytes.ToArray();
byte[] returnedArray = new byte[tempArray.Length];
tempArray.CopyTo(returnedArray, 0);
return(returnedArray);
}
catch (Exception e) {
//catches problems
MessageBox.Show(e.Message + "\n\n\n" + e.StackTrace);
return(null);
}
}
/*
* Kinect 2 functions
*/
private void UpdateKinect2RootData(JointData torso, int player)
{
Vector3 newRootPosition = coordinateSystem.ConvertLocation (coordinateSystem.ConvertRawKinect2Location(torso.position), RUISDevice.Kinect_2);
skeletons [kinect2SensorID, player].root.position = newRootPosition;
skeletons [kinect2SensorID, player].root.positionConfidence = torso.positionConfidence;
skeletons [kinect2SensorID, player].root.rotation = coordinateSystem.ConvertRotation (coordinateSystem.ConvertRawKinect2Rotation(torso.rotation), RUISDevice.Kinect_2);
skeletons [kinect2SensorID, player].root.rotationConfidence = torso.rotationConfidence;
}
/**
* @brief Return the tracking data of a single normalized-hand joint.
* @note This information is available only in full-hand tracking mode, when normalized-skeleton is enabled.
* @see PXCHandConfiguration::SetTrackingMode
* @see PXCHandConfiguration::EnableNormalizedJoints
*
* @param[in] jointLabel - the ID of the requested joint.
* @param[out] jointData - the tracking data of the requested normalized-hand joint.
*
* @return PXCM_STATUS_NO_ERROR - operation succeeded.
*
* @see JointType
* @see JointData
*/
public pxcmStatus QueryNormalizedJoint(JointType jointLabel, out JointData jointData)
{
return(QueryNormalizedJointINT(instance, jointLabel, out jointData));
}
// TODO: Add method that return joints by ID, assign proper jointID in the first line: ... = new JointData(Joint.None);
public JointData GetKinect2JointData(Kinect.Joint jointPosition, Kinect.JointOrientation jointRotation)
{
JointData jointData = new JointData(Joint.None); // Temporary variable used to pass values, jointID can be none
jointData.rotation = new Quaternion(jointRotation.Orientation.X,jointRotation.Orientation.Y,jointRotation.Orientation.Z,jointRotation.Orientation.W);
jointData.position = new Vector3(jointPosition.Position.X, jointPosition.Position.Y, jointPosition.Position.Z);
if(jointPosition.TrackingState == Kinect.TrackingState.Tracked) {
jointData.positionConfidence = 1.0f;
jointData.rotationConfidence = 1.0f;
}
else if(jointPosition.TrackingState == Kinect.TrackingState.Inferred) {
jointData.positionConfidence = 0.5f;
jointData.rotationConfidence = 0.5f;
}
else if(jointPosition.TrackingState == Kinect.TrackingState.NotTracked) {
jointData.positionConfidence = 0.0f;
jointData.rotationConfidence = 0.0f;
}
else {
jointData.positionConfidence = 0.0f;
jointData.rotationConfidence = 0.0f;
}
jointData.TrackingState = jointPosition.TrackingState;
return jointData;
}
private void CreatePlayer()
{
// Instantiate a prefab entity
var prefab = GameObjectConversionUtility.ConvertGameObjectHierarchy(playerPrefab, GameObjectConversionSettings.FromWorld(World.DefaultGameObjectInjectionWorld, assetStore));
var player = entityManager.Instantiate(prefab);
entityManager.SetName(player, "Player");
// Trying to create a limited joint to limit rotation along certain axes
// Creating Constraints
var angularConstraint = new Constraint {
ConstrainedAxes = new bool3(true, false, true),
Type = ConstraintType.Angular,
Min = 0,
Max = 0,
SpringFrequency = Constraint.DefaultSpringFrequency,
SpringDamping = Constraint.DefaultSpringDamping
};
Constraint[] constraints = new Constraint[] { angularConstraint };
// Creating JointData
var jointData = JointData.Create(
new Math.MTransform(float3x3.identity, float3.zero),
new Math.MTransform(float3x3.identity, float3.zero),
constraints
);
// Creating a PhysicsJoint this will constrain EntityA and EntityB from rotating or moving
// along certain Axes based on the above defined constraints.
var componentData = new PhysicsJoint {
JointData = jointData,
EntityA = player,
EntityB = Entity.Null,
EnableCollision = 0
};
// Create the ComponentType for the PhysicsJoint
ComponentType[] componentTypes = new ComponentType[1];
componentTypes[0] = typeof(PhysicsJoint);
// Create an entity to hold the PhysicsJoint component
var jointEntity = entityManager.CreateEntity(componentTypes);
// TODO: Check if this is needed
// Setting the name of the jointEntity
entityManager.SetName(jointEntity, "Joint Entity");
// Add the component data to the jointEntity
entityManager.AddComponentData(jointEntity, componentData);
//// Adding buffer to entity
//entityManager.AddBuffer<BufferCollisionDetails>(player);
//entityManager.AddComponent<ColAngle>(player);
//entityManager.AddComponent<BaseSpeed>(player);
//entityManager.SetComponentData(player, new BaseSpeed { Value = 8f });
//entityManager.AddComponent<JumpHeight>(player);
//entityManager.SetComponentData(player, new JumpHeight { Value = 15f });
//var entity = entityManager.Instantiate(player);
}
internal static pxcmStatus QueryNormalizedJointINT(IntPtr instance, JointType jointLabel, out JointData jointData)
{
jointData = new JointData();
return PXCMHandData_IHand_QueryNormalizedJoint(instance, jointLabel, jointData);
}
private void CreateRagdoll(float3 positionOffset, quaternion rotationOffset, int ragdollIndex = 1, bool internalCollisions = false)
{
NativeList <Entity> 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(float3.zero, 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.Temp);
for (int i = 0; i < torsoMesh.vertices.Length; i++)
{
points[i] = torsoMesh.vertices[i];
}
BlobAssetReference <Unity.Physics.Collider> collider = ConvexCollider.Create(points, 0.01f);
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;
float minPerpendicularAngle = 0.0f; // unlimited
float maxPerpendicularAngle = (float)math.PI; // unlimited
float twistAngle = (float)math.PI / 3.0f;
BlobAssetReference <JointData> ragdoll0, ragdoll1;
JointData.CreateRagdoll(pivotHead, pivotBody, axis, axis, perpendicular, perpendicular,
coneAngle, minPerpendicularAngle, maxPerpendicularAngle, -twistAngle, 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 float3(-armLength / 2, 0, 0), new float3(armLength / 2, 0, 0), armRadius,
armUpperFilter);
BlobAssetReference <Unity.Physics.Collider> foreArmCollider = Unity.Physics.CapsuleCollider.Create(new float3(-armLength / 2, 0, 0), new float3(armLength / 2, 0, 0), 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 float3(-handLength / 2, 0, 0), new float3(handLength / 2, 0, 0), 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;
float minPerpendicularAngle = 0.0f;
float maxPerpendicularAngle = (float)math.PI / 2.0f;
float twistAngle = (float)math.PI / 4.0f;
BlobAssetReference <JointData> ragdoll0, ragdoll1;
JointData.CreateRagdoll(pivotArm, pivotBody, axis, axis, perpendicular, perpendicular,
coneAngle, minPerpendicularAngle, maxPerpendicularAngle, -twistAngle, 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);
float minAngle = 0.0f;
float maxAngle = 3.0f;
BlobAssetReference <JointData> hinge = JointData.CreateLimitedHinge(pivotFore, pivotUpper, axis, axis, perpendicular, perpendicular, minAngle, maxAngle);
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);
float minAngle = -0.3f;
float maxAngle = 0.6f;
BlobAssetReference <JointData> hinge = JointData.CreateLimitedHinge(pivotHand, pivotFore, axis, axis, perpendicular, perpendicular, minAngle, maxAngle);
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 float3(-pelvisLength / 2.0f, 0, 0), new float3(pelvisLength / 2.0f, 0, 0), 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;
float minPerpendicularAngle = -0.1f;
float maxPerpendicularAngle = (float)math.PI;
float twistAngle = 0.1f;
BlobAssetReference <JointData> ragdoll0, ragdoll1;
JointData.CreateRagdoll(pivotPelvis, pivotTorso, axis, axis, perpendicular, perpendicular,
coneAngle, minPerpendicularAngle, maxPerpendicularAngle, -twistAngle, 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 float3(0, -thighLength / 2, 0), new float3(0, thighLength / 2, 0), 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 float3(0, -calfLength / 2, 0), new float3(0, calfLength / 2, 0), 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 float3(0, 0, 0), new float3(0, 0, footLength), 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;
float minPerpendicularAngle = 0.0f;
float maxPerpendicularAngle = 0.2f + (float)math.PI / 2.0f;
float twistAngle = 0.2f;
BlobAssetReference <JointData> ragdoll0, ragdoll1;
JointData.CreateRagdoll(pivotThigh, pivotBody, axis, axis, perpendicular, perpendicular,
coneAngle, minPerpendicularAngle, maxPerpendicularAngle, -twistAngle, 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);
float minAngle = -1.2f;
float maxAngle = 0.0f;
BlobAssetReference <JointData> hinge = JointData.CreateLimitedHinge(pivotCalf, pivotThigh, axis, axis, perpendicular, perpendicular, minAngle, maxAngle);
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);
float minAngle = -0.4f;
float maxAngle = 0.1f;
BlobAssetReference <JointData> hinge = JointData.CreateLimitedHinge(pivotFoot, pivotCalf, axis, axis, perpendicular, perpendicular, minAngle, maxAngle);
CreateJoint(hinge, foot, calf);
}
}
}
// 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);
}
}
}
