コード例 #1
0
        public void Evaluate(int SpreadMax)
        {
            IConstraintContainer inputWorld = this.contraintContainer[0];

            if (inputWorld != null)
            {
                this.persister.UpdateWorld(inputWorld);

                for (int i = 0; i < SpreadMax; i++)
                {
                    if (FCreate[i])
                    {
                        RigidBody body = FBody[i];
                        if (body != null)
                        {
                            HingeConstraint cst = new HingeConstraint(body, this.FPivot1[i].ToBulletVector(), this.FAxis[i].ToBulletVector());
                            cst.SetLimit(this.FLimitLow[i] * (float)Math.PI * 2.0f, this.FLimitHigh[i] * (float)Math.PI * 2.0f);
                            cst.DebugDrawSize = 5.0f;
                            this.persister.Append(cst);
                        }
                    }
                }

                this.persister.Flush();
            }
            else
            {
                this.constraintsOutput.SliceCount = 0;
            }
        }
コード例 #2
0
        // Create a door using a hinge constraint attached to the world
        private void CreateDoor()
        {
            const float mass = 1.0f;

            var doorShape = new BoxShape(2.0f, 5.0f, 0.2f);

            CollisionShapes.Add(doorShape);
            RigidBody doorBody = LocalCreateRigidBody(mass, Matrix.Translation(-5.0f, -2.0f, 0.0f), doorShape);

            doorBody.ActivationState = ActivationState.DisableDeactivation;

            var pivotA = new Vector3(10.0f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
            var axisA  = Vector3.UnitY;                          // pointing upwards, aka Y-axis

            var hinge = new HingeConstraint(doorBody, pivotA, axisA);

            hinge.DebugDrawSize = 5;

            //hinge.SetLimit(0.0f, (float)Math.PI / 2);
            // test problem values
            //hinge.SetLimit(-(float)Math.PI, (float)Math.PI * 0.8f);
            //hinge.SetLimit(1, -1);
            //hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI);
            //hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.3f, 0.0f);
            //hinge.SetLimit(-(float)Math.PI * 0.8f, (float)Math.PI, 0.9f, 0.01f, 0.0f); // "sticky limits"
            hinge.SetLimit(-(float)Math.PI * 0.25f, (float)Math.PI * 0.25f);
            //hinge.SetLimit(0, 0);
            World.AddConstraint(hinge);
        }
コード例 #3
0
        private void CreateGears()
        {
            const float theta   = (float)Math.PI / 4.0f;
            float       radiusA = 2 - (float)Math.Tan(theta);
            float       radiusB = 1 / (float)Math.Cos(theta);
            float       ratio   = radiusB / radiusA;

            Matrix    transform = Matrix.Translation(-8, 1, -8);
            RigidBody gearA     = CreateGear(radiusA, transform);

            gearA.AngularFactor = new Vector3(0, 1, 0);

            var orientation = Quaternion.RotationAxis(new Vector3(0, 0, 1), -theta);

            transform = Matrix.RotationQuaternion(orientation) * Matrix.Translation(-10, 2, -8);
            RigidBody gearB = CreateGear(radiusB, transform);

            gearB.AngularVelocity = new Vector3(0, 3, 0);

            var hinge = new HingeConstraint(gearB, Vector3.Zero, new Vector3(0, 1, 0), true);

            World.AddConstraint(hinge);

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

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

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

            var gear = new GearConstraint(gearA, gearB, axisA, axisB, ratio);

            World.AddConstraint(gear, true);
        }
コード例 #4
0
        // Create a Hinge joint between two dynamic bodies
        private void CreateHinge()
        {
            // static body (parent) on top
            RigidBody bodyA = LocalCreateRigidBody(1.0f, Matrix.Translation(-20, -2, 0), cubeShape);

            bodyA.ActivationState = ActivationState.DisableDeactivation;
            // dynamic body
            RigidBody bodyB = LocalCreateRigidBody(10.0f, Matrix.Translation(-30, -2, 0), cubeShape);

            bodyB.ActivationState = ActivationState.DisableDeactivation;

            // add some data to build constraint frames
            var axisA  = new Vector3(0, 1, 0);
            var axisB  = new Vector3(0, 1, 0);
            var pivotA = new Vector3(-5, 0, 0);
            var pivotB = new Vector3(5, 0, 0);
            var hinge  = new HingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB);

            hinge.SetLimit(-(float)Math.PI / 4, (float)Math.PI / 4);

            // draw constraint frames and limits for debugging
            hinge.DebugDrawSize = 5;

            World.AddConstraint(hinge, true);
        }
コード例 #5
0
        public static void Update(ref BulletPhysicsComponent bpc)
        {
            foreach (var entry in _flippers)
            {
                PhyFlipper phyFlipper = entry.Value;
                phyFlipper._FlipperUpdate(ref bpc);

                // debug
                var dbg = EngineProvider <IDebugUI> .Get();

                bool  isChanged = false;
                float sa        = phyFlipper._startAngle * 180.0f / Mathf.PI;
                float se        = phyFlipper._endAngle * 180.0f / Mathf.PI;

                isChanged |= dbg.GetProperty(phyFlipper.dbgPropStartAngle, ref sa);
                isChanged |= dbg.GetProperty(phyFlipper.dbgPropEndAngle, ref se);

                if (isChanged)
                {
                    phyFlipper._startAngle = sa * Mathf.PI / 180.0f;
                    phyFlipper._endAngle   = se * Mathf.PI / 180.0f;

                    HingeConstraint hinge = (HingeConstraint)phyFlipper._constraint;
                    if (phyFlipper.RotationDirection == 1)
                    {
                        hinge.SetLimit(phyFlipper._startAngle, phyFlipper._endAngle, 0.0f);
                    }
                    else
                    {
                        hinge.SetLimit(phyFlipper._endAngle, phyFlipper._startAngle, 0.0f);
                    }
                }
            }
        }
