Tries to rotate two entities so that they reach a specified relative orientation or speed around an axis.
Inheritance: BEPUphysics.Constraints.TwoEntity.Motors.Motor, I1DImpulseConstraintWithError, I1DJacobianConstraint
Exemplo n.º 1
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA.  Will be kept perpendicular with the twist axis.</param>
        public SwivelHingeJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor, ref Vector3 hingeAxis)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
			Vector3 tmp;
			BallSocketJoint.OffsetB.Invert( out tmp );
            AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, ref hingeAxis, ref tmp );
            HingeLimit = new RevoluteLimit(connectionA, connectionB);
            HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
            TwistLimit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
            TwistMotor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
            HingeLimit.IsActive = false;
            HingeMotor.IsActive = false;
            TwistLimit.IsActive = false;
            TwistMotor.IsActive = false;

			//Ensure that the base and test direction is perpendicular to the free axis.
			Vector3 baseAxis; anchor.Sub( ref connectionA.position, out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
                connectionB.position.Sub( ref anchor, out baseAxis );
            baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot( ref baseAxis, ref hingeAxis) , out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
                }
            }
            HingeLimit.Basis.SetWorldAxes(ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);
            HingeMotor.Basis.SetWorldAxes( ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);

			connectionB.position.Sub( ref anchor, out baseAxis );
            baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot(ref baseAxis, ref hingeAxis), out baseAxis );
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
                }
            }
            HingeLimit.TestAxis = baseAxis;
            HingeMotor.TestAxis = baseAxis;


            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(HingeLimit);
            Add(HingeMotor);
            Add(TwistLimit);
            Add(TwistMotor);
        }
Exemplo n.º 2
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA.  Will be kept perpendicular with the twist axis.</param>
        public SwivelHingeJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 hingeAxis)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
            AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, hingeAxis, -BallSocketJoint.OffsetB);
            HingeLimit = new RevoluteLimit(connectionA, connectionB);
            HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
            TwistLimit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
            TwistMotor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
            HingeLimit.IsActive = false;
            HingeMotor.IsActive = false;
            TwistLimit.IsActive = false;
            TwistMotor.IsActive = false;

            //Ensure that the base and test direction is perpendicular to the free axis.
            Vector3 baseAxis = anchor - connectionA.position;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
                baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
                }
            }
            HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
            HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);

            baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
                }
            }
            HingeLimit.TestAxis = baseAxis;
            HingeMotor.TestAxis = baseAxis;


            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(HingeLimit);
            Add(HingeMotor);
            Add(TwistLimit);
            Add(TwistMotor);
        }
Exemplo n.º 3
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
        /// This constructs the internal constraints, but does not configure them.  Before using a constraint constructed in this manner,
        /// ensure that its active constituent constraints are properly configured.  The entire group as well as all internal constraints are initially inactive (IsActive = false).
        /// </summary>
        public RevoluteJoint()
        {
            IsActive = false;
            BallSocketJoint = new BallSocketJoint();
            AngularJoint = new RevoluteAngularJoint();
            Limit = new RevoluteLimit();
            Motor = new RevoluteMotor();

            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(Limit);
            Add(Motor);
        }
Exemplo n.º 4
0
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities.
        /// </summary>
        /// <param name="connectionA">First entity of the constraint pair.</param>
        /// <param name="connectionB">Second entity of the constraint pair.</param>
        /// <param name="anchor">Point around which both entities rotate.</param>
        /// <param name="freeAxis">Axis around which the hinge can rotate.</param>
        public RevoluteJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 anchor, System.Numerics.Vector3 freeAxis)
        {
            if (connectionA == null)
                connectionA = TwoEntityConstraint.WorldEntity;
            if (connectionB == null)
                connectionB = TwoEntityConstraint.WorldEntity;
            BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
            AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis);
            Limit = new RevoluteLimit(connectionA, connectionB);
            Motor = new RevoluteMotor(connectionA, connectionB, freeAxis);
            Limit.IsActive = false;
            Motor.IsActive = false;

            //Ensure that the base and test direction is perpendicular to the free axis.
            System.Numerics.Vector3 baseAxis = anchor - connectionA.position;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
                baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
                }
            }
            Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
            Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);

            baseAxis = connectionB.position - anchor;
            baseAxis -= Vector3Ex.Dot(baseAxis, freeAxis) * freeAxis;
            if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
            {
                //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
                baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up);
                if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
                {
                    baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right);
                }
            }
            Limit.TestAxis = baseAxis;
            Motor.TestAxis = baseAxis;

            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(Limit);
            Add(Motor);
        }
        /// <summary>
        /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
        /// This constructs the internal constraints, but does not configure them.  Before using a constraint constructed in this manner,
        /// ensure that its active constituent constraints are properly configured.  The entire group as well as all internal constraints are initially inactive (IsActive = false).
        /// </summary>
        public SwivelHingeJoint()
        {
            IsActive = false;
            BallSocketJoint = new BallSocketJoint();
            AngularJoint = new SwivelHingeAngularJoint();
            HingeLimit = new RevoluteLimit();
            HingeMotor = new RevoluteMotor();
            TwistLimit = new TwistLimit();
            TwistMotor = new TwistMotor();

            Add(BallSocketJoint);
            Add(AngularJoint);
            Add(HingeLimit);
            Add(HingeMotor);
            Add(TwistLimit);
            Add(TwistMotor);
        }
