Exemplo n.º 1
0
 public unsafe static Ragdoll Build(model_t *model)
 {
     return(BuildBipped(IEngineStudio.Mod_Extradata(model), BippedBoneManager.GetBippedBone(model), BoneAccessor.Get(IEngineStudio.Mod_Extradata(model))));
 }
Exemplo n.º 2
0
        //public static Ragdoll Build(string modelName, BuildOption buildOption)
        //{
        //    switch (buildOption)
        //    {
        //        case BuildOption.Default:
        //            var defultOption = PhyConfiguration.GetValue("BuildOption");
        //            Debug.LogLine("Default ragdoll build option is [{0}]", defultOption);
        //            return Build(modelName, (BuildOption)Enum.Parse(buildOption.GetType(),defultOption));
        //        case BuildOption.FromFile:
        //            break;
        //        case BuildOption.Bipped:
        //            {
        //                var info = BippedBone.Get(modelName);
        //                if (info != null)
        //                    return BuildBipped(IEngineStudio.Mod_ForName(modelName,true),info,BoneAccessor.GetCurrent());
        //                Debug.LogLine("BippedBone missing. Model:{0}", modelName);
        //            }
        //            break;
        //        case BuildOption.Spider:
        //            break;
        //        default:
        //            break;
        //    }
        //    //missing inoformation to build for this model
        //    return null;
        //}

        //				 6 ------------- 5
        //			   /               /
        //			 /	 |			 /	 |
        //		   /	 |		   /	 | height/height
        //		 2 ------------- 1		 |
        //		         |		         |
        //		 |		         |
        //		 |		 7 ------|------ 4
        //		 |	   /		 |	   /
        //		 |	 /			 |	 / depth/width
        //		   /			   /
        //		 3 ------------- 0
        //           width/length
        private static unsafe Ragdoll BuildBipped(studiohdr_t *pStudioHeader, BippedBone info, BoneAccessor accessor)
        {
            Debug.LogLine();
            Debug.LogLine("=========================================");
            Debug.LogLine("Bipped build begin...");
            var listKeybone = new List <int>()
            {
                info.Head,
                //info.Spine,
                info.Pelvis,
                info.LeftArm,
                info.LeftElbow,
                //info.LeftHand,
                info.LeftHip,
                info.LeftKnee,
                //info.LeftFoot,
                info.RightArm,
                info.RightElbow,
                //info.RightHand,
                info.RightHip,
                info.RightKnee,
                //info.RightFoot,
            };

            var listNonKeybone = new List <int>();

            for (int i = info.Pelvis; i < accessor.BoneCount; i++)
            {
                if (!listKeybone.Contains(i))
                {
                    listNonKeybone.Add(i);
                }
            }
            var ragdoll = new Ragdoll(pStudioHeader)
            {
                RigidBodies           = new RigidBody[(int)BodyPart.Count],
                Constraints           = new TypedConstraint[9],
                BoneRelativeTransform = new Matrix[accessor.BoneCount],
                EntityId    = 0,
                RagdollData = new RagdollData()
                {
                    KeyBoneIndeces    = listKeybone,
                    NonKeyBoneIndeces = listNonKeybone,
                }
            };

            //===========Rigidbody=================
            Debug.LogLine("Now setup rigidbody...");
            var lArm = accessor.Pos(info.LeftArm);
            var rArm = accessor.Pos(info.RightArm);

            var bodyWidth = (lArm - rArm).Length;

            Debug.LogLine("body width:{0}", bodyWidth);

            //head
            var head        = accessor.Pos(info.Head);
            var headWidth   = bodyWidth / 2.5f; //躯干是头的2.5倍
            var headRaito   = 5.85f / 8.1f;     //头宽高比
            var headHeight  = headWidth / headRaito;
            var headShape   = new SphereShape(headHeight / 2);
            var rigidOrigin = head;

            rigidOrigin.Z += headHeight / 2;
            rigidOrigin.X += headWidth / 4.6f;
            var headLookAt = head;

            headLookAt.X += headWidth / 4.6f;
            var headRigid = Matrix.Translation(rigidOrigin);
            var headBone  = accessor.GetWorldTransform(info.Head);

            ragdoll.RigidBodies[(int)BodyPart.Head]           = BulletHelper.CreateBoneRigidbody(1, ref headBone, ref headRigid, headShape);
            ragdoll.RigidBodies[(int)BodyPart.Head].UserIndex = info.Head;

            //lArm
            var lArmBone = accessor.GetWorldTransform(info.LeftArm);
            var lElbow   = accessor.Pos(info.LeftElbow);

            ragdoll.RigidBodies[(int)BodyPart.LeftArm]           = CreateLimb(ref lArmBone, lElbow, headWidth * 0.55f);
            ragdoll.RigidBodies[(int)BodyPart.LeftArm].UserIndex = info.LeftArm;
            //rArm
            var rArmBone = accessor.GetWorldTransform(info.RightArm);
            var rElbow   = accessor.Pos(info.RightElbow);

            ragdoll.RigidBodies[(int)BodyPart.RightArm]           = CreateLimb(ref rArmBone, rElbow, headWidth * 0.55f);
            ragdoll.RigidBodies[(int)BodyPart.RightArm].UserIndex = info.RightArm;
            //lElbow
            var lElbowBone = accessor.GetWorldTransform(info.LeftElbow);
            var lHand      = accessor.Pos(info.LeftHand);

            ragdoll.RigidBodies[(int)BodyPart.LeftElbow]           = CreateLimb(ref lElbowBone, lHand, headWidth * 0.5f);
            ragdoll.RigidBodies[(int)BodyPart.LeftElbow].UserIndex = info.LeftElbow;
            //rElbow
            var rElbowBone = accessor.GetWorldTransform(info.RightElbow);
            var rHand      = accessor.Pos(info.RightHand);

            ragdoll.RigidBodies[(int)BodyPart.RightElbow]           = CreateLimb(ref rElbowBone, rHand, headWidth * 0.5f);
            ragdoll.RigidBodies[(int)BodyPart.RightElbow].UserIndex = info.RightElbow;
            //lHip
            var lHipBone = accessor.GetWorldTransform(info.LeftHip);
            var lKnee    = accessor.Pos(info.LeftKnee);

            ragdoll.RigidBodies[(int)BodyPart.LeftHip]           = CreateLimb(ref lHipBone, lKnee, headWidth * 0.55f);
            ragdoll.RigidBodies[(int)BodyPart.LeftHip].UserIndex = info.LeftHip;
            //rHip
            var rHipBone = accessor.GetWorldTransform(info.RightHip);
            var rKnee    = accessor.Pos(info.RightKnee);

            ragdoll.RigidBodies[(int)BodyPart.RightHip]           = CreateLimb(ref rHipBone, rKnee, headWidth * 0.55f);
            ragdoll.RigidBodies[(int)BodyPart.RightHip].UserIndex = info.RightHip;
            //lKnee
            var lKneeBone = accessor.GetWorldTransform(info.LeftKnee);
            var lFoot     = accessor.Pos(info.LeftFoot);

            ragdoll.RigidBodies[(int)BodyPart.LeftKnee]           = CreateLimb(ref lKneeBone, lFoot, headWidth * 0.5f);
            ragdoll.RigidBodies[(int)BodyPart.LeftKnee].UserIndex = info.LeftKnee;
            //rKnee
            var rKneeBone = accessor.GetWorldTransform(info.RightKnee);
            var rFoot     = accessor.Pos(info.RightFoot);

            ragdoll.RigidBodies[(int)BodyPart.RightKnee]           = CreateLimb(ref rKneeBone, rFoot, headWidth * 0.5f);
            ragdoll.RigidBodies[(int)BodyPart.RightKnee].UserIndex = info.RightKnee;
            //body
            var pelvis         = accessor.Pos(info.Pelvis);
            var pelvisBone     = accessor.GetWorldTransform(info.Pelvis);
            var bodyHeight     = (head - pelvis).Length;
            var bodyShape      = new CapsuleShape(bodyWidth / 2, bodyHeight - bodyWidth);
            var bodyRigidTrans = BulletMathUtils.CenterOf(pelvis, head).LookAt(in head, in Vector3.UnitY);

            ragdoll.RigidBodies[(int)BodyPart.Pelvis]           = BulletHelper.CreateBoneRigidbody(3, ref pelvisBone, ref bodyRigidTrans, bodyShape);
            ragdoll.RigidBodies[(int)BodyPart.Pelvis].UserIndex = info.Pelvis;

            //==============Constraint=============
            Debug.LogLine("Now setup constraint...");
            var bodys = ragdoll.RigidBodies;

            // head constraint
            {
                //ragdoll.Constraints[0] = CreateJoint(bodys[(int)BodyPart.Head], bodys[(int)BodyPart.Pelvis], head);
                var bodyHead    = bodys[(int)BodyPart.Head];
                var bodyPelvis  = bodys[(int)BodyPart.Pelvis];
                var jointTrans  = Matrix.Translation(head).LookAt(rigidOrigin, Vector3.UnitX);
                var localHead   = jointTrans * bodyHead.WorldTransform.GetInverse();
                var localPelvis = jointTrans * bodyPelvis.WorldTransform.GetInverse();
                ragdoll.Constraints[0] = new ConeTwistConstraint(bodyHead, bodyPelvis, localHead, localPelvis);
                (ragdoll.Constraints[0] as ConeTwistConstraint).SetLimit((float)Math.PI / 6.5f, (float)Math.PI / 6.5f, (float)Math.PI * 0.3333333f);
            }
            // left arm constraint
            {
                //ragdoll.Constraints[1] = CreateJoint(bodys[(int)BodyPart.LeftArm], bodys[(int)BodyPart.Pelvis], lArm);
                var bodyLeftArm = bodys[(int)BodyPart.LeftArm];
                var bodyPelvis  = bodys[(int)BodyPart.Pelvis];

                var jointOrigin    = accessor.Pos(info.LeftArm);
                var jointTransform = Matrix.Translation(jointOrigin).LookAt(jointOrigin + new Vector3(0, -1, 0), Vector3.UnitX);

                var localInLeftArm = jointTransform * bodyLeftArm.WorldTransform.GetInverse();
                var localPelvis    = jointTransform * bodyPelvis.WorldTransform.GetInverse();

                var joint = new ConeTwistConstraint(bodyLeftArm, bodyPelvis, localInLeftArm, localPelvis);
                joint.SetLimit(3.1415926f * 0.6f, 3.1415926f * 0.6f, 0);
                ragdoll.Constraints[1] = joint;
            }
            // right arm constraint
            {
                //ragdoll.Constraints[2] = CreateJoint(bodys[(int)BodyPart.RightArm], bodys[(int)BodyPart.Pelvis], rArm);
                var bodyRightArm = bodys[(int)BodyPart.RightArm];
                var bodyPelvis   = bodys[(int)BodyPart.Pelvis];

                var jointOrigin    = accessor.Pos(info.RightArm);
                var jointTransform = Matrix.Translation(jointOrigin).LookAt(jointOrigin + new Vector3(0, 1, 0), Vector3.UnitX);

                var localInRightArm = jointTransform * bodyRightArm.WorldTransform.GetInverse();
                var localPelvis     = jointTransform * bodyPelvis.WorldTransform.GetInverse();

                var joint = new ConeTwistConstraint(bodyRightArm, bodyPelvis, localInRightArm, localPelvis);
                joint.SetLimit(3.1415926f * 0.6f, 3.1415926f * 0.6f, 0);
                ragdoll.Constraints[2] = joint;
            }
            {// Left elbow
             //ragdoll.Constraints[3] = CreateJoint(bodys[(int)BodyPart.LeftElbow], bodys[(int)BodyPart.LeftArm], lElbow);

                var bodyArm   = bodys[(int)BodyPart.LeftArm];
                var bodyElbow = bodys[(int)BodyPart.LeftElbow];


                var jointTrans = Matrix.Translation(lElbow).LookAt(lElbow + new Vector3(0, 0, -1), Vector3.UnitZ);
                // joint's local transform
                var localInArm   = jointTrans * bodyArm.WorldTransform.GetInverse();
                var localInElbow = jointTrans * bodyElbow.WorldTransform.GetInverse();
                var joint        = new HingeConstraint(bodyArm, bodyElbow, localInArm, localInElbow);
                joint.SetAxis(Vector3.UnitZ);
                joint.SetLimit(-(float)Math.PI * 0.7f, 1);
                ragdoll.Constraints[3] = joint;
            }
            {// right elbow
             //ragdoll.Constraints[4] = CreateJoint(bodys[(int)BodyPart.RightElbow], bodys[(int)BodyPart.RightArm], rElbow);

                var bodyArm   = bodys[(int)BodyPart.RightArm];
                var bodyElbow = bodys[(int)BodyPart.RightElbow];


                var jointTrans = Matrix.Translation(rElbow).LookAt(rElbow + new Vector3(0, 0, -1), Vector3.UnitZ);
                // joint's local transform
                var localInArm   = jointTrans * bodyArm.WorldTransform.GetInverse();
                var localInElbow = jointTrans * bodyElbow.WorldTransform.GetInverse();
                var joint        = new HingeConstraint(bodyArm, bodyElbow, localInArm, localInElbow);
                joint.SetAxis(Vector3.UnitZ);
                joint.SetLimit(-(float)Math.PI * 0.7f, 1);
                ragdoll.Constraints[4] = joint;
            }
            {
                //ragdoll.Constraints[5] = CreateJoint(bodys[(int)BodyPart.LeftHip], bodys[(int)BodyPart.Pelvis], accessor.Pos(info.LeftHip));
                var bodyHipL   = bodys[(int)BodyPart.LeftHip];
                var bodyPelvis = bodys[(int)BodyPart.Pelvis];
                var leglen     = (accessor.Pos(info.LeftHip) - lKnee).Length;
                var x          = (float)Math.Cos(Math.PI / 6) * leglen;
                var z          = (float)Math.Sin(Math.PI / 6) * leglen;
                var posA       = accessor.Pos(info.LeftHip);
                posA.Z += z;
                posA.X += x;
                var posB      = lKnee;
                var posCenter = (posA + posB) / 2;

                var jointTrans  = Matrix.Translation(accessor.Pos(info.LeftHip)).LookAt(posCenter, Vector3.UnitX);
                var localPelvis = jointTrans * bodyPelvis.WorldTransform.GetInverse();
                var localHipL   = jointTrans * bodyHipL.WorldTransform.GetInverse();

                ragdoll.Constraints[5] = new ConeTwistConstraint(bodyHipL, bodyPelvis, localHipL, localPelvis);
                (ragdoll.Constraints[5] as ConeTwistConstraint).SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI / 12);
            }
            {
                //ragdoll.Constraints[6] = CreateJoint(bodys[(int)BodyPart.RightHip], bodys[(int)BodyPart.Pelvis], accessor.Pos(info.RightHip));
                var bodyHipR   = bodys[(int)BodyPart.RightHip];
                var bodyPelvis = bodys[(int)BodyPart.Pelvis];
                var leglen     = (accessor.Pos(info.RightHip) - rKnee).Length;
                var x          = (float)Math.Cos(Math.PI / 6) * leglen;
                var z          = (float)Math.Sin(Math.PI / 6) * leglen;
                var posA       = accessor.Pos(info.RightHip);
                posA.Z += z;
                posA.X += x;
                var posB      = rKnee;
                var posCenter = (posA + posB) / 2;

                var jointTrans  = Matrix.Translation(accessor.Pos(info.RightHip)).LookAt(posCenter, Vector3.UnitX);
                var localPelvis = jointTrans * bodyPelvis.WorldTransform.GetInverse();
                var localHipR   = jointTrans * bodyHipR.WorldTransform.GetInverse();

                ragdoll.Constraints[6] = new ConeTwistConstraint(bodyHipR, bodyPelvis, localHipR, localPelvis);
                (ragdoll.Constraints[6] as ConeTwistConstraint).SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI / 12);
            }
            {     // left knee joint
                { // p2p joint
                 //ragdoll.Constraints[7] = CreateJoint(bodys[(int)BodyPart.LeftKnee], bodys[(int)BodyPart.LeftHip], lKnee);
                }
                {// hinge joint 1
                    var bodyHipL  = bodys[(int)BodyPart.LeftHip];
                    var bodyKneeL = bodys[(int)BodyPart.LeftKnee];


                    var jointTrans = Matrix.Translation(lKnee).LookAt(lKnee + new Vector3(0, 1, 0), Vector3.UnitZ);
                    // joint's local transform in left hip
                    var localInHipL  = jointTrans * bodyHipL.WorldTransform.GetInverse();
                    var localInKneeL = jointTrans * bodyKneeL.WorldTransform.GetInverse();
                    ragdoll.Constraints[7] = new HingeConstraint(bodyHipL, bodyKneeL, localInHipL, localInKneeL);
                    (ragdoll.Constraints[7] as HingeConstraint).SetLimit(-(float)Math.PI * 0.7f, 0);
                }
                {// hinge joint 2
                 //var bodyHipL = bodys[(int)BodyPart.LeftHip];
                 //var bodyKneeL = bodys[(int)BodyPart.LeftKnee];

                    //Vector3 axisInA = new Vector3(1, 0, 0);

                    //Matrix transform = Matrix.Invert(bodyB.WorldTransform) * bodyA.WorldTransform;
                    //Vector3 pivotInB = Vector3.TransformCoordinate(pivotInA, transform);

                    //transform = Matrix.Invert(bodyB.WorldTransform) * bodyB.WorldTransform;
                    //Vector3 axisInB = Vector3.TransformCoordinate(axisInA, transform);
                    //var jointTrans = Matrix.Translation(lKnee).LookAt(lKnee + new Vector3(0, 1, 0), Vector3.UnitZ);
                    //// joint's local transform in left hip
                    //var localInHipL = jointTrans * bodyHipL.WorldTransform.GetInverse();
                    //var localInKneeL = jointTrans * bodyKneeL.WorldTransform.GetInverse();
                    //ragdoll.Constraints[7] = new HingeConstraint(bodyHipL, bodyKneeL, localInHipL, localInKneeL);
                    //(ragdoll.Constraints[7] as HingeConstraint).SetLimit(-(float)Math.PI / 6, (float)Math.PI / 6);
                }
            }
            {
                //ragdoll.Constraints[8] = CreateJoint(bodys[(int)BodyPart.RightKnee], bodys[(int)BodyPart.RightHip], rKnee);

                {// hinge joint 1
                    var bodyHipR  = bodys[(int)BodyPart.RightHip];
                    var bodyKneeR = bodys[(int)BodyPart.RightKnee];


                    var jointTrans = Matrix.Translation(rKnee).LookAt(rKnee + new Vector3(0, 1, 0), Vector3.UnitZ);
                    // joint's local transform in left hip
                    var localInHipR  = jointTrans * bodyHipR.WorldTransform.GetInverse();
                    var localInKneeR = jointTrans * bodyKneeR.WorldTransform.GetInverse();
                    ragdoll.Constraints[8] = new HingeConstraint(bodyHipR, bodyKneeR, localInHipR, localInKneeR);
                    (ragdoll.Constraints[8] as HingeConstraint).SetLimit(-(float)Math.PI * 0.7f, 0);
                }
            }
            foreach (var i in ragdoll.RigidBodies)
            {
                i.SetDamping(0.05f, 0.85f);
                i.DeactivationTime = 0.8f;
                i.SetSleepingThresholds(1.6f, 2.5f);
                i.Friction = 2.2f;//v1.0 is 2
            }
            // damping, friction and restitution document forum
            //https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=8100
            // What is the unit of Damping in Generic Spring Constraint?
            // https://github.com/bulletphysics/bullet3/issues/345
            foreach (var i in ragdoll.Constraints)
            {
                i.DebugDrawSize = 3;
            }
            Debug.LogLine("Bipped build complete.");
            Debug.LogLine("=========================================");
            return(ragdoll);
        }