コード例 #6
0
        public void SetMotorTargets(float deltaTime)
        {
            float ms     = deltaTime * 1000000.0f;
            float minFPS = 1000000.0f / 60.0f;

            if (ms > minFPS)
            {
                ms = minFPS;
            }

            m_Time += ms;

            //
            // set per-frame sinusoidal position targets using angular motor (hacky?)
            //
            for (int r = 0; r < m_rigs.Count; r++)
            {
                HingeConstraint[] hingeConstraints = m_rigs[r].GetJoints();
                for (int i = 0; i < 2 * TestRig.NUM_LEGS; i++)
                {
                    HingeConstraint hingeC    = hingeConstraints[i];
                    float           fCurAngle = hingeC.GetHingeAngle();

                    float fTargetPercent     = ((int)(m_Time / 1000) % (int)(m_fCyclePeriod)) / m_fCyclePeriod;
                    float fTargetAngle       = 0.5f * (1 + (float)Math.Sin(MathUtil.SIMD_2_PI * fTargetPercent));
                    float fTargetLimitAngle  = hingeC.GetLowerLimit() + fTargetAngle * (hingeC.GetUpperLimit() - hingeC.GetLowerLimit());
                    float fAngleError        = fTargetLimitAngle - fCurAngle;
                    float fDesiredAngularVel = 1000000.0f * fAngleError / ms;
                    hingeC.EnableAngularMotor(true, fDesiredAngularVel, m_fMuscleStrength);
                }
            }
        }
コード例 #7
0
ファイル: MotorDemo.cs プロジェクト: diqost/bullet
        void SetMotorTargets(float deltaTime)
        {
            float ms     = deltaTime * 1000000.0f;
            float minFPS = 1000000.0f / 60.0f;

            if (ms > minFPS)
            {
                ms = minFPS;
            }

            m_Time += ms;

            //
            // set per-frame sinusoidal position targets using angular motor (hacky?)
            //
            foreach (var rig in rigs)
            {
                for (int i = 0; i < 2 * NumLegs; i++)
                {
                    HingeConstraint hingeC    = rig.GetJoints()[i] as HingeConstraint;
                    float           fCurAngle = hingeC.HingeAngle;

                    float fTargetPercent     = ((int)(m_Time / 1000.0f) % (int)fCyclePeriod) / fCyclePeriod;
                    float fTargetAngle       = (float)(0.5 * (1 + Math.Sin(2.0f * Math.PI * fTargetPercent)));
                    float fTargetLimitAngle  = hingeC.LowerLimit + fTargetAngle * (hingeC.UpperLimit - hingeC.LowerLimit);
                    float fAngleError        = fTargetLimitAngle - fCurAngle;
                    float fDesiredAngularVel = 1000000.0f * fAngleError / ms;
                    hingeC.EnableAngularMotor(true, fDesiredAngularVel, fMuscleStrength);
                }
            }
        }
コード例 #8
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 (targetRigidBodyA == null)
            {
                Debug.LogError("Constraint target rigid body was not set.");
                return(false);
            }
            if (localPivotAxisA == Vector3.zero)
            {
                Debug.LogError("Constaint axis cannot be zero vector");
                return(false);
            }
            RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();

            if (rba == null)
            {
                Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                return(false);
            }
            if (constraintType == ConstraintType.constrainToAnotherBody)
            {
                RigidBody rbb = (RigidBody)targetRigidBodyB.GetCollisionObject();
                if (rbb == null)
                {
                    Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                    return(false);
                }
                constraintPtr = new HingeConstraint(rba, rbb, localPointOnHingeOffsetA.ToBullet(), localPointOnHingeOffsetB.ToBullet(), localPivotAxisA.ToBullet(), localPivotAxisB.ToBullet());
            }
            else
            {
                constraintPtr = new HingeConstraint(rba, localPointOnHingeOffsetA.ToBullet(), localPivotAxisA.ToBullet());
            }
            if (enableMotor)
            {
                ((HingeConstraint)constraintPtr).EnableAngularMotor(true, targetMotorAngularVelocity, maxMotorImpulse);
            }
            if (setLimit)
            {
                ((HingeConstraint)constraintPtr).SetLimit(lowLimitAngleRadians, highLimitAngleRadians, limitSoftness, limitBiasFactor);
            }
            constraintPtr.Userobject = this;
            return(true);
        }