Exemplo n.º 6
0
 public override SolverUpdateable GetBaseJoint()
 {
     Motor = new RevoluteMotor(Ent1.Body, Ent2.Body, Direction.ToBVector());
     if (IsSteering)
     {
         Motor.Settings.Mode = MotorMode.Servomechanism;
         Motor.Basis.SetWorldAxes(Vector3.UnitZ, Vector3.UnitX);
         Motor.TestAxis = Vector3.UnitX;
         Motor.Settings.Servo.BaseCorrectiveSpeed = 5;
     }
     else
     {
         Motor.Settings.Mode = MotorMode.VelocityMotor;
         Motor.Settings.VelocityMotor.Softness = 0.03f;
         Motor.Settings.MaximumForce = 100000;
     }
     return Motor;
 }
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public ReverseTrikeDemo(DemosGame game)
            : base(game)
        {
            game.Camera.Position = new Vector3(0, 2, 15);

            Space.Add(new Box(new Vector3(0, -5, 0), 20, 1, 20));

            var body = new Box(new Vector3(0, 0, 0), 2, 1, 3, 10);
            body.CollisionInformation.LocalPosition = new Vector3(0, .8f, 0);
            Space.Add(body);

            #region First Wheel

            var wheel = new Cylinder(body.Position + new Vector3(-1.3f, 0, -1.5f), .2f, .5f, 4);
            wheel.Material = new Material(1.5f, 1.5f, 0);
            wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);

            //Preventing the occasional pointless collision pair can speed things up.
            CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase);

            //Connect the wheel to the body.
            var ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position);
            var swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right);
            //Motorize the wheel.
            drivingMotor1 = new RevoluteMotor(body, wheel, Vector3.Left);
            drivingMotor1.Settings.VelocityMotor.Softness = .2f;
            //Let it roll when the user isn't giving specific commands.
            drivingMotor1.IsActive = false;
            steeringMotor1 = new RevoluteMotor(body, wheel, Vector3.Up);
            steeringMotor1.Settings.Mode = MotorMode.Servomechanism;
            //The constructor makes a guess about how to set up the constraint.
            //It can't always be right since it doesn't have all the information;
            //in this case, it chooses the basis and test axis incorrectly.
            //This leads to a 'flipping' behavior when the wheel is rolling
            //(the test axis is 'rolling' with the wheel, and passes over
            //a singularity which causes a flip).

            //To fix this, we configure the constraint directly.
            //The basis is aligned with how the wheel is set up; we choose 'up' as 
            //the motorized axis, and right/forward to define the angle measurement plane.
            //The test axis is set to be perpendicular to the wheel's rotation so that
            //it only measures the steering angle.

            //If you're curious, the angle measurement is just a Math.Atan2.
            //The current world test axis is dotted against the two plane axes (Right and Forward here).
            //This gives an x and y value.  These can be plugged into Atan2 just like when
            //you compute an angle on a normal 2d graph.
            steeringMotor1.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
            steeringMotor1.TestAxis = Vector3.Right;


            //Add the wheel and connection to the space.
            Space.Add(wheel);
            Space.Add(ballSocketJoint);
            Space.Add(swivelHingeAngularJoint);
            Space.Add(drivingMotor1);
            Space.Add(steeringMotor1);

            #endregion

            #region Second Wheel

            wheel = new Cylinder(body.Position + new Vector3(1.3f, 0, -1.5f), .2f, .5f, 4);
            wheel.Material = new Material(1.5f, 1.5f, 0);
            wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);


            //Preventing the occasional pointless collision pair can speed things up.
            CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase);

            //Connect the wheel to the body.
            ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position);
            swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right);
            //Motorize the wheel.
            drivingMotor2 = new RevoluteMotor(body, wheel, Vector3.Left);
            drivingMotor2.Settings.VelocityMotor.Softness = .2f;
            //Let it roll when the user isn't giving specific commands.
            drivingMotor2.IsActive = false;
            steeringMotor2 = new RevoluteMotor(body, wheel, Vector3.Up);
            steeringMotor2.Settings.Mode = MotorMode.Servomechanism;
            //Configure the motor.  See wheel 1 for more description.
            steeringMotor2.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
            steeringMotor2.TestAxis = Vector3.Right;


            //Add the wheel and connection to the space.
            Space.Add(wheel);
            Space.Add(ballSocketJoint);
            Space.Add(swivelHingeAngularJoint);
            Space.Add(drivingMotor2);
            Space.Add(steeringMotor2);

            #endregion

            #region Third Wheel

            wheel = new Cylinder(body.Position + new Vector3(0, -.3f, 1.5f), .2f, .5f, 4);
            wheel.Material = new Material(1.5f, 1.5f, 0);
            wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);

            //Preventing the occasional pointless collision pair can speed things up.
            CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase);

            //Connect the wheel to the body.
            ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position);
            //Notice that the third wheel isn't a swivel hinge, it's just a revolute axis.
            //This lets it roll, but prevents flopping around like the wheels of a grocery cart.
            //Could have used a RevoluteJoint solver group here, but this shows it's possible to do
            //the same things without using the combo-constraints.
            var revoluteAngularJoint = new RevoluteAngularJoint(body, wheel, Vector3.Right);

            //Add the wheel and connection to the space.
            Space.Add(wheel);
            Space.Add(ballSocketJoint);
            Space.Add(revoluteAngularJoint);

            #endregion

            int xLength = 180;
            int zLength = 180;

            float xSpacing = 8f;
            float zSpacing = 8f;
            var heights = new float[xLength, zLength];
            for (int i = 0; i < xLength; i++)
            {
                for (int j = 0; j < zLength; j++)
                {
                    float x = i - xLength / 2;
                    float z = j - zLength / 2;
                    //heights[i,j] = (float)(x * y / 1000f);
                    heights[i, j] = (float)(10 * (Math.Sin(x / 8) + Math.Sin(z / 8)));
                    //heights[i,j] = 3 * (float)Math.Sin(x * y / 100f);
                    //heights[i,j] = (x * x * x * y - y * y * y * x) / 1000f;
                }
            }
            //Create the terrain.
            var terrain = new Terrain(heights, new AffineTransform(
                new Vector3(xSpacing, 1, zSpacing), 
                Quaternion.Identity, 
                new Vector3(-xLength * xSpacing / 2, -10, -zLength * zSpacing / 2)));
            Space.Add(terrain);

            game.ModelDrawer.Add(terrain);
        }
        Entity AddDriveWheel(Vector3 wheelOffset, Entity body, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor)
        {
            var wheel = new Cylinder(body.Position + wheelOffset, .4f, .5f, 5f);
            wheel.Material.KineticFriction = 2.5f;
            wheel.Material.StaticFriction = 3.5f;
            wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);

            //Preventing the occasional pointless collision pair can speed things up.
            CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);

            //Connect the wheel to the body.
            var pointOnLineJoint = new PointOnLineJoint(body, wheel, wheel.Position, Vector3.Down, wheel.Position);
            var suspensionLimit = new LinearAxisLimit(body, wheel, wheel.Position, wheel.Position, Vector3.Down, -1, 0);
            //This linear axis motor will give the suspension its springiness by pushing the wheels outward.
            var suspensionSpring = new LinearAxisMotor(body, wheel, wheel.Position, wheel.Position, Vector3.Down);
            suspensionSpring.Settings.Mode = MotorMode.Servomechanism;
            suspensionSpring.Settings.Servo.Goal = 0;
            suspensionSpring.Settings.Servo.SpringSettings.Stiffness = 300;
            suspensionSpring.Settings.Servo.SpringSettings.Damping = 70;

            var swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right);
            //Motorize the wheel.
            drivingMotor = new RevoluteMotor(body, wheel, Vector3.Left);
            drivingMotor.Settings.VelocityMotor.Softness = .3f;
            drivingMotor.Settings.MaximumForce = 100;
            //Let it roll when the user isn't giving specific commands.
            drivingMotor.IsActive = false;
            steeringMotor = new RevoluteMotor(body, wheel, Vector3.Up);
            steeringMotor.Settings.Mode = MotorMode.Servomechanism;
            //The constructor makes a guess about how to set up the constraint.
            //It can't always be right since it doesn't have all the information;
            //in this case, it chooses the basis and test axis incorrectly.
            //This leads to a 'flipping' behavior when the wheel is rolling
            //(the test axis is 'rolling' with the wheel, and passes over
            //a singularity which causes a flip).

            //To fix this, we configure the constraint directly.
            //The basis is aligned with how the wheel is set up; we choose 'up' as 
            //the motorized axis, and right/forward to define the angle measurement plane.
            //The test axis is set to be perpendicular to the wheel's rotation so that
            //it only measures the steering angle.

            //If you're curious, the angle measurement is just a Math.Atan2.
            //The current world test axis is dotted against the two plane axes (Right and Forward here).
            //This gives an x and y value.  These can be plugged into Atan2 just like when
            //you compute an angle on a normal 2d graph.
            steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
            steeringMotor.TestAxis = Vector3.Right;

            steeringMotor.Settings.Servo.BaseCorrectiveSpeed = 5;


            //The revolute motor is weaker than some other types of constraints and maintaining a goal in the presence of extremely fast rotation and integration issues.
            //Laying a revolute limit on top of it can help mitigate the problem.
            var steeringConstraint = new RevoluteLimit(body, wheel, Vector3.Up, Vector3.Right, -maximumTurnAngle, maximumTurnAngle);

    
            //Add the wheel and connection to the space.
            Space.Add(wheel);
            Space.Add(pointOnLineJoint);
            Space.Add(suspensionLimit);
            Space.Add(suspensionSpring);
            Space.Add(swivelHingeAngularJoint);
            Space.Add(drivingMotor);
            Space.Add(steeringMotor); 
            Space.Add(steeringConstraint);

            return wheel;
        }
