コード例 #1
0
        //called by Physics World just before constraint is added to world.
        //the current constraint properties are used to rebuild the constraint.
        internal override bool _BuildConstraint()
        {
            BPhysicsWorld world = BPhysicsWorld.Get();

            if (constraintPtr != null)
            {
                if (isInWorld && world != null)
                {
                    isInWorld = false;
                    world.RemoveConstraint(constraintPtr);
                }
            }
            if (IsValid())
            {
                if (constraintType == ConstraintType.constrainToAnotherBody)
                {
                    constraintPtr = new ConeTwistConstraint((RigidBody)targetRigidBodyA.GetCollisionObject(), (RigidBody)targetRigidBodyB.GetCollisionObject(), frameInA.CreateBSMatrix(), frameInB.CreateBSMatrix());
                }
                else
                {
                    constraintPtr = new ConeTwistConstraint((RigidBody)targetRigidBodyA.GetCollisionObject(), frameInA.CreateBSMatrix());
                }
                ConeTwistConstraint sl = (ConeTwistConstraint)constraintPtr;

                sl.SetLimit(swingSpan1, swingSpan2, twistSpan, softness, biasFactor, relaxationFactor);
                constraintPtr.Userobject = this;
                return(true);
            }
            return(false);
        }
コード例 #2
0
        private void CreateConeTwist()
        {
            RigidBody bodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), cubeShape);

            //bodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), cubeShape);
            bodyA.ActivationState = ActivationState.DisableDeactivation;

            RigidBody bodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), cubeShape);
            //bodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), cubeShape);

            Matrix frameInA = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);

            frameInA *= Matrix.Translation(0, -5, 0);
            Matrix frameInB = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);

            frameInB *= Matrix.Translation(0, 5, 0);

            var coneTwist = new ConeTwistConstraint(bodyA, bodyB, frameInA, frameInB);

            //coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
            //coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
            coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
            coneTwist.DebugDrawSize = 5;
            World.AddConstraint(coneTwist, true);
        }
コード例 #3
0
        protected override void OnInitializePhysics()
        {
            SetupEmptyDynamicsWorld();

            CollisionShape groundShape = new BoxShape(50, 1, 50);

            //CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
            CollisionShapes.Add(groundShape);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);

            body.UserObject = "Ground";

            CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));

            CollisionShapes.Add(shape);


            const float THETA = (float)Math.PI / 4.0f;
            float       L_1   = 2 - (float)Math.Tan(THETA);
            float       L_2   = 1 / (float)Math.Cos(THETA);
            float       RATIO = L_2 / L_1;

            RigidBody bodyA;
            RigidBody bodyB;

            CollisionShape cylA = new CylinderShape(0.2f, 0.25f, 0.2f);
            CollisionShape cylB = new CylinderShape(L_1, 0.025f, L_1);
            CompoundShape  cyl0 = new CompoundShape();

            cyl0.AddChildShape(Matrix.Identity, cylA);
            cyl0.AddChildShape(Matrix.Identity, cylB);

            float   mass = 6.28f;
            Vector3 localInertia;

            cyl0.CalculateLocalInertia(mass, out localInertia);
            RigidBodyConstructionInfo ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);

            ci.StartWorldTransform = Matrix.Translation(-8, 1, -8);

            body = new RigidBody(ci); //1,0,cyl0,localInertia);
            World.AddRigidBody(body);
            body.LinearFactor  = Vector3.Zero;
            body.AngularFactor = new Vector3(0, 1, 0);
            bodyA = body;

            cylA = new CylinderShape(0.2f, 0.26f, 0.2f);
            cylB = new CylinderShape(L_2, 0.025f, L_2);
            cyl0 = new CompoundShape();
            cyl0.AddChildShape(Matrix.Identity, cylA);
            cyl0.AddChildShape(Matrix.Identity, cylB);

            mass = 6.28f;
            cyl0.CalculateLocalInertia(mass, out localInertia);
            ci = new RigidBodyConstructionInfo(mass, null, cyl0, localInertia);
            Quaternion orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);

            ci.StartWorldTransform = Matrix.RotationQuaternion(orn) * Matrix.Translation(-10, 2, -8);

            body = new RigidBody(ci);//1,0,cyl0,localInertia);
            body.LinearFactor = Vector3.Zero;
            HingeConstraint hinge = new HingeConstraint(body, Vector3.Zero, new Vector3(0, 1, 0), true);

            World.AddConstraint(hinge);
            bodyB = body;
            body.AngularVelocity = new Vector3(0, 3, 0);

            World.AddRigidBody(body);

            Vector3 axisA = new Vector3(0, 1, 0);
            Vector3 axisB = new Vector3(0, 1, 0);

            orn = Quaternion.RotationAxis(new Vector3(0, 0, 1), -THETA);
            Matrix mat = Matrix.RotationQuaternion(orn);

            axisB = new Vector3(mat.M21, mat.M22, mat.M23);

            GearConstraint gear = new GearConstraint(bodyA, bodyB, axisA, axisB, RATIO);

            World.AddConstraint(gear, true);


            mass = 1.0f;

            RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);

            RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
            //RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
            //body1.ActivationState = ActivationState.DisableDeactivation;
            //body1.SetDamping(0.3f, 0.3f);

            Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
            Vector3 axisInA  = new Vector3(0, 0, 1);

            Vector3 pivotInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
                pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
            }
            else
            {
                pivotInB = pivotInA;
            }

            Vector3 axisInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
                axisInB = Vector3.TransformCoordinate(axisInA, transform);
            }
            else
            {
                axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
            }

#if P2P
            {
                TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
                //TypedConstraint p2p = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
                //TypedConstraint hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);
                World.AddConstraint(p2p);
                p2p.DebugDrawSize = 5;
            }
#else
            {
                hinge = new HingeConstraint(body0, pivotInA, axisInA);

                //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
                //float	targetVelocity = 0.f;
                //float	maxMotorImpulse = 0.01;
                const float targetVelocity  = 1.0f;
                const float maxMotorImpulse = 1.0f;
                hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
                World.AddConstraint(hinge);
                hinge.DebugDrawSize = 5;
            }