コード例 #9
0
        /// <summary>
        /// Creates the joint data
        /// </summary>
        public void CreateJoint()
        {
            if (joint != null || GetSkeletalJoint() == null)
            {
                return;
            }

            switch (GetSkeletalJoint().GetJointType())
            {
            case SkeletalJointType.ROTATIONAL:
                RotationalJoint_Base nodeR        = (RotationalJoint_Base)GetSkeletalJoint();
                CollisionObject      parentObject = ((BulletRigidNode)GetParent()).BulletObject;
                WheelDriverMeta      wheel        = GetSkeletalJoint().cDriver.GetInfo <WheelDriverMeta>();

                Vector3 pivot = nodeR.basePoint.Convert();

                Matrix4 locJ, locP;     //Local Joint Pivot, Local Parent Pivot

                BulletObject.WorldTransform = parentObject.WorldTransform * Matrix4.CreateTranslation(pivot);

                if (debug)
                {
                    Console.WriteLine("Pivot at " + pivot);
                }

                GetFrames(nodeR.basePoint.Convert(), parentObject.WorldTransform, BulletObject.WorldTransform, out locP, out locJ);

                //HingeConstraint temp = new HingeConstraint((RigidBody)parentObject, /*(RigidBody)*/BulletObject, locP, locJ);
                HingeConstraint temp = new HingeConstraint((RigidBody)parentObject, BulletObject, pivot, Vector3.Zero, nodeR.axis.Convert(), nodeR.axis.Convert());
                joint = temp;
                temp.SetAxis(nodeR.axis.Convert());

                if (nodeR.hasAngularLimit)
                {
                    temp.SetLimit(nodeR.angularLimitLow, nodeR.angularLimitHigh);
                }

                //also need to find a less screwy way to do this
                Update = (f) => { (/*(RigidBody)*/ BulletObject).ApplyTorque(nodeR.axis.Convert() * f * 25); };

                if (debug)
                {
                    Console.WriteLine("{0} joint made", wheel == null ? "Rotational" : "Wheel");
                }
                break;

            default:
                if (debug)
                {
                    Console.WriteLine("Received joint of type {0}", GetSkeletalJoint().GetJointType());
                }
                break;
            }
        }
コード例 #10
0
 public void Evaluate(int SpreadMax)
 {
     for (int i = 0; i < SpreadMax; i++)
     {
         if (this.FConstraint[i] != null && FApply[i])
         {
             HingeConstraint cst = this.FConstraint[i];
             cst.EnableAngularMotor(this.FEnabled[i], this.FTargetVelocity[i], this.FMaxImpulse[i]);
         }
     }
 }
コード例 #11
0
        private void SetJointMotorTarget(HingeConstraint joint, float deltaTimeMs)
        {
            float currentAngle = joint.HingeAngle;

            float targetPercent     = ((int)(_time / 1000.0f) % (int)_cyclePeriod) / _cyclePeriod;
            float targetAngle       = (float)(0.5 * (1 + Math.Sin(2.0f * Math.PI * targetPercent)));
            float targetLimitAngle  = joint.LowerLimit + targetAngle * (joint.UpperLimit - joint.LowerLimit);
            float angleError        = targetLimitAngle - currentAngle;
            float desiredAngularVel = 1000000.0f * angleError / deltaTimeMs;

            joint.EnableAngularMotor(true, desiredAngularVel, _muscleStrength);
        }
コード例 #12
0
ファイル: TestRig.cs プロジェクト: slavslav/BulletSharp
        private static HingeConstraint CreateHipJoint(RigidBody root, RigidBody thigh, float direction)
        {
            Vector3 origin    = ToCartesian(direction, BodyRadius);
            Matrix  rootFrame = Matrix.RotationYawPitchRoll(-direction, 0, 0) * Matrix.Translation(origin);

            Matrix thighFrame = rootFrame * root.WorldTransform * Matrix.Invert(thigh.WorldTransform);

            var hinge = new HingeConstraint(root, thigh, rootFrame, thighFrame);

            hinge.SetLimit(-0.75f * PI_4, PI_8);
            //hingeC.SetLimit(-0.1f, 0.1f);

            return(hinge);
        }
コード例 #13
0
        /*//TODO: What about inheritance problems -> should return any constraint type
         * public IPoint2PointConstraintImp GetConstraint(int i)
         * {
         *  return (Point2PointConstraintImp)BtWorld.GetConstraint(0).UserObject;
         * }*/


        //HingeConstraint
        public IHingeConstraintImp AddHingeConstraint(IRigidBodyImp rigidBodyA, float4x4 frameInA, bool useReferenceFrameA)
        {
            var rigidBodyAImp     = (RigidBodyImp)rigidBodyA;
            var btRigidBodyA      = rigidBodyAImp._rbi;
            var btFframeInA       = Translater.Float4X4ToBtMatrix(frameInA);
            var btHingeConstraint = new HingeConstraint(btRigidBodyA, btFframeInA, useReferenceFrameA);

            BtWorld.AddConstraint(btHingeConstraint);

            var retval = new HingeConstraintImp();

            retval._hci = btHingeConstraint;
            btHingeConstraint.UserObject = retval;
            return(retval);
        }
コード例 #14
0
ファイル: TestRig.cs プロジェクト: slavslav/BulletSharp
        private static HingeConstraint CreateKneeJoint(RigidBody root, RigidBody thigh, RigidBody shin, float direction)
        {
            Vector3 origin     = ToCartesian(direction, BodyRadius + ThighLength);
            Matrix  localFrame = Matrix.RotationYawPitchRoll(-direction, 0, 0) * Matrix.Translation(origin);

            Matrix thighFrame = localFrame * root.WorldTransform * Matrix.Invert(thigh.WorldTransform);
            Matrix shinFrame  = localFrame * root.WorldTransform * Matrix.Invert(shin.WorldTransform);

            var hinge = new HingeConstraint(thigh, shin, thighFrame, shinFrame);

            //hingeC.SetLimit(-0.01f, 0.01f);
            hinge.SetLimit(-PI_8, 0.2f);

            return(hinge);
        }