Exemplo n.º 9
0
        public TreadSegment(Vector3 segmentPosition, Entity body, TreadSegmentDescription treadSegmentDescription)
        {
            Entity = new Cylinder(segmentPosition, treadSegmentDescription.Width, treadSegmentDescription.Radius, treadSegmentDescription.Mass);

            Entity.Material.KineticFriction = treadSegmentDescription.Friction;
            Entity.Material.StaticFriction = treadSegmentDescription.Friction;
            Entity.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);

            //Preventing the occasional pointless collision pair can speed things up.
            CollisionRules.AddRule(Entity, body, CollisionRule.NoBroadPhase);

            //Connect the wheel to the body.
            SuspensionAxisJoint = new PointOnLineJoint(body, Entity, Entity.Position, Vector3.Down, Entity.Position);
            SuspensionLengthLimit = new LinearAxisLimit(body, Entity, Entity.Position, Entity.Position, Vector3.Down, -treadSegmentDescription.SuspensionLength, 0);
            //This linear axis motor will give the suspension its springiness by pushing the wheels outward.
            SuspensionSpring = new LinearAxisMotor(body, Entity, Entity.Position, Entity.Position, Vector3.Down);
            SuspensionSpring.Settings.Mode = MotorMode.Servomechanism;
            SuspensionSpring.Settings.Servo.Goal = 0;
            SuspensionSpring.Settings.Servo.SpringSettings.Stiffness = treadSegmentDescription.SuspensionStiffness;
            SuspensionSpring.Settings.Servo.SpringSettings.Damping = treadSegmentDescription.SuspensionDamping;

            SuspensionAngularJoint = new RevoluteAngularJoint(body, Entity, Vector3.Right);
            //Make the joint extremely rigid.  There are going to be extreme conditions when the wheels get up to speed;
            //we don't want the forces involved to torque the wheel off the frame!
            SuspensionAngularJoint.SpringSettings.Damping *= Entity.Mass * 50;
            SuspensionAngularJoint.SpringSettings.Stiffness *= Entity.Mass * 50;
            //Motorize the wheel.
            Motor = new RevoluteMotor(body, Entity, Vector3.Left);
            Motor.Settings.VelocityMotor.Softness = treadSegmentDescription.MotorSoftness;
            Motor.Settings.MaximumForce = treadSegmentDescription.MotorMaximumForce;

        }