#endif

            RigidBody pRbA1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
            //RigidBody pRbA1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
            pRbA1.ActivationState = ActivationState.DisableDeactivation;

            // add dynamic rigid body B1
            RigidBody pRbB1 = LocalCreateRigidBody(mass, Matrix.Translation(-20, 0, 30), shape);
            //RigidBody pRbB1 = LocalCreateRigidBody(0.0f, Matrix.Translation(-20, 0, 30), shape);
            pRbB1.ActivationState = ActivationState.DisableDeactivation;

            // create slider constraint between A1 and B1 and add it to world
            SliderConstraint spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, true);
            //spSlider1 = new SliderConstraint(pRbA1, pRbB1, Matrix.Identity, Matrix.Identity, false);
            spSlider1.LowerLinearLimit = -15.0f;
            spSlider1.UpperLinearLimit = -5.0f;
            spSlider1.LowerLinearLimit = 5.0f;
            spSlider1.UpperLinearLimit = 15.0f;
            spSlider1.LowerLinearLimit = -10.0f;
            spSlider1.UpperLinearLimit = -10.0f;

            spSlider1.LowerAngularLimit = -(float)Math.PI / 3.0f;
            spSlider1.UpperAngularLimit = (float)Math.PI / 3.0f;

            World.AddConstraint(spSlider1, true);
            spSlider1.DebugDrawSize = 5.0f;


            //create a slider, using the generic D6 constraint
            Vector3     sliderWorldPos = new Vector3(0, 10, 0);
            Vector3     sliderAxis     = Vector3.UnitX;
            const float angle          = 0; //SIMD_RADS_PER_DEG * 10.f;
            Matrix      trans          = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);
            d6body0 = LocalCreateRigidBody(mass, trans, shape);
            d6body0.ActivationState = ActivationState.DisableDeactivation;

            RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);
            World.AddRigidBody(fixedBody1);

            Matrix frameInA = Matrix.Translation(0, 5, 0);
            Matrix frameInB = Matrix.Translation(0, 5, 0);

            //bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
            const bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
            spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA)
            {
                LinearLowerLimit = lowerSliderLimit,
                LinearUpperLimit = hiSliderLimit,

                //range should be small, otherwise singularities will 'explode' the constraint
                //AngularLowerLimit = new Vector3(-1.5f,0,0),
                //AngularUpperLimit = new Vector3(1.5f,0,0),
                //AngularLowerLimit = new Vector3(0,0,0),
                //AngularUpperLimit = new Vector3(0,0,0),
                AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0),
                AngularUpperLimit = new Vector3(1.5f, 0, 0)
            };

            //spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
            spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
            spSlider6Dof.TranslationalLimitMotor.MaxMotorForce  = new Vector3(0.1f, 0, 0);

            World.AddConstraint(spSlider6Dof);
            spSlider6Dof.DebugDrawSize = 5;



            // create a door using hinge constraint attached to the world

            CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);
            CollisionShapes.Add(pDoorShape);
            RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);
            pDoorBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
            Vector3 btAxisA  = Vector3.UnitY;                          // pointing upwards, aka Y-axis

            spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);

            //spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
            // test problem values
            //spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);

            //spDoorHinge.SetLimit(1, -1);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
            spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
            //spDoorHinge.SetLimit(0, 0);
            World.AddConstraint(spDoorHinge);
            spDoorHinge.DebugDrawSize = 5;

            RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);



            // create a generic 6DOF constraint

            //RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
            RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);
            //RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);
            //RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(-5, 0, 0);
            frameInB = Matrix.Translation(5, 0, 0);

            Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);
            //Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
            pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
            pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
            //pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
            //pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);

            //pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
            //pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
            //pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);

            //pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);

            pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
            pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);

            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);



            // create a ConeTwist constraint

            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
            //pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
            //pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);

            frameInA  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInA *= Matrix.Translation(0, -5, 0);
            frameInB  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInB *= Matrix.Translation(0, 5, 0);

            coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
            //coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
            //coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
            coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
            World.AddConstraint(coneTwist, true);
            coneTwist.DebugDrawSize = 5;



            // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)

            RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);
            pBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 pivotA = new Vector3(10.0f, 0.0f, 0.0f);
            btAxisA = new Vector3(0.0f, 0.0f, 1.0f);

            HingeConstraint pHinge = new HingeConstraint(pBody, pivotA, btAxisA);
            //pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
            pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
            World.AddConstraint(pHinge);
            pHinge.DebugDrawSize = 5;



            // create a universal joint using generic 6DOF constraint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some (arbitrary) data to build constraint frames
            Vector3 parentAxis = new Vector3(1, 0, 0);
            Vector3 childAxis  = new Vector3(0, 0, 1);
            Vector3 anchor     = new Vector3(20, 2, 0);

            UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
            pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
            pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pUniv, true);
            // draw constraint frames and limits for debugging
            pUniv.DebugDrawSize = 5;

            World.AddConstraint(pGen6DOF, true);
            pGen6DOF.DebugDrawSize = 5;



            // create a generic 6DOF constraint with springs

            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(10, 0, 0);
            frameInB = Matrix.Identity;

            Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true)
            {
                LinearUpperLimit  = new Vector3(5, 0, 0),
                LinearLowerLimit  = new Vector3(-5, 0, 0),
                AngularLowerLimit = new Vector3(0, 0, -1.5f),
                AngularUpperLimit = new Vector3(0, 0, 1.5f),
                DebugDrawSize     = 5
            };
            World.AddConstraint(pGen6DOFSpring, true);

            pGen6DOFSpring.EnableSpring(0, true);
            pGen6DOFSpring.SetStiffness(0, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.5f);
            pGen6DOFSpring.EnableSpring(5, true);
            pGen6DOFSpring.SetStiffness(5, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.3f);
            pGen6DOFSpring.SetEquilibriumPoint();



            // create a Hinge2 joint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            parentAxis = new Vector3(0, 1, 0);
            childAxis  = new Vector3(1, 0, 0);
            anchor     = new Vector3(-20, 0, 0);
            Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);
            pHinge2.SetLowerLimit(-(float)Math.PI / 4);
            pHinge2.SetUpperLimit((float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pHinge2, true);
            // draw constraint frames and limits for debugging
            pHinge2.DebugDrawSize = 5;



            // create a Hinge joint between two dynamic bodies
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB:
            pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            axisA = new Vector3(0, 1, 0);
            axisB = new Vector3(0, 1, 0);
            Vector3 pivotA2 = new Vector3(-5, 0, 0);
            Vector3 pivotB  = new Vector3(5, 0, 0);
            spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA2, pivotB, axisA, axisB);
            spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(spHingeDynAB, true);
            // draw constraint frames and limits for debugging
            spHingeDynAB.DebugDrawSize = 5;
        }