コード例 #15
0
        public IHingeConstraintImp AddHingeConstraint(IRigidBodyImp rigidBodyA, float3 pivotInA, float3 axisInA, bool useReferenceFrameA)
        {
            var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
            var btRigidBodyA  = rigidBodyAImp._rbi;

            var btHingeConstraint = new HingeConstraint(btRigidBodyA, new Vector3(pivotInA.x, pivotInA.y, pivotInA.z), new Vector3(axisInA.x, axisInA.y, axisInA.z), useReferenceFrameA);

            BtWorld.AddConstraint(btHingeConstraint);

            var retval = new HingeConstraintImp();

            retval._hci = btHingeConstraint;
            btHingeConstraint.UserObject = retval;
            return(retval);
        }
コード例 #16
0
 public HingeJoint(
     ICollisionShape shapeA,
     ICollisionShape shapeB,
     Vector3d startAnchorPosition,
     Vector3d hingeAxis,
     double restoreCoefficient,
     double springCoefficient)
 {
     hingeConstraint = new HingeConstraint(
         ((IMapper)shapeA).GetShape(),
         ((IMapper)shapeB).GetShape(),
         startAnchorPosition,
         hingeAxis,
         restoreCoefficient,
         springCoefficient);
 }
コード例 #17
0
        private void CreateMotorHinge2()
        {
            RigidBody body = LocalCreateRigidBody(1.0f, Matrix.Identity, cubeShape);

            body.ActivationState = ActivationState.DisableDeactivation;

            Vector3 pivotInA = new Vector3(10.0f, 0.0f, 0.0f);
            Vector3 axisInA  = new Vector3(0, 0, 1);

            var         hinge           = new HingeConstraint(body, pivotInA, axisInA);
            const float targetVelocity  = -1.0f;
            const float maxMotorImpulse = 1.65f;

            hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
            hinge.DebugDrawSize = 5;
            World.AddConstraint(hinge);
        }
コード例 #18
0
        // Hinge connected to the world, with motor
        private void CreateMotorHinge()
        {
            const float mass = 1.0f;
            RigidBody   body = LocalCreateRigidBody(mass, Matrix.Translation(0, 20, 0), cubeShape);

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

            var hinge = new HingeConstraint(body, pivotInA, axisInA);

            //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
            //const float targetVelocity = 0;
            //const float maxMotorImpulse = 0.01f;
            const float targetVelocity  = 1.0f;
            const float maxMotorImpulse = 1.0f;

            hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
            hinge.DebugDrawSize = 5;
            World.AddConstraint(hinge);
        }
コード例 #19
0
ファイル: SoftDemo.cs プロジェクト: superowner/BulletSharp
        void CreateGear(Vector3 pos, float speed)
        {
            Matrix        startTransform = Matrix.Translation(pos);
            CompoundShape shape          = new CompoundShape();

#if true
            shape.AddChildShape(Matrix.Identity, new BoxShape(5, 1, 6));
            shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(5, 1, 6));
#else
            shape.AddChildShape(Matrix.Identity, new CylinderShapeZ(5, 1, 7));
            shape.AddChildShape(Matrix.RotationZ((float)Math.PI), new BoxShape(4, 1, 8));
#endif
            RigidBody body = LocalCreateRigidBody(10, startTransform, shape);
            body.Friction = 1;
            HingeConstraint hinge = new HingeConstraint(body, Matrix.Identity);
            if (speed != 0)
            {
                hinge.EnableAngularMotor(true, speed, 3);
            }
            World.AddConstraint(hinge);
        }
コード例 #20
0
        /// <summary>
        /// Adds the hinge constraint.
        /// </summary>
        /// <param name="rigidBodyA">The rigid body a.</param>
        /// <param name="rigidBodyB">The rigid body b.</param>
        /// <param name="brAFrame">The br a frame.</param>
        /// <param name="brBFrame">The br b frame.</param>
        /// <param name="useReferenceFrameA">if set to <c>true</c> [use reference frame a].</param>
        /// <returns></returns>
        public IHingeConstraintImp AddHingeConstraint(IRigidBodyImp rigidBodyA, IRigidBodyImp rigidBodyB, float4x4 brAFrame, float4x4 brBFrame, bool useReferenceFrameA)
        {
            var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
            var btRigidBodyA  = rigidBodyAImp._rbi;

            var rigidBodyBImp = (RigidBodyImp)rigidBodyB;
            var btRigidBodyB  = rigidBodyBImp._rbi;

            var btRbAFrame = Translator.Float4X4ToBtMatrix(brAFrame);
            var btRbBFrame = Translator.Float4X4ToBtMatrix(brBFrame);

            var btHingeConstraint = new HingeConstraint(btRigidBodyA, btRigidBodyB, btRbAFrame, btRbBFrame, useReferenceFrameA);

            BtWorld.AddConstraint(btHingeConstraint);

            var retval = new HingeConstraintImp();

            retval._hci = btHingeConstraint;
            btHingeConstraint.UserObject = retval;
            return(retval);
        }
コード例 #21
0
        HingeConstraint _AddGateHinge()
        {
            Matrix AFrame = Matrix.RotationX(0) * Matrix.RotationZ(90.0f.ToRad()) * Matrix.RotationY(90.0f.ToRad()) * Matrix.Translation(offset); // rotatione required to make X axis pivot
            Matrix BFrame = AFrame * matrix;

            //var hinge = new HingeConstraint(
            //    body,
            //    Vector3.Zero + base.offset,
            //    Vector3.UnitX,
            //    true);

            //var ma = hinge.AFrame;
            //var mb = hinge.BFrame;
            //var oa = hinge.FrameOffsetA;
            //var ob = hinge.FrameOffsetB;
            //var im = matrix;
            //im.Invert();
            //var mb2 = mb * im;

            //hinge.Dispose();
            var hinge = new HingeConstraint(
                body,
                TypedConstraint.GetFixedBody(),
                AFrame,
                BFrame,
                true) ;

            if (_type!= GateType.Spinner)
            {
                hinge.SetLimit(_startAngle, _endAngle, 0.0f); // revers angles to have same as in VPX behavior
            }
            
            
            //body.ActivationState = ActivationState.DisableDeactivation;
            //body.SetSleepingThresholds(float.MaxValue, 0.0f); // no sleep for flippers

            return hinge;
        }