Exemplo n.º 10
0
        void AddDriveWheel(Vector3 suspensionOffset, Entity body, bool leftSide, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor, out Box suspensionLeg)
        {
            suspensionLeg = new Box(body.Position + suspensionOffset, 0.25f, 0.8f, 0.25f, 10);

            const float horizontalWheelOffset = 0.2f;

            var wheel = new Cylinder(suspensionLeg.Position + new Vector3(leftSide ? -horizontalWheelOffset : horizontalWheelOffset, -suspensionLeg.HalfHeight, 0), .2f, .3f, 5f);
            wheel.Material.KineticFriction = 2.5f;
            wheel.Material.StaticFriction = 3.5f;
            wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);

            //Preventing the occasional pointless collision pair can speed things up.
            CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);
            CollisionRules.AddRule(wheel, suspensionLeg, CollisionRule.NoBroadPhase);
            CollisionRules.AddRule(suspensionLeg, body, CollisionRule.NoBroadPhase);

            //Connect the suspension to the body.
            var bodyToSuspension = new LineSliderJoint(body, suspensionLeg, suspensionLeg.Position, Vector3.Down, suspensionLeg.Position);
            bodyToSuspension.Limit.IsActive = true;
            bodyToSuspension.Limit.Minimum = -0.5f;
            bodyToSuspension.Limit.Maximum = 0;

            //This linear axis motor will give the suspension its springiness by pushing the wheels outward.
            bodyToSuspension.Motor.IsActive = true;
            bodyToSuspension.Motor.Settings.Mode = MotorMode.Servomechanism;
            bodyToSuspension.Motor.Settings.Servo.Goal = 0;
            bodyToSuspension.Motor.Settings.Servo.SpringSettings.Stiffness = 300;
            bodyToSuspension.Motor.Settings.Servo.SpringSettings.Damping = 70;

            steeringMotor = new RevoluteMotor(body, suspensionLeg, Vector3.Up);
            steeringMotor.Settings.Mode = MotorMode.Servomechanism;
            //The constructor makes a guess about how to set up the constraint.
            //It can't always be right since it doesn't have all the information;
            //in this case, it chooses the basis and test axis incorrectly.
            //This leads to a 'flipping' behavior when the wheel is rolling
            //(the test axis is 'rolling' with the wheel, and passes over
            //a singularity which causes a flip).

            //To fix this, we configure the constraint directly.
            //The basis is aligned with how the wheel is set up; we choose 'up' as
            //the motorized axis, and right/forward to define the angle measurement plane.
            //The test axis is set to be perpendicular to the wheel's rotation so that
            //it only measures the steering angle.

            //If you're curious, the angle measurement is just a Math.Atan2.
            //The current world test axis is dotted against the two plane axes (Right and Forward here).
            //This gives an x and y value.  These can be plugged into Atan2 just like when
            //you compute an angle on a normal 2d graph.
            steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
            steeringMotor.TestAxis = Vector3.Right;

            //To make the steering a little more responsive, set a base speed at which error gets corrected.
            //This works on top of the default error reduction implied by the constraint's spring constants.
            steeringMotor.Settings.Servo.BaseCorrectiveSpeed = 1;

            //The revolute motor is weaker than some other types of constraints and maintaining a goal in the presence of extremely fast rotation and integration issues.
            //Laying a revolute limit on top of it can help mitigate the problem.
            var steeringConstraint = new RevoluteLimit(body, suspensionLeg, Vector3.Up, Vector3.Right, -maximumTurnAngle, maximumTurnAngle);

            //Connect the wheel to the suspension.
            var suspensionToWheel = new RevoluteJoint(suspensionLeg, wheel, wheel.Position, Vector3.Right);
            drivingMotor = suspensionToWheel.Motor;
            //The driving motor's default, created by the RevoluteJoint constructor above, chose the axis of rotation such that negatives values made the car go forward and vice versa.
            //Swap it around so that the positive values make the car roll forward instead!
            drivingMotor.Basis.SetWorldAxes(Vector3.Left, Vector3.Forward);
            drivingMotor.TestAxis = Vector3.Forward;
            drivingMotor.Settings.VelocityMotor.Softness = .3f;
            drivingMotor.Settings.MaximumForce = 100;

            //Add the wheel and connection to the space.
            Space.Add(wheel);
            Space.Add(suspensionLeg);
            Space.Add(bodyToSuspension);
            Space.Add(drivingMotor);
            Space.Add(steeringMotor);
            Space.Add(steeringConstraint);
            Space.Add(suspensionToWheel);
        }