コード例 #4
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.07f, 0.45f);
            shapes[(int)BodyPart.LeftLowerLeg]  = new CapsuleShape(0.05f, 0.37f);
            shapes[(int)BodyPart.RightUpperLeg] = new CapsuleShape(0.07f, 0.45f);
            shapes[(int)BodyPart.RightLowerLeg] = new CapsuleShape(0.05f, 0.37f);
            shapes[(int)BodyPart.LeftUpperArm]  = new CapsuleShape(0.05f, 0.33f);
            shapes[(int)BodyPart.LeftLowerArm]  = new CapsuleShape(0.04f, 0.25f);
            shapes[(int)BodyPart.RightUpperArm] = new CapsuleShape(0.05f, 0.33f);
            shapes[(int)BodyPart.RightLowerArm] = new CapsuleShape(0.04f, 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.RotationX((float)Math.PI / 2) * offset * Matrix.Translation(-0.35f, 1.45f, 0);
            bodies[(int)BodyPart.LeftUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftUpperArm]);

            transform = Matrix.RotationY((float)Math.PI / 2) * offset * Matrix.Translation(-0.7f, 1.45f, 0);
            bodies[(int)BodyPart.LeftLowerArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.LeftLowerArm]);

            transform = Matrix.RotationY(-(float)Math.PI / 2) * offset * Matrix.Translation(0.35f, 1.45f, 0);
            bodies[(int)BodyPart.RightUpperArm] = LocalCreateRigidBody(1, transform, shapes[(int)BodyPart.RightUpperArm]);

            transform = Matrix.RotationY(-(float)Math.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(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.15f, 0);
            localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.15f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.Pelvis], bodies[(int)BodyPart.Spine], localA, localB);
            hingeC.SetLimit(-(float)Math.PI / 4, -(float)Math.PI / 2);
            joints[(int)Joint.PelvisSpine] = hingeC;
            hingeC.DebugDrawSize           = ConstraintDebugSize;

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


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

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


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

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


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

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


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

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


            localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.225f, 0);
            localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.185f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.RightUpperLeg], bodies[(int)BodyPart.RightLowerLeg], localA, localB);
            hingeC.SetLimit(0, (float)Math.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, (float)Math.PI / 2) * Matrix.Translation(0, -0.18f, 0);
            coneC  = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.LeftUpperArm], localA, localB);
            coneC.SetLimit((float)Math.PI / 2, (float)Math.PI / 2, 0);
            joints[(int)Joint.LeftShoulder] = coneC;
            coneC.DebugDrawSize             = ConstraintDebugSize;

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


            localA = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, 0.18f, 0);
            localB = Matrix.RotationYawPitchRoll(0, (float)Math.PI / 2, 0) * Matrix.Translation(0, -0.14f, 0);
            hingeC = new HingeConstraint(bodies[(int)BodyPart.LeftUpperArm], bodies[(int)BodyPart.LeftLowerArm], localA, localB);
            hingeC.SetLimit(0, (float)Math.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, (float)Math.PI / 2) * Matrix.Translation(0, -0.18f, 0);
            coneC  = new ConeTwistConstraint(bodies[(int)BodyPart.Spine], bodies[(int)BodyPart.RightUpperArm], localA, localB);
            coneC.SetLimit((float)Math.PI / 2, (float)Math.PI / 2, 0);
            joints[(int)Joint.RightShoulder] = coneC;
            coneC.DebugDrawSize = ConstraintDebugSize;

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


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

            ownerWorld.AddConstraint(joints[(int)Joint.RightElbow], true);
        }
コード例 #5
0
        private void SetupConstraints()
        {
            HingeConstraint     hinge;
            ConeTwistConstraint cone;

            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);
            hinge  = new HingeConstraint(_bodies[(int)BodyPart.Pelvis], _bodies[(int)BodyPart.Spine], localA, localB);
            hinge.SetLimit(-PI_4, PI_2);
            _joints[(int)Joint.PelvisSpine] = hinge;
            hinge.DebugDrawSize             = ConstraintDebugSize;

            _world.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);
            cone   = new ConeTwistConstraint(_bodies[(int)BodyPart.Spine], _bodies[(int)BodyPart.Head], localA, localB);
            cone.SetLimit(PI_4, PI_4, PI_2);
            _joints[(int)Joint.SpineHead] = cone;
            cone.DebugDrawSize            = ConstraintDebugSize;

            _world.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);
            cone   = new ConeTwistConstraint(_bodies[(int)BodyPart.Pelvis], _bodies[(int)BodyPart.LeftUpperLeg], localA, localB);
            cone.SetLimit(PI_4, PI_4, 0);
            _joints[(int)Joint.LeftHip] = cone;
            cone.DebugDrawSize          = ConstraintDebugSize;

            _world.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);
            hinge  = new HingeConstraint(_bodies[(int)BodyPart.LeftUpperLeg], _bodies[(int)BodyPart.LeftLowerLeg], localA, localB);
            hinge.SetLimit(0, PI_2);
            _joints[(int)Joint.LeftKnee] = hinge;
            hinge.DebugDrawSize          = ConstraintDebugSize;

            _world.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);
            cone   = new ConeTwistConstraint(_bodies[(int)BodyPart.Pelvis], _bodies[(int)BodyPart.RightUpperLeg], localA, localB);
            cone.SetLimit(PI_4, PI_4, 0);
            _joints[(int)Joint.RightHip] = cone;
            cone.DebugDrawSize           = ConstraintDebugSize;

            _world.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);
            hinge  = new HingeConstraint(_bodies[(int)BodyPart.RightUpperLeg], _bodies[(int)BodyPart.RightLowerLeg], localA, localB);
            hinge.SetLimit(0, PI_2);
            _joints[(int)Joint.RightKnee] = hinge;
            hinge.DebugDrawSize           = ConstraintDebugSize;

            _world.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);
            cone   = new ConeTwistConstraint(_bodies[(int)BodyPart.Spine], _bodies[(int)BodyPart.LeftUpperArm], localA, localB);
            cone.SetLimit(PI_2, PI_2, 0);
            _joints[(int)Joint.LeftShoulder] = cone;
            cone.DebugDrawSize = ConstraintDebugSize;

            _world.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);
            hinge  = new HingeConstraint(_bodies[(int)BodyPart.LeftUpperArm], _bodies[(int)BodyPart.LeftLowerArm], localA, localB);
            hinge.SetLimit(0, PI_2);
            _joints[(int)Joint.LeftElbow] = hinge;
            hinge.DebugDrawSize           = ConstraintDebugSize;

            _world.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);
            cone   = new ConeTwistConstraint(_bodies[(int)BodyPart.Spine], _bodies[(int)BodyPart.RightUpperArm], localA, localB);
            cone.SetLimit(PI_2, PI_2, 0);
            _joints[(int)Joint.RightShoulder] = cone;
            cone.DebugDrawSize = ConstraintDebugSize;

            _world.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);
            hinge  = new HingeConstraint(_bodies[(int)BodyPart.RightUpperArm], _bodies[(int)BodyPart.RightLowerArm], localA, localB);
            //hinge.SetLimit(-PI_2, 0);
            hinge.SetLimit(0, PI_2);
            _joints[(int)Joint.RightElbow] = hinge;
            hinge.DebugDrawSize            = ConstraintDebugSize;

            _world.AddConstraint(_joints[(int)Joint.RightElbow], true);
        }
コード例 #6
0
        //called by Physics World just before constraint is added to world.
        //the current constraint properties are used to rebuild the constraint.
        internal override bool _BuildConstraint()
        {
            BPhysicsWorld world = BPhysicsWorld.Get();

            if (m_constraintPtr != null)
            {
                if (m_isInWorld && world != null)
                {
                    m_isInWorld = false;
                    world.RemoveConstraint(m_constraintPtr);
                    return(false);
                }
            }

            BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();

            if (targetRigidBodyA == null)
            {
                Debug.LogError("ConeTwistConstraint needs to be added to a component with a BRigidBody.");
                return(false);
            }
            if (!targetRigidBodyA.isInWorld)
            {
                world.AddRigidBody(targetRigidBodyA);
            }
            if (m_constraintType == ConstraintType.constrainToAnotherBody)
            {
                if (m_otherRigidBody == null)
                {
                    Debug.LogError("Other rigid body was not set");
                    return(false);
                }
                if (!m_otherRigidBody.isInWorld)
                {
                    world.AddRigidBody(m_otherRigidBody);
                }
                BM.Matrix frameInA, frameInOther;
                string    errormsg = "";
                if (CreateFramesA_B(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, out frameInA, out frameInOther, ref errormsg))
                {
                    m_constraintPtr = new ConeTwistConstraint((RigidBody)m_otherRigidBody.GetCollisionObject(), (RigidBody)targetRigidBodyA.GetCollisionObject(), frameInOther, frameInA);
                }
                else
                {
                    Debug.LogError(errormsg);
                    return(false);
                }
            }
            else
            {
                //TODO this is broken
                string    errormsg = "";
                BM.Matrix frameInB = BM.Matrix.Identity;
                if (CreateFrame(m_localConstraintAxisX, m_localConstraintAxisY, m_localConstraintPoint, ref frameInB, ref errormsg))
                {
                    m_constraintPtr = new ConeTwistConstraint((RigidBody)targetRigidBodyA.GetCollisionObject(), frameInB);
                }
                else
                {
                    Debug.LogError(errormsg);
                    return(false);
                }
            }
            ConeTwistConstraint sl = (ConeTwistConstraint)m_constraintPtr;

            sl.SetLimit(m_swingSpan1Radians, m_swingSpan2Radians, m_twistSpanRadians, m_softness, m_biasFactor, m_relaxationFactor);
            m_constraintPtr.Userobject               = this;
            m_constraintPtr.DebugDrawSize            = m_debugDrawSize;
            m_constraintPtr.BreakingImpulseThreshold = m_breakingImpulseThreshold;
            return(true);
        }