コード例 #22
0
        TypedConstraint _AddFlipperHinge()
        {
            float hh = _height * 0.5f;

            var hinge = new HingeConstraint(
                body,
                new Vector3(0, 0, hh),
                Vector3.UnitZ,
                false);

            body.ActivationState = ActivationState.DisableDeactivation;
            body.SetSleepingThresholds(float.MaxValue, 0.0f); // no sleep for flippers

            if (RotationDirection == 1)
            {
                hinge.SetLimit(_startAngle, _endAngle, 0.0f);
            }
            else
            {
                hinge.SetLimit(_endAngle, _startAngle, 0.0f);
            }

            return(hinge);
        }
コード例 #23
0
        private void CreateHingedBoxes()
        {
            const float mass = 1.0f;

            RigidBody body0 = LocalCreateRigidBody(mass, Matrix.Translation(0, 24, 0), cubeShape);
            RigidBody body1 = LocalCreateRigidBody(mass, Matrix.Translation(2 * CubeHalfExtent, 24, 0), cubeShape);

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

            Matrix  transform = Matrix.Invert(body1.CenterOfMassTransform) * body0.CenterOfMassTransform;
            Vector3 pivotInB  = Vector3.TransformCoordinate(pivotInA, transform);

            transform = Matrix.Invert(body1.CenterOfMassTransform) * body1.CenterOfMassTransform;
            Vector3 axisInB = Vector3.TransformCoordinate(axisInA, transform);

            //var pointToPoint = new Point2PointConstraint(body0, body1, pivotInA, pivotInB);
            //pointToPoint.DebugDrawSize = 5;
            //World.AddConstraint(pointToPoint);

            var hinge = new HingeConstraint(body0, body1, pivotInA, pivotInB, axisInA, axisInB);

            World.AddConstraint(hinge);
        }
コード例 #24
0
        //----------------------------------------------------------------------------------------------------------------

        public override void InitializeDemo()
        {
            CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 3, 50));

            //CollisionShape groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);


            m_collisionShapes.Add(groundShape);
            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_broadphase = new SimpleBroadphase(100, null);

            m_constraintSolver = new SequentialImpulseConstraintSolver();
            m_dynamicsWorld    = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);

            //m_dynamicsWorld.setGravity(new IndexedVector3(0,0,0));
            IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);

            //either use heightfield or triangle mesh

            //create ground object
            LocalCreateRigidBody(0f, ref tr, groundShape);

            CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));

            m_collisionShapes.Add(chassisShape);

            CompoundShape compound = new CompoundShape();

            m_collisionShapes.Add(compound);
            //localTrans effectively shifts the center of mass with respect to the chassis
            IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);

            compound.AddChildShape(ref localTrans, chassisShape);

            {
                CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
                //localTrans effectively shifts the center of mass with respect to the chassis
                IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
                compound.AddChildShape(ref suppLocalTrans, suppShape);
            }

            tr._origin = IndexedVector3.Zero;

            m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
            //m_carChassis = LocalCreateRigidBody(800f, ref tr, chassisShape);//chassisShape);
            //CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
            //m_collisionShapes.Add(liftShape);
            //m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);

            //IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
            //m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);

            //IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
            //localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);

            //IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
            //localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);

            //m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
            ////		m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
            //m_liftHinge.SetLimit(0.0f, 0.0f);
            //m_dynamicsWorld.AddConstraint(m_liftHinge, true);


            //CompoundShape forkCompound = new CompoundShape();
            //m_collisionShapes.Add(forkCompound);

            //IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
            //CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
            //m_collisionShapes.Add(forkShapeA);
            //forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);

            //CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
            //m_collisionShapes.Add(forkShapeB);
            //forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
            //forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);

            //CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
            //m_collisionShapes.Add(forkShapeC);
            //forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
            //forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);

            //m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
            //IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);

            //m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);

            //localA = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
            //localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);

            //IndexedVector3 col0 = MathUtil.matrixColumn(ref localA, 0);
            //IndexedVector3 col1 = MathUtil.matrixColumn(ref localA, 1);
            //IndexedVector3 col2 = MathUtil.matrixColumn(ref localA, 2);



            ////localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
            //localB = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
            //localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);

            //m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);

            //m_forkSlider.SetLowerLinLimit(0.1f);
            //m_forkSlider.SetUpperLinLimit(0.1f);
            ////		m_forkSlider.setLowerAngLimit(-LIFT_EPS);
            ////		m_forkSlider.setUpperAngLimit(LIFT_EPS);
            //m_forkSlider.SetLowerAngLimit(0.0f);
            //m_forkSlider.SetUpperAngLimit(0.0f);

            //IndexedMatrix localAVec = IndexedMatrix.Identity;
            //IndexedMatrix localBVec = IndexedMatrix.Identity;

            //m_forkSlider2 = new HingeConstraint(m_liftBody, m_forkBody, ref localAVec, ref localBVec);
            //m_dynamicsWorld.AddConstraint(m_forkSlider, true);
            //m_dynamicsWorld.addConstraint(m_forkSlider2, true);


            CompoundShape loadCompound = new CompoundShape(true);

            m_collisionShapes.Add(loadCompound);
            CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));

            m_collisionShapes.Add(loadShapeA);
            IndexedMatrix loadTrans = IndexedMatrix.Identity;

            loadCompound.AddChildShape(ref loadTrans, loadShapeA);
            CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));

            m_collisionShapes.Add(loadShapeB);
            loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
            loadCompound.AddChildShape(ref loadTrans, loadShapeB);
            CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));

            m_collisionShapes.Add(loadShapeC);
            loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
            loadCompound.AddChildShape(ref loadTrans, loadShapeC);
            m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
            loadTrans      = IndexedMatrix.CreateTranslation(m_loadStartPos);

            m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);


