예제 #1
0
        public Ragdoll(DynamicsWorld ownerWorld, Vector3 positionOffset)
        {
            this.ownerWorld = ownerWorld;

            // Setup the geometry
            shapes[(int)BodyPart.Pelvis] = new CapsuleShape(0.15f, 0.20f);
            shapes[(int)BodyPart.Spine] = new CapsuleShape(0.15f, 0.28f);
            shapes[(int)BodyPart.Head] = new CapsuleShape(0.10f, 0.05f);
            shapes[(int)BodyPart.LeftUpperLeg] = new CapsuleShape(0.05f, 0.45f);
            shapes[(int)BodyPart.LeftLowerLeg] = new CapsuleShape(0.04f, 0.37f);
            shapes[(int)BodyPart.RightUpperLeg] = new CapsuleShape(0.05f, 0.45f);
            shapes[(int)BodyPart.RightLowerLeg] = new CapsuleShape(0.04f, 0.37f);
            shapes[(int)BodyPart.LeftUpperArm] = new CapsuleShape(0.04f, 0.33f);
            shapes[(int)BodyPart.LeftLowerArm] = new CapsuleShape(0.03f, 0.25f);
            shapes[(int)BodyPart.RightUpperArm] = new CapsuleShape(0.04f, 0.33f);
            shapes[(int)BodyPart.RightLowerArm] = new CapsuleShape(0.03f, 0.25f);

            Matrix offset = Matrix.Translation(positionOffset);
            Matrix transform;
            transform = offset * Matrix.Translation(0, 1, 0);
            bodies[(int)BodyPart.Pelvis] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Pelvis]);

            transform = offset * Matrix.Translation(0, 1.2f, 0);
            bodies[(int)BodyPart.Spine] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Spine]);

            transform = offset * Matrix.Translation(0, 1.6f, 0);
            bodies[(int)BodyPart.Head] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.Head]);

            transform = offset * Matrix.Translation(-0.18f, 0.6f, 0);
            bodies[(int)BodyPart.LeftUpperLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperLeg]);

            transform = offset * Matrix.Translation(-0.18f, 0.2f, 0);
            bodies[(int)BodyPart.LeftLowerLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerLeg]);

            transform = offset * Matrix.Translation(0.18f, 0.65f, 0);
            bodies[(int)BodyPart.RightUpperLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperLeg]);

            transform = offset * Matrix.Translation(0.18f, 0.2f, 0);
            bodies[(int)BodyPart.RightLowerLeg] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightLowerLeg]);

            transform = Matrix.RotationZ(PI_2) * offset * Matrix.Translation(-0.35f, 1.45f, 0);
            bodies[(int)BodyPart.LeftUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperArm]);

            transform = Matrix.RotationZ(PI_2) * offset * Matrix.Translation(-0.7f, 1.45f, 0);
            bodies[(int)BodyPart.LeftLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerArm]);

            transform = Matrix.RotationZ(-PI_2) * offset * Matrix.Translation(0.35f, 1.45f, 0);
            bodies[(int)BodyPart.RightUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperArm]);

            transform = Matrix.RotationZ(-PI_2) * offset * Matrix.Translation(0.7f, 1.45f, 0);
            bodies[(int)BodyPart.RightLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightLowerArm]);

            // Setup some damping on the m_bodies
            foreach (RigidBody body in bodies)
            {
                body.SetDamping(0.05f, 0.85f);
                body.DeactivationTime = 0.8f;
                body.SetSleepingThresholds(1.6f, 2.5f);
            }

            // Now setup the constraints
            HingeConstraint hingeC;
            ConeTwistConstraint coneC;

            Matrix localA, localB;

            localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.15f, 0);
            localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.15f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.Spine], localA, localB);
            hingeC.SetLimit(-PI_4, PI_2);
            joints[(int)Joint.PelvisSpine] = hingeC;
            hingeC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.PelvisSpine], true);

            localA = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, 0.30f, 0);
            localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.14f, 0);
            coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.Head], localA, localB);
            coneC.SetLimit(PI_4, PI_4, PI_2);
            joints[(int)Joint.SpineHead] = coneC;
            coneC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.SpineHead], true);

            localA = Matrix.RotationYawPitchRoll(0, 0, -PI_4 * 5) * Matrix.Translation(-0.18f, -0.18f, 0);
            localB = Matrix.RotationYawPitchRoll(0, 0, -PI_4 * 5) * Matrix.Translation(0, 0.225f, 0);
            coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.LeftUpperLeg], localA, localB);
            coneC.SetLimit(PI_4, PI_4, 0);
            joints[(int)Joint.LeftHip] = coneC;
            coneC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.LeftHip], true);

            localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.225f, 0);
            localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.185f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperLeg], bodies[(int)BodyPart.LeftLowerLeg], localA, localB);
            hingeC.SetLimit(0, PI_2);
            joints[(int)Joint.LeftKnee] = hingeC;
            hingeC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.LeftKnee], true);

            localA = Matrix.RotationYawPitchRoll(0, 0, PI_4) * Matrix.Translation(0.18f, -0.10f, 0);
            localB = Matrix.RotationYawPitchRoll(0, 0, PI_4) * Matrix.Translation(0, 0.225f, 0);
            coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.RightUpperLeg], localA, localB);
            coneC.SetLimit(PI_4, PI_4, 0);
            joints[(int)Joint.RightHip] = coneC;
            coneC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.RightHip], true);

            localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.225f, 0);
            localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.185f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperLeg], bodies[(int)BodyPart.RightLowerLeg], localA, localB);
            hingeC.SetLimit(0, PI_2);
            joints[(int)Joint.RightKnee] = hingeC;
            hingeC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.RightKnee], true);

            localA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI) * Matrix.Translation(-0.2f, 0.15f, 0);
            localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.18f, 0);
            coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.LeftUpperArm], localA, localB);
            coneC.SetLimit(PI_2, PI_2, 0);
            joints[(int)Joint.LeftShoulder] = coneC;
            coneC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.LeftShoulder], true);

            localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.18f, 0);
            localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.14f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperArm], bodies[(int)BodyPart.LeftLowerArm], localA, localB);
            hingeC.SetLimit(0, PI_2);
            joints[(int)Joint.LeftElbow] = hingeC;
            hingeC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.LeftElbow], true);

            localA = Matrix.RotationYawPitchRoll(0, 0, 0) * Matrix.Translation(0.2f, 0.15f, 0);
            localB = Matrix.RotationYawPitchRoll(0, 0, PI_2) * Matrix.Translation(0, -0.18f, 0);
            coneC = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.RightUpperArm], localA, localB);
            coneC.SetLimit(PI_2, PI_2, 0);
            joints[(int)Joint.RightShoulder] = coneC;
            coneC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.RightShoulder], true);

            localA = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, 0.18f, 0);
            localB = Matrix.RotationYawPitchRoll(PI_2, 0, 0) * Matrix.Translation(0, -0.14f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperArm], bodies[(int)BodyPart.RightLowerArm], localA, localB);
            //hingeC.SetLimit(-PI_2, 0);
            hingeC.SetLimit(0, PI_2);
            joints[(int)Joint.RightElbow] = hingeC;
            hingeC.DebugDrawSize = ConstraintDebugSize;

            ownerWorld.AddConstraint(joints[(int)Joint.RightElbow], true);
        }