コード例 #7
0
            public RagDoll(RagDollDemo ragDollDemo, DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, StreamWriter streamWriter)

            {
                m_ownerWorld  = ownerWorld;
                m_ragDollDemo = ragDollDemo;
                // Setup the geometry
                m_shapes[(int)BODYPART.PELVIS]          = new CapsuleShape(0.15f, 0.20f);
                m_shapes[(int)BODYPART.SPINE]           = new CapsuleShape(0.15f, 0.28f);
                m_shapes[(int)BODYPART.HEAD]            = new CapsuleShape(0.10f, 0.05f);
                m_shapes[(int)BODYPART.LEFT_UPPER_LEG]  = new CapsuleShape(0.07f, 0.45f);
                m_shapes[(int)BODYPART.LEFT_LOWER_LEG]  = new CapsuleShape(0.05f, 0.37f);
                m_shapes[(int)BODYPART.RIGHT_UPPER_LEG] = new CapsuleShape(0.07f, 0.45f);
                m_shapes[(int)BODYPART.RIGHT_LOWER_LEG] = new CapsuleShape(0.05f, 0.37f);
                m_shapes[(int)BODYPART.LEFT_UPPER_ARM]  = new CapsuleShape(0.05f, 0.33f);
                m_shapes[(int)BODYPART.LEFT_LOWER_ARM]  = new CapsuleShape(0.04f, 0.25f);
                m_shapes[(int)BODYPART.RIGHT_UPPER_ARM] = new CapsuleShape(0.05f, 0.33f);
                m_shapes[(int)BODYPART.RIGHT_LOWER_ARM] = new CapsuleShape(0.04f, 0.25f);

                // Setup all the rigid bodies
                IndexedMatrix offset = IndexedMatrix.CreateTranslation(positionOffset);

                IndexedMatrix transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1, 0));

                IndexedMatrix adjusted = offset * transform;

                m_bodies[(int)BODYPART.PELVIS] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.PELVIS], true);
                m_bodies[(int)BODYPART.PELVIS].SetUserPointer("PELVIS");
                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1.2f, 0));
                adjusted  = offset * transform;
                m_bodies[(int)BODYPART.SPINE] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.SPINE], true);
                m_bodies[(int)BODYPART.SPINE].SetUserPointer("SPINE");

                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1.6f, 0));
                adjusted  = offset * transform;
                m_bodies[(int)BODYPART.HEAD] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.HEAD], true);
                m_bodies[(int)BODYPART.HEAD].SetUserPointer("HEAD");

                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.65f, 0));
                adjusted  = offset * transform
                ;
                m_bodies[(int)BODYPART.LEFT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_LEG], true);
                m_bodies[(int)BODYPART.LEFT_UPPER_LEG].SetUserPointer("LEFTUPPERLEG");

                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.2f, 0));
                adjusted  = offset * transform;
                m_bodies[(int)BODYPART.LEFT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_LEG], true);
                m_bodies[(int)BODYPART.LEFT_LOWER_LEG].SetUserPointer("LEFTLOWERLEG");

                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.65f, 0));
                adjusted  = offset * transform;
                m_bodies[(int)BODYPART.RIGHT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_LEG], true);
                m_bodies[(int)BODYPART.RIGHT_UPPER_LEG].SetUserPointer("RIGHTUPPERLEG");

                transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.2f, 0));
                adjusted  = offset * transform;
                m_bodies[(int)BODYPART.RIGHT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_LEG], true);
                m_bodies[(int)BODYPART.RIGHT_LOWER_LEG].SetUserPointer("RIGHTLOWERLEG");

                transform         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                transform._origin = new IndexedVector3(-0.35f, 1.45f, 0);
                adjusted          = offset * transform;
                m_bodies[(int)BODYPART.LEFT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_ARM], true);
                m_bodies[(int)BODYPART.LEFT_UPPER_ARM].SetUserPointer("LEFTUPPERARM");

                transform         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                transform._origin = new IndexedVector3(-0.7f, 1.45f, 0);
                adjusted          = offset * transform;
                m_bodies[(int)BODYPART.LEFT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_ARM], true);
                m_bodies[(int)BODYPART.LEFT_LOWER_ARM].SetUserPointer("LEFTLOWERARM");

                transform         = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
                transform._origin = new IndexedVector3(0.35f, 1.45f, 0);
                adjusted          = offset * transform;
                m_bodies[(int)BODYPART.RIGHT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_ARM], true);
                m_bodies[(int)BODYPART.RIGHT_UPPER_ARM].SetUserPointer("RIGHTUPPERARM");

                transform         = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
                transform._origin = new IndexedVector3(0.7f, 1.45f, 0);
                adjusted          = offset * transform;
                m_bodies[(int)BODYPART.RIGHT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_ARM], true);
                m_bodies[(int)BODYPART.RIGHT_LOWER_ARM].SetUserPointer("RIGHTLOWERARM");

                // Setup some damping on the m_bodies
                for (int i = 0; i < (int)BODYPART.COUNT; ++i)
                {
                    if (m_bodies[i] != null)
                    {
                        //m_bodies[i].SetDamping(0.05f, 0.85f);
                        m_bodies[i].SetDamping(0.5f, 0.85f);
                        m_bodies[i].SetDeactivationTime(0.8f);
                        m_bodies[i].SetSleepingThresholds(1.6f, 2.5f);
                    }
                }

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

                IndexedMatrix localA = IndexedMatrix.Identity;
                IndexedMatrix localB = IndexedMatrix.Identity;

                localA         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localA._origin = new IndexedVector3(0.0f, 0.15f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localB._origin = new IndexedVector3(0.0f, -0.15f, 0.0f);
                hingeC         = new HingeConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.SPINE], ref localA, ref localB);
                hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.PELVIS_SPINE]             = hingeC;
                m_joints[(int)JOINT.PELVIS_SPINE].m_debugName = "PELVIS_SPINE";
                hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.PELVIS_SPINE], true);


                localA         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                localA._origin = new IndexedVector3(0.0f, 0.30f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
                coneC          = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.HEAD], ref localA, ref localB);
                coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.SPINE_HEAD]             = coneC;
                m_joints[(int)JOINT.SPINE_HEAD].m_debugName = "SPINE_HEAD";

                coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.SPINE_HEAD], true);


                localA         = IndexedMatrix.Identity;
                localB         = IndexedMatrix.Identity;
                localA         = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
                localA._origin = new IndexedVector3(-0.18f, -0.10f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
                localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
                coneC          = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.LEFT_UPPER_LEG], ref localA, ref localB);
                coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
                m_joints[(int)JOINT.LEFT_HIP]             = coneC;
                m_joints[(int)JOINT.LEFT_HIP].m_debugName = "LEFT_HIP";

                coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_HIP], true);

                localA         = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
                localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
                hingeC         = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_LEG], m_bodies[(int)BODYPART.LEFT_LOWER_LEG], ref localA, ref localB);
                hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.LEFT_KNEE]             = hingeC;
                m_joints[(int)JOINT.LEFT_KNEE].m_debugName = "LEFT_KNEE";

                hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_KNEE], true);


                localA         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
                localA._origin = new IndexedVector3(0.18f, -0.10f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
                localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
                coneC          = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], ref localA, ref localB);
                coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
                m_joints[(int)JOINT.RIGHT_HIP]             = coneC;
                m_joints[(int)JOINT.RIGHT_HIP].m_debugName = "RIGHT_HIP";

                coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_HIP], true);

                localA         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
                hingeC         = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], m_bodies[(int)BODYPART.RIGHT_LOWER_LEG], ref localA, ref localB);
                hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.RIGHT_KNEE]             = hingeC;
                m_joints[(int)JOINT.RIGHT_KNEE].m_debugName = "RIGHT_KNEE";

                hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_KNEE], true);


                localA         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_PI);
                localA._origin = new IndexedVector3(-0.2f, 0.15f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
                coneC          = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.LEFT_UPPER_ARM], ref localA, ref localB);
                coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
                coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_joints[(int)JOINT.LEFT_SHOULDER]             = coneC;
                m_joints[(int)JOINT.LEFT_SHOULDER].m_debugName = "LEFT_SHOULDER";

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_SHOULDER], true);

                localA         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
                hingeC         = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_ARM], m_bodies[(int)BODYPART.LEFT_LOWER_ARM], ref localA, ref localB);
                //		hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
                hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.LEFT_ELBOW]             = hingeC;
                m_joints[(int)JOINT.LEFT_ELBOW].m_debugName = "LEFT_ELBOW";

                hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_ELBOW], true);

                localA         = MathUtil.SetEulerZYX(0, 0, 0);
                localA._origin = new IndexedVector3(0.2f, 0.15f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
                coneC          = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], ref localA, ref localB);
                coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
                m_joints[(int)JOINT.RIGHT_SHOULDER]             = coneC;
                m_joints[(int)JOINT.RIGHT_SHOULDER].m_debugName = "RIGHT_SHOULDER";

                coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_SHOULDER], true);

                localA         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
                localB         = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
                localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
                hingeC         = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], m_bodies[(int)BODYPART.RIGHT_LOWER_ARM], ref localA, ref localB);
                //		hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
                hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
                m_joints[(int)JOINT.RIGHT_ELBOW]             = hingeC;
                m_joints[(int)JOINT.RIGHT_ELBOW].m_debugName = "RIGHT_ELBOW";

                hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);

                m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_ELBOW], true);
            }