#if false
            {
                CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
                m_collisionShapes.Add(liftShape);
                IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
                m_liftBody = localCreateRigidBody(10f, ref liftTrans, liftShape);

                IndexedMatrix localA = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
                localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);

                IndexedMatrix localB = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
                localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);

                m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
                //		m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
                m_liftHinge.setLimit(0.0f, 0.0f);
                m_dynamicsWorld.addConstraint(m_liftHinge, true);

                CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
                m_collisionShapes.Add(forkShapeA);
                CompoundShape forkCompound = new CompoundShape();
                m_collisionShapes.Add(forkCompound);
                IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
                forkCompound.addChildShape(ref forkLocalTrans, forkShapeA);

                CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
                m_collisionShapes.Add(forkShapeB);
                forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
                forkCompound.addChildShape(ref forkLocalTrans, forkShapeB);

                CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
                m_collisionShapes.Add(forkShapeC);
                forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
                forkCompound.addChildShape(ref forkLocalTrans, forkShapeC);

                m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
                IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);

                m_forkBody = localCreateRigidBody(5f, ref forkTrans, forkCompound);

                localA         = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
                localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);

                localB         = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
                localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);

                m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
                m_forkSlider.setLowerLinLimit(0.1f);
                m_forkSlider.setUpperLinLimit(0.1f);
                //		m_forkSlider.setLowerAngLimit(-LIFT_EPS);
                //		m_forkSlider.setUpperAngLimit(LIFT_EPS);
                m_forkSlider.setLowerAngLimit(0.0f);
                m_forkSlider.setUpperAngLimit(0.0f);
                m_dynamicsWorld.addConstraint(m_forkSlider, true);


                CompoundShape loadCompound = new CompoundShape();
                m_collisionShapes.Add(loadCompound);
                CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
                m_collisionShapes.Add(loadShapeA);
                IndexedMatrix loadTrans = IndexedMatrix.Identity;
                loadCompound.addChildShape(ref loadTrans, loadShapeA);
                CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
                m_collisionShapes.Add(loadShapeB);
                loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
                loadCompound.addChildShape(ref loadTrans, loadShapeB);
                CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
                m_collisionShapes.Add(loadShapeC);
                loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
                loadCompound.addChildShape(ref loadTrans, loadShapeC);
                m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
                loadTrans      = IndexedMatrix.CreateTranslation(m_loadStartPos);

                m_loadBody = localCreateRigidBody(4f, ref loadTrans, loadCompound);
            }
#endif
            //m_carChassis.setDamping(0.2f, 0.2f);

            ClientResetScene();

            /// create vehicle

            SetCameraDistance(26.0f);
            SetTexturing(true);
            SetShadows(true);
        }
コード例 #25
0
        /// <summary>
        /// Builds the constraint.
        /// </summary>
        /// <returns></returns>
        internal override bool _BuildConstraint()
        {
            BPhysicsWorld world = BPhysicsWorld.Get();

            if (m_constraintPtr != null)
            {
                if (m_isInWorld && world != null)
                {
                    m_isInWorld = false;
                    world.RemoveConstraint(m_constraintPtr);
                }
            }
            BRigidBody targetRigidBodyA = GetComponent <BRigidBody>();

            if (targetRigidBodyA == null)
            {
                Debug.LogError("BHingedConstraint needs to be added to a component with a BRigidBody.");
                return(false);
            }
            if (!targetRigidBodyA.isInWorld)
            {
                world.AddRigidBody(targetRigidBodyA);
            }
            if (m_localConstraintAxisX == Vector3.zero)
            {
                Debug.LogError("Constaint axis cannot be zero vector");
                return(false);
            }
            RigidBody rba = (RigidBody)targetRigidBodyA.GetCollisionObject();

            if (rba == null)
            {
                Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                return(false);
            }
            if (m_constraintType == ConstraintType.constrainToAnotherBody)
            {
                if (m_otherRigidBody == null)
                {
                    Debug.LogError("Other rigid body must not be null");
                    return(false);
                }
                if (!m_otherRigidBody.isInWorld)
                {
                    world.AddRigidBody(m_otherRigidBody);
                }
                RigidBody rbb = (RigidBody)m_otherRigidBody.GetCollisionObject();
                if (rbb == null)
                {
                    Debug.LogError("Constraint could not get bullet RigidBody from target rigid body");
                    return(false);
                }

                m_constraintPtr = new HingeConstraint(rba, rbb, m_localConstraintPoint.ToBullet(),
                                                      m_otherRigidBody.transform.InverseTransformPoint(transform.TransformPoint(m_localConstraintPoint)).ToBullet(),
                                                      m_axisInA.ToBullet(), m_axisInB.ToBullet());
            }
            else
            {
                m_constraintPtr = new HingeConstraint(rba, m_localConstraintPoint.ToBullet(), m_axisInA.ToBullet(), false);
            }
            if (m_enableMotor)
            {
                ((HingeConstraint)m_constraintPtr).EnableAngularMotor(true, m_targetMotorAngularVelocity, m_maxMotorImpulse);
            }
            if (m_setLimit)
            {
                ((HingeConstraint)m_constraintPtr).SetLimit(m_lowLimitAngleRadians, m_highLimitAngleRadians, m_limitSoftness, m_limitBiasFactor);
            }
            m_constraintPtr.Userobject                  = this;
            m_constraintPtr.DebugDrawSize               = m_debugDrawSize;
            m_constraintPtr.BreakingImpulseThreshold    = m_breakingImpulseThreshold;
            m_constraintPtr.OverrideNumSolverIterations = m_overrideNumSolverIterations;
            return(true);
        }