예제 #2
0
            public TestRig(DynamicsWorld ownerWorld, Vector3 positionOffset, bool bFixed)
            {
                this.ownerWorld = ownerWorld;
                Vector3 vUp = new Vector3(0, 1, 0);

                //
                // Setup geometry
                //
                const float fBodySize = 0.25f;
                const float fLegLength = 0.45f;
                const float fForeLegLength = 0.75f;
                const float PI_2 = (float)(0.5f * Math.PI);
                const float PI_4 = (float)(0.25f * Math.PI);
                const float PI_8 = (float)(0.125f * Math.PI);
                shapes[0] = new CapsuleShape(fBodySize, 0.10f);
                int i;
                for (i = 0; i < NumLegs; i++)
                {
                    shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength);
                    shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength);
                }

                //
                // Setup rigid bodies
                //
                const float fHeight = 0.5f;
                Matrix offset = Matrix.Translation(positionOffset);

                // root
                Vector3 vRoot = new Vector3(0, fHeight, 0);
                Matrix transform = Matrix.Translation(vRoot);
                if (bFixed)
                {
                    bodies[0] = LocalCreateRigidBody(0, transform * offset, shapes[0]);
                }
                else
                {
                    bodies[0] = LocalCreateRigidBody(1, transform * offset, shapes[0]);
                }
                // legs
                for (i = 0; i < NumLegs; i++)
                {
                    float fAngle = (float)(2 * Math.PI * i / NumLegs);
                    float fSin = (float)Math.Sin(fAngle);
                    float fCos = (float)Math.Cos(fAngle);

                    Vector3 vBoneOrigin = new Vector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength));

                    // thigh
                    Vector3 vToBone = (vBoneOrigin - vRoot);
                    vToBone.Normalize();
                    Vector3 vAxis = Vector3.Cross(vToBone, vUp);
                    transform = Matrix.RotationQuaternion(Quaternion.RotationAxis(vAxis, PI_2)) * Matrix.Translation(vBoneOrigin);
                    bodies[1 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[1 + 2 * i]);

                    // shin
                    transform = Matrix.Translation(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength));
                    bodies[2 + 2 * i] = LocalCreateRigidBody(1, transform * offset, shapes[2 + 2 * i]);
                }

                // Setup some damping on the bodies
                for (i = 0; i < BodyPartCount; ++i)
                {
                    bodies[i].SetDamping(0.05f, 0.85f);
                    bodies[i].DeactivationTime = 0.8f;
                    //bodies[i].SetSleepingThresholds(1.6f, 2.5f);
                    bodies[i].SetSleepingThresholds(0.5f, 0.5f);
                }

                //
                // Setup the constraints
                //
                HingeConstraint hingeC;
                //ConeTwistConstraint coneC;

                Matrix localA, localB, localC;

                for (i = 0; i < NumLegs; i++)
                {
                    float fAngle = (float)(2 * Math.PI * i / NumLegs);
                    float fSin = (float)Math.Sin(fAngle);
                    float fCos = (float)Math.Cos(fAngle);

                    // hip joints
                    localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * fBodySize, 0, fSin * fBodySize); // OK
                    localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform);
                    hingeC = new HingeConstraint(bodies[0], bodies[1 + 2 * i], localA, localB);
                    hingeC.SetLimit(-0.75f * PI_4, PI_8);
                    //hingeC.SetLimit(-0.1f, 0.1f);
                    joints[2 * i] = hingeC;
                    ownerWorld.AddConstraint(joints[2 * i], true);

                    // knee joints
                    localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * (fBodySize + fLegLength), 0, fSin * (fBodySize + fLegLength));
                    localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform);
                    localC = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[2 + 2 * i].WorldTransform);
                    hingeC = new HingeConstraint(bodies[1 + 2 * i], bodies[2 + 2 * i], localB, localC);
                    //hingeC.SetLimit(-0.01f, 0.01f);
                    hingeC.SetLimit(-PI_8, 0.2f);
                    joints[1 + 2 * i] = hingeC;
                    ownerWorld.AddConstraint(joints[1 + 2 * i], true);
                }
            }