Exemplo n.º 11
0
        void createSteerableVehicle(Vector3 position)
        {
            float width = 15;
            float height = 2;
            float length = 5;
            float wheelWidth = 1;
            float wheelRadius = 2;

            BepuEntity chassis = createBox(position, width, height, length);
            chassis.LoadContent();
            chassis.body.Mass = 100;

            RevoluteMotor motor;

            BepuEntity wheel;
            RevoluteJoint joint;
            SwivelHingeJoint steerJoint;
            Vector3 wheelPos = new Vector3(position.X - (width / 2) + wheelRadius, position.Y, position.Z - (length / 2));
            wheel = createWheel(wheelPos, wheelWidth, wheelRadius);
            motor = new RevoluteMotor(chassis.body, wheel.body, Vector3.UnitZ);
            motor.Settings.VelocityMotor.GoalVelocity = -6;
            motor.IsActive = true;
            wheelPos = new Vector3(position.X - (width / 2) + wheelRadius, position.Y, position.Z - (length / 2) - (wheelWidth * 2));
            space.Add(motor);

            joint = new RevoluteJoint(chassis.body, wheel.body, wheel.body.Position, new Vector3(0, 0, -1));
            space.Add(joint);

            wheel = createWheel(new Vector3(position.X - (width / 2) + wheelRadius, position.Y, position.Z + (length / 2) + wheelWidth), wheelWidth, wheelRadius);
            joint = new RevoluteJoint(chassis.body, wheel.body, wheel.body.Position, new Vector3(0, 0, -1));
            space.Add(joint);

            wheel = createWheel(new Vector3(position.X + (width / 2) - wheelRadius, position.Y, position.Z - (length / 2) - wheelWidth), wheelWidth, wheelRadius);
            motor = new RevoluteMotor(chassis.body, wheel.body, Vector3.UnitZ);
            motor.Settings.VelocityMotor.GoalVelocity = -6;
            motor.IsActive = true;
            joint = new RevoluteJoint(chassis.body, wheel.body, wheel.body.Position, new Vector3(0, 0, -1));
            space.Add(joint);
            space.Add(motor);

            wheel = createWheel(new Vector3(position.X + (width / 2) - wheelRadius, position.Y, position.Z + (length / 2) + wheelWidth), wheelWidth, wheelRadius);
            joint = new RevoluteJoint(chassis.body, wheel.body, wheel.body.Position, new Vector3(0, 0, -1));
            space.Add(joint);

            BepuEntity pole = new BepuEntity();
            pole.modelName = "cube";
            pole.body = new Box(new Vector3(0, position.Y + 12, 0), 2, 18, 2, 1); // I have no idea whatsoever why the height has to be one, but it won't work otherwise...
            pole.localTransform = Matrix.CreateScale(new Vector3(2, 18, 2));
            //pole.body.CollisionInformation.LocalPosition = new Vector3(0, 20, 0);
            pole.configureEvents();

            joint = new RevoluteJoint(chassis.body, pole.body, chassis.body.Position, new Vector3(-1, 0, 0));

            joint.Limit.IsActive = true;
            joint.Limit.MaximumAngle = MathHelper.ToRadians(45.0f);
            joint.Limit.MinimumAngle = MathHelper.ToRadians(-45.0f);
            joint.Motor.Settings.Mode = MotorMode.Servomechanism;
            joint.Motor.Settings.Servo.Goal = MathHelper.ToRadians(-45.0f);
            joint.Motor.Settings.Servo.BaseCorrectiveSpeed = 8f;
            joint.Motor.Settings.MaximumForce = 2;
            joint.Motor.IsActive = true;

            children.Add(pole);
            space.Add(pole.body);
            space.Add(joint);
        }