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); // ジョイントの生成。両端のオブジェクトに相当するエンティティを特定する。 // ・ジョイントはエンティティとして生成する。 // (コライダと同じエンティティに着けても動作したが、サンプルではこうしている) // (また、ラグドールジョイントはなぜか2つジョイントを返してくるので、同じエンティティには付けられない) 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":汎用だと1つしかもてない、用途ごとにコンポーネントデータを定義しないといけない //{ // 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); // XY回転拘束だけ特例で設定する 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); } } }