コード例 #26
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);
        }
コード例 #27
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);
        }
コード例 #28
0
        public IHingeConstraintImp AddHingeConstraint(IRigidBodyImp rigidBodyA, IRigidBodyImp rigidBodyB, float4x4 brAFrame, float4x4 brBFrame, bool useReferenceFrameA)
        {
            var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
            var btRigidBodyA = rigidBodyAImp._rbi;

            var rigidBodyBImp = (RigidBodyImp)rigidBodyB;
            var btRigidBodyB = rigidBodyBImp._rbi;

            var btRbAFrame = Translater.Float4X4ToBtMatrix(brAFrame);
            var btRbBFrame = Translater.Float4X4ToBtMatrix(brBFrame);

            var btHingeConstraint = new HingeConstraint(btRigidBodyA, btRigidBodyB,btRbAFrame, btRbBFrame, useReferenceFrameA);
            BtWorld.AddConstraint(btHingeConstraint);

            var retval = new HingeConstraintImp();
            retval._hci = btHingeConstraint;
            btHingeConstraint.UserObject = retval;
            return retval;
        }
コード例 #29
0
        public IHingeConstraintImp AddHingeConstraint(IRigidBodyImp rigidBodyA, IRigidBodyImp rigidBodyB, float3 pivotInA, float3 pivotInB, float3 axisInA, float3 AxisInB, bool useReferenceFrameA)
        {
            var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
            var btRigidBodyA = rigidBodyAImp._rbi;

            var rigidBodyBImp = (RigidBodyImp)rigidBodyB;
            var btRigidBodyB = rigidBodyBImp._rbi;

            var btHingeConstraint = new HingeConstraint(btRigidBodyA, btRigidBodyB, new Vector3(pivotInA.x, pivotInA.y, pivotInA.z), new Vector3(pivotInB.x, pivotInB.y, pivotInB.z), new Vector3(axisInA.x, axisInA.y, axisInA.z), new Vector3(AxisInB.x, AxisInB.y, AxisInB.z), useReferenceFrameA);
            BtWorld.AddConstraint(btHingeConstraint);

            var retval = new HingeConstraintImp();
            retval._hci = btHingeConstraint;
            btHingeConstraint.UserObject = retval;
            return retval;
        }