コード例 #8
0
 /// <summary>
 /// Sets the limit.
 /// </summary>
 /// <param name="limitIndex">Index of the limit.</param>
 /// <param name="limitValue">The limit value.</param>
 public void SetLimit(int limitIndex, float limitValue)
 {
     _cti.SetLimit(limitIndex, limitValue);
 }
コード例 #9
0
        public override void InitializeDemo()
        {
            m_collisionConfiguration = new DefaultCollisionConfiguration();
            m_dispatcher             = new CollisionDispatcher(m_collisionConfiguration);
            IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
            IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);

            m_broadphase       = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
            m_constraintSolver = new SequentialImpulseConstraintSolver();
            m_dynamicsWorld    = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
            m_dynamicsWorld.SetDebugDrawer(m_debugDraw);

            SetCameraDistance(26f);

            //CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f));
            CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40);

            m_collisionShapes.Add(groundShape);
            IndexedMatrix groundTransform = IndexedMatrix.Identity;

            groundTransform._origin = new IndexedVector3(0, -56, 0);
            RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape);

            CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS));

            m_collisionShapes.Add(shape);
            IndexedMatrix trans = IndexedMatrix.Identity;

            trans._origin = new IndexedVector3(0, 20, 0);

            float mass = 1f;

        #if true
            //point to point constraint (ball socket)
            //SEEMS OK
            {
                RigidBody body0 = LocalCreateRigidBody(mass, ref trans, shape);
                trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0);

                mass = 1f;
                RigidBody body1 = null;        //localCreateRigidBody( mass,trans,shape);

                IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS);
                IndexedVector3 axisInA  = new IndexedVector3(0, 0, 1);

                IndexedVector3 pivotInB = body1 != null?body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA;

                IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis *axisInA)) :
                                         body0.GetCenterOfMassTransform()._basis *axisInA;

                HingeConstraint hinge = new HingeConstraint(body0, ref pivotInA, ref axisInA, false);

                float targetVelocity  = 1f;
                float maxMotorImpulse = 1.0f;
                hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);

                m_dynamicsWorld.AddConstraint(hinge);        //p2p);
                hinge.SetDbgDrawSize(5f);
            }
        #endif

#if true
            //create a slider, using the generic D6 constraint
            // SEEMS OK
            {
                mass = 1f;
                IndexedVector3     sliderWorldPos    = new IndexedVector3(0, 10, 0);
                IndexedVector3     sliderAxis        = new IndexedVector3(1, 0, 0);
                float              angle             = 0f;//SIMD_RADS_PER_DEG * 10.f;
                IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(Quaternion.CreateFromAxisAngle(sliderAxis.ToVector3(), angle));
                trans         = IndexedMatrix.Identity;
                trans._origin = sliderWorldPos;
                //trans.setBasis(sliderOrientation);
                sliderTransform = trans;

                d6body0 = LocalCreateRigidBody(mass, ref trans, shape);
                d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                RigidBody fixedBody1 = LocalCreateRigidBody(0, ref trans, null);
                m_dynamicsWorld.AddRigidBody(fixedBody1);

                IndexedMatrix frameInA, frameInB;
                frameInA         = IndexedMatrix.Identity;
                frameInB         = IndexedMatrix.Identity;
                frameInA._origin = new IndexedVector3(0, 5, 0);
                frameInB._origin = new IndexedVector3(0, 5, 0);

                //		bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
                bool useLinearReferenceFrameA = true;        //use fixed frame A for linear llimits
                spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, ref frameInA, ref frameInB, useLinearReferenceFrameA);
                spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit);
                spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit);

                //range should be small, otherwise singularities will 'explode' the constraint
                IndexedVector3 angularLower = new IndexedVector3(-1.5f, 0, 0);
                IndexedVector3 angularUpper = -angularLower;
                spSlider6Dof.SetAngularLowerLimit(ref angularLower);
                spSlider6Dof.SetAngularUpperLimit(ref angularUpper);
                //		slider.setAngularLowerLimit(IndexedVector3(0,0,0));
                //		slider.setAngularUpperLimit(IndexedVector3(0,0,0));

                spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true;
                spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f;
                spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f;


                m_dynamicsWorld.AddConstraint(spSlider6Dof);
                spSlider6Dof.SetDbgDrawSize(5f);
            }