コード例 #30
0
        public TestRig(DemoApplication demoApplication, DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, bool bFixed)
        {
            m_dynamicsWorld = ownerWorld;
            IndexedVector3 vUp = new IndexedVector3(0, 1, 0);

            //
            // Setup geometry
            //
            float fBodySize      = 0.25f;
            float fLegLength     = 0.45f;
            float fForeLegLength = 0.75f;

            m_shapes[0] = new CapsuleShape(fBodySize, 0.10f);
            for (int i = 0; i < NUM_LEGS; i++)
            {
                m_shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength);
                m_shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength);
            }

            //
            // Setup rigid bodies
            //
            float         fHeight = 0.5f;
            IndexedMatrix offset  = IndexedMatrix.Identity;

            offset._origin = positionOffset;

            // root
            IndexedVector3 vRoot     = new IndexedVector3(0, fHeight, 0);
            IndexedMatrix  transform = IndexedMatrix.Identity;

            transform._origin = vRoot;

            if (bFixed)
            {
                m_bodies[0] = demoApplication.LocalCreateRigidBody(0.0f, offset * transform, m_shapes[0]);
            }
            else
            {
                m_bodies[0] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[0]);
            }
            // legs
            for (int i = 0; i < NUM_LEGS; i++)
            {
                float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
                float fSin   = (float)Math.Sin(fAngle);
                float fCos   = (float)Math.Cos(fAngle);

                transform = IndexedMatrix.Identity;
                IndexedVector3 vBoneOrigin = new IndexedVector3(fCos * (fBodySize + 0.5f * fLegLength), fHeight, fSin * (fBodySize + 0.5f * fLegLength));
                transform._origin = vBoneOrigin;

                // thigh
                IndexedVector3 vToBone = (vBoneOrigin - vRoot);
                vToBone.Normalize();

                IndexedVector3 vAxis = IndexedVector3.Cross(vToBone, vUp);
                transform._basis    = new IndexedBasisMatrix(new IndexedQuaternion(vAxis, MathUtil.SIMD_HALF_PI));
                transform._origin   = vBoneOrigin;
                m_bodies[1 + 2 * i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[1 + 2 * i]);

                // shin
                transform           = IndexedMatrix.Identity;
                transform._origin   = new IndexedVector3(fCos * (fBodySize + fLegLength), fHeight - 0.5f * fForeLegLength, fSin * (fBodySize + fLegLength));
                m_bodies[2 + 2 * i] = demoApplication.LocalCreateRigidBody(1.0f, offset * transform, m_shapes[2 + 2 * i]);
            }

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


            //
            // Setup the constraints
            //
            HingeConstraint hingeC;

            IndexedMatrix localA, localB, localC;

            for (int i = 0; i < NUM_LEGS; i++)
            {
                float fAngle = MathUtil.SIMD_2_PI * i / NUM_LEGS;
                float fSin   = (float)Math.Sin(fAngle);
                float fCos   = (float)Math.Cos(fAngle);

                // hip joints
                localA = IndexedMatrix.Identity;
                localB = IndexedMatrix.Identity;

                localA         = MathUtil.SetEulerZYX(0f, -fAngle, 0f);
                localA._origin = new IndexedVector3(fCos * fBodySize, 0.0f, fSin * fBodySize);
                localB         = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;


                if (BulletGlobals.g_streamWriter != null && false)
                {
                    MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalA", localA);
                    MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Hip LocalB", localB);
                }

                hingeC = new HingeConstraint(m_bodies[0], m_bodies[1 + 2 * i], ref localA, ref localB);
                hingeC.SetLimit(-0.75f * MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI / 2f);
                m_joints[2 * i] = hingeC;
                m_dynamicsWorld.AddConstraint(m_joints[2 * i], true);

                // knee joints
                localA = IndexedMatrix.Identity;
                localB = IndexedMatrix.Identity;
                localC = IndexedMatrix.Identity;

                localA         = MathUtil.SetEulerZYX(0f, -fAngle, 0f);
                localA._origin = new IndexedVector3(fCos * (fBodySize + fLegLength), 0.0f, fSin * (fBodySize + fLegLength));

                localB = m_bodies[1 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;
                localC = m_bodies[2 + 2 * i].GetWorldTransform().Inverse() * m_bodies[0].GetWorldTransform() * localA;


                if (BulletGlobals.g_streamWriter != null && false)
                {
                    MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalA", localA);
                    MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalB", localB);
                    MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "Knee LocalC", localC);
                }

                hingeC = new HingeConstraint(m_bodies[1 + 2 * i], m_bodies[2 + 2 * i], ref localB, ref localC);
                //hingeC.setLimit(float(-0.01), float(0.01));
                hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI / 2f, 0.2f);
                m_joints[1 + 2 * i] = hingeC;
                m_dynamicsWorld.AddConstraint(m_joints[1 + 2 * i], true);
            }
        }
コード例 #31
0
ファイル: MotorDemo.cs プロジェクト: diqost/bullet
            public TestRig(DynamicsWorld ownerWorld, Vector3 positionOffset, bool bFixed)
            {
                this.ownerWorld = ownerWorld;
                Vector3 vUp = new Vector3(0, 1, 0);

                //
                // Setup geometry
                //
                const float fBodySize      = 0.25f;
                const float fLegLength     = 0.45f;
                const float fForeLegLength = 0.75f;
                const float PI_2           = (float)(0.5f * Math.PI);
                const float PI_4           = (float)(0.25f * Math.PI);
                const float PI_8           = (float)(0.125f * Math.PI);

                shapes[0] = new CapsuleShape(fBodySize, 0.10f);
                int i;

                for (i = 0; i < NumLegs; i++)
                {
                    shapes[1 + 2 * i] = new CapsuleShape(0.10f, fLegLength);
                    shapes[2 + 2 * i] = new CapsuleShape(0.08f, fForeLegLength);
                }

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

                // root
                Vector3 vRoot     = new Vector3(0, fHeight, 0);
                Matrix  transform = Matrix.Translation(vRoot);

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

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

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

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

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

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

                Matrix localA, localB, localC;

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

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

                    // knee joints
                    localA = Matrix.RotationYawPitchRoll(-fAngle, 0, 0) * Matrix.Translation(fCos * (fBodySize + fLegLength), 0, fSin * (fBodySize + fLegLength));
                    localB = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[1 + 2 * i].WorldTransform);
                    localC = localA * bodies[0].WorldTransform * Matrix.Invert(bodies[2 + 2 * i].WorldTransform);
                    hingeC = new HingeConstraint(bodies[1 + 2 * i], bodies[2 + 2 * i], localB, localC);
                    //hingeC.SetLimit(-0.01f, 0.01f);
                    hingeC.SetLimit(-PI_8, 0.2f);
                    joints[1 + 2 * i] = hingeC;
                    ownerWorld.AddConstraint(joints[1 + 2 * i], true);
                }
            }
コード例 #32
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;
        }
コード例 #33
0
        /*//TODO: What about inheritance problems -> should return any constraint type
        public IPoint2PointConstraintImp GetConstraint(int i)
        {
            return (Point2PointConstraintImp)BtWorld.GetConstraint(0).UserObject;
        }*/
        //HingeConstraint
        public IHingeConstraintImp AddHingeConstraint(IRigidBodyImp rigidBodyA, float4x4 frameInA, bool useReferenceFrameA)
        {
            var rigidBodyAImp = (RigidBodyImp)rigidBodyA;
            var btRigidBodyA = rigidBodyAImp._rbi;
            var btFframeInA = Translater.Float4X4ToBtMatrix(frameInA);
            var btHingeConstraint = new HingeConstraint(btRigidBodyA, btFframeInA, useReferenceFrameA);
            BtWorld.AddConstraint(btHingeConstraint);

            var retval = new HingeConstraintImp();
            retval._hci = btHingeConstraint;
            btHingeConstraint.UserObject = retval;
            return retval;
        }