#endif
#if true
            {     // create a door using hinge constraint attached to the world
                CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f));
                m_collisionShapes.Add(pDoorShape);
                IndexedMatrix doorTrans = IndexedMatrix.Identity;
                doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f);
                RigidBody pDoorBody = LocalCreateRigidBody(1.0f, ref doorTrans, pDoorShape);
                pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                IndexedVector3 btPivotA = new IndexedVector3(10f + 2.1f, -2.0f, 0.0f);   // right next to the door slightly outside
                IndexedVector3 btAxisA  = new IndexedVector3(0.0f, 1.0f, 0.0f);          // pointing upwards, aka Y-axis

                spDoorHinge = new HingeConstraint(pDoorBody, ref btPivotA, ref btAxisA, false);

                spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f);
                m_dynamicsWorld.AddConstraint(spDoorHinge);
                spDoorHinge.SetDbgDrawSize(5.0f);
            }
#endif
#if true
            { // create a generic 6DOF constraint
                // SEEMS OK - But debug draw a bit wrong?
                IndexedMatrix tr = IndexedMatrix.Identity;
                tr._origin = new IndexedVector3(10f, 6f, 0f);
                //tr.getBasis().setEulerZYX(0,0,0);
                //		RigidBody pBodyA = localCreateRigidBody( mass, tr, shape);
                RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
                //		RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0);
                pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);

                tr         = IndexedMatrix.Identity;
                tr._origin = new IndexedVector3(0f, 6f, 0f);
                //tr.getBasis().setEulerZYX(0,0,0);
                RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape);
                pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);

                IndexedMatrix frameInA, frameInB;
                frameInA = IndexedMatrix.CreateTranslation(-5, 0, 0);
                frameInB = IndexedMatrix.CreateTranslation(5, 0, 0);

                Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
                //		btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
                IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1);
                pGen6DOF.SetLinearLowerLimit(ref linearLower);
                IndexedVector3 linearUpper = new IndexedVector3(10, 2, 1);
                pGen6DOF.SetLinearUpperLimit(ref linearUpper);
                // ? why again?
                //linearLower = new IndexedVector3(-10,0,0);
                //pGen6DOF.setLinearLowerLimit(ref linearLower);
                //		pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.));
                //		pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.));
                //		pGen6DOF.setLinearUpperLimit(IndexedVector3(0., 0., 0.));

                //		pGen6DOF.getTranslationalLimitMotor().m_enableMotor[0] = true;
                //		pGen6DOF.getTranslationalLimitMotor().m_targetVelocity[0] = 5.0f;
                //		pGen6DOF.getTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f;


                //		pGen6DOF.setAngularLowerLimit(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.));
                //		pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.));
                //		pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI));
                //		pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI));

                IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f);
                IndexedVector3 angularUpper = -angularLower;
                pGen6DOF.SetAngularLowerLimit(ref angularLower);
                pGen6DOF.SetAngularUpperLimit(ref angularUpper);
                //		pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f));
                //		pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f));
                //		pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f));
                //		pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f,  -SIMD_HALF_PI * 1.98f));



                //		pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5));
                //		pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5));
                //		pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.));
                //		pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.));

                m_dynamicsWorld.AddConstraint(pGen6DOF, true);
                pGen6DOF.SetDbgDrawSize(5.0f);
            }
#endif
#if true
            { // create a ConeTwist constraint
                IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10, 5, 0);

                RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
                pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);

                tr = IndexedMatrix.CreateTranslation(-10, -5, 0);

                RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape);

                IndexedMatrix frameInA, frameInB;
                frameInA         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                frameInA._origin = new IndexedVector3(0, -5, 0);
                frameInB         = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
                frameInB._origin = new IndexedVector3(0, 5, 0);

                ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB);
                pCT.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 1.0f, 0.3f, 1.0f);       // soft limit == hard limit
                m_dynamicsWorld.AddConstraint(pCT, true);
                pCT.SetDbgDrawSize(5.0f);
            }
#endif
#if true
            { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
                // WORKS OK
                IndexedMatrix tr    = IndexedMatrix.Identity;
                RigidBody     pBody = LocalCreateRigidBody(1.0f, ref tr, shape);
                pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                IndexedVector3 btPivotA = new IndexedVector3(10.0f, 0.0f, 0.0f);
                IndexedVector3 btAxisA  = new IndexedVector3(0.0f, 0.0f, 1.0f);

                HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA, false);
                //		pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver
                pHinge.EnableAngularMotor(true, -1.0f, 1.65f);         // use for the new SIMD solver
                m_dynamicsWorld.AddConstraint(pHinge);
                pHinge.SetDbgDrawSize(5.0f);
            }
#endif
#if true
            {
                // WORKS OK
                // create a universal joint using generic 6DOF constraint
                // create two rigid bodies
                // static bodyA (parent) on top:
                IndexedMatrix tr     = IndexedMatrix.CreateTranslation(20, 4, 0);
                RigidBody     pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
                pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                // dynamic bodyB (child) below it :
                tr = IndexedMatrix.CreateTranslation(20, 0, 0);
                RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
                pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                // add some (arbitrary) data to build constraint frames
                IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f);
                IndexedVector3 childAxis  = new IndexedVector3(0.0f, 0.0f, 1.0f);
                IndexedVector3 anchor     = new IndexedVector3(20.0f, 2.0f, 0.0f);

                UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
                pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f);
                pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
                // add constraint to world
                m_dynamicsWorld.AddConstraint(pUniv, true);
                // draw constraint frames and limits for debugging
                pUniv.SetDbgDrawSize(5.0f);
            }
#endif

#if true
            // WORKS OK
            { // create a generic 6DOF constraint with springs
                IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 16f, 0f);
                //tr.setIdentity();
                //tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.)));
                //tr.getBasis().setEulerZYX(0,0,0);
                RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
                pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);

                //tr.setIdentity();
                //tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
                //tr.getBasis().setEulerZYX(0,0,0);
                tr = IndexedMatrix.CreateTranslation(-10, 16, 0);
                RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
                pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);

                IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f, 0f, 0f);
                IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f, 0f, 0f);

                Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
                pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f));
                pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f));

                pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f));
                pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f));

                m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true);
                pGen6DOFSpring.SetDbgDrawSize(5.0f);

                pGen6DOFSpring.EnableSpring(0, true);
                pGen6DOFSpring.SetStiffness(0, 39.478f);
                pGen6DOFSpring.SetDamping(0, 0.5f);
                pGen6DOFSpring.EnableSpring(5, true);
                pGen6DOFSpring.SetStiffness(5, 39.478f);
                pGen6DOFSpring.SetDamping(0, 0.3f);
                pGen6DOFSpring.SetEquilibriumPoint();
            }
#endif
#if true
            {
                // WORKS OK
                // create a Hinge2 joint
                // create two rigid bodies
                // static bodyA (parent) on top:
                IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 4f, 0f);

                RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
                pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                // dynamic bodyB (child) below it :
                tr = IndexedMatrix.CreateTranslation(-20f, 0f, 0f);
                RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
                pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                // add some data to build constraint frames
                IndexedVector3   parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f);
                IndexedVector3   childAxis  = new IndexedVector3(1.0f, 0.0f, 0.0f);
                IndexedVector3   anchor     = new IndexedVector3(-20.0f, 0.0f, 0.0f);
                Hinge2Constraint pHinge2    = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
                pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f);
                pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f);
                // add constraint to world
                m_dynamicsWorld.AddConstraint(pHinge2, true);
                // draw constraint frames and limits for debugging
                pHinge2.SetDbgDrawSize(5.0f);
            }
#endif
#if true
            {
                // WORKS OK
                // create a Hinge joint between two dynamic bodies
                // create two rigid bodies
                // static bodyA (parent) on top:
                IndexedMatrix tr     = IndexedMatrix.CreateTranslation(-20f, -2f, 0f);
                RigidBody     pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
                pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                // dynamic bodyB:
                tr = IndexedMatrix.CreateTranslation(-30f, -2f, 0f);
                RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape);
                pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
                // add some data to build constraint frames
                IndexedVector3 axisA  = new IndexedVector3(0.0f, 1.0f, 0.0f);
                IndexedVector3 axisB  = new IndexedVector3(0.0f, 1.0f, 0.0f);
                IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f);
                IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f);

                spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB);
                spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
                // add constraint to world
                m_dynamicsWorld.AddConstraint(spHingeDynAB, true);
                // draw constraint frames and limits for debugging
                spHingeDynAB.SetDbgDrawSize(5.0f);
            }
#endif
        }
コード例 #10
0
ファイル: Physics.cs プロジェクト: diqost/bullet
        public Physics()
        {
            SetupEmptyDynamicsWorld();

            CollisionShape groundShape = new BoxShape(50, 1, 50);

            //CollisionShape groundShape = new StaticPlaneShape(Vector3.UnitY, 40);
            CollisionShapes.Add(groundShape);
            RigidBody body = LocalCreateRigidBody(0, Matrix.Translation(0, -16, 0), groundShape);

            body.UserObject = "Ground";

            CollisionShape shape = new BoxShape(new Vector3(CubeHalfExtents));

            CollisionShapes.Add(shape);


            float mass = 1.0f;

            RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), shape);

            RigidBody body1 = null;//LocalCreateRigidBody(mass, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), shape);
            //RigidBody body1 = LocalCreateRigidBody(0, Matrix.Translation(2*CUBE_HALF_EXTENTS,20,0), null);
            //body1.ActivationState = ActivationState.DisableDeactivation;
            //body1.SetDamping(0.3f, 0.3f);

            Vector3 pivotInA = new Vector3(CubeHalfExtents, -CubeHalfExtents, -CubeHalfExtents);
            Vector3 axisInA  = new Vector3(0, 0, 1);

            Vector3 pivotInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
                pivotInB = Vector3.TransformCoordinate(pivotInA, transform);
            }
            else
            {
                pivotInB = pivotInA;
            }

            Vector3 axisInB;

            if (body1 != null)
            {
                Matrix transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
                axisInB = Vector3.TransformCoordinate(axisInA, transform);
            }
            else
            {
                axisInB = Vector3.TransformCoordinate(axisInA, body0.CenterOfMassTransform);
            }

            if (P2P)
            {
                TypedConstraint p2p = new Point2PointConstraint(body0, pivotInA);
                //TypedConstraint p2p = new Point2PointConstraint(body0,body1,pivotInA,pivotInB);
                //TypedConstraint hinge = new HingeConstraint(body0,body1,pivotInA,pivotInB,axisInA,axisInB);
                World.AddConstraint(p2p);
                p2p.DebugDrawSize = 5;
            }
            else
            {
                HingeConstraint hinge = new HingeConstraint(body0, pivotInA, axisInA);

                //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
                //float	targetVelocity = 0.f;
                //float	maxMotorImpulse = 0.01;
                float targetVelocity  = 1.0f;
                float maxMotorImpulse = 1.0f;
                hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
                World.AddConstraint(hinge);
                hinge.DebugDrawSize = 5;
            }



            //create a slider, using the generic D6 constraint
            Vector3 sliderWorldPos = new Vector3(0, 10, 0);
            Vector3 sliderAxis     = Vector3.UnitX;
            float   angle          = 0;//SIMD_RADS_PER_DEG * 10.f;
            Matrix  trans          = Matrix.RotationAxis(sliderAxis, angle) * Matrix.Translation(sliderWorldPos);

            d6body0 = LocalCreateRigidBody(mass, trans, shape);
            d6body0.ActivationState = ActivationState.DisableDeactivation;

            RigidBody fixedBody1 = LocalCreateRigidBody(0, trans, null);

            World.AddRigidBody(fixedBody1);

            Matrix frameInA = Matrix.Translation(0, 5, 0);
            Matrix frameInB = Matrix.Translation(0, 5, 0);

            //bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
            bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits

            spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, frameInA, frameInB, useLinearReferenceFrameA);
            spSlider6Dof.LinearLowerLimit = lowerSliderLimit;
            spSlider6Dof.LinearUpperLimit = hiSliderLimit;

            //range should be small, otherwise singularities will 'explode' the constraint
            //spSlider6Dof.AngularLowerLimit = new Vector3(-1.5f,0,0);
            //spSlider6Dof.AngularUpperLimit = new Vector3(1.5f,0,0);
            //spSlider6Dof.AngularLowerLimit = new Vector3(0,0,0);
            //spSlider6Dof.AngularUpperLimit = new Vector3(0,0,0);
            spSlider6Dof.AngularLowerLimit = new Vector3((float)-Math.PI, 0, 0);
            spSlider6Dof.AngularUpperLimit = new Vector3(1.5f, 0, 0);

            spSlider6Dof.TranslationalLimitMotor.EnableMotor[0] = true;
            spSlider6Dof.TranslationalLimitMotor.TargetVelocity = new Vector3(-5.0f, 0, 0);
            spSlider6Dof.TranslationalLimitMotor.MaxMotorForce  = new Vector3(0.1f, 0, 0);

            World.AddConstraint(spSlider6Dof);
            spSlider6Dof.DebugDrawSize = 5;



            // create a door using hinge constraint attached to the world

            CollisionShape pDoorShape = new BoxShape(2.0f, 5.0f, 0.2f);

            CollisionShapes.Add(pDoorShape);
            RigidBody pDoorBody = LocalCreateRigidBody(1.0f, Matrix.Translation(-5.0f, -2.0f, 0.0f), pDoorShape);

            pDoorBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 btPivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
            Vector3 btAxisA  = Vector3.UnitY;                          // pointing upwards, aka Y-axis

            spDoorHinge = new HingeConstraint(pDoorBody, btPivotA, btAxisA);

            //spDoorHinge.SetLimit(0.0f, (float)Math.PI / 2);
            // test problem values
            //spDoorHinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);

            //spDoorHinge.SetLimit(1, -1);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
            //spDoorHinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
            spDoorHinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
            //spDoorHinge.SetLimit(0, 0);
            World.AddConstraint(spDoorHinge);
            spDoorHinge.DebugDrawSize = 5;

            RigidBody pDropBody = LocalCreateRigidBody(10.0f, Matrix.Translation(-5.0f, 2.0f, 0.0f), shape);



            // create a generic 6DOF constraint

            //RigidBody pBodyA = LocalCreateRigidBody(mass, Matrix.Translation(10.0f, 6.0f, 0), shape);
            RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), shape);

            //RigidBody pBodyA = LocalCreateRigidBody(0, Matrix.Translation(10, 6, 0), null);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            RigidBody pBodyB = LocalCreateRigidBody(mass, Matrix.Translation(0, 6, 0), shape);

            //RigidBody pBodyB = LocalCreateRigidBody(0, Matrix.Translation(0, 6, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(-5, 0, 0);
            frameInB = Matrix.Translation(5, 0, 0);

            Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, true);

            //Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, frameInA, frameInB, false);
            pGen6DOF.LinearLowerLimit = new Vector3(-10, -2, -1);
            pGen6DOF.LinearUpperLimit = new Vector3(10, 2, 1);
            //pGen6DOF.LinearLowerLimit = new Vector3(-10, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(10, 0, 0);
            //pGen6DOF.LinearLowerLimit = new Vector3(0, 0, 0);
            //pGen6DOF.LinearUpperLimit = new Vector3(0, 0, 0);

            //pGen6DOF.TranslationalLimitMotor.EnableMotor[0] = true;
            //pGen6DOF.TranslationalLimitMotor.TargetVelocity = new Vector3(5, 0, 0);
            //pGen6DOF.TranslationalLimitMotor.MaxMotorForce = new Vector3(0.1f, 0, 0);

            //pGen6DOF.AngularLowerLimit = new Vector3(0, (float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, -(float)Math.PI * 0.9f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, 0, -(float)Math.PI);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0, (float)Math.PI);

            pGen6DOF.AngularLowerLimit = new Vector3(-(float)Math.PI / 4, -0.75f, -(float)Math.PI * 0.4f);
            pGen6DOF.AngularUpperLimit = new Vector3((float)Math.PI / 4, 0.75f, (float)Math.PI * 0.4f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.75f, (float)Math.PI * 0.8f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.75f, -(float)Math.PI * 0.8f);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -(float)Math.PI * 0.8f, (float)Math.PI * 1.98f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, (float)Math.PI * 0.8f, -(float)Math.PI * 1.98f);

            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, -0.5f, -0.5f);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0.5f, 0.5f);
            //pGen6DOF.AngularLowerLimit = new Vector3(-0.75f, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0.75f, 0, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(0, -0.7f, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(0, 0.7f, 0);
            //pGen6DOF.AngularLowerLimit = new Vector3(-1, 0, 0);
            //pGen6DOF.AngularUpperLimit = new Vector3(1, 0, 0);



            // create a ConeTwist constraint

            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 5, 0), shape);
            //pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-10, 5, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(0, Matrix.Translation(-10, -5, 0), shape);
            //pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, -5, 0), shape);

            frameInA  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInA *= Matrix.Translation(0, -5, 0);
            frameInB  = Matrix.RotationYawPitchRoll(0, 0, (float)Math.PI / 2);
            frameInB *= Matrix.Translation(0, 5, 0);

            coneTwist = new ConeTwistConstraint(pBodyA, pBodyB, frameInA, frameInB);
            //coneTwist.SetLimit((float)Math.PI / 4, (float)Math.PI / 4, (float)Math.PI * 0.8f);
            //coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 1.0f); // soft limit == hard limit
            coneTwist.SetLimit((((float)Math.PI / 4) * 0.6f), (float)Math.PI / 4, (float)Math.PI * 0.8f, 0.5f);
            World.AddConstraint(coneTwist, true);
            coneTwist.DebugDrawSize = 5;



            // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)

            RigidBody pBody = LocalCreateRigidBody(1.0f, Matrix.Identity, shape);

            pBody.ActivationState = ActivationState.DisableDeactivation;
            Vector3 PivotA = new Vector3(10.0f, 0.0f, 0.0f);

            btAxisA = new Vector3(0.0f, 0.0f, 1.0f);

            HingeConstraint pHinge = new HingeConstraint(pBody, btPivotA, btAxisA);

            //pHinge.EnableAngularMotor(true, -1.0f, 0.165f); // use for the old solver
            pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
            World.AddConstraint(pHinge);
            pHinge.DebugDrawSize = 5;



            // create a universal joint using generic 6DOF constraint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some (arbitrary) data to build constraint frames
            Vector3 parentAxis = new Vector3(1, 0, 0);
            Vector3 childAxis  = new Vector3(0, 0, 1);
            Vector3 anchor     = new Vector3(20, 2, 0);

            UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);

            pUniv.SetLowerLimit(-(float)Math.PI / 4, -(float)Math.PI / 4);
            pUniv.SetUpperLimit((float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pUniv, true);
            // draw constraint frames and limits for debugging
            pUniv.DebugDrawSize = 5;

            World.AddConstraint(pGen6DOF, true);
            pGen6DOF.DebugDrawSize = 5;



            // create a generic 6DOF constraint with springs

            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 16, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;

            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-10, 16, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;

            frameInA = Matrix.Translation(10, 0, 0);
            frameInB = Matrix.Identity;

            Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, frameInA, frameInB, true);

            pGen6DOFSpring.LinearUpperLimit = new Vector3(5, 0, 0);
            pGen6DOFSpring.LinearLowerLimit = new Vector3(-5, 0, 0);

            pGen6DOFSpring.AngularLowerLimit = new Vector3(0, 0, -1.5f);
            pGen6DOFSpring.AngularUpperLimit = new Vector3(0, 0, 1.5f);

            World.AddConstraint(pGen6DOFSpring, true);
            pGen6DOFSpring.DebugDrawSize = 5;

            pGen6DOFSpring.EnableSpring(0, true);
            pGen6DOFSpring.SetStiffness(0, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.5f);
            pGen6DOFSpring.EnableSpring(5, true);
            pGen6DOFSpring.SetStiffness(5, 39.478f);
            pGen6DOFSpring.SetDamping(0, 0.3f);
            pGen6DOFSpring.SetEquilibriumPoint();



            // create a Hinge2 joint
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(0, Matrix.Translation(-20, 4, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB (child) below it :
            pBodyB = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, 0, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            parentAxis = new Vector3(0, 1, 0);
            childAxis  = new Vector3(1, 0, 0);
            anchor     = new Vector3(-20, 0, 0);
            Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, anchor, parentAxis, childAxis);

            pHinge2.SetLowerLimit(-(float)Math.PI / 4);
            pHinge2.SetUpperLimit((float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(pHinge2, true);
            // draw constraint frames and limits for debugging
            pHinge2.DebugDrawSize = 5;



            // create a Hinge joint between two dynamic bodies
            // create two rigid bodies
            // static bodyA (parent) on top:
            pBodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), shape);
            pBodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic bodyB:
            pBodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), shape);
            pBodyB.ActivationState = ActivationState.DisableDeactivation;
            // add some data to build constraint frames
            Vector3 axisA  = new Vector3(0, 1, 0);
            Vector3 axisB  = new Vector3(0, 1, 0);
            Vector3 pivotA = new Vector3(-5, 0, 0);
            Vector3 pivotB = new Vector3(5, 0, 0);

            spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, pivotA, pivotB, axisA, axisB);
            spHingeDynAB.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);
            // add constraint to world
            World.AddConstraint(spHingeDynAB, true);
            // draw constraint frames and limits for debugging
            spHingeDynAB.DebugDrawSize = 5;
        }
コード例 #11
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);
        }